diff --git a/.gitignore b/.gitignore index 789f2728f3bdff..05c14a98abfebb 100644 --- a/.gitignore +++ b/.gitignore @@ -20,7 +20,9 @@ a.out .*.un~ *.tmp *.o +*.o-* *.os +*.os-* *.so *.a *.clb diff --git a/README.md b/README.md index 1ed8ca60cdd4f0..ec1aef0817a8c2 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,9 @@ + +WARNING: TESLA ONLY OPENPILOT 0.7.4-T15 +====== +This repo contains code that was modified specifically for Tesla and will not work on other cars! +Main Comma.ai code is Copyright (c) 2018, Comma.ai, Inc. Additonal work (ALCA, webcamera, any modifications to base Comma.ao code) is licensed under Creative Commons License
This work is licensed under a Creative Commons Attribution-NonCommercial-NoDerivatives 4.0 International License. + [![](https://i.imgur.com/UelUjKAh.png)](#) Table of Contents @@ -57,106 +63,120 @@ openpilot should preserve all other vehicle's stock features, including, but are Supported Hardware ------ -At the moment, openpilot supports the [EON DevKit](https://comma.ai/shop/products/eon-dashcam-devkit) and the [comma two](https://comma.ai/shop/products/comma-two-devkit). A [car harness](https://comma.ai/shop/products/car-harness) is recommended to connect the EON or comma two to the car. In the future, we'd like to support other platforms as well, like gaming PCs. +At the moment, openpilot supports the [EON DevKit](https://comma.ai/shop/products/eon-dashcam-devkit) and the [comma two](https://comma.ai/shop/products/comma-two-devkit). A [car harness](https://comma.ai/shop/products/car-harness) is recommended to connect the EON or comma two to the car. For experimental purposes, openpilot can also run on an Ubuntu computer with external [webcams](https://github.com/commaai/openpilot/tree/master/tools/webcam). Supported Cars ------ | Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below | | ----------| ------------------------------| ------------------| -----------------| -------------------| ------------------| -| Acura | ILX 2016-18 | AcuraWatch Plus | openpilot | 25mph6 | 25mph | -| Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph6 | 12mph | -| Chrysler | Pacifica 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph | -| Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph | -| Chrysler | Pacifica Hybrid 2019-20 | Adaptive Cruise | Stock | 0mph | 39mph | +| Acura | ILX 2016-18 | AcuraWatch Plus | openpilot | 25mph1 | 25mph | +| Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph1 | 12mph | | Honda | Accord 2018-19 | All | Stock | 0mph | 3mph | | Honda | Accord Hybrid 2018-19 | All | Stock | 0mph | 3mph | | Honda | Civic Hatchback 2017-19 | Honda Sensing | Stock | 0mph | 12mph | | Honda | Civic Sedan/Coupe 2016-18 | Honda Sensing | openpilot | 0mph | 12mph | -| Honda | Civic Sedan/Coupe 2019 | Honda Sensing | Stock | 0mph | 2mph4 | -| Honda | CR-V 2015-16 | Touring | openpilot | 25mph6 | 12mph | +| Honda | Civic Sedan/Coupe 2019-20 | Honda Sensing | Stock | 0mph | 2mph2 | +| Honda | CR-V 2015-16 | Touring | openpilot | 25mph1 | 12mph | | Honda | CR-V 2017-19 | Honda Sensing | Stock | 0mph | 12mph | | Honda | CR-V Hybrid 2017-2019 | Honda Sensing | Stock | 0mph | 12mph | -| Honda | Fit 2018-19 | Honda Sensing | openpilot | 25mph6 | 12mph | +| Honda | Fit 2018-19 | Honda Sensing | openpilot | 25mph1 | 12mph | +| Honda | HR-V 2019 | Honda Sensing | openpilot | 25mph1 | 12mph | | Honda | Insight 2019 | Honda Sensing | Stock | 0mph | 3mph | -| Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph6 | 0mph | -| Honda | Passport 2019 | All | openpilot | 25mph6 | 12mph | -| Honda | Pilot 2016-18 | Honda Sensing | openpilot | 25mph6 | 12mph | -| Honda | Pilot 2019 | All | openpilot | 25mph6 | 12mph | -| Honda | Ridgeline 2017-19 | Honda Sensing | openpilot | 25mph6 | 12mph | -| Hyundai | Elantra 2017-191 | SCC + LKAS | Stock | 19mph | 34mph | -| Hyundai | Genesis 20181 | All | Stock | 19mph | 34mph | -| Hyundai | Santa Fe 20191 | All | Stock | 0mph | 0mph | -| Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Stock | 0mph | 9mph | -| Jeep | Grand Cherokee 2019 | Adaptive Cruise | Stock | 0mph | 39mph | -| Kia | Optima 20191 | SCC + LKAS | Stock | 0mph | 0mph | -| Kia | Sorento 20181 | All | Stock | 0mph | 0mph | -| Kia | Stinger 20181 | SCC + LKAS | Stock | 0mph | 0mph | -| Lexus | CT Hybrid 2017-18 | All | Stock5| 0mph | 0mph | +| Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph1 | 0mph | +| Honda | Passport 2019 | All | openpilot | 25mph1 | 12mph | +| Honda | Pilot 2016-18 | Honda Sensing | openpilot | 25mph1 | 12mph | +| Honda | Pilot 2019 | All | openpilot | 25mph1 | 12mph | +| Honda | Ridgeline 2017-20 | Honda Sensing | openpilot | 25mph1 | 12mph | +| Lexus | CT Hybrid 2017-18 | All | Stock3| 0mph | 0mph | | Lexus | ES 2019 | All | openpilot | 0mph | 0mph | | Lexus | ES Hybrid 2019 | All | openpilot | 0mph | 0mph | | Lexus | IS 2017-2019 | All | Stock | 22mph | 0mph | | Lexus | IS Hybrid 2017 | All | Stock | 0mph | 0mph | -| Lexus | NX Hybrid 2018 | All | Stock5| 0mph | 0mph | -| Lexus | RX 2016-17 | All | Stock5| 0mph | 0mph | +| Lexus | NX Hybrid 2018 | All | Stock3| 0mph | 0mph | +| Lexus | RX 2016-17 | All | Stock3| 0mph | 0mph | | Lexus | RX 2020 | All | openpilot | 0mph | 0mph | -| Lexus | RX Hybrid 2016-19 | All | Stock5| 0mph | 0mph | -| Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph | -| Subaru | Impreza 2019-20 | EyeSight | Stock | 0mph | 0mph | -| Toyota | Avalon 2016 | TSS-P | Stock5| 20mph6 | 0mph | -| Toyota | Avalon 2017-18 | All | Stock5| 20mph6 | 0mph | -| Toyota | Camry 2018-19 | All | Stock | 0mph2 | 0mph | -| Toyota | Camry Hybrid 2018-19 | All | Stock | 0mph2 | 0mph | +| Lexus | RX Hybrid 2016-19 | All | Stock3| 0mph | 0mph | +| Toyota | Avalon 2016 | TSS-P | Stock3| 20mph1 | 0mph | +| Toyota | Avalon 2017-18 | All | Stock3| 20mph1 | 0mph | +| Toyota | Camry 2018-20 | All | Stock | 0mph4 | 0mph | +| Toyota | Camry Hybrid 2018-19 | All | Stock | 0mph4 | 0mph | | Toyota | C-HR 2017-19 | All | Stock | 0mph | 0mph | | Toyota | C-HR Hybrid 2017-19 | All | Stock | 0mph | 0mph | -| Toyota | Corolla 2017-19 | All | Stock5| 20mph6 | 0mph | +| Toyota | Corolla 2017-19 | All | Stock3| 20mph1 | 0mph | | Toyota | Corolla 2020 | All | openpilot | 0mph | 0mph | | Toyota | Corolla Hatchback 2019-20 | All | openpilot | 0mph | 0mph | | Toyota | Corolla Hybrid 2020 | All | openpilot | 0mph | 0mph | -| Toyota | Highlander 2017-19 | All | Stock5| 0mph | 0mph | -| Toyota | Highlander Hybrid 2017-19 | All | Stock5| 0mph | 0mph | +| Toyota | Highlander 2017-19 | All | Stock3| 0mph | 0mph | +| Toyota | Highlander Hybrid 2017-19 | All | Stock3| 0mph | 0mph | | Toyota | Highlander 2020 | All | openpilot | 0mph | 0mph | -| Toyota | Prius 2016 | TSS-P | Stock5| 0mph | 0mph | -| Toyota | Prius 2017-19 | All | Stock5| 0mph | 0mph | -| Toyota | Prius Prime 2017-20 | All | Stock5| 0mph | 0mph | -| Toyota | Rav4 2016 | TSS-P | Stock5| 20mph6 | 0mph | -| Toyota | Rav4 2017-18 | All | Stock5| 20mph6 | 0mph | -| Toyota | Rav4 2019 | All | openpilot | 0mph | 0mph | -| Toyota | Rav4 Hybrid 2016 | TSS-P | Stock5| 0mph | 0mph | -| Toyota | Rav4 Hybrid 2017-18 | All | Stock5| 0mph | 0mph | +| Toyota | Prius 2016 | TSS-P | Stock3| 0mph | 0mph | +| Toyota | Prius 2017-19 | All | Stock3| 0mph | 0mph | +| Toyota | Prius Prime 2017-20 | All | Stock3| 0mph | 0mph | +| Toyota | Rav4 2016 | TSS-P | Stock3| 20mph1 | 0mph | +| Toyota | Rav4 2017-18 | All | Stock3| 20mph1 | 0mph | +| Toyota | Rav4 2019-20 | All | openpilot | 0mph | 0mph | +| Toyota | Rav4 Hybrid 2016 | TSS-P | Stock3| 0mph | 0mph | +| Toyota | Rav4 Hybrid 2017-18 | All | Stock3| 0mph | 0mph | | Toyota | Rav4 Hybrid 2019-20 | All | openpilot | 0mph | 0mph | -| Toyota | Sienna 2018 | All | Stock5| 0mph | 0mph | -| Volkswagen| Golf 2016-193 | Driver Assistance | Stock | 0mph | 0mph | +| Toyota | Sienna 2018 | All | Stock3| 0mph | 0mph | -1Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and open sourced [Hyundai giraffe](https://github.com/commaai/neo/tree/master/giraffe/hyundai), designed for the 2019 Sante Fe; pinout may differ for other Hyundai and Kia models.
-228mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
-3Requires a [custom connector](https://community.comma.ai/wiki/index.php/Volkswagen#Integration_at_R242_Camera) for the [car harness](https://comma.ai/shop/products/car-harness)
-42019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.
+1[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma](https://comma.ai).***
+22019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.
+3When disconnecting the Driver Support Unit (DSU), openpilot ACC will replace stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota). ***NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).***
+428mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
Community Maintained Cars and Features ------ | Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below | | ----------| ------------------------------| ------------------| -----------------| -------------------| -------------| -| Buick | Regal 20187 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Cadillac | ATS 20187 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Chevrolet | Malibu 20177 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Chevrolet | Volt 2017-187 | Adaptive Cruise | openpilot | 0mph | 7mph | -| GMC | Acadia Denali 20187| Adaptive Cruise | openpilot | 0mph | 7mph | -| Holden | Astra 20177 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Buick | Regal 20181 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Cadillac | ATS 20181 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Chevrolet | Malibu 20171 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Chevrolet | Volt 2017-181 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Chrysler | Pacifica 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph | +| Chrysler | Pacifica 2020 | Adaptive Cruise | Stock | 0mph | 39mph | +| Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph | +| Chrysler | Pacifica Hybrid 2019-20 | Adaptive Cruise | Stock | 0mph | 39mph | +| GMC | Acadia Denali 20183| Adaptive Cruise | openpilot | 0mph | 7mph | +| Holden | Astra 20171 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Hyundai | Elantra 2017-192 | SCC + LKAS | Stock | 19mph | 34mph | +| Hyundai | Genesis 2015-162 | SCC + LKAS | Stock | 19mph | 37mph | +| Hyundai | Ioniq 20172 | SCC + LKAS | Stock | 0mph | 32mph | +| Hyundai | Ioniq 2019 EV2 | SCC + LKAS | Stock | 0mph | 32mph | +| Hyundai | Kona 2017-192 | SCC + LKAS | Stock | 22mph | 0mph | +| Hyundai | Kona 2019 EV2 | SCC + LKAS | Stock | 0mph | 0mph | +| Hyundai | Palisade 20202 | All | Stock | 0mph | 0mph | +| Hyundai | Santa Fe 20192 | All | Stock | 0mph | 0mph | +| Hyundai | Sonata 20202 | All | Stock | 0mph | 0mph | +| Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Stock | 0mph | 9mph | +| Jeep | Grand Cherokee 2019 | Adaptive Cruise | Stock | 0mph | 39mph | +| Kia | Optima 20192 | SCC + LKAS | Stock | 0mph | 0mph | +| Kia | Sorento 20182 | All | Stock | 0mph | 0mph | +| Kia | Stinger 20182 | SCC + LKAS | Stock | 0mph | 0mph | +| Nissan | Leaf 2019 | Propilot | Stock | 0mph | 0mph | +| Nissan | X-Trail 2018 | Propilot | Stock | 0mph | 0mph | +| Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph | +| Subaru | Impreza 2018-20 | EyeSight | Stock | 0mph | 0mph | +| Volkswagen| Golf 2016-193 | Driver Assistance | Stock | 0mph | 0mph | + +1Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and [community built giraffe](https://zoneos.com/volt/). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).***
+2Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and open sourced [Hyundai giraffe](https://github.com/commaai/neo/tree/master/giraffe/hyundai), designed for the 2019 Sante Fe; pinout may differ for other Hyundai and Kia models.
+3Requires a [custom connector](https://community.comma.ai/wiki/index.php/Volkswagen#Integration_at_R242_Camera) for the [car harness](https://comma.ai/shop/products/car-harness)
-5When disconnecting the Driver Support Unit (DSU), openpilot ACC will replace stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota). ***NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).***
-6[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma](https://comma.ai).***
-7Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and [community built giraffe](https://zoneos.com/volt/). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).***
+Although it's not upstream, there's a community of people getting openpilot to run on Tesla's [here](https://tinkla.us/) Community Maintained Cars and Features are not verified by comma to meet our [safety model](SAFETY.md). Be extra cautious using them. They are only available after enabling the toggle in `Settings->Developer->Enable Community Features`. +To promote a car from community maintained, it must meet a few requirements. We must own one from the brand, we must sell the harness for it, has full ISO26262 in both panda and openpilot, there must be a path forward for longitudinal control, it must have AEB still enabled, and it must support fingerprinting 2.0 + Installation Instructions ------ -Install openpilot on a EON by entering ``https://openpilot.comma.ai`` during the installer setup. +Install openpilot on an EON or comma two by entering ``https://openpilot.comma.ai`` during the installer setup. -Follow this [video instructions](https://youtu.be/3nlkomHathI) to properly mount the EON on the windshield. Note: openpilot features an automatic pose calibration routine and openpilot performance should not be affected by small pitch and yaw misalignments caused by imprecise EON mounting. +Follow these [video instructions](https://youtu.be/3nlkomHathI) to properly mount the device on the windshield. Note: openpilot features an automatic pose calibration routine and openpilot performance should not be affected by small pitch and yaw misalignments caused by imprecise device mounting. Before placing the device on your windshield, check the state and local laws and ordinances where you drive. Some state laws prohibit or restrict the placement of objects on the windshield of a motor vehicle. @@ -174,7 +194,7 @@ Many factors can impact the performance of openpilot ALC and openpilot LDW, caus * Poor visibility (heavy rain, snow, fog, etc.) or weather conditions that may interfere with sensor operation. * The road facing camera is obstructed, covered or damaged by mud, ice, snow, etc. * Obstruction caused by applying excessive paint or adhesive products (such as wraps, stickers, rubber coating, etc.) onto the vehicle. -* The EON is mounted incorrectly. +* The device is mounted incorrectly. * When in sharp curves, like on-off ramps, intersections etc...; openpilot is designed to be limited in the amount of steering torque it can produce. * In the presence of restricted lanes or construction zones. * When driving on highly banked roads or in presence of strong cross-wind. @@ -194,7 +214,7 @@ Many factors can impact the performance of openpilot ACC and openpilot FCW, caus * Poor visibility (heavy rain, snow, fog, etc.) or weather conditions that may interfere with sensor operation. * The road facing camera or radar are obstructed, covered, or damaged by mud, ice, snow, etc. * Obstruction caused by applying excessive paint or adhesive products (such as wraps, stickers, rubber coating, etc.) onto the vehicle. -* The EON is mounted incorrectly. +* The device is mounted incorrectly. * Approaching a toll booth, a bridge or a large metal plate. * When driving on roads with pedestrians, cyclists, etc... * In presence of traffic signs or stop lights, which are not detected by openpilot at this time. @@ -212,13 +232,13 @@ The list above does not represent an exhaustive list of situations that may inte Limitations of openpilot DM ------ -openpilot DM should not be considered an exact measurements of the status of alertness of the driver. +openpilot DM should not be considered an exact measurement of the alertness of the driver. Many factors can impact the performance of openpilot DM, causing it to be unable to function as intended. These include, but are not limited to: * Low light conditions, such as driving at night or in dark tunnels. * Bright light (due to oncoming headlights, direct sunlight, etc.). -* The driver face is partially or completely outside field of view of the driver facing camera. +* The driver's face is partially or completely outside field of view of the driver facing camera. * Right hand driving vehicles. * The driver facing camera is obstructed, covered, or damaged. @@ -240,7 +260,7 @@ Safety and Testing ---- * openpilot observes ISO26262 guidelines, see [SAFETY.md](SAFETY.md) for more detail. -* openpilot has software in the loop [tests](run_docker_tests.sh) that run on every commit. +* openpilot has software in the loop [tests](.github/workflows/test.yaml) that run on every commit. * The safety model code lives in panda and is written in C, see [code rigor](https://github.com/commaai/panda#code-rigor) for more details. * panda has software in the loop [safety tests](https://github.com/commaai/panda/tree/master/tests/safety). * Internally, we have a hardware in the loop Jenkins test suite that builds and unit tests the various processes. @@ -267,15 +287,15 @@ Directory Structure ------ . ├── apk # The apk files used for the UI - ├── cereal # The messaging spec and libs used for all logs on EON + ├── cereal # The messaging spec and libs used for all logs ├── common # Library like functionality we've developed here ├── installer/updater # Manages auto-updates of openpilot ├── opendbc # Files showing how to interpret data from cars ├── panda # Code used to communicate on CAN - ├── phonelibs # Libraries used on EON - ├── pyextra # Libraries used on EON + ├── phonelibs # Libraries used on NEOS devices + ├── pyextra # Libraries used on NEOS devices └── selfdrive # Code needed to drive the car - ├── assets # Fonts and images for UI + ├── assets # Fonts, images, and sounds for UI ├── athena # Allows communication with the app ├── boardd # Daemon to talk to the board ├── camerad # Driver to capture images from the camera sensors @@ -289,7 +309,7 @@ Directory Structure ├── modeld # Driving and monitoring model runners ├── proclogd # Logs information from proc ├── sensord # IMU / GPS interface code - ├── tests # Unit tests, system tests and a car simulator + ├── test # Unit tests, system tests and a car simulator └── ui # The UI To understand how the services interact, see `cereal/service_list.yaml`. diff --git a/RELEASES.md b/RELEASES.md index 542c7a27b88e18..7e2ccdcd65d963 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,11 @@ +Version 0.7.5 (2020-xx-xx) +======================== +* Right-Hand Drive support for both driving and driver monitoring! +* New driving model: improved at sharp turns and lead speed estimation +* New driver monitoring model: overall improvement on comma two +* Driver camera preview in settings to improve mounting position +* 2019 Nissan X-Trail and 2018 Nissan Leaf support thanks to avolmensky! + Version 0.7.4 (2020-03-20) ======================== * New driving model: improved lane changes and lead car detection diff --git a/SConstruct b/SConstruct index 5944541b5b8155..ce7ceb92e4a829 100644 --- a/SConstruct +++ b/SConstruct @@ -12,34 +12,89 @@ AddOption('--asan', help='turn on ASAN') arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() +is_tbp = os.path.isfile('/data/tinkla_buddy_pro') +if arch == "aarch64" and is_tbp: + arch = "jarch64" if platform.system() == "Darwin": arch = "Darwin" +if arch == "aarch64" and (not os.path.isdir("/system")): + arch = "larch64" -if arch == "aarch64": +webcam = bool(ARGUMENTS.get("use_webcam", 0)) + +if arch == "aarch64" or arch == "larch64": lenv = { "LD_LIBRARY_PATH": '/data/data/com.termux/files/usr/lib', "PATH": os.environ['PATH'], - "ANDROID_DATA": os.environ['ANDROID_DATA'], - "ANDROID_ROOT": os.environ['ANDROID_ROOT'], } + if arch == "aarch64": + # android + lenv["ANDROID_DATA"] = os.environ['ANDROID_DATA'] + lenv["ANDROID_ROOT"] = os.environ['ANDROID_ROOT'] + cpppath = [ "#phonelibs/opencl/include", + "#phonelibs/snpe/include", ] + libpath = [ - "#phonelibs/snpe/aarch64-android-clang3.8", "/usr/lib", "/data/data/com.termux/files/usr/lib", "/system/vendor/lib64", "/system/comma/usr/lib", - "#phonelibs/nanovg", - "#phonelibs/libyuv/lib", + "#phonelibs/nanovg", ] - cflags = ["-DQCOM", "-mcpu=cortex-a57"] - cxxflags = ["-DQCOM", "-mcpu=cortex-a57"] - - rpath = ["/system/vendor/lib64"] + if arch == "larch64": + cpppath += ["#phonelibs/capnp-cpp/include", "#phonelibs/capnp-c/include"] + libpath += ["#phonelibs/snpe/larch64"] + libpath += ["#phonelibs/libyuv/larch64/lib"] + libpath += ["#external/capnparm/lib", "/usr/lib/aarch64-linux-gnu"] + cflags = ["-DQCOM2", "-mcpu=cortex-a57"] + cxxflags = ["-DQCOM2", "-mcpu=cortex-a57"] + rpath = ["/usr/local/lib"] + else: + libpath += ["#phonelibs/snpe/aarch64"] + libpath += ["#phonelibs/libyuv/lib"] + cflags = ["-DQCOM", "-mcpu=cortex-a57"] + cxxflags = ["-DQCOM", "-mcpu=cortex-a57"] + rpath = ["/system/vendor/lib64"] +elif arch == "jarch64": + webcam=True + lenv = { + "LD_LIBRARY_PATH": '/usr/lib:/usr/local/lib/:/usr/lib/aarch64-linux-gnu', + "PATH": os.environ['PATH'], + "ANDROID_DATA": "/data", + "ANDROID_ROOT": "/", + } + cflags = [] + cxxflags = [] + cpppath = [ + "/usr/local/include", + "/usr/local/include/opencv4", + "/usr/include/khronos-api", + "/usr/include", + "#phonelibs/snpe/include", + "/usr/include/aarch64-linux-gnu", + ] + libpath = [ + "/usr/local/lib", + "/usr/lib/aarch64-linux-gnu", + "/usr/lib", + "/data/op_rk3399_setup/external/snpe/lib/lib", + "/data/data/com.termux/files/usr/lib", + "#phonelibs/nanovg", + "/data/op_rk3399_setup/support_files/lib", + ] + rpath = ["/usr/local/lib", + "/usr/lib/aarch64-linux-gnu", + "/usr/lib", + "/data/op_rk3399_setup/external/snpe/lib/lib", + "/data/op_rk3399_setup/support_files/lib", + "cereal", + "selfdrive/common", + ] else: lenv = { "PATH": "#external/bin:" + os.environ['PATH'], @@ -49,6 +104,7 @@ else: "#phonelibs/capnp-c/include", "#phonelibs/zmq/x64/include", "#external/tensorflow/include", + "#phonelibs/snpe/include", ] if arch == "Darwin": @@ -117,13 +173,12 @@ env = Environment( "#phonelibs/json11", "#phonelibs/eigen", "#phonelibs/curl/include", - "#phonelibs/opencv/include", + #"#phonelibs/opencv/include", "#phonelibs/libgralloc/include", "#phonelibs/android_frameworks_native/include", "#phonelibs/android_hardware_libhardware/include", "#phonelibs/android_system_core/include", "#phonelibs/linux/include", - "#phonelibs/snpe/include", "#phonelibs/nanovg", "#selfdrive/common", "#selfdrive/camerad", @@ -175,10 +230,14 @@ def abspath(x): # rpath works elsewhere return x[0].path.rsplit("/", 1)[1][:-3] -#zmq = 'zmq' # still needed for apks -zmq = FindFile("libzmq.a", libpath) -Export('env', 'arch', 'zmq', 'SHARED') +if arch == 'larch64': + zmq = 'zmq' +else: + zmq = FindFile("libzmq.a", libpath) +if is_tbp: + zmq = FindFile("libzmq.so", libpath) +Export('env', 'arch', 'zmq', 'SHARED', 'webcam') # cereal and messaging are shared with the system SConscript(['cereal/SConscript']) @@ -215,6 +274,7 @@ if arch != "Darwin": SConscript(['selfdrive/controls/lib/cluster/SConscript']) SConscript(['selfdrive/controls/lib/lateral_mpc/SConscript']) SConscript(['selfdrive/controls/lib/longitudinal_mpc/SConscript']) +SConscript(['selfdrive/controls/lib/longitudinal_mpc_model/SConscript']) SConscript(['selfdrive/boardd/SConscript']) SConscript(['selfdrive/proclogd/SConscript']) diff --git a/cereal/SConscript b/cereal/SConscript index e0c1b2b52c16a4..166232ad4684e0 100644 --- a/cereal/SConscript +++ b/cereal/SConscript @@ -1,31 +1,36 @@ Import('env', 'arch', 'zmq') - gen_dir = Dir('gen') messaging_dir = Dir('messaging') # TODO: remove src-prefix and cereal from command string. can we set working directory? env.Command(["gen/c/include/c++.capnp.h", "gen/c/include/java.capnp.h"], [], "mkdir -p " + gen_dir.path + "/c/include && touch $TARGETS") env.Command( - ['gen/c/car.capnp.c', 'gen/c/log.capnp.c', 'gen/c/car.capnp.h', 'gen/c/log.capnp.h'], - ['car.capnp', 'log.capnp'], + ['gen/c/car.capnp.c', 'gen/c/log.capnp.c', 'gen/c/ui.capnp.c', 'gen/c/tinkla.capnp.c', 'gen/c/tesla.capnp.c', 'gen/c/car.capnp.h', 'gen/c/log.capnp.h', 'gen/c/ui.capnp.h', 'gen/c/tinkla.capnp.h', 'gen/c/tesla.capnp.h'], + ['car.capnp', 'log.capnp', 'tesla.capnp', 'tinkla.capnp', 'ui.capnp'], 'capnpc $SOURCES --src-prefix=cereal -o c:' + gen_dir.path + '/c/') env.Command( - ['gen/cpp/car.capnp.c++', 'gen/cpp/log.capnp.c++', 'gen/cpp/car.capnp.h', 'gen/cpp/log.capnp.h'], - ['car.capnp', 'log.capnp'], + ['gen/cpp/car.capnp.c++', 'gen/cpp/log.capnp.c++', 'gen/cpp/ui.capnp.c++', 'gen/cpp/tinkla.capnp.c++', 'gen/cpp/tesla.capnp.c++', 'gen/cpp/car.capnp.h', 'gen/cpp/log.capnp.h', 'gen/cpp/ui.capnp.h', 'gen/cpp/tinkla.capnp.h', 'gen/cpp/tesla.capnp.h'], + ['car.capnp', 'log.capnp', 'tesla.capnp', 'tinkla.capnp', 'ui.capnp'], 'capnpc $SOURCES --src-prefix=cereal -o c++:' + gen_dir.path + '/cpp/') import shutil if shutil.which('capnpc-java'): env.Command( - ['gen/java/Car.java', 'gen/java/Log.java'], - ['car.capnp', 'log.capnp'], + ['gen/java/Car.java', 'gen/java/Log.java', 'gen/java/Tesla.java', 'gen/java/Tinkla.java', 'gen/java/Ui.java'], + ['car.capnp', 'log.capnp', 'tesla.capnp','tinkla.capnp', 'ui.capnp'], 'capnpc $SOURCES --src-prefix=cereal -o java:' + gen_dir.path + '/java/') # TODO: remove non shared cereal and messaging cereal_objects = env.SharedObject([ 'gen/c/car.capnp.c', 'gen/c/log.capnp.c', + 'gen/c/tesla.capnp.c', + 'gen/c/ui.capnp.c', + 'gen/c/tinkla.capnp.c', 'gen/cpp/car.capnp.c++', 'gen/cpp/log.capnp.c++', + 'gen/cpp/tesla.capnp.c++', + 'gen/cpp/ui.capnp.c++', + 'gen/cpp/tinkla.capnp.c++', ]) env.Library('cereal', cereal_objects) diff --git a/cereal/SConstruct b/cereal/SConstruct index a72286b279bd74..7572b571cae8d0 100644 --- a/cereal/SConstruct +++ b/cereal/SConstruct @@ -3,6 +3,9 @@ import subprocess zmq = 'zmq' arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() +is_tbp = os.path.isfile('/data/tinkla_buddy_pro') +if arch == "aarch64" and is_tbp: + arch = "jarch64" cereal_dir = Dir('.') diff --git a/cereal/car.capnp b/cereal/car.capnp index 695f4a91bc47fd..bb0577ac6d9b83 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -91,6 +91,10 @@ struct CarEvent @0x9b1657f34caf3ad3 { carUnrecognized @66; radarCommIssue @67; driverMonitorLowAcc @68; + invalidLkasSetting @69; + speedTooHigh @70; + laneChangeBlocked @71; + relayMalfunction @72; } } @@ -151,7 +155,7 @@ struct CarState { # which packets this state came from canMonoTimes @12: List(UInt64); - + # blindspot sensors leftBlindspot @33 :Bool; # Is there something blocking the left lane change rightBlindspot @34 :Bool; # Is there something blocking the right lane change @@ -309,6 +313,7 @@ struct CarControl { chimeWarning2 @5; chimeWarningRepeat @6; chimePrompt @7; + chimeWarning2Repeat @8; } } } @@ -379,6 +384,7 @@ struct CarParams { radarTimeStep @45: Float32 = 0.05; # time delta between radar updates, 20Hz is very standard communityFeature @46: Bool; # true if a community maintained feature is detected fingerprintSource @49: FingerprintSource; + networkLocation @50 :NetworkLocation; # Where Panda/C2 is integrated into the car's CAN network struct LateralParams { torqueBP @0 :List(Int32); @@ -456,8 +462,9 @@ struct CarParams { enum TransmissionType { unknown @0; - automatic @1; - manual @2; + automatic @1; # Traditional auto, including DSG + manual @2; # True "stick shift" only + direct @3; # Electric vehicle or other direct drive } struct CarFw { @@ -496,4 +503,9 @@ struct CarParams { fw @1; fixed @2; } + + enum NetworkLocation { + fwdCamera @0; # Standard/default integration at LKAS camera + gateway @1; # Integration at vehicle's CAN gateway + } } diff --git a/cereal/log.capnp b/cereal/log.capnp index 01df744e3ee6d7..3744c46126f7fb 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -129,6 +129,7 @@ struct FrameData { gainFrac @15 :Float32; focusVal @16 :List(Int16); focusConf @17 :List(UInt8); + sharpnessScore @18 :List(UInt16); frameType @7 :FrameType; timestampSof @8 :UInt64; @@ -352,6 +353,25 @@ struct HealthData { enum FaultType { relayMalfunction @0; + unusedInterruptHandled @1; + interruptRateCan1 @2; + interruptRateCan2 @3; + interruptRateCan3 @4; + interruptRateTach @5; + interruptRateGmlan @6; + interruptRateInterrupts @7; + interruptRateSpiDma @8; + interruptRateSpiCs @9; + interruptRateUart1 @10; + interruptRateUart2 @11; + interruptRateUart3 @12; + interruptRateUart5 @13; + interruptRateUartDma @14; + interruptRateUsb @15; + interruptRateTim1 @16; + interruptRateTim3 @17; + registerDivergent @18; + # Update max fault type in boardd when adding faults } enum HwType { @@ -805,7 +825,7 @@ struct PathPlan { struct LiveLocationKalman { - # More info on reference frames: + # More info on reference frames: # https://github.com/commaai/openpilot/tree/master/common/transformations positionECEF @0 : Measurement; @@ -821,10 +841,10 @@ struct LiveLocationKalman { orientationECEF @6 : Measurement; orientationNED @7 : Measurement; angularVelocityDevice @8 : Measurement; - + # orientationNEDCalibrated transforms to rot matrix: NED_from_calibrated orientationNEDCalibrated @9 : Measurement; - + # Calibrated frame is simply device frame # aligned with the vehicle velocityCalibrated @10 : Measurement; @@ -835,7 +855,7 @@ struct LiveLocationKalman { gpsTimeOfWeek @14 :Float64; status @15 :Status; unixTimestampMillis @16 :Int64; - + enum Status { uninitialized @0; uncalibrated @1; @@ -1775,12 +1795,14 @@ struct UiLayoutState { activeApp @0 :App; sidebarCollapsed @1 :Bool; mapEnabled @2 :Bool; + mockEngaged @3 :Bool; enum App { home @0; music @1; nav @2; settings @3; + none @4; } } @@ -1879,6 +1901,7 @@ struct DMonitoringState { awarenessPassive @12 :Float32; isLowStd @13 :Bool; hiStdCount @14 :UInt32; + isPreview @15 :Bool; } struct Boot { @@ -1971,7 +1994,7 @@ struct Event { sendcan @17 :List(CanData); logMessage @18 :Text; liveCalibration @19 :LiveCalibrationData; - androidLogEntry @20 :AndroidLogEntry; + androidLog @20 :AndroidLogEntry; gpsLocation @21 :GpsLocationData; carState @22 :Car.CarState; carControl @23 :Car.CarControl; @@ -2025,4 +2048,4 @@ struct Event { liveLocationKalman @72 :LiveLocationKalman; sentinel @73 :Sentinel; } -} \ No newline at end of file +} diff --git a/cereal/messaging/__init__.py b/cereal/messaging/__init__.py index b53d6fb7ab6247..01de59f7eccfc7 100644 --- a/cereal/messaging/__init__.py +++ b/cereal/messaging/__init__.py @@ -142,6 +142,7 @@ def __init__(self, services, ignore_alive=None, addr="127.0.0.1"): self.data = {} self.logMonoTime = {} self.valid = {} + self.valid_cnt = {s: 0 for s in services} if ignore_alive is not None: self.ignore_alive = ignore_alive @@ -162,6 +163,10 @@ def __init__(self, services, ignore_alive=None, addr="127.0.0.1"): self.data[s] = getattr(data, s) self.logMonoTime[s] = 0 self.valid[s] = data.valid + if data.valid: + self.valid_cnt[s] = 0 + else: + self.valid_cnt[s] +=1 def __getitem__(self, s): return self.data[s] @@ -211,6 +216,60 @@ def all_alive_and_valid(self, service_list=None): service_list = self.alive.keys() return self.all_alive(service_list=service_list) and self.all_valid(service_list=service_list) + def all_alive_with_info(self, service_list=None): + """Returns alive state for tracked processes. + Args: + service_list (list): Optional service list. + Returns: + tuple: areAllAlive, processName, count + """ + if service_list is None: # check all + service_list = self.alive.keys() + areAllAlive = True + processName = "" + count = 0 + for s in service_list: + if not self.alive[s]: + areAllAlive = False + processName = s + count = self.valid_cnt[s] + break + return (areAllAlive, processName, count) + + def all_valid_with_info(self, service_list=None): + """Returns valid state for tracked processes. + Args: + service_list (list): Optional service list. + Returns: + tuple: areAllValid, processName, count + """ + if service_list is None: # check all + service_list = self.valid.keys() + areAllValid = True + processName = "" + count = 0 + for s in service_list: + if not self.valid[s]: + areAllValid = False + processName = s + count = self.valid_cnt[s] + break + return (areAllValid, processName, count) + + def all_alive_and_valid_with_info(self, service_list=None): + """Returns alive and valid state for tracked processes. + Args: + service_list (list): Optional service list. + Returns: + tuple: areAllAlive, areAllValid, aliveProcessName, aliveCount, validProcessName, validCount + """ + if service_list is None: # check all + service_list = self.alive.keys() + areAllAlive, aliveProcessName, aliveCount = self.all_alive(service_list=service_list) + areAllValid, validProcessName, validCount = self.all_valid(service_list=service_list) + return (areAllAlive, areAllValid, aliveProcessName, aliveCount, validProcessName, ) + + class PubMaster(): def __init__(self, services): @@ -223,3 +282,4 @@ def send(self, s, dat): if not isinstance(dat, bytes): dat = dat.to_bytes() self.sock[s].send(dat) + diff --git a/cereal/messaging/messaging_pyx_setup.py b/cereal/messaging/messaging_pyx_setup.py index a763d89f862586..f5962d0c2b2310 100644 --- a/cereal/messaging/messaging_pyx_setup.py +++ b/cereal/messaging/messaging_pyx_setup.py @@ -34,7 +34,8 @@ def get_ext_filename(self, ext_name): libraries = ['zmq'] ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg -if ARCH == "aarch64": +if ARCH == "aarch64" and os.path.isdir("/system"): + # android extra_compile_args += ["-Wno-deprecated-register"] libraries += ['gnustl_shared'] diff --git a/cereal/service_list.yaml b/cereal/service_list.yaml index 2b306f70df4e8d..5f157c8b42d8a7 100644 --- a/cereal/service_list.yaml +++ b/cereal/service_list.yaml @@ -76,6 +76,7 @@ carEvents: [8070, true, 1., 1] carParams: [8071, true, 0.02, 1] frontFrame: [8072, true, 10.] dMonitoringState: [8073, true, 5., 1] +offroadLayout: [8074, false, 0.] testModel: [8040, false, 0.] testLiveLocation: [8045, false, 0.] @@ -123,19 +124,31 @@ ahbInfo: [8212, false, 0.] # gpsd -- publishes EON's gps # publishes: gpsNMEA -# visiond -- talks to the cameras, runs the model, saves the videos -# publishes: frame, model, driverMonitoring, cameraOdometry, thumbnail +# camerad -- publishes camera frames +# publishes: frame, frontFrame, thumbnail +# subscribes: driverState + +# dmonitoringmodeld -- runs face detection on camera frames +# publishes: driverState # **** stateful data transformers **** +# modeld -- runs & publishes the model +# publishes: model, cameraOdometry +# subscribes: liveCalibration, pathPlan + # plannerd -- decides where to drive the car # subscribes: carState, model, radarState, controlsState, liveParameters # publishes: plan, pathPlan, liveMpc, liveLongitudinalMpc # controlsd -- drives the car by sending CAN messages to panda -# subscribes: can, thermal, health, plan, pathPlan, driverMonitoring, liveCalibration +# subscribes: can, thermal, health, plan, pathPlan, dMonitoringState, liveCalibration, model # publishes: carState, carControl, sendcan, controlsState, carEvents, carParams +# dmonitoringd -- processes driver monitoring data and publishes driver awareness +# subscribes: driverState, liveCalibration, carState, model, gpsLocation +# publishes: dMonitoringState + # radard -- processes the radar and vision data # subscribes: can, controlsState, model, liveParameters # publishes: radarState, liveTracks diff --git a/cereal/tesla.capnp b/cereal/tesla.capnp index b29d4617904d4e..56134ef183c8e8 100644 --- a/cereal/tesla.capnp +++ b/cereal/tesla.capnp @@ -7,7 +7,7 @@ $Java.outerClassname("Alca"); @0xca61a35dedbd6327; -const interfaceVersion :Float32 = 4.0; +#const interfaceVersion :Float32 = 4.0; struct ALCAStatus { # ALCA info diff --git a/common/basedir.py b/common/basedir.py index e928ded4c4d681..c5f2e513575c95 100644 --- a/common/basedir.py +++ b/common/basedir.py @@ -2,9 +2,13 @@ BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")) from common.android import ANDROID +is_tbp = os.path.isfile('/data/tinkla_buddy_pro') if ANDROID: PERSIST = "/persist" PARAMS = "/data/params" +elif is_tbp: + PERSIST = "/data/params/persist" + PARAMS = "/data/params" else: PERSIST = os.path.join(BASEDIR, "persist") PARAMS = os.path.join(BASEDIR, "persist", "params") diff --git a/common/filter_simple.py b/common/filter_simple.py index a3206db1cc5684..fa291bcb6fdba6 100644 --- a/common/filter_simple.py +++ b/common/filter_simple.py @@ -6,5 +6,4 @@ def __init__(self, x0, ts, dt): def update(self, x): self.x = (1. - self.k) * self.x + self.k * x - - + return self.x diff --git a/common/params.py b/common/params.py index 22ee0aa55b7a06..38296ad1c856d2 100755 --- a/common/params.py +++ b/common/params.py @@ -45,6 +45,7 @@ class TxType(Enum): PERSISTENT = 1 CLEAR_ON_MANAGER_START = 2 CLEAR_ON_PANDA_DISCONNECT = 3 + CLEAR_ON_CAR_START = 4 class UnknownKeyName(Exception): @@ -52,10 +53,10 @@ class UnknownKeyName(Exception): keys = { - "AccessToken": [TxType.PERSISTENT], + "AccessToken": [TxType.CLEAR_ON_MANAGER_START], #BB what happends if no int connection? "AthenadPid": [TxType.PERSISTENT], "CalibrationParams": [TxType.PERSISTENT], - "CarParams": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], + "CarParams": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], #BB we had [TxType.CLEAR_ON_CAR_START], "CarParamsCache": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], "CarVin": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], "CommunityFeaturesToggle": [TxType.PERSISTENT], @@ -70,6 +71,7 @@ class UnknownKeyName(Exception): "GithubSshKeys": [TxType.PERSISTENT], "HasAcceptedTerms": [TxType.PERSISTENT], "HasCompletedSetup": [TxType.PERSISTENT], + "IsDriverViewEnabled": [TxType.CLEAR_ON_MANAGER_START], "IsLdwEnabled": [TxType.PERSISTENT], "IsGeofenceEnabled": [TxType.PERSISTENT], "IsMetric": [TxType.PERSISTENT], @@ -78,6 +80,7 @@ class UnknownKeyName(Exception): "IsTakingSnapshot": [TxType.CLEAR_ON_MANAGER_START], "IsUpdateAvailable": [TxType.CLEAR_ON_MANAGER_START], "IsUploadRawEnabled": [TxType.PERSISTENT], + "LastAthenaPingTime": [TxType.PERSISTENT], "LastUpdateTime": [TxType.PERSISTENT], "LimitSetSpeed": [TxType.PERSISTENT], "LimitSetSpeedNeural": [TxType.PERSISTENT], @@ -106,6 +109,13 @@ class UnknownKeyName(Exception): "Offroad_PandaFirmwareMismatch": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], "Offroad_InvalidTime": [TxType.CLEAR_ON_MANAGER_START], "Offroad_IsTakingSnapshot": [TxType.CLEAR_ON_MANAGER_START], + "DriverUsbCameraID": [TxType.PERSISTENT], + "RoadUsbCameraID": [TxType.PERSISTENT], + "DriverUsbCameraFx": [TxType.PERSISTENT], + "DriverUsbCameraFlip": [TxType.PERSISTENT], + "RoadUsbCameraFx": [TxType.PERSISTENT], + "RoadUsbCameraFlip": [TxType.PERSISTENT], + "TeslaModel": [TxType.PERSISTENT], } @@ -355,6 +365,9 @@ def manager_start(self): def panda_disconnect(self): self._clear_keys_with_type(TxType.CLEAR_ON_PANDA_DISCONNECT) + + def car_start(self): + self._clear_keys_with_type(TxType.CLEAR_ON_CAR_START) def delete(self, key): with self.transaction(write=True) as txn: diff --git a/common/realtime.py b/common/realtime.py index d97eb88b28f677..c21222e88c2285 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -55,12 +55,10 @@ def remaining(self): return self._remaining # Maintain loop rate by calling this at the end of each loop - def keep_time(self, offset=0.): + def keep_time(self): lagged = self.monitor_time() if self._remaining > 0: time.sleep(self._remaining) - elif not offset == 0.: - self._next_frame_time += offset return lagged # this only monitor the cumulative lag, but does not enforce a rate diff --git a/common/spinner.py b/common/spinner.py index 3582d3feed030a..da8084037e7341 100644 --- a/common/spinner.py +++ b/common/spinner.py @@ -50,6 +50,9 @@ def __enter__(self): def update(self, _): pass + def close(self): + pass + def __exit__(self, type, value, traceback): pass diff --git a/common/stat_live.py b/common/stat_live.py index f392956efd83f0..e528c3deac5a82 100644 --- a/common/stat_live.py +++ b/common/stat_live.py @@ -53,7 +53,7 @@ def params_to_save(self): class RunningStatFilter(): def __init__(self, raw_priors=None, filtered_priors=None, max_trackable=-1): - self.raw_stat = RunningStat(raw_priors, max_trackable) + self.raw_stat = RunningStat(raw_priors, -1) self.filtered_stat = RunningStat(filtered_priors, max_trackable) def reset(self): diff --git a/common/text_window.py b/common/text_window.py new file mode 100755 index 00000000000000..0d94bc7470c789 --- /dev/null +++ b/common/text_window.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 +import os +import time +import subprocess +from common.basedir import BASEDIR + + +class TextWindow(): + def __init__(self, s): + try: + self.text_proc = subprocess.Popen(["./text", s], + stdin=subprocess.PIPE, + cwd=os.path.join(BASEDIR, "selfdrive", "ui", "text"), + close_fds=True) + except OSError: + self.text_proc = None + + def get_status(self): + if self.text_proc is not None: + self.text_proc.poll() + return self.text_proc.returncode + + return None + + def __enter__(self): + return self + + def close(self): + if self.text_proc is not None: + self.text_proc.terminate() + self.text_proc = None + + def wait_for_exit(self): + while True: + if self.get_status() == 1: + return + time.sleep(0.1) + + def __del__(self): + self.close() + + def __exit__(self, type, value, traceback): + self.close() + + +class FakeTextWindow(): + def __init__(self, s): + pass + + def get_status(self): + return 1 + + def wait_for_exit(self): + return + + def __enter__(self): + return self + + def update(self, _): + pass + + def __exit__(self, type, value, traceback): + pass + + +if __name__ == "__main__": + text = """Traceback (most recent call last): + File "./controlsd.py", line 608, in + main() + File "./controlsd.py", line 604, in main + controlsd_thread(sm, pm, logcan) + File "./controlsd.py", line 455, in controlsd_thread + 1/0 +ZeroDivisionError: division by zero""" + print(text) + + with TextWindow(text) as s: + for _ in range(100): + if s.get_status() == 1: + print("Got exit button") + break + time.sleep(0.1) + print("gone") diff --git a/common/transformations/camera.py b/common/transformations/camera.py index d8729e2f4b2c0c..5ca9eb57d5e75c 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -1,6 +1,5 @@ import numpy as np import common.transformations.orientation as orient -import math FULL_FRAME_SIZE = (1164, 874) W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1] @@ -147,81 +146,3 @@ def pretransform_from_calib(calib): camera_frame_from_calib_frame = get_camera_frame_from_calib_frame(camera_frame_from_road_frame) return np.linalg.inv(camera_frame_from_calib_frame) - -def transform_img(base_img, - augment_trans=np.array([0,0,0]), - augment_eulers=np.array([0,0,0]), - from_intr=eon_intrinsics, - to_intr=eon_intrinsics, - output_size=None, - pretransform=None, - top_hacks=False, - yuv=False, - alpha=1.0, - beta=0, - blur=0): - import cv2 # pylint: disable=import-error - cv2.setNumThreads(1) - - if yuv: - base_img = cv2.cvtColor(base_img, cv2.COLOR_YUV2RGB_I420) - - size = base_img.shape[:2] - if not output_size: - output_size = size[::-1] - - cy = from_intr[1,2] - def get_M(h=1.22): - quadrangle = np.array([[0, cy + 20], - [size[1]-1, cy + 20], - [0, size[0]-1], - [size[1]-1, size[0]-1]], dtype=np.float32) - quadrangle_norm = np.hstack((normalize(quadrangle, intrinsics=from_intr), np.ones((4,1)))) - quadrangle_world = np.column_stack((h*quadrangle_norm[:,0]/quadrangle_norm[:,1], - h*np.ones(4), - h/quadrangle_norm[:,1])) - rot = orient.rot_from_euler(augment_eulers) - to_extrinsics = np.hstack((rot.T, -augment_trans[:,None])) - to_KE = to_intr.dot(to_extrinsics) - warped_quadrangle_full = np.einsum('jk,ik->ij', to_KE, np.hstack((quadrangle_world, np.ones((4,1))))) - warped_quadrangle = np.column_stack((warped_quadrangle_full[:,0]/warped_quadrangle_full[:,2], - warped_quadrangle_full[:,1]/warped_quadrangle_full[:,2])).astype(np.float32) - M = cv2.getPerspectiveTransform(quadrangle, warped_quadrangle.astype(np.float32)) - return M - - M = get_M() - if pretransform is not None: - M = M.dot(pretransform) - augmented_rgb = cv2.warpPerspective(base_img, M, output_size, borderMode=cv2.BORDER_REPLICATE) - - if top_hacks: - cyy = int(math.ceil(to_intr[1,2])) - M = get_M(1000) - if pretransform is not None: - M = M.dot(pretransform) - augmented_rgb[:cyy] = cv2.warpPerspective(base_img, M, (output_size[0], cyy), borderMode=cv2.BORDER_REPLICATE) - - # brightness and contrast augment - augmented_rgb = np.clip((float(alpha)*augmented_rgb + beta), 0, 255).astype(np.uint8) - - # gaussian blur - if blur > 0: - augmented_rgb = cv2.GaussianBlur(augmented_rgb,(blur*2+1,blur*2+1),cv2.BORDER_DEFAULT) - - if yuv: - augmented_img = cv2.cvtColor(augmented_rgb, cv2.COLOR_RGB2YUV_I420) - else: - augmented_img = augmented_rgb - return augmented_img - - -def yuv_crop(frame, output_size, center=None): - # output_size in camera coordinates so u,v - # center in array coordinates so row, column - import cv2 # pylint: disable=import-error - rgb = cv2.cvtColor(frame, cv2.COLOR_YUV2RGB_I420) - if not center: - center = (rgb.shape[0]/2, rgb.shape[1]/2) - rgb_crop = rgb[center[0] - output_size[1]/2: center[0] + output_size[1]/2, - center[1] - output_size[0]/2: center[1] + output_size[0]/2] - return cv2.cvtColor(rgb_crop, cv2.COLOR_RGB2YUV_I420) diff --git a/common/transformations/model.py b/common/transformations/model.py index c13cc7e38752da..c47c9e987ffdcd 100644 --- a/common/transformations/model.py +++ b/common/transformations/model.py @@ -44,24 +44,15 @@ # BIG model -BIGMODEL_INPUT_SIZE = (864, 288) +BIGMODEL_INPUT_SIZE = (1024, 512) BIGMODEL_YUV_SIZE = (BIGMODEL_INPUT_SIZE[0], BIGMODEL_INPUT_SIZE[1] * 3 // 2) bigmodel_zoom = 1. bigmodel_intrinsics = np.array( [[ eon_focal_length / bigmodel_zoom, 0. , 0.5 * BIGMODEL_INPUT_SIZE[0]], - [ 0. , eon_focal_length / bigmodel_zoom, 0.2 * BIGMODEL_INPUT_SIZE[1]], + [ 0. , eon_focal_length / bigmodel_zoom, 256+MEDMODEL_CY], [ 0. , 0. , 1.]]) - -bigmodel_border = np.array([ - [0,0,1], - [BIGMODEL_INPUT_SIZE[0], 0, 1], - [BIGMODEL_INPUT_SIZE[0], BIGMODEL_INPUT_SIZE[1], 1], - [0, BIGMODEL_INPUT_SIZE[1], 1], -]) - - model_frame_from_road_frame = np.dot(model_intrinsics, get_view_frame_from_road_frame(0, 0, 0, model_height)) @@ -72,6 +63,7 @@ get_view_frame_from_road_frame(0, 0, 0, model_height)) model_frame_from_bigmodel_frame = np.dot(model_intrinsics, np.linalg.inv(bigmodel_intrinsics)) +medmodel_frame_from_bigmodel_frame = np.dot(medmodel_intrinsics, np.linalg.inv(bigmodel_intrinsics)) # 'camera from model camera' def get_model_height_transform(camera_frame_from_road_frame, height): diff --git a/common/xattr.py b/common/xattr.py new file mode 100644 index 00000000000000..fa61b9e0fc5635 --- /dev/null +++ b/common/xattr.py @@ -0,0 +1,45 @@ +import os +from cffi import FFI + +# Workaround for the EON/termux build of Python having os.*xattr removed. +ffi = FFI() +ffi.cdef(""" +int setxattr(const char *path, const char *name, const void *value, size_t size, int flags); +ssize_t getxattr(const char *path, const char *name, void *value, size_t size); +ssize_t listxattr(const char *path, char *list, size_t size); +int removexattr(const char *path, const char *name); +""") +libc = ffi.dlopen(None) + +def setxattr(path, name, value, flags=0): + path = path.encode() + name = name.encode() + if libc.setxattr(path, name, value, len(value), flags) == -1: + raise OSError(ffi.errno, f"{os.strerror(ffi.errno)}: setxattr({path}, {name}, {value}, {flags})") + +def getxattr(path, name, size=128): + path = path.encode() + name = name.encode() + value = ffi.new(f"char[{size}]") + l = libc.getxattr(path, name, value, size) + if l == -1: + # errno 61 means attribute hasn't been set + if ffi.errno == 61: + return None + raise OSError(ffi.errno, f"{os.strerror(ffi.errno)}: getxattr({path}, {name}, {size})") + return ffi.buffer(value)[:l] + +def listxattr(path, size=128): + path = path.encode() + attrs = ffi.new(f"char[{size}]") + l = libc.listxattr(path, attrs, size) + if l == -1: + raise OSError(ffi.errno, f"{os.strerror(ffi.errno)}: listxattr({path}, {size})") + # attrs is b'\0' delimited values (so chop off trailing empty item) + return [a.decode() for a in ffi.buffer(attrs)[:l].split(b"\0")[0:-1]] + +def removexattr(path, name): + path = path.encode() + name = name.encode() + if libc.removexattr(path, name) == -1: + raise OSError(ffi.errno, f"{os.strerror(ffi.errno)}: removexattr({path}, {name})") diff --git a/installer/updater/update.json b/installer/updater/update.json index 169aa784cd8b97..6e2692bfad93cc 100644 --- a/installer/updater/update.json +++ b/installer/updater/update.json @@ -1,7 +1,7 @@ { - "ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-16e6e346e0e29ba9953d0ebd7185a7955b8e2261790f0e68aee74ced3028dfb8.zip", - "ota_hash": "16e6e346e0e29ba9953d0ebd7185a7955b8e2261790f0e68aee74ced3028dfb8", - "recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-4772f9348e04b560b9df87d6dea6c740fa8d62ea41a8db3842eec216f04e3110.img", - "recovery_len": 15922476, - "recovery_hash": "4772f9348e04b560b9df87d6dea6c740fa8d62ea41a8db3842eec216f04e3110" -} \ No newline at end of file + "ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-efdf7de63b1aef63d68301e6175930991bf9a5927d16ec6fcc69287e2ee7ca4a.zip", + "ota_hash": "efdf7de63b1aef63d68301e6175930991bf9a5927d16ec6fcc69287e2ee7ca4a", + "recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-97c27e6ed04ed6bb0608b845a2d4100912093f9380c3f2ba6b56bccd608e5f6e.img", + "recovery_len": 15861036, + "recovery_hash": "97c27e6ed04ed6bb0608b845a2d4100912093f9380c3f2ba6b56bccd608e5f6e" +} diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index a33deaafd61ff1..7e377c78897fa5 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -14,12 +14,27 @@ if [ -z "$PASSIVE" ]; then export PASSIVE="1" fi +. /data/openpilot/selfdrive/car/tesla/readconfig.sh STAGING_ROOT="/data/safe_staging" + function launch { # Wifi scan wpa_cli IFNAME=wlan0 SCAN + #BB here was to prevent the autoupdate; need to find another way + # # apply update + # if [ $do_auto_update == "True" ]; then + # if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then + # git reset --hard @{u} && + # git clean -xdf && + + # # Touch all files on release2 after checkout to prevent rebuild + # BRANCH=$(git rev-parse --abbrev-ref HEAD) + # if [[ "$BRANCH" == "release2" ]]; then + # touch ** + # fi + # Check to see if there's a valid overlay-based update available. Conditions # are as follows: # @@ -28,8 +43,10 @@ function launch { # switching branches/forks, which should not be overwritten. # 2. The FINALIZED consistent file has to exist, indicating there's an update # that completed successfully and synced to disk. + + - if [ -f "${BASEDIR}/.overlay_init" ]; then + if [ $do_auto_update == "True" ] && [ -f "${BASEDIR}/.overlay_init" ]; then find ${BASEDIR}/.git -newer ${BASEDIR}/.overlay_init | grep -q '.' 2> /dev/null if [ $? -eq 0 ]; then echo "${BASEDIR} has been modified, skipping overlay update installation" diff --git a/opendbc/can/common.cc b/opendbc/can/common.cc index 5c7efe4ed27341..68c3ee07326bc5 100644 --- a/opendbc/can/common.cc +++ b/opendbc/can/common.cc @@ -18,8 +18,19 @@ unsigned int toyota_checksum(unsigned int address, uint64_t d, int l) { d >>= 8; // remove checksum unsigned int s = l; - while (address) { s += address & 0xff; address >>= 8; } - while (d) { s += d & 0xff; d >>= 8; } + while (address) { s += address & 0xFF; address >>= 8; } + while (d) { s += d & 0xFF; d >>= 8; } + + return s & 0xFF; +} + +unsigned int subaru_checksum(unsigned int address, uint64_t d, int l) { + d >>= ((8-l)*8); // remove padding + + unsigned int s = 0; + while (address) { s += address & 0xFF; address >>= 8; } + l -= 1; // checksum is first byte + while (l) { s += d & 0xFF; d >>= 8; l -= 1; } return s & 0xFF; } @@ -106,6 +117,9 @@ unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l) { 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 0x30F: // SWA_01 Lane Change Assist (SpurWechselAssistent) + crc ^= (uint8_t[]){0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C}[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; diff --git a/opendbc/can/common.h b/opendbc/can/common.h index 4d0e14cbc92d98..841c0603be0bb2 100644 --- a/opendbc/can/common.h +++ b/opendbc/can/common.h @@ -13,6 +13,7 @@ // 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 subaru_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); diff --git a/opendbc/can/common.pxd b/opendbc/can/common.pxd index e07e0b35c70c43..00821104a40303 100644 --- a/opendbc/can/common.pxd +++ b/opendbc/can/common.pxd @@ -18,7 +18,8 @@ cdef extern from "common_dbc.h": PEDAL_CHECKSUM, PEDAL_COUNTER, VOLKSWAGEN_CHECKSUM, - VOLKSWAGEN_COUNTER + VOLKSWAGEN_COUNTER, + SUBARU_CHECKSUM cdef struct Signal: const char* name diff --git a/opendbc/can/common_dbc.h b/opendbc/can/common_dbc.h index ae6f443ec90cd3..317e80526ecffe 100644 --- a/opendbc/can/common_dbc.h +++ b/opendbc/can/common_dbc.h @@ -38,6 +38,7 @@ enum SignalType { PEDAL_COUNTER, VOLKSWAGEN_CHECKSUM, VOLKSWAGEN_COUNTER, + SUBARU_CHECKSUM, }; struct Signal { diff --git a/opendbc/can/dbc_template.cc b/opendbc/can/dbc_template.cc index f9540fcee9af16..439643a3e43cfc 100644 --- a/opendbc/can/dbc_template.cc +++ b/opendbc/can/dbc_template.cc @@ -29,6 +29,8 @@ const Signal sigs_{{address}}[] = { .type = SignalType::VOLKSWAGEN_CHECKSUM, {% elif checksum_type == "volkswagen" and sig.name == "COUNTER" %} .type = SignalType::VOLKSWAGEN_COUNTER, + {% elif checksum_type == "subaru" and sig.name == "CHECKSUM" %} + .type = SignalType::SUBARU_CHECKSUM, {% elif address in [512, 513] and sig.name == "CHECKSUM_PEDAL" %} .type = SignalType::PEDAL_CHECKSUM, {% elif address in [512, 513] and sig.name == "COUNTER_PEDAL" %} diff --git a/opendbc/can/packer.cc b/opendbc/can/packer.cc index fb81952768dd14..7efedfaf2309fe 100644 --- a/opendbc/can/packer.cc +++ b/opendbc/can/packer.cc @@ -99,6 +99,9 @@ uint64_t CANPacker::pack(uint32_t address, const std::vector &s // The correct fix is unclear but this works for the moment. unsigned int chksm = volkswagen_crc(address, ReverseBytes(ret), message_lookup[address].size); ret = set_value(ret, sig, chksm); + } else if (sig.type == SignalType::SUBARU_CHECKSUM) { + unsigned int chksm = subaru_checksum(address, ret, message_lookup[address].size); + ret = set_value(ret, sig, chksm); } else { //WARN("CHECKSUM signal type not valid\n"); } diff --git a/opendbc/can/process_dbc.py b/opendbc/can/process_dbc.py index a1c91a8978db58..c39184de4a3d20 100755 --- a/opendbc/can/process_dbc.py +++ b/opendbc/can/process_dbc.py @@ -46,6 +46,13 @@ def process(in_fn, out_fn): checksum_start_bit = 0 counter_start_bit = 0 little_endian = True + elif can_dbc.name.startswith(("subaru_")): + checksum_type = "subaru" + checksum_size = 8 + counter_size = None + checksum_start_bit = 0 + counter_start_bit = None + little_endian = True else: checksum_type = None checksum_size = None diff --git a/opendbc/tesla_can_epas.dbc b/opendbc/tesla_can_epas.dbc index abd3f1431cb4c8..b496cf23d230e9 100644 --- a/opendbc/tesla_can_epas.dbc +++ b/opendbc/tesla_can_epas.dbc @@ -45,7 +45,42 @@ BU_: APP DAS +BO_ 513 SDM1: 5 GTW + SG_ SDM_bcklPassStatus : 3|2@0+ (1,0) [0|3] "" NEO + SG_ SDM_bcklDrivStatus : 5|2@0+ (1,0) [0|3] "" NEO +BO_ 643 BODY_R1: 8 GTW + SG_ AirTemp_Insd : 47|8@0+ (0.25,0) [0|63.5] "C" NEO + SG_ AirTemp_Outsd : 63|8@0+ (0.5,-40) [-40|86.5] "C" NEO + SG_ Bckl_Sw_RL_Stat_SAM_R : 49|2@0+ (1,0) [-1|4] "" NEO + SG_ Bckl_Sw_RM_Stat_SAM_R : 53|2@0+ (1,0) [-1|4] "" NEO + SG_ Bckl_Sw_RR_Stat_SAM_R : 51|2@0+ (1,0) [-1|4] "" NEO + SG_ DL_RLtch_Stat : 9|2@0+ (1,0) [-1|4] "" NEO + SG_ DrRLtch_FL_Stat : 1|2@0+ (1,0) [-1|4] "" NEO + SG_ DrRLtch_FR_Stat : 3|2@0+ (1,0) [-1|4] "" NEO + SG_ DrRLtch_RL_Stat : 5|2@0+ (1,0) [-1|4] "" NEO + SG_ DrRLtch_RR_Stat : 7|2@0+ (1,0) [-1|4] "" NEO + SG_ EngHd_Stat : 11|2@0+ (1,0) [-1|4] "" NEO + SG_ LoBm_On_Rq : 32|1@0+ (1,0) [0|1] "" NEO + SG_ HiBm_On : 33|1@0+ (1,0) [0|1] "" NEO + SG_ Hrn_On : 26|1@0+ (1,0) [0|1] "" NEO + SG_ IrLmp_D_Lt_Flt : 34|1@0+ (1,0) [0|1] "" NEO + SG_ IrLmp_P_Rt_Flt : 35|1@0+ (1,0) [0|1] "" NEO + SG_ LgtSens_Twlgt : 18|3@0+ (1,0) [0|7] "Steps" NEO + SG_ LgtSens_SNA : 19|1@0+ (1,0) [0|1] "" NEO + SG_ LgtSens_Tunnel : 20|1@0+ (1,0) [0|1] "" NEO + SG_ LgtSens_Flt : 21|1@0+ (1,0) [0|1] "" NEO + SG_ LgtSens_Night : 22|1@0+ (1,0) [-1|2] "" NEO + SG_ ADL_LoBm_On_Rq : 23|1@0+ (1,0) [0|1] "" NEO + SG_ LoBm_D_Lt_Flt : 36|1@0+ (1,0) [0|1] "" NEO + SG_ LoBm_P_Rt_Flt : 37|1@0+ (1,0) [0|1] "" NEO + SG_ MPkBrk_Stat : 28|1@0+ (1,0) [-1|2] "" NEO + SG_ RevGr_Engg : 39|2@0+ (1,0) [-1|4] "" NEO + SG_ StW_Cond_Stat : 55|2@0+ (1,0) [-1|4] "" NEO + SG_ Term54_Actv : 27|1@0+ (1,0) [0|1] "" NEO + SG_ Trlr_Stat : 25|2@0+ (1,0) [-1|4] "" NEO + SG_ VTA_Alm_Actv : 13|1@0+ (1,0) [0|1] "" NEO + SG_ WprOutsdPkPosn : 29|1@0+ (1,0) [0|1] "" NEO BO_ 880 EPAS_sysStatus: 8 EPAS SG_ EPAS_currentTuneMode : 7|4@0+ (1,0) [8|15] "" NEO @@ -59,7 +94,23 @@ BO_ 880 EPAS_sysStatus: 8 EPAS SG_ EPAS_sysStatusChecksum : 63|8@0+ (1,0) [0|255] "" NEO SG_ EPAS_sysStatusCounter : 51|4@0+ (1,0) [0|15] "" NEO SG_ EPAS_torsionBarTorque : 19|12@0+ (0.01,-20.5) [0|0] "Nm" NEO - + +VAL_ 643 AirTemp_Insd 255 "SNA" ; +VAL_ 643 AirTemp_Outsd 254 "INIT" 255 "SNA" ; +VAL_ 643 Bckl_Sw_RL_Stat_SAM_R 2 "FLT" 1 "NOT" 0 "OK" 3 "SNA" ; +VAL_ 643 Bckl_Sw_RM_Stat_SAM_R 2 "FLT" 1 "NOT" 0 "OK" 3 "SNA" ; +VAL_ 643 Bckl_Sw_RR_Stat_SAM_R 2 "FLT" 1 "NOT" 0 "OK" 3 "SNA" ; +VAL_ 643 DL_RLtch_Stat 1 "CLS" 0 "NDEF0" 2 "OPN" 3 "SNA" ; +VAL_ 643 DrRLtch_FL_Stat 1 "CLS" 0 "NDEF0" 2 "OPN" 3 "SNA" ; +VAL_ 643 DrRLtch_FR_Stat 1 "CLS" 0 "NDEF0" 2 "OPN" 3 "SNA" ; +VAL_ 643 DrRLtch_RL_Stat 1 "CLS" 0 "NDEF0" 2 "OPN" 3 "SNA" ; +VAL_ 643 DrRLtch_RR_Stat 1 "CLS" 0 "NDEF0" 2 "OPN" 3 "SNA" ; +VAL_ 643 EngHd_Stat 1 "CLS" 0 "NDEF0" 2 "OPN" 3 "SNA" ; +VAL_ 643 LgtSens_Night 0 "DAY" 1 "NIGHT" ; +VAL_ 643 MPkBrk_Stat 1 "ENGG" 0 "RELS" ; +VAL_ 643 RevGr_Engg 0 "DISENGG" 1 "ENGG" 2 "NDEF2" 3 "SNA" ; +VAL_ 643 StW_Cond_Stat 3 "BLINK" 1 "NDEF1" 0 "OFF" 2 "ON" ; +VAL_ 643 Trlr_Stat 2 "NDEF2" 0 "NONE" 1 "OK" 3 "SNA" ; VAL_ 880 EPAS_currentTuneMode 1 "DM_COMFORT" 3 "DM_SPORT" 2 "DM_STANDARD" 0 "FAIL_SAFE_DEFAULT" 4 "RWD_COMFORT" 6 "RWD_SPORT" 5 "RWD_STANDARD" 7 "UNAVAILABLE" ; VAL_ 880 EPAS_eacErrorCode 14 "EAC_ERROR_EPB_INHIBIT" 3 "EAC_ERROR_HANDS_ON" 7 "EAC_ERROR_HIGH_ANGLE_RATE_REQ" 9 "EAC_ERROR_HIGH_ANGLE_RATE_SAFETY" 6 "EAC_ERROR_HIGH_ANGLE_REQ" 8 "EAC_ERROR_HIGH_ANGLE_SAFETY" 10 "EAC_ERROR_HIGH_MMOT_SAFETY" 11 "EAC_ERROR_HIGH_TORSION_SAFETY" 0 "EAC_ERROR_IDLE" 12 "EAC_ERROR_LOW_ASSIST" 2 "EAC_ERROR_MAX_SPEED" 1 "EAC_ERROR_MIN_SPEED" 13 "EAC_ERROR_PINION_VEL_DIFF" 4 "EAC_ERROR_TMP_FAULT" 5 "EAR_ERROR_MAX_STEER_DELTA" 15 "SNA" ; VAL_ 880 EPAS_eacStatus 2 "EAC_ACTIVE" 1 "EAC_AVAILABLE" 3 "EAC_FAULT" 0 "EAC_INHIBITED" 4 "SNA" ; diff --git a/panda/.circleci/config.yml b/panda/.circleci/config.yml index a4ae26efe51f35..d74c1a20e09686 100644 --- a/panda/.circleci/config.yml +++ b/panda/.circleci/config.yml @@ -11,7 +11,7 @@ jobs: - run: name: Run safety test command: | - docker run panda_safety /bin/bash -c "cd /panda/tests/safety; PYTHONPATH=/ ./test.sh" + docker run panda_safety /bin/bash -c "cd /panda/tests/safety; ./test.sh" misra-c2012: machine: @@ -64,7 +64,7 @@ jobs: - run: name: Build ESP image command: | - docker run panda_build /bin/bash -c "cd /panda/boardesp; ./get_sdk.sh; make user1.bin" + docker run panda_build /bin/bash -c "cd /panda/boardesp; make user1.bin" safety_replay: machine: @@ -92,20 +92,6 @@ jobs: command: | docker run language_check /bin/bash -c "cd /panda/tests/language; ./test_language.py" - linter_python: - machine: - docker_layer_caching: true - steps: - - checkout - - run: - name: Build image - command: "docker build -t linter_python -f tests/linter_python/Dockerfile ." - - run: - name: Run linter python test - command: | - docker run linter_python /bin/bash -c "cd /panda/tests/linter_python; PYTHONPATH=/ ./flake8_panda.sh" - docker run linter_python /bin/bash -c "cd /panda/tests/linter_python; PYTHONPATH=/ ./pylint_panda.sh" - workflows: version: 2 main: @@ -115,4 +101,3 @@ workflows: - build - safety_replay - language_check - - linter_python diff --git a/panda/board/build.mk b/panda/board/build.mk index 21daf53ad02710..8c5bff8c0dd40c 100644 --- a/panda/board/build.mk +++ b/panda/board/build.mk @@ -43,7 +43,7 @@ bin: obj/$(PROJ_NAME).bin # this flashes everything recover: obj/bootstub.$(PROJ_NAME).bin obj/$(PROJ_NAME).bin -PYTHONPATH=../ python3 -c "from python import Panda; Panda().reset(enter_bootloader=True)" - sleep 1.0 + sleep 2.0 $(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08004000 -D obj/$(PROJ_NAME).bin $(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.$(PROJ_NAME).bin @@ -68,7 +68,7 @@ obj/$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/main.$(PROJ_NAME).o $(OBJCOPY) -v -O binary obj/$(PROJ_NAME).elf obj/code.bin SETLEN=1 ../crypto/sign.py obj/code.bin $@ $(CERT) @BINSIZE=$$(du -b "obj/$(PROJ_NAME).bin" | cut -f 1) ; \ - if [ $$BINSIZE -ge 49152 ]; then echo "ERROR obj/$(PROJ_NAME).bin is too big!"; exit 1; fi; + if [ $$BINSIZE -ge 49152 ]; then echo "WARNING obj/$(PROJ_NAME).bin might be too big!"; fi; obj/bootstub.$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/bootstub.$(PROJ_NAME).o obj/sha.$(PROJ_NAME).o obj/rsa.$(PROJ_NAME).o $(CC) $(CFLAGS) -o obj/bootstub.$(PROJ_NAME).elf $^ diff --git a/panda/board/main.c b/panda/board/main.c index 8bc4ac78f1b43b..2d64e6f9b47dd2 100644 --- a/panda/board/main.c +++ b/panda/board/main.c @@ -29,6 +29,7 @@ #include "drivers/clock.h" #include "gpio.h" +#include "drivers/lin.h" #ifndef EON #include "drivers/spi.h" @@ -39,6 +40,7 @@ #include "drivers/can.h" +uint16_t prev_safety_mode = 0; extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used struct __attribute__((packed)) health_t { @@ -107,12 +109,23 @@ void debug_ring_callback(uart_ring *ring) { // this is the only way to leave silent mode void set_safety_mode(uint16_t mode, int16_t param) { + //BB to prevent any changes to safety + UNUSED(mode); + UNUSED(param); +} + +// this is the only way to leave silent mode +void set_safety_mode2(uint16_t mode, int16_t param) { + if (prev_safety_mode == mode) { + return; + } + //BB we had this now they changed it to below: int err = safety_set_mode2(mode, param); uint16_t mode_copy = mode; - int err = set_safety_hooks(mode_copy, param); + int err = set_safety_hooks2(mode_copy, param); if (err == -1) { puts("Error: safety set mode failed. Falling back to SILENT\n"); mode_copy = SAFETY_SILENT; - err = set_safety_hooks(mode_copy, 0); + err = set_safety_hooks2(mode_copy, 0); if (err == -1) { puts("Error: Failed setting SILENT mode. Hanging\n"); while (true) { @@ -152,6 +165,7 @@ void set_safety_mode(uint16_t mode, int16_t param) { can_silent = ALL_CAN_LIVE; break; } + prev_safety_mode = mode; can_init_all(); } @@ -695,6 +709,9 @@ void TIM1_BRK_TIM9_IRQ_Handler(void) { heartbeat_counter += 1U; } + // check heartbeat counter if we are running EON code. If the heartbeat has been gone for a while, go to NOOUTPUT safety mode. + //BB we do not want to disable safety mode when on tesla + /* #ifdef EON // check heartbeat counter if we are running EON code. // if the heartbeat has been gone for a while, go to SILENT safety mode and enter power save @@ -719,6 +736,7 @@ void TIM1_BRK_TIM9_IRQ_Handler(void) { current_board->set_usb_power_mode(USB_POWER_CDP); } #endif + */ // check registers check_registers(); @@ -807,7 +825,8 @@ int main(void) { // use TIM2->CNT to read // init to SILENT and can silent - set_safety_mode(SAFETY_SILENT, 0); + //set_safety_mode(SAFETY_SILENT, 0); + set_safety_mode2(SAFETY_TESLA,0 ); // enable CAN TXs current_board->enable_can_transcievers(true); diff --git a/panda/board/pedal/main.c b/panda/board/pedal/main.c index 3192f4b2bd2f77..ba842d64c650a6 100644 --- a/panda/board/pedal/main.c +++ b/panda/board/pedal/main.c @@ -106,11 +106,25 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) #endif +// ***************************** pedal can checksum ***************************** + + +// ***************************** tesla can checksum ***************************** +uint8_t tesla_can_cksum(uint8_t *dat, int len, int addr) { + int i; + uint8_t s = 0; + s += ((addr)&0xFF) + ((addr>>8)&0xFF); + for (i = 0; i < len; i++) { + s = (s + dat[i]) & 0xFF; + } + return s; +} + // ***************************** can port ***************************** // addresses to be used on CAN -#define CAN_GAS_INPUT 0x200 -#define CAN_GAS_OUTPUT 0x201U +#define CAN_GAS_INPUT 0x551U +#define CAN_GAS_OUTPUT 0x552U #define CAN_GAS_SIZE 6 #define COUNTER_CYCLE 0xFU @@ -156,6 +170,7 @@ void CAN1_RX0_IRQ_Handler(void) { } else { puts("Failed entering Softloader or Bootloader\n"); } + return; } // normal packet @@ -167,7 +182,7 @@ void CAN1_RX0_IRQ_Handler(void) { uint16_t value_1 = (dat[2] << 8) | dat[3]; bool enable = ((dat[4] >> 7) & 1U) != 0U; uint8_t index = dat[4] & COUNTER_CYCLE; - if (crc_checksum(dat, CAN_GAS_SIZE - 1, crc_poly) == dat[5]) { + if (tesla_can_cksum(dat, CAN_GAS_SIZE - 1,CAN_GAS_INPUT) == dat[5]) { if (((current_index + 1U) & COUNTER_CYCLE) == index) { #ifdef DEBUG puts("setting gas "); @@ -230,7 +245,7 @@ void TIM3_IRQ_Handler(void) { dat[2] = (pdl1 >> 8) & 0xFFU; dat[3] = (pdl1 >> 0) & 0xFFU; dat[4] = ((state & 0xFU) << 4) | pkt_idx; - dat[5] = crc_checksum(dat, CAN_GAS_SIZE - 1, crc_poly); + dat[5] = tesla_can_cksum(dat, CAN_GAS_SIZE - 1,CAN_GAS_OUTPUT); CAN->sTxMailBox[0].TDLR = dat[0] | (dat[1] << 8) | (dat[2] << 16) | (dat[3] << 24); CAN->sTxMailBox[0].TDHR = dat[4] | (dat[5] << 8); CAN->sTxMailBox[0].TDTR = 6; // len of packet is 5 @@ -268,8 +283,16 @@ void pedal(void) { // write the pedal to the DAC if (state == NO_FAULT) { - dac_set(0, MAX(gas_set_0, pdl0)); - dac_set(1, MAX(gas_set_1, pdl1)); + if (pdl0 > 500) { + dac_set(0, MAX(gas_set_0, pdl0)); + dac_set(1, MAX(gas_set_1, pdl1)); + } else if (gas_set_0 > 0) { + dac_set(0, gas_set_0); + dac_set(1, gas_set_1); + } else { + dac_set(0, pdl0); + dac_set(1, pdl1); + } } else { dac_set(0, pdl0); dac_set(1, pdl1); diff --git a/panda/board/power_saving.h b/panda/board/power_saving.h index f7e42371aa2188..f965d06f211e7d 100644 --- a/panda/board/power_saving.h +++ b/panda/board/power_saving.h @@ -1,7 +1,7 @@ // WARNING: To stay in compliance with the SIL2 rules laid out in STM UM1840, we should never implement any of the available hardware low power modes. // See rule: CoU_3 -#define POWER_SAVE_STATUS_DISABLED 0 +#define POWER_SAVE_STATUS_DISABLED 1 #define POWER_SAVE_STATUS_ENABLED 1 int power_save_status = POWER_SAVE_STATUS_DISABLED; diff --git a/panda/board/safety.h b/panda/board/safety.h index ef5a4efc8cbcaf..c7e438e2d9378f 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -216,6 +216,13 @@ const safety_hook_config safety_hook_registry[] = { int current_safety = -1; int set_safety_hooks(uint16_t mode, int16_t param) { + //BB prevent resetting if already in the correct mode + UNUSED(mode); + UNUSED(param); + return 1; +} + +int set_safety_hooks2(uint16_t mode, int16_t param) { safety_mode_cnt = 0U; // reset safety mode timer int set_status = -1; // not set int hook_config_count = sizeof(safety_hook_registry) / sizeof(safety_hook_config); diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h index 78b80e3bc535b6..46760735c5e260 100644 --- a/panda/board/safety/safety_tesla.h +++ b/panda/board/safety/safety_tesla.h @@ -7,118 +7,1476 @@ // accel rising edge // brake rising edge // brake > 0mph -// -bool fmax_limit_check(float val, const float MAX_VAL, const float MIN_VAL) { - return (val > MAX_VAL) || (val < MIN_VAL); -} +void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number, bool skip_tx_hook); // 2m/s are added to be less restrictive const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_UP = { {2., 7., 17.}, - {5., .8, .25}}; + {25., 25., 25.}}; const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_DOWN = { {2., 7., 17.}, - {5., 3.5, .8}}; + {25., 26., 25.}}; const struct lookup_t TESLA_LOOKUP_MAX_ANGLE = { {2., 29., 38.}, - {410., 92., 36.}}; + {500., 500., 500.}}; + +const AddrBus TESLA_TX_MSGS[] = {//chassis CAN +{0x045,0}, {0x209,0}, {0x219,0}, {0x229,0}, {0x239,0}, {0x249,0}, + {0x2B9,0}, {0x309,0}, {0x329,0}, {0x349,0}, {0x369,0}, {0x379,0}, {0x389,0}, + {0x399,0}, {0x3A9,0}, {0x3B1,0}, {0x3D9,0}, {0x3E9,0}, {0x400,0}, {0x488,0}, + {0x409,0}, {0x551,0}, {0x539,0}, {0x554,0}, {0x556,0}, {0x557,0}, {0x559,0}, + {0x560,0}, {0x5D9,0}, {0x5F9,0}, {0x609,0}, {0x639,0}, {0x659,0}, {0x65A,0}, + {0x669,0}, {0x720,0}, +//radar CAN +{0x101,1}, {0x109,1}, {0x119,1}, {0x129,1}, {0x149,1}, {0x159,1}, {0x169,1}, + {0x199,1}, {0x1A9,1}, {0x209,1}, {0x219,1}, {0x2A9,1}, {0x2B9,1}, {0x2D9,1}, + {0x641,1}, +//epas CAN +{0x488,2}, {0x551,2},}; + +// TODO: do checksum and counter checks. Add correct timestep, 0.1s for now. +AddrCheckStruct tesla_rx_checks[] = { + {.addr = {0x00E}, .bus =0, .check_checksum = false, .max_counter = 15U, .expected_timestep = 100000U}, + {.addr = {0x045}, .bus =0, .check_checksum = false, .max_counter = 15U, .expected_timestep = 100000U}, + {.addr = {0x108}, .bus =0, .check_checksum = false, .max_counter = 15U, .expected_timestep = 100000U}, + {.addr = {0x118}, .bus =0, .check_checksum = false, .max_counter = 15U, .expected_timestep = 100000U}, + {.addr = {0x318}, .bus =0, .check_checksum = false, .max_counter = 15U, .expected_timestep = 100000U}, + {.addr = {0x348}, .bus =0, .check_checksum = false, .max_counter = 15U, .expected_timestep = 100000U}, + {.addr = {0x368}, .bus =0, .check_checksum = false, .max_counter = 15U, .expected_timestep = 100000U}, +}; +const int TESLA_RX_CHECK_LEN = sizeof(tesla_rx_checks) / sizeof(tesla_rx_checks[0]); + const uint32_t TESLA_RT_INTERVAL = 250000; // 250ms between real time checks +struct sample_t tesla_angle_meas; // last 3 steer angles + // state of angle limits -float tesla_desired_angle_last = 0; // last desired steer angle -float tesla_rt_angle_last = 0.; // last real time angle -float tesla_ts_angle_last = 0; +int tesla_desired_angle_last = 0; // last desired steer angle +int16_t tesla_rt_angle_last = 0.; // last real time angle +uint32_t tesla_ts_angle_last = 0; int tesla_controls_allowed_last = 0; - +int steer_allowed = 1; +int EON_is_connected = 0; +int tesla_brake_prev = 0; +int tesla_gas_prev = 0; int tesla_speed = 0; +int current_car_time = -1; +int time_at_last_stalk_pull = -1; int eac_status = 0; -void set_gmlan_digital_output(int to_set); -void reset_gmlan_switch_timeout(void); -void gmlan_switch_init(int timeout_enable); +/* <-- revB giraffe GPIO */ +/* +#include "../drivers/uja1023.h" + +uint32_t tesla_ts_brakelight_on_last = 0; +const uint32_t BRAKELIGHT_CLEAR_INTERVAL = 250000; //25ms; needs to be slower than the framerate difference between the DI_torque2 (~100Hz) and DI_state messages (~10hz). +const uint32_t STW_MENU_BTN_HOLD_INTERVAL = 750000; //75ms, how long before we recognize the user is holding this steering wheel button down + +uint32_t stw_menu_btn_pressed_ts = 0; +int stw_menu_current_output_state = 0; +int stw_menu_btn_state_last = 0; +int stw_menu_output_flag = 0; +int high_beam_lever_state = 0; +*/ +/* revB giraffe GPIO --> */ + +int tesla_radar_status = 0; //0-not present, 1-initializing, 2-active +uint32_t tesla_last_radar_signal = 0; +const uint32_t TESLA_RADAR_TIMEOUT = 10000000; // 10s second between real time checks +char radar_VIN[] = " "; //leave empty if your radar VIN matches the car VIN +int tesla_radar_vin_complete = 0; +uint8_t tesla_radar_can = 1; +uint8_t tesla_epas_can = 2; +int tesla_radar_trigger_message_id = 0; //not used by tesla, to showcase for other cars +int radarPosition = 0; //0 nosecone, 1 facelift +int radarEpasType = 0; //0/1 bosch, 2-4 mando + + +//settings from bb_openpilot.cfg + +int DAS_gtwConfigReceived = 0; +int enable_das_emulation = 1; +int enable_radar_emulation = 1; + +//for street signs +int stopSignWarning = 0; +int stopLightWarning = 0; +int streetSignObject_b0 = 0; +int streetSignObject_b1 = 0; +int streetSignObject_b2 = 0; +int streetSignObject_b3 = 0; +int streetSignObject_active = 0; + +//fake DAS counters +int DAS_uom = 0; //units of measure, 0 = MPH, 1 = km/h +int DAS_bootID_sent = 0; +int fake_DAS_counter = 0; +int DAS_object_idx = 0; +int DAS_control_idx = 0; +int DAS_pscControl_idx = 0; +int DAS_telemetryPeriodic_idx1 = 0; +int DAS_telemetryPeriodic_idx2 = 0; +int DAS_lanes_idx = 0; +int DAS_status_idx = 0; +int DAS_status2_idx = 0; +int DAS_bodyControls_idx = 0; +int DAS_info_idx = 0; +int DAS_warningMatrix0_idx = 0; +int DAS_warningMatrix1_idx = 0; +int DAS_warningMatrix3_idx = 0; +int DAS_steeringControl_idx = 0; +int DAS_diState_idx = 0; +int DAS_longControl_idx = 0; +int DI_locStatus_idx = 0; +int DAS_carLog_idx = 0; +int DAS_telemetry_idx = 0; + + +//fake DAS variables +int DAS_longC_enabled = 0; +int DAS_speed_limit_kph = 0; +int DAS_accel_min = 0; +int DAS_accel_max = 0; +int DAS_aeb_event = 0x00; +int DAS_acc_state = 0x00; +int DAS_jerk_min = 0x000; +int DAS_jerk_max = 0x0F; +int DAS_gas_to_resume = 0; +int DAS_206_apUnavailable = 0; +int DAS_haptic_request = 0; +uint8_t EPB_epasControl_idx = 0; + +//fake DAS objects +int DAS_LEAD_OBJECT_MLB = 0xFFFFFF00; +int DAS_LEAD_OBJECT_MHB = 0x03FFFF83; +int DAS_LEFT_OBJECT_MLB = 0xFFFFFF01; +int DAS_LEFT_OBJECT_MHB = 0x03FFFF83; +int DAS_RIGHT_OBJECT_MLB = 0xFFFFFF02; +int DAS_RIGHT_OBJECT_MHB = 0x03FFFF83; +//fake DAS for DAS_status and DAS_status2 +int DAS_op_status = 1; +int DAS_op_status_last_received = 1; +int DAS_alca_state = 0x05; +int DAS_hands_on_state = 0; +int DAS_forward_collision_warning = 0; +int DAS_units_included = 0; +int DAS_cc_state = 0; +int DAS_acc_speed_limit = 0; //in car speed units +int DAS_acc_speed_kph = 0; +int DAS_collision_warning = 0; +int DAS_ldwStatus = 0; + +//lanes and objects +int tLeadDx = 0; +int tLeadDy = 0; +int tLeadClass = 0; +int rLine = 0; +int lLine = 0; +int rLine_qual = 0; +int lLine_qual = 0; +int lWidth = 29; +int curvC0 = 0x64; +int curvC1 = 0x7F; +int curvC2 = 0x7F; +int curvC3 = 0x7F; +int laneRange = 160; // max 160m + +//fake DAS for telemetry +int DAS_telLeftMarkerQuality = 3; //3-high, 2-medium, 1-low 0-lowest +int DAS_telRightMarkerQuality = 3; //3-high, 2-medium, 1-low 0-lowest +int DAS_telRightLaneType = 3;//0-undecided, 1-solid, 2-road edge, 3-dashed 4-double 5-botts dots 6-barrier +int DAS_telLeftLaneType = 3; //0-undecided, 1-solid, 2-road edge, 3-dashed 4-double 5-botts dots 6-barrier +int DAS_telRightLaneCrossing = 0; //0-not crossing 1-crossing +int DAS_telLeftLaneCrossing = 0; //0-not crossing 1-crossing +int DAS_telLeftMarkerColor = 1; //0-unknown, 1-white, 2-yellow, 3-blue +int DAS_telRightMarkerColor = 1; //0-unknown, 1-white, 2-yellow, 3-blue +//fake DAS for DAS_bodyControls +int DAS_turn_signal_request = 0; + +//fake DAS for DAS_steeringControls +int DAS_steeringAngle = 0x4000; +int DAS_steeringEnabled = 0; + +//fake DAS controll +int time_last_DAS_data = -1; +int time_last_EPAS_data = -10; + +//fake DAS using pedal +int DAS_usingPedal = 0; + +//fake DAS - are we in drive? +int DAS_inDrive = 0; +int DAS_inDrive_prev = 0; +int DAS_present = 0; +int tesla_radar_should_send = 0; +int DAS_noEpasHarness = 0; +int DAS_usesApillarHarness=0; + +//fake DAS - last stalk data used to cancel +uint32_t DAS_lastStalkL =0x00; +uint32_t DAS_lastStalkH = 0x00; + +//fake DAS - pedal pressed (with Pedal) +int DAS_pedalPressed = 0; + +//fake DAS - DI_state spamming +uint32_t DAS_diStateL =0x00; +uint32_t DAS_diStateH = 0x00; + +//fake DAS - warning messages +int DAS_211_accNoSeatBelt = 0x00; +int DAS_canErrors = 0x00; +int DAS_202_noisyEnvironment = 0x00; +int DAS_doorOpen = 0x00; +int DAS_notInDrive = 0x00; +int DAS_222_accCameraBlind = 0x00; +int DAS_219_lcTempUnavailableSpeed = 0x00; +int DAS_220_lcTempUnavailableRoad = 0x00; +int DAS_207_lkasUnavailable = 0x00; +int DAS_208_rackDetected = 0x00; +int DAS_025_steeringOverride = 0x00; +int DAS_221_lcAborting = 0x00; + +//fake DAS - high beam request +int DAS_high_low_beam_request = 0x00; +int DAS_high_low_beam_reason = 0x00; +int DAS_ahb_is_enabled = 0; + +//fake DAS - plain CC condition +int DAS_plain_cc_enabled = 0x00; + +//fake DAS - emergency brakes use +int DAS_emergency_brake_request = 0x00; +int DAS_fleet_speed_state = 0x00; + + +static int add_tesla_crc(uint32_t MLB, uint32_t MHB , int msg_len) { + //"""Calculate CRC8 using 1D poly, FF start, FF end""" + int crc_lookup[256] = { 0x00, 0x1D, 0x3A, 0x27, 0x74, 0x69, 0x4E, 0x53, 0xE8, 0xF5, 0xD2, 0xCF, 0x9C, 0x81, 0xA6, 0xBB, + 0xCD, 0xD0, 0xF7, 0xEA, 0xB9, 0xA4, 0x83, 0x9E, 0x25, 0x38, 0x1F, 0x02, 0x51, 0x4C, 0x6B, 0x76, + 0x87, 0x9A, 0xBD, 0xA0, 0xF3, 0xEE, 0xC9, 0xD4, 0x6F, 0x72, 0x55, 0x48, 0x1B, 0x06, 0x21, 0x3C, + 0x4A, 0x57, 0x70, 0x6D, 0x3E, 0x23, 0x04, 0x19, 0xA2, 0xBF, 0x98, 0x85, 0xD6, 0xCB, 0xEC, 0xF1, + 0x13, 0x0E, 0x29, 0x34, 0x67, 0x7A, 0x5D, 0x40, 0xFB, 0xE6, 0xC1, 0xDC, 0x8F, 0x92, 0xB5, 0xA8, + 0xDE, 0xC3, 0xE4, 0xF9, 0xAA, 0xB7, 0x90, 0x8D, 0x36, 0x2B, 0x0C, 0x11, 0x42, 0x5F, 0x78, 0x65, + 0x94, 0x89, 0xAE, 0xB3, 0xE0, 0xFD, 0xDA, 0xC7, 0x7C, 0x61, 0x46, 0x5B, 0x08, 0x15, 0x32, 0x2F, + 0x59, 0x44, 0x63, 0x7E, 0x2D, 0x30, 0x17, 0x0A, 0xB1, 0xAC, 0x8B, 0x96, 0xC5, 0xD8, 0xFF, 0xE2, + 0x26, 0x3B, 0x1C, 0x01, 0x52, 0x4F, 0x68, 0x75, 0xCE, 0xD3, 0xF4, 0xE9, 0xBA, 0xA7, 0x80, 0x9D, + 0xEB, 0xF6, 0xD1, 0xCC, 0x9F, 0x82, 0xA5, 0xB8, 0x03, 0x1E, 0x39, 0x24, 0x77, 0x6A, 0x4D, 0x50, + 0xA1, 0xBC, 0x9B, 0x86, 0xD5, 0xC8, 0xEF, 0xF2, 0x49, 0x54, 0x73, 0x6E, 0x3D, 0x20, 0x07, 0x1A, + 0x6C, 0x71, 0x56, 0x4B, 0x18, 0x05, 0x22, 0x3F, 0x84, 0x99, 0xBE, 0xA3, 0xF0, 0xED, 0xCA, 0xD7, + 0x35, 0x28, 0x0F, 0x12, 0x41, 0x5C, 0x7B, 0x66, 0xDD, 0xC0, 0xE7, 0xFA, 0xA9, 0xB4, 0x93, 0x8E, + 0xF8, 0xE5, 0xC2, 0xDF, 0x8C, 0x91, 0xB6, 0xAB, 0x10, 0x0D, 0x2A, 0x37, 0x64, 0x79, 0x5E, 0x43, + 0xB2, 0xAF, 0x88, 0x95, 0xC6, 0xDB, 0xFC, 0xE1, 0x5A, 0x47, 0x60, 0x7D, 0x2E, 0x33, 0x14, 0x09, + 0x7F, 0x62, 0x45, 0x58, 0x0B, 0x16, 0x31, 0x2C, 0x97, 0x8A, 0xAD, 0xB0, 0xE3, 0xFE, 0xD9, 0xC4 }; + int crc = 0xFF; + for (int x = 0; x < msg_len; x++) { + int v = 0; + if (x <= 3) { + v = (MLB >> (x * 8)) & 0xFF; + } else { + v = (MHB >> ( (x-4) * 8)) & 0xFF; + } + crc = crc_lookup[crc ^ v]; + } + crc = crc ^ 0xFF; + return crc; +} + +static int add_tesla_cksm(CAN_FIFOMailBox_TypeDef *msg , int msg_id, int msg_len) { + int cksm = (0xFF & msg_id) + (0xFF & (msg_id >> 8)); + for (int x = 0; x < msg_len; x++) { + int v = 0; + if (x <= 3) { + v = (msg->RDLR >> (x * 8)) & 0xFF; + } else { + v = (msg->RDHR >> ( (x-4) * 8)) & 0xFF; + } + cksm = (cksm + v) & 0xFF; + } + return cksm; +} + +static int add_tesla_cksm2(uint32_t dl, uint32_t dh, int msg_id, int msg_len) { + CAN_FIFOMailBox_TypeDef to_check; + to_check.RDLR = dl; + to_check.RDHR = dh; + return add_tesla_cksm(&to_check,msg_id,msg_len); +} + +// interp function that holds extreme values +float tesla_interpolate(struct lookup_t xy, float x) +{ + int size = sizeof(xy.x) / sizeof(xy.x[0]); + // x is lower than the first point in the x array. Return the first point + if (x <= xy.x[0]) + { + return xy.y[0]; + } + else + { + // find the index such that (xy.x[i] <= x < xy.x[i+1]) and linearly interp + for (int i = 0; i < size - 1; i++) + { + if (x < xy.x[i + 1]) + { + float x0 = xy.x[i]; + float y0 = xy.y[i]; + float dx = xy.x[i + 1] - x0; + float dy = xy.y[i + 1] - y0; + // dx should not be zero as xy.x is supposed ot be monotonic + if (dx <= 0.) + dx = 0.0001; + return dy * (x - x0) / dx + y0; + } + } + // if no such point is found, then x > xy.x[size-1]. Return last point + return xy.y[size - 1]; + } +} + +static uint32_t bitShift(int value, int which_octet, int starting_bit_in_octet) { + //which octet - 1-4 + //starting_bit_in_octet 1-8 + return ( value << ((starting_bit_in_octet - 1) + (which_octet -1) * 8)); +} + + + +static void send_fake_message(uint32_t RIR, uint32_t RDTR,int msg_len, int msg_addr, uint8_t bus_num, uint32_t data_lo, uint32_t data_hi) { + CAN_FIFOMailBox_TypeDef to_send; + uint32_t addr_mask = 0x001FFFFF; + to_send.RIR = (msg_addr << 21) + (addr_mask & (RIR | 1)); + to_send.RDTR = (RDTR & 0xFFFFFFF0) | msg_len; + to_send.RDLR = data_lo; + to_send.RDHR = data_hi; + can_send(&to_send, bus_num, false); +} + +static void reset_DAS_data(void) { + //fake DAS variables + //DAS_gtwConfigReceived = 0; + DAS_present = 0; + DAS_longC_enabled = 0; + DAS_speed_limit_kph = 0; + DAS_accel_min = 0; + DAS_accel_max = 0; + DAS_aeb_event = 0x00; + DAS_acc_state = 0x00; + DAS_jerk_min = 0x000; + DAS_jerk_max = 0x0F; + DAS_gas_to_resume = 0; + DAS_206_apUnavailable = 0; + DAS_haptic_request = 0; + DAS_op_status = 1; //unavailable + DAS_alca_state = 0x05; + DAS_hands_on_state = 0; + DAS_forward_collision_warning = 0; + DAS_cc_state = 0; + DAS_acc_speed_limit = 0; + DAS_acc_speed_kph = 0; + DAS_collision_warning = 0; + DAS_telLeftMarkerQuality = 3; //3-high, 2-medium, 1-low 0-lowest + DAS_telRightMarkerQuality = 3; //3-high, 2-medium, 1-low 0-lowest + DAS_telRightLaneType = 3; //0-undecided, 1-solid, 2-road edge, 3-dashed 4-double 5-botts dots 6-barrier + DAS_telLeftLaneType = 3; //0-undecided, 1-solid, 2-road edge, 3-dashed 4-double 5-botts dots 6-barrier + DAS_telRightLaneCrossing = 0; //0-not crossing 1-crossing + DAS_telLeftLaneCrossing = 0; //0-not crossing 1-crossing + DAS_telLeftMarkerColor = 1; //0-unknown, 1-white, 2-yellow, 3-blue + DAS_telRightMarkerColor = 1; //0-unknown, 1-white, 2-yellow, 3-blue + DAS_turn_signal_request = 0; + DAS_high_low_beam_request = 0; + DAS_high_low_beam_reason = 0; + DAS_steeringAngle = 0x4000; + DAS_steeringEnabled = 0; + DAS_usingPedal = 0; + DAS_lastStalkL =0x00; + DAS_lastStalkH = 0x00; + DAS_pedalPressed = 0; + DAS_diStateL =0x00; + DAS_diStateH = 0x00; + DAS_211_accNoSeatBelt = 0x00; + DAS_canErrors = 0x00; + DAS_202_noisyEnvironment = 0x00; + DAS_doorOpen = 0x00; + DAS_notInDrive = 0x00; + DAS_222_accCameraBlind = 0x00; + DAS_219_lcTempUnavailableSpeed = 0x00; + DAS_220_lcTempUnavailableRoad = 0x00; + DAS_207_lkasUnavailable = 0x00; + DAS_208_rackDetected = 0x00; + DAS_025_steeringOverride = 0x00; + DAS_221_lcAborting = 0x00; + //reset fake DAS objects + DAS_LEAD_OBJECT_MLB = 0xFFFFFF00; + DAS_LEAD_OBJECT_MHB = 0x03FFFF83; + DAS_LEFT_OBJECT_MLB = 0xFFFFFF01; + DAS_LEFT_OBJECT_MHB = 0x03FFFF83; + DAS_RIGHT_OBJECT_MLB = 0xFFFFFF02; + DAS_RIGHT_OBJECT_MHB = 0x03FFFF83; + DAS_plain_cc_enabled = 0x00; + DAS_emergency_brake_request = 0x00; + DAS_fleet_speed_state = 0x00; +} + + +static void do_fake_DAS(uint32_t RIR, uint32_t RDTR) { + + fake_DAS_counter++; + fake_DAS_counter = fake_DAS_counter % 300; + uint32_t MLB = 0; + uint32_t MHB = 0; + + //check if we got data from OP in the last two seconds + if (current_car_time - time_last_DAS_data > 2) { + //no message in the last 2 seconds, reset all variables + //BB_no_harness: why are we resetting here.... maybe it should be only when disconnecting or not at all + //reset_DAS_data(); + if (EON_is_connected == 1) { + reset_DAS_data(); + EON_is_connected = 0; + } + } else { + EON_is_connected = 1; + } + + //ldw type of warning... use haptic if we manually steer + if (DAS_ldwStatus > 0) { + DAS_haptic_request = 1; + //if OP not enabled or manual steering, only use haptic + if (((DAS_219_lcTempUnavailableSpeed == 1) || (DAS_op_status < 5)) && (DAS_ldwStatus > 2)) { + DAS_ldwStatus -= 2; + } + //if OP enabled and no manual steering, always force audible alert + if ((DAS_219_lcTempUnavailableSpeed == 0) && (DAS_op_status == 5) && (DAS_ldwStatus > 0) && (DAS_ldwStatus < 3)) { + DAS_ldwStatus += 2; + } + } else { + DAS_haptic_request = 0; + } + + if (fake_DAS_counter % 2 == 0) { + //send DAS_steeringControl - 0x488 + MHB = 0x00; + MLB = ((DAS_steeringAngle >> 8) & 0x7F) + (DAS_haptic_request << 7) + + ((DAS_steeringAngle & 0xFF) << 8) + + (((((DAS_steeringEnabled & controls_allowed & DAS_inDrive) )<< 6) + DAS_steeringControl_idx) << 16); + int cksm = add_tesla_cksm2(MLB, MHB, 0x488, 3); + MLB = MLB + (cksm << 24); + if (DAS_noEpasHarness == 0) { + send_fake_message(RIR,RDTR,4,0x488,2,MLB,MHB); + } + send_fake_message(RIR,RDTR,4,0x488,0,MLB,MHB); + DAS_steeringControl_idx ++; + DAS_steeringControl_idx = DAS_steeringControl_idx % 16; + } + + + if (enable_das_emulation == 0) { + return; + } + + //if we never sent DAS_bootID message, send it now + if (DAS_bootID_sent == 0) { + MLB = 0x00E30055; + MHB = 0x002003BD; + send_fake_message(RIR,RDTR,8,0x639,0,MLB,MHB); + DAS_bootID_sent = 1; + } + + if (fake_DAS_counter == 299) { + //send DAS_carLog 0x5D9 + if (DAS_carLog_idx == 0) { + MLB = 0x00041603; + MHB = 0x00000000; + } + if (DAS_carLog_idx == 1) { + MLB = 0x00010002; + MHB = 0x00000000; + } + if (DAS_carLog_idx == 2) { + MLB = 0x00000001; + MHB = 0x00000000; + } + send_fake_message(RIR,RDTR,8,0x5D9,0,MLB,MHB); + DAS_carLog_idx ++; + DAS_carLog_idx = DAS_carLog_idx % 3; + } + + + + if (fake_DAS_counter % 3 == 0) { + //send DAS_object - 0x309 + //fix - 0x81,0xC0,0xF8,0xF3,0x43,0x7F,0xFD,0xF1 + //when idx==0 is lead vehicle + if (DAS_object_idx == 0) { + MLB = DAS_LEAD_OBJECT_MLB; + MHB = DAS_LEAD_OBJECT_MHB; + } + if (DAS_object_idx == 1) { + MLB = DAS_LEFT_OBJECT_MLB; + MHB = DAS_LEFT_OBJECT_MHB; + } + if (DAS_object_idx == 2) { + MLB = DAS_RIGHT_OBJECT_MLB; + MHB = DAS_RIGHT_OBJECT_MHB; + } + if (DAS_object_idx == 3) { + MLB = 0xFFFFFF03; + MHB = 0x00000003; + } + //when idx == 4 we send street sign if it is valid + if (DAS_object_idx == 4) { + if (streetSignObject_active == 1) { + MLB = (streetSignObject_b3 << 24) + (streetSignObject_b2 << 16) + (streetSignObject_b1 << 8) + (streetSignObject_b0 << 0); + MHB = 0x00000000; + } else { + DAS_object_idx = 5; + } + + } + if (DAS_object_idx == 5) { + MLB = 0xFFFFFF05; + MHB = 0xFFFFFFFF; + } + send_fake_message(RIR,RDTR,8,0x309,0,MLB,MHB); + DAS_object_idx++; + DAS_object_idx = DAS_object_idx % 6; + } + + if (fake_DAS_counter % 4 == 0) { + int acc_speed_kph = 0xFFF; + int acc_state = 0x00; + int aeb_event = 0x00; + int jerk_min = 0x1FF; + int jerk_max = 0xFF; + int accel_min = 0x1FF; + int accel_max = 0x1FF; + //send DAS_control - 0x2B9 + //when not in drive it should send FF 0F FE FF FF FF XF YY - X counter YY Checksum + if (DAS_inDrive ==1) { + acc_state = 0x04; + if (DAS_longC_enabled > 0) { + jerk_min = 0x000; + jerk_max = 0x0F; + acc_state = 0x04;//bb send HOLD? + float uom = 1; + if ((DAS_uom == 0) && (DAS_units_included == 0)) { + uom = 1.609; + } + acc_speed_kph = (int)(DAS_acc_speed_limit * uom * 10.0); + accel_max = 0x1FE; //(int)((DAS_accel_max + 15 ) / 0.04); + accel_min = 0x001; //(int)((DAS_accel_min + 15 ) / 0.04); + } + MLB = (0xff & acc_speed_kph) + + (((acc_state << 4) + ((acc_speed_kph & 0xF00) >> 8)) << 8) + + ((((jerk_min << 2) & 0xFF) + aeb_event) << 16 ) + + ((((jerk_max << 3) & 0xFF) +((jerk_min >> 6) & 0x07)) << 24); + MHB = ((((accel_min << 3) & 0xFF) + ((jerk_max >> 5) & 0x07)) ) + + ((((accel_max << 4) & 0xFF) + ((accel_min >> 5) & 0x0F)) << 8) + + (((DAS_control_idx << 5) + ((accel_max >> 4) & 0x1F))<< 16); + } else { + MLB = 0xFFFE0FFF; + MHB = 0x001FFFFF + (DAS_control_idx << 21); + } + int cksm = add_tesla_cksm2(MLB, MHB, 0x2B9, 7); + MHB = MHB + (cksm << 24); + send_fake_message(RIR,RDTR,8,0x2B9,0,MLB,MHB); + DAS_control_idx++; + DAS_control_idx = DAS_control_idx % 8; + //send DAS_longControl - 0x209 + if (DAS_inDrive == 1) { + int das_loc_mode = 0x01;//normal + int das_loc_state = 0x00;//healthy + int das_loc_request = 0x00; //idle + jerk_min = 0xFF; + jerk_max = 0xFF; + accel_min = 0x1FF; + accel_max = 0x1FF; + acc_speed_kph = 0x7FF; + if (DAS_longC_enabled > 0) { + acc_state = 0x04; + jerk_min = 0x01; + jerk_max = 0xFE; + acc_speed_kph = (int)(DAS_acc_speed_kph * 10.0); + das_loc_request = 0x01;//forward + accel_min = 0x001; + accel_max = 0x1FE; + } + MLB = das_loc_mode + (das_loc_state << 2) + (das_loc_request << 5) + + (jerk_min << 8) + (jerk_max << 16) + ((acc_speed_kph & 0xFF) << 24); + MHB = ((acc_speed_kph >> 8) & 0x07) + ((accel_min << 3) & 0xFF) + + ((((accel_min >>5) & 0x0F) + ((accel_max << 4) & 0xFF))<< 8) + + ((((accel_max >> 4) & 0x1F) + (DAS_longControl_idx << 5)) << 16); + } else { + MLB = 0xFFFFFF00; + MHB = 0x001FFFFF + (DAS_longControl_idx << 21); + } + cksm = add_tesla_cksm2(MLB, MHB, 0x209, 7); + MHB = MHB + (cksm << 24); + //send_fake_message(RIR,RDTR,8,0x209,0,MLB,MHB); + DAS_longControl_idx++; + DAS_longControl_idx = DAS_longControl_idx % 8; + } + if (fake_DAS_counter % 4 == 2) { + //send DAS_pscControl - 0x219 + // 0x90 + DAS_pscControl_idx,0x00,0x00,0x00,0x00,0x00,0x00,0x00 + uint8_t eacStatus = 1; + if ((DAS_steeringEnabled & controls_allowed & DAS_inDrive) == 1) { + eacStatus = 2; + } + //BB disabling to see if this is the issue + eacStatus = 0; + MLB = 0x90 + DAS_pscControl_idx + (eacStatus << 8); + MHB = 0x00; + int cksm = add_tesla_cksm2(MLB, MHB, 0x219, 2); + MLB = MLB + (cksm << 16); + send_fake_message(RIR,RDTR,3,0x219,0,MLB,MHB); + DAS_pscControl_idx++; + DAS_pscControl_idx = DAS_pscControl_idx % 16; + + //send DAS_telemetryFurniture - 0x3B1 + //NOT SENDING FOR NOW + } + + if (fake_DAS_counter % 4 == 3) { + //send DAS_telemetryPeriodic - 0x379 + //static for now + switch (DAS_telemetryPeriodic_idx2) { + case 0: //Y0 3A 83 D3 E8 07 6D 11 + MLB = 0xd3833a00; + MHB = 0x116d07e8; + break; + case 1: //Y1 E1 D7 E6 FB BF 7D 00 + MLB = 0xE6D7E101; + MHB = 0x007DBFFB; + break; + case 2: //42 49 F4 27 FF D1 7D 00 + MLB = 0x27F44902; + MHB = 0x007DD1FF; + break; + case 3: //43 D7 AB A7 FF F3 FE C0 + MLB = 0xA7ABD703; + MHB = 0xC0FEF3FF; + break; + case 4: //44 7B FB B7 F1 33 83 FF + MLB = 0xB7FB7B04; + MHB = 0xFF8333F1; + break; + case 5: //45 EA C7 5F F7 27 83 40 + MLB = 0x5Fc7EA05; + MHB = 0x408327F7; + break; + case 6: //46 01 48 00 00 00 00 00 + MLB = 0x00480106; + MHB = 0x00000000; + break; + case 7: //47 01 48 00 00 00 00 00 + MLB = 0x00480107; + MHB = 0x00000000; + break; + case 8: //48 00 00 00 00 00 00 00 + MLB = 0x00000008; + MHB = 0x00000000; + break; + default: //49 FA 37 00 00 00 00 08 + MLB = 0x0037FA09; + MHB = 0x08000000; + break; + } + MLB = MLB + (DAS_telemetryPeriodic_idx2 << 5); + //send_fake_message(RIR,RDTR,1,0x379,0,MLB,MHB); + DAS_telemetryPeriodic_idx2++; + DAS_telemetryPeriodic_idx2 = DAS_telemetryPeriodic_idx2 % 10; + if (DAS_telemetryPeriodic_idx2 == 0) { + DAS_telemetryPeriodic_idx1 += 1; + DAS_telemetryPeriodic_idx1 = DAS_telemetryPeriodic_idx1 % 8; + } + + } + + if (fake_DAS_counter % 6 == 0) { + //send DAS_integratedSafetyFront - 0x299 + //NOT SENDING FOR NOW + + } + + if (fake_DAS_counter % 10 == 3) { + //send DAS_lanes - 0x239 + //for now fixed 0x33,0xC8,0xF0,0x7F,0x70,0x70,0x33,(idx << 4)+0x0F + int fuse = 2; + MLB = (lWidth << 4) + (rLine << 1) + lLine + (laneRange << 8) + (curvC0 << 16) + (curvC1 << 24); + MHB = 0x00F00000 + (DAS_lanes_idx << 28) + curvC2 + (curvC3 << 8) + ((((rLine * fuse) << 2) + (lLine * fuse)) << 16); //0x0F000000 + send_fake_message(RIR,RDTR,8,0x239,0,MLB,MHB); + DAS_lanes_idx ++; + DAS_lanes_idx = DAS_lanes_idx % 16; + + //send DI_locStatus2 - 0x4F6 + //hoping to show speed for ACC in IC + int DI_crsState = 0x01; + int DI_locState = 0x01; + int DI_locSpeed_kph = 0x00; + if (DAS_longC_enabled > 0) { + DI_locSpeed_kph = (int)(DAS_acc_speed_kph * 2.0); + DI_crsState = 0x02; + DI_locState = 0x02; + } + MLB = 0x00 + ((DI_locStatus_idx << 8) & 0x0F) + + (DI_crsState << 12) + (DI_locState << 16) + + ((DI_locSpeed_kph & 0x1F) << 19) + + ((DI_locSpeed_kph >> 5) & 0x0F); + MHB = 0x00; + int cksm = add_tesla_cksm2(MLB, MHB, 0x4F6, 4); + MLB = MLB + cksm; + DI_locStatus_idx += 1; + DI_locStatus_idx = DI_locStatus_idx % 16; + //send_fake_message(RIR,RDTR,4,0x4F6,0,MLB,MHB); + } + + if (fake_DAS_counter % 10 == 4) { + //send DAS_telemetry - 0x3A9 + //no counter - 00 00 00 00 00 00 00 00 + //ROAD INFO + //if (DAS_telemetry_idx==0) { + DAS_telRightMarkerQuality = rLine_qual; + DAS_telLeftMarkerQuality = lLine_qual; + if (rLine == 1) { + DAS_telRightLaneType = 3; + } else { + DAS_telRightLaneType = 7; + } + if (lLine == 1) { + DAS_telLeftLaneType = 3; + } else { + DAS_telLeftLaneType = 7; + } + if (DAS_alca_state == 0x09) { + //alca in progress, left + DAS_telRightLaneCrossing = 0; + DAS_telLeftLaneCrossing = 1; + } else if (DAS_alca_state == 0x0a) { + //alca in progress, right + DAS_telRightLaneCrossing = 1; + DAS_telLeftLaneCrossing = 0; + } else { + DAS_telRightLaneCrossing = 0; + DAS_telLeftLaneCrossing = 0; + } + MLB = 0x00 + ((DAS_telLeftLaneType + (DAS_telRightLaneType << 3) + (DAS_telLeftMarkerQuality << 6)) << 8 ) + + ((DAS_telRightMarkerQuality +(DAS_telLeftMarkerColor << 2) + + (DAS_telRightMarkerColor << 4) + (DAS_telLeftLaneCrossing << 6) +(DAS_telRightLaneCrossing <<7)) << 16); + MHB =0x00; + //} + //ACC DRIVER MONITORING + //if (DAS_telemetry_idx==1) { + // MLB=0x01; + // MHB=0x00; + //} + //TRAFFIC SIGNS + //if (DAS_telemetry_idx==2) { + // MLB=0x02; + // MHB=0x00; + //} + //CSA EVENT + //if (DAS_telemetry_idx==3) { + // MLB=0x07; + // MHB=0x00; + //} + //STATIONARY OBJECT + //if (DAS_telemetry_idx==4) { + // MLB=0x08; + // MHB=0x00; + //CRUISE STALK + //if (DAS_telemetry_idx==5) { + // MLB=0x09; + // MHB=0x00; + //} + DAS_telemetry_idx ++; + DAS_telemetry_idx = DAS_telemetry_idx % 6; + send_fake_message(RIR,RDTR,8,0x3A9,0,MLB,MHB); + } + + if (fake_DAS_counter % 10 == 5) { + //send DAS_chNm - 0x409 + //no counter + MLB = 0x00; + MHB = 0x00; + send_fake_message(RIR,RDTR,1,0x409,0,MLB,MHB); + + //send DAS_telemetryEvent - 0x3D9 + //NOT SENDING FOR NOW + + //send DAS_visualDebug - 0x249 + //no counter - 8A 06 21 1F 00 00 00 00 + //80 04 21 10 40 00 00 01 + //7th octet should be 05 when acc enabled and 01 when not? + int acc = 0x01; + if (DAS_cc_state >= 2) { + acc = 0x05; + } + MLB = 0x10210480; + MHB = 0x01000040 + (acc << 16); + send_fake_message(RIR,RDTR,8,0x249,0,MLB,MHB); + + } + + + if (fake_DAS_counter % 50 == 0) { + //send DAS_status - 0x399 + MLB = DAS_op_status + 0xF0 + (DAS_speed_limit_kph << 8) + (((DAS_collision_warning << 6) + DAS_speed_limit_kph) << 16); + MHB = ((DAS_fleet_speed_state & 0x03) << 3) + (DAS_ldwStatus << 5) + + (((DAS_hands_on_state << 2) + ((DAS_alca_state & 0x03) << 6) + DAS_fleet_speed_state) << 8) + + ((( DAS_status_idx << 4) + (DAS_alca_state >> 2)) << 16); + int cksm = add_tesla_cksm2(MLB, MHB, 0x399, 7); + MHB = MHB + (cksm << 24); + send_fake_message(RIR,RDTR,8,0x399,0,MLB,MHB); + DAS_status_idx ++; + DAS_status_idx = DAS_status_idx % 16; + + //send DAS_status2 - 0x389 + float uom = 1.0; + if ((DAS_uom == 1) && (DAS_units_included == 1)){ + uom = 1.0/1.609; + } + int sl2 = ((int)(DAS_acc_speed_limit * uom / 0.2)) & 0x3FF; + if (sl2 == 0) { + sl2 = 0x3FF; + } + MLB = sl2; + int lcw = 0x0F; + if (DAS_forward_collision_warning > 0) { + lcw = 0x01; + } + int b4 = 0x80; + int b5 = 0x13; + if (DAS_cc_state > 1) { //enabled or hold + b4 = 0x84; + b5 = 0x25; //BB lssState 0x 0x03 should be LSS_STATE_ELK enhanced LK + int _DAS_RobState = 0x02; //active + int _DAS_radarTelemetry = 0x01; // normal + int _DAS_lssState = 0x03; + int _DAS_acc_report = 0x01; //ACC_report_target_CIPV + if (DAS_fleet_speed_state == 2) { + _DAS_acc_report = 0x12; //ACC_report_fleet_speed + } + b4 = (_DAS_acc_report << 2) + ((_DAS_lssState & 0x01) << 7); + b5 =((_DAS_lssState >> 1) & 0x01) + (_DAS_radarTelemetry << 2) + (_DAS_RobState <<4); + } + MLB = MLB + (b4 << 24); + MHB = 0x8000 + b5 + (DAS_status2_idx << 20) + (lcw << 16); + cksm = add_tesla_cksm2(MLB, MHB, 0x389, 7); + MHB = MHB + (cksm << 24); + send_fake_message(RIR,RDTR,8,0x389,0,MLB,MHB); + DAS_status2_idx ++; + DAS_status2_idx = DAS_status2_idx % 16; + } + if (fake_DAS_counter % 50 == 9) { + //send DAS_bodyControls - 0x3E9 + //0xf1,0x0c + turn_signal_request,0x00,0x00,0x00,0x00,(idx << 4) + MLB = 0x000000F1 + (DAS_turn_signal_request << 8) + (DAS_high_low_beam_request << 10) + (DAS_high_low_beam_reason << 12); + MHB = 0x00 + (DAS_bodyControls_idx << 20); + int cksm = add_tesla_cksm2(MLB, MHB, 0x3E9, 7); + MHB = MHB + (cksm << 24); + send_fake_message(RIR,RDTR,8,0x3E9,0,MLB,MHB); + DAS_bodyControls_idx ++; + DAS_bodyControls_idx = DAS_bodyControls_idx % 16; + } + + if (fake_DAS_counter % 100 == 0) { + //send DAS_dtcMatrix - 0x669 + //NOT SENDING FOR NOW + + //send DAS_info - 0x539 + switch (DAS_info_idx) { + case 0: //changed + MLB = 0x0003010a; + MHB = 0x78000000; + break; + case 1: + MLB = 0x0102000b; + MHB = 0x00000001; + break; + case 2: //changed + MLB = 0x0000000d; + MHB = 0x9c20c11e; + break; + case 3: + MLB = 0xc9060010; + MHB = 0x000021b9; + break; + case 4: + MLB = 0x00000011; + MHB = 0x00000000; + break; + case 5: + MLB = 0x34577f12; + MHB = 0x0b89706f; + break; + case 6: + MLB = 0xffff0113; + MHB = 0x0000fcff; + break; + case 7: + MLB = 0x00000514; + MHB = 0xc309fce5; + break; + case 8: + MLB = 0xe7700017; + MHB = 0x000091c4; + break; + default: + MLB = 0xb2d50018; + MHB = 0x0000786c; + break; + } + send_fake_message(RIR,RDTR,8,0x539,0,MLB,MHB); + DAS_info_idx ++; + DAS_info_idx = DAS_info_idx % 10; + } + if (fake_DAS_counter % 100 == 45) { + //send DAS_warningMatrix0 - 0x329 + MLB = 0x00 + bitShift(DAS_canErrors,4,7) + bitShift(DAS_025_steeringOverride,4,1); + MHB = 0x00 + bitShift(DAS_notInDrive,2,2); + send_fake_message(RIR,RDTR,8,0x329,0,MLB,MHB); + DAS_warningMatrix0_idx ++; + DAS_warningMatrix0_idx = DAS_warningMatrix0_idx % 16; + + //send DAS_warningMatrix1 - 0x369 + MLB = 0x00 ; + MHB = 0x00; + send_fake_message(RIR,RDTR,8,0x369,0,MLB,MHB); + DAS_warningMatrix1_idx ++; + DAS_warningMatrix1_idx = DAS_warningMatrix1_idx % 16; + + //send DAS_warningMatrix3 - 0x349 + int ovr = 0; + int lcAborting = 0; + int lcUnavailableSpeed = 0; + if (DAS_alca_state == 0x14) { + //lcAborting == 1; + } + if (DAS_alca_state == 0x05) { + //lcUnavailableSpeed = 1; + } + if ((DAS_cc_state == 2) && (DAS_pedalPressed > 10)) { + ovr = 1; + } + + MLB = 0x00 + bitShift(DAS_gas_to_resume,1,2) + bitShift(DAS_206_apUnavailable,2,6) + bitShift(ovr,3,8) + + bitShift(lcAborting,2,1) + bitShift(lcUnavailableSpeed,4,3) + bitShift(DAS_211_accNoSeatBelt,3,3) + bitShift(DAS_202_noisyEnvironment,4,6) + + bitShift(stopSignWarning,1,4) + bitShift(stopLightWarning,1,5) + bitShift(DAS_207_lkasUnavailable,2,7) + bitShift(DAS_219_lcTempUnavailableSpeed,4,3) + + bitShift(DAS_220_lcTempUnavailableRoad,4,4) + bitShift(DAS_221_lcAborting,4,5) + bitShift(DAS_222_accCameraBlind,4,6) + bitShift(DAS_208_rackDetected,2,8); + MHB = 0x00; + send_fake_message(RIR,RDTR,8,0x349,0,MLB,MHB); + DAS_warningMatrix3_idx ++; + DAS_warningMatrix3_idx = DAS_warningMatrix3_idx % 16; + } +} + +static void do_EPB_epasControl(uint32_t RIR, uint32_t RDTR) { + uint32_t MLB; + uint32_t MHB; + MLB = 0x01 + (EPB_epasControl_idx << 8); + MHB = 0x00; + int cksm = add_tesla_cksm2(MLB, MHB, 0x214, 2); + MLB = MLB + (cksm << 16); + if (DAS_noEpasHarness == 0) { + send_fake_message(RIR,RDTR,3,0x214,2,MLB,MHB); + } + EPB_epasControl_idx++; + EPB_epasControl_idx = EPB_epasControl_idx % 16; +} + +static void do_fake_stalk_cancel(uint32_t RIR, uint32_t RDTR) { + uint32_t MLB; + uint32_t MHB; + if ((DAS_lastStalkL == 0x00) && (DAS_lastStalkH == 0x00)) { + return; + } + MLB = (DAS_lastStalkL & 0xFFFFFFC0) + 0x01; + MHB = (DAS_lastStalkH & 0x000FFFFF); + int idx = (DAS_lastStalkH & 0xF00000 ) >> 20; + idx = ( idx + 1 ) % 16; + MHB = MHB + (idx << 20); + int crc = add_tesla_crc(MLB, MHB,7); + MHB = MHB + (crc << 24); + DAS_lastStalkH = MHB; + send_fake_message(RIR,RDTR,8,0x45,0,MLB,MHB); +} + + + +static int tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) +{ + bool valid = false; + //not much to check yet on these, each tesla messaage has checksum in a different place + + + if (DAS_noEpasHarness == 0) { + set_gmlan_digital_output(GMLAN_HIGH); + reset_gmlan_switch_timeout(); //we're still in tesla safety mode, reset the timeout counter and make sure our output is enabled + } + uint32_t ts = TIM2->CNT; + + uint8_t bus_number = (to_push->RDTR >> 4) & 0xFF; + uint32_t addr; + + if (to_push->RIR & 4) + { + // Extended + // Not looked at, but have to be separated + // to avoid address collision + addr = to_push->RIR >> 3; + } + else + { + // Normal + addr = to_push->RIR >> 21; + } + if (bus_number == 1) { + //radar is always accepted + valid = true; + } else + if ((bus_number == 0) || (bus_number == 2)) { + if ((addr == 0x00E ) || (addr == 0x045 ) || (addr == 0x108 ) || + (addr == 0x118 ) || (addr == 0x318 ) || (addr == 0x348 ) || (addr == 0x368)) + { + valid = addr_safety_check(to_push, tesla_rx_checks, TESLA_RX_CHECK_LEN, + NULL, NULL, NULL); + } else { + valid = true; + } + + } + + if ((addr == 0x398) && (bus_number == 0)) { + if (DAS_gtwConfigReceived == 0) { + DAS_gtwConfigReceived = 1; + int dashw = ((to_push->RDLR >> 6) & 0x03); + int radhw = ((to_push->RDLR >> 10) & 0x03); + if (dashw == 1) { + DAS_noEpasHarness = 1; + enable_das_emulation = 1; + } + if (radhw > 0) { + enable_radar_emulation = 1; + } + } + } + + //let's see if the pedal was pressed + if ((addr == 0x552) && (bus_number == tesla_epas_can)) { + //m1 = 0.050796813 + //m2 = 0.101593626 + //d = -22.85856576 + DAS_pedalPressed = (int)((((to_push->RDLR & 0xFF00) >> 8) + ((to_push->RDLR & 0xFF) << 8)) * 0.050796813 -22.85856576); + } + + //we use 0x108 at 100Hz to detect timing of messages sent by our fake DAS and EPB + if ((addr == 0x108) && (bus_number == 0)) { + if (DAS_gtwConfigReceived == 1) { + if ((fake_DAS_counter % 10 == 5) && (DAS_noEpasHarness == 0)) { + do_EPB_epasControl(to_push->RIR,to_push->RDTR); + } + do_fake_DAS(to_push->RIR,to_push->RDTR); + } + return valid; + } + + // Record the current car time in current_car_time (for use with double-pulling cruise stalk) + if ((addr == 0x318) && (bus_number == 0)) { + int hour = (to_push->RDLR & 0x1F000000) >> 24; + int minute = (to_push->RDHR & 0x3F00) >> 8; + int second = (to_push->RDLR & 0x3F0000) >> 16; + current_car_time = (hour * 3600) + (minute * 60) + second; + } + + //we use EPAS_sysStatus 0x370 to determine if the car is off or on + if ((addr == 0x370) && (bus_number != 1)) { + time_last_EPAS_data = current_car_time; + } + + //we just passed EPAS checkpoint let's see if we have timeout and send car off message + if (current_car_time - time_last_EPAS_data > 2) { + //no message in the last 2 seconds, car is off + // GTW_status + time_last_EPAS_data = -10; + } else { + } -static int tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - set_gmlan_digital_output(0); // #define GMLAN_HIGH 0 - reset_gmlan_switch_timeout(); //we're still in tesla safety mode, reset the timeout counter and make sure our output is enabled + //see if cruise is enabled [Enabled, standstill or Override] and cancel if using pedal + if ((addr == 0x368) && (bus_number == 0)) { + //first save values for spamming + DAS_diStateL = to_push->RDLR; + DAS_diStateH = to_push->RDHR; + DAS_diState_idx = (DAS_diStateH & 0xF000 ) >> 12; + int acc_state = ((to_push->RDLR & 0xF000) >> 12); + DAS_uom = (to_push->RDLR >> 31) & 1; + //if pedal and CC looks enabled, disable + if ((DAS_usingPedal == 1) && ( acc_state >= 2) && ( acc_state <= 4)) { + do_fake_stalk_cancel(to_push->RIR, to_push->RDTR); + } + //if ACC not enabled but CC is enabled, disable if more than 2 seconds since last pull + if ((EON_is_connected == 1) && (DAS_usingPedal == 0) && (DAS_cc_state != 2) && ( acc_state >= 2) && ( acc_state <= 4)) { + //disable if more than two seconds since last pull, or there was never a stalk pull + if (((current_car_time >= time_at_last_stalk_pull + 2) && (current_car_time != -1) && (time_at_last_stalk_pull != -1)) || (time_at_last_stalk_pull == -1)) { + if (DAS_plain_cc_enabled == 0) { + do_fake_stalk_cancel(to_push->RIR, to_push->RDTR); + } + } + } + } - int addr = GET_ADDR(to_push); + //looking for radar messages; + if ((addr == 0x300) && (bus_number == tesla_radar_can)) + { + uint32_t ts = TIM2->CNT; + uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_last_radar_signal); + if (tesla_radar_status == 1) { + tesla_radar_status = 2; + puts("Tesla Radar Active! \n"); + tesla_last_radar_signal = ts; + } else + if ((ts_elapsed > TESLA_RADAR_TIMEOUT) && (tesla_radar_status > 0)) { + tesla_radar_status = 0; + puts("Tesla Radar Inactive! (timeout 1) \n"); + } else + if ((ts_elapsed <= TESLA_RADAR_TIMEOUT) && (tesla_radar_status == 2)) { + tesla_last_radar_signal = ts; + } + } + + //0x631 is sent by radar to initiate the sync + if ((addr == 0x631) && (bus_number == tesla_radar_can)) + { + uint32_t ts = TIM2->CNT; + uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_last_radar_signal); + if (tesla_radar_status == 0) { + tesla_radar_status = 1; + tesla_last_radar_signal = ts; + puts("Tesla Radar Initializing... \n"); + } else + if ((ts_elapsed > TESLA_RADAR_TIMEOUT) && (tesla_radar_status > 0)) { + tesla_radar_status = 0; + puts("Tesla Radar Inactive! (timeout 2) \n"); + } else + if ((ts_elapsed <= TESLA_RADAR_TIMEOUT) && (tesla_radar_status > 0)) { + tesla_last_radar_signal = ts; + } + } - if (addr == 0x45) { + if ((addr == 0x45) && (bus_number == 0)) { + //first save for future use + DAS_lastStalkL = to_push->RDLR; + DAS_lastStalkH = to_push->RDHR; // 6 bits starting at position 0 - int lever_position = GET_BYTE(to_push, 0) & 0x3F; - if (lever_position == 2) { // pull forward + int ap_lever_position = GET_BYTE(to_push, 0) & 0x3F; + if (ap_lever_position == 2) + { // pull forward // activate openpilot + // TODO: uncomment the if to use double pull to activate + //if (current_car_time <= time_at_last_stalk_pull + 1 && current_car_time != -1 && time_at_last_stalk_pull != -1) { controls_allowed = 1; + //} + time_at_last_stalk_pull = current_car_time; } - if (lever_position == 1) { // push towards the back + else if (ap_lever_position == 1) + { // push towards the back // deactivate openpilot controls_allowed = 0; } + //if using pedal, send a cancel immediately to cancel the pedal + if ((DAS_usingPedal == 1) && (ap_lever_position > 1)) { + do_fake_stalk_cancel(to_push->RIR, to_push->RDTR); + } + /* <-- revB giraffe GPIO */ + /* + int turn_signal_lever = (to_push->RDLR >> 16) & 0x3; //TurnIndLvr_Stat : 16|2@1+ + int stw_menu_button = (to_push->RDHR >> 5) & 0x1; //StW_Sw05_Psd : 37|1@1+ + high_beam_lever_state = (to_push->RDLR >> 18) & 0x3; //SG_ HiBmLvr_Stat : 18|2@1+ + + //TurnIndLvr_Stat 3 "SNA" 2 "RIGHT" 1 "LEFT" 0 "IDLE" ; + if (turn_signal_lever == 1) + { + //Left turn signal is on, turn on output pin 3 + set_uja1023_output_bits(1 << 3); + //puts(" Left turn on!\n"); + } + else + { + clear_uja1023_output_bits(1 << 3); + } + if (turn_signal_lever == 2) + { + //Right turn signal is on, turn on output pin 4 + set_uja1023_output_bits(1 << 4); + //puts(" Right turn on!\n"); + } + else + { + clear_uja1023_output_bits(1 << 4); + } + + if (stw_menu_button == 1) + { + //menu button is pushed, if it wasn't last time, set the initial timestamp + if (stw_menu_btn_state_last == 0) + { + stw_menu_btn_state_last = 1; + stw_menu_btn_pressed_ts = ts; + } + else + { + uint32_t stw_ts_elapsed = get_ts_elapsed(ts, stw_menu_btn_pressed_ts); + if (stw_ts_elapsed > STW_MENU_BTN_HOLD_INTERVAL) + { + //user held the button, do stuff! + if (stw_menu_current_output_state == 0 && stw_menu_output_flag == 0) + { + stw_menu_output_flag = 1; + stw_menu_current_output_state = 1; + //set_uja1023_output_bits(1 << 5); + //puts("Menu Button held, setting output 5 HIGH\n"); + } + else if (stw_menu_current_output_state == 1 && stw_menu_output_flag == 0) + { + stw_menu_output_flag = 1; + stw_menu_current_output_state = 0; + //clear_uja1023_output_bits(1 << 5); + //puts("Menu Button held, setting output 5 LOW\n"); + } + } //held + } + } //stw menu button pressed + else if (stw_menu_button == 0) + { + stw_menu_output_flag = 0; + stw_menu_btn_state_last = 0; + } + */ + /* revB giraffe GPIO --> */ } + // Detect drive rail on (ignition) (start recording) + if ((addr == 0x348) && (bus_number == 0)) + { + //ALSO use this for radar timeout, this message is always on + uint32_t ts = TIM2->CNT; + uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_last_radar_signal); + if ((ts_elapsed > TESLA_RADAR_TIMEOUT) && (tesla_radar_status > 0)) { + tesla_radar_status = 0; + puts("Tesla Radar Inactive! (timeout 3) \n"); + } + } + + // exit controls on brake press // DI_torque2::DI_brakePedal 0x118 - if (addr == 0x118) { - // 1 bit at position 16 - if ((GET_BYTE(to_push, 1) & 0x80) != 0) { - // disable break cancel by commenting line below - controls_allowed = 0; + + /* revB giraffe GPIO --> */ + /* + if ((addr == 0x118) && (bus_number == 0)) + { + int drive_state = (to_push->RDLR >> 12) & 0x7; //DI_gear : 12|3@1+ + int brake_pressed = (to_push->RDLR & 0x8000) >> 15; + int tesla_speed_mph = (((((GET_BYTE(to_push, 3) & 0xF) << 8) + GET_BYTE(to_push, 2)) * 0.05) - 25); + + + + //if the car goes into reverse, set UJA1023 output pin 5 to high. If Drive, set pin 1 high + //DI_gear 7 "DI_GEAR_SNA" 4 "DI_GEAR_D" 3 "DI_GEAR_N" 2 "DI_GEAR_R" 1 "DI_GEAR_P" 0 "DI_GEAR_INVALID" ; + //UJA1023 pin0 is our output to the camera switcher + + if (drive_state == 2) + { + //reverse_state = 1; + set_uja1023_output_bits(1 << 5); + + //if we're in reverse, we always want the rear camera up: + set_uja1023_output_bits(1 << 0); //show rear camera + //puts(" Got Reverse\n"); + + } + else + { + //reverse_state = 0; + clear_uja1023_output_bits(1 << 5); + + //if we're in not in reverse and button state is 0, set output low (show front camera) + if (stw_menu_current_output_state == 0) + { + clear_uja1023_output_bits(1 << 0); //show front camera + } + + //if we're not in reverse and button state is 1, set the output high (show the rear camera) + else + { + set_uja1023_output_bits(1 << 0); //show rear camera + } + } + + if (drive_state == 4) + { + set_uja1023_output_bits(1 << 1); + //puts(" Got Drive\n"); + } + else + { + clear_uja1023_output_bits(1 << 1); } - //get vehicle speed in m/s. Tesla gives MPH - tesla_speed = (((((GET_BYTE(to_push, 3) & 0xF) << 8) + GET_BYTE(to_push, 2)) * 0.05) - 25) * 1.609 / 3.6; - if (tesla_speed < 0) { + + if (brake_pressed == 1) + { + //disable break cancel by commenting line below + //controls_allowed = 0; + + set_uja1023_output_bits(1 << 2); + //puts(" Brake on!\n"); + tesla_ts_brakelight_on_last = ts; + } + else + { + uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_ts_brakelight_on_last); + if (ts_elapsed > BRAKELIGHT_CLEAR_INTERVAL) + { + clear_uja1023_output_bits(1 << 2); + //puts(" Brakelight off!\n"); + } + } + } + */ + /* revB giraffe GPIO --> */ + if ((addr == 0x118) && (bus_number == 0)) + { + int tesla_speed_mph = (((((GET_BYTE(to_push, 3) & 0xF) << 8) + GET_BYTE(to_push, 2)) * 0.05) - 25); + + + //get vehicle speed in m/2. Tesla gives MPH + tesla_speed = (tesla_speed_mph*1.609/3.6); + if (tesla_speed < 0) + { tesla_speed = 0; } + DAS_inDrive_prev = DAS_inDrive; + if (((to_push->RDLR & 0x7000 ) >> 12) == 4) { + DAS_inDrive = 1; + } else { + DAS_inDrive = 0; + } + if ((DAS_inDrive == 0) && (DAS_inDrive_prev == 1)) { + //BB_no_harness: why are we resetting here? + //reset_DAS_data(); + } } // exit controls on EPAS error // EPAS_sysStatus::EPAS_eacStatus 0x370 - if (addr == 0x370) { + //BB this was on bus_number 1 which is wrong, but not tested on 2 yet + if ((addr == 0x370) && (bus_number == tesla_epas_can)) + { // if EPAS_eacStatus is not 1 or 2, disable control eac_status = (GET_BYTE(to_push, 6) >> 5) & 0x7; // For human steering override we must not disable controls when eac_status == 0 - // Additional safety: we could only allow eac_status == 0 when we have human steering allowed - if (controls_allowed && (eac_status != 0) && (eac_status != 1) && (eac_status != 2)) { + // Additional safety: we could only allow eac_status == 0 when we have human steerign allowed + if ((controls_allowed == 1) && (eac_status != 0) && (eac_status != 1) && (eac_status != 2)) + { controls_allowed = 0; - //puts("EPAS error! \n"); + puts("EPAS error! \n"); } } //get latest steering wheel angle - if (addr == 0x00E) { - float angle_meas_now = (int)(((((GET_BYTE(to_push, 0) & 0x3F) << 8) + GET_BYTE(to_push, 1)) * 0.1) - 819.2); - uint32_t ts = TIM2->CNT; + if ((addr == 0x00E) && (bus_number == 0)) + { + int angle_meas_now = (int)(((((GET_BYTE(to_push, 0) & 0x3F) << 8) + GET_BYTE(to_push, 1)) * 0.1) - 819.2); + //uint32_t ts = TIM2->CNT; uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_ts_angle_last); // *** angle real time check // add 1 to not false trigger the violation and multiply by 25 since the check is done every 250 ms and steer angle is updated at 100Hz - float rt_delta_angle_up = (interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed) * 25.) + 1.; - float rt_delta_angle_down = (interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) * 25.) + 1.; - float highest_rt_angle = tesla_rt_angle_last + ((tesla_rt_angle_last > 0.) ? rt_delta_angle_up : rt_delta_angle_down); - float lowest_rt_angle = tesla_rt_angle_last - ((tesla_rt_angle_last > 0.) ? rt_delta_angle_down : rt_delta_angle_up); + int rt_delta_angle_up = ((int)((tesla_interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed) * 25. + 1.))); + int rt_delta_angle_down = ((int)((tesla_interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) * 25. + 1.))); + int highest_rt_angle = tesla_rt_angle_last + (tesla_rt_angle_last > 0 ? rt_delta_angle_up : rt_delta_angle_down); + int lowest_rt_angle = tesla_rt_angle_last - (tesla_rt_angle_last > 0 ? rt_delta_angle_down : rt_delta_angle_up); - if ((ts_elapsed > TESLA_RT_INTERVAL) || (controls_allowed && !tesla_controls_allowed_last)) { + if ((ts_elapsed > TESLA_RT_INTERVAL) || (controls_allowed && !tesla_controls_allowed_last)) + { tesla_rt_angle_last = angle_meas_now; tesla_ts_angle_last = ts; } + // update array of samples + update_sample(&tesla_angle_meas, angle_meas_now); + // check for violation; - if (fmax_limit_check(angle_meas_now, highest_rt_angle, lowest_rt_angle)) { + if (max_limit_check(angle_meas_now, highest_rt_angle, lowest_rt_angle)) + { // We should not be able to STEER under these conditions // Other sending is fine (to allow human override) - controls_allowed = 0; - //puts("WARN: RT Angle - No steer allowed! \n"); - } else { - controls_allowed = 1; + steer_allowed = 0; + puts("WARN: RT Angle - No steer allowed! \n"); + } + else + { + steer_allowed = 1; } tesla_controls_allowed_last = controls_allowed; } - return 1; + + /* <-- revB giraffe GPIO */ + /* + //BO_ 1001 DAS_bodyControls: 8 XXX + if ((addr == 0x3e9) && (bus_number == 0)) { + int high_beam_decision = (to_push->RDLR >> 10) & 0x3; //DAS_highLowBeamDecision : 10|2@1+ + // highLowBeamDecision: + //0: Undecided (Car off) + //1: Off + //2: On + //3: Auto High Beam is disabled + //VAL_ 69 HiBmLvr_Stat 3 "SNA" 2 "HIBM_FLSH_ON_PSD" 1 "HIBM_ON_PSD" 0 "IDLE" ; + + //If the lever is in either high beam position and auto high beam is off or indicates highs should be on + if ((high_beam_decision == 3 && (high_beam_lever_state == 2 || high_beam_lever_state == 1)) + || (high_beam_decision == 2 && (high_beam_lever_state == 2 || high_beam_lever_state == 1))) + { + //high beams are on. Set the output 6 high + set_uja1023_output_bits(1 << 6); + //puts("High Beam on!\n"); + } //high beams on! + else + { + //high beams are off. Set the output 6 low + clear_uja1023_output_bits(1 << 6); + //puts("High Beam off!\n"); + } //high beams off + } //DAS_bodyControls + + //BO_ 872 DI_state: 8 DI + if ((addr == 0x368) && (bus_number == 0)) { + int regen_brake_light = (to_push->RDLR >> 8) & 0x1; //DI_regenLight : 8|1@1+ + //if the car's brake lights are on, set pin 2 to high + if (regen_brake_light == 1) + { + set_uja1023_output_bits(1 << 2); + //puts(" Regen Brake Light on!\n"); + tesla_ts_brakelight_on_last = ts; + } + else + { + uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_ts_brakelight_on_last); + if (ts_elapsed > BRAKELIGHT_CLEAR_INTERVAL) + { + clear_uja1023_output_bits(1 << 2); + //puts(" Brakelight off!\n"); + } + } + }*/ + /* revB giraffe GPIO --> */ + return valid; } // all commands: gas/regen, friction brake and steering @@ -127,80 +1485,605 @@ static int tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // else // block all commands that produce actuation -static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { +static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) +{ + + uint32_t addr; + int desired_angle; + + addr = to_send->RIR >> 21; - int tx = 1; - int addr = GET_ADDR(to_send); + //capture message for radarVIN and settings + if (addr == 0x560) { + int id = (to_send->RDLR & 0xFF); + int radarVin_b1 = ((to_send->RDLR >> 8) & 0xFF); + int radarVin_b2 = ((to_send->RDLR >> 16) & 0xFF); + int radarVin_b3 = ((to_send->RDLR >> 24) & 0xFF); + int radarVin_b4 = (to_send->RDHR & 0xFF); + int radarVin_b5 = ((to_send->RDHR >> 8) & 0xFF); + int radarVin_b6 = ((to_send->RDHR >> 16) & 0xFF); + int radarVin_b7 = ((to_send->RDHR >> 24) & 0xFF); + if (id == 0) { + tesla_radar_should_send = (radarVin_b2 & 0x01); + radarPosition = ((radarVin_b2 >> 1) & 0x03); + radarEpasType = ((radarVin_b2 >> 3) & 0x07); + tesla_radar_trigger_message_id = (radarVin_b3 << 8) + radarVin_b4; + tesla_radar_can = radarVin_b1; + radar_VIN[0] = radarVin_b5; + radar_VIN[1] = radarVin_b6; + radar_VIN[2] = radarVin_b7; + tesla_radar_vin_complete = tesla_radar_vin_complete | 1; + } + if (id == 1) { + radar_VIN[3] = radarVin_b1; + radar_VIN[4] = radarVin_b2; + radar_VIN[5] = radarVin_b3; + radar_VIN[6] = radarVin_b4; + radar_VIN[7] = radarVin_b5; + radar_VIN[8] = radarVin_b6; + radar_VIN[9] = radarVin_b7; + tesla_radar_vin_complete = tesla_radar_vin_complete | 2; + } + if (id == 2) { + radar_VIN[10] = radarVin_b1; + radar_VIN[11] = radarVin_b2; + radar_VIN[12] = radarVin_b3; + radar_VIN[13] = radarVin_b4; + radar_VIN[14] = radarVin_b5; + radar_VIN[15] = radarVin_b6; + radar_VIN[16] = radarVin_b7; + tesla_radar_vin_complete = tesla_radar_vin_complete | 4; + } + return false; + } + //capture message for fake DAS street sign object + if (addr == 0x556) { + streetSignObject_b0 = (to_send->RDLR & 0xFF); + streetSignObject_b1 = ((to_send->RDLR >> 8) & 0xFF); + streetSignObject_b2 = ((to_send->RDLR >> 16) & 0xFF); + streetSignObject_b3 = ((to_send->RDLR >> 24) & 0xFF); + streetSignObject_active = 0; + int signType = ((streetSignObject_b0 >> 6) & 0x03) + ((streetSignObject_b1 << 2) & 0xFF); + if (signType < 0xFF) { + streetSignObject_active = 1; + } + return false; + } - // do not transmit CAN message if steering angle too high - // DAS_steeringControl::DAS_steeringAngleRequest - if (addr == 0x488) { - float angle_raw = ((GET_BYTE(to_send, 0) & 0x7F) << 8) + GET_BYTE(to_send, 1); - float desired_angle = (angle_raw * 0.1) - 1638.35; - bool violation = 0; - int st_enabled = GET_BYTE(to_send, 2) & 0x40; + //capture message for DAS objects + if (addr == 0x559) { + int lane = (to_send->RDLR & 0x03); + if (lane == 0) { + DAS_LEAD_OBJECT_MLB = to_send->RDLR; + DAS_LEAD_OBJECT_MHB = to_send->RDHR; + if(((to_send->RDLR >> 8) & 0xFF) == 0) { + DAS_LEAD_OBJECT_MLB = 0xFFFFFF00; + DAS_LEAD_OBJECT_MHB = 0x03FFFF83; + } + } + if (lane == 1) { + DAS_LEFT_OBJECT_MLB = to_send->RDLR; + DAS_LEFT_OBJECT_MHB = to_send->RDHR; + if(((to_send->RDLR >> 8) & 0xFF) == 0) { + DAS_LEFT_OBJECT_MLB = 0xFFFFFF01; + DAS_LEFT_OBJECT_MHB = 0x03FFFF83; + } + } + if (lane == 2) { + DAS_RIGHT_OBJECT_MLB = to_send->RDLR; + DAS_RIGHT_OBJECT_MHB = to_send->RDHR; + if(((to_send->RDLR >> 8) & 0xFF) == 0) { + DAS_RIGHT_OBJECT_MLB = 0xFFFFFF02; + DAS_RIGHT_OBJECT_MHB = 0x03FFFF83; + } + } + return false; + } - if (st_enabled == 0) { + //capture message for fake DAS lane and object info + if (addr == 0x557) { + tLeadDx = (to_send->RDLR & 0xFF); + tLeadDy = ((to_send->RDLR >> 8) & 0xFF); + rLine_qual = ((to_send->RDLR >> 16) & 0x03); + lLine_qual = ((to_send->RDLR >> 18) & 0x03); + if (rLine_qual > 1) { + rLine = 1; + } else { + rLine = 0; + } + if (lLine_qual > 1) { + lLine = 1; + } else { + lLine = 0; + } + lWidth = ((to_send->RDLR >> 20) & 0x0F); + curvC0 = ((to_send->RDLR >> 24) & 0xFF); + curvC1 = (to_send->RDHR & 0xFF); + curvC2 = ((to_send->RDHR >> 8) & 0xFF); + curvC3 = ((to_send->RDHR >> 16) & 0xFF); + laneRange = ((to_send->RDHR >> 24) & 0x3F) * 4; + tLeadClass = ((to_send->RDHR >> 30) & 0x03); + tLeadClass ++; + //we used 4 - bicycle for 5-pedestrian + if (tLeadClass == 4) { + tLeadClass = 5; + } + return false; + } + + + //capture message for fake DAS warning + if (addr == 0x554) { + int b0 = (to_send->RDLR & 0xFF); + int b1 = ((to_send->RDLR >> 8) & 0xFF); + int b2 = ((to_send->RDLR >> 16) & 0xFF); + DAS_211_accNoSeatBelt = ((b0 >> 4) & 0x01); + DAS_canErrors = ((b0 >> 3) & 0x01); + DAS_202_noisyEnvironment = ((b0 >> 2) & 0x01); + DAS_doorOpen = ((b0 >> 1) & 0x01); + DAS_notInDrive = ((b0 >> 0) & 0x01); + enable_das_emulation = ((b0 >> 5) & 0x01); + enable_radar_emulation = ((b0 >> 6) & 0x01); + stopLightWarning = ((b0 >> 7) & 0x01); + stopSignWarning = ((b1 >> 0) & 0x01); + DAS_222_accCameraBlind = ((b1 >> 1) & 0x01); //we will not override the one set in 0x553 + DAS_219_lcTempUnavailableSpeed = ((b1 >> 2) & 0x01); + DAS_220_lcTempUnavailableRoad = ((b1 >> 3) & 0x01); + DAS_221_lcAborting = ((b1 >> 4) & 0x01); + DAS_207_lkasUnavailable = ((b1 >> 5) & 0x01); + DAS_208_rackDetected = ((b1 >> 6) & 0x01); + DAS_025_steeringOverride = ((b1 >> 7) & 0x01); + DAS_ldwStatus = (b2 & 0x07); + //FLAG NOT USED = ((b2 >> 3) & 0x01); + DAS_noEpasHarness = ((b2 >> 4) & 0x01); + DAS_usesApillarHarness = ((b2 >> 5) & 0x01); + if (DAS_noEpasHarness == 1) { + tesla_epas_can = 0; + } else { + tesla_epas_can = 2; + } + return false; + } + + //capture message for AHB and parse + if (addr == 0x65A) { + int b0 = (to_send->RDLR & 0xFF); + int b1 = ((to_send->RDLR >> 8) & 0xFF); + int b2 = ((to_send->RDLR >> 16) & 0xFF); + DAS_high_low_beam_request = b0; + DAS_high_low_beam_reason = b1; + DAS_ahb_is_enabled = b2 & 0x01; + DAS_fleet_speed_state = (b2 >> 1) & 0x03; + //intercept and do not forward + return false; + } + + //capture message for fake DAS and parse + if (addr == 0x659) { //0x553) { + int b0 = (to_send->RDLR & 0xFF); + int b1 = ((to_send->RDLR >> 8) & 0xFF); + int b2 = ((to_send->RDLR >> 16) & 0xFF); + int b3 = ((to_send->RDLR >> 24) & 0xFF); + int b4 = (to_send->RDHR & 0xFF); + int b5 = ((to_send->RDHR >> 8) & 0xFF); + int b6 = ((to_send->RDHR >> 16) & 0xFF); + int b7 = ((to_send->RDHR >> 24) & 0xFF); + + DAS_acc_speed_kph = b1; + DAS_acc_speed_limit = b4; + DAS_longC_enabled = ((b0 & 0x80) >> 7); + DAS_gas_to_resume = ((b0 & 0x40) >> 6); + DAS_206_apUnavailable = ((b0 & 0x20) >> 5); + DAS_collision_warning = ((b0 & 0x10) >> 4); + DAS_op_status = (b0 & 0x0F); + DAS_turn_signal_request = ((b2 & 0xC0) >> 6); + DAS_forward_collision_warning = ((b2 & 0x10) >> 4); + DAS_units_included = ((b2 & 0x20) >> 5); + if (((b2 >> 3) & 0x01) == 0) { + DAS_plain_cc_enabled = 1; + } else { + DAS_plain_cc_enabled = 0; + } + DAS_hands_on_state = (b2 & 0x07); + DAS_cc_state = ((b3 & 0xC0)>>6); + DAS_usingPedal = ((b3 & 0x20) >> 5); + DAS_alca_state = (b3 & 0x1F); + DAS_speed_limit_kph = (b5 & 0x1F); + DAS_emergency_brake_request = ((b5 & 0x20) >> 5); + time_last_DAS_data = current_car_time; + DAS_present = 1; + DAS_steeringAngle = ((b7 << 8) + b6) & 0x7FFF; + DAS_steeringEnabled = (b7 >> 7); + + desired_angle = DAS_steeringAngle * 0.1 - 1638.35; + //bool violation = 0; + + if (DAS_steeringEnabled == 0) { //steering is not enabled, do not check angles and do send tesla_desired_angle_last = desired_angle; - } else if (controls_allowed) { + } else + if (controls_allowed) + { // add 1 to not false trigger the violation - float delta_angle_up = interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed) + 1.; - float delta_angle_down = interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) + 1.; - float highest_desired_angle = tesla_desired_angle_last + ((tesla_desired_angle_last > 0.) ? delta_angle_up : delta_angle_down); - float lowest_desired_angle = tesla_desired_angle_last - ((tesla_desired_angle_last > 0.) ? delta_angle_down : delta_angle_up); - float TESLA_MAX_ANGLE = interpolate(TESLA_LOOKUP_MAX_ANGLE, tesla_speed) + 1.; - - //check for max angles - violation |= fmax_limit_check(desired_angle, TESLA_MAX_ANGLE, -TESLA_MAX_ANGLE); - - //check for angle delta changes - violation |= fmax_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle); + int delta_angle_up = (int)(tesla_interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed) * 25. + 1.); + int delta_angle_down = (int)(tesla_interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) * 25. + 1.); + int highest_desired_angle = tesla_desired_angle_last + (tesla_desired_angle_last > 0 ? delta_angle_up : delta_angle_down); + int lowest_desired_angle = tesla_desired_angle_last - (tesla_desired_angle_last > 0 ? delta_angle_down : delta_angle_up); + int TESLA_MAX_ANGLE = (int)(tesla_interpolate(TESLA_LOOKUP_MAX_ANGLE, tesla_speed) + 1.); - if (violation) { + if (max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle)) + { + //violation = 1; controls_allowed = 0; - tx = 0; + puts("Angle limit - delta! \n"); + } + if (max_limit_check(desired_angle, TESLA_MAX_ANGLE, -TESLA_MAX_ANGLE)) + { + //violation = 1; + controls_allowed = 0; + puts("Angle limit - max! \n"); } - tesla_desired_angle_last = desired_angle; - } else { - tx = 0; } + tesla_desired_angle_last = desired_angle; + + return true; //we have to send this to the spamIC } - return tx; + + return true; } -static void tesla_init(int16_t param) { +static void tesla_init(int16_t param) +{ UNUSED(param); - controls_allowed = 0; - gmlan_switch_init(1); //init the gmlan switch with 1s timeout enabled + if (DAS_noEpasHarness == 0) { + gmlan_switch_init(1); //init the gmlan switch with 1s timeout enabled + } + //uja1023_init(); +} + + +static void tesla_fwd_to_radar_as_is(uint8_t bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { + if ((enable_radar_emulation == 0) || (tesla_radar_vin_complete !=7) || (tesla_radar_should_send==0) ) { + return; + } + CAN_FIFOMailBox_TypeDef to_send; + to_send.RIR = to_fwd->RIR | 1; // TXRQ + to_send.RDTR = to_fwd->RDTR; + to_send.RDLR = to_fwd->RDLR; + to_send.RDHR = to_fwd->RDHR; + can_send(&to_send, bus_num, true); +} + + +static uint32_t radar_VIN_char(int pos, int shift) { + return (((int)radar_VIN[pos]) << (shift * 8)); +} + + +static void tesla_fwd_to_radar_modded(uint8_t bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { + if ((enable_radar_emulation == 0) || (tesla_radar_vin_complete !=7) || (tesla_radar_should_send==0) ) { + return; + } + int32_t addr = to_fwd->RIR >> 21; + CAN_FIFOMailBox_TypeDef to_send; + to_send.RIR = to_fwd->RIR | 1; // TXRQ + to_send.RDTR = to_fwd->RDTR; + to_send.RDLR = to_fwd->RDLR; + to_send.RDHR = to_fwd->RDHR; + uint32_t addr_mask = 0x001FFFFF; + //now modd messages as needed + // if DAS_noEpasHarness == 1, only modify IDs because we have iBooster and thus everything else is correct + if (addr == 0x405 ) + { + to_send.RIR = (0x2B9 << 21) + (addr_mask & (to_fwd->RIR | 1)); + if (((to_send.RDLR & 0x10) == 0x10) && (sizeof(radar_VIN) >= 4) && (DAS_noEpasHarness == 0)) + { + int rec = to_send.RDLR & 0xFF; + if (rec == 0x10) { + to_send.RDLR = 0x00000000 | rec; + to_send.RDHR = radar_VIN_char(0,1) | radar_VIN_char(1,2) | radar_VIN_char(2,3); + } + if (rec == 0x11) { + to_send.RDLR = rec | radar_VIN_char(3,1) | radar_VIN_char(4,2) | radar_VIN_char(5,3); + to_send.RDHR = radar_VIN_char(6,0) | radar_VIN_char(7,1) | radar_VIN_char(8,2) | radar_VIN_char(9,3); + } + if (rec == 0x12) { + to_send.RDLR = rec | radar_VIN_char(10,1) | radar_VIN_char(11,2) | radar_VIN_char(12,3); + to_send.RDHR = radar_VIN_char(13,0) | radar_VIN_char(14,1) | radar_VIN_char(15,2) | radar_VIN_char(16,3); + } + } + can_send(&to_send, bus_num, true); + + return; + } + if (addr == 0x398 ) + { + + if (DAS_noEpasHarness == 0) { + //change frontradarHW = 1 and dashw = 1 + //SG_ GTW_dasHw : 7|2@0+ (1,0) [0|0] "" NEO + //SG_ GTW_parkAssistInstalled : 11|2@0+ (1,0) [0|0] "" NEO + + to_send.RDHR = to_send.RDHR | 0x100; //TODO if this is Park Assist, it should be RDLR not RDHR + //resend on CAN 0 first + to_send.RIR = (to_fwd->RIR | 1); + //can_send(&to_send,0, true); + + + to_send.RDLR = to_send.RDLR & 0xFFFFF33F; + to_send.RDLR = to_send.RDLR | 0x440; + // change the autopilot to 1 + to_send.RDHR = to_fwd->RDHR & 0xCFFF0F0F; + to_send.RDHR = to_send.RDHR | 0x10000000 | (radarPosition << 4) | (radarEpasType << 12); + + if ((sizeof(radar_VIN) >= 4) && (((int)(radar_VIN[7]) == 0x32) || ((int)(radar_VIN[7]) == 0x34))) { + //also change to AWD if needed (most likely) if manual VIN and if position 8 of VIN is a 2 (dual motor) + to_send.RDLR = to_send.RDLR | 0x08; + } + } + //now change address and send to radar + to_send.RIR = (0x2A9 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num, true); + + return; + } + if (addr == 0x00E ) + { + to_send.RIR = (0x199 << 21) + (addr_mask & (to_fwd->RIR | 1)); + if (DAS_noEpasHarness == 0) { + //check if angular speed sends SNA (0x3FFF) + if (((to_send.RDLR >> 16) & 0xFF3F) == 0xFF3F) { + //if yes replace 0x3FFFF with 0x2000 which is 0 angular change + to_send.RDLR = (to_send.RDLR & 0x00C0FFFF) | (0x0020 << 16); + //if this is the case, most likely we need to change the model too + //so remove CRC and StW_AnglHP_Sens_Id (1st octet of RDHR) + to_send.RDHR = to_send.RDHR & 0x00FFFFF0; + //force StW_AnglHP_Sens_Id to DELPHI (0x04 1st octet of RDHR) + to_send.RDHR = to_send.RDHR | 0x00000004; + //compute new CRC + int crc = add_tesla_crc(to_send.RDLR, to_send.RDHR,7); + //Add new CRC + to_send.RDHR = to_send.RDHR | (crc << 24); + } + } + can_send(&to_send, bus_num, true); + return; + } + if (addr == 0x20A ) + { + to_send.RIR = (0x159 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num, true); + + return; + } + + if ((addr == 0x148) && (DAS_noEpasHarness == 1)) + { + to_send.RIR = (0x1A9 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num, true); + return; + } + + if (addr == 0x115 ) + { + + int counter = ((to_fwd->RDHR & 0xF0) >> 4 ) & 0x0F; + + to_send.RIR = (0x129 << 21) + (addr_mask & (to_fwd->RIR | 1)); + int cksm = (0x16 + (counter << 4)) & 0xFF; + can_send(&to_send, bus_num, true); + + if (DAS_noEpasHarness == 0) { + //we don't get 0x148 DI_espControl so send as 0x1A9 on CAN1 and also as 0x148 on CAN0 + to_send.RDTR = (to_fwd->RDTR & 0xFFFFFFF0) | 0x05; + to_send.RIR = (0x148 << 21) + (addr_mask & (to_fwd->RIR | 1)); + to_send.RDLR = 0x000C0000 | (counter << 28); + cksm = (0x38 + 0x0C + (counter << 4)) & 0xFF; + to_send.RDHR = cksm; + //can_send(&to_send, 0, true); + + to_send.RIR = (0x1A9 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num, true); + } + + return; + } + + if (addr == 0x145) + { + to_send.RIR = (0x149 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num, true); + + return; + } + + if ((addr == 0x175) && (DAS_noEpasHarness == 1)) + { + to_send.RIR = (0x169 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num, true); + return; + } + + if (addr == 0x118 ) + { + to_send.RIR = (0x119 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num, true); + + if (DAS_noEpasHarness == 0) { + //we don't get 0x175 ESP_wheelSpeeds so send as 0x169 on CAN1 and also as 0x175 on CAN0 + int counter = to_fwd->RDHR & 0x0F; + to_send.RIR = (0x169 << 21) + (addr_mask & (to_fwd->RIR | 1)); + to_send.RDTR = (to_fwd->RDTR & 0xFFFFFFF0) | 0x08; + int32_t speed_kph = (((0xFFF0000 & to_send.RDLR) >> 16) * 0.05 -25) * 1.609; + if (speed_kph < 0) { + speed_kph = 0; + } + // is AHB is enabled, use low apeed to spread radar angle + if ((speed_kph > 2 ) && (DAS_ahb_is_enabled == 1)) { + // speed_kph = 2; + } + if (((0xFFF0000 & to_send.RDLR) >> 16) == 0xFFF) { + speed_kph = 0x1FFF; //0xFFF is signal not available for DI_Torque2 speed 0x118; should be SNA or 0x1FFF for 0x169 + } else { + speed_kph = (int)(speed_kph/0.04) & 0x1FFF; + } + to_send.RDLR = (speed_kph | (speed_kph << 13) | (speed_kph << 26)) & 0xFFFFFFFF; + to_send.RDHR = ((speed_kph >> 6) | (speed_kph << 7) | (counter << 20)) & 0x00FFFFFF; + int cksm = 0x76; + cksm = (cksm + (to_send.RDLR & 0xFF) + ((to_send.RDLR >> 8) & 0xFF) + ((to_send.RDLR >> 16) & 0xFF) + ((to_send.RDLR >> 24) & 0xFF)) & 0xFF; + cksm = (cksm + (to_send.RDHR & 0xFF) + ((to_send.RDHR >> 8) & 0xFF) + ((to_send.RDHR >> 16) & 0xFF) + ((to_send.RDHR >> 24) & 0xFF)) & 0xFF; + to_send.RDHR = to_send.RDHR | (cksm << 24); + can_send(&to_send, bus_num, true); + } + + return; + } + if (addr == 0x108 ) + { + to_send.RIR = (0x109 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num, true); + + return; + } + if (addr == 0x308 ) + { + to_send.RIR = (0x209 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num, true); + + return; + } + if (addr == 0x45 ) + { + to_send.RIR = (0x219 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num, true); + + return; + } + if (addr == 0x148 ) + { + to_send.RIR = (0x1A9 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num,true); + + return; + } + if (addr == 0x30A) + { + to_send.RIR = (0x2D9 << 21) + (addr_mask & (to_fwd->RIR | 1)); + can_send(&to_send, bus_num, true); + + return; + } + } -static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { +static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) +{ - int bus_fwd = -1; - int addr = GET_ADDR(to_fwd); + int32_t addr = to_fwd->RIR >> 21; + int ret_val = -1; + //if we never got the config from gtw, don't forward anything anywhere + if (DAS_gtwConfigReceived == 0) { + return -1; + } + + //first let's deal with the messages we need to send to radar + if ((bus_num == 0) || ((bus_num == 2 ) && (DAS_usesApillarHarness == 1))) + { + + //compute return value; do not forward 0->2 and 2->0 if no epas harness + if (bus_num == 0) { + if (DAS_noEpasHarness == 0) { + ret_val=2; + } else { + ret_val=-1; + } + } else if (bus_num == 2) { + if (DAS_noEpasHarness == 0) { + ret_val=0; + } else { + ret_val=-1; + } + } + + //check all messages we need to also send to radar, moddified, after we receive 0x631 from radar + //148 does not exist, we use 115 at the same frequency to trigger and pass static vals + //175 does not exist, we use 118 at the same frequency to trigger and pass vehicle speed + if ((tesla_radar_status > 0 ) && (enable_radar_emulation == 1) && ((addr == 0x20A ) || (addr == 0x118 ) || (addr == 0x108 ) || + (addr == 0x115 ) || (addr == 0x148 ) || (addr == 0x145))) + { + tesla_fwd_to_radar_modded(tesla_radar_can, to_fwd); + return ret_val; + } + + //check all messages we need to also send to radar, moddified, all the time + if (((addr == 0xE ) || (addr == 0x308 ) || (addr == 0x45 ) || (addr == 0x398 ) || + (addr == 0x405 ) || (addr == 0x30A)) && (enable_radar_emulation == 1)) { + tesla_fwd_to_radar_modded(tesla_radar_can, to_fwd); + return ret_val; + } + + //forward to radar unmodded the UDS messages 0x641 + if (addr == 0x641 ) { + tesla_fwd_to_radar_as_is(tesla_radar_can, to_fwd); + return ret_val; + } + + } + //now let's deal with CAN0 alone if (bus_num == 0) { - // change inhibit of GTW_epasControl - if (addr != 0x214) { - // remove EPB_epasControl - bus_fwd = 2; // Custom EPAS bus + if (DAS_noEpasHarness == 0) { + ret_val=2; + } else { + ret_val=-1; } - if (addr == 0x101) { - to_fwd->RDLR = GET_BYTES_04(to_fwd) | 0x4000; // 0x4000: WITH_ANGLE, 0xC000: WITH_BOTH (angle and torque) - uint32_t checksum = (GET_BYTE(to_fwd, 1) + GET_BYTE(to_fwd, 0) + 2) & 0xFF; + + // change inhibit of GTW_epasControl and enabled haptic for LDW + if (addr == 0x101) + { + to_fwd->RDLR = GET_BYTES_04(to_fwd) | 0x4000 | 0x1000; + // 0x4000: WITH_ANGLE, 0xC000: WITH_BOTH (angle and torque) + // 0x1000: enabled LDW for haptic + int checksum = (GET_BYTE(to_fwd, 1) + GET_BYTE(to_fwd, 0) + 2) & 0xFF; to_fwd->RDLR = GET_BYTES_04(to_fwd) & 0xFFFF; to_fwd->RDLR = GET_BYTES_04(to_fwd) + (checksum << 16); + return ret_val; } + + if (addr == 0x214) { + //inhibit ibooster epas kill signal + return -1; + } + + //forward everything else to CAN 2 unless claiming no harness + return ret_val; + } + + //now let's deal with CAN1 - Radar + if (bus_num == tesla_radar_can) { + //send radar 0x531 and 0x651 from Radar CAN to CAN0 + if ((addr == 0x531) || (addr == 0x651)){ + return 0; + } + + //block everything else from radar + return -1; } - if (bus_num == 2) { - // remove GTW_epasControl in forwards - if (addr != 0x101) { - bus_fwd = 0; // Chassis CAN + + //now let's deal with CAN2 + if ((bus_num == tesla_epas_can) && (bus_num > 0)) + { + + // remove Pedal in forwards + if ((addr == 0x551) || (addr == 0x552)) { + return -1; } + + //forward everything else to CAN 0 unless claiming no harness + return 0; } - return bus_fwd; + return -1; } const safety_hooks tesla_hooks = { @@ -209,4 +2092,6 @@ const safety_hooks tesla_hooks = { .tx = tesla_tx_hook, .tx_lin = nooutput_tx_lin_hook, .fwd = tesla_fwd_hook, + .addr_check = tesla_rx_checks, + .addr_check_len = sizeof(tesla_rx_checks) / sizeof(tesla_rx_checks[0]), }; diff --git a/phonelibs/fastcv/aarch64/libfastcvadsp_stub.so b/phonelibs/fastcv/aarch64/libfastcvadsp_stub.so index 78498b6b704f01..0a1bdaf04b1cb3 100644 Binary files a/phonelibs/fastcv/aarch64/libfastcvadsp_stub.so and b/phonelibs/fastcv/aarch64/libfastcvadsp_stub.so differ diff --git a/phonelibs/fastcv/aarch64/libfastcvopt.so b/phonelibs/fastcv/aarch64/libfastcvopt.so index 02b5b0a580e44a..1e81b92f08b5e8 100644 Binary files a/phonelibs/fastcv/aarch64/libfastcvopt.so and b/phonelibs/fastcv/aarch64/libfastcvopt.so differ diff --git a/phonelibs/linux/include/msm_ion.h b/phonelibs/linux/include/msm_ion.h index e41b3170fd438e..d5caed5eb36e1b 100644 --- a/phonelibs/linux/include/msm_ion.h +++ b/phonelibs/linux/include/msm_ion.h @@ -166,7 +166,7 @@ struct ion_flush_data { struct ion_prefetch_regions { unsigned int vmid; - size_t __user *sizes; + size_t *sizes; unsigned int nr_sizes; }; @@ -174,7 +174,7 @@ struct ion_prefetch_data { int heap_id; unsigned long len; /* Is unsigned long bad? 32bit compiler vs 64 bit compiler*/ - struct ion_prefetch_regions __user *regions; + struct ion_prefetch_regions *regions; unsigned int nr_regions; }; diff --git a/phonelibs/nanovg/stb_truetype.h b/phonelibs/nanovg/stb_truetype.h index bfb1841f6989ce..62595a15fdf6b2 100644 --- a/phonelibs/nanovg/stb_truetype.h +++ b/phonelibs/nanovg/stb_truetype.h @@ -1,11 +1,21 @@ -// stb_truetype.h - v1.09 - public domain -// authored from 2009-2015 by Sean Barrett / RAD Game Tools +// stb_truetype.h - v1.24 - public domain +// authored from 2009-2020 by Sean Barrett / RAD Game Tools +// +// ======================================================================= +// +// NO SECURITY GUARANTEE -- DO NOT USE THIS ON UNTRUSTED FONT FILES +// +// This library does no range checking of the offsets found in the file, +// meaning an attacker can use it to read arbitrary memory. +// +// ======================================================================= // // This library processes TrueType files: // parse files // extract glyph metrics // extract glyph shapes // render glyphs to one-channel bitmaps with antialiasing (box filter) +// render glyphs to one-channel SDF bitmaps (signed-distance field/function) // // Todo: // non-MS cmaps @@ -20,36 +30,49 @@ // // Mikko Mononen: compound shape support, more cmap formats // Tor Andersson: kerning, subpixel rendering -// -// Bug/warning reports/fixes: -// "Zer" on mollyrocket (with fix) -// Cass Everitt -// stoiko (Haemimont Games) -// Brian Hook -// Walter van Niftrik -// David Gow -// David Given -// Ivan-Assen Ivanov -// Anthony Pesch -// Johan Duparc -// Hou Qiming -// Fabian "ryg" Giesen -// Martins Mozeiko -// Cap Petschulat -// Omar Cornut -// github:aloucks -// Peter LaValle -// Sergey Popov -// Giumo X. Clanjor -// Higor Euripedes -// Thomas Fields -// Derek Vinyard +// Dougall Johnson: OpenType / Type 2 font handling +// Daniel Ribeiro Maciel: basic GPOS-based kerning // // Misc other: // Ryan Gordon +// Simon Glass +// github:IntellectualKitty +// Imanol Celaya +// Daniel Ribeiro Maciel +// +// Bug/warning reports/fixes: +// "Zer" on mollyrocket Fabian "ryg" Giesen github:NiLuJe +// Cass Everitt Martins Mozeiko github:aloucks +// stoiko (Haemimont Games) Cap Petschulat github:oyvindjam +// Brian Hook Omar Cornut github:vassvik +// Walter van Niftrik Ryan Griege +// David Gow Peter LaValle +// David Given Sergey Popov +// Ivan-Assen Ivanov Giumo X. Clanjor +// Anthony Pesch Higor Euripedes +// Johan Duparc Thomas Fields +// Hou Qiming Derek Vinyard +// Rob Loach Cort Stratton +// Kenney Phillis Jr. Brian Costabile +// Ken Voskuil (kaesve) // // VERSION HISTORY // +// 1.24 (2020-02-05) fix warning +// 1.23 (2020-02-02) query SVG data for glyphs; query whole kerning table (but only kern not GPOS) +// 1.22 (2019-08-11) minimize missing-glyph duplication; fix kerning if both 'GPOS' and 'kern' are defined +// 1.21 (2019-02-25) fix warning +// 1.20 (2019-02-07) PackFontRange skips missing codepoints; GetScaleFontVMetrics() +// 1.19 (2018-02-11) GPOS kerning, STBTT_fmod +// 1.18 (2018-01-29) add missing function +// 1.17 (2017-07-23) make more arguments const; doc fix +// 1.16 (2017-07-12) SDF support +// 1.15 (2017-03-03) make more arguments const +// 1.14 (2017-01-16) num-fonts-in-TTC function +// 1.13 (2017-01-02) support OpenType fonts, certain Apple fonts +// 1.12 (2016-10-25) suppress warnings about casting away const with -Wcast-qual +// 1.11 (2016-04-02) fix unused-variable warning +// 1.10 (2016-04-02) user-defined fabs(); rare memory leak; remove duplicate typedef // 1.09 (2016-01-16) warning fix; avoid crash on outofmem; use allocation userdata properly // 1.08 (2015-09-13) document stbtt_Rasterize(); fixes for vertical & horizontal edges // 1.07 (2015-08-01) allow PackFontRanges to accept arrays of sparse codepoints; @@ -57,24 +80,16 @@ // fix stbtt_GetFontOFfsetForIndex (never worked for non-0 input?); // fixed an assert() bug in the new rasterizer // replace assert() with STBTT_assert() in new rasterizer -// 1.06 (2015-07-14) performance improvements (~35% faster on x86 and x64 on test machine) -// also more precise AA rasterizer, except if shapes overlap -// remove need for STBTT_sort -// 1.05 (2015-04-15) fix misplaced definitions for STBTT_STATIC -// 1.04 (2015-04-15) typo in example -// 1.03 (2015-04-12) STBTT_STATIC, fix memory leak in new packing, various fixes // // Full history can be found at the end of this file. // // LICENSE // -// This software is in the public domain. Where that dedication is not -// recognized, you are granted a perpetual, irrevocable license to copy, -// distribute, and modify this file as you see fit. +// See end of file for license information. // // USAGE // -// Include this file in whatever places neeed to refer to it. In ONE C/C++ +// Include this file in whatever places need to refer to it. In ONE C/C++ // file, write: // #define STB_TRUETYPE_IMPLEMENTATION // before the #include of this file. This expands out the actual @@ -90,14 +105,15 @@ // Improved 3D API (more shippable): // #include "stb_rect_pack.h" -- optional, but you really want it // stbtt_PackBegin() -// stbtt_PackSetOversample() -- for improved quality on small fonts +// stbtt_PackSetOversampling() -- for improved quality on small fonts // stbtt_PackFontRanges() -- pack and renders // stbtt_PackEnd() // stbtt_GetPackedQuad() // // "Load" a font file from a memory buffer (you have to keep the buffer loaded) // stbtt_InitFont() -// stbtt_GetFontOffsetForIndex() -- use for TTC font collections +// stbtt_GetFontOffsetForIndex() -- indexing for TTC font collections +// stbtt_GetNumberOfFonts() -- number of fonts for TTC font collections // // Render a unicode codepoint to a bitmap // stbtt_GetCodepointBitmap() -- allocates and returns a bitmap @@ -107,6 +123,7 @@ // Character advance/positioning // stbtt_GetCodepointHMetrics() // stbtt_GetFontVMetrics() +// stbtt_GetFontVMetricsOS2() // stbtt_GetCodepointKernAdvance() // // Starting with version 1.06, the rasterizer was replaced with a new, @@ -162,7 +179,7 @@ // measurement for describing font size, defined as 72 points per inch. // stb_truetype provides a point API for compatibility. However, true // "per inch" conventions don't make much sense on computer displays -// since they different monitors have different number of pixels per +// since different monitors have different number of pixels per // inch. For example, Windows traditionally uses a convention that // there are 96 pixels per inch, thus making 'inch' measurements have // nothing to do with inches, and thus effectively defining a point to @@ -172,6 +189,39 @@ // for non-commercial fonts, thus making fonts scaled in points // according to the TrueType spec incoherently sized in practice. // +// DETAILED USAGE: +// +// Scale: +// Select how high you want the font to be, in points or pixels. +// Call ScaleForPixelHeight or ScaleForMappingEmToPixels to compute +// a scale factor SF that will be used by all other functions. +// +// Baseline: +// You need to select a y-coordinate that is the baseline of where +// your text will appear. Call GetFontBoundingBox to get the baseline-relative +// bounding box for all characters. SF*-y0 will be the distance in pixels +// that the worst-case character could extend above the baseline, so if +// you want the top edge of characters to appear at the top of the +// screen where y=0, then you would set the baseline to SF*-y0. +// +// Current point: +// Set the current point where the first character will appear. The +// first character could extend left of the current point; this is font +// dependent. You can either choose a current point that is the leftmost +// point and hope, or add some padding, or check the bounding box or +// left-side-bearing of the first character to be displayed and set +// the current point based on that. +// +// Displaying a character: +// Compute the bounding box of the character. It will contain signed values +// relative to . I.e. if it returns x0,y0,x1,y1, +// then the character should be displayed in the rectangle from +// to #define STBTT_ifloor(x) ((int) floor(x)) @@ -404,6 +442,23 @@ int main(int arg, char **argv) #ifndef STBTT_sqrt #include #define STBTT_sqrt(x) sqrt(x) + #define STBTT_pow(x,y) pow(x,y) + #endif + + #ifndef STBTT_fmod + #include + #define STBTT_fmod(x,y) fmod(x,y) + #endif + + #ifndef STBTT_cos + #include + #define STBTT_cos(x) cos(x) + #define STBTT_acos(x) acos(x) + #endif + + #ifndef STBTT_fabs + #include + #define STBTT_fabs(x) fabs(x) #endif // #define your own functions "STBTT_malloc" / "STBTT_free" to avoid malloc.h @@ -424,7 +479,7 @@ int main(int arg, char **argv) #endif #ifndef STBTT_memcpy - #include + #include #define STBTT_memcpy memcpy #define STBTT_memset memset #endif @@ -450,6 +505,14 @@ int main(int arg, char **argv) extern "C" { #endif +// private structure +typedef struct +{ + unsigned char *data; + int cursor; + int size; +} stbtt__buf; + ////////////////////////////////////////////////////////////////////////////// // // TEXTURE BAKING API @@ -479,7 +542,7 @@ typedef struct float x1,y1,s1,t1; // bottom-right } stbtt_aligned_quad; -STBTT_DEF void stbtt_GetBakedQuad(stbtt_bakedchar *chardata, int pw, int ph, // same data as above +STBTT_DEF void stbtt_GetBakedQuad(const stbtt_bakedchar *chardata, int pw, int ph, // same data as above int char_index, // character to display float *xpos, float *ypos, // pointers to current position in screen pixel space stbtt_aligned_quad *q, // output: quad to draw @@ -494,6 +557,8 @@ STBTT_DEF void stbtt_GetBakedQuad(stbtt_bakedchar *chardata, int pw, int ph, // // // It's inefficient; you might want to c&p it and optimize it. +STBTT_DEF void stbtt_GetScaledFontVMetrics(const unsigned char *fontdata, int index, float size, float *ascent, float *descent, float *lineGap); +// Query the font vertical metrics without having to create a font first. ////////////////////////////////////////////////////////////////////////////// @@ -519,7 +584,7 @@ typedef struct stbrp_rect stbrp_rect; STBTT_DEF int stbtt_PackBegin(stbtt_pack_context *spc, unsigned char *pixels, int width, int height, int stride_in_bytes, int padding, void *alloc_context); // Initializes a packing context stored in the passed-in stbtt_pack_context. // Future calls using this context will pack characters into the bitmap passed -// in here: a 1-channel bitmap that is weight x height. stride_in_bytes is +// in here: a 1-channel bitmap that is width * height. stride_in_bytes is // the distance from one row to the next (or 0 to mean they are packed tightly // together). "padding" is the amount of padding to leave between each // character (normally you want '1' for bitmaps you'll use as textures with @@ -532,7 +597,7 @@ STBTT_DEF void stbtt_PackEnd (stbtt_pack_context *spc); #define STBTT_POINT_SIZE(x) (-(x)) -STBTT_DEF int stbtt_PackFontRange(stbtt_pack_context *spc, unsigned char *fontdata, int font_index, float font_size, +STBTT_DEF int stbtt_PackFontRange(stbtt_pack_context *spc, const unsigned char *fontdata, int font_index, float font_size, int first_unicode_char_in_range, int num_chars_in_range, stbtt_packedchar *chardata_for_range); // Creates character bitmaps from the font_index'th font found in fontdata (use // font_index=0 if you don't know what that is). It creates num_chars_in_range @@ -557,7 +622,7 @@ typedef struct unsigned char h_oversample, v_oversample; // don't set these, they're used internally } stbtt_pack_range; -STBTT_DEF int stbtt_PackFontRanges(stbtt_pack_context *spc, unsigned char *fontdata, int font_index, stbtt_pack_range *ranges, int num_ranges); +STBTT_DEF int stbtt_PackFontRanges(stbtt_pack_context *spc, const unsigned char *fontdata, int font_index, stbtt_pack_range *ranges, int num_ranges); // Creates character bitmaps from multiple ranges of characters stored in // ranges. This will usually create a better-packed bitmap than multiple // calls to stbtt_PackFontRange. Note that you can call this multiple @@ -579,19 +644,25 @@ STBTT_DEF void stbtt_PackSetOversampling(stbtt_pack_context *spc, unsigned int h // To use with PackFontRangesGather etc., you must set it before calls // call to PackFontRangesGatherRects. -STBTT_DEF void stbtt_GetPackedQuad(stbtt_packedchar *chardata, int pw, int ph, // same data as above +STBTT_DEF void stbtt_PackSetSkipMissingCodepoints(stbtt_pack_context *spc, int skip); +// If skip != 0, this tells stb_truetype to skip any codepoints for which +// there is no corresponding glyph. If skip=0, which is the default, then +// codepoints without a glyph recived the font's "missing character" glyph, +// typically an empty box by convention. + +STBTT_DEF void stbtt_GetPackedQuad(const stbtt_packedchar *chardata, int pw, int ph, // same data as above int char_index, // character to display float *xpos, float *ypos, // pointers to current position in screen pixel space stbtt_aligned_quad *q, // output: quad to draw int align_to_integer); -STBTT_DEF int stbtt_PackFontRangesGatherRects(stbtt_pack_context *spc, stbtt_fontinfo *info, stbtt_pack_range *ranges, int num_ranges, stbrp_rect *rects); +STBTT_DEF int stbtt_PackFontRangesGatherRects(stbtt_pack_context *spc, const stbtt_fontinfo *info, stbtt_pack_range *ranges, int num_ranges, stbrp_rect *rects); STBTT_DEF void stbtt_PackFontRangesPackRects(stbtt_pack_context *spc, stbrp_rect *rects, int num_rects); -STBTT_DEF int stbtt_PackFontRangesRenderIntoRects(stbtt_pack_context *spc, stbtt_fontinfo *info, stbtt_pack_range *ranges, int num_ranges, stbrp_rect *rects); +STBTT_DEF int stbtt_PackFontRangesRenderIntoRects(stbtt_pack_context *spc, const stbtt_fontinfo *info, stbtt_pack_range *ranges, int num_ranges, stbrp_rect *rects); // Calling these functions in sequence is roughly equivalent to calling // stbtt_PackFontRanges(). If you more control over the packing of multiple // fonts, or if you want to pack custom data into a font texture, take a look -// at the source to of stbtt_PackFontRanges() and create a custom version +// at the source to of stbtt_PackFontRanges() and create a custom version // using these functions, e.g. call GatherRects multiple times, // building up a single array of rects, then call PackRects once, // then call RenderIntoRects repeatedly. This may result in a @@ -607,6 +678,7 @@ struct stbtt_pack_context { int height; int stride_in_bytes; int padding; + int skip_missing; unsigned int h_oversample, v_oversample; unsigned char *pixels; void *nodes; @@ -618,18 +690,23 @@ struct stbtt_pack_context { // // +STBTT_DEF int stbtt_GetNumberOfFonts(const unsigned char *data); +// This function will determine the number of fonts in a font file. TrueType +// collection (.ttc) files may contain multiple fonts, while TrueType font +// (.ttf) files only contain one font. The number of fonts can be used for +// indexing with the previous function where the index is between zero and one +// less than the total fonts. If an error occurs, -1 is returned. + STBTT_DEF int stbtt_GetFontOffsetForIndex(const unsigned char *data, int index); // Each .ttf/.ttc file may have more than one font. Each font has a sequential // index number starting from 0. Call this function to get the font offset for // a given index; it returns -1 if the index is out of range. A regular .ttf // file will only define one font and it always be at offset 0, so it will -// return '0' for index 0, and -1 for all other indices. You can just skip -// this step if you know it's that kind of font. +// return '0' for index 0, and -1 for all other indices. - -// The following structure is defined publically so you can declare one on +// The following structure is defined publicly so you can declare one on // the stack or as a global or etc, but you should treat it as opaque. -typedef struct stbtt_fontinfo +struct stbtt_fontinfo { void * userdata; unsigned char * data; // pointer to .ttf file @@ -637,10 +714,17 @@ typedef struct stbtt_fontinfo int numGlyphs; // number of glyphs, needed for range checking - int loca,head,glyf,hhea,hmtx,kern; // table locations as offset from start of .ttf + int loca,head,glyf,hhea,hmtx,kern,gpos,svg; // table locations as offset from start of .ttf int index_map; // a cmap mapping for our chosen character encoding int indexToLocFormat; // format needed to map from glyph index to glyph -} stbtt_fontinfo; + + stbtt__buf cff; // cff font data + stbtt__buf charstrings; // the charstring index + stbtt__buf gsubrs; // global charstring subroutines index + stbtt__buf subrs; // private charstring subroutines index + stbtt__buf fontdicts; // array of font dicts + stbtt__buf fdselect; // map from glyph to fontdict +}; STBTT_DEF int stbtt_InitFont(stbtt_fontinfo *info, const unsigned char *data, int offset); // Given an offset into the file that defines a font, this function builds @@ -659,6 +743,7 @@ STBTT_DEF int stbtt_FindGlyphIndex(const stbtt_fontinfo *info, int unicode_codep // and you want a speed-up, call this function with the character you're // going to process, then use glyph-based functions instead of the // codepoint-based functions. +// Returns 0 if the character codepoint is not defined in the font. ////////////////////////////////////////////////////////////////////////////// @@ -687,6 +772,12 @@ STBTT_DEF void stbtt_GetFontVMetrics(const stbtt_fontinfo *info, int *ascent, in // these are expressed in unscaled coordinates, so you must multiply by // the scale factor for a given size +STBTT_DEF int stbtt_GetFontVMetricsOS2(const stbtt_fontinfo *info, int *typoAscent, int *typoDescent, int *typoLineGap); +// analogous to GetFontVMetrics, but returns the "typographic" values from the OS/2 +// table (specific to MS/Windows TTF files). +// +// Returns 1 on success (table present), 0 on failure. + STBTT_DEF void stbtt_GetFontBoundingBox(const stbtt_fontinfo *info, int *x0, int *y0, int *x1, int *y1); // the bounding box around all possible characters @@ -706,6 +797,18 @@ STBTT_DEF int stbtt_GetGlyphKernAdvance(const stbtt_fontinfo *info, int glyph1, STBTT_DEF int stbtt_GetGlyphBox(const stbtt_fontinfo *info, int glyph_index, int *x0, int *y0, int *x1, int *y1); // as above, but takes one or more glyph indices for greater efficiency +typedef struct stbtt_kerningentry +{ + int glyph1; // use stbtt_FindGlyphIndex + int glyph2; + int advance; +} stbtt_kerningentry; + +STBTT_DEF int stbtt_GetKerningTableLength(const stbtt_fontinfo *info); +STBTT_DEF int stbtt_GetKerningTable(const stbtt_fontinfo *info, stbtt_kerningentry* table, int table_length); +// Retrieves a complete list of all of the kerning pairs provided by the font +// stbtt_GetKerningTable never writes more than table_length entries and returns how many entries it did write. +// The table will be sorted by (a.glyph1 == b.glyph1)?(a.glyph2 < b.glyph2):(a.glyph1 < b.glyph1) ////////////////////////////////////////////////////////////////////////////// // @@ -717,7 +820,8 @@ STBTT_DEF int stbtt_GetGlyphBox(const stbtt_fontinfo *info, int glyph_index, in enum { STBTT_vmove=1, STBTT_vline, - STBTT_vcurve + STBTT_vcurve, + STBTT_vcubic }; #endif @@ -726,7 +830,7 @@ STBTT_DEF int stbtt_GetGlyphBox(const stbtt_fontinfo *info, int glyph_index, in #define stbtt_vertex_type short // can't use stbtt_int16 because that's not visible in the header file typedef struct { - stbtt_vertex_type x,y,cx,cy; + stbtt_vertex_type x,y,cx,cy,cx1,cy1; unsigned char type,padding; } stbtt_vertex; #endif @@ -739,7 +843,7 @@ STBTT_DEF int stbtt_GetGlyphShape(const stbtt_fontinfo *info, int glyph_index, s // returns # of vertices and fills *vertices with the pointer to them // these are expressed in "unscaled" coordinates // -// The shape is a series of countours. Each one starts with +// The shape is a series of contours. Each one starts with // a STBTT_moveto, then consists of a series of mixed // STBTT_lineto and STBTT_curveto segments. A lineto // draws a line from previous endpoint to its x,y; a curveto @@ -749,6 +853,11 @@ STBTT_DEF int stbtt_GetGlyphShape(const stbtt_fontinfo *info, int glyph_index, s STBTT_DEF void stbtt_FreeShape(const stbtt_fontinfo *info, stbtt_vertex *vertices); // frees the data allocated above +STBTT_DEF int stbtt_GetCodepointSVG(const stbtt_fontinfo *info, int unicode_codepoint, const char **svg); +STBTT_DEF int stbtt_GetGlyphSVG(const stbtt_fontinfo *info, int gl, const char **svg); +// fills svg with the character's SVG data. +// returns data size or 0 if SVG not found. + ////////////////////////////////////////////////////////////////////////////// // // BITMAP RENDERING @@ -780,6 +889,10 @@ STBTT_DEF void stbtt_MakeCodepointBitmapSubpixel(const stbtt_fontinfo *info, uns // same as stbtt_MakeCodepointBitmap, but you can specify a subpixel // shift for the character +STBTT_DEF void stbtt_MakeCodepointBitmapSubpixelPrefilter(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, float shift_x, float shift_y, int oversample_x, int oversample_y, float *sub_x, float *sub_y, int codepoint); +// same as stbtt_MakeCodepointBitmapSubpixel, but prefiltering +// is performed (see stbtt_PackSetOversampling) + STBTT_DEF void stbtt_GetCodepointBitmapBox(const stbtt_fontinfo *font, int codepoint, float scale_x, float scale_y, int *ix0, int *iy0, int *ix1, int *iy1); // get the bbox of the bitmap centered around the glyph origin; so the // bitmap width is ix1-ix0, height is iy1-iy0, and location to place @@ -797,6 +910,7 @@ STBTT_DEF unsigned char *stbtt_GetGlyphBitmap(const stbtt_fontinfo *info, float STBTT_DEF unsigned char *stbtt_GetGlyphBitmapSubpixel(const stbtt_fontinfo *info, float scale_x, float scale_y, float shift_x, float shift_y, int glyph, int *width, int *height, int *xoff, int *yoff); STBTT_DEF void stbtt_MakeGlyphBitmap(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, int glyph); STBTT_DEF void stbtt_MakeGlyphBitmapSubpixel(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, float shift_x, float shift_y, int glyph); +STBTT_DEF void stbtt_MakeGlyphBitmapSubpixelPrefilter(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, float shift_x, float shift_y, int oversample_x, int oversample_y, float *sub_x, float *sub_y, int glyph); STBTT_DEF void stbtt_GetGlyphBitmapBox(const stbtt_fontinfo *font, int glyph, float scale_x, float scale_y, int *ix0, int *iy0, int *ix1, int *iy1); STBTT_DEF void stbtt_GetGlyphBitmapBoxSubpixel(const stbtt_fontinfo *font, int glyph, float scale_x, float scale_y,float shift_x, float shift_y, int *ix0, int *iy0, int *ix1, int *iy1); @@ -819,6 +933,64 @@ STBTT_DEF void stbtt_Rasterize(stbtt__bitmap *result, // 1-channel bitmap int invert, // if non-zero, vertically flip shape void *userdata); // context for to STBTT_MALLOC +////////////////////////////////////////////////////////////////////////////// +// +// Signed Distance Function (or Field) rendering + +STBTT_DEF void stbtt_FreeSDF(unsigned char *bitmap, void *userdata); +// frees the SDF bitmap allocated below + +STBTT_DEF unsigned char * stbtt_GetGlyphSDF(const stbtt_fontinfo *info, float scale, int glyph, int padding, unsigned char onedge_value, float pixel_dist_scale, int *width, int *height, int *xoff, int *yoff); +STBTT_DEF unsigned char * stbtt_GetCodepointSDF(const stbtt_fontinfo *info, float scale, int codepoint, int padding, unsigned char onedge_value, float pixel_dist_scale, int *width, int *height, int *xoff, int *yoff); +// These functions compute a discretized SDF field for a single character, suitable for storing +// in a single-channel texture, sampling with bilinear filtering, and testing against +// larger than some threshold to produce scalable fonts. +// info -- the font +// scale -- controls the size of the resulting SDF bitmap, same as it would be creating a regular bitmap +// glyph/codepoint -- the character to generate the SDF for +// padding -- extra "pixels" around the character which are filled with the distance to the character (not 0), +// which allows effects like bit outlines +// onedge_value -- value 0-255 to test the SDF against to reconstruct the character (i.e. the isocontour of the character) +// pixel_dist_scale -- what value the SDF should increase by when moving one SDF "pixel" away from the edge (on the 0..255 scale) +// if positive, > onedge_value is inside; if negative, < onedge_value is inside +// width,height -- output height & width of the SDF bitmap (including padding) +// xoff,yoff -- output origin of the character +// return value -- a 2D array of bytes 0..255, width*height in size +// +// pixel_dist_scale & onedge_value are a scale & bias that allows you to make +// optimal use of the limited 0..255 for your application, trading off precision +// and special effects. SDF values outside the range 0..255 are clamped to 0..255. +// +// Example: +// scale = stbtt_ScaleForPixelHeight(22) +// padding = 5 +// onedge_value = 180 +// pixel_dist_scale = 180/5.0 = 36.0 +// +// This will create an SDF bitmap in which the character is about 22 pixels +// high but the whole bitmap is about 22+5+5=32 pixels high. To produce a filled +// shape, sample the SDF at each pixel and fill the pixel if the SDF value +// is greater than or equal to 180/255. (You'll actually want to antialias, +// which is beyond the scope of this example.) Additionally, you can compute +// offset outlines (e.g. to stroke the character border inside & outside, +// or only outside). For example, to fill outside the character up to 3 SDF +// pixels, you would compare against (180-36.0*3)/255 = 72/255. The above +// choice of variables maps a range from 5 pixels outside the shape to +// 2 pixels inside the shape to 0..255; this is intended primarily for apply +// outside effects only (the interior range is needed to allow proper +// antialiasing of the font at *smaller* sizes) +// +// The function computes the SDF analytically at each SDF pixel, not by e.g. +// building a higher-res bitmap and approximating it. In theory the quality +// should be as high as possible for an SDF of this size & representation, but +// unclear if this is true in practice (perhaps building a higher-res bitmap +// and computing from that can allow drop-out prevention). +// +// The algorithm has not been optimized at all, so expect it to be slow +// if computing lots of characters or very large sizes. + + + ////////////////////////////////////////////////////////////////////////////// // // Finding the right font... @@ -942,6 +1114,158 @@ typedef int stbtt__test_oversample_pow2[(STBTT_MAX_OVERSAMPLE & (STBTT_MAX_OVERS #define STBTT_RASTERIZER_VERSION 2 #endif +#ifdef _MSC_VER +#define STBTT__NOTUSED(v) (void)(v) +#else +#define STBTT__NOTUSED(v) (void)sizeof(v) +#endif + +////////////////////////////////////////////////////////////////////////// +// +// stbtt__buf helpers to parse data from file +// + +static stbtt_uint8 stbtt__buf_get8(stbtt__buf *b) +{ + if (b->cursor >= b->size) + return 0; + return b->data[b->cursor++]; +} + +static stbtt_uint8 stbtt__buf_peek8(stbtt__buf *b) +{ + if (b->cursor >= b->size) + return 0; + return b->data[b->cursor]; +} + +static void stbtt__buf_seek(stbtt__buf *b, int o) +{ + STBTT_assert(!(o > b->size || o < 0)); + b->cursor = (o > b->size || o < 0) ? b->size : o; +} + +static void stbtt__buf_skip(stbtt__buf *b, int o) +{ + stbtt__buf_seek(b, b->cursor + o); +} + +static stbtt_uint32 stbtt__buf_get(stbtt__buf *b, int n) +{ + stbtt_uint32 v = 0; + int i; + STBTT_assert(n >= 1 && n <= 4); + for (i = 0; i < n; i++) + v = (v << 8) | stbtt__buf_get8(b); + return v; +} + +static stbtt__buf stbtt__new_buf(const void *p, size_t size) +{ + stbtt__buf r; + STBTT_assert(size < 0x40000000); + r.data = (stbtt_uint8*) p; + r.size = (int) size; + r.cursor = 0; + return r; +} + +#define stbtt__buf_get16(b) stbtt__buf_get((b), 2) +#define stbtt__buf_get32(b) stbtt__buf_get((b), 4) + +static stbtt__buf stbtt__buf_range(const stbtt__buf *b, int o, int s) +{ + stbtt__buf r = stbtt__new_buf(NULL, 0); + if (o < 0 || s < 0 || o > b->size || s > b->size - o) return r; + r.data = b->data + o; + r.size = s; + return r; +} + +static stbtt__buf stbtt__cff_get_index(stbtt__buf *b) +{ + int count, start, offsize; + start = b->cursor; + count = stbtt__buf_get16(b); + if (count) { + offsize = stbtt__buf_get8(b); + STBTT_assert(offsize >= 1 && offsize <= 4); + stbtt__buf_skip(b, offsize * count); + stbtt__buf_skip(b, stbtt__buf_get(b, offsize) - 1); + } + return stbtt__buf_range(b, start, b->cursor - start); +} + +static stbtt_uint32 stbtt__cff_int(stbtt__buf *b) +{ + int b0 = stbtt__buf_get8(b); + if (b0 >= 32 && b0 <= 246) return b0 - 139; + else if (b0 >= 247 && b0 <= 250) return (b0 - 247)*256 + stbtt__buf_get8(b) + 108; + else if (b0 >= 251 && b0 <= 254) return -(b0 - 251)*256 - stbtt__buf_get8(b) - 108; + else if (b0 == 28) return stbtt__buf_get16(b); + else if (b0 == 29) return stbtt__buf_get32(b); + STBTT_assert(0); + return 0; +} + +static void stbtt__cff_skip_operand(stbtt__buf *b) { + int v, b0 = stbtt__buf_peek8(b); + STBTT_assert(b0 >= 28); + if (b0 == 30) { + stbtt__buf_skip(b, 1); + while (b->cursor < b->size) { + v = stbtt__buf_get8(b); + if ((v & 0xF) == 0xF || (v >> 4) == 0xF) + break; + } + } else { + stbtt__cff_int(b); + } +} + +static stbtt__buf stbtt__dict_get(stbtt__buf *b, int key) +{ + stbtt__buf_seek(b, 0); + while (b->cursor < b->size) { + int start = b->cursor, end, op; + while (stbtt__buf_peek8(b) >= 28) + stbtt__cff_skip_operand(b); + end = b->cursor; + op = stbtt__buf_get8(b); + if (op == 12) op = stbtt__buf_get8(b) | 0x100; + if (op == key) return stbtt__buf_range(b, start, end-start); + } + return stbtt__buf_range(b, 0, 0); +} + +static void stbtt__dict_get_ints(stbtt__buf *b, int key, int outcount, stbtt_uint32 *out) +{ + int i; + stbtt__buf operands = stbtt__dict_get(b, key); + for (i = 0; i < outcount && operands.cursor < operands.size; i++) + out[i] = stbtt__cff_int(&operands); +} + +static int stbtt__cff_index_count(stbtt__buf *b) +{ + stbtt__buf_seek(b, 0); + return stbtt__buf_get16(b); +} + +static stbtt__buf stbtt__cff_index_get(stbtt__buf b, int i) +{ + int count, offsize, start, end; + stbtt__buf_seek(&b, 0); + count = stbtt__buf_get16(&b); + offsize = stbtt__buf_get8(&b); + STBTT_assert(i >= 0 && i < count); + STBTT_assert(offsize >= 1 && offsize <= 4); + stbtt__buf_skip(&b, i*offsize); + start = stbtt__buf_get(&b, offsize); + end = stbtt__buf_get(&b, offsize); + return stbtt__buf_range(&b, 2+(count+1)*offsize+start, end - start); +} + ////////////////////////////////////////////////////////////////////////// // // accessors to parse data from file @@ -954,32 +1278,22 @@ typedef int stbtt__test_oversample_pow2[(STBTT_MAX_OVERSAMPLE & (STBTT_MAX_OVERS #define ttCHAR(p) (* (stbtt_int8 *) (p)) #define ttFixed(p) ttLONG(p) -#if defined(STB_TRUETYPE_BIGENDIAN) && !defined(ALLOW_UNALIGNED_TRUETYPE) - - #define ttUSHORT(p) (* (stbtt_uint16 *) (p)) - #define ttSHORT(p) (* (stbtt_int16 *) (p)) - #define ttULONG(p) (* (stbtt_uint32 *) (p)) - #define ttLONG(p) (* (stbtt_int32 *) (p)) - -#else - - static stbtt_uint16 ttUSHORT(const stbtt_uint8 *p) { return p[0]*256 + p[1]; } - static stbtt_int16 ttSHORT(const stbtt_uint8 *p) { return p[0]*256 + p[1]; } - static stbtt_uint32 ttULONG(const stbtt_uint8 *p) { return (p[0]<<24) + (p[1]<<16) + (p[2]<<8) + p[3]; } - static stbtt_int32 ttLONG(const stbtt_uint8 *p) { return (p[0]<<24) + (p[1]<<16) + (p[2]<<8) + p[3]; } - -#endif +static stbtt_uint16 ttUSHORT(stbtt_uint8 *p) { return p[0]*256 + p[1]; } +static stbtt_int16 ttSHORT(stbtt_uint8 *p) { return p[0]*256 + p[1]; } +static stbtt_uint32 ttULONG(stbtt_uint8 *p) { return (p[0]<<24) + (p[1]<<16) + (p[2]<<8) + p[3]; } +static stbtt_int32 ttLONG(stbtt_uint8 *p) { return (p[0]<<24) + (p[1]<<16) + (p[2]<<8) + p[3]; } #define stbtt_tag4(p,c0,c1,c2,c3) ((p)[0] == (c0) && (p)[1] == (c1) && (p)[2] == (c2) && (p)[3] == (c3)) #define stbtt_tag(p,str) stbtt_tag4(p,str[0],str[1],str[2],str[3]) -static int stbtt__isfont(const stbtt_uint8 *font) +static int stbtt__isfont(stbtt_uint8 *font) { // check the version number if (stbtt_tag4(font, '1',0,0,0)) return 1; // TrueType 1 if (stbtt_tag(font, "typ1")) return 1; // TrueType with type 1 font -- we don't support this! if (stbtt_tag(font, "OTTO")) return 1; // OpenType with CFF if (stbtt_tag4(font, 0,1,0,0)) return 1; // OpenType 1.0 + if (stbtt_tag(font, "true")) return 1; // Apple specification for TrueType fonts return 0; } @@ -997,7 +1311,7 @@ static stbtt_uint32 stbtt__find_table(stbtt_uint8 *data, stbtt_uint32 fontstart, return 0; } -STBTT_DEF int stbtt_GetFontOffsetForIndex(const unsigned char *font_collection, int index) +static int stbtt_GetFontOffsetForIndex_internal(unsigned char *font_collection, int index) { // if it's just a font, there's only one valid index if (stbtt__isfont(font_collection)) @@ -1016,14 +1330,59 @@ STBTT_DEF int stbtt_GetFontOffsetForIndex(const unsigned char *font_collection, return -1; } -STBTT_DEF int stbtt_InitFont(stbtt_fontinfo *info, const unsigned char *data2, int fontstart) +static int stbtt_GetNumberOfFonts_internal(unsigned char *font_collection) +{ + // if it's just a font, there's only one valid font + if (stbtt__isfont(font_collection)) + return 1; + + // check if it's a TTC + if (stbtt_tag(font_collection, "ttcf")) { + // version 1? + if (ttULONG(font_collection+4) == 0x00010000 || ttULONG(font_collection+4) == 0x00020000) { + return ttLONG(font_collection+8); + } + } + return 0; +} + +static stbtt__buf stbtt__get_subrs(stbtt__buf cff, stbtt__buf fontdict) +{ + stbtt_uint32 subrsoff = 0, private_loc[2] = { 0, 0 }; + stbtt__buf pdict; + stbtt__dict_get_ints(&fontdict, 18, 2, private_loc); + if (!private_loc[1] || !private_loc[0]) return stbtt__new_buf(NULL, 0); + pdict = stbtt__buf_range(&cff, private_loc[1], private_loc[0]); + stbtt__dict_get_ints(&pdict, 19, 1, &subrsoff); + if (!subrsoff) return stbtt__new_buf(NULL, 0); + stbtt__buf_seek(&cff, private_loc[1]+subrsoff); + return stbtt__cff_get_index(&cff); +} + +// since most people won't use this, find this table the first time it's needed +static int stbtt__get_svg(stbtt_fontinfo *info) +{ + stbtt_uint32 t; + if (info->svg < 0) { + t = stbtt__find_table(info->data, info->fontstart, "SVG "); + if (t) { + stbtt_uint32 offset = ttULONG(info->data + t + 2); + info->svg = t + offset; + } else { + info->svg = 0; + } + } + return info->svg; +} + +static int stbtt_InitFont_internal(stbtt_fontinfo *info, unsigned char *data, int fontstart) { - stbtt_uint8 *data = (stbtt_uint8 *) data2; stbtt_uint32 cmap, t; stbtt_int32 i,numTables; info->data = data; info->fontstart = fontstart; + info->cff = stbtt__new_buf(NULL, 0); cmap = stbtt__find_table(data, fontstart, "cmap"); // required info->loca = stbtt__find_table(data, fontstart, "loca"); // required @@ -1032,8 +1391,62 @@ STBTT_DEF int stbtt_InitFont(stbtt_fontinfo *info, const unsigned char *data2, i info->hhea = stbtt__find_table(data, fontstart, "hhea"); // required info->hmtx = stbtt__find_table(data, fontstart, "hmtx"); // required info->kern = stbtt__find_table(data, fontstart, "kern"); // not required - if (!cmap || !info->loca || !info->head || !info->glyf || !info->hhea || !info->hmtx) + info->gpos = stbtt__find_table(data, fontstart, "GPOS"); // not required + + if (!cmap || !info->head || !info->hhea || !info->hmtx) return 0; + if (info->glyf) { + // required for truetype + if (!info->loca) return 0; + } else { + // initialization for CFF / Type2 fonts (OTF) + stbtt__buf b, topdict, topdictidx; + stbtt_uint32 cstype = 2, charstrings = 0, fdarrayoff = 0, fdselectoff = 0; + stbtt_uint32 cff; + + cff = stbtt__find_table(data, fontstart, "CFF "); + if (!cff) return 0; + + info->fontdicts = stbtt__new_buf(NULL, 0); + info->fdselect = stbtt__new_buf(NULL, 0); + + // @TODO this should use size from table (not 512MB) + info->cff = stbtt__new_buf(data+cff, 512*1024*1024); + b = info->cff; + + // read the header + stbtt__buf_skip(&b, 2); + stbtt__buf_seek(&b, stbtt__buf_get8(&b)); // hdrsize + + // @TODO the name INDEX could list multiple fonts, + // but we just use the first one. + stbtt__cff_get_index(&b); // name INDEX + topdictidx = stbtt__cff_get_index(&b); + topdict = stbtt__cff_index_get(topdictidx, 0); + stbtt__cff_get_index(&b); // string INDEX + info->gsubrs = stbtt__cff_get_index(&b); + + stbtt__dict_get_ints(&topdict, 17, 1, &charstrings); + stbtt__dict_get_ints(&topdict, 0x100 | 6, 1, &cstype); + stbtt__dict_get_ints(&topdict, 0x100 | 36, 1, &fdarrayoff); + stbtt__dict_get_ints(&topdict, 0x100 | 37, 1, &fdselectoff); + info->subrs = stbtt__get_subrs(b, topdict); + + // we only support Type 2 charstrings + if (cstype != 2) return 0; + if (charstrings == 0) return 0; + + if (fdarrayoff) { + // looks like a CID font + if (!fdselectoff) return 0; + stbtt__buf_seek(&b, fdarrayoff); + info->fontdicts = stbtt__cff_get_index(&b); + info->fdselect = stbtt__buf_range(&b, fdselectoff, b.size-fdselectoff); + } + + stbtt__buf_seek(&b, charstrings); + info->charstrings = stbtt__cff_get_index(&b); + } t = stbtt__find_table(data, fontstart, "maxp"); if (t) @@ -1041,6 +1454,8 @@ STBTT_DEF int stbtt_InitFont(stbtt_fontinfo *info, const unsigned char *data2, i else info->numGlyphs = 0xffff; + info->svg = -1; + // find a cmap encoding table we understand *now* to avoid searching // later. (todo: could make this installable) // the same regardless of glyph. @@ -1184,6 +1599,8 @@ static int stbtt__GetGlyfOffset(const stbtt_fontinfo *info, int glyph_index) { int g1,g2; + STBTT_assert(!info->cff.size); + if (glyph_index >= info->numGlyphs) return -1; // glyph index out of range if (info->indexToLocFormat >= 2) return -1; // unknown index->glyph map format @@ -1198,15 +1615,21 @@ static int stbtt__GetGlyfOffset(const stbtt_fontinfo *info, int glyph_index) return g1==g2 ? -1 : g1; // if length is 0, return -1 } +static int stbtt__GetGlyphInfoT2(const stbtt_fontinfo *info, int glyph_index, int *x0, int *y0, int *x1, int *y1); + STBTT_DEF int stbtt_GetGlyphBox(const stbtt_fontinfo *info, int glyph_index, int *x0, int *y0, int *x1, int *y1) { - int g = stbtt__GetGlyfOffset(info, glyph_index); - if (g < 0) return 0; + if (info->cff.size) { + stbtt__GetGlyphInfoT2(info, glyph_index, x0, y0, x1, y1); + } else { + int g = stbtt__GetGlyfOffset(info, glyph_index); + if (g < 0) return 0; - if (x0) *x0 = ttSHORT(info->data + g + 2); - if (y0) *y0 = ttSHORT(info->data + g + 4); - if (x1) *x1 = ttSHORT(info->data + g + 6); - if (y1) *y1 = ttSHORT(info->data + g + 8); + if (x0) *x0 = ttSHORT(info->data + g + 2); + if (y0) *y0 = ttSHORT(info->data + g + 4); + if (x1) *x1 = ttSHORT(info->data + g + 6); + if (y1) *y1 = ttSHORT(info->data + g + 8); + } return 1; } @@ -1218,7 +1641,10 @@ STBTT_DEF int stbtt_GetCodepointBox(const stbtt_fontinfo *info, int codepoint, i STBTT_DEF int stbtt_IsGlyphEmpty(const stbtt_fontinfo *info, int glyph_index) { stbtt_int16 numberOfContours; - int g = stbtt__GetGlyfOffset(info, glyph_index); + int g; + if (info->cff.size) + return stbtt__GetGlyphInfoT2(info, glyph_index, NULL, NULL, NULL, NULL) == 0; + g = stbtt__GetGlyfOffset(info, glyph_index); if (g < 0) return 1; numberOfContours = ttSHORT(info->data + g); return numberOfContours == 0; @@ -1240,7 +1666,7 @@ static int stbtt__close_shape(stbtt_vertex *vertices, int num_vertices, int was_ return num_vertices; } -STBTT_DEF int stbtt_GetGlyphShape(const stbtt_fontinfo *info, int glyph_index, stbtt_vertex **pvertices) +static int stbtt__GetGlyphShapeTT(const stbtt_fontinfo *info, int glyph_index, stbtt_vertex **pvertices) { stbtt_int16 numberOfContours; stbtt_uint8 *endPtsOfContours; @@ -1336,7 +1762,7 @@ STBTT_DEF int stbtt_GetGlyphShape(const stbtt_fontinfo *info, int glyph_index, s if (i != 0) num_vertices = stbtt__close_shape(vertices, num_vertices, was_off, start_off, sx,sy,scx,scy,cx,cy); - // now start the new one + // now start the new one start_off = !(flags & 1); if (start_off) { // if we start off with an off-curve point, then when we need to find a point on the curve @@ -1378,7 +1804,7 @@ STBTT_DEF int stbtt_GetGlyphShape(const stbtt_fontinfo *info, int glyph_index, s } } num_vertices = stbtt__close_shape(vertices, num_vertices, was_off, start_off, sx,sy,scx,scy,cx,cy); - } else if (numberOfContours == -1) { + } else if (numberOfContours < 0) { // Compound shapes. int more = 1; stbtt_uint8 *comp = data + g + 10; @@ -1389,7 +1815,7 @@ STBTT_DEF int stbtt_GetGlyphShape(const stbtt_fontinfo *info, int glyph_index, s int comp_num_verts = 0, i; stbtt_vertex *comp_verts = 0, *tmp = 0; float mtx[6] = {1,0,0,1,0,0}, m, n; - + flags = ttSHORT(comp); comp+=2; gidx = ttSHORT(comp); comp+=2; @@ -1419,7 +1845,7 @@ STBTT_DEF int stbtt_GetGlyphShape(const stbtt_fontinfo *info, int glyph_index, s mtx[2] = ttSHORT(comp)/16384.0f; comp+=2; mtx[3] = ttSHORT(comp)/16384.0f; comp+=2; } - + // Find transformation scales. m = (float) STBTT_sqrt(mtx[0]*mtx[0] + mtx[1]*mtx[1]); n = (float) STBTT_sqrt(mtx[2]*mtx[2] + mtx[3]*mtx[3]); @@ -1455,9 +1881,6 @@ STBTT_DEF int stbtt_GetGlyphShape(const stbtt_fontinfo *info, int glyph_index, s // More components ? more = flags & (1<<5); } - } else if (numberOfContours < 0) { - // @TODO other compound variations? - STBTT_assert(0); } else { // numberOfCounters == 0, do nothing } @@ -1466,6 +1889,414 @@ STBTT_DEF int stbtt_GetGlyphShape(const stbtt_fontinfo *info, int glyph_index, s return num_vertices; } +typedef struct +{ + int bounds; + int started; + float first_x, first_y; + float x, y; + stbtt_int32 min_x, max_x, min_y, max_y; + + stbtt_vertex *pvertices; + int num_vertices; +} stbtt__csctx; + +#define STBTT__CSCTX_INIT(bounds) {bounds,0, 0,0, 0,0, 0,0,0,0, NULL, 0} + +static void stbtt__track_vertex(stbtt__csctx *c, stbtt_int32 x, stbtt_int32 y) +{ + if (x > c->max_x || !c->started) c->max_x = x; + if (y > c->max_y || !c->started) c->max_y = y; + if (x < c->min_x || !c->started) c->min_x = x; + if (y < c->min_y || !c->started) c->min_y = y; + c->started = 1; +} + +static void stbtt__csctx_v(stbtt__csctx *c, stbtt_uint8 type, stbtt_int32 x, stbtt_int32 y, stbtt_int32 cx, stbtt_int32 cy, stbtt_int32 cx1, stbtt_int32 cy1) +{ + if (c->bounds) { + stbtt__track_vertex(c, x, y); + if (type == STBTT_vcubic) { + stbtt__track_vertex(c, cx, cy); + stbtt__track_vertex(c, cx1, cy1); + } + } else { + stbtt_setvertex(&c->pvertices[c->num_vertices], type, x, y, cx, cy); + c->pvertices[c->num_vertices].cx1 = (stbtt_int16) cx1; + c->pvertices[c->num_vertices].cy1 = (stbtt_int16) cy1; + } + c->num_vertices++; +} + +static void stbtt__csctx_close_shape(stbtt__csctx *ctx) +{ + if (ctx->first_x != ctx->x || ctx->first_y != ctx->y) + stbtt__csctx_v(ctx, STBTT_vline, (int)ctx->first_x, (int)ctx->first_y, 0, 0, 0, 0); +} + +static void stbtt__csctx_rmove_to(stbtt__csctx *ctx, float dx, float dy) +{ + stbtt__csctx_close_shape(ctx); + ctx->first_x = ctx->x = ctx->x + dx; + ctx->first_y = ctx->y = ctx->y + dy; + stbtt__csctx_v(ctx, STBTT_vmove, (int)ctx->x, (int)ctx->y, 0, 0, 0, 0); +} + +static void stbtt__csctx_rline_to(stbtt__csctx *ctx, float dx, float dy) +{ + ctx->x += dx; + ctx->y += dy; + stbtt__csctx_v(ctx, STBTT_vline, (int)ctx->x, (int)ctx->y, 0, 0, 0, 0); +} + +static void stbtt__csctx_rccurve_to(stbtt__csctx *ctx, float dx1, float dy1, float dx2, float dy2, float dx3, float dy3) +{ + float cx1 = ctx->x + dx1; + float cy1 = ctx->y + dy1; + float cx2 = cx1 + dx2; + float cy2 = cy1 + dy2; + ctx->x = cx2 + dx3; + ctx->y = cy2 + dy3; + stbtt__csctx_v(ctx, STBTT_vcubic, (int)ctx->x, (int)ctx->y, (int)cx1, (int)cy1, (int)cx2, (int)cy2); +} + +static stbtt__buf stbtt__get_subr(stbtt__buf idx, int n) +{ + int count = stbtt__cff_index_count(&idx); + int bias = 107; + if (count >= 33900) + bias = 32768; + else if (count >= 1240) + bias = 1131; + n += bias; + if (n < 0 || n >= count) + return stbtt__new_buf(NULL, 0); + return stbtt__cff_index_get(idx, n); +} + +static stbtt__buf stbtt__cid_get_glyph_subrs(const stbtt_fontinfo *info, int glyph_index) +{ + stbtt__buf fdselect = info->fdselect; + int nranges, start, end, v, fmt, fdselector = -1, i; + + stbtt__buf_seek(&fdselect, 0); + fmt = stbtt__buf_get8(&fdselect); + if (fmt == 0) { + // untested + stbtt__buf_skip(&fdselect, glyph_index); + fdselector = stbtt__buf_get8(&fdselect); + } else if (fmt == 3) { + nranges = stbtt__buf_get16(&fdselect); + start = stbtt__buf_get16(&fdselect); + for (i = 0; i < nranges; i++) { + v = stbtt__buf_get8(&fdselect); + end = stbtt__buf_get16(&fdselect); + if (glyph_index >= start && glyph_index < end) { + fdselector = v; + break; + } + start = end; + } + } + if (fdselector == -1) stbtt__new_buf(NULL, 0); + return stbtt__get_subrs(info->cff, stbtt__cff_index_get(info->fontdicts, fdselector)); +} + +static int stbtt__run_charstring(const stbtt_fontinfo *info, int glyph_index, stbtt__csctx *c) +{ + int in_header = 1, maskbits = 0, subr_stack_height = 0, sp = 0, v, i, b0; + int has_subrs = 0, clear_stack; + float s[48]; + stbtt__buf subr_stack[10], subrs = info->subrs, b; + float f; + +#define STBTT__CSERR(s) (0) + + // this currently ignores the initial width value, which isn't needed if we have hmtx + b = stbtt__cff_index_get(info->charstrings, glyph_index); + while (b.cursor < b.size) { + i = 0; + clear_stack = 1; + b0 = stbtt__buf_get8(&b); + switch (b0) { + // @TODO implement hinting + case 0x13: // hintmask + case 0x14: // cntrmask + if (in_header) + maskbits += (sp / 2); // implicit "vstem" + in_header = 0; + stbtt__buf_skip(&b, (maskbits + 7) / 8); + break; + + case 0x01: // hstem + case 0x03: // vstem + case 0x12: // hstemhm + case 0x17: // vstemhm + maskbits += (sp / 2); + break; + + case 0x15: // rmoveto + in_header = 0; + if (sp < 2) return STBTT__CSERR("rmoveto stack"); + stbtt__csctx_rmove_to(c, s[sp-2], s[sp-1]); + break; + case 0x04: // vmoveto + in_header = 0; + if (sp < 1) return STBTT__CSERR("vmoveto stack"); + stbtt__csctx_rmove_to(c, 0, s[sp-1]); + break; + case 0x16: // hmoveto + in_header = 0; + if (sp < 1) return STBTT__CSERR("hmoveto stack"); + stbtt__csctx_rmove_to(c, s[sp-1], 0); + break; + + case 0x05: // rlineto + if (sp < 2) return STBTT__CSERR("rlineto stack"); + for (; i + 1 < sp; i += 2) + stbtt__csctx_rline_to(c, s[i], s[i+1]); + break; + + // hlineto/vlineto and vhcurveto/hvcurveto alternate horizontal and vertical + // starting from a different place. + + case 0x07: // vlineto + if (sp < 1) return STBTT__CSERR("vlineto stack"); + goto vlineto; + case 0x06: // hlineto + if (sp < 1) return STBTT__CSERR("hlineto stack"); + for (;;) { + if (i >= sp) break; + stbtt__csctx_rline_to(c, s[i], 0); + i++; + vlineto: + if (i >= sp) break; + stbtt__csctx_rline_to(c, 0, s[i]); + i++; + } + break; + + case 0x1F: // hvcurveto + if (sp < 4) return STBTT__CSERR("hvcurveto stack"); + goto hvcurveto; + case 0x1E: // vhcurveto + if (sp < 4) return STBTT__CSERR("vhcurveto stack"); + for (;;) { + if (i + 3 >= sp) break; + stbtt__csctx_rccurve_to(c, 0, s[i], s[i+1], s[i+2], s[i+3], (sp - i == 5) ? s[i + 4] : 0.0f); + i += 4; + hvcurveto: + if (i + 3 >= sp) break; + stbtt__csctx_rccurve_to(c, s[i], 0, s[i+1], s[i+2], (sp - i == 5) ? s[i+4] : 0.0f, s[i+3]); + i += 4; + } + break; + + case 0x08: // rrcurveto + if (sp < 6) return STBTT__CSERR("rcurveline stack"); + for (; i + 5 < sp; i += 6) + stbtt__csctx_rccurve_to(c, s[i], s[i+1], s[i+2], s[i+3], s[i+4], s[i+5]); + break; + + case 0x18: // rcurveline + if (sp < 8) return STBTT__CSERR("rcurveline stack"); + for (; i + 5 < sp - 2; i += 6) + stbtt__csctx_rccurve_to(c, s[i], s[i+1], s[i+2], s[i+3], s[i+4], s[i+5]); + if (i + 1 >= sp) return STBTT__CSERR("rcurveline stack"); + stbtt__csctx_rline_to(c, s[i], s[i+1]); + break; + + case 0x19: // rlinecurve + if (sp < 8) return STBTT__CSERR("rlinecurve stack"); + for (; i + 1 < sp - 6; i += 2) + stbtt__csctx_rline_to(c, s[i], s[i+1]); + if (i + 5 >= sp) return STBTT__CSERR("rlinecurve stack"); + stbtt__csctx_rccurve_to(c, s[i], s[i+1], s[i+2], s[i+3], s[i+4], s[i+5]); + break; + + case 0x1A: // vvcurveto + case 0x1B: // hhcurveto + if (sp < 4) return STBTT__CSERR("(vv|hh)curveto stack"); + f = 0.0; + if (sp & 1) { f = s[i]; i++; } + for (; i + 3 < sp; i += 4) { + if (b0 == 0x1B) + stbtt__csctx_rccurve_to(c, s[i], f, s[i+1], s[i+2], s[i+3], 0.0); + else + stbtt__csctx_rccurve_to(c, f, s[i], s[i+1], s[i+2], 0.0, s[i+3]); + f = 0.0; + } + break; + + case 0x0A: // callsubr + if (!has_subrs) { + if (info->fdselect.size) + subrs = stbtt__cid_get_glyph_subrs(info, glyph_index); + has_subrs = 1; + } + // fallthrough + case 0x1D: // callgsubr + if (sp < 1) return STBTT__CSERR("call(g|)subr stack"); + v = (int) s[--sp]; + if (subr_stack_height >= 10) return STBTT__CSERR("recursion limit"); + subr_stack[subr_stack_height++] = b; + b = stbtt__get_subr(b0 == 0x0A ? subrs : info->gsubrs, v); + if (b.size == 0) return STBTT__CSERR("subr not found"); + b.cursor = 0; + clear_stack = 0; + break; + + case 0x0B: // return + if (subr_stack_height <= 0) return STBTT__CSERR("return outside subr"); + b = subr_stack[--subr_stack_height]; + clear_stack = 0; + break; + + case 0x0E: // endchar + stbtt__csctx_close_shape(c); + return 1; + + case 0x0C: { // two-byte escape + float dx1, dx2, dx3, dx4, dx5, dx6, dy1, dy2, dy3, dy4, dy5, dy6; + float dx, dy; + int b1 = stbtt__buf_get8(&b); + switch (b1) { + // @TODO These "flex" implementations ignore the flex-depth and resolution, + // and always draw beziers. + case 0x22: // hflex + if (sp < 7) return STBTT__CSERR("hflex stack"); + dx1 = s[0]; + dx2 = s[1]; + dy2 = s[2]; + dx3 = s[3]; + dx4 = s[4]; + dx5 = s[5]; + dx6 = s[6]; + stbtt__csctx_rccurve_to(c, dx1, 0, dx2, dy2, dx3, 0); + stbtt__csctx_rccurve_to(c, dx4, 0, dx5, -dy2, dx6, 0); + break; + + case 0x23: // flex + if (sp < 13) return STBTT__CSERR("flex stack"); + dx1 = s[0]; + dy1 = s[1]; + dx2 = s[2]; + dy2 = s[3]; + dx3 = s[4]; + dy3 = s[5]; + dx4 = s[6]; + dy4 = s[7]; + dx5 = s[8]; + dy5 = s[9]; + dx6 = s[10]; + dy6 = s[11]; + //fd is s[12] + stbtt__csctx_rccurve_to(c, dx1, dy1, dx2, dy2, dx3, dy3); + stbtt__csctx_rccurve_to(c, dx4, dy4, dx5, dy5, dx6, dy6); + break; + + case 0x24: // hflex1 + if (sp < 9) return STBTT__CSERR("hflex1 stack"); + dx1 = s[0]; + dy1 = s[1]; + dx2 = s[2]; + dy2 = s[3]; + dx3 = s[4]; + dx4 = s[5]; + dx5 = s[6]; + dy5 = s[7]; + dx6 = s[8]; + stbtt__csctx_rccurve_to(c, dx1, dy1, dx2, dy2, dx3, 0); + stbtt__csctx_rccurve_to(c, dx4, 0, dx5, dy5, dx6, -(dy1+dy2+dy5)); + break; + + case 0x25: // flex1 + if (sp < 11) return STBTT__CSERR("flex1 stack"); + dx1 = s[0]; + dy1 = s[1]; + dx2 = s[2]; + dy2 = s[3]; + dx3 = s[4]; + dy3 = s[5]; + dx4 = s[6]; + dy4 = s[7]; + dx5 = s[8]; + dy5 = s[9]; + dx6 = dy6 = s[10]; + dx = dx1+dx2+dx3+dx4+dx5; + dy = dy1+dy2+dy3+dy4+dy5; + if (STBTT_fabs(dx) > STBTT_fabs(dy)) + dy6 = -dy; + else + dx6 = -dx; + stbtt__csctx_rccurve_to(c, dx1, dy1, dx2, dy2, dx3, dy3); + stbtt__csctx_rccurve_to(c, dx4, dy4, dx5, dy5, dx6, dy6); + break; + + default: + return STBTT__CSERR("unimplemented"); + } + } break; + + default: + if (b0 != 255 && b0 != 28 && (b0 < 32 || b0 > 254)) + return STBTT__CSERR("reserved operator"); + + // push immediate + if (b0 == 255) { + f = (float)(stbtt_int32)stbtt__buf_get32(&b) / 0x10000; + } else { + stbtt__buf_skip(&b, -1); + f = (float)(stbtt_int16)stbtt__cff_int(&b); + } + if (sp >= 48) return STBTT__CSERR("push stack overflow"); + s[sp++] = f; + clear_stack = 0; + break; + } + if (clear_stack) sp = 0; + } + return STBTT__CSERR("no endchar"); + +#undef STBTT__CSERR +} + +static int stbtt__GetGlyphShapeT2(const stbtt_fontinfo *info, int glyph_index, stbtt_vertex **pvertices) +{ + // runs the charstring twice, once to count and once to output (to avoid realloc) + stbtt__csctx count_ctx = STBTT__CSCTX_INIT(1); + stbtt__csctx output_ctx = STBTT__CSCTX_INIT(0); + if (stbtt__run_charstring(info, glyph_index, &count_ctx)) { + *pvertices = (stbtt_vertex*)STBTT_malloc(count_ctx.num_vertices*sizeof(stbtt_vertex), info->userdata); + output_ctx.pvertices = *pvertices; + if (stbtt__run_charstring(info, glyph_index, &output_ctx)) { + STBTT_assert(output_ctx.num_vertices == count_ctx.num_vertices); + return output_ctx.num_vertices; + } + } + *pvertices = NULL; + return 0; +} + +static int stbtt__GetGlyphInfoT2(const stbtt_fontinfo *info, int glyph_index, int *x0, int *y0, int *x1, int *y1) +{ + stbtt__csctx c = STBTT__CSCTX_INIT(1); + int r = stbtt__run_charstring(info, glyph_index, &c); + if (x0) *x0 = r ? c.min_x : 0; + if (y0) *y0 = r ? c.min_y : 0; + if (x1) *x1 = r ? c.max_x : 0; + if (y1) *y1 = r ? c.max_y : 0; + return r ? c.num_vertices : 0; +} + +STBTT_DEF int stbtt_GetGlyphShape(const stbtt_fontinfo *info, int glyph_index, stbtt_vertex **pvertices) +{ + if (!info->cff.size) + return stbtt__GetGlyphShapeTT(info, glyph_index, pvertices); + else + return stbtt__GetGlyphShapeT2(info, glyph_index, pvertices); +} + STBTT_DEF void stbtt_GetGlyphHMetrics(const stbtt_fontinfo *info, int glyph_index, int *advanceWidth, int *leftSideBearing) { stbtt_uint16 numOfLongHorMetrics = ttUSHORT(info->data+info->hhea + 34); @@ -1478,7 +2309,49 @@ STBTT_DEF void stbtt_GetGlyphHMetrics(const stbtt_fontinfo *info, int glyph_inde } } -STBTT_DEF int stbtt_GetGlyphKernAdvance(const stbtt_fontinfo *info, int glyph1, int glyph2) +STBTT_DEF int stbtt_GetKerningTableLength(const stbtt_fontinfo *info) +{ + stbtt_uint8 *data = info->data + info->kern; + + // we only look at the first table. it must be 'horizontal' and format 0. + if (!info->kern) + return 0; + if (ttUSHORT(data+2) < 1) // number of tables, need at least 1 + return 0; + if (ttUSHORT(data+8) != 1) // horizontal flag must be set in format + return 0; + + return ttUSHORT(data+10); +} + +STBTT_DEF int stbtt_GetKerningTable(const stbtt_fontinfo *info, stbtt_kerningentry* table, int table_length) +{ + stbtt_uint8 *data = info->data + info->kern; + int k, length; + + // we only look at the first table. it must be 'horizontal' and format 0. + if (!info->kern) + return 0; + if (ttUSHORT(data+2) < 1) // number of tables, need at least 1 + return 0; + if (ttUSHORT(data+8) != 1) // horizontal flag must be set in format + return 0; + + length = ttUSHORT(data+10); + if (table_length < length) + length = table_length; + + for (k = 0; k < length; k++) + { + table[k].glyph1 = ttUSHORT(data+18+(k*6)); + table[k].glyph2 = ttUSHORT(data+20+(k*6)); + table[k].advance = ttSHORT(data+22+(k*6)); + } + + return length; +} + +static int stbtt__GetGlyphKernInfoAdvance(const stbtt_fontinfo *info, int glyph1, int glyph2) { stbtt_uint8 *data = info->data + info->kern; stbtt_uint32 needle, straw; @@ -1508,9 +2381,260 @@ STBTT_DEF int stbtt_GetGlyphKernAdvance(const stbtt_fontinfo *info, int glyph1, return 0; } +static stbtt_int32 stbtt__GetCoverageIndex(stbtt_uint8 *coverageTable, int glyph) +{ + stbtt_uint16 coverageFormat = ttUSHORT(coverageTable); + switch(coverageFormat) { + case 1: { + stbtt_uint16 glyphCount = ttUSHORT(coverageTable + 2); + + // Binary search. + stbtt_int32 l=0, r=glyphCount-1, m; + int straw, needle=glyph; + while (l <= r) { + stbtt_uint8 *glyphArray = coverageTable + 4; + stbtt_uint16 glyphID; + m = (l + r) >> 1; + glyphID = ttUSHORT(glyphArray + 2 * m); + straw = glyphID; + if (needle < straw) + r = m - 1; + else if (needle > straw) + l = m + 1; + else { + return m; + } + } + } break; + + case 2: { + stbtt_uint16 rangeCount = ttUSHORT(coverageTable + 2); + stbtt_uint8 *rangeArray = coverageTable + 4; + + // Binary search. + stbtt_int32 l=0, r=rangeCount-1, m; + int strawStart, strawEnd, needle=glyph; + while (l <= r) { + stbtt_uint8 *rangeRecord; + m = (l + r) >> 1; + rangeRecord = rangeArray + 6 * m; + strawStart = ttUSHORT(rangeRecord); + strawEnd = ttUSHORT(rangeRecord + 2); + if (needle < strawStart) + r = m - 1; + else if (needle > strawEnd) + l = m + 1; + else { + stbtt_uint16 startCoverageIndex = ttUSHORT(rangeRecord + 4); + return startCoverageIndex + glyph - strawStart; + } + } + } break; + + default: { + // There are no other cases. + STBTT_assert(0); + } break; + } + + return -1; +} + +static stbtt_int32 stbtt__GetGlyphClass(stbtt_uint8 *classDefTable, int glyph) +{ + stbtt_uint16 classDefFormat = ttUSHORT(classDefTable); + switch(classDefFormat) + { + case 1: { + stbtt_uint16 startGlyphID = ttUSHORT(classDefTable + 2); + stbtt_uint16 glyphCount = ttUSHORT(classDefTable + 4); + stbtt_uint8 *classDef1ValueArray = classDefTable + 6; + + if (glyph >= startGlyphID && glyph < startGlyphID + glyphCount) + return (stbtt_int32)ttUSHORT(classDef1ValueArray + 2 * (glyph - startGlyphID)); + + classDefTable = classDef1ValueArray + 2 * glyphCount; + } break; + + case 2: { + stbtt_uint16 classRangeCount = ttUSHORT(classDefTable + 2); + stbtt_uint8 *classRangeRecords = classDefTable + 4; + + // Binary search. + stbtt_int32 l=0, r=classRangeCount-1, m; + int strawStart, strawEnd, needle=glyph; + while (l <= r) { + stbtt_uint8 *classRangeRecord; + m = (l + r) >> 1; + classRangeRecord = classRangeRecords + 6 * m; + strawStart = ttUSHORT(classRangeRecord); + strawEnd = ttUSHORT(classRangeRecord + 2); + if (needle < strawStart) + r = m - 1; + else if (needle > strawEnd) + l = m + 1; + else + return (stbtt_int32)ttUSHORT(classRangeRecord + 4); + } + + classDefTable = classRangeRecords + 6 * classRangeCount; + } break; + + default: { + // There are no other cases. + STBTT_assert(0); + } break; + } + + return -1; +} + +// Define to STBTT_assert(x) if you want to break on unimplemented formats. +#define STBTT_GPOS_TODO_assert(x) + +static stbtt_int32 stbtt__GetGlyphGPOSInfoAdvance(const stbtt_fontinfo *info, int glyph1, int glyph2) +{ + stbtt_uint16 lookupListOffset; + stbtt_uint8 *lookupList; + stbtt_uint16 lookupCount; + stbtt_uint8 *data; + stbtt_int32 i; + + if (!info->gpos) return 0; + + data = info->data + info->gpos; + + if (ttUSHORT(data+0) != 1) return 0; // Major version 1 + if (ttUSHORT(data+2) != 0) return 0; // Minor version 0 + + lookupListOffset = ttUSHORT(data+8); + lookupList = data + lookupListOffset; + lookupCount = ttUSHORT(lookupList); + + for (i=0; i> 1; + pairValue = pairValueArray + (2 + valueRecordPairSizeInBytes) * m; + secondGlyph = ttUSHORT(pairValue); + straw = secondGlyph; + if (needle < straw) + r = m - 1; + else if (needle > straw) + l = m + 1; + else { + stbtt_int16 xAdvance = ttSHORT(pairValue + 2); + return xAdvance; + } + } + } break; + + case 2: { + stbtt_uint16 valueFormat1 = ttUSHORT(table + 4); + stbtt_uint16 valueFormat2 = ttUSHORT(table + 6); + + stbtt_uint16 classDef1Offset = ttUSHORT(table + 8); + stbtt_uint16 classDef2Offset = ttUSHORT(table + 10); + int glyph1class = stbtt__GetGlyphClass(table + classDef1Offset, glyph1); + int glyph2class = stbtt__GetGlyphClass(table + classDef2Offset, glyph2); + + stbtt_uint16 class1Count = ttUSHORT(table + 12); + stbtt_uint16 class2Count = ttUSHORT(table + 14); + STBTT_assert(glyph1class < class1Count); + STBTT_assert(glyph2class < class2Count); + + // TODO: Support more formats. + STBTT_GPOS_TODO_assert(valueFormat1 == 4); + if (valueFormat1 != 4) return 0; + STBTT_GPOS_TODO_assert(valueFormat2 == 0); + if (valueFormat2 != 0) return 0; + + if (glyph1class >= 0 && glyph1class < class1Count && glyph2class >= 0 && glyph2class < class2Count) { + stbtt_uint8 *class1Records = table + 16; + stbtt_uint8 *class2Records = class1Records + 2 * (glyph1class * class2Count); + stbtt_int16 xAdvance = ttSHORT(class2Records + 2 * glyph2class); + return xAdvance; + } + } break; + + default: { + // There are no other cases. + STBTT_assert(0); + break; + }; + } + } + break; + }; + + default: + // TODO: Implement other stuff. + break; + } + } + + return 0; +} + +STBTT_DEF int stbtt_GetGlyphKernAdvance(const stbtt_fontinfo *info, int g1, int g2) +{ + int xAdvance = 0; + + if (info->gpos) + xAdvance += stbtt__GetGlyphGPOSInfoAdvance(info, g1, g2); + else if (info->kern) + xAdvance += stbtt__GetGlyphKernInfoAdvance(info, g1, g2); + + return xAdvance; +} + STBTT_DEF int stbtt_GetCodepointKernAdvance(const stbtt_fontinfo *info, int ch1, int ch2) { - if (!info->kern) // if no kerning table, don't waste time looking up both codepoint->glyphs + if (!info->kern && !info->gpos) // if no kerning table, don't waste time looking up both codepoint->glyphs return 0; return stbtt_GetGlyphKernAdvance(info, stbtt_FindGlyphIndex(info,ch1), stbtt_FindGlyphIndex(info,ch2)); } @@ -1527,6 +2651,17 @@ STBTT_DEF void stbtt_GetFontVMetrics(const stbtt_fontinfo *info, int *ascent, in if (lineGap) *lineGap = ttSHORT(info->data+info->hhea + 8); } +STBTT_DEF int stbtt_GetFontVMetricsOS2(const stbtt_fontinfo *info, int *typoAscent, int *typoDescent, int *typoLineGap) +{ + int tab = stbtt__find_table(info->data, info->fontstart, "OS/2"); + if (!tab) + return 0; + if (typoAscent ) *typoAscent = ttSHORT(info->data+tab + 68); + if (typoDescent) *typoDescent = ttSHORT(info->data+tab + 70); + if (typoLineGap) *typoLineGap = ttSHORT(info->data+tab + 72); + return 1; +} + STBTT_DEF void stbtt_GetFontBoundingBox(const stbtt_fontinfo *info, int *x0, int *y0, int *x1, int *y1) { *x0 = ttSHORT(info->data + info->head + 36); @@ -1552,6 +2687,45 @@ STBTT_DEF void stbtt_FreeShape(const stbtt_fontinfo *info, stbtt_vertex *v) STBTT_free(v, info->userdata); } +STBTT_DEF stbtt_uint8 *stbtt_FindSVGDoc(const stbtt_fontinfo *info, int gl) +{ + int i; + stbtt_uint8 *data = info->data; + stbtt_uint8 *svg_doc_list = data + stbtt__get_svg((stbtt_fontinfo *) info); + + int numEntries = ttUSHORT(svg_doc_list); + stbtt_uint8 *svg_docs = svg_doc_list + 2; + + for(i=0; i= ttUSHORT(svg_doc)) && (gl <= ttUSHORT(svg_doc + 2))) + return svg_doc; + } + return 0; +} + +STBTT_DEF int stbtt_GetGlyphSVG(const stbtt_fontinfo *info, int gl, const char **svg) +{ + stbtt_uint8 *data = info->data; + stbtt_uint8 *svg_doc; + + if (info->svg == 0) + return 0; + + svg_doc = stbtt_FindSVGDoc(info, gl); + if (svg_doc != NULL) { + *svg = (char *) data + info->svg + ttULONG(svg_doc + 4); + return ttULONG(svg_doc + 8); + } else { + return 0; + } +} + +STBTT_DEF int stbtt_GetCodepointSVG(const stbtt_fontinfo *info, int unicode_codepoint, const char **svg) +{ + return stbtt_GetGlyphSVG(info, stbtt_FindGlyphIndex(info, unicode_codepoint), svg); +} + ////////////////////////////////////////////////////////////////////////////// // // antialiasing software rasterizer @@ -1623,7 +2797,7 @@ static void *stbtt__hheap_alloc(stbtt__hheap *hh, size_t size, void *userdata) hh->num_remaining_in_head_chunk = count; } --hh->num_remaining_in_head_chunk; - return (char *) (hh->head) + size * hh->num_remaining_in_head_chunk; + return (char *) (hh->head) + sizeof(stbtt__hheap_chunk) + size * hh->num_remaining_in_head_chunk; } } @@ -1677,7 +2851,7 @@ static stbtt__active_edge *stbtt__new_active(stbtt__hheap *hh, stbtt__edge *e, i float dxdy = (e->x1 - e->x0) / (e->y1 - e->y0); STBTT_assert(z != NULL); if (!z) return z; - + // round dx down to avoid overshooting if (dxdy < 0) z->dx = -STBTT_ifloor(STBTT_FIX * -dxdy); @@ -1755,7 +2929,7 @@ static void stbtt__fill_active_edges(unsigned char *scanline, int len, stbtt__ac } } } - + e = e->next; } } @@ -1993,7 +3167,7 @@ static void stbtt__fill_active_edges_new(float *scanline, float *scanline_fill, } y_crossing += dy * (x2 - (x1+1)); - STBTT_assert(fabs(area) <= 1.01f); + STBTT_assert(STBTT_fabs(area) <= 1.01f); scanline[x2] += area + sign * (1-((x2-x2)+(x_bottom-x2))/2) * (sy1-y_crossing); @@ -2019,19 +3193,18 @@ static void stbtt__fill_active_edges_new(float *scanline, float *scanline_fill, // from the other y segment, and it might ignored as an empty segment. to avoid // that, we need to explicitly produce segments based on x positions. - // rename variables to clear pairs + // rename variables to clearly-defined pairs float y0 = y_top; float x1 = (float) (x); float x2 = (float) (x+1); float x3 = xb; float y3 = y_bottom; - float y1,y2; // x = e->x + e->dx * (y-y_top) // (y-y_top) = (x - e->x) / e->dx // y = (x - e->x) / e->dx + y_top - y1 = (x - x0) / dx + y_top; - y2 = (x+1 - x0) / dx + y_top; + float y1 = (x - x0) / dx + y_top; + float y2 = (x+1 - x0) / dx + y_top; if (x0 < x1 && x3 > x2) { // three segments descending down-right stbtt__handle_clipped_edge(scanline,x,e, x0,y0, x1,y1); @@ -2071,6 +3244,8 @@ static void stbtt__rasterize_sorted_edges(stbtt__bitmap *result, stbtt__edge *e, int y,j=0, i; float scanline_data[129], *scanline, *scanline2; + STBTT__NOTUSED(vsubsample); + if (result->w > 64) scanline = (float *) STBTT_malloc((result->w*2+1) * sizeof(float), userdata); else @@ -2109,7 +3284,13 @@ static void stbtt__rasterize_sorted_edges(stbtt__bitmap *result, stbtt__edge *e, if (e->y0 != e->y1) { stbtt__active_edge *z = stbtt__new_active(&hh, e, off_x, scan_y_top, userdata); if (z != NULL) { - STBTT_assert(z->ey >= scan_y_top); + if (j == 0 && off_y != 0) { + if (z->ey < scan_y_top) { + // this can happen due to subpixel positioning and some kind of fp rounding error i think + z->ey = scan_y_top; + } + } + STBTT_assert(z->ey >= scan_y_top); // if we get really unlucky a tiny bit of an edge can be out of bounds // insert at front z->next = active; active = z; @@ -2129,7 +3310,7 @@ static void stbtt__rasterize_sorted_edges(stbtt__bitmap *result, stbtt__edge *e, int m; sum += scanline2[i]; k = scanline[i] + sum; - k = (float) fabs(k)*255 + 0.5f; + k = (float) STBTT_fabs(k)*255 + 0.5f; m = (int) k; if (m > 255) m = 255; result->pixels[j*result->stride + i] = (unsigned char) m; @@ -2178,7 +3359,7 @@ static void stbtt__sort_edges_ins_sort(stbtt__edge *p, int n) static void stbtt__sort_edges_quicksort(stbtt__edge *p, int n) { - /* threshhold for transitioning to insertion sort */ + /* threshold for transitioning to insertion sort */ while (n > 12) { stbtt__edge t; int c01,c12,c,m,i,j; @@ -2313,7 +3494,7 @@ static void stbtt__add_point(stbtt__point *points, int n, float x, float y) points[n].y = y; } -// tesselate until threshhold p is happy... @TODO warped to compensate for non-linear stretching +// tessellate until threshold p is happy... @TODO warped to compensate for non-linear stretching static int stbtt__tesselate_curve(stbtt__point *points, int *num_points, float x0, float y0, float x1, float y1, float x2, float y2, float objspace_flatness_squared, int n) { // midpoint @@ -2334,6 +3515,48 @@ static int stbtt__tesselate_curve(stbtt__point *points, int *num_points, float x return 1; } +static void stbtt__tesselate_cubic(stbtt__point *points, int *num_points, float x0, float y0, float x1, float y1, float x2, float y2, float x3, float y3, float objspace_flatness_squared, int n) +{ + // @TODO this "flatness" calculation is just made-up nonsense that seems to work well enough + float dx0 = x1-x0; + float dy0 = y1-y0; + float dx1 = x2-x1; + float dy1 = y2-y1; + float dx2 = x3-x2; + float dy2 = y3-y2; + float dx = x3-x0; + float dy = y3-y0; + float longlen = (float) (STBTT_sqrt(dx0*dx0+dy0*dy0)+STBTT_sqrt(dx1*dx1+dy1*dy1)+STBTT_sqrt(dx2*dx2+dy2*dy2)); + float shortlen = (float) STBTT_sqrt(dx*dx+dy*dy); + float flatness_squared = longlen*longlen-shortlen*shortlen; + + if (n > 16) // 65536 segments on one curve better be enough! + return; + + if (flatness_squared > objspace_flatness_squared) { + float x01 = (x0+x1)/2; + float y01 = (y0+y1)/2; + float x12 = (x1+x2)/2; + float y12 = (y1+y2)/2; + float x23 = (x2+x3)/2; + float y23 = (y2+y3)/2; + + float xa = (x01+x12)/2; + float ya = (y01+y12)/2; + float xb = (x12+x23)/2; + float yb = (y12+y23)/2; + + float mx = (xa+xb)/2; + float my = (ya+yb)/2; + + stbtt__tesselate_cubic(points, num_points, x0,y0, x01,y01, xa,ya, mx,my, objspace_flatness_squared,n+1); + stbtt__tesselate_cubic(points, num_points, mx,my, xb,yb, x23,y23, x3,y3, objspace_flatness_squared,n+1); + } else { + stbtt__add_point(points, *num_points,x3,y3); + *num_points = *num_points+1; + } +} + // returns number of contours static stbtt__point *stbtt_FlattenCurves(stbtt_vertex *vertices, int num_verts, float objspace_flatness, int **contour_lengths, int *num_contours, void *userdata) { @@ -2390,6 +3613,14 @@ static stbtt__point *stbtt_FlattenCurves(stbtt_vertex *vertices, int num_verts, objspace_flatness_squared, 0); x = vertices[i].x, y = vertices[i].y; break; + case STBTT_vcubic: + stbtt__tesselate_cubic(points, &num_points, x,y, + vertices[i].cx, vertices[i].cy, + vertices[i].cx1, vertices[i].cy1, + vertices[i].x, vertices[i].y, + objspace_flatness_squared, 0); + x = vertices[i].x, y = vertices[i].y; + break; } } (*contour_lengths)[n] = num_points - start; @@ -2406,8 +3637,9 @@ static stbtt__point *stbtt_FlattenCurves(stbtt_vertex *vertices, int num_verts, STBTT_DEF void stbtt_Rasterize(stbtt__bitmap *result, float flatness_in_pixels, stbtt_vertex *vertices, int num_verts, float scale_x, float scale_y, float shift_x, float shift_y, int x_off, int y_off, int invert, void *userdata) { - float scale = scale_x > scale_y ? scale_y : scale_x; - int winding_count, *winding_lengths; + float scale = scale_x > scale_y ? scale_y : scale_x; + int winding_count = 0; + int *winding_lengths = NULL; stbtt__point *windings = stbtt_FlattenCurves(vertices, num_verts, flatness_in_pixels / scale, &winding_lengths, &winding_count, userdata); if (windings) { stbtt__rasterize(result, windings, winding_lengths, winding_count, scale_x, scale_y, shift_x, shift_y, x_off, y_off, invert, userdata); @@ -2425,12 +3657,15 @@ STBTT_DEF unsigned char *stbtt_GetGlyphBitmapSubpixel(const stbtt_fontinfo *info { int ix0,iy0,ix1,iy1; stbtt__bitmap gbm; - stbtt_vertex *vertices; + stbtt_vertex *vertices; int num_verts = stbtt_GetGlyphShape(info, glyph, &vertices); if (scale_x == 0) scale_x = scale_y; if (scale_y == 0) { - if (scale_x == 0) return NULL; + if (scale_x == 0) { + STBTT_free(vertices, info->userdata); + return NULL; + } scale_y = scale_x; } @@ -2445,7 +3680,7 @@ STBTT_DEF unsigned char *stbtt_GetGlyphBitmapSubpixel(const stbtt_fontinfo *info if (height) *height = gbm.h; if (xoff ) *xoff = ix0; if (yoff ) *yoff = iy0; - + if (gbm.w && gbm.h) { gbm.pixels = (unsigned char *) STBTT_malloc(gbm.w * gbm.h, info->userdata); if (gbm.pixels) { @@ -2456,7 +3691,7 @@ STBTT_DEF unsigned char *stbtt_GetGlyphBitmapSubpixel(const stbtt_fontinfo *info } STBTT_free(vertices, info->userdata); return gbm.pixels; -} +} STBTT_DEF unsigned char *stbtt_GetGlyphBitmap(const stbtt_fontinfo *info, float scale_x, float scale_y, int glyph, int *width, int *height, int *xoff, int *yoff) { @@ -2468,7 +3703,7 @@ STBTT_DEF void stbtt_MakeGlyphBitmapSubpixel(const stbtt_fontinfo *info, unsigne int ix0,iy0; stbtt_vertex *vertices; int num_verts = stbtt_GetGlyphShape(info, glyph, &vertices); - stbtt__bitmap gbm; + stbtt__bitmap gbm; stbtt_GetGlyphBitmapBoxSubpixel(info, glyph, scale_x, scale_y, shift_x, shift_y, &ix0,&iy0,0,0); gbm.pixels = output; @@ -2490,7 +3725,12 @@ STBTT_DEF void stbtt_MakeGlyphBitmap(const stbtt_fontinfo *info, unsigned char * STBTT_DEF unsigned char *stbtt_GetCodepointBitmapSubpixel(const stbtt_fontinfo *info, float scale_x, float scale_y, float shift_x, float shift_y, int codepoint, int *width, int *height, int *xoff, int *yoff) { return stbtt_GetGlyphBitmapSubpixel(info, scale_x, scale_y,shift_x,shift_y, stbtt_FindGlyphIndex(info,codepoint), width,height,xoff,yoff); -} +} + +STBTT_DEF void stbtt_MakeCodepointBitmapSubpixelPrefilter(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, float shift_x, float shift_y, int oversample_x, int oversample_y, float *sub_x, float *sub_y, int codepoint) +{ + stbtt_MakeGlyphBitmapSubpixelPrefilter(info, output, out_w, out_h, out_stride, scale_x, scale_y, shift_x, shift_y, oversample_x, oversample_y, sub_x, sub_y, stbtt_FindGlyphIndex(info,codepoint)); +} STBTT_DEF void stbtt_MakeCodepointBitmapSubpixel(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, float shift_x, float shift_y, int codepoint) { @@ -2500,7 +3740,7 @@ STBTT_DEF void stbtt_MakeCodepointBitmapSubpixel(const stbtt_fontinfo *info, uns STBTT_DEF unsigned char *stbtt_GetCodepointBitmap(const stbtt_fontinfo *info, float scale_x, float scale_y, int codepoint, int *width, int *height, int *xoff, int *yoff) { return stbtt_GetCodepointBitmapSubpixel(info, scale_x, scale_y, 0.0f,0.0f, codepoint, width,height,xoff,yoff); -} +} STBTT_DEF void stbtt_MakeCodepointBitmap(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, int codepoint) { @@ -2513,7 +3753,7 @@ STBTT_DEF void stbtt_MakeCodepointBitmap(const stbtt_fontinfo *info, unsigned ch // // This is SUPER-CRAPPY packing to keep source code small -STBTT_DEF int stbtt_BakeFontBitmap(const unsigned char *data, int offset, // font location (use offset=0 for plain .ttf) +static int stbtt_BakeFontBitmap_internal(unsigned char *data, int offset, // font location (use offset=0 for plain .ttf) float pixel_height, // height of font in pixels unsigned char *pixels, int pw, int ph, // bitmap to be filled in int first_char, int num_chars, // characters to bake @@ -2559,11 +3799,11 @@ STBTT_DEF int stbtt_BakeFontBitmap(const unsigned char *data, int offset, // fo return bottom_y; } -STBTT_DEF void stbtt_GetBakedQuad(stbtt_bakedchar *chardata, int pw, int ph, int char_index, float *xpos, float *ypos, stbtt_aligned_quad *q, int opengl_fillrule) +STBTT_DEF void stbtt_GetBakedQuad(const stbtt_bakedchar *chardata, int pw, int ph, int char_index, float *xpos, float *ypos, stbtt_aligned_quad *q, int opengl_fillrule) { float d3d_bias = opengl_fillrule ? 0 : -0.5f; float ipw = 1.0f / pw, iph = 1.0f / ph; - stbtt_bakedchar *b = chardata + char_index; + const stbtt_bakedchar *b = chardata + char_index; int round_x = STBTT_ifloor((*xpos + b->xoff) + 0.5f); int round_y = STBTT_ifloor((*ypos + b->yoff) + 0.5f); @@ -2586,11 +3826,6 @@ STBTT_DEF void stbtt_GetBakedQuad(stbtt_bakedchar *chardata, int pw, int ph, int // #ifndef STB_RECT_PACK_VERSION -#ifdef _MSC_VER -#define STBTT__NOTUSED(v) (void)(v) -#else -#define STBTT__NOTUSED(v) (void)sizeof(v) -#endif typedef int stbrp_coord; @@ -2630,7 +3865,7 @@ static void stbrp_init_target(stbrp_context *con, int pw, int ph, stbrp_node *no con->y = 0; con->bottom_y = 0; STBTT__NOTUSED(nodes); - STBTT__NOTUSED(num_nodes); + STBTT__NOTUSED(num_nodes); } static void stbrp_pack_rects(stbrp_context *con, stbrp_rect *rects, int num_rects) @@ -2684,6 +3919,7 @@ STBTT_DEF int stbtt_PackBegin(stbtt_pack_context *spc, unsigned char *pixels, in spc->stride_in_bytes = stride_in_bytes != 0 ? stride_in_bytes : pw; spc->h_oversample = 1; spc->v_oversample = 1; + spc->skip_missing = 0; stbrp_init_target(context, pw-padding, ph-padding, nodes, num_nodes); @@ -2709,6 +3945,11 @@ STBTT_DEF void stbtt_PackSetOversampling(stbtt_pack_context *spc, unsigned int h spc->v_oversample = v_oversample; } +STBTT_DEF void stbtt_PackSetSkipMissingCodepoints(stbtt_pack_context *spc, int skip) +{ + spc->skip_missing = skip; +} + #define STBTT__OVER_MASK (STBTT_MAX_OVERSAMPLE-1) static void stbtt__h_prefilter(unsigned char *pixels, int w, int h, int stride_in_bytes, unsigned int kernel_width) @@ -2848,9 +4089,10 @@ static float stbtt__oversample_shift(int oversample) } // rects array must be big enough to accommodate all characters in the given ranges -STBTT_DEF int stbtt_PackFontRangesGatherRects(stbtt_pack_context *spc, stbtt_fontinfo *info, stbtt_pack_range *ranges, int num_ranges, stbrp_rect *rects) +STBTT_DEF int stbtt_PackFontRangesGatherRects(stbtt_pack_context *spc, const stbtt_fontinfo *info, stbtt_pack_range *ranges, int num_ranges, stbrp_rect *rects) { int i,j,k; + int missing_glyph_added = 0; k=0; for (i=0; i < num_ranges; ++i) { @@ -2862,13 +4104,19 @@ STBTT_DEF int stbtt_PackFontRangesGatherRects(stbtt_pack_context *spc, stbtt_fon int x0,y0,x1,y1; int codepoint = ranges[i].array_of_unicode_codepoints == NULL ? ranges[i].first_unicode_codepoint_in_range + j : ranges[i].array_of_unicode_codepoints[j]; int glyph = stbtt_FindGlyphIndex(info, codepoint); - stbtt_GetGlyphBitmapBoxSubpixel(info,glyph, - scale * spc->h_oversample, - scale * spc->v_oversample, - 0,0, - &x0,&y0,&x1,&y1); - rects[k].w = (stbrp_coord) (x1-x0 + spc->padding + spc->h_oversample-1); - rects[k].h = (stbrp_coord) (y1-y0 + spc->padding + spc->v_oversample-1); + if (glyph == 0 && (spc->skip_missing || missing_glyph_added)) { + rects[k].w = rects[k].h = 0; + } else { + stbtt_GetGlyphBitmapBoxSubpixel(info,glyph, + scale * spc->h_oversample, + scale * spc->v_oversample, + 0,0, + &x0,&y0,&x1,&y1); + rects[k].w = (stbrp_coord) (x1-x0 + spc->padding + spc->h_oversample-1); + rects[k].h = (stbrp_coord) (y1-y0 + spc->padding + spc->v_oversample-1); + if (glyph == 0) + missing_glyph_added = 1; + } ++k; } } @@ -2876,10 +4124,33 @@ STBTT_DEF int stbtt_PackFontRangesGatherRects(stbtt_pack_context *spc, stbtt_fon return k; } +STBTT_DEF void stbtt_MakeGlyphBitmapSubpixelPrefilter(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, float shift_x, float shift_y, int prefilter_x, int prefilter_y, float *sub_x, float *sub_y, int glyph) +{ + stbtt_MakeGlyphBitmapSubpixel(info, + output, + out_w - (prefilter_x - 1), + out_h - (prefilter_y - 1), + out_stride, + scale_x, + scale_y, + shift_x, + shift_y, + glyph); + + if (prefilter_x > 1) + stbtt__h_prefilter(output, out_w, out_h, out_stride, prefilter_x); + + if (prefilter_y > 1) + stbtt__v_prefilter(output, out_w, out_h, out_stride, prefilter_y); + + *sub_x = stbtt__oversample_shift(prefilter_x); + *sub_y = stbtt__oversample_shift(prefilter_y); +} + // rects array must be big enough to accommodate all characters in the given ranges -STBTT_DEF int stbtt_PackFontRangesRenderIntoRects(stbtt_pack_context *spc, stbtt_fontinfo *info, stbtt_pack_range *ranges, int num_ranges, stbrp_rect *rects) +STBTT_DEF int stbtt_PackFontRangesRenderIntoRects(stbtt_pack_context *spc, const stbtt_fontinfo *info, stbtt_pack_range *ranges, int num_ranges, stbrp_rect *rects) { - int i,j,k, return_value = 1; + int i,j,k, missing_glyph = -1, return_value = 1; // save current values int old_h_over = spc->h_oversample; @@ -2898,7 +4169,7 @@ STBTT_DEF int stbtt_PackFontRangesRenderIntoRects(stbtt_pack_context *spc, stbtt sub_y = stbtt__oversample_shift(spc->v_oversample); for (j=0; j < ranges[i].num_chars; ++j) { stbrp_rect *r = &rects[k]; - if (r->was_packed) { + if (r->was_packed && r->w != 0 && r->h != 0) { stbtt_packedchar *bc = &ranges[i].chardata_for_range[j]; int advance, lsb, x0,y0,x1,y1; int codepoint = ranges[i].array_of_unicode_codepoints == NULL ? ranges[i].first_unicode_codepoint_in_range + j : ranges[i].array_of_unicode_codepoints[j]; @@ -2944,6 +4215,13 @@ STBTT_DEF int stbtt_PackFontRangesRenderIntoRects(stbtt_pack_context *spc, stbtt bc->yoff = (float) y0 * recip_v + sub_y; bc->xoff2 = (x0 + r->w) * recip_h + sub_x; bc->yoff2 = (y0 + r->h) * recip_v + sub_y; + + if (glyph == 0) + missing_glyph = j; + } else if (spc->skip_missing) { + return_value = 0; + } else if (r->was_packed && r->w == 0 && r->h == 0 && missing_glyph >= 0) { + ranges[i].chardata_for_range[j] = ranges[i].chardata_for_range[missing_glyph]; } else { return_value = 0; // if any fail, report failure } @@ -2964,7 +4242,7 @@ STBTT_DEF void stbtt_PackFontRangesPackRects(stbtt_pack_context *spc, stbrp_rect stbrp_pack_rects((stbrp_context *) spc->pack_info, rects, num_rects); } -STBTT_DEF int stbtt_PackFontRanges(stbtt_pack_context *spc, unsigned char *fontdata, int font_index, stbtt_pack_range *ranges, int num_ranges) +STBTT_DEF int stbtt_PackFontRanges(stbtt_pack_context *spc, const unsigned char *fontdata, int font_index, stbtt_pack_range *ranges, int num_ranges) { stbtt_fontinfo info; int i,j,n, return_value = 1; @@ -2982,7 +4260,7 @@ STBTT_DEF int stbtt_PackFontRanges(stbtt_pack_context *spc, unsigned char *fontd n = 0; for (i=0; i < num_ranges; ++i) n += ranges[i].num_chars; - + rects = (stbrp_rect *) STBTT_malloc(sizeof(*rects) * n, spc->user_allocator_context); if (rects == NULL) return 0; @@ -2993,14 +4271,14 @@ STBTT_DEF int stbtt_PackFontRanges(stbtt_pack_context *spc, unsigned char *fontd n = stbtt_PackFontRangesGatherRects(spc, &info, ranges, num_ranges, rects); stbtt_PackFontRangesPackRects(spc, rects, n); - + return_value = stbtt_PackFontRangesRenderIntoRects(spc, &info, ranges, num_ranges, rects); STBTT_free(rects, spc->user_allocator_context); return return_value; } -STBTT_DEF int stbtt_PackFontRange(stbtt_pack_context *spc, unsigned char *fontdata, int font_index, float font_size, +STBTT_DEF int stbtt_PackFontRange(stbtt_pack_context *spc, const unsigned char *fontdata, int font_index, float font_size, int first_unicode_codepoint_in_range, int num_chars_in_range, stbtt_packedchar *chardata_for_range) { stbtt_pack_range range; @@ -3012,10 +4290,23 @@ STBTT_DEF int stbtt_PackFontRange(stbtt_pack_context *spc, unsigned char *fontda return stbtt_PackFontRanges(spc, fontdata, font_index, &range, 1); } -STBTT_DEF void stbtt_GetPackedQuad(stbtt_packedchar *chardata, int pw, int ph, int char_index, float *xpos, float *ypos, stbtt_aligned_quad *q, int align_to_integer) +STBTT_DEF void stbtt_GetScaledFontVMetrics(const unsigned char *fontdata, int index, float size, float *ascent, float *descent, float *lineGap) +{ + int i_ascent, i_descent, i_lineGap; + float scale; + stbtt_fontinfo info; + stbtt_InitFont(&info, fontdata, stbtt_GetFontOffsetForIndex(fontdata, index)); + scale = size > 0 ? stbtt_ScaleForPixelHeight(&info, size) : stbtt_ScaleForMappingEmToPixels(&info, -size); + stbtt_GetFontVMetrics(&info, &i_ascent, &i_descent, &i_lineGap); + *ascent = (float) i_ascent * scale; + *descent = (float) i_descent * scale; + *lineGap = (float) i_lineGap * scale; +} + +STBTT_DEF void stbtt_GetPackedQuad(const stbtt_packedchar *chardata, int pw, int ph, int char_index, float *xpos, float *ypos, stbtt_aligned_quad *q, int align_to_integer) { float ipw = 1.0f / pw, iph = 1.0f / ph; - stbtt_packedchar *b = chardata + char_index; + const stbtt_packedchar *b = chardata + char_index; if (align_to_integer) { float x = (float) STBTT_ifloor((*xpos + b->xoff) + 0.5f); @@ -3039,6 +4330,382 @@ STBTT_DEF void stbtt_GetPackedQuad(stbtt_packedchar *chardata, int pw, int ph, i *xpos += b->xadvance; } +////////////////////////////////////////////////////////////////////////////// +// +// sdf computation +// + +#define STBTT_min(a,b) ((a) < (b) ? (a) : (b)) +#define STBTT_max(a,b) ((a) < (b) ? (b) : (a)) + +static int stbtt__ray_intersect_bezier(float orig[2], float ray[2], float q0[2], float q1[2], float q2[2], float hits[2][2]) +{ + float q0perp = q0[1]*ray[0] - q0[0]*ray[1]; + float q1perp = q1[1]*ray[0] - q1[0]*ray[1]; + float q2perp = q2[1]*ray[0] - q2[0]*ray[1]; + float roperp = orig[1]*ray[0] - orig[0]*ray[1]; + + float a = q0perp - 2*q1perp + q2perp; + float b = q1perp - q0perp; + float c = q0perp - roperp; + + float s0 = 0., s1 = 0.; + int num_s = 0; + + if (a != 0.0) { + float discr = b*b - a*c; + if (discr > 0.0) { + float rcpna = -1 / a; + float d = (float) STBTT_sqrt(discr); + s0 = (b+d) * rcpna; + s1 = (b-d) * rcpna; + if (s0 >= 0.0 && s0 <= 1.0) + num_s = 1; + if (d > 0.0 && s1 >= 0.0 && s1 <= 1.0) { + if (num_s == 0) s0 = s1; + ++num_s; + } + } + } else { + // 2*b*s + c = 0 + // s = -c / (2*b) + s0 = c / (-2 * b); + if (s0 >= 0.0 && s0 <= 1.0) + num_s = 1; + } + + if (num_s == 0) + return 0; + else { + float rcp_len2 = 1 / (ray[0]*ray[0] + ray[1]*ray[1]); + float rayn_x = ray[0] * rcp_len2, rayn_y = ray[1] * rcp_len2; + + float q0d = q0[0]*rayn_x + q0[1]*rayn_y; + float q1d = q1[0]*rayn_x + q1[1]*rayn_y; + float q2d = q2[0]*rayn_x + q2[1]*rayn_y; + float rod = orig[0]*rayn_x + orig[1]*rayn_y; + + float q10d = q1d - q0d; + float q20d = q2d - q0d; + float q0rd = q0d - rod; + + hits[0][0] = q0rd + s0*(2.0f - 2.0f*s0)*q10d + s0*s0*q20d; + hits[0][1] = a*s0+b; + + if (num_s > 1) { + hits[1][0] = q0rd + s1*(2.0f - 2.0f*s1)*q10d + s1*s1*q20d; + hits[1][1] = a*s1+b; + return 2; + } else { + return 1; + } + } +} + +static int equal(float *a, float *b) +{ + return (a[0] == b[0] && a[1] == b[1]); +} + +static int stbtt__compute_crossings_x(float x, float y, int nverts, stbtt_vertex *verts) +{ + int i; + float orig[2], ray[2] = { 1, 0 }; + float y_frac; + int winding = 0; + + orig[0] = x; + orig[1] = y; + + // make sure y never passes through a vertex of the shape + y_frac = (float) STBTT_fmod(y, 1.0f); + if (y_frac < 0.01f) + y += 0.01f; + else if (y_frac > 0.99f) + y -= 0.01f; + orig[1] = y; + + // test a ray from (-infinity,y) to (x,y) + for (i=0; i < nverts; ++i) { + if (verts[i].type == STBTT_vline) { + int x0 = (int) verts[i-1].x, y0 = (int) verts[i-1].y; + int x1 = (int) verts[i ].x, y1 = (int) verts[i ].y; + if (y > STBTT_min(y0,y1) && y < STBTT_max(y0,y1) && x > STBTT_min(x0,x1)) { + float x_inter = (y - y0) / (y1 - y0) * (x1-x0) + x0; + if (x_inter < x) + winding += (y0 < y1) ? 1 : -1; + } + } + if (verts[i].type == STBTT_vcurve) { + int x0 = (int) verts[i-1].x , y0 = (int) verts[i-1].y ; + int x1 = (int) verts[i ].cx, y1 = (int) verts[i ].cy; + int x2 = (int) verts[i ].x , y2 = (int) verts[i ].y ; + int ax = STBTT_min(x0,STBTT_min(x1,x2)), ay = STBTT_min(y0,STBTT_min(y1,y2)); + int by = STBTT_max(y0,STBTT_max(y1,y2)); + if (y > ay && y < by && x > ax) { + float q0[2],q1[2],q2[2]; + float hits[2][2]; + q0[0] = (float)x0; + q0[1] = (float)y0; + q1[0] = (float)x1; + q1[1] = (float)y1; + q2[0] = (float)x2; + q2[1] = (float)y2; + if (equal(q0,q1) || equal(q1,q2)) { + x0 = (int)verts[i-1].x; + y0 = (int)verts[i-1].y; + x1 = (int)verts[i ].x; + y1 = (int)verts[i ].y; + if (y > STBTT_min(y0,y1) && y < STBTT_max(y0,y1) && x > STBTT_min(x0,x1)) { + float x_inter = (y - y0) / (y1 - y0) * (x1-x0) + x0; + if (x_inter < x) + winding += (y0 < y1) ? 1 : -1; + } + } else { + int num_hits = stbtt__ray_intersect_bezier(orig, ray, q0, q1, q2, hits); + if (num_hits >= 1) + if (hits[0][0] < 0) + winding += (hits[0][1] < 0 ? -1 : 1); + if (num_hits >= 2) + if (hits[1][0] < 0) + winding += (hits[1][1] < 0 ? -1 : 1); + } + } + } + } + return winding; +} + +static float stbtt__cuberoot( float x ) +{ + if (x<0) + return -(float) STBTT_pow(-x,1.0f/3.0f); + else + return (float) STBTT_pow( x,1.0f/3.0f); +} + +// x^3 + c*x^2 + b*x + a = 0 +static int stbtt__solve_cubic(float a, float b, float c, float* r) +{ + float s = -a / 3; + float p = b - a*a / 3; + float q = a * (2*a*a - 9*b) / 27 + c; + float p3 = p*p*p; + float d = q*q + 4*p3 / 27; + if (d >= 0) { + float z = (float) STBTT_sqrt(d); + float u = (-q + z) / 2; + float v = (-q - z) / 2; + u = stbtt__cuberoot(u); + v = stbtt__cuberoot(v); + r[0] = s + u + v; + return 1; + } else { + float u = (float) STBTT_sqrt(-p/3); + float v = (float) STBTT_acos(-STBTT_sqrt(-27/p3) * q / 2) / 3; // p3 must be negative, since d is negative + float m = (float) STBTT_cos(v); + float n = (float) STBTT_cos(v-3.141592/2)*1.732050808f; + r[0] = s + u * 2 * m; + r[1] = s - u * (m + n); + r[2] = s - u * (m - n); + + //STBTT_assert( STBTT_fabs(((r[0]+a)*r[0]+b)*r[0]+c) < 0.05f); // these asserts may not be safe at all scales, though they're in bezier t parameter units so maybe? + //STBTT_assert( STBTT_fabs(((r[1]+a)*r[1]+b)*r[1]+c) < 0.05f); + //STBTT_assert( STBTT_fabs(((r[2]+a)*r[2]+b)*r[2]+c) < 0.05f); + return 3; + } +} + +STBTT_DEF unsigned char * stbtt_GetGlyphSDF(const stbtt_fontinfo *info, float scale, int glyph, int padding, unsigned char onedge_value, float pixel_dist_scale, int *width, int *height, int *xoff, int *yoff) +{ + float scale_x = scale, scale_y = scale; + int ix0,iy0,ix1,iy1; + int w,h; + unsigned char *data; + + if (scale == 0) return NULL; + + stbtt_GetGlyphBitmapBoxSubpixel(info, glyph, scale, scale, 0.0f,0.0f, &ix0,&iy0,&ix1,&iy1); + + // if empty, return NULL + if (ix0 == ix1 || iy0 == iy1) + return NULL; + + ix0 -= padding; + iy0 -= padding; + ix1 += padding; + iy1 += padding; + + w = (ix1 - ix0); + h = (iy1 - iy0); + + if (width ) *width = w; + if (height) *height = h; + if (xoff ) *xoff = ix0; + if (yoff ) *yoff = iy0; + + // invert for y-downwards bitmaps + scale_y = -scale_y; + + { + int x,y,i,j; + float *precompute; + stbtt_vertex *verts; + int num_verts = stbtt_GetGlyphShape(info, glyph, &verts); + data = (unsigned char *) STBTT_malloc(w * h, info->userdata); + precompute = (float *) STBTT_malloc(num_verts * sizeof(float), info->userdata); + + for (i=0,j=num_verts-1; i < num_verts; j=i++) { + if (verts[i].type == STBTT_vline) { + float x0 = verts[i].x*scale_x, y0 = verts[i].y*scale_y; + float x1 = verts[j].x*scale_x, y1 = verts[j].y*scale_y; + float dist = (float) STBTT_sqrt((x1-x0)*(x1-x0) + (y1-y0)*(y1-y0)); + precompute[i] = (dist == 0) ? 0.0f : 1.0f / dist; + } else if (verts[i].type == STBTT_vcurve) { + float x2 = verts[j].x *scale_x, y2 = verts[j].y *scale_y; + float x1 = verts[i].cx*scale_x, y1 = verts[i].cy*scale_y; + float x0 = verts[i].x *scale_x, y0 = verts[i].y *scale_y; + float bx = x0 - 2*x1 + x2, by = y0 - 2*y1 + y2; + float len2 = bx*bx + by*by; + if (len2 != 0.0f) + precompute[i] = 1.0f / (bx*bx + by*by); + else + precompute[i] = 0.0f; + } else + precompute[i] = 0.0f; + } + + for (y=iy0; y < iy1; ++y) { + for (x=ix0; x < ix1; ++x) { + float val; + float min_dist = 999999.0f; + float sx = (float) x + 0.5f; + float sy = (float) y + 0.5f; + float x_gspace = (sx / scale_x); + float y_gspace = (sy / scale_y); + + int winding = stbtt__compute_crossings_x(x_gspace, y_gspace, num_verts, verts); // @OPTIMIZE: this could just be a rasterization, but needs to be line vs. non-tesselated curves so a new path + + for (i=0; i < num_verts; ++i) { + float x0 = verts[i].x*scale_x, y0 = verts[i].y*scale_y; + + // check against every point here rather than inside line/curve primitives -- @TODO: wrong if multiple 'moves' in a row produce a garbage point, and given culling, probably more efficient to do within line/curve + float dist2 = (x0-sx)*(x0-sx) + (y0-sy)*(y0-sy); + if (dist2 < min_dist*min_dist) + min_dist = (float) STBTT_sqrt(dist2); + + if (verts[i].type == STBTT_vline) { + float x1 = verts[i-1].x*scale_x, y1 = verts[i-1].y*scale_y; + + // coarse culling against bbox + //if (sx > STBTT_min(x0,x1)-min_dist && sx < STBTT_max(x0,x1)+min_dist && + // sy > STBTT_min(y0,y1)-min_dist && sy < STBTT_max(y0,y1)+min_dist) + float dist = (float) STBTT_fabs((x1-x0)*(y0-sy) - (y1-y0)*(x0-sx)) * precompute[i]; + STBTT_assert(i != 0); + if (dist < min_dist) { + // check position along line + // x' = x0 + t*(x1-x0), y' = y0 + t*(y1-y0) + // minimize (x'-sx)*(x'-sx)+(y'-sy)*(y'-sy) + float dx = x1-x0, dy = y1-y0; + float px = x0-sx, py = y0-sy; + // minimize (px+t*dx)^2 + (py+t*dy)^2 = px*px + 2*px*dx*t + t^2*dx*dx + py*py + 2*py*dy*t + t^2*dy*dy + // derivative: 2*px*dx + 2*py*dy + (2*dx*dx+2*dy*dy)*t, set to 0 and solve + float t = -(px*dx + py*dy) / (dx*dx + dy*dy); + if (t >= 0.0f && t <= 1.0f) + min_dist = dist; + } + } else if (verts[i].type == STBTT_vcurve) { + float x2 = verts[i-1].x *scale_x, y2 = verts[i-1].y *scale_y; + float x1 = verts[i ].cx*scale_x, y1 = verts[i ].cy*scale_y; + float box_x0 = STBTT_min(STBTT_min(x0,x1),x2); + float box_y0 = STBTT_min(STBTT_min(y0,y1),y2); + float box_x1 = STBTT_max(STBTT_max(x0,x1),x2); + float box_y1 = STBTT_max(STBTT_max(y0,y1),y2); + // coarse culling against bbox to avoid computing cubic unnecessarily + if (sx > box_x0-min_dist && sx < box_x1+min_dist && sy > box_y0-min_dist && sy < box_y1+min_dist) { + int num=0; + float ax = x1-x0, ay = y1-y0; + float bx = x0 - 2*x1 + x2, by = y0 - 2*y1 + y2; + float mx = x0 - sx, my = y0 - sy; + float res[3],px,py,t,it; + float a_inv = precompute[i]; + if (a_inv == 0.0) { // if a_inv is 0, it's 2nd degree so use quadratic formula + float a = 3*(ax*bx + ay*by); + float b = 2*(ax*ax + ay*ay) + (mx*bx+my*by); + float c = mx*ax+my*ay; + if (a == 0.0) { // if a is 0, it's linear + if (b != 0.0) { + res[num++] = -c/b; + } + } else { + float discriminant = b*b - 4*a*c; + if (discriminant < 0) + num = 0; + else { + float root = (float) STBTT_sqrt(discriminant); + res[0] = (-b - root)/(2*a); + res[1] = (-b + root)/(2*a); + num = 2; // don't bother distinguishing 1-solution case, as code below will still work + } + } + } else { + float b = 3*(ax*bx + ay*by) * a_inv; // could precompute this as it doesn't depend on sample point + float c = (2*(ax*ax + ay*ay) + (mx*bx+my*by)) * a_inv; + float d = (mx*ax+my*ay) * a_inv; + num = stbtt__solve_cubic(b, c, d, res); + } + if (num >= 1 && res[0] >= 0.0f && res[0] <= 1.0f) { + t = res[0], it = 1.0f - t; + px = it*it*x0 + 2*t*it*x1 + t*t*x2; + py = it*it*y0 + 2*t*it*y1 + t*t*y2; + dist2 = (px-sx)*(px-sx) + (py-sy)*(py-sy); + if (dist2 < min_dist * min_dist) + min_dist = (float) STBTT_sqrt(dist2); + } + if (num >= 2 && res[1] >= 0.0f && res[1] <= 1.0f) { + t = res[1], it = 1.0f - t; + px = it*it*x0 + 2*t*it*x1 + t*t*x2; + py = it*it*y0 + 2*t*it*y1 + t*t*y2; + dist2 = (px-sx)*(px-sx) + (py-sy)*(py-sy); + if (dist2 < min_dist * min_dist) + min_dist = (float) STBTT_sqrt(dist2); + } + if (num >= 3 && res[2] >= 0.0f && res[2] <= 1.0f) { + t = res[2], it = 1.0f - t; + px = it*it*x0 + 2*t*it*x1 + t*t*x2; + py = it*it*y0 + 2*t*it*y1 + t*t*y2; + dist2 = (px-sx)*(px-sx) + (py-sy)*(py-sy); + if (dist2 < min_dist * min_dist) + min_dist = (float) STBTT_sqrt(dist2); + } + } + } + } + if (winding == 0) + min_dist = -min_dist; // if outside the shape, value is negative + val = onedge_value + pixel_dist_scale * min_dist; + if (val < 0) + val = 0; + else if (val > 255) + val = 255; + data[(y-iy0)*w+(x-ix0)] = (unsigned char) val; + } + } + STBTT_free(precompute, info->userdata); + STBTT_free(verts, info->userdata); + } + return data; +} + +STBTT_DEF unsigned char * stbtt_GetCodepointSDF(const stbtt_fontinfo *info, float scale, int codepoint, int padding, unsigned char onedge_value, float pixel_dist_scale, int *width, int *height, int *xoff, int *yoff) +{ + return stbtt_GetGlyphSDF(info, scale, stbtt_FindGlyphIndex(info, codepoint), padding, onedge_value, pixel_dist_scale, width, height, xoff, yoff); +} + +STBTT_DEF void stbtt_FreeSDF(unsigned char *bitmap, void *userdata) +{ + STBTT_free(bitmap, userdata); +} ////////////////////////////////////////////////////////////////////////////// // @@ -3046,7 +4713,7 @@ STBTT_DEF void stbtt_GetPackedQuad(stbtt_packedchar *chardata, int pw, int ph, i // // check if a utf8 string contains a prefix which is the utf16 string; if so return length of matching utf8 string -static stbtt_int32 stbtt__CompareUTF8toUTF16_bigendian_prefix(const stbtt_uint8 *s1, stbtt_int32 len1, const stbtt_uint8 *s2, stbtt_int32 len2) +static stbtt_int32 stbtt__CompareUTF8toUTF16_bigendian_prefix(stbtt_uint8 *s1, stbtt_int32 len1, stbtt_uint8 *s2, stbtt_int32 len2) { stbtt_int32 i=0; @@ -3085,9 +4752,9 @@ static stbtt_int32 stbtt__CompareUTF8toUTF16_bigendian_prefix(const stbtt_uint8 return i; } -STBTT_DEF int stbtt_CompareUTF8toUTF16_bigendian(const char *s1, int len1, const char *s2, int len2) +static int stbtt_CompareUTF8toUTF16_bigendian_internal(char *s1, int len1, char *s2, int len2) { - return len1 == stbtt__CompareUTF8toUTF16_bigendian_prefix((const stbtt_uint8*) s1, len1, (const stbtt_uint8*) s2, len2); + return len1 == stbtt__CompareUTF8toUTF16_bigendian_prefix((stbtt_uint8*) s1, len1, (stbtt_uint8*) s2, len2); } // returns results in whatever encoding you request... but note that 2-byte encodings @@ -3143,7 +4810,7 @@ static int stbtt__matchpair(stbtt_uint8 *fc, stbtt_uint32 nm, stbtt_uint8 *name, return 1; } else if (matchlen < nlen && name[matchlen] == ' ') { ++matchlen; - if (stbtt_CompareUTF8toUTF16_bigendian((char*) (name+matchlen), nlen-matchlen, (char*)(fc+stringOffset+off),slen)) + if (stbtt_CompareUTF8toUTF16_bigendian_internal((char*) (name+matchlen), nlen-matchlen, (char*)(fc+stringOffset+off),slen)) return 1; } } else { @@ -3189,7 +4856,7 @@ static int stbtt__matches(stbtt_uint8 *fc, stbtt_uint32 offset, stbtt_uint8 *nam return 0; } -STBTT_DEF int stbtt_FindMatchingFont(const unsigned char *font_collection, const char *name_utf8, stbtt_int32 flags) +static int stbtt_FindMatchingFont_internal(unsigned char *font_collection, char *name_utf8, stbtt_int32 flags) { stbtt_int32 i; for (i=0;;++i) { @@ -3200,11 +4867,64 @@ STBTT_DEF int stbtt_FindMatchingFont(const unsigned char *font_collection, const } } +#if defined(__GNUC__) || defined(__clang__) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-qual" +#endif + +STBTT_DEF int stbtt_BakeFontBitmap(const unsigned char *data, int offset, + float pixel_height, unsigned char *pixels, int pw, int ph, + int first_char, int num_chars, stbtt_bakedchar *chardata) +{ + return stbtt_BakeFontBitmap_internal((unsigned char *) data, offset, pixel_height, pixels, pw, ph, first_char, num_chars, chardata); +} + +STBTT_DEF int stbtt_GetFontOffsetForIndex(const unsigned char *data, int index) +{ + return stbtt_GetFontOffsetForIndex_internal((unsigned char *) data, index); +} + +STBTT_DEF int stbtt_GetNumberOfFonts(const unsigned char *data) +{ + return stbtt_GetNumberOfFonts_internal((unsigned char *) data); +} + +STBTT_DEF int stbtt_InitFont(stbtt_fontinfo *info, const unsigned char *data, int offset) +{ + return stbtt_InitFont_internal(info, (unsigned char *) data, offset); +} + +STBTT_DEF int stbtt_FindMatchingFont(const unsigned char *fontdata, const char *name, int flags) +{ + return stbtt_FindMatchingFont_internal((unsigned char *) fontdata, (char *) name, flags); +} + +STBTT_DEF int stbtt_CompareUTF8toUTF16_bigendian(const char *s1, int len1, const char *s2, int len2) +{ + return stbtt_CompareUTF8toUTF16_bigendian_internal((char *) s1, len1, (char *) s2, len2); +} + +#if defined(__GNUC__) || defined(__clang__) +#pragma GCC diagnostic pop +#endif + #endif // STB_TRUETYPE_IMPLEMENTATION // FULL VERSION HISTORY // +// 1.19 (2018-02-11) OpenType GPOS kerning (horizontal only), STBTT_fmod +// 1.18 (2018-01-29) add missing function +// 1.17 (2017-07-23) make more arguments const; doc fix +// 1.16 (2017-07-12) SDF support +// 1.15 (2017-03-03) make more arguments const +// 1.14 (2017-01-16) num-fonts-in-TTC function +// 1.13 (2017-01-02) support OpenType fonts, certain Apple fonts +// 1.12 (2016-10-25) suppress warnings about casting away const with -Wcast-qual +// 1.11 (2016-04-02) fix unused-variable warning +// 1.10 (2016-04-02) allow user-defined fabs() replacement +// fix memory leak if fontsize=0.0 +// fix warning from duplicate typedef // 1.09 (2016-01-16) warning fix; avoid crash on outofmem; use alloc userdata for PackFontRanges // 1.08 (2015-09-13) document stbtt_Rasterize(); fixes for vertical & horizontal edges // 1.07 (2015-08-01) allow PackFontRanges to accept arrays of sparse codepoints; @@ -3247,3 +4967,45 @@ STBTT_DEF int stbtt_FindMatchingFont(const unsigned char *font_collection, const // 0.2 (2009-03-11) Fix unsigned/signed char warnings // 0.1 (2009-03-09) First public release // + +/* +------------------------------------------------------------------------------ +This software is available under 2 licenses -- choose whichever you prefer. +------------------------------------------------------------------------------ +ALTERNATIVE A - MIT License +Copyright (c) 2017 Sean Barrett +Permission is hereby granted, free of charge, to any person obtaining a copy of +this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to +use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies +of the Software, and to permit persons to whom the Software is furnished to do +so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +------------------------------------------------------------------------------ +ALTERNATIVE B - Public Domain (www.unlicense.org) +This is free and unencumbered software released into the public domain. +Anyone is free to copy, modify, publish, use, compile, sell, or distribute this +software, either in source code form or as a compiled binary, for any purpose, +commercial or non-commercial, and by any means. +In jurisdictions that recognize copyright laws, the author or authors of this +software dedicate any and all copyright interest in the software to the public +domain. We make this dedication for the benefit of the public at large and to +the detriment of our heirs and successors. We intend this dedication to be an +overt act of relinquishment in perpetuity of all present and future rights to +this software under copyright law. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN +ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION +WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +------------------------------------------------------------------------------ +*/ diff --git a/phonelibs/snpe/aarch64 b/phonelibs/snpe/aarch64 new file mode 120000 index 00000000000000..baf4e9cb6ed759 --- /dev/null +++ b/phonelibs/snpe/aarch64 @@ -0,0 +1 @@ +aarch64-android-clang6.0 \ No newline at end of file diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libatomic.so.1 b/phonelibs/snpe/aarch64-linux-gcc4.9/libatomic.so.1 new file mode 100644 index 00000000000000..809fd4ef6ffe79 Binary files /dev/null and b/phonelibs/snpe/aarch64-linux-gcc4.9/libatomic.so.1 differ diff --git a/phonelibs/snpe/include/DiagLog/Options.hpp b/phonelibs/snpe/include/DiagLog/Options.hpp index ba0c82119bfebb..ea247e953f42ac 100644 --- a/phonelibs/snpe/include/DiagLog/Options.hpp +++ b/phonelibs/snpe/include/DiagLog/Options.hpp @@ -32,7 +32,8 @@ class ZDL_LOGGING_EXPORT Options DiagLogMask(""), LogFileDirectory("diaglogs"), LogFileName("DiagLog"), - LogFileRotateCount(20) + LogFileRotateCount(20), + LogFileReplace(true) { // Solves the empty string problem with multiple std libs DiagLogMask.reserve(1); @@ -65,6 +66,13 @@ class ZDL_LOGGING_EXPORT Options /// any existing log file that may exist. /// Default value is 20 uint32_t LogFileRotateCount; + + /// @brief + /// + /// If the log file already exists, control whether it will be replaced + /// (existing contents truncated), or appended. + /// Default value is true + bool LogFileReplace; }; /** @} */ /* end_addtogroup c_plus_plus_apis C++ */ diff --git a/phonelibs/snpe/include/DlContainer/IDlContainer.hpp b/phonelibs/snpe/include/DlContainer/IDlContainer.hpp index c4b465fd04bbe6..f2022ca9f50625 100644 --- a/phonelibs/snpe/include/DlContainer/IDlContainer.hpp +++ b/phonelibs/snpe/include/DlContainer/IDlContainer.hpp @@ -1,6 +1,6 @@ //============================================================================= // -// Copyright (c) 2015 Qualcomm Technologies, Inc. +// Copyright (c) 2015,2019 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -43,7 +43,15 @@ struct ZDL_EXPORT DlcRecord : name(std::move(other.name)) , data(std::move(other.data)) {} - + DlcRecord(const std::string& new_name) + : name(new_name) + , data() + { + if(name.empty()) + { + name.reserve(1); + } + } DlcRecord(const DlcRecord&) = delete; }; @@ -63,9 +71,9 @@ class ZDL_EXPORT IDlContainer public: /** * Initializes a container from a container archive file. - * + * * @param[in] filename Container archive file path. - * + * * @return A pointer to the initialized container */ ZDL_EXPORT static std::unique_ptr @@ -83,10 +91,10 @@ class ZDL_EXPORT IDlContainer /** * Initializes a container from a byte buffer. - * - * @param[in] buffer Byte buffer holding the contents of an archive + * + * @param[in] buffer Byte buffer holding the contents of an archive * file. - * + * * @return A pointer to the initialized container */ ZDL_EXPORT static std::unique_ptr @@ -94,12 +102,12 @@ class ZDL_EXPORT IDlContainer /** * Initializes a container from a byte buffer. - * - * @param[in] buffer Byte buffer holding the contents of an archive + * + * @param[in] buffer Byte buffer holding the contents of an archive * file. * * @param[in] size Size of the byte buffer. - * + * * @return A pointer to the initialized container */ ZDL_EXPORT static std::unique_ptr @@ -110,8 +118,8 @@ class ZDL_EXPORT IDlContainer /** * Get the record catalog for a container. - * - * @param[out] catalog Buffer that will hold the record names on + * + * @param[out] catalog Buffer that will hold the record names on * return. */ virtual void getCatalog(std::set &catalog) const = 0; @@ -126,7 +134,7 @@ class ZDL_EXPORT IDlContainer /** * Get a record from a container by name. - * + * * @param[in] name Name of the record to fetch. * @param[out] record The passed in record will be populated with the * record data on return. Note that the caller @@ -146,6 +154,34 @@ class ZDL_EXPORT IDlContainer */ virtual void getRecord(const zdl::DlSystem::String &name, DlcRecord &record) const = 0; + /** + * Save the container to an archive on disk. This function will save the + * container if the filename is different from the file that it was opened + * from, or if at least one record was modified since the container was + * opened. + * + * It will truncate any existing file at the target path. + * + * @param filename Container archive file path. + * + * @return indication of success/failure + */ + virtual bool save(const std::string &filename) = 0; + + /** + * Save the container to an archive on disk. This function will save the + * container if the filename is different from the file that it was opened + * from, or if at least one record was modified since the container was + * opened. + * + * It will truncate any existing file at the target path. + * + * @param filename Container archive file path. + * + * @return indication of success/failure + */ + virtual bool save (const zdl::DlSystem::String &filename) = 0; + virtual ~IDlContainer() {} }; diff --git a/phonelibs/snpe/include/DlSystem/DlEnums.hpp b/phonelibs/snpe/include/DlSystem/DlEnums.hpp index fde252ca7f6742..5b8b6b35fcc621 100644 --- a/phonelibs/snpe/include/DlSystem/DlEnums.hpp +++ b/phonelibs/snpe/include/DlSystem/DlEnums.hpp @@ -1,6 +1,6 @@ //============================================================================== // -// Copyright (c) 2014-2018 Qualcomm Technologies, Inc. +// Copyright (c) 2014-2019 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -43,6 +43,12 @@ enum class Runtime_t /// Math: float 16bit GPU_FLOAT16 = 3, + /// Run the processing on Snapdragon AIX+HVX. + /// Data: 8bit fixed point Tensorflow style format + /// Math: 8bit fixed point Tensorflow style format + AIP_FIXED8_TF = 5, + AIP_FIXED_TF = AIP_FIXED8_TF, + /// Default legacy enum to retain backward compatibility. /// CPU = CPU_FLOAT32 CPU = CPU_FLOAT32, @@ -53,7 +59,23 @@ enum class Runtime_t /// Default legacy enum to retain backward compatibility. /// DSP = DSP_FIXED8_TF - DSP = DSP_FIXED8_TF + DSP = DSP_FIXED8_TF, + + /// Special value indicating the property is unset. + UNSET = -1 +}; + +/** + * Enumeration of runtime available check options. + */ +enum class RuntimeCheckOption_t +{ + /// Perform standard runtime available check + DEFAULT = 0, + /// Perform standard runtime available check + NORMAL_CHECK = 0, + /// Perform basic runtime available check, may be runtime specific + BASIC_CHECK = 1, }; /** @@ -68,17 +90,47 @@ enum class PerformanceProfile_t BALANCED = 0, /// Run in high performance mode - HIGH_PERFORMANCE, + HIGH_PERFORMANCE = 1, /// Run in a power sensitive mode, at the expense of performance. - POWER_SAVER, + POWER_SAVER = 2, /// Use system settings. SNPE makes no calls to any performance related APIs. - SYSTEM_SETTINGS, + SYSTEM_SETTINGS = 3, /// Run in sustained high performance mode - SUSTAINED_HIGH_PERFORMANCE + SUSTAINED_HIGH_PERFORMANCE = 4, + /// Run in burst mode + BURST = 5, + + /// Run in lower clock than POWER_SAVER, at the expense of performance. + LOW_POWER_SAVER = 6, + + /// Run in higher clock and provides better performance than POWER_SAVER. + HIGH_POWER_SAVER = 7, + + /// Run in lower balanced mode + LOW_BALANCED = 8, +}; + +/** + * Enumeration of various profilngLevels that can be requested. + */ +enum class ProfilingLevel_t +{ + /// No profiling. + /// Collects no runtime stats in the DiagLog + OFF = 0, + + /// Basic profiling + /// Collects some runtime stats in the DiagLog + BASIC = 1, + + /// Detailed profiling + /// Collects more runtime stats in the DiagLog + /// Performance may be impacted + DETAILED = 2 }; /** @@ -90,10 +142,10 @@ enum class ExecutionPriorityHint_t NORMAL = 0, /// Higher than normal priority - HIGH, + HIGH = 1, /// Lower priority - LOW + LOW = 2 }; @@ -110,34 +162,34 @@ enum class ImageEncoding_t /// The RGB format consists of 3 bytes per pixel: one byte for /// Red, one for Green, and one for Blue. The byte ordering is /// endian independent and is always in RGB byte order. - RGB, + RGB = 1, /// The ARGB32 format consists of 4 bytes per pixel: one byte for /// Red, one for Green, one for Blue, and one for the alpha channel. /// The alpha channel is ignored. The byte ordering depends on the /// underlying CPU. For little endian CPUs, the byte order is BGRA. /// For big endian CPUs, the byte order is ARGB. - ARGB32, + ARGB32 = 2, /// The RGBA format consists of 4 bytes per pixel: one byte for /// Red, one for Green, one for Blue, and one for the alpha channel. /// The alpha channel is ignored. The byte ordering is endian independent /// and is always in RGBA byte order. - RGBA, + RGBA = 3, /// The GRAYSCALE format is for 8-bit grayscale. - GRAYSCALE, + GRAYSCALE = 4, /// NV21 is the Android version of YUV. The Chrominance is down /// sampled and has a subsampling ratio of 4:2:0. Note that this /// image format has 3 channels, but the U and V channels /// are subsampled. For every four Y pixels there is one U and one V pixel. @newpage - NV21, + NV21 = 5, /// The BGR format consists of 3 bytes per pixel: one byte for /// Red, one for Green and one for Blue. The byte ordering is /// endian independent and is always BGR byte order. - BGR + BGR = 6 }; }} // namespaces end diff --git a/phonelibs/snpe/include/DlSystem/DlError.hpp b/phonelibs/snpe/include/DlSystem/DlError.hpp index 383403b5418a26..a8b0b4ea7557d0 100644 --- a/phonelibs/snpe/include/DlSystem/DlError.hpp +++ b/phonelibs/snpe/include/DlSystem/DlError.hpp @@ -1,6 +1,6 @@ //============================================================================== // -// Copyright (c) 2016-18 Qualcomm Technologies, Inc. +// Copyright (c) 2016-2019 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -34,6 +34,7 @@ enum class ZDL_EXPORT ErrorCode : uint32_t { SNPE_CONFIG_WRONG_INPUT_NAME = 105, SNPE_CONFIG_INCORRECT_INPUT_DIMENSIONS = 106, SNPE_CONFIG_DIMENSIONS_MODIFICATION_NOT_SUPPORTED = 107, + SNPE_CONFIG_BOTH_OUTPUT_LAYER_TENSOR_NAMES_SET = 108, SNPE_CONFIG_NNCONFIG_ONLY_TENSOR_SUPPORTED = 120, SNPE_CONFIG_NNCONFIG_ONLY_USER_BUFFER_SUPPORTED = 121, @@ -56,8 +57,11 @@ enum class ZDL_EXPORT ErrorCode : uint32_t { SNPE_DLSYSTEM_BUFFER_MANAGER_MISSING = 214, SNPE_DLSYSTEM_RUNTIME_BUFFER_SOURCE_UNSUPPORTED = 215, SNPE_DLSYSTEM_BUFFER_CAST_FAILED = 216, + SNPE_DLSYSTEM_WRONG_TRANSITION_TYPE = 217, + SNPE_DLSYSTEM_LAYER_ALREADY_REGISTERED = 218, SNPE_DLSYSTEM_BUFFERENCODING_UNKNOWN = 240, + SNPE_DLSYSTEM_BUFFER_INVALID_PARAM = 241, // DlContainer errors SNPE_DLCONTAINER_MODEL_PARSING_FAILED = 300, @@ -95,6 +99,7 @@ enum class ZDL_EXPORT ErrorCode : uint32_t { SNPE_CPU_LAYER_PARAM_COMBINATION_INVALID = 603, SNPE_CPU_BUFFER_NOT_FOUND = 604, SNPE_CPU_NETWORK_NOT_SUPPORTED = 605, + SNPE_CPU_UDO_OPERATION_FAILED = 606, // CPU fixed-point runtime errors SNPE_CPU_FXP_LAYER_NOT_SUPPORTED = 700, @@ -119,7 +124,8 @@ enum class ZDL_EXPORT ErrorCode : uint32_t { SNPE_GPU_LAYER_PROXY_ERROR = 814, SNPE_GPU_BUFFER_IN_USE = 815, SNPE_GPU_BUFFER_MODIFICATION_ERROR = 816, - + SNPE_GPU_DATA_ARRANGEMENT_INVALID = 817, + SNPE_GPU_UDO_OPERATION_FAILED = 818, // DSP runtime errors SNPE_DSP_LAYER_NOT_SUPPORTED = 900, SNPE_DSP_LAYER_PARAM_NOT_SUPPORTED = 901, @@ -132,6 +138,7 @@ enum class ZDL_EXPORT ErrorCode : uint32_t { SNPE_DSP_RUNTIME_COMMUNICATION_ERROR = 908, SNPE_DSP_RUNTIME_INVALID_PARAM_ERROR = 909, SNPE_DSP_RUNTIME_SYSTEM_ERROR = 910, + SNPE_DSP_RUNTIME_CRASHED_ERROR = 911, // Model validataion errors SNPE_MODEL_VALIDATION_LAYER_NOT_SUPPORTED = 1000, @@ -143,6 +150,8 @@ enum class ZDL_EXPORT ErrorCode : uint32_t { SNPE_MODEL_VALIDATION_INVALID_CONSTRAINT = 1006, SNPE_MODEL_VALIDATION_MISSING_BUFFER = 1007, SNPE_MODEL_VALIDATION_BUFFER_REUSE_NOT_SUPPORTED = 1008, + SNPE_MODEL_VALIDATION_LAYER_COULD_NOT_BE_ASSIGNED = 1009, + SNPE_MODEL_VALIDATION_UDO_LAYER_FAILED = 1010, // UDL errors SNPE_UDL_LAYER_EMPTY_UDL_NETWORK = 1100, @@ -151,6 +160,9 @@ enum class ZDL_EXPORT ErrorCode : uint32_t { SNPE_UDL_LAYER_SETUP_FAILED = 1103, SNPE_UDL_EXECUTE_FAILED = 1104, SNPE_UDL_BUNDLE_INVALID = 1105, + SNPE_UDO_REGISTRATION_FAILED = 1106, + SNPE_UDO_GET_PACKAGE_FAILED = 1107, + SNPE_UDO_GET_IMPLEMENTATION_FAILED = 1108, // Dependent library errors SNPE_STD_LIBRARY_ERROR = 1200, @@ -161,6 +173,26 @@ enum class ZDL_EXPORT ErrorCode : uint32_t { // Storage Errors SNPE_STORAGE_INVALID_KERNEL_REPO = 1300, + // AIP runtime errors + SNPE_AIP_LAYER_NOT_SUPPORTED = 1400, + SNPE_AIP_LAYER_PARAM_NOT_SUPPORTED = 1401, + SNPE_AIP_LAYER_PARAM_INVALID = 1402, + SNPE_AIP_LAYER_PARAM_COMBINATION_INVALID = 1403, + SNPE_AIP_STUB_NOT_PRESENT = 1404, + SNPE_AIP_LAYER_NAME_TRUNCATED = 1405, + SNPE_AIP_LAYER_INPUT_BUFFER_NAME_TRUNCATED = 1406, + SNPE_AIP_LAYER_OUTPUT_BUFFER_NAME_TRUNCATED = 1407, + SNPE_AIP_RUNTIME_COMMUNICATION_ERROR = 1408, + SNPE_AIP_RUNTIME_INVALID_PARAM_ERROR = 1409, + SNPE_AIP_RUNTIME_SYSTEM_ERROR = 1410, + SNPE_AIP_RUNTIME_TENSOR_MISSING = 1411, + SNPE_AIP_RUNTIME_TENSOR_SHAPE_MISMATCH = 1412, + SNPE_AIP_RUNTIME_BAD_AIX_RECORD = 1413, + + // DlCaching errors + SNPE_DLCACHING_INVALID_METADATA = 1500, + SNPE_DLCACHING_INVALID_INITBLOB = 1501 + }; @@ -187,6 +219,11 @@ ZDL_EXPORT ErrorCode getLastErrorCode(); */ ZDL_EXPORT const char* getLastErrorString(); +/** + * Returns the info string of the last error encountered. + */ +ZDL_EXPORT const char* getLastInfoString(); + /** * Returns the uint32_t representation of the error code enum. * diff --git a/phonelibs/snpe/include/DlSystem/DlOptional.hpp b/phonelibs/snpe/include/DlSystem/DlOptional.hpp index 2c41b6b6894ee4..35416b4267844a 100644 --- a/phonelibs/snpe/include/DlSystem/DlOptional.hpp +++ b/phonelibs/snpe/include/DlSystem/DlOptional.hpp @@ -35,10 +35,10 @@ template class ZDL_EXPORT Optional final { public: enum class LIFECYCLE { - NONE, - REFERENCE_OWNED, - POINTER_OWNED, - POINTER_NOT_OWNED + NONE = 0, + REFERENCE_OWNED = 1, + POINTER_OWNED = 2, + POINTER_NOT_OWNED = 3 }; struct ReferenceCount { diff --git a/phonelibs/snpe/include/DlSystem/IBufferAttributes.hpp b/phonelibs/snpe/include/DlSystem/IBufferAttributes.hpp index e80f6c134c9ab4..f893e0a2370880 100644 --- a/phonelibs/snpe/include/DlSystem/IBufferAttributes.hpp +++ b/phonelibs/snpe/include/DlSystem/IBufferAttributes.hpp @@ -1,6 +1,6 @@ //============================================================================== // -// Copyright (c) 2017 Qualcomm Technologies, Inc. +// Copyright (c) 2017-2019 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -21,7 +21,6 @@ namespace zdl { namespace zdl { namespace DlSystem { - /** * @brief IBufferAttributes returns a buffer's dimension and alignment * requirements, along with info on its encoding type @@ -65,6 +64,15 @@ class ZDL_EXPORT IBufferAttributes { */ virtual const TensorShape getAlignments() const noexcept = 0; + /** + * @brief Gets the buffer encoding returned from the network responsible + * for generating this buffer. Depending on the encoding type, this will + * be an instance of an encoding type specific derived class. + * + * @return Derived user buffer encoding object. + */ + virtual zdl::DlSystem::UserBufferEncoding* getEncoding() const noexcept = 0; + virtual ~IBufferAttributes() {} }; diff --git a/phonelibs/snpe/include/DlSystem/IUserBuffer.hpp b/phonelibs/snpe/include/DlSystem/IUserBuffer.hpp index 70f6b33cbba1a9..aefa0eda9460f1 100644 --- a/phonelibs/snpe/include/DlSystem/IUserBuffer.hpp +++ b/phonelibs/snpe/include/DlSystem/IUserBuffer.hpp @@ -1,6 +1,6 @@ //============================================================================== // -// Copyright (c) 2017-2018 Qualcomm Technologies, Inc. +// Copyright (c) 2017-2019 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -11,6 +11,7 @@ #include "TensorShape.hpp" #include "ZdlExportDefine.hpp" +#include namespace zdl { namespace DlSystem { @@ -44,7 +45,10 @@ class ZDL_EXPORT UserBufferEncoding { UNSIGNED8BIT = 2, /// Each element is presented by an 8-bit quantized value. - TF8 = 10 + TF8 = 10, + + /// Each element is presented by an 16-bit quantized value. + TF16 = 11 }; /** @@ -147,6 +151,7 @@ class ZDL_EXPORT UserBufferEncodingFloat : public UserBufferEncoding { * An encoding type where each element is represented by tf8, which is an * 8-bit quantizd value, which has an exact representation of 0.0 */ + class ZDL_EXPORT UserBufferEncodingTf8 : public UserBufferEncodingUnsigned8Bit { public: UserBufferEncodingTf8() = delete; @@ -155,60 +160,72 @@ class ZDL_EXPORT UserBufferEncodingTf8 : public UserBufferEncodingUnsigned8Bit { m_StepExactly0(stepFor0), m_QuantizedStepSize(stepSize) {}; - /** + +/** * @brief Sets the step value that represents 0 * * @param[in] stepExactly0 The step value that represents 0 * */ + void setStepExactly0(const unsigned char stepExactly0) { m_StepExactly0 = stepExactly0; } - /** + +/** * @brief Sets the float value that each step represents * * @param[in] quantizedStepSize The float value of each step size * */ + void setQuantizedStepSize(const float quantizedStepSize) { m_QuantizedStepSize = quantizedStepSize; } - /** + +/** * @brief Retrieves the step that represents 0.0 * * @return Step value */ + unsigned char getStepExactly0() const { return m_StepExactly0; } - /** + +/** * Calculates the minimum floating point value that * can be represented with this encoding. * * @return Minimum representable floating point value */ + float getMin() const { return m_QuantizedStepSize * (0 - m_StepExactly0); } - /** + +/** * Calculates the maximum floating point value that * can be represented with this encoding. * * @return Maximum representable floating point value */ + float getMax() const { return m_QuantizedStepSize * (255 - m_StepExactly0); } - /** + +/** * @brief Retrieves the step size * * @return Step size */ + float getQuantizedStepSize() const { return m_QuantizedStepSize; } @@ -219,6 +236,84 @@ class ZDL_EXPORT UserBufferEncodingTf8 : public UserBufferEncodingUnsigned8Bit { float m_QuantizedStepSize; }; + + +class ZDL_EXPORT UserBufferEncodingTfN : public UserBufferEncoding { +public: + UserBufferEncodingTfN() = delete; + UserBufferEncodingTfN(uint64_t stepFor0, float stepSize, uint8_t bWidth=8): + UserBufferEncoding(getTypeFromWidth(bWidth)), + bitWidth(bWidth), + m_StepExactly0(stepFor0), + m_QuantizedStepSize(stepSize){}; + size_t getElementSize() const noexcept override; + /** + * @brief Sets the step value that represents 0 + * + * @param[in] stepExactly0 The step value that represents 0 + * + */ + void setStepExactly0(uint64_t stepExactly0) { + m_StepExactly0 = stepExactly0; + } + + /** + * @brief Sets the float value that each step represents + * + * @param[in] quantizedStepSize The float value of each step size + * + */ + void setQuantizedStepSize(const float quantizedStepSize) { + m_QuantizedStepSize = quantizedStepSize; + } + + /** + * @brief Retrieves the step that represents 0.0 + * + * @return Step value + */ + uint64_t getStepExactly0() const { + return m_StepExactly0; + } + + /** + * Calculates the minimum floating point value that + * can be represented with this encoding. + * + * @return Minimum representable floating point value + */ + float getMin() const { + return m_QuantizedStepSize * (0 - (double)m_StepExactly0); + } + + /** + * Calculates the maximum floating point value that + * can be represented with this encoding. + * + * @return Maximum representable floating point value + */ + float getMax() const{ + return m_QuantizedStepSize * (pow(2,bitWidth)-1 - (double)m_StepExactly0); + }; + + /** + * @brief Retrieves the step size + * + * @return Step size + */ + float getQuantizedStepSize() const { + return m_QuantizedStepSize; + } + + ElementType_t getTypeFromWidth(uint8_t width); + + uint8_t bitWidth; +private: + uint64_t m_StepExactly0; + float m_QuantizedStepSize; +}; + + /** * @brief UserBuffer contains a pointer and info on how to walk it and interpret its content. */ @@ -243,6 +338,24 @@ class ZDL_EXPORT IUserBuffer { */ virtual size_t getSize() const = 0; + /** + * @brief Retrieves the size of the inference data in the buffer, in bytes. + * + * The inference results from a dynamic-sized model may not be exactly the same size + * as the UserBuffer provided to SNPE. This function can be used to get the amount + * of output inference data, which may be less or greater than the size of the UserBuffer. + * + * If the inference results fit in the UserBuffer, getOutputSize() would be less than + * or equal to getSize(). But if the inference results were more than the capacity of + * the provided UserBuffer, the results would be truncated to fit the UserBuffer. But, + * getOutputSize() would be greater than getSize(), which indicates a bigger buffer + * needs to be provided to SNPE to hold all of the inference results. + * + * @return Size required for the buffer to hold all inference results, which can be less + * or more than the size of the buffer, in bytes. + */ + virtual size_t getOutputSize() const = 0; + /** * @brief Changes the underlying memory that backs the UserBuffer. * diff --git a/phonelibs/snpe/include/DlSystem/PlatformConfig.hpp b/phonelibs/snpe/include/DlSystem/PlatformConfig.hpp index df452dc46970bf..3f15f177e3eeba 100644 --- a/phonelibs/snpe/include/DlSystem/PlatformConfig.hpp +++ b/phonelibs/snpe/include/DlSystem/PlatformConfig.hpp @@ -10,6 +10,7 @@ #define _DL_SYSTEM_PLATFORM_CONFIG_HPP_ #include "DlSystem/ZdlExportDefine.hpp" +#include namespace zdl{ namespace DlSystem @@ -92,7 +93,8 @@ class ZDL_EXPORT PlatformConfig PlatformConfigInfo(){}; }; - PlatformConfig() : m_PlatformType(PlatformType_t::UNKNOWN) {}; + PlatformConfig() : m_PlatformType(PlatformType_t::UNKNOWN), + m_PlatformOptions("") {}; /** * @brief Retrieves the platform type @@ -156,9 +158,42 @@ class ZDL_EXPORT PlatformConfig return false; } + /** + * @brief Sets the platform options + * + * @param[in] options Options as a string in the form of "keyword:options" + * + * @return True if options are pass validation; otherwise false. If false, the options are not updated. + */ + bool setPlatformOptions(std::string options) { + std::string oldOptions = m_PlatformOptions; + m_PlatformOptions = options; + if (isOptionsValid()) { + return true; + } else { + m_PlatformOptions = oldOptions; + return false; + } + } + + /** + * @brief Indicates whther the plaform configuration is valid. + * + * @return True if the platform configuration is valid; false otherwise. + */ + bool isOptionsValid() const; + + /** + * @brief Gets the platform options + * + * @return Options as a string + */ + std::string getPlatformOptions() const { return m_PlatformOptions; } + private: PlatformType_t m_PlatformType; PlatformConfigInfo m_PlatformConfigInfo; + std::string m_PlatformOptions; }; /** @} */ /* end_addtogroup c_plus_plus_apis C++ */ diff --git a/phonelibs/snpe/include/DlSystem/RuntimeList.hpp b/phonelibs/snpe/include/DlSystem/RuntimeList.hpp new file mode 100644 index 00000000000000..1088a5b31f4538 --- /dev/null +++ b/phonelibs/snpe/include/DlSystem/RuntimeList.hpp @@ -0,0 +1,154 @@ +//============================================================================= +// +// Copyright (c) 2019 Qualcomm Technologies, Inc. +// All Rights Reserved. +// Confidential and Proprietary - Qualcomm Technologies, Inc. +// +//============================================================================= + +#include "ZdlExportDefine.hpp" +#include "DlSystem/DlEnums.hpp" +#include "DlSystem/StringList.hpp" +#include +#include + +#ifndef DL_SYSTEM_RUNTIME_LIST_HPP +#define DL_SYSTEM_RUNTIME_LIST_HPP + +namespace DlSystem +{ + // Forward declaration of Runtime List implementation. + class RuntimeListImpl; +} + +namespace zdl +{ +namespace DlSystem +{ + +/** @addtogroup c_plus_plus_apis C++ +@{ */ + +/** + * @brief . + * + * A class representing list of runtimes + */ +class ZDL_EXPORT RuntimeList final +{ +public: + + /** + * @brief . + * + * Creates a new runtime list + * + */ + RuntimeList(); + + /** + * @brief . + * + * copy constructor. + * @param[in] other object to copy. + */ + RuntimeList(const RuntimeList& other); + + /** + * @brief . + * + * constructor with single Runtime_t object + * @param[in] Runtime_t object + */ + RuntimeList(const zdl::DlSystem::Runtime_t& runtime); + + /** + * @brief . + * + * assignment operator. + */ + RuntimeList& operator=(const RuntimeList& other); + + /** + * @brief . + * + * subscript operator. + */ + Runtime_t& operator[](size_t index); + + /** + * @brief Adds runtime to the end of the runtime list + * order of precedence is former followed by latter entry + * + * @param[in] runtime to add + * + * Ruturns false If the runtime already exists + */ + bool add(const zdl::DlSystem::Runtime_t& runtime); + + /** + * @brief Removes the runtime from the list + * + * @param[in] runtime to be removed + * + * @note If the runtime is not found, nothing is done. + */ + void remove(const zdl::DlSystem::Runtime_t runtime) noexcept; + + /** + * @brief Returns the number of runtimes in the list + */ + size_t size() const noexcept; + + /** + * @brief Returns true if the list is empty + */ + bool empty() const noexcept; + + /** + * @brief . + * + * Removes all runtime from the list + */ + void clear() noexcept; + + /** + * @brief . + * + * Returns a StringList of names from the runtime list in + * order of precedence + */ + zdl::DlSystem::StringList getRuntimeListNames() const; + + /** + * @brief . + * + * @param[in] runtime string + * Returns a Runtime enum corresponding to the in param string + * + */ + static zdl::DlSystem::Runtime_t stringToRuntime(const char* runtimeStr); + + /** + * @brief . + * + * @param[in] runtime + * Returns a string corresponding to the in param runtime enum + * + */ + static const char* runtimeToString(const zdl::DlSystem::Runtime_t runtime); + + ~RuntimeList(); + +private: + void deepCopy(const RuntimeList &other); + std::unique_ptr<::DlSystem::RuntimeListImpl> m_RuntimeListImpl; +}; + +} // DlSystem namespace +} // zdl namespace + +/** @} */ /* end_addtogroup c_plus_plus_apis C++ */ + +#endif // DL_SYSTEM_RUNTIME_LIST_HPP + diff --git a/phonelibs/snpe/include/SNPE/ApplicationBufferMap.hpp b/phonelibs/snpe/include/SNPE/ApplicationBufferMap.hpp new file mode 100644 index 00000000000000..380a3e0e6e77ee --- /dev/null +++ b/phonelibs/snpe/include/SNPE/ApplicationBufferMap.hpp @@ -0,0 +1,101 @@ +//============================================================================== +// +// Copyright (c) 2019 Qualcomm Technologies, Inc. +// All Rights Reserved. +// Confidential and Proprietary - Qualcomm Technologies, Inc. +// +//============================================================================== + +#ifndef PSNPE_APPLICATIONBUFFERMAP_HPP +#define PSNPE_APPLICATIONBUFFERMAP_HPP +#include +#include +#include + +#include "DlSystem/UserBufferMap.hpp" +#include "DlSystem/ZdlExportDefine.hpp" + +namespace zdl +{ +namespace PSNPE +{ +/** @addtogroup c_plus_plus_apis C++ +@{ */ + +/** + * @brief . + * + * A class representing the UserBufferMap of Input and Output asynchronous mode. + */ + +class ZDL_EXPORT ApplicationBufferMap final +{ + + public: + /** + * @brief Adds a name and the corresponding buffer + * to the map + * + * @param[in] name The name of the UserBuffer + * @param[in] buffer The vector of the uint8_t data + * + * @note If a UserBuffer with the same name already exists, the new + * UserBuffer pointer would be updated. + */ + void add(const char* name, std::vector& buff) noexcept; + void add(const char* name, std::vector& buff) noexcept; + /** + * @brief Removes a mapping of one UserBuffer and its name by its name + * + * @param[in] name The name of UserBuffer to be removed + * + * @note If no UserBuffer with the specified name is found, nothing + * is done. + */ + void remove(const char* name) noexcept; + + /** + * @brief Returns the number of UserBuffers in the map + */ + size_t size() const noexcept; + + /** + * @brief . + * + * Removes all UserBuffers from the map + */ + void clear() noexcept; + + /** + * @brief Returns the UserBuffer given its name. + * + * @param[in] name The name of the UserBuffer to get. + * + * @return nullptr if no UserBuffer with the specified name is + * found; otherwise, a valid pointer to the UserBuffer. + */ + const std::vector& getUserBuffer(const char* name) const; + const std::vector& operator[](const char* name) const; + /** + * @brief . + * + * Returns the names of all UserAsyncBufferMap + * + * @return A list of UserBuffer names. + */ + zdl::DlSystem::StringList getUserBufferNames() const; + const std::unordered_map>& getUserBuffer() const; + explicit ApplicationBufferMap(); + ~ApplicationBufferMap(); + explicit ApplicationBufferMap( + const std::unordered_map> buffer); + + private: + std::unordered_map> m_UserMap; +}; + +/** @} */ /* end_addtogroup c_plus_plus_apis C++ */ +} // namespace PSNPE +} // namespace zdl + +#endif // PSNPE_APPLICATIONBUFFERMAP_HPP diff --git a/phonelibs/snpe/include/SNPE/PSNPE.hpp b/phonelibs/snpe/include/SNPE/PSNPE.hpp new file mode 100644 index 00000000000000..732b7aa4d349ed --- /dev/null +++ b/phonelibs/snpe/include/SNPE/PSNPE.hpp @@ -0,0 +1,174 @@ +// ============================================================================= +// +// Copyright (c) 2019 Qualcomm Technologies, Inc. +// All Rights Reserved. +// Confidential and Proprietary - Qualcomm Technologies, Inc. +// +// ============================================================================= + +#ifndef PSNPE_HPP +#define PSNPE_HPP + +#include +#include +#include +#include "SNPE/SNPE.hpp" +#include "DlSystem/UserBufferMap.hpp" +#include "DlContainer/IDlContainer.hpp" +#include "DlSystem/DlEnums.hpp" +#include "DlSystem/ZdlExportDefine.hpp" + +#include "UserBufferList.hpp" +#include "RuntimeConfigList.hpp" +#include "ApplicationBufferMap.hpp" + +namespace zdl +{ +namespace PSNPE +{ + +/** @addtogroup c_plus_plus_apis C++ +@{ */ + +/** + *@ brief build snpe instance in serial or parallel + * + */ +enum ZDL_EXPORT BuildMode { + SERIAL = 0, + PARALLEL = 1 +}; +/** + * @brief Input and output transmission mode + */ +enum ZDL_EXPORT InputOutputTransmissionMode +{ + sync = 0, + outputAsync = 1, + inputOutputAsync = 2 +}; + +/** + * @brief A structure representing parameters of callback function of Async Output mode + */ +struct ZDL_EXPORT OutputAsyncCallbackParam +{ + size_t dataIndex; + bool executeStatus; + OutputAsyncCallbackParam(size_t _index,bool _status) + : dataIndex(_index),executeStatus(_status){}; +}; +/** + * @brief A structure representing parameters of callback function of Async Input/Output mode + */ +struct ZDL_EXPORT InputOutputAsyncCallbackParam +{ + size_t dataIndex; + const ApplicationBufferMap& outputMap; + bool executeStatus; + InputOutputAsyncCallbackParam(size_t _index, const ApplicationBufferMap& output_map,bool _status) + : dataIndex(_index) + , outputMap(output_map) + ,executeStatus(_status){ + + }; +}; +using OutputAsyncCallbackFunc = std::function; +using InputOutputAsyncCallbackFunc = std::function; +/** + * @brief . + * + * A structure BulkSNPE configuration + */ +struct ZDL_EXPORT BuildConfig final +{ + BuildMode buildMode = BuildMode::SERIAL; + zdl::DlContainer::IDlContainer* container; + zdl::DlSystem::StringList outputBufferNames; + RuntimeConfigList runtimeConfigList; + OutputAsyncCallbackFunc outputCallback; + InputOutputAsyncCallbackFunc inputOutputCallback; + InputOutputTransmissionMode inputOutputTransmissionMode = InputOutputTransmissionMode::sync; +}; +/** + * @brief . + * + * The class for executing SNPE instances in parallel. + */ +class ZDL_EXPORT PSNPE final +{ + public: + ~PSNPE(); + + explicit PSNPE() noexcept :m_TransmissionMode(InputOutputTransmissionMode::sync){}; + + /** + * @brief Build snpe instances. + * + */ + bool build(BuildConfig& buildConfig) noexcept; + + /** + * @brief Execute snpe instances in Async Output mode and Sync mode + * + * @param[in] inputBufferList A list of user buffers that contains the input data + * + * @param[in,out] outputBufferList A list of user buffers that will hold the output data + * + */ + bool execute(UserBufferList& inputBufferList, UserBufferList& outputBufferList) noexcept; + + /** + * @brief Execute snpe instances in Async Input/Output mode + * + * @param[in]inputMap A map of input buffers that contains input data. The names of buffers + * need to be matched with names retrived through getInputTensorNames() + * + * @param dataIndex Index of the input data + * + * @param isTF8buff Whether prefer to using 8 bit quantized element for inference + * + * @return True if executed successfully; flase, otherwise. + */ + bool executeInputOutputAsync(const ApplicationBufferMap& inputMap, size_t dataIndex, bool isTF8buff) noexcept; + /** + * @brief Returns the input layer names of the network. + * + * @return StringList which contains the input layer names + */ + const zdl::DlSystem::StringList getInputTensorNames() const noexcept; + + /** + * @brief Returns the output layer names of the network. + * + * @return StringList which contains the output layer names + */ + const zdl::DlSystem::StringList getOutputTensorNames() const noexcept; + + /** + * @brief Returns the input tensor dimensions of the network. + * + * @return TensorShape which contains the dimensions. + */ + const zdl::DlSystem::TensorShape getInputDimensions() const noexcept; + + /** + * @brief Returns attributes of buffers. + * + * @see zdl::SNPE + * + * @return BufferAttributes of input/output tensor named. + */ + const zdl::DlSystem::TensorShape getBufferAttributesDims(const char *name) const noexcept; + + private: + PSNPE(const PSNPE&) = delete; + PSNPE& operator=(const PSNPE&) = delete; + zdl::PSNPE::InputOutputTransmissionMode m_TransmissionMode; + +}; + +/** @} */ /* end_addtogroup c_plus_plus_apis C++ */ +} // namespace PSNPE +} // namespace zdl +#endif // PSNPE_HPP diff --git a/phonelibs/snpe/include/SNPE/RuntimeConfigList.hpp b/phonelibs/snpe/include/SNPE/RuntimeConfigList.hpp new file mode 100644 index 00000000000000..ecd438b9101ec2 --- /dev/null +++ b/phonelibs/snpe/include/SNPE/RuntimeConfigList.hpp @@ -0,0 +1,88 @@ +//============================================================================== +// +// Copyright (c) 2019 Qualcomm Technologies, Inc. +// All Rights Reserved. +// Confidential and Proprietary - Qualcomm Technologies, Inc. +// +//============================================================================== +#ifndef PSNPE_RUNTIMECONFIGLIST_HPP +#define PSNPE_RUNTIMECONFIGLIST_HPP + +#include +#include "DlSystem/DlEnums.hpp" +#include "DlContainer/IDlContainer.hpp" +#include "DlSystem/ZdlExportDefine.hpp" +#include "DlSystem/RuntimeList.hpp" + +namespace zdl { +namespace PSNPE +{ + +/** @addtogroup c_plus_plus_apis C++ +@{ */ + +/** + * @brief . + * + * The structure for configuring a BulkSNPE runtime + * + */ +struct ZDL_EXPORT RuntimeConfig final { + zdl::DlSystem::Runtime_t runtime; + zdl::DlSystem::RuntimeList runtimeList; + zdl::DlSystem::PerformanceProfile_t perfProfile; + bool enableCPUFallback; + RuntimeConfig(): runtime{zdl::DlSystem::Runtime_t::CPU_FLOAT32}, + perfProfile{zdl::DlSystem::PerformanceProfile_t::HIGH_PERFORMANCE}, + enableCPUFallback{false} + {} + RuntimeConfig(const RuntimeConfig& other) + { + runtime = other.runtime; + runtimeList = other.runtimeList; + perfProfile = other.perfProfile; + enableCPUFallback = other.enableCPUFallback; + } + + RuntimeConfig& operator=(const RuntimeConfig &other) + { + this->runtimeList = other.runtimeList; + this->runtime = other.runtime; + this->perfProfile = other.perfProfile; + this->enableCPUFallback = other.enableCPUFallback; + return *this; + } + + ~RuntimeConfig() {} + +}; + +/** +* @brief . +* +* The class for creating a RuntimeConfig container. +* +*/ +class ZDL_EXPORT RuntimeConfigList final +{ +public: + RuntimeConfigList(); + RuntimeConfigList(const size_t size); + void push_back(const RuntimeConfig &runtimeConfig); + RuntimeConfig& operator[](const size_t index); + RuntimeConfigList& operator =(const RuntimeConfigList &other); + size_t size() const noexcept; + size_t capacity() const noexcept; + void clear() noexcept; + ~RuntimeConfigList() = default; + +private: + void swap(const RuntimeConfigList &other); + std::vector m_runtimeConfigs; + +}; +/** @} */ /* end_addtogroup c_plus_plus_apis C++ */ + +} // namespace PSNPE +} // namespace zdl +#endif //PSNPE_RUNTIMECONFIGLIST_HPP diff --git a/phonelibs/snpe/include/SNPE/SNPE.hpp b/phonelibs/snpe/include/SNPE/SNPE.hpp index ba84265b9663f8..e0821b90b1397c 100644 --- a/phonelibs/snpe/include/SNPE/SNPE.hpp +++ b/phonelibs/snpe/include/SNPE/SNPE.hpp @@ -83,6 +83,14 @@ class ZDL_EXPORT SNPE final zdl::DlSystem::Optional getOutputTensorNames() const noexcept; + /** + * @brief Gets the name of output tensor from the input layer name + * + * @return Output tensor name. + */ + zdl::DlSystem::StringList + getOutputTensorNamesByLayerName(const char *name) const noexcept; + /** * @brief Processes the input data and returns the output * diff --git a/phonelibs/snpe/include/SNPE/SNPEBuilder.hpp b/phonelibs/snpe/include/SNPE/SNPEBuilder.hpp index 13e80e8c1a703d..f314d88bcff949 100644 --- a/phonelibs/snpe/include/SNPE/SNPEBuilder.hpp +++ b/phonelibs/snpe/include/SNPE/SNPEBuilder.hpp @@ -1,6 +1,6 @@ //============================================================================== // -// Copyright (c) 2017 Qualcomm Technologies, Inc. +// Copyright (c) 2017-2019 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -15,6 +15,7 @@ #include "DlSystem/DlOptional.hpp" #include "DlSystem/TensorShapeMap.hpp" #include "DlSystem/PlatformConfig.hpp" +#include "DlSystem/RuntimeList.hpp" namespace zdl { namespace DlContainer @@ -55,6 +56,9 @@ class ZDL_EXPORT SNPEBuilder final ~SNPEBuilder(); /** + * NOTE: DEPRECATED, MAY BE REMOVED IN THE FUTURE. Please use + * setRuntimeProcessorOrder() + * * @brief Sets the runtime processor. * * @param[in] targetRuntimeProcessor The target runtime. @@ -74,6 +78,17 @@ class ZDL_EXPORT SNPEBuilder final SNPEBuilder& setPerformanceProfile( zdl::DlSystem::PerformanceProfile_t performanceProfile); + /** + * @brief Sets the profiling level. Default profiling level for + * SNPEBuilder is off. Off and basic only applies to DSP runtime. + * + * @param[in] profilingLevel The target profiling level. + * + * @return The current instance of SNPEBuilder. + */ + SNPEBuilder& setProfilingLevel( + zdl::DlSystem::ProfilingLevel_t profilingLevel); + /** * @brief Sets a preference for execution priority. * @@ -104,6 +119,20 @@ class ZDL_EXPORT SNPEBuilder final SNPEBuilder& setOutputLayers( const zdl::DlSystem::StringList& outputLayerNames); + /** + * @brief Sets the output tensor names. + * + * @param[in] outputTensorNames List of tensor names to + * output. An empty list will + * result in producing output for the final + * output tensor of the model. + * The list will be copied. + * + * @return The current instance of SNPEBuilder. + */ + SNPEBuilder& setOutputTensors( + const zdl::DlSystem::StringList& outputTensorNames); + /** * @brief Passes in a User-defined layer. * @@ -144,6 +173,9 @@ class ZDL_EXPORT SNPEBuilder final bool debugMode); /** + * NOTE: DEPRECATED, MAY BE REMOVED IN THE FUTURE. Please use + * setRuntimeProcessorOrder() + * * @brief Sets the mode of CPU fallback functionality. * * @param[in] mode This flag enables/disables the functionality @@ -185,6 +217,22 @@ class ZDL_EXPORT SNPEBuilder final */ SNPEBuilder& setInputDimensions(const zdl::DlSystem::TensorShapeMap& inputDimensionsMap); + /** + * @brief Sets the mode of init caching functionality. + * + * @param[in] mode This flag enables/disables the functionality of init caching. + * When init caching functionality is enabled, a set of init caches + * will be created during network building/initialization process + * and will be added to DLC container. If such DLC container is saved + * by the user, in subsequent network building/initialization processes + * these init caches will be loaded from the DLC so as to reduce initialization time. + * In disable mode, no init caches will be added to DLC container. + * + * @return The current instance of SNPEBuilder. + */ + SNPEBuilder& setInitCacheMode( + bool cacheMode); + /** * @brief Returns an instance of SNPE based on the current parameters. * @@ -202,6 +250,31 @@ class ZDL_EXPORT SNPEBuilder final */ SNPEBuilder& setPlatformConfig(const zdl::DlSystem::PlatformConfig& platformConfig); + /** + * @brief Sets network's runtime order of precedence. Example: + * CPU_FLOAT32, GPU_FLOAT16, AIP_FIXED8_TF + * Note:- setRuntimeProcessor() or setCPUFallbackMode() will be silently ignored when + * setRuntimeProcessorOrder() is invoked + * + * @param[in] runtimeList The list of runtime in order of precedence + * + * @return The current instance of SNPEBuilder. + */ + SNPEBuilder& setRuntimeProcessorOrder(const zdl::DlSystem::RuntimeList& runtimeList); + + /** + * @brief Sets the unconsumed tensors as output + * + * @param[in] setOutput This enables unconsumed tensors (i.e) + * outputs which are not inputs to any + * layer (basically dead ends) to be marked + * for output + * + * @return The current instance of SNPEBuilder. + */ + SNPEBuilder& setUnconsumedTensorsAsOutputs( + bool setOutput); + }; /** @} */ /* end_addtogroup c_plus_plus_apis C++ */ diff --git a/phonelibs/snpe/include/SNPE/SNPEFactory.hpp b/phonelibs/snpe/include/SNPE/SNPEFactory.hpp index bb9921afe10591..7bef45d877c4c1 100644 --- a/phonelibs/snpe/include/SNPE/SNPEFactory.hpp +++ b/phonelibs/snpe/include/SNPE/SNPEFactory.hpp @@ -1,6 +1,6 @@ //============================================================================== // -// Copyright (c) 2015-2016 Qualcomm Technologies, Inc. +// Copyright (c) 2015-2020 Qualcomm Technologies, Inc. // All Rights Reserved. // Confidential and Proprietary - Qualcomm Technologies, Inc. // @@ -42,19 +42,35 @@ class ZDL_EXPORT SNPEFactory public: /** - * Indicates whether the supplied runtime is available on the + * Indicates whether the supplied runtime is available on the * current platform. - * + * * @param[in] runtime The target runtime to check. - * - * @return True if the supplied runtime is available; false, + * + * @param[in] option Extent to perform runtime available check. + * + * @return True if the supplied runtime is available; false, * otherwise. */ static bool isRuntimeAvailable(zdl::DlSystem::Runtime_t runtime); + /** + * Indicates whether the supplied runtime is available on the + * current platform. + * + * @param[in] runtime The target runtime to check. + * + * @param[in] option Extent to perform runtime available check. + * + * @return True if the supplied runtime is available; false, + * otherwise. + */ + static bool isRuntimeAvailable(zdl::DlSystem::Runtime_t runtime, + zdl::DlSystem::RuntimeCheckOption_t option); + /** * Gets a reference to the tensor factory. - * + * * @return A reference to the tensor factory. */ static zdl::DlSystem::ITensorFactory& getTensorFactory(); @@ -87,6 +103,17 @@ class ZDL_EXPORT SNPEFactory */ static bool setSNPEStorageLocation(const char* storagePath); + /** + * @brief Register a user-defined op package with SNPE. + * + * @param[in] regLibraryPath Path to the registration library + * that allows clients to register a set of operations that are + * part of the package, and share op info with SNPE + * + * @return True if successful, False otherwise. + */ + static bool addOpPackage( const std::string& regLibraryPath ); + /** * Indicates whether the OpenGL and OpenCL interoperability is supported * on GPU platform. diff --git a/phonelibs/snpe/include/SNPE/UserBufferList.hpp b/phonelibs/snpe/include/SNPE/UserBufferList.hpp new file mode 100644 index 00000000000000..a660bca0f6a525 --- /dev/null +++ b/phonelibs/snpe/include/SNPE/UserBufferList.hpp @@ -0,0 +1,49 @@ +//============================================================================== +// +// Copyright (c) 2019 Qualcomm Technologies, Inc. +// All Rights Reserved. +// Confidential and Proprietary - Qualcomm Technologies, Inc. +// +//============================================================================== +#ifndef PSNPE_USERBUFFERLIST_HPP +#define PSNPE_USERBUFFERLIST_HPP + +#include +#include "DlSystem/UserBufferMap.hpp" +#include "DlSystem/ZdlExportDefine.hpp" + +namespace zdl { +namespace PSNPE +{ + +/** @addtogroup c_plus_plus_apis C++ +@{ */ +/** +* @brief . +* +* The class for creating a UserBufferMap container. +* +*/ +class ZDL_EXPORT UserBufferList final +{ +public: + UserBufferList(); + UserBufferList(const size_t size); + void push_back(const zdl::DlSystem::UserBufferMap &userBufferMap); + zdl::DlSystem::UserBufferMap& operator[](const size_t index); + UserBufferList& operator =(const UserBufferList &other); + size_t size() const noexcept; + size_t capacity() const noexcept; + void clear() noexcept; + ~UserBufferList() = default; + +private: + void swap(const UserBufferList &other); + std::vector m_userBufferMaps; + +}; +/** @} */ /* end_addtogroup c_plus_plus_apis C++ */ + +} // namespace PSNPE +} // namespace zdl +#endif //PSNPE_USERBUFFERLIST_HPP diff --git a/phonelibs/snpe/larch64 b/phonelibs/snpe/larch64 new file mode 120000 index 00000000000000..e5c222b1c9be49 --- /dev/null +++ b/phonelibs/snpe/larch64 @@ -0,0 +1 @@ +aarch64-linux-gcc4.9 \ No newline at end of file diff --git a/phonelibs/snpe/x86_64 b/phonelibs/snpe/x86_64 new file mode 120000 index 00000000000000..700025efab335e --- /dev/null +++ b/phonelibs/snpe/x86_64 @@ -0,0 +1 @@ +x86_64-linux-clang \ No newline at end of file diff --git a/selfdrive/assets/sounds/warning_2.wav b/selfdrive/assets/sounds/warning_2.wav index e9709d9fdb7b26..4909f11982df11 100644 Binary files a/selfdrive/assets/sounds/warning_2.wav and b/selfdrive/assets/sounds/warning_2.wav differ diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 90043bfed88e72..a8b0377c1a161b 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -22,6 +22,7 @@ from common.basedir import PERSIST from common.api import Api from common.params import Params +from common.realtime import sec_since_boot from cereal.services import service_list from selfdrive.swaglog import cloudlog @@ -272,8 +273,13 @@ def ws_proxy_send(ws, local_sock, signal_sock, end_event): def ws_recv(ws, end_event): while not end_event.is_set(): try: - data = ws.recv() - payload_queue.put_nowait(data) + opcode, data = ws.recv_data(control_frame=True) + if opcode in (ABNF.OPCODE_TEXT, ABNF.OPCODE_BINARY): + if opcode == ABNF.OPCODE_TEXT: + data = data.decode("utf-8") + payload_queue.put_nowait(data) + elif opcode == ABNF.OPCODE_PING: + Params().put("LastAthenaPingTime", str(int(sec_since_boot()*1e9))) except WebSocketTimeoutException: pass except Exception: @@ -316,6 +322,7 @@ def main(): except Exception: cloudlog.exception("athenad.main.exception") conn_retries += 1 + params.delete("LastAthenaPingTime") time.sleep(backoff(conn_retries)) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 715e0200ac5584..17340c7bc99dc4 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -29,6 +29,7 @@ #include "messaging.hpp" #include +#include //triple the FIFO size #define RECV_SIZE (0x3F00) @@ -56,7 +57,7 @@ struct __attribute__((packed)) timestamp_t { }; libusb_context *ctx = NULL; -libusb_device_handle *dev_handle; +libusb_device_handle *dev_handle = NULL; pthread_mutex_t usb_lock; bool spoofing_started = false; @@ -104,11 +105,13 @@ void *safety_setter_thread(void *s) { usleep(100*1000); } LOGW("got CarVin %s", value_vin); + free(value_vin); // VIN query done, stop listening to OBDII - 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); + // BB prevent switch to no output +// 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; @@ -135,6 +138,8 @@ void *safety_setter_thread(void *s) { auto safety_param = car_params.getSafetyParam(); LOGW("setting safety model: %d with param %d", safety_model, safety_param); + + /*BB stop changes to safety model pthread_mutex_lock(&usb_lock); // set in the mutex to avoid race @@ -143,7 +148,7 @@ void *safety_setter_thread(void *s) { libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_model, safety_param, NULL, 0, TIMEOUT); pthread_mutex_unlock(&usb_lock); - +*/ return NULL; } @@ -159,6 +164,11 @@ bool usb_connect() { ignition_last = false; + if (dev_handle != NULL){ + libusb_close(dev_handle); + dev_handle = NULL; + } + dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc); if (dev_handle == NULL) { goto fail; } @@ -259,6 +269,7 @@ bool usb_connect() { return false; } +// must be called before threads or with mutex void usb_retry_connect() { LOG("attempting to connect"); while (!usb_connect()) { usleep(100*1000); } @@ -357,15 +368,32 @@ void can_health(PubSocket *publisher) { uint8_t power_save_enabled; } health; + // create message + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto healthData = event.initHealth(); + + bool received = false; + // recv from board - pthread_mutex_lock(&usb_lock); - do { + if (dev_handle != NULL) { + pthread_mutex_lock(&usb_lock); cnt = libusb_control_transfer(dev_handle, 0xc0, 0xd2, 0, 0, (unsigned char*)&health, sizeof(health), TIMEOUT); - if (cnt != sizeof(health)) { - handle_usb_issue(cnt, __func__); - } - } while(cnt != sizeof(health)); - pthread_mutex_unlock(&usb_lock); + pthread_mutex_unlock(&usb_lock); + + received = (cnt == sizeof(health)); + } + + // No panda connected, send empty health packet + if (!received){ + healthData.setHwType(cereal::HealthData::HwType::UNKNOWN); + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + publisher->send((char*)bytes.begin(), bytes.size()); + return; + } if (spoofing_started) { health.ignition_line = 1; @@ -475,12 +503,6 @@ void can_health(PubSocket *publisher) { ignition_last = ignition; - // create message - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); - auto healthData = event.initHealth(); - // set fields healthData.setUptime(health.uptime); healthData.setVoltage(health.voltage); @@ -501,16 +523,26 @@ void can_health(PubSocket *publisher) { healthData.setFaultStatus(cereal::HealthData::FaultStatus(health.fault_status)); healthData.setPowerSaveEnabled((bool)(health.power_save_enabled)); + // Convert faults bitset to capnp list + std::bitset fault_bits(health.faults); + auto faults = healthData.initFaults(fault_bits.count()); + + size_t i = 0; + for (size_t f = size_t(cereal::HealthData::FaultType::RELAY_MALFUNCTION); + f <= size_t(cereal::HealthData::FaultType::REGISTER_DIVERGENT); f++){ + if (fault_bits.test(f)) { + faults.set(i, cereal::HealthData::FaultType(f)); + i++; + } + } // send to health auto words = capnp::messageToFlatArray(msg); auto bytes = words.asBytes(); publisher->send((char*)bytes.begin(), bytes.size()); - pthread_mutex_lock(&usb_lock); - // send heartbeat back to panda + pthread_mutex_lock(&usb_lock); libusb_control_transfer(dev_handle, 0x40, 0xf3, 1, 0, NULL, 0, TIMEOUT); - pthread_mutex_unlock(&usb_lock); } @@ -557,10 +589,19 @@ void can_send(SubSocket *subscriber) { int sent; pthread_mutex_lock(&usb_lock); + if (!fake_send) { do { - err = libusb_bulk_transfer(dev_handle, 3, (uint8_t*)send, msg_count*0x10, &sent, TIMEOUT); - if (err != 0 || msg_count*0x10 != sent) { handle_usb_issue(err, __func__); } + // Try sending can messages. If the receive buffer on the panda is full it will NAK + // and libusb will try again. After 5ms, it will time out. We will drop the messages. + err = libusb_bulk_transfer(dev_handle, 3, (uint8_t*)send, msg_count*0x10, &sent, 5); + if (err == LIBUSB_ERROR_TIMEOUT) { + LOGW("Transmit buffer full"); + break; + } else if (err != 0 || msg_count*0x10 != sent) { + LOGW("Error"); + handle_usb_issue(err, __func__); + } } while(err != 0); } @@ -594,6 +635,9 @@ void *can_send_thread(void *crap) { while (!do_exit) { can_send(subscriber); } + + delete subscriber; + delete context; return NULL; } @@ -624,6 +668,9 @@ void *can_recv_thread(void *crap) { next_frame_time += dt; } + + delete publisher; + delete c; return NULL; } @@ -639,6 +686,9 @@ void *can_health_thread(void *crap) { can_health(publisher); usleep(500*1000); } + + delete publisher; + delete c; return NULL; } @@ -795,7 +845,7 @@ void pigeon_init() { usleep(100*1000); // init from ubloxd - // To generate this data, run test/ubloxd.py with the print statements enabled in the write function in panda/python/serial.py + // To generate this data, run test/ubloxd.py with the print statements enabled in the write function in panda/python/serial.py pigeon_send("\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F"); pigeon_send("\xB5\x62\x06\x3E\x00\x00\x44\xD2"); pigeon_send("\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35"); @@ -873,6 +923,8 @@ void *pigeon_thread(void *crap) { cnt++; } + delete publisher; + delete context; return NULL; } @@ -899,21 +951,30 @@ int main() { loopback_can = true; } + err = pthread_mutex_init(&usb_lock, NULL); + assert(err == 0); + // init libusb err = libusb_init(&ctx); assert(err == 0); - libusb_set_debug(ctx, 3); - - // connect to the board - usb_retry_connect(); +#if LIBUSB_API_VERSION >= 0x01000106 + libusb_set_option(ctx, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_INFO); +#else + libusb_set_debug(ctx, 3); +#endif - // create threads pthread_t can_health_thread_handle; err = pthread_create(&can_health_thread_handle, NULL, can_health_thread, NULL); assert(err == 0); + // connect to the board + pthread_mutex_lock(&usb_lock); + usb_retry_connect(); + pthread_mutex_unlock(&usb_lock); + + // create threads pthread_t can_send_thread_handle; err = pthread_create(&can_send_thread_handle, NULL, can_send_thread, NULL); diff --git a/selfdrive/boardd/boardd_setup.py b/selfdrive/boardd/boardd_setup.py index 1da81997c31fd9..b8997f85427905 100644 --- a/selfdrive/boardd/boardd_setup.py +++ b/selfdrive/boardd/boardd_setup.py @@ -21,7 +21,10 @@ ARCH_DIR = 'x64' else: libraries = [':libcan_list_to_can_capnp.a', 'capnp', 'kj'] - ARCH_DIR = 'aarch64' + if os.path.isdir("/system"): + ARCH_DIR = 'aarch64' + else: + ARCH_DIR = 'larch64' setup(name='Boardd API Implementation', cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, diff --git a/selfdrive/boardd/tests/replay_many.py b/selfdrive/boardd/tests/replay_many.py index 024867769dc059..7596b47a145f15 100755 --- a/selfdrive/boardd/tests/replay_many.py +++ b/selfdrive/boardd/tests/replay_many.py @@ -4,7 +4,8 @@ import time import signal import traceback -from panda import Panda +import usb1 +from panda import Panda, PandaDFU from multiprocessing import Pool jungle = "JUNGLE" in os.environ @@ -21,27 +22,32 @@ def initializer(): def send_thread(sender_serial): global jungle - try: - if jungle: - sender = PandaJungle(sender_serial) - else: - sender = Panda(sender_serial) - sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT) - sender.set_can_loopback(False) + while True: + try: + if jungle: + sender = PandaJungle(sender_serial) + else: + sender = Panda(sender_serial) + sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + + sender.set_can_loopback(False) + can_sock = messaging.sub_sock('can') - can_sock = messaging.sub_sock('can') + while True: + tsc = messaging.recv_one(can_sock) + snd = can_capnp_to_can_list(tsc.can) + snd = list(filter(lambda x: x[-1] <= 2, 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)) - sender.can_send_many(snd) + try: + sender.can_send_many(snd) + except usb1.USBErrorTimeout: + pass - # Drain panda message buffer - sender.can_recv() - except Exception: - traceback.print_exc() + # Drain panda message buffer + sender.can_recv() + except Exception: + traceback.print_exc() + time.sleep(1) if __name__ == "__main__": if jungle: @@ -50,12 +56,22 @@ def send_thread(sender_serial): serials = Panda.list() num_senders = len(serials) + if num_senders == 0: print("No senders found. Exiting") sys.exit(1) else: print("%d senders found. Starting broadcast" % num_senders) + if "FLASH" in os.environ: + for s in PandaDFU.list(): + PandaDFU(s).recover() + + time.sleep(1) + for s in serials: + Panda(s).recover() + Panda(s).flash() + pool = Pool(num_senders, initializer=initializer) pool.map_async(send_thread, serials) diff --git a/selfdrive/camerad/SConscript b/selfdrive/camerad/SConscript index 12e1afe5acd492..e30a04fd337788 100644 --- a/selfdrive/camerad/SConscript +++ b/selfdrive/camerad/SConscript @@ -1,13 +1,31 @@ -Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc', 'cereal') +Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc', 'cereal', 'webcam') libs = ['m', 'pthread', common, 'jpeg', 'json', cereal, 'OpenCL', messaging, 'czmq', 'zmq', 'capnp', 'kj', 'capnp_c', visionipc, gpucommon] if arch == "aarch64": libs += ['gsl', 'CB', 'adreno_utils', 'EGL', 'GLESv3', 'cutils', 'ui'] cameras = ['cameras/camera_qcom.c'] -else: +elif arch == "larch64": libs += [] - cameras = ['cameras/camera_frame_stream.cc'] + cameras = ['cameras/camera_qcom2.c'] +elif arch == "jarch64": + libs += ['opencv_core', 'opencv_highgui', 'opencv_video', 'opencv_imgproc', 'opencv_videoio'] + cameras = ['cameras/camera_webcam.cc'] + env = env.Clone() + env.Append(CXXFLAGS = '-DWEBCAM') + env.Append(CFLAGS = '-DWEBCAM') + #env.Append(CPPPATH = '/usr/local/include/opencv4') +else: + if webcam: + libs += ['opencv_core', 'opencv_highgui', 'opencv_imgproc', 'opencv_videoio'] + cameras = ['cameras/camera_webcam.cc'] + env = env.Clone() + env.Append(CXXFLAGS = '-DWEBCAM') + env.Append(CFLAGS = '-DWEBCAM') + env.Append(CPPPATH = '/usr/local/include/opencv4') + else: + libs += [] + cameras = ['cameras/camera_frame_stream.cc'] env.SharedLibrary('snapshot/visionipc', ["#selfdrive/common/visionipc.c", "#selfdrive/common/ipc.c"]) diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py index 0da8d679736f8e..4ba10bf4bea392 100755 --- a/selfdrive/camerad/snapshot/snapshot.py +++ b/selfdrive/camerad/snapshot/snapshot.py @@ -22,6 +22,9 @@ def snapshot(): params = Params() front_camera_allowed = int(params.get("RecordFront")) + if params.get("IsTakingSnapshot") == b"1": + return None + params.put("IsTakingSnapshot", "1") params.put("Offroad_IsTakingSnapshot", json.dumps(OFFROAD_ALERTS["Offroad_IsTakingSnapshot"])) time.sleep(2.0) # Give thermald time to read the param, or if just started give camerad time to start diff --git a/selfdrive/camerad/transforms/rgb_to_yuv.c b/selfdrive/camerad/transforms/rgb_to_yuv.c index 2626e6eebe765f..5bea8088b70ebd 100644 --- a/selfdrive/camerad/transforms/rgb_to_yuv.c +++ b/selfdrive/camerad/transforms/rgb_to_yuv.c @@ -8,13 +8,15 @@ void rgb_to_yuv_init(RGBToYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height, int rgb_stride) { int err = 0; memset(s, 0, sizeof(*s)); + printf("width %d, height %d, rgb_stride %d\n", width, height, rgb_stride); assert(width % 2 == 0); assert(height % 2 == 0); s->width = width; s->height = height; char args[1024]; + printf("snprintf"); snprintf(args, sizeof(args), - "-cl-fast-relaxed-math -cl-denorms-are-zero " + "-cl-fast-relaxed-math -cl-denorms-are-zero " #ifdef CL_DEBUG "-DCL_DEBUG " #endif diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index e99d61e4c1cf1a..260c64a7d2a41e 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -7,6 +7,7 @@ from selfdrive.swaglog import cloudlog import cereal.messaging as messaging from selfdrive.car import gen_empty_fingerprint +from selfdrive.car.tesla.readconfig import CarSettings from cereal import car @@ -27,12 +28,17 @@ def load_interfaces(brand_names): for brand_name in brand_names: path = ('selfdrive.car.%s' % brand_name) CarInterface = __import__(path + '.interface', fromlist=['CarInterface']).CarInterface + + if os.path.exists(BASEDIR + '/' + path.replace('.', '/') + '/carstate.py'): + CarState = __import__(path + '.carstate', fromlist=['CarState']).CarState + else: + CarState = None + if os.path.exists(BASEDIR + '/' + path.replace('.', '/') + '/carcontroller.py'): CarController = __import__(path + '.carcontroller', fromlist=['CarController']).CarController - CarState = __import__(path + '.carstate', fromlist=['CarState']).CarState else: CarController = None - CarState = None + for model_name in brand_names[brand_name]: ret[model_name] = (CarInterface, CarController, CarState) return ret @@ -126,6 +132,13 @@ def fingerprint(logcan, sendcan, has_relay): if frame > frame_fingerprint: # fingerprint done car_fingerprint = candidate_cars[b][0] + + + if (car_fingerprint is None) and CarSettings().forceFingerprintTesla: + print ("Fingerprinting Failed: Returning Tesla (based on branch)") + car_fingerprint = "TESLA MODEL S" + vin = "TESLAFAKEVIN12345" + # bail if no cars left or we've been waiting for more than 2s failed = all(len(cc) == 0 for cc in candidate_cars.values()) or frame > 200 @@ -150,8 +163,18 @@ def fingerprint(logcan, sendcan, has_relay): return car_fingerprint, finger, vin, car_fw, source -def get_car(logcan, sendcan, has_relay=False): - candidate, fingerprints, vin, car_fw, source = fingerprint(logcan, sendcan, has_relay) +def get_car(logcan, sendcan, has_relay = False): + if CarSettings().forceFingerprintTesla: + candidate="TESLA MODEL S" + fingerprints=["","",""] + vin="TESLAFORCED123456" + #BB + car_fw = [] + source=car.CarParams.FingerprintSource.fixed + cloudlog.warning("VIN %s", vin) + Params().put("CarVin", vin) + else: + candidate, fingerprints, vin, car_fw, source = fingerprint(logcan, sendcan, has_relay) if candidate is None: cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index a25df91f9b40c9..c85ad04e5e3244 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -17,9 +17,6 @@ def __init__(self, CP, CarController, CarState): self.VM = VehicleModel(CP) self.frame = 0 - self.gas_pressed_prev = False - self.brake_pressed_prev = False - self.cruise_enabled_prev = False self.low_speed_alert = False self.CS = CarState(CP) @@ -81,7 +78,7 @@ def update(self, c, can_strings): def apply(self, c): raise NotImplementedError - def create_common_events(self, cs_out, extra_gears=[], gas_resume_speed=-1): + def create_common_events(self, cs_out, extra_gears=[], gas_resume_speed=-1, pcm_enable=True): events = [] if cs_out.doorOpen: @@ -108,10 +105,17 @@ def create_common_events(self, cs_out, extra_gears=[], gas_resume_speed=-1): # Disable on rising edge of gas or brake. Also disable on brake when speed > 0. # Optionally allow to press gas at zero speed to resume. # e.g. Chrysler does not spam the resume button yet, so resuming with gas is handy. FIXME! - if (cs_out.gasPressed and (not self.gas_pressed_prev) and cs_out.vEgo > gas_resume_speed) or \ - (cs_out.brakePressed and (not self.brake_pressed_prev or not cs_out.standstill)): + if (cs_out.gasPressed and (not self.CS.out.gasPressed) and cs_out.vEgo > gas_resume_speed) or \ + (cs_out.brakePressed and (not self.CS.out.brakePressed or not cs_out.standstill)): events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + # we engage when pcm is active (rising edge) + if pcm_enable: + if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled: + events.append(create_event('pcmEnable', [ET.ENABLE])) + elif not cs_out.cruiseState.enabled: + events.append(create_event('pcmDisable', [ET.USER_DISABLE])) + return events class RadarInterfaceBase(): @@ -133,6 +137,7 @@ def __init__(self, CP): self.CP = CP self.car_fingerprint = CP.carFingerprint self.cruise_buttons = 0 + self.out = car.CarState.new_message() # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) # R = 1e3 diff --git a/selfdrive/car/modules/ALCA_module.py b/selfdrive/car/modules/ALCA_module.py index 63e5dd333dc5e0..6c2aaf92610342 100644 --- a/selfdrive/car/modules/ALCA_module.py +++ b/selfdrive/car/modules/ALCA_module.py @@ -47,8 +47,8 @@ from common.numpy_fast import interp,clip from selfdrive.controls.lib.pid import PIController from common.realtime import sec_since_boot -from selfdrive.services import service_list -import selfdrive.messaging as messaging +from cereal.services import service_list +import cereal.messaging as messaging import numpy as np from cereal import tesla diff --git a/selfdrive/car/modules/GYRO_module.py b/selfdrive/car/modules/GYRO_module.py index 8e7ae580f503e5..4f9ef34d4e4f99 100644 --- a/selfdrive/car/modules/GYRO_module.py +++ b/selfdrive/car/modules/GYRO_module.py @@ -1,6 +1,6 @@ -from selfdrive.services import service_list +from cereal.services import service_list from collections import deque -import selfdrive.messaging as messaging +import cereal.messaging as messaging import cereal import math from common.realtime import sec_since_boot diff --git a/selfdrive/car/modules/UIBT_module.py b/selfdrive/car/modules/UIBT_module.py index a92341a26843bd..f6612cfa846caf 100644 --- a/selfdrive/car/modules/UIBT_module.py +++ b/selfdrive/car/modules/UIBT_module.py @@ -3,6 +3,7 @@ from ctypes import create_string_buffer import os from datetime import datetime +from common.basedir import BASEDIR buttons_file_rw = "wb" @@ -84,8 +85,8 @@ def __init__(self, carstate,car,folder,showLogo,showCar): self.CS = carstate self.car_folder = folder self.car_name = car - self.buttons_labels_path = "/data/openpilot/selfdrive/car/"+self.car_folder+"/buttons.msg" - self.buttons_status_out_path = "/data/openpilot/selfdrive/car/"+self.car_folder+"/buttons.cc.msg" + self.buttons_labels_path = BASEDIR + "/selfdrive/car/"+self.car_folder+"/buttons.msg" + self.buttons_status_out_path = BASEDIR + "/selfdrive/car/"+self.car_folder+"/buttons.cc.msg" self.btns = [] self.hasChanges = True self.last_in_read_time = datetime.min diff --git a/selfdrive/car/modules/UIEV_module.py b/selfdrive/car/modules/UIEV_module.py index 712c787cb2fbeb..5c84044d241050 100644 --- a/selfdrive/car/modules/UIEV_module.py +++ b/selfdrive/car/modules/UIEV_module.py @@ -1,6 +1,6 @@ from cereal import ui from common import realtime -import selfdrive.messaging as messaging +import cereal.messaging as messaging class UIEvents(): def __init__(self,carstate): diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index 68ee860196f4c9..5be456016c9297 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -1,7 +1,7 @@ from selfdrive.car.tesla.speed_utils.fleet_speed import FleetSpeed from selfdrive.car.tesla.values import CruiseButtons, CruiseState from selfdrive.config import Conversions as CV -import selfdrive.messaging as messaging +import cereal.messaging as messaging import sys import time diff --git a/selfdrive/car/tesla/AHB_module.py b/selfdrive/car/tesla/AHB_module.py index d7a92cf946e22f..0f5f4254f0b440 100644 --- a/selfdrive/car/tesla/AHB_module.py +++ b/selfdrive/car/tesla/AHB_module.py @@ -1,6 +1,6 @@ from selfdrive.config import Conversions as CV from cereal import tesla,log -import selfdrive.messaging as messaging +import cereal.messaging as messaging import time DEBUG = False diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py index fb2e7c6c42794e..9ee508c70a6c28 100644 --- a/selfdrive/car/tesla/PCC_module.py +++ b/selfdrive/car/tesla/PCC_module.py @@ -6,8 +6,8 @@ from selfdrive.car.tesla.values import CruiseState, CruiseButtons from selfdrive.config import Conversions as CV from selfdrive.controls.lib.speed_smoother import speed_smoother -from selfdrive.controls.lib.planner import calc_cruise_accel_limits -import selfdrive.messaging as messaging +from selfdrive.controls.lib.planner import calc_cruise_accel_limits, limit_accel_in_turns +import cereal.messaging as messaging import time import math from collections import OrderedDict @@ -25,8 +25,8 @@ MAX_RADAR_DISTANCE = 120. #max distance to take in consideration radar reading MAX_PEDAL_VALUE = 112. PEDAL_HYST_GAP = 1.0 # don't change pedal command for small oscilalitons within this value -# Cap the pedal to go from 0 to max in 4 seconds -PEDAL_MAX_UP = MAX_PEDAL_VALUE * _DT / 4 +# Cap the pedal to go from 0 to max in 2 seconds +PEDAL_MAX_UP = MAX_PEDAL_VALUE * _DT / 2 # Cap the pedal to go from max to 0 in 0.4 seconds PEDAL_MAX_DOWN = MAX_PEDAL_VALUE * _DT / 0.4 @@ -173,26 +173,32 @@ def __init__(self,carcontroller): self.params = Params() average_speed_over_x_suggestions = 6 # 0.3 seconds (20x a second) self.fleet_speed = FleetSpeed(average_speed_over_x_suggestions) - + def load_pid(self): - try: - v_pid_json = open(V_PID_FILE) - data = json.load(v_pid_json) - if (self.LoC): - if self.LoC.pid: - self.LoC.pid.p = data['p'] - self.LoC.pid.i = data['i'] - self.LoC.pid.f = data['f'] - else: - print("self.LoC not initialized!") - except : - print("file not present, creating at next reset") + try: + v_pid_json = open(V_PID_FILE) + data = json.load(v_pid_json) + if (self.LoC): + if self.LoC.pid: + self.LoC.pid.p = data['p'] + self.LoC.pid.i = data['i'] + if 'd' not in data: + self.Loc.pid.d = 0.01 + else: + self.LoC.pid.d = data['d'] + self.LoC.pid.f = data['f'] + else: + print("self.LoC not initialized!") + except : + print("file not present, creating at next reset") + #Helper function for saving the PCC pid constants across drives def save_pid(self, pid): data = {} data['p'] = pid.p data['i'] = pid.i + data['d'] = pid.d data['f'] = pid.f try: with open(V_PID_FILE , 'w') as outfile : @@ -223,7 +229,10 @@ def update_stat(self, CS, frame): # send reset command idx = self.pedal_idx self.pedal_idx = (self.pedal_idx + 1) % 16 - can_sends.append(teslacan.create_pedal_command_msg(0, 0, idx)) + pedalcan = 2 + if CS.useWithoutHarness: + pedalcan = 0 + can_sends.append(teslacan.create_pedal_command_msg(0, 0, idx, pedalcan)) return can_sends prev_enable_pedal_cruise = self.enable_pedal_cruise @@ -308,7 +317,9 @@ def update_stat(self, CS, frame): return can_sends - def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, set_speed_limit_active, speed_limit_offset,alca_enabled): + def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, pcm_override, + speed_limit_ms, set_speed_limit_active, speed_limit_offset, + alca_enabled): idx = self.pedal_idx self.prev_speed_limit_kph = self.speed_limit_kph @@ -356,12 +367,17 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s v_ego = CS.v_ego - following = self.lead_1.status and self.lead_1.dRel < MAX_RADAR_DISTANCE and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0 - accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego)] + + following = False + if self.lead_1: + following = self.lead_1.status and self.lead_1.dRel < MAX_RADAR_DISTANCE and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0 + accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego,following,is_tesla=True)] + + accel_limits[1] *= _accel_limit_multiplier(CS, self.lead_1) accel_limits[0] = _decel_limit(accel_limits[0], CS.v_ego, self.lead_1, CS, self.pedal_speed_kph) jerk_limits = [min(-0.1, accel_limits[0]/2.), max(0.1, accel_limits[1]/2.)] # TODO: make a separate lookup for jerk tuning - #accel_limits = limit_accel_in_turns(v_ego, CS.angle_steers, accel_limits, CS.CP) + accel_limits = limit_accel_in_turns(v_ego, CS.angle_steers, accel_limits, CS.CP) output_gb = 0 #################################################################### @@ -371,10 +387,11 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s # how much accel and break we have to do #################################################################### if PCCModes.is_selected(FollowMode(), CS.cstm_btns): + MPC_BRAKE_MULTIPLIER = 6. enabled = self.enable_pedal_cruise and self.LoC.long_control_state in [LongCtrlState.pid, LongCtrlState.stopping] # determine if pedal is pressed by human self.prev_accelerator_pedal_pressed = self.accelerator_pedal_pressed - self.accelerator_pedal_pressed = CS.pedal_interceptor_value > 10 + self.accelerator_pedal_pressed = CS.pedal_interceptor_value > 5 #reset PID if we just lifted foot of accelerator if (not self.accelerator_pedal_pressed) and self.prev_accelerator_pedal_pressed: self.reset(CS.v_ego) @@ -391,13 +408,12 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s # cruise speed can't be negative even if user is distracted self.v_pid = max(self.v_pid, 0.) - jerk_min, jerk_max = _jerk_limits(CS.v_ego, self.lead_1, self.v_pid * CV.MS_TO_KPH, self.lead_last_seen_time_ms, CS) self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start, self.v_pid, accel_limits[1], accel_limits[0], - jerk_limits[1], jerk_limits[0], #jerk_max, jerk_min, + jerk_limits[1], jerk_limits[0], _DT_MPC) - + # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) @@ -450,12 +466,12 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s ############################################################## elif PCCModes.is_selected(OpMode(), CS.cstm_btns): output_gb = actuators.gas - actuators.brake - self.v_pid = -10. + self.v_pid = pcm_override + MPC_BRAKE_MULTIPLIER = 12. self.last_output_gb = output_gb # accel and brake - apply_accel = clip(output_gb, 0., _accel_pedal_max(CS.v_ego, self.v_pid, self.lead_1, self.prev_tesla_accel, CS)) - MPC_BRAKE_MULTIPLIER = 6. + apply_accel = clip(output_gb, 0., 1) #_accel_pedal_max(CS.v_ego, self.v_pid, self.lead_1, self.prev_tesla_accel, CS)) apply_brake = -clip(output_gb * MPC_BRAKE_MULTIPLIER, _brake_pedal_min(CS.v_ego, self.v_pid, self.lead_1, CS, self.pedal_speed_kph), 0.) # if speed is over 5mph, the "zero" is at PedalForZeroTorque; otherwise it is zero @@ -669,10 +685,10 @@ def _accel_limit_multiplier(CS, lead): if CS.teslaModel in ["SP","SPD"]: accel_by_speed = OrderedDict([ # (speed m/s, decel) - (0., 0.95), # 0 kmh - (10., 0.95), # 35 kmh - (20., 0.925), # 72 kmh - (30., 0.875)]) # 107 kmh + (0., 0.985), # 0 kmh + (10., 0.975), # 35 kmh + (20., 0.95), # 72 kmh + (30., 0.9)]) # 107 kmh accel_mult = _interp_map(CS.v_ego, accel_by_speed) if _is_present(lead): safe_dist_m = _safe_distance_m(CS.v_ego,CS) @@ -731,35 +747,6 @@ def _decel_limit(accel_min,v_ego, lead, CS, max_speed_kph): #BB: if we don't have a lead, don't do full regen to slow down smoother return accel_min * 0.5 * max_speed_mult -def _accel_pedal_max(v_ego, v_target, lead, prev_tesla_accel,CS): - pedal_max = prev_tesla_accel - if _is_present(lead): - #we have lead, base on speed and distance - safe_dist_m = _safe_distance_m(CS.v_ego,CS) - v_rel = lead.vLeadK - v_ego - accel_speed_map = OrderedDict([ - # (speed m/s, decel) change in accel (0..1) per second - (0., 0.01), # 0 MPH - (1., 0.1), # 4 MPH - (5., 0.15), # 11 MPH - (30., 0.20)]) # 67 MPH - accel_distance_map = OrderedDict([ - # (distance in m, acceleration fraction) - (0.6 * safe_dist_m, 0.3), - (1.0 * safe_dist_m, 1.0), - (3.0 * safe_dist_m, 2.0)]) - pedal_max = prev_tesla_accel + _interp_map(safe_dist_m, accel_distance_map) * _interp_map(v_rel, accel_speed_map) * _DT - else: - #no lead, do just based on speed - accel_speed_map = OrderedDict([ - # (speed m/s, decel) change in accel (0..1) per second - (0., 0.25), # 0 MPH - (10., 0.15), # 22 MPH - (20., 0.12), # 45 MPH - (30., 0.10)]) # 67 MPH - pedal_max = prev_tesla_accel + _interp_map(v_ego, accel_speed_map) * _DT - return 1. #pedal_max - def _brake_pedal_min(v_ego, v_target, lead, CS, max_speed_kph): #if less than 7 MPH we don't have much left till 5MPH to brake, so full regen if v_ego <= 7 * CV.MPH_TO_MS: @@ -787,51 +774,4 @@ def _brake_pedal_min(v_ego, v_target, lead, CS, max_speed_kph): brake_mult2 = _interp_map(lead.dRel, brake_distance_map) brake_mult = max(brake_mult1, brake_mult2) return -brake_mult - -def _jerk_limits(v_ego, lead, max_speed_kph, lead_last_seen_time_ms, CS): - # Allow higher accel jerk at low speed, to get started - accel_jerk_by_speed = OrderedDict([ - # (Speed in m/s, accel jerk) - (0, 0.18), - (9, 0.10)]) - accel_jerk = _interp_map(v_ego, accel_jerk_by_speed) - - # prevent high accel jerk near max speed - near_max_speed_multipliers = OrderedDict([ - # (kph under max speed, accel jerk multiplier) - (0, 0.01), - (4, 1.0)]) - near_max_speed_multiplier = _interp_map(max_speed_kph - v_ego * CV.MS_TO_KPH, near_max_speed_multipliers) - accel_jerk *= near_max_speed_multiplier - if _is_present(lead): - # pick decel jerk based on how much time we have til collision - decel_jerk_map = OrderedDict([ - # (sec to collision, decel jerk) - (0, -1.00), - (2, -0.25), - (4, -0.01), - (80, -0.001)]) - decel_jerk = _interp_map(_sec_til_collision(lead, CS), decel_jerk_map) - safe_dist_m = _safe_distance_m(v_ego,CS) - distance_multipliers = OrderedDict([ - # (distance in m, accel jerk) - (0.8 * safe_dist_m, 0.01), - (2.8 * safe_dist_m, 1.00)]) - distance_multiplier = _interp_map(lead.dRel, distance_multipliers) - accel_jerk *= distance_multiplier - return decel_jerk, accel_jerk - else: - # In the absence of a lead car - decel_jerk = -0.15 - # Limit accel jerk if the lead was only recently lost, to prevent - # bucking as a lead is intermittently detected. - time_since_lead_seen_ms = _current_time_millis() - lead_last_seen_time_ms - time_since_lead_seen_multipliers = OrderedDict([ - # (ms since last lead sighting, accel jerk multiplier) - (0, 0.1), - (2000, 1.0)]) - time_since_lead_seen_multiplier = _interp_map(time_since_lead_seen_ms, time_since_lead_seen_multipliers) - accel_jerk *= time_since_lead_seen_multiplier - - return decel_jerk, accel_jerk diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 51743a30960ff1..eee928ee78b524 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -8,7 +8,7 @@ from selfdrive.car.tesla.blinker_module import Blinker from selfdrive.car.tesla.speed_utils.fleet_speed import FleetSpeed from selfdrive.car.tesla.values import AH, CM -from selfdrive.can.packer import CANPacker +from opendbc.can.packer import CANPacker from selfdrive.config import Conversions as CV from selfdrive.car.modules.ALCA_module import ALCAController from selfdrive.car.modules.GYRO_module import GYROController @@ -17,7 +17,7 @@ from selfdrive.car.tesla.HSO_module import HSOController from selfdrive.car.tesla.speed_utils.movingaverage import MovingAverage from selfdrive.car.tesla.AHB_module import AHBController -import selfdrive.messaging as messaging +import cereal.messaging as messaging # Steer angle limits ANGLE_MAX_BP = [0., 27., 36.] @@ -76,7 +76,7 @@ def process_hud_alert(hud_alert): class CarController(): - def __init__(self, dbc_name): + def __init__(self, dbc_name,CP,VM): self.fleet_speed_state = 0 self.cc_counter = 0 self.alcaStateData = None @@ -578,7 +578,7 @@ def update(self, enabled, CS, frame, actuators, \ if send_fake_msg: if enable_steer_control and op_status == 3: op_status = 0x5 - park_brake_request = int(CS.ahbEnabled) + park_brake_request = 0 #experimental; disabled for now if park_brake_request == 1: print("Park Brake Request received") adaptive_cruise = 1 if (not self.PCC.pcc_available and self.ACC.adaptive) or self.PCC.pcc_available else 0 @@ -597,7 +597,7 @@ def update(self, enabled, CS, frame, actuators, \ self.DAS_202_noisyEnvironment, CS.DAS_doorOpen, CS.DAS_notInDrive, CS.enableDasEmulation, CS.enableRadarEmulation, \ self.stopSignWarning, self.stopLightWarning, \ self.DAS_222_accCameraBlind, self.DAS_219_lcTempUnavailableSpeed, self.DAS_220_lcTempUnavailableRoad, self.DAS_221_lcAborting, \ - self.DAS_207_lkasUnavailable,self.DAS_208_rackDetected, self.DAS_025_steeringOverride,self.ldwStatus,0,CS.useWithoutHarness)) + self.DAS_207_lkasUnavailable,self.DAS_208_rackDetected, self.DAS_025_steeringOverride,self.ldwStatus,CS.useWithoutHarness,CS.usesApillarHarness)) self.stopLightWarning_last = self.stopLightWarning self.stopSignWarning_last = self.stopSignWarning self.warningNeeded = 0 @@ -622,10 +622,19 @@ def update(self, enabled, CS, frame, actuators, \ can_sends.insert(0, cruise_msg) apply_accel = 0. if self.PCC.pcc_available and frame % 5 == 0: # pedal processed at 20Hz - apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(enabled, CS, frame, actuators, pcm_speed, \ + pedalcan = 2 + if CS.useWithoutHarness: + pedalcan = 0 + apply_accel, accel_needed, accel_idx = self.PCC.update_pdl( + enabled, + CS, + frame, + actuators, + pcm_speed, + pcm_override, self.speed_limit_ms, self.set_speed_limit_active, self.speed_limit_offset * CV.KPH_TO_MS, self.alca_enabled) - can_sends.append(teslacan.create_pedal_command_msg(apply_accel, int(accel_needed), accel_idx)) + can_sends.append(teslacan.create_pedal_command_msg(apply_accel, int(accel_needed), accel_idx,pedalcan)) self.last_angle = apply_angle self.last_accel = apply_accel diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index e901b02165865d..b7cd5bb5448397 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -1,5 +1,5 @@ from common.kalman.simple_kalman import KF1D -from selfdrive.can.parser import CANParser +from opendbc.can.parser import CANParser from selfdrive.config import Conversions as CV from selfdrive.car.tesla.ACC_module import ACCMode from selfdrive.car.tesla.PCC_module import PCCModes @@ -7,9 +7,10 @@ from selfdrive.car.modules.UIBT_module import UIButtons from selfdrive.car.modules.UIEV_module import UIEvents from selfdrive.car.tesla.readconfig import read_config_file +from selfdrive.car.interfaces import CarStateBase import os import subprocess -from common.params import read_db, write_db +from common.params import Params def parse_gear_shifter(can_gear_shifter, car_fingerprint): @@ -150,7 +151,6 @@ def get_can_signals(CP): return signals, checks def get_epas_can_signals(CP): -# this function generates lists for signal, messages and initial values signals = [ ("EPAS_torsionBarTorque", "EPAS_sysStatus", 0), # Used in interface.py ("EPAS_eacStatus", "EPAS_sysStatus", 0), @@ -158,12 +158,17 @@ def get_epas_can_signals(CP): ("EPAS_handsOnLevel", "EPAS_sysStatus", 0), ("EPAS_steeringFault", "EPAS_sysStatus", 0), ("EPAS_internalSAS", "EPAS_sysStatus", 0), #BB see if this works better than STW_ANGLHP_STAT for angle + ("SDM_bcklDrivStatus", "SDM1", 0), + ("LoBm_On_Rq","BODY_R1" , 0), + ("HiBm_On", "BODY_R1", 0), + ("LgtSens_Night", "BODY_R1", 0), ] checks = [ ("EPAS_sysStatus", 12), #JCT Actual message freq is 1.3 Hz (0.76 sec) ] + #checks = [] return signals, checks @@ -179,20 +184,10 @@ def get_pedal_can_signals(CP): checks = [] return signals, checks -def get_can_parser(CP,mydbc): - signals, checks = get_can_signals(CP) - return CANParser(mydbc, signals, checks, 0) - -def get_epas_parser(CP,epascan): - signals, checks = get_epas_can_signals(CP) - return CANParser(DBC[CP.carFingerprint]['pt']+"_epas", signals, checks, epascan) - -def get_pedal_parser(CP): - signals, checks = get_pedal_can_signals(CP) - return CANParser(DBC[CP.carFingerprint]['pt']+"_pedal", signals, checks, 2) - -class CarState(): +class CarState(CarStateBase): def __init__(self, CP): + super().__init__(CP) + self.params = Params() self.speed_control_enabled = 0 self.CL_MIN_V = 8.9 self.CL_MAX_A = 20. @@ -207,6 +202,7 @@ def __init__(self, CP): ### START OF MAIN CONFIG OPTIONS ### ### Do NOT modify here, modify in /data/bb_openpilot.cfg and reboot self.forcePedalOverCC = True + self.usesApillarHarness = False self.enableHSO = True self.enableALCA = True self.enableDasEmulation = True @@ -238,6 +234,13 @@ def __init__(self, CP): self.ldwNumbPeriod = 1.5 self.tapBlinkerExtension = 2 self.ahbOffDuration = 5 + self.roadCameraID = "" + self.driverCameraID = "" + self.roadCameraFx = 0.73 + self.driverCameraFx = 0.75 + self.roadCameraFlip = 0 + self.driverCameraFlip = 0 + self.monitorForcedRes = "" #read config file read_config_file(self) ### END OF MAIN CONFIG OPTIONS ### @@ -250,7 +253,7 @@ def __init__(self, CP): # Tesla Model self.teslaModelDetected = 1 - self.teslaModel = read_db('/data/params','TeslaModel') + self.teslaModel = self.params.get('TeslaModel') if self.teslaModel is not None: self.teslaModel = self.teslaModel.decode() if self.teslaModel is None: @@ -420,6 +423,28 @@ def __init__(self, CP): self.ahbLoBeamOn = 0 self.ahbHiBeamOn = 0 self.ahbNightMode = 0 + + @staticmethod + def get_can_parser(CP): + signals, checks = get_can_signals(CP) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) + + @staticmethod + def get_can_parser2(CP,mydbc): + signals, checks = get_can_signals(CP) + return CANParser(mydbc, signals, checks, 0) + + @staticmethod + def get_epas_parser(CP,epascan): + signals, checks = get_epas_can_signals(CP) + return CANParser(DBC[CP.carFingerprint]['pt']+"_epas", signals, checks, epascan) + + @staticmethod + def get_pedal_parser(CP,pedalcan): + signals, checks = get_pedal_can_signals(CP) + return CANParser(DBC[CP.carFingerprint]['pt']+"_pedal", signals, checks, pedalcan) + + def config_ui_buttons(self, pcc_available, pcc_blocked_by_acc_mode): if pcc_available: @@ -489,7 +514,10 @@ def update(self, cp, epas_cp, pedal_cp): # ******************* parse out can ******************* self.door_all_closed = not any([cp.vl["GTW_carState"]['DOOR_STATE_FL'], cp.vl["GTW_carState"]['DOOR_STATE_FR'], cp.vl["GTW_carState"]['DOOR_STATE_RL'], cp.vl["GTW_carState"]['DOOR_STATE_RR']]) #JCT - self.seatbelt = cp.vl["SDM1"]['SDM_bcklDrivStatus'] + if self.usesApillarHarness: + self.seatbelt = epas_cp.vl["SDM1"]['SDM_bcklDrivStatus'] + else: + self.seatbelt = cp.vl["SDM1"]['SDM_bcklDrivStatus'] #self.seatbelt = cp.vl["SDM1"]['SDM_bcklDrivStatus'] and cp.vl["GTW_status"]['GTW_driverPresent'] if (cp.vl["GTW_carConfig"]['GTW_performanceConfig']) and (cp.vl["GTW_carConfig"]['GTW_performanceConfig'] > 0): prev_teslaModel = self.teslaModel @@ -499,7 +527,7 @@ def update(self, cp, epas_cp, pedal_cp): if (cp.vl["GTW_carConfig"]['GTW_fourWheelDrive'] == 1): self.teslaModel = self.teslaModel + "D" if (self.teslaModelDetected == 0) or (prev_teslaModel != self.teslaModel): - write_db('/data/params','TeslaModel',self.teslaModel) + self.params.put('TeslaModel',self.teslaModel) self.teslaModelDetected = 1 #Nav Map Data @@ -538,9 +566,14 @@ def update(self, cp, epas_cp, pedal_cp): #AHB info self.ahbHighBeamStalkPosition = cp.vl["STW_ACTN_RQ"]["HiBmLvr_Stat"] self.ahbEnabled = cp.vl["MCU_chassisControl"]["MCU_ahlbEnable"] - self.ahbLoBeamOn = cp.vl["BODY_R1"]["LoBm_On_Rq"] - self.ahbHiBeamOn = cp.vl["BODY_R1"]["HiBm_On"] - self.ahbNightMode = cp.vl["BODY_R1"]["LgtSens_Night"] + if self.usesApillarHarness: + self.ahbLoBeamOn = epas_cp.vl["BODY_R1"]["LoBm_On_Rq"] + self.ahbHiBeamOn = epas_cp.vl["BODY_R1"]["HiBm_On"] + self.ahbNightMode = epas_cp.vl["BODY_R1"]["LgtSens_Night"] + else: + self.ahbLoBeamOn = cp.vl["BODY_R1"]["LoBm_On_Rq"] + self.ahbHiBeamOn = cp.vl["BODY_R1"]["HiBm_On"] + self.ahbNightMode = cp.vl["BODY_R1"]["LgtSens_Night"] usu = cp.vl['UI_gpsVehicleSpeed']["UI_userSpeedOffsetUnits"] if usu == 1: @@ -627,7 +660,6 @@ def update(self, cp, epas_cp, pedal_cp): self.prev_turn_signal_stalk_state = self.turn_signal_stalk_state self.turn_signal_stalk_state = 0 if cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat'] == 3 else int(cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat']) - self.park_brake = 0 # TODO self.brake_hold = 0 # TODO self.main_on = 1 #cp.vl["SCM_BUTTONS"]['MAIN_ON'] @@ -635,6 +667,7 @@ def update(self, cp, epas_cp, pedal_cp): if self.imperial_speed_units: self.DI_cruiseSet = self.DI_cruiseSet * CV.MPH_TO_KPH self.gear_shifter = parse_gear_shifter(can_gear_shifter, self.CP.carFingerprint) + self.park_brake = self.gear_shifter == 'park' # TODO self.pedal_gas = 0. # cp.vl["DI_torque1"]['DI_pedalPos'] / 102 #BB: to make it between 0..1 self.car_gas = self.pedal_gas diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 270ca8db2528ef..1b19227bd004dd 100644 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -5,14 +5,12 @@ from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.car.tesla.carstate import CarState, get_can_parser, get_epas_parser, get_pedal_parser from selfdrive.car.tesla.values import CruiseButtons, CM, BP, AH, CAR,DBC from common.params import read_db -from selfdrive.car import STD_CARGO_KG +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.car.tesla.readconfig import CarSettings -import selfdrive.messaging as messaging -from selfdrive.services import service_list from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V +from selfdrive.car.interfaces import CarInterfaceBase A_ACC_MAX = max(_A_CRUISE_MAX_V) AudibleAlert = car.CarControl.HUDControl.AudibleAlert @@ -21,12 +19,11 @@ K_MULT = 0.8 K_MULTi = 280000. -def tesla_compute_gb(accel, speed): - return float(accel) / 3. -class CarInterface(): - def __init__(self, CP, CarController): +class CarInterface(CarInterfaceBase): + def __init__(self, CP, CarController, CarState): + super().__init__(CP, CarController, CarState) self.CP = CP self.frame = 0 @@ -43,20 +40,25 @@ def __init__(self, CP, CarController): mydbc = DBC[CP.carFingerprint]['pt'] if CP.carFingerprint == CAR.MODELS and self.CS.fix1916: mydbc = mydbc + "1916" - self.cp = get_can_parser(CP,mydbc) + self.cp = self.CS.get_can_parser2(CP,mydbc) self.epas_cp = None + self.pedal_cp = None if self.CS.useWithoutHarness: - self.epas_cp = get_epas_parser(CP,0) + self.epas_cp = self.CS.get_epas_parser(CP,0) + self.pedal_cp = self.CS.get_pedal_parser(CP,0) else: - self.epas_cp = get_epas_parser(CP,2) - self.pedal_cp = get_pedal_parser(CP) + self.epas_cp = self.CS.get_epas_parser(CP,2) + self.pedal_cp = self.CS.get_pedal_parser(CP,2) self.CC = None if CarController is not None: - self.CC = CarController(self.cp.dbc_name) + self.CC = CarController(self.cp.dbc_name,CP,self.VM) + + + @staticmethod + def compute_gb(accel, speed): + return float(accel) / 3. - self.compute_gb = tesla_compute_gb - @staticmethod @@ -91,18 +93,17 @@ def calc_accel_override(a_ego, a_target, v_ego, v_target): return float(max(max_accel, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter) @staticmethod - def get_params(candidate, fingerprint, vin="", is_panda_black=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): # Scaled tire stiffness ts_factor = 8 - ret = car.CarParams.new_message() + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "tesla" ret.carFingerprint = candidate - ret.isPandaBlack = is_panda_black - teslaModel = read_db('/data/params','TeslaModel') + teslaModel = read_db('/data/params/d/','TeslaModel') if teslaModel is not None: teslaModel = teslaModel.decode() if teslaModel is None: @@ -110,7 +111,7 @@ def get_params(candidate, fingerprint, vin="", is_panda_black=False): ret.safetyModel = car.CarParams.SafetyModel.tesla ret.safetyParam = 1 - ret.carVin = vin + ret.carVin = "TESLAFAKEVIN12345" ret.enableCamera = True ret.enableGasInterceptor = False #keep this False for now @@ -146,31 +147,31 @@ def get_params(candidate, fingerprint, vin="", is_panda_black=False): # Kp and Ki for the longitudinal control if teslaModel == "S": - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kpV = [0.50, 0.45, 0.4] - ret.longitudinalTuning.kiBP = [0., 5., 35.] - ret.longitudinalTuning.kiV = [0.01,0.01,0.01] + ret.longitudinalTuning.kpBP = [0., 5., 22., 35.] + ret.longitudinalTuning.kpV = [0.50, 0.45, 0.4, 0.4] + ret.longitudinalTuning.kiBP = [0., 5., 22., 35.] + ret.longitudinalTuning.kiV = [0.01,0.01,0.01,0.01] elif teslaModel == "SP": - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kpV = [0.375, 0.325, 0.325] - ret.longitudinalTuning.kiBP = [0., 5., 35.] - ret.longitudinalTuning.kiV = [0.00915,0.00825,0.00725] + ret.longitudinalTuning.kpBP = [0., 5., 22., 35.] # 0km/h, 18 km/h, 80, 128km/h + ret.longitudinalTuning.kiBP = [0., 5., 22., 35.] + ret.longitudinalTuning.kpV = [0.3, 0.3, 0.35, 0.37] + ret.longitudinalTuning.kiV = [0.07, 0.07, 0.093, 0.092] elif teslaModel == "SD": - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kpV = [0.50, 0.45, 0.4] - ret.longitudinalTuning.kiBP = [0., 5., 35.] - ret.longitudinalTuning.kiV = [0.01,0.01,0.01] + ret.longitudinalTuning.kpBP = [0., 5., 22., 35.] + ret.longitudinalTuning.kpV = [0.50, 0.45, 0.4,0.4] + ret.longitudinalTuning.kiBP = [0., 5., 22., 35.] + ret.longitudinalTuning.kiV = [0.01,0.01,0.01,0.01] elif teslaModel == "SPD": - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kpV = [0.375, 0.325, 0.325] - ret.longitudinalTuning.kiBP = [0., 5., 35.] - ret.longitudinalTuning.kiV = [0.00915,0.00825,0.00725] + ret.longitudinalTuning.kpBP = [0., 5., 22., 35.] + ret.longitudinalTuning.kpV = [0.375, 0.325, 0.325, 0.325] + ret.longitudinalTuning.kiBP = [0., 5., 22.,35.] + ret.longitudinalTuning.kiV = [0.00915,0.00825,0.00725, 0.00725] else: #use S numbers if we can't match anything - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kpV = [0.375, 0.325, 0.3] - ret.longitudinalTuning.kiBP = [0., 5., 35.] - ret.longitudinalTuning.kiV = [0.08,0.08,0.08] + ret.longitudinalTuning.kpBP = [0., 5., 22., 35.] + ret.longitudinalTuning.kpV = [0.375, 0.325, 0.3, 0.3] + ret.longitudinalTuning.kiBP = [0., 5., 22., 35.] + ret.longitudinalTuning.kiV = [0.08,0.08,0.08, 0.08] else: @@ -204,13 +205,13 @@ def get_params(candidate, fingerprint, vin="", is_panda_black=False): ret.steerMaxBP = [0.,15.] # m/s ret.steerMaxV = [420.,420.] # max steer allowed - ret.gasMaxBP = [0.] # m/s - ret.gasMaxV = [0.3] #if ret.enableGasInterceptor else [0.] # max gas allowed - ret.brakeMaxBP = [0., 20.] # m/s - ret.brakeMaxV = [1., 1.] # max brake allowed - BB: since we are using regen, make this even + ret.gasMaxBP = [0., 20.] # m/s + ret.gasMaxV = [0.225, 0.525] #if ret.enableGasInterceptor else [0.] # max gas allowed + ret.brakeMaxBP = [0.] # m/s + ret.brakeMaxV = [1.] # max brake allowed - BB: since we are using regen, make this even - ret.longitudinalTuning.deadzoneBP = [0., 9.] #BB: added from Toyota to start pedal work; need to tune - ret.longitudinalTuning.deadzoneV = [0., 0.] #BB: added from Toyota to start pedal work; need to tune; changed to 0 for now + ret.longitudinalTuning.deadzoneBP = [0.] #BB: added from Toyota to start pedal work; need to tune + ret.longitudinalTuning.deadzoneV = [0.] #BB: added from Toyota to start pedal work; need to tune; changed to 0 for now ret.stoppingControl = True ret.openpilotLongitudinalControl = True @@ -219,6 +220,7 @@ def get_params(candidate, fingerprint, vin="", is_panda_black=False): ret.steerRateCost = 1.0 ret.radarOffCan = not CarSettings().get_value("useTeslaRadar") + ret.radarTimeStep = 0.05 #20Hz return ret @@ -391,6 +393,8 @@ def update(self, c, can_strings): self.CS.DAS_notInDrive = 1 if self.CC.opState == 1: self.CC.opState = 0 + if ret.gearShifter == 'drive': + self.CS.DAS_notInDrive =0 if self.CS.brake_hold: events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CC.opState == 1: @@ -417,8 +421,8 @@ def update(self, c, can_strings): #else: # if ret.brakePressed: # events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - if ret.gasPressed: - events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) + #if ret.gasPressed: + # events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low diff --git a/selfdrive/car/tesla/radar_interface.py b/selfdrive/car/tesla/radar_interface.py index d003c238fdd865..b570bcf59dd276 100644 --- a/selfdrive/car/tesla/radar_interface.py +++ b/selfdrive/car/tesla/radar_interface.py @@ -2,13 +2,14 @@ import os import time from cereal import car, tesla -from selfdrive.can.parser import CANParser -from common.realtime import DT_RDR -from selfdrive.services import service_list -import selfdrive.messaging as messaging +from opendbc.can.parser import CANParser +from cereal.services import service_list +import cereal.messaging as messaging from selfdrive.car.interfaces import RadarInterfaceBase from selfdrive.car.tesla.readconfig import CarSettings from selfdrive.tinklad.tinkla_interface import TinklaClient +from selfdrive.car.interfaces import RadarInterfaceBase + BOSCH_MAX_DIST = 250. #max distance for radar #use these for tracks (5 tracks) @@ -67,18 +68,21 @@ class RadarInterface(RadarInterfaceBase): tinklaClient = TinklaClient() def __init__(self,CP): - super().__init__(self) # radar self.pts = {} self.extPts = {} - self.delay = int(0.1 / DT_RDR) - self.useTeslaRadar = CarSettings().get_value("useTeslaRadar") + self.delay = 0 self.TRACK_LEFT_LANE = True self.TRACK_RIGHT_LANE = True self.updated_messages = set() self.canErrorCounter = 0 self.AHB_car_detected = False - if self.useTeslaRadar: + self.track_id = 0 + self.radar_fault = False + self.radar_wrong_config = False + self.radar_off_can = CP.radarOffCan + self.radar_ts = CP.radarTimeStep + if not self.radar_off_can: self.pts = {} self.extPts = {} self.valid_cnt = {key: 0 for key in RADAR_A_MSGS} @@ -88,12 +92,13 @@ def __init__(self,CP): self.trigger_start_msg = RADAR_A_MSGS[0] self.trigger_end_msg = RADAR_B_MSGS[-1] - + self.delay = int(round(0.1 / CP.radarTimeStep)) # 0.1s delay of radar def update(self, can_strings,v_ego): # radard at 20Hz and return no points - if not self.useTeslaRadar: - time.sleep(0.05) + if self.radar_off_can: + if 'NO_RADAR_SLEEP' not in os.environ: + time.sleep(self.radar_ts) return car.RadarData.new_message(),self.extPts.values(),self.AHB_car_detected if can_strings is not None: diff --git a/selfdrive/car/tesla/radar_tools/calibrateRadar.py b/selfdrive/car/tesla/radar_tools/calibrateRadar.py index 9f8c660df45443..a98c551ee286b8 100644 --- a/selfdrive/car/tesla/radar_tools/calibrateRadar.py +++ b/selfdrive/car/tesla/radar_tools/calibrateRadar.py @@ -8,10 +8,10 @@ from cereal import car import time import os -from selfdrive.can.parser import CANParser +from opendbc.can.parser import CANParser from common.realtime import sec_since_boot -from selfdrive.services import service_list -import selfdrive.messaging as messaging +from cereal.services import service_list +import cereal.messaging as messaging from selfdrive.car.tesla.readconfig import read_config_file,CarSettings from selfdrive.car.tesla.radar_interface import RadarInterface diff --git a/selfdrive/car/tesla/readconfig.py b/selfdrive/car/tesla/readconfig.py index 7974c0d6cc9dda..759d4cff1b458c 100644 --- a/selfdrive/car/tesla/readconfig.py +++ b/selfdrive/car/tesla/readconfig.py @@ -1,6 +1,9 @@ import configparser +from common.params import Params +import subprocess +from common.basedir import BASEDIR -default_config_file_path = '/data/bb_openpilot.cfg' +default_config_file_path = '%s/../bb_openpilot.cfg' % BASEDIR class ConfigFile(): config_file_r = 'r' @@ -10,6 +13,7 @@ class ConfigFile(): def read(self, into, config_path): configr = configparser.RawConfigParser() file_changed = False + params = Params() try: configr.read(config_path) @@ -22,10 +26,12 @@ def read(self, into, config_path): main_section = 'OP_CONFIG' pref_section = 'OP_PREFERENCES' + jetson_section = 'JETSON_PREFERENCES' logging_section = 'LOGGING' config = configparser.RawConfigParser(allow_no_value=True) config.add_section(main_section) config.add_section(pref_section) + config.add_section(jetson_section) config.add_section(logging_section) #user_handle -> userHandle @@ -46,6 +52,15 @@ def read(self, into, config_path): ) file_changed |= didUpdate + #uses_a_pillar_harness -> usesApillarHarness + into.usesApillarHarness, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = main_section, + entry = 'uses_a_pillar_harness', entry_type = bool, + default_value = False, + comment = 'Enable when using the new A pillar harness.' + ) + file_changed |= didUpdate + #force_pedal_over_cc -> forcePedalOverCC into.forcePedalOverCC, didUpdate = self.read_config_entry( config, configr, prev_file_contents, section = main_section, @@ -208,6 +223,7 @@ def read(self, into, config_path): comment = 'If you use an aftermarket Tesla Bosch Radar that already has a coded VIN, you will have to enter that VIN value here.' ) file_changed |= didUpdate + if into.radarVIN == '': into.radarVIN = default_radar_vin file_changed = True @@ -329,6 +345,90 @@ def read(self, into, config_path): ) file_changed |= didUpdate + #jetson_road_camera_id -> roadCameraID + into.roadCameraID, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = jetson_section, + entry = 'jetson_road_camera_id', entry_type = str, + default_value = 'NotSet', + comment = 'ID of camera facing road, as seen in ls -al /dev/v4l/by-id' + ) + file_changed |= didUpdate + + #jetson_driver_camera_id -> driverCameraID + into.driverCameraID, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = jetson_section, + entry = 'jetson_driver_camera_id', entry_type = str, + default_value = 'NotSet', + comment = 'ID of camera facing driver, as seen in ls -al /dev/v4l/by-id' + ) + file_changed |= didUpdate + + #jetson_road_camera_fx -> roadCameraFx + into.roadCameraFx, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = jetson_section, + entry = 'jetson_road_camera_fx', entry_type = float, + default_value = 0.73, + comment = 'Focal length correction factor to match the EON camera view window' + ) + file_changed |= didUpdate + + #jetson_driver_camera_fx -> driverCameraFx + into.driverCameraFx, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = jetson_section, + entry = 'jetson_driver_camera_fx', entry_type = float, + default_value = 0.75, + comment = 'Focal length correction factor to match the EON camera view window' + ) + file_changed |= didUpdate + + #jetson_driver_camera_flip -> driverCameraFlip + into.driverCameraFlip, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = jetson_section, + entry = 'jetson_driver_camera_flip', entry_type = int, + default_value = 0, + comment = 'Flip the image for the driver camera' + ) + file_changed |= didUpdate + + #jetson_road_camera_flip -> roadCameraFlip + into.roadCameraFlip, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = jetson_section, + entry = 'jetson_road_camera_flip', entry_type = int, + default_value = 0, + comment = 'Flip the image for the road camera' + ) + file_changed |= didUpdate + + #jetson_monitor_forced_resolution -> monitorForcedRes + into.monitorForcedRes, didUpdate = self.read_config_entry( + config, configr, prev_file_contents, section = jetson_section, + entry = 'jetson_monitor_forced_resolution', entry_type = str, + default_value = "-rez 1280 720", + comment = 'To use the Pi 7" HDMI screen this has to be set to "-rez 800 480"' + ) + file_changed |= didUpdate + + #check camera_id values against LiveParams + savedRoadCameraID = params.get("RoadUsbCameraID") + savedDriverCameraID = params.get("DriverUsbCameraID") + savedRoadCameraFx = params.get("RoadUsbCameraFx") + savedDriverCameraFx = params.get("DriverUsbCameraFx") + savedRoadCameraFlip = params.get("RoadUsbCameraFlip") + savedDriverCameraFlip = params.get("DriverUsbCameraFlip") + if into.driverCameraID != savedDriverCameraID: + params.put("DriverUsbCameraID",into.driverCameraID) + if into.roadCameraID != savedRoadCameraID: + params.put("RoadUsbCameraID",into.roadCameraID) + if into.driverCameraFx != savedDriverCameraFx: + params.put("DriverUsbCameraFx","%f" % into.driverCameraFx) + if into.roadCameraFx != savedRoadCameraFx: + params.put("RoadUsbCameraFx","%f" % into.roadCameraFx) + if into.driverCameraFlip != savedDriverCameraFlip: + params.put("DriverUsbCameraFlip","%d" % into.driverCameraFlip) + if into.roadCameraFlip != savedRoadCameraFlip: + params.put("RoadUsbCameraFlip","%d" % into.roadCameraFlip) + + into.shouldLogCanErrors, didUpdate = self.read_config_entry( config, configr, prev_file_contents, section = logging_section, entry = 'should_log_can_errors', entry_type = bool, @@ -417,6 +517,14 @@ class CarSettings(): ldwNumbPeriod = None tapBlinkerExtension = None ahbOffDuration = None + usesApillarHarness = None + roadCameraID = None + driverCameraID = None + roadCameraFx = None + driverCameraFx = None + roadCameraFlip = None + driverCameraFlip = None + monitorForcedRes = None def __init__(self, optional_config_file_path = default_config_file_path): config_file = ConfigFile() diff --git a/selfdrive/car/tesla/readconfig.sh b/selfdrive/car/tesla/readconfig.sh index 352ab9e66720f5..9146fab54430a2 100755 --- a/selfdrive/car/tesla/readconfig.sh +++ b/selfdrive/car/tesla/readconfig.sh @@ -1,4 +1,10 @@ -#!/usr/bin/bash -CFG_FILE=/data/bb_openpilot.cfg -CFG_CONTENT=$(cat $CFG_FILE | sed -r '/[^=]+=[^=]+/!d' | sed -r 's/\s+=\s/=/g') -eval "$CFG_CONTENT" +if [ -z "$BASEDIR" ]; then + BASEDIR="/data/openpilot" +fi +CFG_FILE="$BASEDIR/../bb_openpilot.cfg" +CFG_CONTENT=$(cat $CFG_FILE | sed -r "s/'/SINGLE_Q/" | sed -r '/[^=]+=[^=]+/!d' | sed -r 's/\s+=\s/=/g' | sed -e 's/[[:space:]]*\=[[:space:]]*/=/g' \ + -e 's/#.*$//' \ + -e 's/[[:space:]]*$//' \ + -e 's/^[[:space:]]*//' \ + -e "s/^\(.*\)=\([^\"']*\)$/\1=\"\2\"/" | sed -r "s/SINGLE_Q/'/" ) +eval "export $CFG_CONTENT" diff --git a/selfdrive/car/tesla/speed_utils/movingaverage.py b/selfdrive/car/tesla/speed_utils/movingaverage.py index 07ba16f463a116..5281f40ff2f1a7 100644 --- a/selfdrive/car/tesla/speed_utils/movingaverage.py +++ b/selfdrive/car/tesla/speed_utils/movingaverage.py @@ -1,49 +1,20 @@ +import queue + class MovingAverage(): def __init__(self, length): - self.position = 0 self.length = length - self.sum = 0. - self.no_items = 0 - self.values = [0.] * length - + self.reset() + def reset(self): - self.position = 0 - self.sum = 0. - self.no_items = 0 - self.values = [0.] * self.length + self.queue = queue.Queue(maxsize=self.length) + self.sum = 0 - def add(self,element): - if self.no_items == self.length: - self.no_items -= 1 - self.sum -= self.values[self.position] - self.values[self.position] = element - self.sum += self.values[self.position] - self.no_items += 1 - self.position += 1 - if self.sum == 0.: - #all empty so initialize - self.position = 0 - self.sum = 0. - self.no_items = 0 - return 0. - self.position = self.position % self.length - return self.sum/self.no_items + def add(self, sample): + if self.queue.full(): + self.sum -= self.queue.get_nowait() + self.queue.put_nowait(sample) + self.sum += sample + return self.sum / self.queue.qsize() - def dele(self): - if self.no_items == 0: - return 0. - if self.no_items > 0: - self.no_items -= 1 - self.sum -= self.values[self.position] - self.values[self.position] = 0. - self.position -= 1 - if self.position < 0: - self.position = self.length-1 - if self.sum == 0. or self.no_items == 0.: - #all empty so initialize - self.position = 0 - self.sum = 0. - self.no_items = 0 - return 0. - self.position = self.position % self.length - return self.sum/self.no_items + def full(self): + return self.queue.full() diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py index 10823f303c5696..c7988633cba88f 100644 --- a/selfdrive/car/tesla/teslacan.py +++ b/selfdrive/car/tesla/teslacan.py @@ -37,7 +37,7 @@ def add_tesla_checksum(msg_id,msg): return checksum -def create_pedal_command_msg(accelCommand, enable, idx): +def create_pedal_command_msg(accelCommand, enable, idx, pedalcan): """Create GAS_COMMAND (0x551) message to comma pedal""" msg_id = 0x551 msg_len = 6 @@ -55,7 +55,7 @@ def create_pedal_command_msg(accelCommand, enable, idx): struct.pack_into('BBBBB', msg, 0, int((int_accelCommand >> 8) & 0xFF), int_accelCommand & 0xFF, \ int((int_accelCommand2 >> 8) & 0xFF), int_accelCommand2 & 0XFF,((enable << 7) + idx) & 0xFF) struct.pack_into('B', msg, msg_len-1, add_tesla_checksum(msg_id,msg)) - return [msg_id, 0, msg.raw, 2] + return [msg_id, 0, msg.raw, pedalcan] def create_enabled_eth_msg(status): msg_id = 0x018 @@ -206,7 +206,7 @@ def create_fake_DAS_warning(DAS_211_accNoSeatBelt, DAS_canErrors, \ DAS_202_noisyEnvironment, DAS_doorOpen, DAS_notInDrive, enableDasEmulation, enableRadarEmulation, \ stopSignWarning, stopLightWarning, \ DAS_222_accCameraBlind, DAS_219_lcTempUnavailableSpeed, DAS_220_lcTempUnavailableRoad, DAS_221_lcAborting, \ - DAS_207_lkasUnavailable,DAS_208_rackDetected, DAS_025_steeringOverride, ldwStatus,FLAG_notUsed,useWithoutHarness): + DAS_207_lkasUnavailable,DAS_208_rackDetected, DAS_025_steeringOverride, ldwStatus, useWithoutHarness,usesApillarHarness): msg_id = 0x554 msg_len = 3 fd = 0 @@ -219,9 +219,13 @@ def create_fake_DAS_warning(DAS_211_accNoSeatBelt, DAS_canErrors, \ wh = 0 if useWithoutHarness: wh = 1 + aph = 0 + if usesApillarHarness: + aph=1 + autoPilotAborting = 0 #not used at the moment warn1 = (stopLightWarning<< 7) + (rd << 6) + (fd << 5) + (DAS_211_accNoSeatBelt << 4) + (DAS_canErrors << 3) + (DAS_202_noisyEnvironment << 2) + (DAS_doorOpen << 1) + DAS_notInDrive warn2 = stopSignWarning + (DAS_222_accCameraBlind << 1) + (DAS_219_lcTempUnavailableSpeed << 2) + (DAS_220_lcTempUnavailableRoad << 3) + (DAS_221_lcAborting << 4) + (DAS_207_lkasUnavailable << 5) + (DAS_208_rackDetected << 6) + (DAS_025_steeringOverride << 7) - warn3 = ldwStatus + (FLAG_notUsed << 3) + (wh << 4) + warn3 = ldwStatus + (autoPilotAborting << 3) + (wh << 4) + (aph << 5) msg = create_string_buffer(msg_len) struct.pack_into('BBB',msg ,0 , warn1,warn2,warn3) return [msg_id,0,msg.raw,0] diff --git a/selfdrive/common/SConscript b/selfdrive/common/SConscript index 6f40e6a8f0a20e..c6c9f978464949 100644 --- a/selfdrive/common/SConscript +++ b/selfdrive/common/SConscript @@ -16,7 +16,14 @@ files = [ 'visionimg.cc', ] -if arch == "aarch64": + +if arch == "jarch64": + defines = {} #{"CLU_NO_CACHE": None} + files += [ + 'visionbuf_cl.c', + ] + _gpu_libs = ["GL"] +elif arch == "aarch64": defines = {} files += [ 'framebuffer.cc', diff --git a/selfdrive/common/framebuffer.cc b/selfdrive/common/framebuffer.cc index 788b812997f3f3..7bea565d5979b4 100644 --- a/selfdrive/common/framebuffer.cc +++ b/selfdrive/common/framebuffer.cc @@ -126,8 +126,11 @@ extern "C" FramebufferState* framebuffer_init( // set brightness int brightness_fd = open(BACKLIGHT_CONTROL, O_RDWR); - const char brightness_level[] = BACKLIGHT_LEVEL; - write(brightness_fd, brightness_level, strlen(brightness_level)); + if (brightness_fd != -1){ + const char brightness_level[] = BACKLIGHT_LEVEL; + write(brightness_fd, brightness_level, strlen(brightness_level)); + close(brightness_fd); + } if (out_w) *out_w = w; if (out_h) *out_h = h; diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc index 79bc5d911f4fe2..9ef6609af7ad80 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -292,9 +292,6 @@ int read_db_all(const char* params_path, std::map *par while ((de = readdir(d))) { if (!isalnum(de->d_name[0])) continue; std::string key = std::string(de->d_name); - - if (key == "AccessToken") continue; - std::string value = util::read_file(util::string_format("%s/%s", key_path.c_str(), key.c_str())); (*params)[key] = value; diff --git a/selfdrive/common/touch.c b/selfdrive/common/touch.c index 748dd37cf95c52..4527cb53201e67 100644 --- a/selfdrive/common/touch.c +++ b/selfdrive/common/touch.c @@ -96,7 +96,7 @@ int touch_read(TouchState *s, int* out_x, int* out_y) { struct input_event event; int err = read(s->fd, &event, sizeof(event)); if (err < sizeof(event)) { - return false; //-1; + return -1; } bool up = false; switch (event.type) { diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 714b71d8ea2d4d..1e0a0e0c42c140 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.7.4-release" +#define COMMA_VERSION "0.7.5" diff --git a/selfdrive/common/visionbuf.h b/selfdrive/common/visionbuf.h index 3457f6dff33949..8958bdd1d033a6 100644 --- a/selfdrive/common/visionbuf.h +++ b/selfdrive/common/visionbuf.h @@ -1,6 +1,7 @@ #ifndef IONBUF_H #define IONBUF_H +#define CL_USE_DEPRECATED_OPENCL_1_2_APIS #ifdef __APPLE__ #include #else diff --git a/selfdrive/common/visionbuf_cl.c b/selfdrive/common/visionbuf_cl.c index 333bed55c636c6..5e946498113273 100644 --- a/selfdrive/common/visionbuf_cl.c +++ b/selfdrive/common/visionbuf_cl.c @@ -8,6 +8,7 @@ #include #include +#define CL_USE_DEPRECATED_OPENCL_1_2_APIS #ifdef __APPLE__ #include #else diff --git a/selfdrive/common/visionbuf_ion.c b/selfdrive/common/visionbuf_ion.c index 724e75e9b0c0d5..e5dddfad2201bc 100644 --- a/selfdrive/common/visionbuf_ion.c +++ b/selfdrive/common/visionbuf_ion.c @@ -1,8 +1,12 @@ #include #include +#include #include #include #include +#include +#include +#include #include #include diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8c135782249614..02289176a6ca80 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -6,7 +6,7 @@ from common.numpy_fast import clip from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper, DT_CTRL from common.profiler import Profiler -from common.params import Params +from common.params import Params, put_nonblocking import cereal.messaging as messaging from selfdrive.config import Conversions as CV from selfdrive.boardd.boardd import can_list_to_can_capnp @@ -25,8 +25,12 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.planner import LON_MPC_STEP from selfdrive.locationd.calibration_helpers import Calibration, Filter +from selfdrive.tinklad.tinkla_interface import TinklaClient + LANE_DEPARTURE_THRESHOLD = 0.1 +STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL +STEER_ANGLE_SATURATION_THRESHOLD = 250 # Degrees ThermalStatus = log.ThermalData.ThermalStatus State = log.ControlsState.OpenpilotState @@ -114,6 +118,9 @@ def data_sample(CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter else: events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if CS.vEgo > 150 * CV.MPH_TO_MS: + events.append(create_event('speedTooHigh', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + # When the panda and controlsd do not agree on controls_allowed # we want to disengage openpilot. However the status from the panda goes through # another socket other than the CAN messages and one can arrive earlier than the other. @@ -218,7 +225,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, - AM, rk, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame): + AM, rk, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, saturated_count): """Given the state, this function returns an actuators packet""" actuators = car.CarControl.Actuators.new_message() @@ -266,8 +273,14 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr # Steering PID loop and lateral MPC actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CS.steeringRateLimited, CP, path_plan) + # Check for difference between desired angle and angle for angle based control + angle_control_saturated = CP.steerControlType == car.CarParams.SteerControlType.angle and \ + abs(actuators.steerAngle - CS.steeringAngle) > STEER_ANGLE_SATURATION_THRESHOLD + + saturated_count = saturated_count + 1 if angle_control_saturated and not CS.steeringPressed and active else 0 + # Send a "steering required alert" if saturation count has reached the limit - if lac_log.saturated and not CS.steeringPressed: + if (lac_log.saturated and not CS.steeringPressed) or (saturated_count > STEER_ANGLE_SATURATION_TIMEOUT): # Check if we deviated from the path left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1 right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1 @@ -286,7 +299,7 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph" AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2) - return actuators, v_cruise_kph, v_acc_sol, a_acc_sol, lac_log, last_blinker_frame + return actuators, v_cruise_kph, v_acc_sol, a_acc_sol, lac_log, last_blinker_frame, saturated_count def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, @@ -344,7 +357,7 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk can_sends = CI.apply(CC) pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) - force_decel = sm['dMonitoringState'].awarenessStatus < 0. + force_decel = (sm['dMonitoringState'].awarenessStatus < 0.) or (state == State.softDisabling) # controlsState dat = messaging.new_message('controlsState') @@ -426,6 +439,13 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk return CC, events_bytes +def logAllAliveAndValidInfoToTinklad(sm, tinklaClient): + areAllAlive,aliveProcessName,aliveCount = sm.all_alive_with_info() + areAllValid, validProcessName, validCount = sm.all_valid_with_info() + if not areAllAlive: + tinklaClient.logProcessCommErrorEvent(source="carcontroller", processName=aliveProcessName, count=aliveCount, eventType="Not Alive") + else: + tinklaClient.logProcessCommErrorEvent(source="carcontroller", processName=validProcessName, count=validCount, eventType="Not Valid") def controlsd_thread(sm=None, pm=None, can_sock=None): gc.disable() @@ -435,6 +455,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): params = Params() + tinklaClient = TinklaClient() is_metric = params.get("IsMetric", encoding='utf8') == "1" is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1" passive = params.get("Passive", encoding='utf8') == "1" @@ -443,6 +464,10 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): passive = passive or not openpilot_enabled_toggle + # Passive if internet needed + internet_needed = params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None + passive = passive or internet_needed + # Pub/Sub Sockets if pm is None: pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams']) @@ -451,7 +476,6 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'dMonitoringState', 'plan', 'pathPlan', \ 'model']) - if can_sock is None: can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100 can_sock = messaging.sub_sock('can', timeout=can_timeout) @@ -475,8 +499,8 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): # Write CarParams for radard and boardd safety mode cp_bytes = CP.to_bytes() params.put("CarParams", cp_bytes) - params.put("CarParamsCache", cp_bytes) - params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0") + put_nonblocking("CarParamsCache", cp_bytes) + put_nonblocking("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0") CC = car.CarControl.new_message() AM = AlertManager() @@ -501,6 +525,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): mismatch_counter = 0 can_error_counter = 0 last_blinker_frame = 0 + saturated_count = 0 events_prev = [] sm['liveCalibration'].calStatus = Calibration.INVALID @@ -517,7 +542,6 @@ 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) - internet_needed = params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None prof = Profiler(False) # off by default @@ -534,9 +558,10 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) elif not sm.all_alive_and_valid(): events.append(create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + logAllAliveAndValidInfoToTinklad(sm=sm, tinklaClient=tinklaClient) if not sm['pathPlan'].mpcSolutionValid: events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - if not sm['pathPlan'].sensorValid: + if not sm['pathPlan'].sensorValid and os.getenv("NOSENSOR") is None: events.append(create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT])) if not sm['pathPlan'].paramsValid: events.append(create_event('vehicleModelInvalid', [ET.WARNING])) @@ -548,6 +573,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): events.append(create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not CS.canValid: events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + tinklaClient.logCANErrorEvent(source="carcontroller", canMessage=0, additionalInformation="Invalid CAN") if not sounds_available: events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT])) if internet_needed: @@ -556,6 +582,9 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): events.append(create_event('communityFeatureDisallowed', [ET.PERMANENT])) if read_only and not passive: events.append(create_event('carUnrecognized', [ET.PERMANENT])) + if log.HealthData.FaultType.relayMalfunction in sm['health'].faults: + events.append(create_event('relayMalfunction', [ET.NO_ENTRY, ET.PERMANENT, ET.IMMEDIATE_DISABLE])) + # Only allow engagement with brake pressed when stopped behind another stopped car if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: @@ -568,9 +597,9 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) - actuators, v_cruise_kph, v_acc, a_acc, lac_log, last_blinker_frame = \ + actuators, v_cruise_kph, v_acc, a_acc, lac_log, last_blinker_frame, saturated_count = \ state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, - LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame) + LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, saturated_count) prof.checkpoint("State Control") diff --git a/selfdrive/controls/dmonitoringd.py b/selfdrive/controls/dmonitoringd.py index 86afb9dcff4283..c9c3704422dca8 100755 --- a/selfdrive/controls/dmonitoringd.py +++ b/selfdrive/controls/dmonitoringd.py @@ -1,12 +1,11 @@ #!/usr/bin/env python3 import gc from common.realtime import set_realtime_priority -from common.params import Params, put_nonblocking +from common.params import Params import cereal.messaging as messaging from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, MAX_TERMINAL_DURATION from selfdrive.locationd.calibration_helpers import Calibration -from selfdrive.controls.lib.gps_helpers import is_rhd_region def dmonitoringd_thread(sm=None, pm=None): gc.disable() @@ -21,12 +20,13 @@ def dmonitoringd_thread(sm=None, pm=None): pm = messaging.PubMaster(['dMonitoringState']) if sm is None: - sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'model', 'gpsLocation'], ignore_alive=['gpsLocation']) + sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'model']) driver_status = DriverStatus() is_rhd = params.get("IsRHD") if is_rhd is not None: driver_status.is_rhd_region = bool(int(is_rhd)) + driver_status.is_rhd_region_checked = True sm['liveCalibration'].calStatus = Calibration.INVALID sm['carState'].vEgo = 0. @@ -44,13 +44,6 @@ def dmonitoringd_thread(sm=None, pm=None): while True: sm.update() - # GPS coords RHD parsing, once every restart - if not driver_status.is_rhd_region_checked and sm.updated['gpsLocation']: - is_rhd = is_rhd_region(sm['gpsLocation'].latitude, sm['gpsLocation'].longitude) - driver_status.is_rhd_region = is_rhd - driver_status.is_rhd_region_checked = True - put_nonblocking("IsRHD", "1" if is_rhd else "0") - # Handle calibration if sm.updated['liveCalibration']: if sm['liveCalibration'].calStatus == Calibration.CALIBRATED: @@ -99,6 +92,7 @@ def dmonitoringd_thread(sm=None, pm=None): "awarenessPassive": driver_status.awareness_passive, "isLowStd": driver_status.pose.low_std, "hiStdCount": driver_status.hi_stds, + "isPreview": False, } pm.send('dMonitoringState', dat) diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 88fb8fbb03b0f3..bd77d32282b75b 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -110,7 +110,7 @@ def __gt__(self, alert2): Alert( "preDriverDistracted", - "KEEP EYES ON ROAD: Driver Appears Distracted", + "KEEP EYES ON ROAD: Driver Distracted", "", AlertStatus.normal, AlertSize.small, Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75), @@ -120,7 +120,7 @@ def __gt__(self, alert2): "KEEP EYES ON ROAD", "Driver Appears Distracted", AlertStatus.userPrompt, AlertSize.mid, - Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1), + Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2Repeat, .1, .1, .1), Alert( "driverDistracted", @@ -141,7 +141,7 @@ def __gt__(self, alert2): "TOUCH STEERING WHEEL", "Driver Is Unresponsive", AlertStatus.userPrompt, AlertSize.mid, - Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1), + Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2Repeat, .1, .1, .1), Alert( "driverUnresponsive", @@ -188,7 +188,7 @@ def __gt__(self, alert2): Alert( "startupNoCar", "Dashcam mode with unsupported car", - "Always keep hands on wheel and eyes on road", + "If Tesla, please force fingerprint", AlertStatus.normal, AlertSize.mid, Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), @@ -272,7 +272,7 @@ def __gt__(self, alert2): Alert( "dataNeededNoEntry", "openpilot Unavailable", - "Data Needed for Calibration. Upload Drive, Try Again", + "Calibration Needs Data. Upload Drive, Try Again", AlertStatus.normal, AlertSize.mid, Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 0., 3.), @@ -503,6 +503,14 @@ def __gt__(self, alert2): AlertStatus.critical, AlertSize.full, Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.), + Alert( + "relayMalfunction", + "TAKE CONTROL IMMEDIATELY", + "Harness Malfunction", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.), + + # not loud cancellations (user is in control) Alert( "noTarget", @@ -518,6 +526,13 @@ def __gt__(self, alert2): AlertStatus.normal, AlertSize.mid, Priority.HIGH, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.), + Alert( + "speedTooHigh", + "Speed Too High", + "Slow down to resume operation", + AlertStatus.normal, AlertSize.mid, + Priority.HIGH, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.), + # Cancellation alerts causing non-entry Alert( "overheatNoEntry", @@ -536,7 +551,7 @@ def __gt__(self, alert2): Alert( "calibrationInvalidNoEntry", "openpilot Unavailable", - "Calibration Invalid: Reposition Device and Recalibrate", + "Calibration Invalid: Reposition Device & Recalibrate", AlertStatus.normal, AlertSize.mid, Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), @@ -687,6 +702,20 @@ def __gt__(self, alert2): AlertStatus.normal, AlertSize.mid, Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.), + Alert( + "speedTooHighNoEntry", + "Speed Too High", + "Slow down to engage", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "relayMalfunctionNoEntry", + "openpilot Unavailable", + "Harness Malfunction", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + # permanent alerts Alert( "steerUnavailablePermanent", @@ -723,6 +752,13 @@ def __gt__(self, alert2): AlertStatus.normal, AlertSize.mid, Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Alert( + "invalidLkasSettingPermanent", + "Stock LKAS is turned on", + "Turn off stock LKAS to engage", + AlertStatus.normal, AlertSize.mid, + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Alert( "internetConnectivityNeededPermanent", "Please connect to Internet", @@ -765,6 +801,13 @@ def __gt__(self, alert2): AlertStatus.normal, AlertSize.mid, Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Alert( + "relayMalfunctionPermanent", + "Harness Malfunction", + "Please Check Hardware", + AlertStatus.normal, AlertSize.mid, + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Alert( "vehicleModelInvalid", "Vehicle Parameter Identification Failed", diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index 8f0dbda706b611..73ef07777ea34f 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -5,6 +5,12 @@ from common.filter_simple import FirstOrderFilter from common.stat_live import RunningStatFilter +# ****************************************************************************************** +# NOTE: To fork maintainers. +# Disabling or nerfing safety features may get you and your users banned from our servers. +# We recommend that you do not change these numbers from the defaults. +# ****************************************************************************************** + _AWARENESS_TIME = 70. # one minute limit without user touching steering wheels make the car enter a terminal status _AWARENESS_PRE_TIME_TILL_TERMINAL = 15. # a first alert is issued 25s before expiration _AWARENESS_PROMPT_TIME_TILL_TERMINAL = 6. # a second alert is issued 15s before start decelerating the car @@ -31,8 +37,8 @@ _DISTRACTED_FILTER_TS = 0.25 # 0.6Hz _POSE_CALIB_MIN_SPEED = 13 # 30 mph -_POSE_OFFSET_MIN_COUNT = 60 # valid data counts before calibration completes, 1 seg is 600 counts -_POSE_OFFSET_MAX_COUNT = 360 # stop deweighting new data after 6 min, aka "short term memory" +_POSE_OFFSET_MIN_COUNT = 600 # valid data counts before calibration completes, 1 seg is 600 counts +_POSE_OFFSET_MAX_COUNT = 3600 # stop deweighting new data after 6 min, aka "short term memory" _RECOVERY_FACTOR_MAX = 5. # relative to minus step change _RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change @@ -49,7 +55,7 @@ class DistractedType(): BAD_POSE = 1 BAD_BLINK = 2 -def face_orientation_from_net(angles_desc, pos_desc, rpy_calib): +def face_orientation_from_net(angles_desc, pos_desc, rpy_calib, is_rhd): # the output of these angles are in device frame # so from driver's perspective, pitch is up and yaw is right @@ -67,7 +73,7 @@ def face_orientation_from_net(angles_desc, pos_desc, rpy_calib): # no calib for roll pitch -= rpy_calib[1] - yaw -= rpy_calib[2] + yaw -= rpy_calib[2] * (1 - 2 * int(is_rhd)) # lhd -> -=, rhd -> += return roll, pitch, yaw class DriverPose(): @@ -174,7 +180,7 @@ def get_pose(self, driver_state, cal_rpy, car_speed, op_engaged): if len(driver_state.faceOrientation) == 0 or len(driver_state.facePosition) == 0 or len(driver_state.faceOrientationStd) == 0 or len(driver_state.facePositionStd) == 0: return - self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_state.faceOrientation, driver_state.facePosition, cal_rpy) + self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_state.faceOrientation, driver_state.facePosition, cal_rpy, self.is_rhd_region) self.pose.pitch_std = driver_state.faceOrientationStd[0] self.pose.yaw_std = driver_state.faceOrientationStd[1] # self.pose.roll_std = driver_state.faceOrientationStd[2] @@ -183,8 +189,7 @@ def get_pose(self, driver_state, cal_rpy, car_speed, op_engaged): self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb>_EYE_THRESHOLD) self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb>_EYE_THRESHOLD) self.face_detected = driver_state.faceProb > _FACE_THRESHOLD and \ - abs(driver_state.facePosition[0]) <= 0.4 and abs(driver_state.facePosition[1]) <= 0.45 and \ - not self.is_rhd_region + abs(driver_state.facePosition[0]) <= 0.4 and abs(driver_state.facePosition[1]) <= 0.45 self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0 # first order filters diff --git a/selfdrive/controls/lib/driverview.py b/selfdrive/controls/lib/driverview.py new file mode 100755 index 00000000000000..298858bd4dfd9a --- /dev/null +++ b/selfdrive/controls/lib/driverview.py @@ -0,0 +1,74 @@ +#!/usr/bin/env python3 +import os +import subprocess +import multiprocessing +import signal +import time + +import cereal.messaging as messaging +from common.params import Params + +from common.basedir import BASEDIR + +KILL_TIMEOUT = 15 + +def send_controls_packet(pm): + while True: + dat = messaging.new_message('controlsState') + dat.controlsState = { + "rearViewCam": True, + } + pm.send('controlsState', dat) + +def send_dmon_packet(pm, d): + dat = messaging.new_message('dMonitoringState') + dat.dMonitoringState = { + "isRHD": d[0], + "rhdChecked": d[1], + "isPreview": d[2], + } + pm.send('dMonitoringState', dat) + +def main(): + pm = messaging.PubMaster(['controlsState', 'dMonitoringState']) + controls_sender = multiprocessing.Process(target=send_controls_packet, args=[pm]) + controls_sender.start() + + # TODO: refactor with manager start/kill + proc_cam = subprocess.Popen(os.path.join(BASEDIR, "selfdrive/camerad/camerad"), cwd=os.path.join(BASEDIR, "selfdrive/camerad")) + proc_mon = subprocess.Popen(os.path.join(BASEDIR, "selfdrive/modeld/dmonitoringmodeld"), cwd=os.path.join(BASEDIR, "selfdrive/modeld")) + + params = Params() + is_rhd = False + is_rhd_checked = False + should_exit = False + + def terminate(signalNumber, frame): + print('got SIGTERM, exiting..') + should_exit = True + send_dmon_packet(pm, [is_rhd, is_rhd_checked, not should_exit]) + proc_cam.send_signal(signal.SIGINT) + proc_mon.send_signal(signal.SIGINT) + kill_start = time.time() + while proc_cam.poll() is None: + if time.time() - kill_start > KILL_TIMEOUT: + from selfdrive.swaglog import cloudlog + cloudlog.critical("FORCE REBOOTING PHONE!") + os.system("date >> /sdcard/unkillable_reboot") + os.system("reboot") + raise RuntimeError + continue + controls_sender.terminate() + exit() + + signal.signal(signal.SIGTERM, terminate) + + while True: + send_dmon_packet(pm, [is_rhd, is_rhd_checked, not should_exit]) + + if not is_rhd_checked: + is_rhd = params.get("IsRHD") == b"1" + is_rhd_checked = True + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index ff090bcd9758f8..891f20ad91f343 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -1,9 +1,11 @@ from common.numpy_fast import interp import numpy as np +from selfdrive.car.modules.ALCA_module import ALCAModelParser from cereal import log CAMERA_OFFSET = 0.06 # m from center car to camera + def compute_path_pinv(l=50): deg = 3 x = np.arange(l*1.0) @@ -16,11 +18,22 @@ def model_polyfit(points, path_pinv): return np.dot(path_pinv, [float(x) for x in points]) -def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width): +def eval_poly(poly, x): + return poly[3] + poly[2]*x + poly[1]*x**2 + poly[0]*x**3 + + +def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width, v_ego): # This will improve behaviour when lanes suddenly widen + # these numbers were tested on 2000segments and found to work well lane_width = min(4.0, lane_width) - l_prob = l_prob * interp(abs(l_poly[3]), [2, 2.5], [1.0, 0.0]) - r_prob = r_prob * interp(abs(r_poly[3]), [2, 2.5], [1.0, 0.0]) + width_poly = l_poly - r_poly + prob_mods = [] + for t_check in [0.0, 1.5, 3.0]: + width_at_t = eval_poly(width_poly, t_check * (v_ego + 7)) + prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0])) + mod = min(prob_mods) + l_prob = mod * l_prob + r_prob = mod * r_prob path_from_left_lane = l_poly.copy() path_from_left_lane[3] -= lane_width / 2.0 @@ -34,7 +47,7 @@ def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width): class LanePlanner(): - def __init__(self): + def __init__(self,shouldUseAlca): self.l_poly = [0., 0., 0., 0.] self.r_poly = [0., 0., 0., 0.] self.p_poly = [0., 0., 0., 0.] @@ -52,6 +65,9 @@ def __init__(self): self._path_pinv = compute_path_pinv() self.x_points = np.arange(50) + self.shouldUseAlca = shouldUseAlca + if shouldUseAlca: + self.ALCAMP = ALCAModelParser() def parse_model(self, md): if len(md.leftLane.poly): @@ -69,7 +85,7 @@ def parse_model(self, md): self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft - 1] self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight - 1] - def update_d_poly(self, v_ego): + def update_d_poly(self, v_ego, md, alca ): # only offset left and right lane lines; offsetting p_poly does not make sense self.l_poly[3] += CAMERA_OFFSET self.r_poly[3] += CAMERA_OFFSET @@ -82,8 +98,12 @@ def update_d_poly(self, v_ego): self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \ (1 - self.lane_width_certainty) * speed_lane_width - self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width) + # ALCA integration + if self.shouldUseAlca and alca: + self.r_poly,self.l_poly,self.r_prob,self.l_prob,self.lane_width, self.p_poly = self.ALCAMP.update(v_ego, md, np.array(self.r_poly), np.array(self.l_poly), self.r_prob, self.l_prob, self.lane_width, self.p_poly) + + self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width,v_ego) - def update(self, v_ego, md): + def update(self, v_ego, md, alca): self.parse_model(md) - self.update_d_poly(v_ego) + self.update_d_poly(v_ego, md, alca) diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 6a59565509ce8d..31b00a93f14498 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -24,7 +24,7 @@ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, ste pid_log.active = False self.pid.reset() else: - self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner + self.angle_steers_des = path_plan.angleSteers steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max @@ -46,4 +46,9 @@ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, ste pid_log.output = output_steer pid_log.saturated = bool(self.pid.saturated) - return output_steer, float(self.angle_steers_des), pid_log + + # we only deal with Tesla, so we return two angles, no torque info + if CP.carName == "tesla": + return float(self.angle_steers_des), path_plan.angleSteers, pid_log + else: + return output_steer, float(self.angle_steers_des), pid_log diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 3fc48aa264962d..4ef67b5becc7cd 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,6 +1,7 @@ from cereal import log from common.numpy_fast import clip, interp from selfdrive.controls.lib.pid import PIController +from selfdrive.controls.lib.pid_real import PIDController LongCtrlState = log.ControlsState.LongControlState @@ -57,12 +58,25 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, class LongControl(): def __init__(self, CP, compute_gb): + + kdBp = [0, 5., 22.,35.] + kdV = [0.02, 0.02, 0.022, 0.025] + self.long_control_state = LongCtrlState.off # initialized to off - self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), + if CP.carName == "tesla": + self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), + (kdBp,kdV), rate=RATE, sat_limit=0.8, convert=compute_gb) + else: + self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), + (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), + rate=RATE, + sat_limit=0.8, + convert=compute_gb) + self.v_pid = 0.0 self.last_output_gb = 0.0 @@ -127,4 +141,4 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ final_gas = clip(output_gb, 0., gas_max) final_brake = -clip(output_gb, -brake_max, 0.) - return final_gas, final_brake \ No newline at end of file + return final_gas, final_brake diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 0e8a4effe3af57..d6cacc27103817 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -41,14 +41,14 @@ def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay): - states[0].x = v_ego * delay + states[0].x = max(v_ego * delay, 0.0) states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay return states class PathPlanner(): def __init__(self, CP): - self.LP = LanePlanner() + self.LP = LanePlanner(shouldUseAlca=(CP.carName=="tesla")) self.last_cloudlog_t = 0 self.steer_rate_cost = CP.steerRateCost @@ -81,6 +81,7 @@ def setup_mpc(self): def update(self, sm, pm, CP, VM): v_ego = sm['carState'].vEgo angle_steers = sm['carState'].steeringAngle + #angle_steers_des = sm['controlsState'].angleSteersDes active = sm['controlsState'].active angle_offset = sm['liveParameters'].angleOffset @@ -101,7 +102,9 @@ def update(self, sm, pm, CP, VM): elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right - if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not one_blinker) or (not self.lane_change_enabled): + + #BB blocking comma ALCA for now + if (CP.carName == "tesla") or (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not one_blinker) or (not self.lane_change_enabled): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: @@ -126,8 +129,8 @@ def update(self, sm, pm, CP, VM): # starting elif self.lane_change_state == LaneChangeState.laneChangeStarting: - # fade out lanelines over 1s - self.lane_change_ll_prob = max(self.lane_change_ll_prob - DT_MDL, 0.0) + # fade out lanelines over .5s + self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2*DT_MDL, 0.0) # 98% certainty if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: self.lane_change_state = LaneChangeState.laneChangeFinishing @@ -154,13 +157,15 @@ def update(self, sm, pm, CP, VM): if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: self.LP.l_prob *= self.lane_change_ll_prob self.LP.r_prob *= self.lane_change_ll_prob - self.libmpc.init_weights(MPC_COST_LAT.PATH / 10.0, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) + self.libmpc.init_weights(MPC_COST_LAT.PATH / 3.0, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) else: self.libmpc.init_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) - self.LP.update_d_poly(v_ego) + self.LP.update_d_poly(v_ego, sm['model'],True) # account for actuation delay + + self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed diff --git a/selfdrive/controls/lib/pid_real.py b/selfdrive/controls/lib/pid_real.py new file mode 100644 index 00000000000000..76f9652aa13177 --- /dev/null +++ b/selfdrive/controls/lib/pid_real.py @@ -0,0 +1,113 @@ +import numpy as np +from common.numpy_fast import clip, interp +from selfdrive.car.tesla.speed_utils.movingaverage import MovingAverage + + +def apply_deadzone(error, deadzone): + if error > deadzone: + error -= deadzone + elif error < - deadzone: + error += deadzone + else: + error = 0. + return error + +class PIDController(): + def __init__(self, k_p, k_i, k_d, k_f=0.85, pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None): + self._k_p = k_p # proportional gain + self._k_i = k_i # integral gain + self._k_d = k_d # derivative gain + self.k_f = k_f # feedforward gain + + self.pos_limit = pos_limit + self.neg_limit = neg_limit + + self.sat_count_rate = 1.0 / rate + self.i_unwind_rate = 0.3 / rate + self.i_rate = 1.0 / rate + self.rate = 1.0 / rate + self.d_rate = 7.0 / rate + self.sat_limit = sat_limit + self.convert = convert + self.past_errors_avg = 0 + self.past_errors = MovingAverage(3) + + self.reset() + + @property + def k_p(self): + return interp(self.speed, self._k_p[0], self._k_p[1]) + + @property + def k_i(self): + return interp(self.speed, self._k_i[0], self._k_i[1]) + + @property + def k_d(self): + return interp(self.speed, self._k_d[0], self._k_d[1]) + + + def _check_saturation(self, control, override, error): + saturated = (control < self.neg_limit) or (control > self.pos_limit) + + if saturated and not override and abs(error) > 0.1: + self.sat_count += self.sat_count_rate + else: + self.sat_count -= self.sat_count_rate + + self.sat_count = clip(self.sat_count, 0.0, 1.0) + + return self.sat_count > self.sat_limit + + def reset(self): + self.p = 0.0 + self.i = 0.0 + self.f = 0.0 + self.d = 0.0 + self.sat_count = 0.0 + self.saturated = False + self.control = 0 + self.past_errors_avg = 0 + self.past_errors.reset() + + def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False): + self.speed = speed + + error = float(apply_deadzone(setpoint - measurement, deadzone)) + self.p = error * self.k_p + + #clip the feedforward during the last 5 kmh for smooth transition + clipped_error = clip(error,0,5) + self.k_f = clipped_error * 0.2 + + self.f = feedforward * self.k_f + self.d = 0.0 + if self.past_errors.full(): + self.d = self.k_d * ((error - self.past_errors_avg) / self.d_rate) + self.past_errors_avg = self.past_errors.add(error) + + if override: + self.i -= self.i_unwind_rate * float(np.sign(self.i)) + else: + i = self.i + error * self.k_i * self.i_rate + control = self.p + self.f + i + self.d + + if self.convert is not None: + control = self.convert(control, speed=self.speed) + + if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or \ + (error <= 0 and (control >= self.neg_limit or i > 0.0))) and \ + not freeze_integrator: + self.i = i + else: + control = self.p + self.f + self.i + self.d + if self.convert is not None: + control = self.convert(control, speed=self.speed) + + if check_saturation: + self.saturated = self._check_saturation(control, override, error) + else: + self.saturated = False + + self.control = clip(control, self.neg_limit, self.pos_limit) + return self.control diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 7e6a126e47abff..173f73c41c5c0a 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -22,16 +22,22 @@ # lookup tables VS speed to determine min and max accels in cruise # make sure these accelerations are smaller than mpc limits +#BB editing for TESLA only +_A_CRUISE_MIN_V_TESLA = [-4.0, -4.0, -4.0, -4.0, -4.0] _A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30] _A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.] # need fast accel at very low speed for stop and go # make sure these accelerations are smaller than mpc limits + +_A_CRUISE_MAX_V_TESLA = [1.775, 1.85, 1.325, 1.25] _A_CRUISE_MAX_V = [1.2, 1.2, 0.65, .4] +_A_CRUISE_MAX_V_FOLLOWING_TESLA = [1.6, 1.6, 0.65, .4] _A_CRUISE_MAX_V_FOLLOWING = [1.6, 1.6, 0.65, .4] _A_CRUISE_MAX_BP = [0., 6.4, 22.5, 40.] # Lookup table for turns +_A_TOTAL_MAX_V_TESLA = [1.7, 3.2] _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] @@ -39,25 +45,37 @@ SPEED_PERCENTILE_IDX = 7 -def calc_cruise_accel_limits(v_ego, following): - a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) - - if following: - a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_FOLLOWING) +def calc_cruise_accel_limits(v_ego, following, is_tesla = False): + if is_tesla: + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_TESLA) + + if following: + a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_FOLLOWING_TESLA) + else: + a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_TESLA) else: - a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) - return np.vstack([a_cruise_min, a_cruise_max]) + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) + + if following: + a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_FOLLOWING) + else: + a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) + return np.vstack([a_cruise_min, a_cruise_max]) def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): """ This function returns a limited long acceleration allowed, depending on the existing lateral acceleration this should avoid accelerating when losing the target in turns """ - - a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) - a_y = v_ego**2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) - a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.)) + if CP.carName == "tesla": + a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V_TESLA) + a_y = v_ego**2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) + a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.)) + else: + a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) + a_y = v_ego**2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) + a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.)) return [a_target[0], min(a_target[1], a_x_allowed)] @@ -150,7 +168,8 @@ def update(self, sm, pm, CP, VM, PP): # Calculate speed for normal cruise control if enabled and not self.first_loop: - accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)] + is_tesla = (CP.carName == "tesla") + accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following,is_tesla)] jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 818739967988bb..ade98b56813797 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -1,6 +1,6 @@ from common.kalman.simple_kalman import KF1D from selfdrive.config import RADAR_TO_CAMERA - +from common.numpy_fast import clip, interp # the longer lead decels, the more likely it will keep decelerating # TODO is this a good default? @@ -22,15 +22,32 @@ def __init__(self, v_lead, kalman_params): self.K_K = kalman_params.K self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K) - def update(self, d_rel, y_rel, v_rel, v_lead, measured): + def update(self, d_rel, y_rel, v_rel,measured, a_rel, vy_rel, oClass, length, track_id,movingState, d_path, v_ego_t_aligned,v_lead,use_tesla_radar): + # relative values, copy self.dRel = d_rel # LONG_DIST self.yRel = y_rel # -LAT_DIST self.vRel = v_rel # REL_SPEED self.vLead = v_lead self.measured = measured # measured or estimate + self.track_id = track_id + self.oClass = oClass # object class + + if use_tesla_radar: + self.aRel = a_rel # rel acceleration + self.vLat = vy_rel # rel lateral speed + self.length = length #length + self.measured = measured # measured or estimate + self.dPath = d_path + self.stationary = (movingState == 3) + else: + self.aRel = float('nan') # rel acceleration + self.vLat = float('nan') # rel lateral speed + self.length = 0. #length + self.dPath = 0. + self.stationary = False - # computed velocity and accelerations + if self.cnt > 0: self.kf.update(self.vLead) @@ -49,6 +66,10 @@ def get_key_for_cluster(self): # Weigh y higher since radar is inaccurate in this dimension return [self.dRel, self.yRel*2, self.vRel] + def get_key_for_cluster_dy(self, dy): + # Weigh y higher since radar is inaccurate in this dimension + return [self.dRel, (self.yRel-dy)*2, self.vRel] + def reset_a_lead(self, aLeadK, aLeadTau): self.kf = KF1D([[self.vLead], [aLeadK]], self.K_A, self.K_C, self.K_K) self.aLeadK = aLeadK @@ -59,8 +80,11 @@ def mean(l): class Cluster(): - def __init__(self): + def __init__(self,use_tesla_radar): self.tracks = set() + #BB frame delay for dRel calculation, in seconds + self.frame_delay = 0.2 + self.useTeslaRadar = use_tesla_radar def add(self, t): # add the first track @@ -69,7 +93,7 @@ def add(self, t): # TODO: make generic @property def dRel(self): - return mean([t.dRel for t in self.tracks]) + return min([t.dRel for t in self.tracks]) @property def yRel(self): @@ -117,9 +141,28 @@ def aLeadTau(self): def measured(self): return any(t.measured for t in self.tracks) + @property + def oClass(self): + return min([t.oClass for t in self.tracks]) + + @property + def length(self): + return max([t.length for t in self.tracks]) + + @property + def track_id(self): + return max([t.track_id for t in self.tracks]) + + @property + def stationary(self): + return all([t.stationary for t in self.tracks]) + def get_RadarState(self, model_prob=0.0): + dRel_delta_estimate = 0. + if self.useTeslaRadar: + dRel_delta_estimate = (self.vRel + self.aRel * self.frame_delay / 2.) * self.frame_delay return { - "dRel": float(self.dRel), + "dRel": float(self.dRel + dRel_delta_estimate), "yRel": float(self.yRel), "vRel": float(self.vRel), "vLead": float(self.vLead), @@ -127,9 +170,13 @@ def get_RadarState(self, model_prob=0.0): "aLeadK": float(self.aLeadK), "status": True, "fcw": self.is_potential_fcw(model_prob), + "aLeadTau": float(self.aLeadTau), "modelProb": model_prob, "radar": True, - "aLeadTau": float(self.aLeadTau) + }, { + "trackId": int(self.track_id % 32), + "oClass": int(self.oClass), + "length": float(self.length), } def get_RadarState_from_vision(self, lead_msg, v_ego): @@ -151,6 +198,65 @@ def __str__(self): ret = "x: %4.1f y: %4.1f v: %4.1f a: %4.1f" % (self.dRel, self.yRel, self.vRel, self.aLeadK) return ret + def is_potential_lead(self, v_ego): + # predict cut-ins by extrapolating lateral speed by a lookahead time + # lookahead time depends on cut-in distance. more attentive for close cut-ins + # also, above 50 meters the predicted path isn't very reliable + + # average dist + d_path = self.dPath + + # correct d_path for lookahead time, considering only cut-ins and no more than 1m impact. + lat_corr = 0. # BB disables for now : clip(t_lookahead * self.vLat, -1., 1.) if self.measured else 0. + + # consider only cut-ins + d_path = clip(d_path + lat_corr, min(0., d_path), max(0.,d_path)) + + return abs(d_path) < 1.5 and not self.stationary #and not self.oncoming + + def is_potential_lead_dy(self, v_ego,dy): + # predict cut-ins by extrapolating lateral speed by a lookahead time + # lookahead time depends on cut-in distance. more attentive for close cut-ins + # also, above 50 meters the predicted path isn't very reliable + + # the distance at which v_lat matters is higher at higher speed + lookahead_dist = 40. + v_ego/1.2 #40m at 0mph, ~70m at 80mph + + t_lookahead_v = [1., 0.] + t_lookahead_bp = [10., lookahead_dist] + + # average dist + d_path = self.dPath - dy + + # lat_corr used to be gated on enabled, now always running + t_lookahead = interp(self.dRel, t_lookahead_bp, t_lookahead_v) + + # correct d_path for lookahead time, considering only cut-ins and no more than 1m impact. + lat_corr = clip(t_lookahead * self.vLat, -1., 1.) if self.measured else 0. + + # consider only cut-ins + d_path = clip(d_path + lat_corr, min(0., d_path), max(0.,d_path)) + + return abs(d_path) < abs(dy/2.) and not self.stationary #and not self.oncoming + + def is_truck(self,lead_clusters): + return False + if len(lead_clusters) > 0: + lead_cluster = lead_clusters[0] + # check if the new lead is too close and roughly at the same speed of the first lead: + # it might just be the second axle of the same vehicle + return (self.dRel - lead_cluster.dRel < 4.5) and (self.dRel - lead_cluster.dRel > 0.5) and (abs(self.yRel - lead_cluster.yRel) < 2.) and (abs(self.vRel - lead_cluster.vRel) < 0.2) + else: + return False + + def is_potential_lead2(self, lead_clusters): + if len(lead_clusters) > 0: + lead_cluster = lead_clusters[0] + return ((self.dRel - lead_cluster.dRel > 8.) and (lead_cluster.oClass > 0)) or ((self.dRel - lead_cluster.dRel > 15.) and (lead_cluster.oClass == 0)) or abs(self.vRel - lead_cluster.vRel) > 1. + else: + return False + + def potential_low_speed_lead(self, v_ego): # stop for stuff in front of you and low speed, even without model confirmation return abs(self.yRel) < 1.5 and (v_ego < v_ego_stationary) and self.dRel < 25 diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 0b8523313f437d..04f0423c9e256d 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -2,9 +2,10 @@ import importlib import math from collections import defaultdict, deque +import numpy as np import cereal.messaging as messaging -from cereal import car +from cereal import car,tesla from common.numpy_fast import interp from common.params import Params from common.realtime import Ratekeeper, set_realtime_priority @@ -12,7 +13,10 @@ from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid from selfdrive.controls.lib.radar_helpers import Cluster, Track from selfdrive.swaglog import cloudlog +from selfdrive.car.tesla.readconfig import CarSettings +DEBUG = False +RDR_TO_LDR = 0 class KalmanParams(): def __init__(self, dt): @@ -58,8 +62,14 @@ def prob(c): else: return None +def get_rrext_by_trackId(rrext,trackId): + if rrext is not None: + for p in rrext: + if p.trackId == trackId: + return p + return None -def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): +def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True,use_tesla_radar=False): # Determine leads, this is where the essential logic happens if len(clusters) > 0 and ready and lead_msg.prob > .5: cluster = match_vision_to_cluster(v_ego, lead_msg, clusters) @@ -67,10 +77,14 @@ def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): cluster = None lead_dict = {'status': False} + lead_dict_ext = {'trackId': 1, 'oClass': 1, 'length': 0.} + # temporary for development purposes: we set the default lead vehicle type to truck (=0) to distinguish between vision (truck) and radar leads (car) in IC + if use_tesla_radar: + lead_dict_ext['oClass'] = 0 if cluster is not None: - lead_dict = cluster.get_RadarState(lead_msg.prob) + lead_dict,lead_dict_ext = cluster.get_RadarState(lead_msg.prob) elif (cluster is None) and ready and (lead_msg.prob > .5): - lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego) + lead_dict = Cluster(use_tesla_radar).get_RadarState_from_vision(lead_msg, v_ego) if low_speed_override: low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)] @@ -79,15 +93,15 @@ def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): # Only choose new cluster if it is actually closer than the previous one if (not lead_dict['status']) or (closest_cluster.dRel < lead_dict['dRel']): - lead_dict = closest_cluster.get_RadarState() + lead_dict,lead_dict_ext = closest_cluster.get_RadarState() - return lead_dict + return lead_dict,lead_dict_ext class RadarD(): - def __init__(self, radar_ts, delay=0): + def __init__(self, radar_ts, RI,use_tesla_radar, delay=0): self.current_time = 0 - + self.RI = RI self.tracks = defaultdict(dict) self.kalman_params = KalmanParams(radar_ts) @@ -101,20 +115,40 @@ def __init__(self, radar_ts, delay=0): self.v_ego_hist = deque([0], maxlen=delay+1) self.ready = False - - def update(self, frame, sm, rr, has_radar): + self.icCarLR = None + self.use_tesla_radar = use_tesla_radar + if self.use_tesla_radar: + if (RI.TRACK_RIGHT_LANE or RI.TRACK_LEFT_LANE): + self.icCarLR = messaging.pub_sock('uiIcCarLR') + + self.lane_width = 3.0 + #only used for left and right lanes + self.path_x = np.arange(0.0, 160.0, 0.1) # 160 meters is max + self.dPoly = [0.,0.,0.,0.] + + def update(self, frame, sm, rr, has_radar,rrext): self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()]) - + if sm.updated['controlsState']: self.active = sm['controlsState'].active self.v_ego = sm['controlsState'].vEgo self.v_ego_hist.append(self.v_ego) if sm.updated['model']: self.ready = True + if self.use_tesla_radar: + if sm.updated['pathPlan']: + self.lane_width = sm['pathPlan'].laneWidth + self.dPoly = sm['pathPlan'].dPoly + + path_y = np.polyval(self.dPoly, self.path_x) ar_pts = {} for pt in rr.points: - ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured] + if rrext: + extpt = get_rrext_by_trackId(rrext,pt.trackId) + ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured, pt.aRel, pt.yvRel, extpt.objectClass, extpt.length, pt.trackId+2, extpt.movingState] + else: + ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured, pt.aRel, pt.yvRel, 1, 0, pt.trackId+2, 1] # *** remove missing points from meta data *** for ids in list(self.tracks.keys()): @@ -128,10 +162,15 @@ def update(self, frame, sm, rr, has_radar): # align v_ego by a fixed time to align it with the radar measurement v_lead = rpt[2] + self.v_ego_hist[0] + # distance relative to path + d_path = np.sqrt(np.amin((self.path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2)) + # add sign + d_path *= np.sign(rpt[1] - np.interp(rpt[0], self.path_x, path_y)) + # create the track if it doesn't exist or it's a new track if ids not in self.tracks: self.tracks[ids] = Track(v_lead, self.kalman_params) - self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3]) + self.tracks[ids].update(rpt[0], rpt[1], rpt[2], rpt[3], rpt[4],rpt[5],rpt[6],rpt[7],rpt[8],rpt[9], d_path, self.v_ego,v_lead,self.use_tesla_radar) idens = list(sorted(self.tracks.keys())) track_pts = list([self.tracks[iden].get_key_for_cluster() for iden in idens]) @@ -145,12 +184,12 @@ def update(self, frame, sm, rr, has_radar): for idx in range(len(track_pts)): cluster_i = cluster_idxs[idx] if clusters[cluster_i] is None: - clusters[cluster_i] = Cluster() + clusters[cluster_i] = Cluster(self.use_tesla_radar) clusters[cluster_i].add(self.tracks[idens[idx]]) elif len(track_pts) == 1: # FIXME: cluster_point_centroid hangs forever if len(track_pts) == 1 cluster_idxs = [0] - clusters = [Cluster()] + clusters = [Cluster(self.use_tesla_radar)] clusters[0].add(self.tracks[idens[0]]) else: clusters = [] @@ -161,6 +200,150 @@ def update(self, frame, sm, rr, has_radar): aLeadK = clusters[cluster_idxs[idx]].aLeadK aLeadTau = clusters[cluster_idxs[idx]].aLeadTau self.tracks[idens[idx]].reset_a_lead(aLeadK, aLeadTau) + + ### START REVIEW SECTION + + ################################################################# + #BB For Tesla integration we will also track Left and Right lanes + ################################################################# + if self.use_tesla_radar: + if (self.RI.TRACK_RIGHT_LANE or self.RI.TRACK_LEFT_LANE): + datrl = tesla.ICCarsLR.new_message() + datrl.v1Type = int(0) + datrl.v1Dx = float(0.) + datrl.v1Vrel = float(0.) + datrl.v1Dy = float(0.) + datrl.v1Id = int(0) + datrl.v2Type = int(0) + datrl.v2Dx = float(0.) + datrl.v2Vrel = float(0.) + datrl.v2Dy = float(0.) + datrl.v2Id = int(0) + datrl.v3Type = int(0) + datrl.v3Dx = float(0.) + datrl.v3Vrel = float(0.) + datrl.v3Dy = float(0.) + datrl.v3Id = int(0) + datrl.v4Type = int(0) + datrl.v4Dx = float(0.) + datrl.v4Vrel = float(0.) + datrl.v4Dy = float(0.) + datrl.v4Id = int(0) + lane_offset = 0. + #LEFT LANE + if self.RI.TRACK_LEFT_LANE: + ll_track_pts = np.array([self.tracks[iden].get_key_for_cluster_dy(-self.lane_width) for iden in idens]) + # If we have multiple points, cluster them + if len(ll_track_pts) > 1: + ll_cluster_idxs = cluster_points_centroid(ll_track_pts, 2.5) + ll_clusters = [None] * (max(ll_cluster_idxs) + 1) + + for idx in range(len(ll_track_pts)): + ll_cluster_i = ll_cluster_idxs[idx] + + if ll_clusters[ll_cluster_i] == None: + ll_clusters[ll_cluster_i] = Cluster(self.use_tesla_radar) + ll_clusters[ll_cluster_i].add(self.tracks[idens[idx]]) + elif len(ll_track_pts) == 1: + # TODO: why do we need this? + ll_clusters = [Cluster(self.use_tesla_radar)] + ll_clusters[0].add(self.tracks[idens[0]]) + else: + ll_clusters = [] + if DEBUG: + for i in ll_clusters: + print(i) + # *** extract the lead car *** + ll_lead_clusters = [c for c in ll_clusters + if c.is_potential_lead_dy(self.v_ego,-self.lane_width)] + ll_lead_clusters.sort(key=lambda x: x.dRel) + ll_lead_len = len(ll_lead_clusters) + ll_lead1_truck = (len([c for c in ll_lead_clusters + if c.is_truck(ll_lead_clusters)]) > 0) + + # *** extract the second lead from the whole set of leads *** + ll_lead2_clusters = [c for c in ll_lead_clusters + if c.is_potential_lead2(ll_lead_clusters)] + ll_lead2_clusters.sort(key=lambda x: x.dRel) + ll_lead2_len = len(ll_lead2_clusters) + ll_lead2_truck = (len([c for c in ll_lead_clusters + if c.is_truck(ll_lead2_clusters)]) > 0) + # publish data + if ll_lead_len > 0: + datrl.v1Type = int(ll_lead_clusters[0].oClass) + if datrl.v1Type == 1 and ll_lead1_truck: + datrl.v1Type = 0 + datrl.v1Dx = float(ll_lead_clusters[0].dRel) + datrl.v1Vrel = float(ll_lead_clusters[0].vRel) + datrl.v1Dy = float(-ll_lead_clusters[0].yRel - lane_offset) + datrl.v1Id = int(ll_lead_clusters[0].track_id % 32) + if ll_lead2_len > 0: + datrl.v2Type = int(ll_lead2_clusters[0].oClass) + if datrl.v2Type == 1 and ll_lead2_truck: + datrl.v2Type = 0 + datrl.v2Dx = float(ll_lead2_clusters[0].dRel) + datrl.v2Vrel = float(ll_lead2_clusters[0].vRel) + datrl.v2Dy = float(-ll_lead2_clusters[0].yRel - lane_offset) + datrl.v2Id = int(ll_lead2_clusters[0].track_id % 32) + #RIGHT LANE + if self.RI.TRACK_RIGHT_LANE: + rl_track_pts = np.array([self.tracks[iden].get_key_for_cluster_dy(self.lane_width) for iden in idens]) + # If we have multiple points, cluster them + if len(rl_track_pts) > 1: + rl_cluster_idxs = cluster_points_centroid(rl_track_pts, 2.5) + rl_clusters = [None] * (max(rl_cluster_idxs) + 1) + + for idx in range(len(rl_track_pts)): + rl_cluster_i = rl_cluster_idxs[idx] + + if rl_clusters[rl_cluster_i] == None: + rl_clusters[rl_cluster_i] = Cluster(self.use_tesla_radar) + rl_clusters[rl_cluster_i].add(self.tracks[idens[idx]]) + elif len(rl_track_pts) == 1: + # TODO: why do we need this? + rl_clusters = [Cluster(self.use_tesla_radar)] + rl_clusters[0].add(self.tracks[idens[0]]) + else: + rl_clusters = [] + if DEBUG: + for i in rl_clusters: + print(i) + # *** extract the lead car *** + rl_lead_clusters = [c for c in rl_clusters + if c.is_potential_lead_dy(self.v_ego,self.lane_width)] + rl_lead_clusters.sort(key=lambda x: x.dRel) + rl_lead_len = len(rl_lead_clusters) + rl_lead1_truck = (len([c for c in rl_lead_clusters + if c.is_truck(rl_lead_clusters)]) > 0) + # *** extract the second lead from the whole set of leads *** + rl_lead2_clusters = [c for c in rl_lead_clusters + if c.is_potential_lead2(rl_lead_clusters)] + rl_lead2_clusters.sort(key=lambda x: x.dRel) + rl_lead2_len = len(rl_lead2_clusters) + rl_lead2_truck = (len([c for c in rl_lead_clusters + if c.is_truck(rl_lead2_clusters)]) > 0) + # publish data + if rl_lead_len > 0: + datrl.v3Type = int(rl_lead_clusters[0].oClass) + if datrl.v3Type == 1 and rl_lead1_truck: + datrl.v3Type = 0 + datrl.v3Dx = float(rl_lead_clusters[0].dRel) + datrl.v3Vrel = float(rl_lead_clusters[0].vRel) + datrl.v3Dy = float(-rl_lead_clusters[0].yRel+ lane_offset) + datrl.v3Id = int(rl_lead_clusters[0].track_id % 32) + if rl_lead2_len > 0: + datrl.v4Type = int(rl_lead2_clusters[0].oClass) + if datrl.v4Type == 1 and rl_lead2_truck: + datrl.v4Type = 0 + datrl.v4Dx = float(rl_lead2_clusters[0].dRel) + datrl.v4Vrel = float(rl_lead2_clusters[0].vRel) + datrl.v4Dy = float(-rl_lead2_clusters[0].yRel + lane_offset) + datrl.v4Id = int(rl_lead2_clusters[0].track_id % 32) + if (self.RI.TRACK_RIGHT_LANE or self.RI.TRACK_LEFT_LANE): + self.icCarLR.send(datrl.to_bytes()) + + ### END REVIEW SECTION + # *** publish radarState *** dat = messaging.new_message('radarState') @@ -170,10 +353,20 @@ def update(self, frame, sm, rr, has_radar): dat.radarState.radarErrors = list(rr.errors) dat.radarState.controlsStateMonoTime = self.last_controls_state_ts + datext = tesla.ICLeads.new_message() if has_radar: - dat.radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True) - dat.radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False) - return dat + l1d,l1x = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True,use_tesla_radar=self.use_tesla_radar) + l2d,l2x = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False, use_tesla_radar=self.use_tesla_radar) + dat.radarState.leadOne = l1d + dat.radarState.leadTwo = l2d + + datext.lead1trackId = l1x['trackId'] + datext.lead1oClass = l1x['oClass'] + datext.lead1length = l1x['length'] + datext.lead2trackId = l2x['trackId'] + datext.lead2oClass = l2x['oClass'] + datext.lead2length = l2x['length'] + return dat, datext # fuses camera and radar data for best lead detection @@ -183,6 +376,7 @@ def radard_thread(sm=None, pm=None, can_sock=None): # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) + use_tesla_radar = CarSettings().get_value("useTeslaRadar") cloudlog.info("radard got CarParams") # import the radar from the fingerprint @@ -193,33 +387,59 @@ def radard_thread(sm=None, pm=None, can_sock=None): can_sock = messaging.sub_sock('can') if sm is None: - sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters']) + if CP.carName == "tesla": + sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters', 'pathPlan']) + else: + sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters']) + # *** publish radarState and liveTracks if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) + if CP.carName == "tesla": + icLeads = messaging.pub_sock('uiIcLeads') + ahbInfo = messaging.pub_sock('ahbInfo') RI = RadarInterface(CP) rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) - RD = RadarD(CP.radarTimeStep, RI.delay) - - has_radar = not CP.radarOffCan + RD = RadarD(CP.radarTimeStep, RI, use_tesla_radar,RI.delay) + has_radar = not CP.radarOffCan + v_ego = 0. + print("Working with ",CP.carName," with radarOffCan=",CP.radarOffCan) while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) - rr = RI.update(can_strings) + + if CP.carName == "tesla": + rr,rrext,ahbCarDetected = RI.update(can_strings,v_ego) + else: + rr = RI.update(can_strings) + rrext = None + ahbCarDetected = False if rr is None: - continue + continue sm.update(0) - dat = RD.update(rk.frame, sm, rr, has_radar) + if sm.updated['controlsState']: + v_ego = sm['controlsState'].vEgo + + dat,datext = RD.update(rk.frame, sm, rr, has_radar, rrext) dat.radarState.cumLagMs = -rk.remaining*1000. pm.send('radarState', dat) + if CP.carName == "tesla": + icLeads.send(datext.to_bytes()) + ahbInfoMsg = tesla.AHBinfo.new_message() + ahbInfoMsg.source = 0 + ahbInfoMsg.radarCarDetected = ahbCarDetected + ahbInfoMsg.cameraCarDetected = False + ahbInfo.send(ahbInfoMsg.to_bytes()) + + # *** publish tracks for UI debugging (keep last) *** tracks = RD.tracks dat = messaging.new_message('liveTracks', len(tracks)) @@ -237,8 +457,7 @@ def radard_thread(sm=None, pm=None, can_sock=None): def main(sm=None, pm=None, can_sock=None): - radard_thread(sm, pm, can_sock) - + radard_thread(sm,pm,can_sock) if __name__ == "__main__": - main() + main() diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 8dfff81ad40b59..3c1f3932ffc0cc 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -25,12 +25,11 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0., p_p = poly_p.copy() p_p[3] += poly_shift - d_poly = calc_d_poly(p_l, p_r, p_p, l_prob, r_prob, lane_width) + d_poly = calc_d_poly(p_l, p_r, p_p, l_prob, r_prob, lane_width, v_ref) CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") VM = VehicleModel(CP) - v_ref = v_ref curvature_factor = VM.curvature_factor(v_ref) l_poly = libmpc_py.ffi.new("double[4]", list(map(float, p_l))) diff --git a/selfdrive/crash.py b/selfdrive/crash.py index 74a7094ae52a6e..8b79417ba347f3 100644 --- a/selfdrive/crash.py +++ b/selfdrive/crash.py @@ -4,8 +4,8 @@ import threading import capnp from selfdrive.version import version, dirty - from selfdrive.swaglog import cloudlog +from selfdrive.tinklad.tinkla_interface import TinklaClient from common.android import ANDROID if os.getenv("NOLOG") or os.getenv("NOCRASH") or not ANDROID: @@ -23,8 +23,13 @@ def install(): client = Client('https://1994756b5e6f41cf939a4c65de45f4f2:cefebaf3a8aa40d182609785f7189bd7@app.getsentry.com/77924', install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty}) + def sendCrashInfoToTinklad(): + tinklaClient = TinklaClient() + tinklaClient.logCrashStackTraceEvent() + def capture_exception(*args, **kwargs): exc_info = sys.exc_info() + sendCrashInfoToTinklad() if not exc_info[0] is capnp.lib.capnp.KjException: client.captureException(*args, **kwargs) cloudlog.error("crash", exc_info=kwargs.get('exc_info', 1)) diff --git a/selfdrive/debug/filter_log_message.py b/selfdrive/debug/filter_log_message.py index 8ddb12e2d78b87..a619e5df5ae2e2 100755 --- a/selfdrive/debug/filter_log_message.py +++ b/selfdrive/debug/filter_log_message.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 -import os import argparse import json @@ -14,6 +13,15 @@ "CRITICAL": 50, } +ANDROID_LOG_SOURCE = { + 0: "MAIN", + 1: "RADIO", + 2: "EVENTS", + 3: "SYSTEM", + 4: "CRASH", + 5: "KERNEL", +} + if __name__ == "__main__": @@ -23,20 +31,26 @@ parser.add_argument("socket", type=str, nargs='*', help="socket name") args = parser.parse_args() - if args.addr != "127.0.0.1": - os.environ["ZMQ"] = "1" - messaging.context = messaging.Context() - - poller = messaging.Poller() - sock = messaging.sub_sock("logMessage", poller, addr=args.addr) + sm = messaging.SubMaster(['logMessage', 'androidLog'], addr=args.addr) min_level = LEVELS[args.level] while True: - polld = poller.poll(1000) - for sock in polld: - evt = messaging.recv_one(sock) - log = json.loads(evt.logMessage) - + sm.update() + + if sm.updated['logMessage']: + t = sm.logMonoTime['logMessage'] + try: + log = json.loads(sm['logMessage']) + if log['levelnum'] >= min_level: + print(f"[{t / 1e9:.6f}] {log['filename']}:{log.get('lineno', '')} - {log.get('funcname', '')}: {log['msg']}") + except json.decoder.JSONDecodeError: + print(f"[{t / 1e9:.6f}] decode error: {sm['logMessage']}") if log['levelnum'] >= min_level: print(f"{log['filename']}:{log.get('lineno', '')} - {log.get('funcname', '')}: {log['msg']}") + + if sm.updated['androidLog']: + t = sm.logMonoTime['androidLog'] + m = sm['androidLog'] + source = ANDROID_LOG_SOURCE[m.id] + print(f"[{t / 1e9:.6f}] {source} {m.pid} {m.tag} - {m.message}") diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index bc0f0a0396b3e7..8cb8e4027fefc4 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -59,9 +59,13 @@ def __init__(self, param_put=False): self.valid_blocks = 0 self.cal_status = Calibration.UNCALIBRATED self.just_calibrated = False + self.v_ego = 0 # Read calibration - calibration_params = Params().get("CalibrationParams") + if param_put: + calibration_params = Params().get("CalibrationParams") + else: + calibration_params = None if calibration_params: try: calibration_params = json.loads(calibration_params) @@ -88,8 +92,11 @@ def update_status(self): if start_status == Calibration.UNCALIBRATED and end_status == Calibration.CALIBRATED: self.just_calibrated = True + def handle_v_ego(self, v_ego): + self.v_ego = v_ego + def handle_cam_odom(self, trans, rot, trans_std, rot_std): - straight_and_fast = ((trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER)) + straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER)) certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < MAX_VEL_ANGLE_STD) or (self.valid_blocks < INPUTS_NEEDED)) if straight_and_fast and certain_if_calib: @@ -132,7 +139,7 @@ def send_data(self, pm): def calibrationd_thread(sm=None, pm=None): if sm is None: - sm = messaging.SubMaster(['cameraOdometry']) + sm = messaging.SubMaster(['cameraOdometry', 'carState']) if pm is None: pm = messaging.PubMaster(['liveCalibration']) @@ -143,18 +150,28 @@ def calibrationd_thread(sm=None, pm=None): while 1: sm.update() + # if no inputs still publish calibration + if not sm.updated['carState'] and not sm.updated['cameraOdometry']: + calibrator.send_data(pm) + continue + + if sm.updated['carState']: + calibrator.handle_v_ego(sm['carState'].vEgo) + if send_counter % 25 == 0: + calibrator.send_data(pm) + send_counter += 1 + if sm.updated['cameraOdometry']: new_vp = calibrator.handle_cam_odom(sm['cameraOdometry'].trans, sm['cameraOdometry'].rot, sm['cameraOdometry'].transStd, sm['cameraOdometry'].rotStd) - if DEBUG and new_vp is not None: - print('got new vp', new_vp) - # decimate outputs for efficiency - if (send_counter % 5) == 0: - calibrator.send_data(pm) - send_counter += 1 + + if DEBUG and new_vp is not None: + print('got new vp', new_vp) + + # decimate outputs for efficiency def main(sm=None, pm=None): diff --git a/selfdrive/locationd/kalman/helpers/__init__.py b/selfdrive/locationd/kalman/helpers/__init__.py index 2ec5ba0ddb5d3b..fcccb9cec6c6b3 100644 --- a/selfdrive/locationd/kalman/helpers/__init__.py +++ b/selfdrive/locationd/kalman/helpers/__init__.py @@ -62,6 +62,7 @@ class ObservationKind(): ANGLE_OFFSET_FAST = 27 # [rad] STIFFNESS = 28 # [-] STEER_RATIO = 29 # [-] + ROAD_FRAME_X_SPEED = 30 # (x) [m/s] names = [ 'Unknown', @@ -108,7 +109,7 @@ def to_string(cls, kind): def run_car_ekf_offline(kf, observations_by_kind): - from laika.raw_gnss import GNSSMeasurement + from laika.laika.raw_gnss import GNSSMeasurement observations = [] # create list of observations with element format: [kind, time, data] for kind in observations_by_kind: diff --git a/selfdrive/locationd/kalman/helpers/ekf_sym.py b/selfdrive/locationd/kalman/helpers/ekf_sym.py index c9eb093a4be901..d18feddf05da81 100644 --- a/selfdrive/locationd/kalman/helpers/ekf_sym.py +++ b/selfdrive/locationd/kalman/helpers/ekf_sym.py @@ -291,6 +291,11 @@ def init_state(self, state, covs, filter_time): self.rewind_t = [] self.rewind_states = [] + def reset_rewind(self): + self.rewind_obscache = [] + self.rewind_t = [] + self.rewind_states = [] + def augment(self): # TODO this is not a generalized way of doing this and implies that the augmented states # are simply the first (dim_augment_state) elements of the main state. diff --git a/selfdrive/locationd/kalman/models/car_kf.py b/selfdrive/locationd/kalman/models/car_kf.py index aa729fc1029945..0cc90090b47956 100755 --- a/selfdrive/locationd/kalman/models/car_kf.py +++ b/selfdrive/locationd/kalman/models/car_kf.py @@ -44,37 +44,25 @@ class CarKalman(): 0.0, ]) - # state covariance - P_initial = np.diag([ - .1**2, - .1**2, - math.radians(0.1)**2, - math.radians(0.1)**2, - - 10**2, 10**2, - 1**2, - 1**2, - ]) - # process noise Q = np.diag([ - (.05/10)**2, + (.05/100)**2, .0001**2, - math.radians(0.01)**2, - math.radians(0.2)**2, + math.radians(0.0001)**2, + math.radians(0.1)**2, - .1**2, .1**2, + .1**2, .01**2, math.radians(0.1)**2, math.radians(0.1)**2, ]) + P_initial = Q.copy() obs_noise = { - ObservationKind.ROAD_FRAME_XY_SPEED: np.diag([0.1**2, 0.1**2]), - ObservationKind.ROAD_FRAME_YAW_RATE: np.atleast_2d(math.radians(0.1)**2), - ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.1)**2), + ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.01)**2), ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(5.0)**2), - ObservationKind.STEER_RATIO: np.atleast_2d(50.0**2), - ObservationKind.STIFFNESS: np.atleast_2d(50.0**2), + ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2), + ObservationKind.STIFFNESS: np.atleast_2d(5.0**2), + ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2), } maha_test_kinds = [] # [ObservationKind.ROAD_FRAME_YAW_RATE, ObservationKind.ROAD_FRAME_XY_SPEED] @@ -141,6 +129,7 @@ def generate_code(): obs_eqs = [ [sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None], [sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None], + [sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None], [sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None], [sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None], [sp.Matrix([sR]), ObservationKind.STEER_RATIO, None], @@ -149,8 +138,12 @@ def generate_code(): gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, maha_test_kinds=maha_test_kinds, global_vars=CarKalman.global_vars) - def __init__(self): + def __init__(self, steer_ratio=15, stiffness_factor=1, angle_offset=0): self.dim_state = self.x_initial.shape[0] + x_init = self.x_initial + x_init[States.STEER_RATIO] = steer_ratio + x_init[States.STIFFNESS] = stiffness_factor + x_init[States.ANGLE_OFFSET] = angle_offset # init filter self.filter = EKF_sym(self.name, self.Q, self.x_initial, self.P_initial, self.dim_state, self.dim_state, maha_test_kinds=self.maha_test_kinds, global_vars=self.global_vars) @@ -186,10 +179,14 @@ def init_state(self, state, covs_diag=None, covs=None, filter_time=None): P = self.filter.covs() self.filter.init_state(state, P, filter_time) - def predict_and_observe(self, t, kind, data): + def predict_and_observe(self, t, kind, data, R=None): if len(data) > 0: data = np.atleast_2d(data) - self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data))) + + if R is None: + R = self.get_R(kind, len(data)) + + self.filter.predict_and_update_batch(t, kind, data, R) if __name__ == "__main__": diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index 001f6d2e9f57f0..0df43d0dcdb586 100644 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -2,6 +2,7 @@ import math import numpy as np +import sympy as sp import cereal.messaging as messaging import common.transformations.coordinates as coord @@ -16,6 +17,9 @@ #from datetime import datetime #from laika.gps_time import GPSTime +from sympy.utilities.lambdify import lambdify +from selfdrive.locationd.kalman.helpers.sympy_helpers import euler_rotate + VISION_DECIMATION = 2 SENSOR_DECIMATION = 10 @@ -25,6 +29,22 @@ def to_float(arr): return [float(arr[0]), float(arr[1]), float(arr[2])] +def get_H(): + # this returns a function to eval the jacobian + # of the observation function of the local vel + roll = sp.Symbol('roll') + pitch = sp.Symbol('pitch') + yaw = sp.Symbol('yaw') + vx = sp.Symbol('vx') + vy = sp.Symbol('vy') + vz = sp.Symbol('vz') + + h = euler_rotate(roll, pitch, yaw).T*(sp.Matrix([vx, vy, vz])) + H = h.jacobian(sp.Matrix([roll, pitch, yaw, vx, vy, vz])) + H_f = lambdify([roll, pitch, yaw, vx, vy, vz], H) + return H_f + + class Localizer(): def __init__(self, disabled_logs=[], dog=None): self.kf = LiveKalman() @@ -35,38 +55,52 @@ def __init__(self, disabled_logs=[], dog=None): self.device_from_calib = np.eye(3) self.calib_from_device = np.eye(3) self.calibrated = 0 + self.H = get_H() - def liveLocationMsg(self, time): - predicted_state = self.kf.x - predicted_std = np.diagonal(self.kf.P) + @staticmethod + def msg_from_state(converter, calib_from_device, H, predicted_state, predicted_cov): + predicted_std = np.sqrt(np.diagonal(predicted_cov)) fix_ecef = predicted_state[States.ECEF_POS] fix_ecef_std = predicted_std[States.ECEF_POS_ERR] vel_ecef = predicted_state[States.ECEF_VELOCITY] vel_ecef_std = predicted_std[States.ECEF_VELOCITY_ERR] fix_pos_geo = coord.ecef2geodetic(fix_ecef) - fix_pos_geo_std = coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo - ned_vel = self.converter.ecef2ned(fix_ecef + vel_ecef) - self.converter.ecef2ned(fix_ecef) - ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef) - device_from_ecef = rot_from_quat(predicted_state[States.ECEF_ORIENTATION]).T - vel_device = device_from_ecef.dot(vel_ecef) - vel_device_std = device_from_ecef.dot(vel_ecef_std) + #fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo) orientation_ecef = euler_from_quat(predicted_state[States.ECEF_ORIENTATION]) orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR] - orientation_ned = ned_euler_from_ecef(fix_ecef, orientation_ecef) - orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned - vel_calib = self.calib_from_device.dot(vel_device) - vel_calib_std = self.calib_from_device.dot(vel_device_std) - acc_calib = self.calib_from_device.dot(predicted_state[States.ACCELERATION]) - acc_calib_std = self.calib_from_device.dot(predicted_std[States.ACCELERATION_ERR]) - ang_vel_calib = self.calib_from_device.dot(predicted_state[States.ANGULAR_VELOCITY]) - ang_vel_calib_std = self.calib_from_device.dot(predicted_std[States.ANGULAR_VELOCITY_ERR]) + acc_calib = calib_from_device.dot(predicted_state[States.ACCELERATION]) + acc_calib_std = np.sqrt(np.diagonal(calib_from_device.dot( + predicted_cov[States.ACCELERATION_ERR, States.ACCELERATION_ERR]).dot( + calib_from_device.T))) + ang_vel_calib = calib_from_device.dot(predicted_state[States.ANGULAR_VELOCITY]) + ang_vel_calib_std = np.sqrt(np.diagonal(calib_from_device.dot( + predicted_cov[States.ANGULAR_VELOCITY_ERR, States.ANGULAR_VELOCITY_ERR]).dot( + calib_from_device.T))) + + device_from_ecef = rot_from_quat(predicted_state[States.ECEF_ORIENTATION]).T + vel_device = device_from_ecef.dot(vel_ecef) + device_from_ecef_eul = euler_from_quat(predicted_state[States.ECEF_ORIENTATION]).T + idxs = list(range(States.ECEF_ORIENTATION_ERR.start, States.ECEF_ORIENTATION_ERR.stop)) + list(range(States.ECEF_VELOCITY_ERR.start, States.ECEF_VELOCITY_ERR.stop)) + condensed_cov = predicted_cov[idxs][:,idxs] + HH = H(*list(np.concatenate([device_from_ecef_eul, vel_ecef]))) + vel_device_cov = HH.dot(condensed_cov).dot(HH.T) + vel_device_std = np.sqrt(np.diagonal(vel_device_cov)) + + vel_calib = calib_from_device.dot(vel_device) + vel_calib_std = np.sqrt(np.diagonal(calib_from_device.dot( + vel_device_cov).dot(calib_from_device.T))) + + orientation_ned = ned_euler_from_ecef(fix_ecef, orientation_ecef) + #orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned + ned_vel = converter.ecef2ned(fix_ecef + vel_ecef) - converter.ecef2ned(fix_ecef) + #ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef) fix = messaging.log.LiveLocationKalman.new_message() fix.positionGeodetic.value = to_float(fix_pos_geo) - fix.positionGeodetic.std = to_float(fix_pos_geo_std) - fix.positionGeodetic.valid = True + #fix.positionGeodetic.std = to_float(fix_pos_geo_std) + #fix.positionGeodetic.valid = True fix.positionECEF.value = to_float(fix_ecef) fix.positionECEF.std = to_float(fix_ecef_std) fix.positionECEF.valid = True @@ -74,8 +108,8 @@ def liveLocationMsg(self, time): fix.velocityECEF.std = to_float(vel_ecef_std) fix.velocityECEF.valid = True fix.velocityNED.value = to_float(ned_vel) - fix.velocityNED.std = to_float(ned_vel_std) - fix.velocityNED.valid = True + #fix.velocityNED.std = to_float(ned_vel_std) + #fix.velocityNED.valid = True fix.velocityDevice.value = to_float(vel_device) fix.velocityDevice.std = to_float(vel_device_std) fix.velocityDevice.valid = True @@ -87,8 +121,8 @@ def liveLocationMsg(self, time): fix.orientationECEF.std = to_float(orientation_ecef_std) fix.orientationECEF.valid = True fix.orientationNED.value = to_float(orientation_ned) - fix.orientationNED.std = to_float(orientation_ned_std) - fix.orientationNED.valid = True + #fix.orientationNED.std = to_float(orientation_ned_std) + #fix.orientationNED.valid = True fix.angularVelocityDevice.value = to_float(predicted_state[States.ANGULAR_VELOCITY]) fix.angularVelocityDevice.std = to_float(predicted_std[States.ANGULAR_VELOCITY_ERR]) fix.angularVelocityDevice.valid = True @@ -102,6 +136,10 @@ def liveLocationMsg(self, time): fix.accelerationCalibrated.value = to_float(acc_calib) fix.accelerationCalibrated.std = to_float(acc_calib_std) fix.accelerationCalibrated.valid = True + return fix + + def liveLocationMsg(self, time): + fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P) #fix.gpsWeek = self.time.week #fix.gpsTimeOfWeek = self.time.tow diff --git a/selfdrive/locationd/params_learner.cc b/selfdrive/locationd/params_learner.cc index f66977e1eb0a09..54904a0fef5d0a 100644 --- a/selfdrive/locationd/params_learner.cc +++ b/selfdrive/locationd/params_learner.cc @@ -7,6 +7,7 @@ #include "cereal/gen/cpp/log.capnp.h" #include "cereal/gen/cpp/car.capnp.h" #include "params_learner.h" +#include // #define DEBUG @@ -28,6 +29,13 @@ ParamsLearner::ParamsLearner(cereal::CarParams::Reader car_params, cF0 = car_params.getTireStiffnessFront(); cR0 = car_params.getTireStiffnessRear(); + std::string carName = car_params.getCarName(); + if (carName == "tesla") { + is_tesla = 1; + } else { + is_tesla = 0; + } + prev_u = 0; l = car_params.getWheelbase(); @@ -62,7 +70,7 @@ bool ParamsLearner::update(double psi, double u, double sa) { //only consider if acceleration [abs(prev_speed - speed) * frequency] is less than MAX_ACCEL double a = abs(prev_u - u) * FREQUENCY; - if (a < MAX_ACCEL) { + if ((a < MAX_ACCEL) || (is_tesla == 0)) { ao = new_ao; slow_ao = new_slow_ao; x = new_x; diff --git a/selfdrive/locationd/params_learner.h b/selfdrive/locationd/params_learner.h index aab0f712cb9c8a..3971cc72e99f24 100644 --- a/selfdrive/locationd/params_learner.h +++ b/selfdrive/locationd/params_learner.h @@ -29,6 +29,7 @@ class ParamsLearner { double slow_ao; double x, sR; double prev_u; //BB previous speed so we only learn when speed is constant between iterations + int is_tesla; ParamsLearner(cereal::CarParams::Reader car_params, double angle_offset, diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 93523502c1f706..aaf5017dcb6bda 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -1,15 +1,22 @@ #!/usr/bin/env python3 import math +import json +import numpy as np + import cereal.messaging as messaging -from selfdrive.locationd.kalman.models.car_kf import CarKalman, ObservationKind, States +from cereal import car +from common.params import Params, put_nonblocking +from selfdrive.locationd.kalman.models.car_kf import (CarKalman, + ObservationKind, States) +from selfdrive.swaglog import cloudlog CARSTATE_DECIMATION = 5 class ParamsLearner: - def __init__(self, CP): - self.kf = CarKalman() + def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset): + self.kf = CarKalman(steer_ratio, stiffness_factor, angle_offset) self.kf.filter.set_mass(CP.mass) # pylint: disable=no-member self.kf.filter.set_rotational_inertia(CP.rotationalInertia) # pylint: disable=no-member @@ -25,47 +32,46 @@ def __init__(self, CP): self.steering_angle = 0 self.carstate_counter = 0 - def update_active(self): - self.active = (abs(self.steering_angle) < 45 or not self.steering_pressed) and self.speed > 5 - def handle_log(self, t, which, msg): if which == 'liveLocationKalman': - v_calibrated = msg.velocityCalibrated.value - # v_calibrated_std = msg.velocityCalibrated.std - self.speed = v_calibrated[0] - yaw_rate = msg.angularVelocityCalibrated.value[2] - # yaw_rate_std = msg.angularVelocityCalibrated.std[2] + yaw_rate_std = msg.angularVelocityCalibrated.std[2] - self.update_active() if self.active: - self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_YAW_RATE, [-yaw_rate]) - self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_XY_SPEED, [[v_calibrated[0], -v_calibrated[1]]]) + self.kf.predict_and_observe(t, + ObservationKind.ROAD_FRAME_YAW_RATE, + np.array([[[-yaw_rate]]]), + np.array([np.atleast_2d(yaw_rate_std**2)])) - # Clamp values - x = self.kf.x - if not (10 < x[States.STEER_RATIO] < 25): - self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, [15.0]) + self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]])) - if not (0.5 < x[States.STIFFNESS] < 3.0): - self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, [1.0]) + # Clamp values + # x = self.kf.x + # if not (10 < x[States.STEER_RATIO] < 25): + # self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[[15.0]]])) - else: - self.kf.filter.filter_time = t - 0.1 + # if not (0.5 < x[States.STIFFNESS] < 3.0): + # self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[[1.0]]])) elif which == 'carState': self.carstate_counter += 1 if self.carstate_counter % CARSTATE_DECIMATION == 0: self.steering_angle = msg.steeringAngle self.steering_pressed = msg.steeringPressed + self.speed = msg.vEgo + + in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed + self.active = self.speed > 5 and in_linear_region - self.update_active() if self.active: - self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, [math.radians(msg.steeringAngle)]) - self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, [0]) - else: - self.kf.filter.filter_time = t - 0.1 + self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngle)]]])) + self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]])) + + if not self.active: + # Reset time when stopped so uncertainty doesn't grow + self.kf.filter.filter_time = t + self.kf.filter.reset_rewind() def main(sm=None, pm=None): @@ -74,13 +80,33 @@ def main(sm=None, pm=None): if pm is None: pm = messaging.PubMaster(['liveParameters']) - # TODO: Read from car params at runtime - from selfdrive.car.toyota.interface import CarInterface - from selfdrive.car.toyota.values import CAR - - CP = CarInterface.get_params(CAR.COROLLA_TSS2) - learner = ParamsLearner(CP) - + params_reader = Params() + # wait for stats about the car to come in from controls + cloudlog.info("paramsd is waiting for CarParams") + CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True)) + cloudlog.info("paramsd got CarParams") + + params = params_reader.get("LiveParameters") + + # Check if car model matches + if params is not None: + params = json.loads(params) + if params.get('carFingerprint', None) != CP.carFingerprint: + cloudlog.info("Parameter learner found parameters for wrong car.") + params = None + + if params is None: + params = { + 'carFingerprint': CP.carFingerprint, + 'steerRatio': CP.steerRatio, + 'stiffnessFactor': 1.0, + 'angleOffsetAverage': 0.0, + } + cloudlog.info("Parameter learner resetting to default values") + + learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverage'])) + + i = 0 while True: sm.update() @@ -92,9 +118,6 @@ def main(sm=None, pm=None): # TODO: set valid to false when locationd stops sending # TODO: make sure controlsd knows when there is no gyro - # TODO: move posenetValid somewhere else to show the model uncertainty alert - # TODO: Save and resume values from param - # TODO: Change KF to allow mass, etc to be inputs in predict step if sm.updated['carState']: msg = messaging.new_message('liveParameters') @@ -108,7 +131,17 @@ def main(sm=None, pm=None): msg.liveParameters.steerRatio = float(x[States.STEER_RATIO]) msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) msg.liveParameters.angleOffsetAverage = math.degrees(x[States.ANGLE_OFFSET]) - msg.liveParameters.angleOffset = math.degrees(x[States.ANGLE_OFFSET_FAST]) + msg.liveParameters.angleOffset = msg.liveParameters.angleOffsetAverage + math.degrees(x[States.ANGLE_OFFSET_FAST]) + + i += 1 + if i % 6000 == 0: # once a minute + params = { + 'carFingerprint': CP.carFingerprint, + 'steerRatio': msg.liveParameters.steerRatio, + 'stiffnessFactor': msg.liveParameters.stiffnessFactor, + 'angleOffsetAverage': msg.liveParameters.angleOffsetAverage, + } + put_nonblocking("LiveParameters", json.dumps(params)) # P = learner.kf.P # print() diff --git a/selfdrive/locationd/test/ubloxd.py b/selfdrive/locationd/test/ubloxd.py index c1351740ecbcb4..e1f4b3b1961518 100755 --- a/selfdrive/locationd/test/ubloxd.py +++ b/selfdrive/locationd/test/ubloxd.py @@ -11,7 +11,7 @@ from common import realtime import cereal.messaging as messaging from selfdrive.locationd.test.ephemeris import EphemerisData, GET_FIELD_U -from selfdrive.car.tesla.readconfig import read_config_file,CarSettings +from selfdrive.car.tesla.readconfig import CarSettings panda = os.getenv("PANDA") is not None # panda directly connected grey = not (os.getenv("EVAL") is not None) # panda through boardd diff --git a/selfdrive/locationd/test/ubloxd_easy.py b/selfdrive/locationd/test/ubloxd_easy.py index 8f226604ec75a3..e2637dde029d82 100755 --- a/selfdrive/locationd/test/ubloxd_easy.py +++ b/selfdrive/locationd/test/ubloxd_easy.py @@ -6,6 +6,7 @@ from selfdrive.locationd.test.ubloxd import gen_raw, gen_solution import zmq import cereal.messaging as messaging +from selfdrive.car.tesla.readconfig import CarSettings unlogger = os.getenv("UNLOGGER") is not None # debug prints @@ -13,40 +14,41 @@ def main(): poller = zmq.Poller() - gpsLocationExternal = messaging.pub_sock('gpsLocationExternal') - ubloxGnss = messaging.pub_sock('ubloxGnss') - - # ubloxRaw = messaging.sub_sock('ubloxRaw', poller) - - # buffer with all the messages that still need to be input into the kalman - while 1: - polld = poller.poll(timeout=1000) - for sock, mode in polld: - if mode != zmq.POLLIN: - continue - logs = messaging.drain_sock(sock) - for log in logs: - buff = log.ubloxRaw - time = log.logMonoTime - msg = ublox.UBloxMessage() - msg.add(buff) - if msg.valid(): - if msg.name() == 'NAV_PVT': - sol = gen_solution(msg) - if unlogger: - sol.logMonoTime = time - else: - sol.logMonoTime = int(realtime.sec_since_boot() * 1e9) - gpsLocationExternal.send(sol.to_bytes()) - elif msg.name() == 'RXM_RAW': - raw = gen_raw(msg) - if unlogger: - raw.logMonoTime = time - else: - raw.logMonoTime = int(realtime.sec_since_boot() * 1e9) - ubloxGnss.send(raw.to_bytes()) - else: - print("INVALID MESSAGE") + if not CarSettings().get_value("useTeslaGPS"): + gpsLocationExternal = messaging.pub_sock('gpsLocationExternal') + ubloxGnss = messaging.pub_sock('ubloxGnss') + + # ubloxRaw = messaging.sub_sock('ubloxRaw', poller) + + # buffer with all the messages that still need to be input into the kalman + while 1: + polld = poller.poll(timeout=1000) + for sock, mode in polld: + if mode != zmq.POLLIN: + continue + logs = messaging.drain_sock(sock) + for log in logs: + buff = log.ubloxRaw + time = log.logMonoTime + msg = ublox.UBloxMessage() + msg.add(buff) + if msg.valid(): + if msg.name() == 'NAV_PVT': + sol = gen_solution(msg) + if unlogger: + sol.logMonoTime = time + else: + sol.logMonoTime = int(realtime.sec_since_boot() * 1e9) + gpsLocationExternal.send(sol.to_bytes()) + elif msg.name() == 'RXM_RAW': + raw = gen_raw(msg) + if unlogger: + raw.logMonoTime = time + else: + raw.logMonoTime = int(realtime.sec_since_boot() * 1e9) + ubloxGnss.send(raw.to_bytes()) + else: + print("INVALID MESSAGE") if __name__ == "__main__": diff --git a/selfdrive/locationd/ubloxd_main.cc b/selfdrive/locationd/ubloxd_main.cc index 4e07932fff1192..58abccb86cec2b 100644 --- a/selfdrive/locationd/ubloxd_main.cc +++ b/selfdrive/locationd/ubloxd_main.cc @@ -38,90 +38,115 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func) LOGW("starting ubloxd"); signal(SIGINT, (sighandler_t) set_do_exit); signal(SIGTERM, (sighandler_t) set_do_exit); - - UbloxMsgParser parser; - - Context * c = Context::create(); - PubSocket * gpsLocationExternal = PubSocket::create(c, "gpsLocationExternal"); - PubSocket * ubloxGnss = PubSocket::create(c, "ubloxGnss"); - SubSocket * ubloxRaw = SubSocket::create(c, "ubloxRaw"); - - assert(gpsLocationExternal != NULL); - assert(ubloxGnss != NULL); - assert(ubloxRaw != NULL); - - Poller * poller = Poller::create({ubloxRaw}); - - - while (!do_exit) { - Message * msg = poll_func(poller); - if (msg == NULL) continue; - - auto amsg = kj::heapArray((msg->getSize() / sizeof(capnp::word)) + 1); - memcpy(amsg.begin(), msg->getData(), msg->getSize()); - - capnp::FlatArrayMessageReader cmsg(amsg); - cereal::Event::Reader event = cmsg.getRoot(); - - const uint8_t *data = event.getUbloxRaw().begin(); - size_t len = event.getUbloxRaw().size(); - size_t bytes_consumed = 0; - while(bytes_consumed < len && !do_exit) { - size_t bytes_consumed_this_time = 0U; - if(parser.add_data(data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) { - // New message available - if(parser.msg_class() == CLASS_NAV) { - if(parser.msg_id() == MSG_NAV_PVT) { - //LOGD("MSG_NAV_PVT"); - auto words = parser.gen_solution(); - if(words.size() > 0) { - auto bytes = words.asBytes(); - send_func(gpsLocationExternal, bytes.begin(), bytes.size()); - } - } else - LOGW("Unknown nav msg id: 0x%02X", parser.msg_id()); - } else if(parser.msg_class() == CLASS_RXM) { - if(parser.msg_id() == MSG_RXM_RAW) { - //LOGD("MSG_RXM_RAW"); - auto words = parser.gen_raw(); - if(words.size() > 0) { - auto bytes = words.asBytes(); - 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(ubloxGnss, bytes.begin(), bytes.size()); + int useTeslaGps = 0; + char line[500]; + int linenum=0; + FILE *stream; + stream = fopen("/data/bb_openpilot.cfg","r"); + while(fgets(line, 500, stream) != NULL) + { + char setting[256], value[256], oper[10]; + linenum++; + if(line[0] == '#') continue; + if(sscanf(line, "%s %s %s", setting, oper , value) != 3) + { + //fprintf(stderr, "Syntax error, line %d\n", linenum); + continue; + } + //printf("Found [%s] %s [%s]\n", setting, oper, value); + if ((strcmp("use_tesla_gps",setting) == 0) && (strcmp("True",value) == 0)) { + useTeslaGps = 1; + } + } + fclose(stream); + if (useTeslaGps == 0) { + UbloxMsgParser parser; + + Context * c = Context::create(); + PubSocket * gpsLocationExternal = PubSocket::create(c, "gpsLocationExternal"); + PubSocket * ubloxGnss = PubSocket::create(c, "ubloxGnss"); + SubSocket * ubloxRaw = SubSocket::create(c, "ubloxRaw"); + + assert(gpsLocationExternal != NULL); + assert(ubloxGnss != NULL); + assert(ubloxRaw != NULL); + + Poller * poller = Poller::create({ubloxRaw}); + + + while (!do_exit) { + Message * msg = poll_func(poller); + if (msg == NULL) continue; + + auto amsg = kj::heapArray((msg->getSize() / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), msg->getData(), msg->getSize()); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + + const uint8_t *data = event.getUbloxRaw().begin(); + size_t len = event.getUbloxRaw().size(); + size_t bytes_consumed = 0; + while(bytes_consumed < len && !do_exit) { + size_t bytes_consumed_this_time = 0U; + if(parser.add_data(data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) { + // New message available + if(parser.msg_class() == CLASS_NAV) { + if(parser.msg_id() == MSG_NAV_PVT) { + //LOGD("MSG_NAV_PVT"); + auto words = parser.gen_solution(); + if(words.size() > 0) { + auto bytes = words.asBytes(); + send_func(gpsLocationExternal, bytes.begin(), bytes.size()); + } + } else + LOGW("Unknown nav msg id: 0x%02X", parser.msg_id()); + } else if(parser.msg_class() == CLASS_RXM) { + if(parser.msg_id() == MSG_RXM_RAW) { + //LOGD("MSG_RXM_RAW"); + auto words = parser.gen_raw(); + if(words.size() > 0) { + auto bytes = words.asBytes(); + 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(ubloxGnss, bytes.begin(), bytes.size()); + } + } else + LOGW("Unknown rxm msg id: 0x%02X", parser.msg_id()); + } else if(parser.msg_class() == CLASS_MON) { + if(parser.msg_id() == MSG_MON_HW) { + //LOGD("MSG_MON_HW"); + auto words = parser.gen_mon_hw(); + if(words.size() > 0) { + auto bytes = words.asBytes(); + send_func(ubloxGnss, bytes.begin(), bytes.size()); + } + } else { + LOGW("Unknown mon msg id: 0x%02X", parser.msg_id()); } } else - LOGW("Unknown rxm msg id: 0x%02X", parser.msg_id()); - } else if(parser.msg_class() == CLASS_MON) { - if(parser.msg_id() == MSG_MON_HW) { - //LOGD("MSG_MON_HW"); - auto words = parser.gen_mon_hw(); - if(words.size() > 0) { - auto bytes = words.asBytes(); - send_func(ubloxGnss, bytes.begin(), bytes.size()); - } - } else { - LOGW("Unknown mon msg id: 0x%02X", parser.msg_id()); - } - } else - LOGW("Unknown msg class: 0x%02X", parser.msg_class()); - parser.reset(); + LOGW("Unknown msg class: 0x%02X", parser.msg_class()); + parser.reset(); + } } - bytes_consumed += bytes_consumed_this_time; + delete msg; + } + delete poller; + delete ubloxRaw; + delete ubloxGnss; + delete gpsLocationExternal; + delete c; + return 0; + } else { + LOGW("Using tesla gps..."); + while (!do_exit) { + sleep(1); } - delete msg; + return 0; } - - delete poller; - delete ubloxRaw; - delete ubloxGnss; - delete gpsLocationExternal; - delete c; - - return 0; } diff --git a/selfdrive/logcatd/logcatd.cc b/selfdrive/logcatd/logcatd.cc index 81fafb71d046c0..f1ac06f0bf4ec1 100644 --- a/selfdrive/logcatd/logcatd.cc +++ b/selfdrive/logcatd/logcatd.cc @@ -48,7 +48,7 @@ int main() { capnp::MallocMessageBuilder msg; cereal::Event::Builder event = msg.initRoot(); event.setLogMonoTime(nanos_since_boot()); - auto androidEntry = event.initAndroidLogEntry(); + auto androidEntry = event.initAndroidLog(); androidEntry.setId(log_msg.id()); androidEntry.setTs(entry.tv_sec * 1000000000ULL + entry.tv_nsec); androidEntry.setPriority(entry.priority); diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 4a63241dcd867a..07c64a225ee8ae 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python3.7 +#!/usr/bin/env python3 import os import time import sys @@ -7,14 +7,20 @@ import signal import shutil import subprocess +from selfdrive.tinklad.tinkla_interface import TinklaClient +from cereal import tinkla +from selfdrive.car.tesla.readconfig import CarSettings import datetime +import textwrap +from selfdrive.swaglog import cloudlog, add_logentries_handler from common.basedir import BASEDIR, PARAMS from common.android import ANDROID +WEBCAM = os.getenv("WEBCAM") is not None sys.path.append(os.path.join(BASEDIR, "pyextra")) os.environ['BASEDIR'] = BASEDIR -TOTAL_SCONS_NODES = 1195 +TOTAL_SCONS_NODES = 1140 prebuilt = os.path.exists(os.path.join(BASEDIR, 'prebuilt')) # Create folders needed for msgq @@ -64,9 +70,13 @@ def unblock_stdout(): if __name__ == "__main__": unblock_stdout() + +if __name__ == "__main__" and ANDROID: from common.spinner import Spinner + from common.text_window import TextWindow else: from common.spinner import FakeSpinner as Spinner + from common.text_window import FakeTextWindow as TextWindow import importlib import traceback @@ -87,34 +97,53 @@ def unblock_stdout(): j_flag = "" if nproc is None else "-j%d" % (nproc - 1) scons = subprocess.Popen(["scons", j_flag], cwd=BASEDIR, env=env, stderr=subprocess.PIPE) + compile_output = [] + # Read progress from stderr and update spinner while scons.poll() is None: try: line = scons.stderr.readline() if line is None: continue - line = line.rstrip() + prefix = b'progress: ' if line.startswith(prefix): i = int(line[len(prefix):]) if spinner is not None: - spinner.update("%d" % (50.0 * (i / TOTAL_SCONS_NODES))) + spinner.update("%d" % (70.0 * (i / TOTAL_SCONS_NODES))) elif len(line): - print(line.decode('utf8')) + compile_output.append(line) + print(line.decode('utf8', 'replace')) except Exception: pass if scons.returncode != 0: + # Read remaining output + r = scons.stderr.read().split(b'\n') + compile_output += r + if retry: print("scons build failed, cleaning in") for i in range(3,-1,-1): print("....%d" % i) time.sleep(1) - subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env) + #subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env) shutil.rmtree("/tmp/scons_cache") else: - raise RuntimeError("scons build failed") + # Build failed log errors + errors = [line.decode('utf8', 'replace') for line in compile_output + if any([err in line for err in [b'error: ', b'not found, needed by target']])] + error_s = "\n".join(errors) + add_logentries_handler(cloudlog) + cloudlog.error("scons build failed\n" + error_s) + + # Show TextWindow + error_s = "\n \n".join(["\n".join(textwrap.wrap(e, 65)) for e in errors]) + with TextWindow("Openpilot failed to build\n \n" + error_s) as t: + t.wait_for_exit() + + exit(1) else: break @@ -123,7 +152,6 @@ def unblock_stdout(): from common.params import Params import selfdrive.crash as crash -from selfdrive.swaglog import cloudlog from selfdrive.registration import register from selfdrive.version import version, dirty from selfdrive.loggerd.config import ROOT @@ -136,6 +164,7 @@ def unblock_stdout(): # comment out anything you don't want to run managed_processes = { + "tinklad": "selfdrive.tinklad.tinklad", "thermald": "selfdrive.thermald.thermald", "uploader": "selfdrive.loggerd.uploader", "deleter": "selfdrive.loggerd.deleter", @@ -162,6 +191,7 @@ def unblock_stdout(): "updated": "selfdrive.updated", "dmonitoringmodeld": ("selfdrive/modeld", ["./dmonitoringmodeld"]), "modeld": ("selfdrive/modeld", ["./modeld"]), + "driverview": "selfdrive.controls.lib.driverview", } daemon_processes = { @@ -185,11 +215,16 @@ def get_running(): green_temp_processes = ['uploader'] persistent_processes = [ + 'tinklad', 'thermald', - 'logmessaged', 'ui', - 'uploader', ] +if not WEBCAM: + persistent_processes += [ + 'logmessaged', + 'uploader', + ] + if ANDROID: persistent_processes += [ 'logcatd', @@ -200,7 +235,6 @@ def get_running(): car_started_processes = [ 'controlsd', 'plannerd', - 'loggerd', 'radard', 'dmonitoringd', 'calibrationd', @@ -211,6 +245,12 @@ def get_running(): 'ubloxd', 'locationd', ] + +if not WEBCAM: + car_started_processes += [ + 'loggerd', + ] + if ANDROID: car_started_processes += [ 'sensord', @@ -220,6 +260,11 @@ def get_running(): 'deleter', ] +if WEBCAM: + car_started_processes += [ + 'dmonitoringmodeld', +] + def register_managed_process(name, desc, car_started=False): global managed_processes, car_started_processes, persistent_processes print("registering %s" % name) @@ -353,12 +398,14 @@ def manager_init(should_register=True): if should_register: reg_res = register() if reg_res: - dongle_id, dongle_secret = reg_res + dongle_id = reg_res else: raise Exception("server registration failed") else: dongle_id = "c"*16 - + #BB + if not dongle_id: + dongle_id = "nada" # set dongle id cloudlog.info("dongle id is " + dongle_id) os.environ['DONGLE_ID'] = dongle_id @@ -383,6 +430,32 @@ def manager_init(should_register=True): os.chmod(os.path.join(BASEDIR, "cereal"), 0o755) os.chmod(os.path.join(BASEDIR, "cereal", "libmessaging_shared.so"), 0o755) +def system(cmd): + try: + cloudlog.info("running %s" % cmd) + subprocess.check_output(cmd, stderr=subprocess.STDOUT, shell=True) + except subprocess.CalledProcessError as e: + cloudlog.event("running failed", + cmd=e.cmd, + output=e.output[-1024:], + returncode=e.returncode) + +def sendUserInfoToTinkla(params, tinklaClient): + carSettings = CarSettings() + gitRemote = params.get("GitRemote") + gitBranch = params.get("GitBranch") + gitHash = params.get("GitCommit") + dongleId = params.get("DongleId") + userHandle = carSettings.userHandle + info = tinkla.Interface.UserInfo.new_message( + openPilotId=dongleId, + userHandle=userHandle, + gitRemote=gitRemote, + gitBranch=gitBranch, + gitHash=gitHash + ) + tinklaClient.setUserInfo(info) + def manager_thread(): # now loop thermal_sock = messaging.sub_sock('thermal') @@ -420,6 +493,10 @@ def manager_thread(): logger_dead = False + # Tinkla interface + last_tinklad_send_attempt_time = 0 + tinklaClient = TinklaClient() + sendUserInfoToTinkla(params=params, tinklaClient=tinklaClient) start_t = time.time() first_proc = None @@ -436,10 +513,17 @@ def manager_thread(): if p in persistent_processes: start_managed_process(p) + # Attempt to send pending messages if there's any that queued while offline + # Seems this loop runs every second or so, throttle to once every 30s + now = time.time() + if now - last_tinklad_send_attempt_time >= 30: + tinklaClient.attemptToSendPendingMessages() + last_tinklad_send_attempt_time = now + if msg.thermal.freeSpace < 0.05: logger_dead = True - if msg.thermal.started: + if msg.thermal.started and "driverview" not in running: for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) @@ -449,10 +533,15 @@ def manager_thread(): logger_dead = False for p in reversed(car_started_processes): kill_managed_process(p) + # this is ugly + if "driverview" not in running and params.get("IsDriverViewEnabled") == b"1": + start_managed_process("driverview") + elif "driverview" in running and params.get("IsDriverViewEnabled") == b"0": + kill_managed_process("driverview") # check the status of all processes, did any of them die? - running_list = ["%s%s\u001b[0m" % ("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p) for p in running] - cloudlog.debug(' '.join(running_list)) + # running_list = ["%s%s\u001b[0m" % ("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p) for p in running] + #cloudlog.debug(' '.join(running_list)) # Exit main loop when uninstall is needed if params.get("DoUninstall", encoding='utf8') == "1": @@ -467,22 +556,24 @@ def manager_thread(): # Get last sample and exit if dt > 90: - first_proc = first_proc last_proc = messaging.recv_sock(proc_sock, wait=True) cleanup_all_processes(None, None) sys.exit(print_cpu_usage(first_proc, last_proc)) def manager_prepare(spinner=None): + + carSettings = CarSettings() # build all processes os.chdir(os.path.dirname(os.path.abspath(__file__))) # Spinner has to start from 70 here - total = 100.0 if prebuilt else 50.0 + total = 100.0 if prebuilt else 30.0 for i, p in enumerate(managed_processes): if spinner is not None: - spinner.update("%d" % ((100.0 - total) + total * (i + 1) / len(managed_processes),)) + spinText = carSettings.spinnerText + spinner.update(spinText % ((100.0 - total) + total * (i + 1) / len(managed_processes),)) prepare_managed_process(p) def uninstall(): @@ -496,7 +587,8 @@ def main(): os.environ['PARAMS_PATH'] = PARAMS # the flippening! - os.system('LD_LIBRARY_PATH="" content insert --uri content://settings/system --bind name:s:user_rotation --bind value:i:1') + if not WEBCAM: + os.system('LD_LIBRARY_PATH="" content insert --uri content://settings/system --bind name:s:user_rotation --bind value:i:1') # disable bluetooth os.system('service call bluetooth_manager 8') @@ -507,6 +599,7 @@ def main(): default_params = [ ("CommunityFeaturesToggle", "0"), ("CompletedTrainingVersion", "0"), + ("IsRHD", "0"), ("IsMetric", "0"), ("RecordFront", "0"), ("HasAcceptedTerms", "0"), @@ -518,9 +611,10 @@ def main(): ("LongitudinalControl", "0"), ("LimitSetSpeed", "0"), ("LimitSetSpeedNeural", "0"), - ("LastUpdateTime", datetime.datetime.now().isoformat().encode('utf8')), + ("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')), ("OpenpilotEnabledToggle", "1"), ("LaneChangeEnabled", "1"), + ("IsDriverViewEnabled", "0"), ] # set unset params @@ -554,13 +648,29 @@ def main(): except Exception: traceback.print_exc() crash.capture_exception() + print ("EXIT ON EXCEPTION") finally: cleanup_all_processes(None, None) if params.get("DoUninstall", encoding='utf8') == "1": uninstall() + if __name__ == "__main__": - main() + try: + main() + except Exception: + add_logentries_handler(cloudlog) + cloudlog.exception("Manager failed to start") + + # Show last 3 lines of traceback + error = traceback.format_exc(3) + + error = "Manager failed to start\n \n" + error + with TextWindow(error) as t: + t.wait_for_exit() + + raise + # manual exit because we are forked sys.exit(0) diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 4c58e278f032db..690081f0e9b9a6 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -1,7 +1,7 @@ -Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc') +Import('env', 'arch', 'messaging', 'zmq', 'common', 'gpucommon', 'visionipc') lenv = env.Clone() -libs = [messaging, common, 'OpenCL', 'SNPE', 'capnp', 'zmq', 'kj', 'yuv', gpucommon, visionipc] +libs = [messaging, common, 'OpenCL', 'capnp', zmq, 'kj', 'yuv', gpucommon, visionipc] common_src = [ "models/commonmodel.c", @@ -9,11 +9,23 @@ common_src = [ "transforms/loadyuv.c", "transforms/transform.c"] -if arch == "aarch64": - libs += ['gsl', 'CB', 'gnustl_shared'] +if arch == "jarch64": + libs += ['pthread'] + # for tensorflow support + common_src = [ + "models/commonmodel.c", + "transforms/loadyuv.c", + "transforms/transform.c"] + common_src += ['runners/tfmodel.cc'] + # tell runners to use it + lenv['CFLAGS'].append("-DUSE_TF_MODEL") + lenv['CXXFLAGS'].append("-DUSE_TF_MODEL") +elif arch == "aarch64": + libs += ['SNPE', 'gsl', 'CB', 'gnustl_shared'] +elif arch == "larch64": + libs += ['SNPE', 'gsl', 'CB', 'symphony-cpu', 'pthread'] else: - libs += ['symphony-cpu', 'pthread'] - + libs += ['SNPE', 'symphony-cpu', 'pthread'] # for tensorflow support common_src += ['runners/tfmodel.cc'] # tell runners to use it diff --git a/selfdrive/modeld/dmonitoringmodeld b/selfdrive/modeld/dmonitoringmodeld index 710e0e595b208f..51b520d74333e0 100755 --- a/selfdrive/modeld/dmonitoringmodeld +++ b/selfdrive/modeld/dmonitoringmodeld @@ -1,5 +1,5 @@ #!/bin/sh -export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8:$HOME/openpilot/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH" -export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8/" +export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/:$HOME/openpilot/phonelibs/snpe/larch64:$HOME/openpilot/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH" +export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/" exec ./_dmonitoringmodeld diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc index 9fb67b6360c6d3..ca1f2a4503d45b 100644 --- a/selfdrive/modeld/dmonitoringmodeld.cc +++ b/selfdrive/modeld/dmonitoringmodeld.cc @@ -28,6 +28,8 @@ int main(int argc, char **argv) { // messaging Context *msg_context = Context::create(); PubSocket *dmonitoring_sock = PubSocket::create(msg_context, "driverState"); + SubSocket *dmonstate_sock = SubSocket::create(msg_context, "dMonitoringState", "127.0.0.1", true); + assert(dmonstate_sock != NULL); // init the models DMonitoringModelState dmonitoringmodel; @@ -46,15 +48,35 @@ int main(int argc, char **argv) { LOGW("connected with buffer size: %d", buf_info.buf_len); double last = 0; + int chk_counter = 0; while (!do_exit) { VIPCBuf *buf; VIPCBufExtra extra; buf = visionstream_get(&stream, &extra); if (buf == NULL) { printf("visionstream get failed\n"); + visionstream_destroy(&stream); break; } //printf("frame_id: %d %dx%d\n", extra.frame_id, buf_info.width, buf_info.height); + if (!dmonitoringmodel.is_rhd_checked) { + if (chk_counter >= RHD_CHECK_INTERVAL) { + Message *msg = dmonstate_sock->receive(true); + if (msg != NULL) { + auto amsg = kj::heapArray((msg->getSize() / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), msg->getData(), msg->getSize()); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + + dmonitoringmodel.is_rhd = event.getDMonitoringState().getIsRHD(); + dmonitoringmodel.is_rhd_checked = event.getDMonitoringState().getRhdChecked(); + delete msg; + } + chk_counter = 0; + } + chk_counter += 1; + } double t1 = millis_since_boot(); @@ -74,6 +96,8 @@ int main(int argc, char **argv) { visionstream_destroy(&stream); delete dmonitoring_sock; + delete dmonstate_sock; + delete msg_context; dmonitoring_free(&dmonitoringmodel); return 0; diff --git a/selfdrive/modeld/modeld b/selfdrive/modeld/modeld index bbe7006afc747e..5369034dfeeede 100755 --- a/selfdrive/modeld/modeld +++ b/selfdrive/modeld/modeld @@ -1,4 +1,4 @@ #!/bin/sh -export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8/:$HOME/openpilot/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH" +export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/:$HOME/openpilot/phonelibs/snpe/larch64:$HOME/openpilot/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH" exec ./_modeld diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index fa49d1b5dff303..68657cad46dcdb 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -86,6 +86,9 @@ void* live_thread(void *arg) { } + delete live_calibration_sock; + delete poller; + delete c; return NULL; } @@ -121,7 +124,7 @@ int main(int argc, char **argv) { err = clGetPlatformIDs(sizeof(platform_id)/sizeof(cl_platform_id), platform_id, &num_platforms); assert(err == 0); - + #ifdef QCOM int clPlatform = 0; #else @@ -163,7 +166,7 @@ int main(int argc, char **argv) { VisionStreamBufs buf_info; err = visionstream_init(&stream, VISION_STREAM_YUV, true, &buf_info); if (err) { - printf("visionstream connect fail\n"); + LOGW("visionstream connect failed"); usleep(100000); continue; } @@ -180,7 +183,8 @@ int main(int argc, char **argv) { VIPCBufExtra extra; buf = visionstream_get(&stream, &extra); if (buf == NULL) { - printf("visionstream get failed\n"); + LOGW("visionstream get failed"); + visionstream_destroy(&stream); break; } @@ -205,8 +209,8 @@ int main(int argc, char **argv) { double mt1 = 0, mt2 = 0; if (run_model_this_iter) { - float vec_desire[DESIRE_SIZE] = {0}; - if (desire >= 0 && desire < DESIRE_SIZE) { + float vec_desire[DESIRE_LEN] = {0}; + if (desire >= 0 && desire < DESIRE_LEN) { vec_desire[desire] = 1.0; } @@ -226,9 +230,9 @@ int main(int argc, char **argv) { posenet_publish(posenet_sock, extra.frame_id, model_buf, extra.timestamp_eof); LOGD("model process: %.2fms, from last %.2fms", mt2-mt1, mt1-last); + printf("model process: %.2fms, from last %.2fms\n", mt2-mt1, mt1-last); last = mt1; } - } visionbuf_free(&yuv_ion); } @@ -236,7 +240,10 @@ int main(int argc, char **argv) { visionstream_destroy(&stream); delete model_sock; - + delete posenet_sock; + delete pathplan_sock; + delete msg_context; + model_free(&model); LOG("joining live_thread"); @@ -245,3 +252,4 @@ int main(int argc, char **argv) { return 0; } + diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 4e3ee448cfc78f..0a03d281ad4cc5 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -1,6 +1,7 @@ #ifndef COMMONMODEL_H #define COMMONMODEL_H +#define CL_USE_DEPRECATED_OPENCL_1_2_APIS #include #include "common/mat.h" diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index 787608f91a5b49..7e8d635101dc6e 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -10,8 +10,21 @@ #define MODEL_HEIGHT 320 #define FULL_W 426 +#if defined(QCOM) || defined(QCOM2) +#define input_lambda(x) (x - 128.f) * 0.0078125f +#else +#define input_lambda(x) x // for non SNPE running platforms, assume keras model instead has lambda layer +#endif + void dmonitoring_init(DMonitoringModelState* s) { - s->m = new DefaultRunModel("../../models/dmonitoring_model_q.dlc", (float*)&s->output, OUTPUT_SIZE, USE_DSP_RUNTIME); +#if defined(QCOM) || defined(QCOM2) + const char* model_path = "../../models/dmonitoring_model_q.dlc"; +#else + const char* model_path = "../../models/dmonitoring_model.dlc"; +#endif + s->m = new DefaultRunModel(model_path, (float*)&s->output, OUTPUT_SIZE, USE_DSP_RUNTIME); + s->is_rhd = false; + s->is_rhd_checked = false; } DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height) { @@ -32,7 +45,7 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ uint8_t *cropped_u_buf = cropped_y_buf + (cropped_width * cropped_height); uint8_t *cropped_v_buf = cropped_u_buf + ((cropped_width/2) * (cropped_height/2)); - if (true) { + if (!s->is_rhd) { for (int r = 0; r < height/2; r++) { memcpy(cropped_y_buf + 2*r*cropped_width, raw_y_buf + 2*r*width + (width - cropped_width), cropped_width); memcpy(cropped_y_buf + (2*r+1)*cropped_width, raw_y_buf + (2*r+1)*width + (width - cropped_width), cropped_width); @@ -58,6 +71,7 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ cropped_u_buf, cropped_width/2, cropped_v_buf, cropped_width/2, cropped_width, cropped_height); + delete[] premirror_cropped_buf; } uint8_t *resized_buf = new uint8_t[resized_width*resized_height*3/2]; @@ -84,27 +98,28 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ for (int r = 0; r < MODEL_HEIGHT/2; r++) { for (int c = 0; c < MODEL_WIDTH/2; c++) { // Y_ul - net_input_buf[(c*MODEL_HEIGHT/2) + r] = (resized_buf[(2*r*resized_width) + (2*c)] - 128.f) * 0.0078125f; + net_input_buf[(c*MODEL_HEIGHT/2) + r] = input_lambda(resized_buf[(2*r*resized_width) + (2*c)]); // Y_ur - net_input_buf[(c*MODEL_HEIGHT/2) + r + ((MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(2*r*resized_width) + (2*c+1)] - 128.f) * 0.0078125f; + net_input_buf[(c*MODEL_HEIGHT/2) + r + ((MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width) + (2*c+1)]); // Y_dl - net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(2*r*resized_width+1) + (2*c)] - 128.f) * 0.0078125f; + net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c)]); // Y_dr - net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(2*r*resized_width+1) + (2*c+1)] - 128.f) * 0.0078125f; + net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c+1)]); // U - net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(resized_width*resized_height) + (r*resized_width/2) + c] - 128.f) * 0.0078125f; + net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + (r*resized_width/2) + c]); // V - net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + (r*resized_width/2) + c] - 128.f) * 0.0078125f; + net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + (r*resized_width/2) + c]); } } - // FILE *dump_yuv_file = fopen("/sdcard/rawdump.yuv", "wb"); - // fwrite(raw_buf, height*width*3/2, sizeof(uint8_t), dump_yuv_file); - // fclose(dump_yuv_file); + //printf("preprocess completed. %d \n", yuv_buf_len); + //FILE *dump_yuv_file = fopen("/tmp/rawdump.yuv", "wb"); + //fwrite(raw_buf, height*width*3/2, sizeof(uint8_t), dump_yuv_file); + //fclose(dump_yuv_file); - // FILE *dump_yuv_file2 = fopen("/sdcard/inputdump.yuv", "wb"); - // fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file2); - // fclose(dump_yuv_file2); + //FILE *dump_yuv_file2 = fopen("/tmp/inputdump.yuv", "wb"); + //fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file2); + //fclose(dump_yuv_file2); delete[] cropped_buf; delete[] resized_buf; diff --git a/selfdrive/modeld/models/dmonitoring.h b/selfdrive/modeld/models/dmonitoring.h index 80aae5cd914874..dc5d116e986ab2 100644 --- a/selfdrive/modeld/models/dmonitoring.h +++ b/selfdrive/modeld/models/dmonitoring.h @@ -14,6 +14,7 @@ extern "C" { #endif #define OUTPUT_SIZE 33 +#define RHD_CHECK_INTERVAL 10 typedef struct DMonitoringResult { float face_orientation[3]; @@ -29,6 +30,8 @@ typedef struct DMonitoringResult { typedef struct DMonitoringModelState { RunModel *m; + bool is_rhd; + bool is_rhd_checked; float output[OUTPUT_SIZE]; } DMonitoringModelState; diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index b544a3e59ca035..5f8aaf30f5afba 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -3,6 +3,7 @@ #include #include #include "common/timing.h" +#include "common/params.h" #include "driving.h" @@ -18,7 +19,7 @@ #define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE #define OUTPUT_SIZE POSE_IDX + POSE_SIZE #ifdef TEMPORAL - #define TEMPORAL_SIZE 512 + #define TEMPORAL_SIZE 1024 //needs to be 1024 that with new model #else #define TEMPORAL_SIZE 0 #endif @@ -42,11 +43,29 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t #endif #ifdef DESIRE - s->desire = (float*)malloc(DESIRE_SIZE * sizeof(float)); - for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = 0.0; - s->pulse_desire = (float*)malloc(DESIRE_SIZE * sizeof(float)); - for (int i = 0; i < DESIRE_SIZE; i++) s->pulse_desire[i] = 0.0; - s->m->addDesire(s->pulse_desire, DESIRE_SIZE); + s->prev_desire = (float*)malloc(DESIRE_LEN * sizeof(float)); + for (int i = 0; i < DESIRE_LEN; i++) s->prev_desire[i] = 0.0; + s->pulse_desire = (float*)malloc(DESIRE_LEN * sizeof(float)); + for (int i = 0; i < DESIRE_LEN; i++) s->pulse_desire[i] = 0.0; + s->m->addDesire(s->pulse_desire, DESIRE_LEN); +#endif + +#ifdef TRAFFIC_CONVENTION + s->traffic_convention = (float*)malloc(TRAFFIC_CONVENTION_LEN * sizeof(float)); + for (int i = 0; i < TRAFFIC_CONVENTION_LEN; i++) s->traffic_convention[i] = 0.0; + s->m->addTrafficConvention(s->traffic_convention, TRAFFIC_CONVENTION_LEN); + + char *string; + const int result = read_db_value(NULL, "IsRHD", &string, NULL); + if (result == 0) { + bool is_rhd = string[0] == '1'; + free(string); + if (is_rhd) { + s->traffic_convention[1] = 1.0; + } else { + s->traffic_convention[0] = 1.0; + } + } #endif // Build Vandermonde matrix @@ -61,22 +80,24 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, cl_mem yuv_cl, int width, int height, - mat3 transform, void* sock, float *desire_in) { + mat3 transform, void* sock, + float *desire_in) { #ifdef DESIRE if (desire_in != NULL) { - for (int i = 0; i < DESIRE_SIZE; i++) { + for (int i = 0; i < DESIRE_LEN; i++) { // Model decides when action is completed // so desire input is just a pulse triggered on rising edge - if (desire_in[i] - s->desire[i] == 1) { + if (desire_in[i] - s->prev_desire[i] > .99) { s->pulse_desire[i] = desire_in[i]; } else { s->pulse_desire[i] = 0.0; } - s->desire[i] = desire_in[i]; + s->prev_desire[i] = desire_in[i]; } } #endif + //for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n"); float *new_frame_buf = frame_prepare(&s->frame, q, yuv_cl, width, height, transform); @@ -91,6 +112,8 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, assert(1==2); #endif + clEnqueueUnmapMemObject(q, s->frame.net_input, (void*)new_frame_buf, 0, NULL, NULL); + // net outputs ModelDataRaw net_outputs; net_outputs.path = &s->output[PATH_IDX]; @@ -205,14 +228,18 @@ void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_dat meta.setDesirePrediction(desire_pred); } -void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float * long_v_data, const float * long_a_data) { +void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float * long_x_data, const float * long_v_data, const float * long_a_data) { // just doing 10 vals, 1 every sec for now + float dist_arr[TIME_DISTANCE/10]; float speed_arr[TIME_DISTANCE/10]; float accel_arr[TIME_DISTANCE/10]; for (int i=0; i dist(&dist_arr[0], ARRAYSIZE(dist_arr)); + longi.setDistances(dist); kj::ArrayPtr speed(&speed_arr[0], ARRAYSIZE(speed_arr)); longi.setSpeeds(speed); kj::ArrayPtr accel(&accel_arr[0], ARRAYSIZE(accel_arr)); @@ -237,7 +264,7 @@ void model_publish(PubSocket *sock, uint32_t frame_id, auto right_lane = framed.initRightLane(); fill_path(right_lane, net_outputs.right_lane, true, -1.8); auto longi = framed.initLongitudinal(); - fill_longi(longi, net_outputs.long_v, net_outputs.long_a); + fill_longi(longi, net_outputs.long_x, net_outputs.long_v, net_outputs.long_a); // Find the distribution that corresponds to the current lead @@ -308,3 +335,4 @@ void posenet_publish(PubSocket *sock, uint32_t frame_id, auto bytes = words.asBytes(); sock->send((char*)bytes.begin(), bytes.size()); } + diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 742eb6cedb12ec..640808332321a2 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -4,10 +4,7 @@ // gate this here #define TEMPORAL #define DESIRE - -#ifdef DESIRE - #define DESIRE_SIZE 8 -#endif +#define TRAFFIC_CONVENTION #ifdef QCOM #include @@ -35,6 +32,7 @@ #define POLYFIT_DEGREE 4 #define SPEED_PERCENTILES 10 #define DESIRE_LEN 8 +#define TRAFFIC_CONVENTION_LEN 2 #define DESIRE_PRED_SIZE 32 #define OTHER_META_SIZE 4 #define LEAD_MDN_N 5 // probs for 5 groups @@ -64,9 +62,12 @@ typedef struct ModelState { float *input_frames; RunModel *m; #ifdef DESIRE - float *desire; + float *prev_desire; float *pulse_desire; #endif +#ifdef TRAFFIC_CONVENTION + float *traffic_convention; +#endif } ModelState; void model_init(ModelState* s, cl_device_id device_id, @@ -82,3 +83,4 @@ void model_publish(PubSocket* sock, uint32_t frame_id, void posenet_publish(PubSocket* sock, uint32_t frame_id, const ModelDataRaw data, uint64_t timestamp_eof); #endif + diff --git a/selfdrive/modeld/runners/runmodel.h b/selfdrive/modeld/runners/runmodel.h index 8ddc2503f68e81..d3889aaaabc162 100644 --- a/selfdrive/modeld/runners/runmodel.h +++ b/selfdrive/modeld/runners/runmodel.h @@ -5,6 +5,7 @@ class RunModel { public: virtual void addRecurrent(float *state, int state_size) {} virtual void addDesire(float *state, int state_size) {} + virtual void addTrafficConvention(float *state, int state_size) {} virtual void execute(float *net_input_buf, int buf_size) {} }; diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc index c954f9b996e8ca..084a7c7724d6b1 100644 --- a/selfdrive/modeld/runners/snpemodel.cc +++ b/selfdrive/modeld/runners/snpemodel.cc @@ -94,7 +94,11 @@ SNPEModel::SNPEModel(const char *path, float *output, size_t output_size, int ru } void SNPEModel::addRecurrent(float *state, int state_size) { - recurrentBuffer = this->addExtra(state, state_size, 2); + recurrentBuffer = this->addExtra(state, state_size, 3); +} + +void SNPEModel::addTrafficConvention(float *state, int state_size) { + trafficConventionBuffer = this->addExtra(state, state_size, 2); } void SNPEModel::addDesire(float *state, int state_size) { diff --git a/selfdrive/modeld/runners/snpemodel.h b/selfdrive/modeld/runners/snpemodel.h index f99acd095329a5..9289444b095188 100644 --- a/selfdrive/modeld/runners/snpemodel.h +++ b/selfdrive/modeld/runners/snpemodel.h @@ -24,6 +24,7 @@ class SNPEModel : public RunModel { if (model_data) free(model_data); } void addRecurrent(float *state, int state_size); + void addTrafficConvention(float *state, int state_size); void addDesire(float *state, int state_size); void execute(float *net_input_buf, int buf_size); private: @@ -44,6 +45,7 @@ class SNPEModel : public RunModel { // recurrent and desire std::unique_ptr addExtra(float *state, int state_size, int idx); std::unique_ptr recurrentBuffer; + std::unique_ptr trafficConventionBuffer; std::unique_ptr desireBuffer; }; diff --git a/selfdrive/modeld/transforms/loadyuv.cl b/selfdrive/modeld/transforms/loadyuv.cl index e015429156ce91..dd78b759cd7148 100644 --- a/selfdrive/modeld/transforms/loadyuv.cl +++ b/selfdrive/modeld/transforms/loadyuv.cl @@ -9,9 +9,7 @@ __kernel void loadys(__global uchar8 const * const Y, const int ox = ois % TRANSFORMED_WIDTH; const uchar8 ys = Y[gid]; - - // y = (x - 128) / 128 - const float8 ysf = (convert_float8(ys) - 128.f) * 0.0078125f; + const float8 ysf = convert_float8(ys); // 02 // 13 @@ -36,8 +34,7 @@ __kernel void loaduv(__global uchar8 const * const in, { const int gid = get_global_id(0); const uchar8 inv = in[gid]; - - // y = (x - 128) / 128 - const float8 outv = (convert_float8(inv) - 128.f) * 0.0078125f; + const float8 outv = convert_float8(inv); out[gid + out_offset / 8] = outv; } + diff --git a/selfdrive/modeld/transforms/transform.h b/selfdrive/modeld/transforms/transform.h index 3854be9da62db6..14f9d2365b070e 100644 --- a/selfdrive/modeld/transforms/transform.h +++ b/selfdrive/modeld/transforms/transform.h @@ -4,6 +4,7 @@ #include #include +#define CL_USE_DEPRECATED_OPENCL_1_2_APIS #ifdef __APPLE__ #include #else diff --git a/selfdrive/pandad.py b/selfdrive/pandad.py index bf747af068b29e..49af1a39961dbb 100755 --- a/selfdrive/pandad.py +++ b/selfdrive/pandad.py @@ -72,7 +72,8 @@ def update_panda(): cloudlog.info("Done flashing") if panda.bootstub: - cloudlog.info("Flashed firmware not booting, flashing development bootloader") + bootstub_version = panda.get_version() + cloudlog.info(f"Flashed firmware not booting, flashing development bootloader. Bootstub version: {bootstub_version}") panda.recover() cloudlog.info("Done flashing bootloader") @@ -87,7 +88,8 @@ def update_panda(): def main(): - update_panda() + #BB stop autoflashing of panda on reboot + #update_panda() os.chdir("boardd") os.execvp("./boardd", ["./boardd"]) diff --git a/selfdrive/registration.py b/selfdrive/registration.py index 5b01d1446f5a4b..635fb74723c79e 100644 --- a/selfdrive/registration.py +++ b/selfdrive/registration.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python3 import os import json @@ -36,7 +37,7 @@ def register(): os.chmod(PERSIST+'/comma/', 0o755) os.chmod(PERSIST+'/comma/id_rsa', 0o744) - dongle_id, access_token = params.get("DongleId", encoding='utf8'), params.get("AccessToken", encoding='utf8') + dongle_id = params.get("DongleId", encoding='utf8') public_key = open(PERSIST+"/comma/id_rsa.pub").read() # create registration token @@ -52,15 +53,14 @@ def register(): resp = api_get("v2/pilotauth/", method='POST', timeout=15, imei=get_imei(0), imei2=get_imei(1), serial=get_serial(), public_key=public_key, register_token=register_token) dongleauth = json.loads(resp.text) - dongle_id, access_token = dongleauth["dongle_id"], dongleauth["access_token"] + dongle_id = dongleauth["dongle_id"] params.put("DongleId", dongle_id) - params.put("AccessToken", access_token) - return dongle_id, access_token + return dongle_id except Exception: cloudlog.exception("failed to authenticate") - if dongle_id is not None and access_token is not None: - return dongle_id, access_token + if dongle_id is not None: + return dongle_id else: return None diff --git a/selfdrive/sensord/SConscript b/selfdrive/sensord/SConscript index b285cf97549e9c..4a7f3d28fa71f5 100644 --- a/selfdrive/sensord/SConscript +++ b/selfdrive/sensord/SConscript @@ -2,4 +2,4 @@ Import('env', 'common', 'messaging') env.Program('_sensord', 'sensors.cc', LIBS=['hardware', common, 'json', messaging, 'capnp', 'zmq', 'kj']) lenv = env.Clone() lenv['LIBPATH'] += ['/system/vendor/lib64'] -lenv.Program('_gpsd', ['gpsd.cc', 'rawgps.cc'], LIBS=['hardware', common, 'diag', 'time_genoff', 'json', messaging, 'capnp', 'zmq', 'kj']) +lenv.Program('_gpsd', ['gpsd.cc'], LIBS=['hardware', common, 'diag', 'time_genoff', 'json', messaging, 'capnp', 'zmq', 'kj']) diff --git a/selfdrive/sensord/gpsd.cc b/selfdrive/sensord/gpsd.cc index ae2ecec0352c4b..4f3b7079b13e86 100644 --- a/selfdrive/sensord/gpsd.cc +++ b/selfdrive/sensord/gpsd.cc @@ -24,8 +24,6 @@ #include "cereal/gen/cpp/log.capnp.h" -#include "rawgps.h" - volatile sig_atomic_t do_exit = 0; namespace { @@ -171,13 +169,6 @@ void gps_destroy() { gGpsInterface->cleanup(); } - -int64_t arm_cntpct() { - int64_t v; - asm volatile("mrs %0, cntpct_el0" : "=r"(v)); - return v; -} - } int main() { @@ -189,12 +180,8 @@ int main() { gps_init(); - rawgps_init(); - while(!do_exit) pause(); - rawgps_destroy(); - gps_destroy(); return 0; diff --git a/selfdrive/sensord/sensors.cc b/selfdrive/sensord/sensors.cc index 952a4863c20393..5e8741dbb3c867 100644 --- a/selfdrive/sensord/sensors.cc +++ b/selfdrive/sensord/sensors.cc @@ -225,7 +225,7 @@ void sensor_loop() { break; } } - + sensors_close(device); delete sensor_events_sock; delete c; } diff --git a/selfdrive/swaglog.py b/selfdrive/swaglog.py index 90962cb5d84933..48447527c68c5a 100644 --- a/selfdrive/swaglog.py +++ b/selfdrive/swaglog.py @@ -1,10 +1,18 @@ import os import logging +from logentries import LogentriesHandler import zmq from common.logging_extra import SwagLogger, SwagFormatter + +def get_le_handler(): + # setup logentries. we forward log messages to it + le_token = "e8549616-0798-4d7e-a2ca-2513ae81fa17" + return LogentriesHandler(le_token, use_tls=False, verbose=False) + + class LogMessageHandler(logging.Handler): def __init__(self, formatter): logging.Handler.__init__(self) @@ -31,10 +39,18 @@ def emit(self, record): # drop :/ pass + +def add_logentries_handler(log): + """Function to add the logentries handler to swaglog. + This can be used to send logs when logmessaged is not running.""" + handler = get_le_handler() + handler.setFormatter(SwagFormatter(log)) + log.addHandler(handler) + + cloudlog = log = SwagLogger() log.setLevel(logging.DEBUG) outhandler = logging.StreamHandler() log.addHandler(outhandler) - log.addHandler(LogMessageHandler(SwagFormatter(log))) diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index f57fc83c3d62bf..dfeb40811e4089 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -14,6 +14,7 @@ INJECT_MODEL = 0 segments = [ + ("TESLA", "d3126df386f83c4d|2020-04-22--13-17-39--3"), # TESLA.MODELS ("HONDA", "0375fdf7b1ce594d|2019-06-13--08-32-25--3"), # HONDA.ACCORD ("HONDA", "99c94dc769b5d96e|2019-08-03--14-19-59--2"), # HONDA.CIVIC ("TOYOTA", "77611a1fac303767|2020-02-29--13-29-33--3"), # TOYOTA.COROLLA_TSS2 diff --git a/selfdrive/test/test_car_models.py b/selfdrive/test/test_car_models.py index fd7a8e60bee0d7..743f59d2c69e00 100755 --- a/selfdrive/test/test_car_models.py +++ b/selfdrive/test/test_car_models.py @@ -21,6 +21,7 @@ from selfdrive.car.subaru.values import CAR as SUBARU from selfdrive.car.volkswagen.values import CAR as VOLKSWAGEN from selfdrive.car.nissan.values import CAR as NISSAN +from selfdrive.car.tesla.values import CAR as TESLA os.environ['NOCRASH'] = '1' @@ -335,6 +336,10 @@ def get_route_log(route_name): 'carFingerprint': NISSAN.XTRAIL, 'enableCamera': True, }, + "d3126df386f83c4d|2020-04-22--13-17-39": { + 'carFingerprint': TESLA.MODELS, + 'enableCamera': True, + }, } passive_routes = [ diff --git a/selfdrive/test/test_fingerprints.py b/selfdrive/test/test_fingerprints.py index b0c4d4510f763e..8877255c6b6cb4 100755 --- a/selfdrive/test/test_fingerprints.py +++ b/selfdrive/test/test_fingerprints.py @@ -8,7 +8,7 @@ # (addr, len) CAN_IGNITION_MSGS = { 'gm': [(0x1F1, 8), (0x160, 5)], - 'tesla' : [(0x348, 8)], + #'tesla' : [(0x348, 8)], } def _get_fingerprints(): diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index e03ef69855d705..378bee6f91cd6f 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -75,16 +75,21 @@ def calculate(self, health): # Only integrate when there is no ignition # If health is None, we're probably not in a car, so we don't care - if health is None or (health.health.ignitionLine or health.health.ignitionCan): - self.last_measurement_time = None - self.power_used_uWh = 0 + if health is None or (health.health.ignitionLine or health.health.ignitionCan) or \ + health.health.hwType == log.HealthData.HwType.unknown: + with self.integration_lock: + self.last_measurement_time = None + self.next_pulsed_measurement_time = None + self.power_used_uWh = 0 return # First measurement, set integration time - if self.last_measurement_time is None: - self.last_measurement_time = now - return + with self.integration_lock: + if self.last_measurement_time is None: + self.last_measurement_time = now + return + is_uno = health.health.hwType == log.HealthData.HwType.uno # Get current power draw somehow current_power = 0 if get_battery_status() == 'Discharging': @@ -114,6 +119,7 @@ def perform_pulse_measurement(now): currents.append(get_battery_current()) time.sleep(1) current_power = ((mean(voltages) / 1000000) * (mean(currents) / 1000000)) + self._perform_integration(now, current_power * FUDGE_FACTOR) # Enable charging again @@ -126,7 +132,7 @@ def perform_pulse_measurement(now): self.next_pulsed_measurement_time = None return - elif self.next_pulsed_measurement_time is None: + elif self.next_pulsed_measurement_time is None and not is_uno: # On a charging EON with black panda, or drawing more than 400mA out of a white/grey one # Only way to get the power draw is to turn off charging for a few sec and check what the discharging rate is # We shouldn't do this very often, so make sure it has been some long-ish random time interval @@ -139,14 +145,20 @@ def perform_pulse_measurement(now): # Do the integration self._perform_integration(now, current_power) except Exception: - cloudlog.exception("Power monitoring calculation failed:") + cloudlog.exception("Power monitoring calculation failed") def _perform_integration(self, t, current_power): - self.integration_lock.acquire() - integration_time_h = (t - self.last_measurement_time) / 3600 - self.power_used_uWh += (current_power * 1000000) * integration_time_h - self.last_measurement_time = t - self.integration_lock.release() + with self.integration_lock: + try: + if self.last_measurement_time: + integration_time_h = (t - self.last_measurement_time) / 3600 + power_used = (current_power * 1000000) * integration_time_h + if power_used < 0: + raise ValueError(f"Negative power used! Integration time: {integration_time_h} h Current Power: {power_used} uWh") + self.power_used_uWh += power_used + self.last_measurement_time = t + except Exception: + cloudlog.exception("Integration failed") # Get the power usage def get_power_used(self): diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 8870ffcc0965b5..427f0f02748d08 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -1,14 +1,15 @@ -#!/usr/bin/env python3.7 +#!/usr/bin/env python3 import os import json import copy import datetime import psutil +import sys from smbus2 import SMBus from cereal import log from common.android import ANDROID, get_network_type, get_network_strength from common.basedir import BASEDIR -from common.params import Params +from common.params import Params, put_nonblocking from common.realtime import sec_since_boot, DT_TRML from common.numpy_fast import clip, interp from common.filter_simple import FirstOrderFilter @@ -17,7 +18,13 @@ import cereal.messaging as messaging from selfdrive.loggerd.config import get_available_percent from selfdrive.pandad import get_expected_signature +from selfdrive.car.tesla.readconfig import CarSettings from selfdrive.thermald.power_monitoring import PowerMonitoring, get_battery_capacity, get_battery_status, get_battery_current, get_battery_voltage, get_usb_present +WEBCAM = os.getenv("WEBCAM") is not None +if WEBCAM and (sys.version_info.major == 3 and sys.version_info.minor == 6): + from backports.datetime_fromisoformat import MonkeyPatch + MonkeyPatch.patch_fromisoformat() + FW_SIGNATURE = get_expected_signature() @@ -25,8 +32,10 @@ NetworkType = log.ThermalData.NetworkType NetworkStrength = log.ThermalData.NetworkStrength CURRENT_TAU = 15. # 15s time constant -DAYS_NO_CONNECTIVITY_MAX = 7 # do not allow to engage after a week without internet -DAYS_NO_CONNECTIVITY_PROMPT = 4 # send an offroad prompt after 4 days with no internet +CPU_TEMP_TAU = 5. # 5s time constant +DAYS_NO_CONNECTIVITY_MAX = 70 # do not allow to engage after a week without internet +DAYS_NO_CONNECTIVITY_PROMPT = 64 # send an offroad prompt after 4 days with no internet +DISCONNECT_TIMEOUT = 5. # wait 5 seconds before going offroad after disconnect so you get an alert LEON = False last_eon_fan_val = None @@ -115,7 +124,11 @@ def set_eon_fan(val): _FAN_SPEEDS = [0, 16384, 32768, 65535] # max fan speed only allowed if battery is hot _BAT_TEMP_THERSHOLD = 45. - +if CarSettings().get_value("hasNoctuaFan"): + # fan speed options + _FAN_SPEEDS = [0, 65535, 65535, 65535] # Noctua fan is super quiet, so it can run on high most of the time. + # max fan speed only allowed if battery is hot + _BAT_TEMP_THERSHOLD = 20. # No need to wait for the battery to get hot, when you have a Notua fan. def handle_fan_eon(max_cpu_temp, bat_temp, fan_speed, ignition): new_speed_h = next(speed for speed, temp_h in zip(_FAN_SPEEDS, _TEMP_THRS_H) if temp_h > max_cpu_temp) @@ -137,6 +150,25 @@ def handle_fan_eon(max_cpu_temp, bat_temp, fan_speed, ignition): return fan_speed +def check_car_battery_voltage(should_start, health, charging_disabled, msg, limitBatteryMinMax, batt_min, batt_max): + + # charging disallowed if: + # - there are health packets from panda, and; + # - 12V battery voltage is too low, and; + # - onroad isn't started + + limitBatteryMin = limitBatteryMinMax and (msg.thermal.batteryPercent < batt_min) + limitBatteryMax = limitBatteryMinMax and (msg.thermal.batteryPercent > batt_max) + #print limitBatteryMinMax,batt_min, batt_max, msg.thermal.batteryPercent + if charging_disabled and (health is None or health.health.voltage > 11800) and (limitBatteryMin or not limitBatteryMinMax): + charging_disabled = False + os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled') + elif (not charging_disabled) and ((health is not None and health.health.voltage < 11500 and not should_start) or limitBatteryMax): + charging_disabled = True + os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled') + + return charging_disabled + def handle_fan_uno(max_cpu_temp, bat_temp, fan_speed, ignition): new_speed = int(interp(max_cpu_temp, [40.0, 80.0], [0, 80])) @@ -146,7 +178,16 @@ def handle_fan_uno(max_cpu_temp, bat_temp, fan_speed, ignition): return new_speed + + def thermald_thread(): + #BB + # if limitting battery to Min-Max%. edit /data/bb_openpilot.cfg + car_set = CarSettings() + limitBatteryMinMax = car_set.get_value("limitBatteryMinMax") + batt_min = car_set.get_value("limitBattery_Min") + batt_max = car_set.get_value("limitBattery_Max") + # prevent LEECO from undervoltage BATT_PERC_OFF = 10 if LEON else 3 @@ -173,21 +214,19 @@ def thermald_thread(): network_strength = NetworkStrength.unknown current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) + cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML) health_prev = None fw_version_match_prev = True current_connectivity_alert = None time_valid_prev = True should_start_prev = False - - is_uno = (read_tz(29, clip=False) < -1000) - if is_uno or not ANDROID: - handle_fan = handle_fan_uno - else: - setup_eon_fan() - handle_fan = handle_fan_eon - + handle_fan = None + is_uno = False + # Make sure charging is enabled + charging_disabled = False params = Params() pm = PowerMonitoring() + no_panda_cnt = 0 while 1: health = messaging.recv_sock(health_sock, wait=True) @@ -195,14 +234,39 @@ def thermald_thread(): location = location.gpsLocation if location else None msg = read_thermal() - # clear car params when panda gets disconnected - if health is None and health_prev is not None: - params.panda_disconnect() - health_prev = health - if health is not None: usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client + # If we lose connection to the panda, wait 5 seconds before going offroad + if health.health.hwType == log.HealthData.HwType.unknown: + no_panda_cnt += 1 + if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML: + if ignition: + cloudlog.error("Lost panda connection while onroad") + ignition = False + else: + no_panda_cnt = 0 + ignition = health.health.ignitionLine or health.health.ignitionCan + + # Setup fan handler on first connect to panda + if handle_fan is None and health.health.hwType != log.HealthData.HwType.unknown: + is_uno = health.health.hwType == log.HealthData.HwType.uno + + if is_uno or not ANDROID: + cloudlog.info("Setting up UNO fan handler") + handle_fan = handle_fan_uno + else: + cloudlog.info("Setting up EON fan handler") + setup_eon_fan() + handle_fan = handle_fan_eon + + # Handle disconnect + if health_prev is not None: + if health.health.hwType == log.HealthData.HwType.unknown and \ + health_prev.health.hwType != log.HealthData.HwType.unknown: + params.panda_disconnect() + health_prev = health + # get_network_type is an expensive call. update every 10s if (count % int(10. / DT_TRML)) == 0: try: @@ -230,13 +294,18 @@ def thermald_thread(): current_filter.update(msg.thermal.batteryCurrent / 1e6) # TODO: add car battery voltage check - max_cpu_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, - msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 + max_cpu_temp = cpu_temp_filter.update( + max(msg.thermal.cpu0, + msg.thermal.cpu1, + msg.thermal.cpu2, + msg.thermal.cpu3) / 10.0) + max_comp_temp = max(max_cpu_temp, msg.thermal.mem / 10., msg.thermal.gpu / 10.) bat_temp = msg.thermal.bat / 1000. - fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, ignition) - msg.thermal.fanSpeed = fan_speed + if handle_fan is not None: + fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, ignition) + msg.thermal.fanSpeed = fan_speed # thermal logic with hysterisis if max_cpu_temp > 107. or bat_temp >= 63.: @@ -261,14 +330,14 @@ def thermald_thread(): # **** starting logic **** # Check for last update time and display alerts if needed - now = datetime.datetime.now() + now = datetime.datetime.utcnow() # show invalid date/time alert time_valid = now.year >= 2019 if time_valid and not time_valid_prev: params.delete("Offroad_InvalidTime") if not time_valid and time_valid_prev: - params.put("Offroad_InvalidTime", json.dumps(OFFROAD_ALERTS["Offroad_InvalidTime"])) + put_nonblocking("Offroad_InvalidTime", json.dumps(OFFROAD_ALERTS["Offroad_InvalidTime"])) time_valid_prev = time_valid # Show update prompt @@ -285,7 +354,7 @@ def thermald_thread(): if current_connectivity_alert != "expired": current_connectivity_alert = "expired" params.delete("Offroad_ConnectivityNeededPrompt") - params.put("Offroad_ConnectivityNeeded", json.dumps(OFFROAD_ALERTS["Offroad_ConnectivityNeeded"])) + put_nonblocking("Offroad_ConnectivityNeeded", json.dumps(OFFROAD_ALERTS["Offroad_ConnectivityNeeded"])) elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0)) if current_connectivity_alert != "prompt" + remaining_time: @@ -293,15 +362,12 @@ def thermald_thread(): alert_connectivity_prompt = copy.copy(OFFROAD_ALERTS["Offroad_ConnectivityNeededPrompt"]) alert_connectivity_prompt["text"] += remaining_time + " days." params.delete("Offroad_ConnectivityNeeded") - params.put("Offroad_ConnectivityNeededPrompt", json.dumps(alert_connectivity_prompt)) + put_nonblocking("Offroad_ConnectivityNeededPrompt", json.dumps(alert_connectivity_prompt)) elif current_connectivity_alert is not None: current_connectivity_alert = None params.delete("Offroad_ConnectivityNeeded") params.delete("Offroad_ConnectivityNeededPrompt") - # start constellation of processes when the car starts - ignition = health is not None and (health.health.ignitionLine or health.health.ignitionCan) - do_uninstall = params.get("DoUninstall") == b"1" accepted_terms = params.get("HasAcceptedTerms") == terms_version completed_training = params.get("CompletedTrainingVersion") == training_version @@ -325,20 +391,21 @@ def thermald_thread(): # don't start while taking snapshot if not should_start_prev: + is_viewing_driver = params.get("IsDriverViewEnabled") == b"1" is_taking_snapshot = params.get("IsTakingSnapshot") == b"1" - should_start = should_start and (not is_taking_snapshot) + should_start = should_start and (not is_taking_snapshot) and (not is_viewing_driver) if fw_version_match and not fw_version_match_prev: params.delete("Offroad_PandaFirmwareMismatch") if not fw_version_match and fw_version_match_prev: - params.put("Offroad_PandaFirmwareMismatch", json.dumps(OFFROAD_ALERTS["Offroad_PandaFirmwareMismatch"])) + put_nonblocking("Offroad_PandaFirmwareMismatch", json.dumps(OFFROAD_ALERTS["Offroad_PandaFirmwareMismatch"])) # if any CPU gets above 107 or the battery gets above 63, kill all processes # controls will warn with CPU above 95 or battery above 60 if thermal_status >= ThermalStatus.danger: should_start = False if thermal_status_prev < ThermalStatus.danger: - params.put("Offroad_TemperatureTooHigh", json.dumps(OFFROAD_ALERTS["Offroad_TemperatureTooHigh"])) + put_nonblocking("Offroad_TemperatureTooHigh", json.dumps(OFFROAD_ALERTS["Offroad_TemperatureTooHigh"])) else: if thermal_status_prev >= ThermalStatus.danger: params.delete("Offroad_TemperatureTooHigh") @@ -354,7 +421,7 @@ def thermald_thread(): os.system('echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor') else: if should_start_prev or (count == 0): - params.put("IsOffroad", "1") + put_nonblocking("IsOffroad", "1") started_ts = None if off_ts is None: @@ -371,7 +438,10 @@ def thermald_thread(): pm.calculate(health) msg.thermal.offroadPowerUsage = pm.get_power_used() - msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90 # if current is positive, then battery is being discharged + #BB have to figure how to disable charging now + charging_disabled = check_car_battery_voltage(should_start, health, charging_disabled, msg, limitBatteryMinMax, batt_min, batt_max) + + msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90 and not charging_disabled # if current is positive, then battery is being discharged msg.thermal.started = started_ts is not None msg.thermal.startedTs = int(1e9*(started_ts or 0)) @@ -379,7 +449,7 @@ def thermald_thread(): thermal_sock.send(msg.to_bytes()) if usb_power_prev and not usb_power: - params.put("Offroad_ChargeDisabled", json.dumps(OFFROAD_ALERTS["Offroad_ChargeDisabled"])) + put_nonblocking("Offroad_ChargeDisabled", json.dumps(OFFROAD_ALERTS["Offroad_ChargeDisabled"])) elif usb_power and not usb_power_prev: params.delete("Offroad_ChargeDisabled") diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py old mode 100644 new mode 100755 index 55a09233a34225..d35c3ad29099e9 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -1,7 +1,6 @@ +#!/usr/bin/env python3 import os -import re import time -import datetime from raven import Client from raven.transport.http import HTTPTransport @@ -9,96 +8,42 @@ from selfdrive.version import version, dirty from selfdrive.swaglog import cloudlog +MAX_SIZE = 100000 * 10 # Normal size is 40-100k, allow up to 1M + + def get_tombstones(): + """Returns list of (filename, ctime) for all tombstones in /data/tombstones""" DIR_DATA = "/data/tombstones/" - return [(DIR_DATA + fn, int(os.stat(DIR_DATA + fn).st_ctime) ) + return [(DIR_DATA + fn, int(os.stat(DIR_DATA + fn).st_ctime)) for fn in os.listdir(DIR_DATA) if fn.startswith("tombstone")] + def report_tombstone(fn, client): - mtime = os.path.getmtime(fn) + f_size = os.path.getsize(fn) + if f_size > MAX_SIZE: + cloudlog.error(f"Tombstone {fn} too big, {f_size}. Skipping...") + return + with open(fn, encoding='ISO-8859-1') as f: - dat = f.read() - - # see system/core/debuggerd/tombstone.cpp - parsed = re.match(r"[* ]*\n" - r"(?P
CM Version:[\s\S]*?ABI:.*\n)" - r"(?Ppid:.*\n)" - r"(?Psignal.*\n)?" - r"(?PAbort.*\n)?" - r"(?P\s+x0[\s\S]*?\n)\n" - r"(?:backtrace:\n" - r"(?P[\s\S]*?\n)\n" - r"stack:\n" - r"(?P[\s\S]*?\n)\n" - r")?", dat) - - logtail = re.search(r"--------- tail end of.*\n([\s\S]*?\n)---", dat) - logtail = logtail and logtail.group(1) - - if parsed: - parsedict = parsed.groupdict() - else: - parsedict = {} - - thread_line = parsedict.get('thread', '') - thread_parsed = re.match(r'pid: (?P\d+), tid: (?P\d+), name: (?P.*) >>> (?P.*) <<<', thread_line) - if thread_parsed: - thread_parseddict = thread_parsed.groupdict() - else: - thread_parseddict = {} - pid = thread_parseddict.get('pid', '') - tid = thread_parseddict.get('tid', '') - name = thread_parseddict.get('name', 'unknown') - cmd = thread_parseddict.get('cmd', 'unknown') - - signal_line = parsedict.get('signal', '') - signal_parsed = re.match(r'signal (?P.*?), code (?P.*?), fault addr (?P.*)\n', signal_line) - if signal_parsed: - signal_parseddict = signal_parsed.groupdict() - else: - signal_parseddict = {} - signal = signal_parseddict.get('signal', 'unknown') - code = signal_parseddict.get('code', 'unknown') - fault_addr = signal_parseddict.get('fault_addr', '') - - abort_line = parsedict.get('abort', '') - - if parsed: - message = 'Process {} ({}) got signal {} code {}'.format(name, cmd, signal, code) - if abort_line: - message += '\n'+abort_line - else: - message = fn+'\n'+dat[:1024] + contents = f.read() + + # Get summary for sentry title + message = " ".join(contents.split('\n')[5:7]) + # Cut off pid/tid, since that varies per run + name_idx = message.find('name') + if name_idx >= 0: + message = message[name_idx:] + cloudlog.error({'tombstone': message}) client.captureMessage( message=message, - date=datetime.datetime.utcfromtimestamp(mtime), - data={ - 'logger':'tombstoned', - 'platform':'other', - }, sdk={'name': 'tombstoned', 'version': '0'}, extra={ - 'fault_addr': fault_addr, - 'abort_msg': abort_line, - 'pid': pid, - 'tid': tid, - 'name':'{} ({})'.format(name, cmd), 'tombstone_fn': fn, - 'header': parsedict.get('header'), - 'registers': parsedict.get('registers'), - 'backtrace': parsedict.get('backtrace'), - 'logtail': logtail, - }, - tags={ - 'name':'{} ({})'.format(name, cmd), - 'signal':signal, - 'code':code, - 'fault_addr':fault_addr, + 'tombstone': contents }, ) - cloudlog.error({'tombstone': message}) def main(): @@ -112,11 +57,15 @@ def main(): now_tombstones = set(get_tombstones()) for fn, ctime in (now_tombstones - initial_tombstones): - cloudlog.info("reporting new tombstone %s", fn) - report_tombstone(fn, client) + try: + cloudlog.info(f"reporting new tombstone {fn}") + report_tombstone(fn, client) + except Exception: + cloudlog.exception(f"Error reporting tombstone {fn}") initial_tombstones = now_tombstones time.sleep(5) + if __name__ == "__main__": main() diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index e0c353b43ef3af..336859e5be3161 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -9,7 +9,7 @@ if arch == "aarch64": linkflags = ['-Wl,-rpath=/system/lib64,-rpath=/system/comma/usr/lib'] else: src += ['linux.cc'] - libs += ['pthread', 'glfw'] + libs += ['pthread', 'glfw', 'X11'] linkflags = [] env.Program('_ui', src, diff --git a/selfdrive/ui/bbui.h b/selfdrive/ui/bbui.h index a3b32560a56a08..84271ced1bb63b 100644 --- a/selfdrive/ui/bbui.h +++ b/selfdrive/ui/bbui.h @@ -1,6 +1,21 @@ +#pragma once + #include "cereal/gen/c/ui.capnp.h" +#if !defined(QCOM) && !defined(QCOM2) +#ifndef __APPLE__ +#define GLFW_INCLUDE_ES2 +#else +#define GLFW_INCLUDE_GLCOREARB +#endif +#define GLFW_INCLUDE_GLEXT +#include +int linux_abs_x = 0; +int linux_abs_y = 0; +UIState *mouse_ui_state; +#endif +// TODO: this is also hardcoded in common/transformations/camera.py vec3 bb_car_space_to_full_frame(const UIState *s, vec4 car_space_projective) { @@ -249,7 +264,13 @@ int bb_ui_draw_measure( UIState *s, const char* bb_value, const char* bb_uom, c } + bool bb_handle_ui_touch( UIState *s, int touch_x, int touch_y) { +#if !defined(QCOM) && !defined(QCOM2) + touch_x = (int)(vwp_w * touch_x / 1280); + touch_y = (int)(vwp_h * touch_y / 720); + printf("Linux mouse up at %d, %d ( %d, %d)\n",(int)touch_x, (int)touch_y, linux_abs_x, linux_abs_y); +#endif for(int i=0; i<6; i++) { if (s->b.btns_r[i] > 0) { if ((abs(touch_x - s->b.btns_x[i]) < s->b.btns_r[i]) && (abs(touch_y - s->b.btns_y[i]) < s->b.btns_r[i])) { @@ -290,6 +311,20 @@ bool bb_handle_ui_touch( UIState *s, int touch_x, int touch_y) { return false; }; +#if !defined(QCOM) && !defined(QCOM2) +void bb_mouse_event_handler(GLFWwindow* window, int button, int action, int mods) { + if (button == GLFW_MOUSE_BUTTON_LEFT && action == GLFW_RELEASE) { + + double xpos, ypos; + glfwGetCursorPos(window, &xpos, &ypos); + int w_width,w_height; + glfwGetWindowSize(window, &linux_abs_x, &linux_abs_y); + bb_handle_ui_touch(mouse_ui_state,(int) xpos, (int)ypos); + } +} +#endif + + int bb_get_button_status( UIState *s, char *btn_name) { int ret_status = -1; for (int i = 0; i< 6; i++) { @@ -1324,8 +1359,6 @@ void bb_ui_poll_update( UIState *s) { } capn_free(&ctx); - // wakeup bg thread since status changed - pthread_cond_signal(&s->bg_cond); } if (sock == s->b.uiSetCar_sock) { //set car model socket @@ -1340,20 +1373,20 @@ void bb_ui_poll_update( UIState *s) { if ((strcmp(s->b.car_model,(char *) datad.icCarName.str) != 0) || (strcmp(s->b.car_folder, (char *) datad.icCarFolder.str) !=0)) { strcpy(s->b.car_model, (char *) datad.icCarName.str); strcpy(s->b.car_folder, (char *) datad.icCarFolder.str); - LOGW("Car folder set (%s)", s->b.car_folder); + //LOGW("Car folder set (%s)", s->b.car_folder); if (strcmp(s->b.car_folder,"tesla")==0) { s->b.img_logo = nvgCreateImage(s->vg, "../assets/img_spinner_comma.png", 1); s->b.img_logo2 = nvgCreateImage(s->vg, "../assets/img_spinner_comma2.png", 1); - LOGW("Spinning logo set for Tesla"); + //LOGW("Spinning logo set for Tesla"); } else if (strcmp(s->b.car_folder,"honda")==0) { s->b.img_logo = nvgCreateImage(s->vg, "../assets/img_spinner_comma.honda.png", 1); s->b.img_logo2 = nvgCreateImage(s->vg, "../assets/img_spinner_comma.honda2.png", 1); - LOGW("Spinning logo set for Honda"); + //LOGW("Spinning logo set for Honda"); } else if (strcmp(s->b.car_folder,"toyota")==0) { s->b.img_logo = nvgCreateImage(s->vg, "../assets/img_spinner_comma.toyota.png", 1); s->b.img_logo2 = nvgCreateImage(s->vg, "../assets/img_spinner_comma.toyota2.png", 1); - LOGW("Spinning logo set for Toyota"); + //LOGW("Spinning logo set for Toyota"); }; } if (datad.icShowCar == 1) { diff --git a/selfdrive/ui/bbuistate.h b/selfdrive/ui/bbuistate.h index f0beb395a82c83..38df2f16e461e7 100644 --- a/selfdrive/ui/bbuistate.h +++ b/selfdrive/ui/bbuistate.h @@ -1,3 +1,4 @@ +#pragma once const int touch_timeout = 25; typedef struct UICstmButton { @@ -7,6 +8,15 @@ typedef struct UICstmButton { } UICstmButton; typedef struct BBUIState { + float scr_scale_x; + float scr_scale_y; + int scr_w; + int scr_h; + float scr_device_factor; + float scr_scissor_offset; +#if !defined(QCOM) && !defined(QCOM2) + Display *scr_display; +#endif int touch_last_x; int touch_last_y; bool touch_last; diff --git a/selfdrive/ui/dashcam.h b/selfdrive/ui/dashcam.h index 454300b020c86a..96fb9ffde91c4c 100644 --- a/selfdrive/ui/dashcam.h +++ b/selfdrive/ui/dashcam.h @@ -233,7 +233,7 @@ void draw_lock_button(UIState *s) { static void screen_draw_button(UIState *s, int touch_x, int touch_y) { // Set button to bottom left of screen - if (s->vision_connected && s->plus_state == 0) { + if (s->vision_connected && s->active_app == cereal_UiLayoutState_App_home) { if (captureState == CAPTURE_STATE_CAPTURING) { draw_lock_button(s); diff --git a/selfdrive/ui/linux.cc b/selfdrive/ui/linux.cc index 8e4a8492dcc208..c74839ee7209c5 100644 --- a/selfdrive/ui/linux.cc +++ b/selfdrive/ui/linux.cc @@ -18,11 +18,9 @@ typedef struct FramebufferState FramebufferState; typedef struct TouchState TouchState; -extern "C" { - -FramebufferState* framebuffer_init( +FramebufferState* framebuffer_init_linux( const char* name, int32_t layer, int alpha, - int *out_w, int *out_h) { + int *out_w, int *out_h, GLFWmousebuttonfun mouse_event_handler) { glfwInit(); #ifndef __APPLE__ @@ -41,7 +39,9 @@ FramebufferState* framebuffer_init( if (!window) { printf("glfwCreateWindow failed\n"); } - + if (mouse_event_handler != NULL) { + glfwSetMouseButtonCallback(window,mouse_event_handler); + } glfwMakeContextCurrent(window); glfwSwapInterval(0); @@ -55,6 +55,8 @@ FramebufferState* framebuffer_init( return (FramebufferState*)window; } +extern "C" { + void framebuffer_set_power(FramebufferState *s, int mode) { } diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index df3e9d8da21e25..6b091737afe5e2 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -12,27 +12,6 @@ extern "C"{ #include "common/glutil.h" } -// TODO: this is also hardcoded in common/transformations/camera.py -const mat3 intrinsic_matrix = (mat3){{ - 910., 0., 582., - 0., 910., 437., - 0., 0., 1. -}}; - -const uint8_t alert_colors[][4] = { - [STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xf1}, - [STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0xc8}, - [STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0xf1}, - [STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0xf1}, - [STATUS_ALERT] = {0xC9, 0x22, 0x31, 0xf1}, -}; - -const int alert_sizes[] = { - [ALERTSIZE_NONE] = 0, - [ALERTSIZE_SMALL] = 241, - [ALERTSIZE_MID] = 390, - [ALERTSIZE_FULL] = vwp_h, -}; // Projects a point in car to space to the corresponding point in full frame // image space. @@ -87,7 +66,6 @@ static void draw_chevron(UIState *s, float x_in, float y_in, float sz, nvgMoveTo(s->vg, x+(sz*1.35)+g_xo, y+sz+g_yo); nvgLineTo(s->vg, x, y-g_xo); nvgLineTo(s->vg, x-(sz*1.35)-g_xo, y+sz+g_yo); - nvgLineTo(s->vg, x+(sz*1.35)+g_xo, y+sz+g_yo); nvgClosePath(s->vg); } nvgFillColor(s->vg, glowColor); @@ -99,7 +77,6 @@ static void draw_chevron(UIState *s, float x_in, float y_in, float sz, nvgMoveTo(s->vg, x+(sz*1.25), y+sz); nvgLineTo(s->vg, x, y); nvgLineTo(s->vg, x-(sz*1.25), y+sz); - nvgLineTo(s->vg, x+(sz*1.25), y+sz); nvgClosePath(s->vg); } nvgFillColor(s->vg, fillColor); @@ -121,7 +98,7 @@ static void draw_lead(UIState *s, float d_rel, float v_rel, float y_rel){ fillAlpha = (int)(fmin(fillAlpha, 255)); } draw_chevron(s, d_rel, y_rel, 25, - nvgRGBA(201, 34, 49, fillAlpha), nvgRGBA(218, 202, 37, 255)); + nvgRGBA(201, 34, 49, fillAlpha), COLOR_YELLOW); } static void ui_draw_lane_line(UIState *s, const model_path_vertices_data *pvd, NVGcolor color) { @@ -260,7 +237,7 @@ static void ui_draw_track(UIState *s, bool is_mpc, track_vertices_data *pvd) { } else { // Draw white vision track track_bg = nvgLinearGradient(s->vg, vwp_w, vwp_h, vwp_w, vwp_h*.4, - nvgRGBA(255, 255, 255, 255), nvgRGBA(255, 255, 255, 0)); + COLOR_WHITE, COLOR_WHITE_ALPHA(0)); } nvgFillPaint(s->vg, track_bg); @@ -281,7 +258,6 @@ static void draw_steering(UIState *s, float curvature) { static void draw_frame(UIState *s) { const UIScene *scene = &s->scene; - float x1, x2, y1, y2; if (s->scene.frontview) { glBindVertexArray(s->frame_vao[1]); } else { @@ -456,7 +432,7 @@ static void ui_draw_vision_maxspeed(UIState *s) { if (is_set_over_limit) { nvgFillColor(s->vg, nvgRGBA(218, 111, 37, 180)); } else { - nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 100)); + nvgFillColor(s->vg, COLOR_BLACK_ALPHA(100)); } nvgFill(s->vg); @@ -464,42 +440,41 @@ static void ui_draw_vision_maxspeed(UIState *s) { nvgBeginPath(s->vg); nvgRoundedRect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, 20); if (is_set_over_limit) { - nvgStrokeColor(s->vg, nvgRGBA(218, 111, 37, 255)); + nvgStrokeColor(s->vg, COLOR_OCHRE); } else if (is_speedlim_valid && !s->is_ego_over_limit) { - nvgStrokeColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgStrokeColor(s->vg, COLOR_WHITE); } else if (is_speedlim_valid && s->is_ego_over_limit) { - nvgStrokeColor(s->vg, nvgRGBA(255, 255, 255, 20)); + nvgStrokeColor(s->vg, COLOR_WHITE_ALPHA(20)); } else { - nvgStrokeColor(s->vg, nvgRGBA(255, 255, 255, 100)); + nvgStrokeColor(s->vg, COLOR_WHITE_ALPHA(100)); } nvgStrokeWidth(s->vg, 10); nvgStroke(s->vg); // Draw "MAX" Text nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); - nvgFontFace(s->vg, "sans-regular"); + nvgFontFaceId(s->vg, s->font_sans_regular); nvgFontSize(s->vg, 26*2.5); if (is_cruise_set) { - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200)); + nvgFillColor(s->vg, COLOR_WHITE_ALPHA(200)); } else { - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 100)); + nvgFillColor(s->vg, COLOR_WHITE_ALPHA(100)); } nvgText(s->vg, viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 148, "MAX", NULL); // Draw Speed Text - nvgFontFace(s->vg, "sans-bold"); + nvgFontFaceId(s->vg, s->font_sans_bold); nvgFontSize(s->vg, 48*2.5); if (is_cruise_set) { snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", maxspeed_calc); - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgFillColor(s->vg, COLOR_WHITE); nvgText(s->vg, viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 242, maxspeed_str, NULL); } else { - nvgFontFace(s->vg, "sans-semibold"); + nvgFontFaceId(s->vg, s->font_sans_semibold); nvgFontSize(s->vg, 42*2.5); - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 100)); + nvgFillColor(s->vg, COLOR_WHITE_ALPHA(100)); nvgText(s->vg, viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 242, "N/A", NULL); } - } static void ui_draw_vision_speedlimit(UIState *s) { @@ -539,9 +514,9 @@ static void ui_draw_vision_speedlimit(UIState *s) { if (is_speedlim_valid && s->is_ego_over_limit) { nvgFillColor(s->vg, nvgRGBA(218, 111, 37, 180)); } else if (is_speedlim_valid) { - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgFillColor(s->vg, COLOR_WHITE); } else { - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 100)); + nvgFillColor(s->vg, COLOR_WHITE_ALPHA(100)); } nvgFill(s->vg); @@ -552,36 +527,36 @@ static void ui_draw_vision_speedlimit(UIState *s) { nvgBeginPath(s->vg); nvgRoundedRect(s->vg, viz_speedlim_x, viz_speedlim_y, viz_speedlim_w, viz_speedlim_h, 20); if (s->is_ego_over_limit) { - nvgStrokeColor(s->vg, nvgRGBA(218, 111, 37, 255)); + nvgStrokeColor(s->vg, COLOR_OCHRE); } else if (is_speedlim_valid) { - nvgStrokeColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgStrokeColor(s->vg, COLOR_WHITE); } } // Draw "Speed Limit" Text nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); - nvgFontFace(s->vg, "sans-semibold"); + nvgFontFaceId(s->vg, s->font_sans_semibold); nvgFontSize(s->vg, 50); - nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 255)); + nvgFillColor(s->vg, COLOR_BLACK); if (is_speedlim_valid && s->is_ego_over_limit) { - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgFillColor(s->vg, COLOR_WHITE); } nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2 + (is_speedlim_valid ? 6 : 0), viz_speedlim_y + (is_speedlim_valid ? 50 : 45), "SMART", NULL); nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2 + (is_speedlim_valid ? 6 : 0), viz_speedlim_y + (is_speedlim_valid ? 90 : 85), "SPEED", NULL); // Draw Speed Text - nvgFontFace(s->vg, "sans-bold"); + nvgFontFaceId(s->vg, s->font_sans_bold); nvgFontSize(s->vg, 48*2.5); if (s->is_ego_over_limit) { - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgFillColor(s->vg, COLOR_WHITE); } else { - nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 255)); + nvgFillColor(s->vg, COLOR_BLACK); } if (is_speedlim_valid) { snprintf(speedlim_str, sizeof(speedlim_str), "%d", speedlim_calc); nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2, viz_speedlim_y + (is_speedlim_valid ? 170 : 165), speedlim_str, NULL); } else { - nvgFontFace(s->vg, "sans-semibold"); + nvgFontFaceId(s->vg, s->font_sans_semibold); nvgFontSize(s->vg, 42*2.5); nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2, viz_speedlim_y + (is_speedlim_valid ? 170 : 165), "N/A", NULL); } @@ -606,14 +581,14 @@ static void ui_draw_vision_speed(UIState *s) { } else { snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 2.2369363 + 0.5)); } - nvgFontFace(s->vg, "sans-bold"); + nvgFontFaceId(s->vg, s->font_sans_bold); nvgFontSize(s->vg, 96*2.5); - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgFillColor(s->vg, COLOR_WHITE); nvgText(s->vg, viz_speed_x+viz_speed_w/2, 240, speed_str, NULL); - nvgFontFace(s->vg, "sans-regular"); + nvgFontFaceId(s->vg, s->font_sans_regular); nvgFontSize(s->vg, 36*2.5); - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200)); + nvgFillColor(s->vg, COLOR_WHITE_ALPHA(200)); if (s->is_metric) { nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "kph", NULL); @@ -633,15 +608,7 @@ static void ui_draw_vision_event(UIState *s) { if (s->scene.decel_for_model && s->scene.engaged) { // draw winding road sign const int img_turn_size = 160*1.5; - const int img_turn_x = viz_event_x-(img_turn_size/4); - const int img_turn_y = viz_event_y+bdr_s-25; - float img_turn_alpha = 1.0f; - nvgBeginPath(s->vg); - NVGpaint imgPaint = nvgImagePattern(s->vg, img_turn_x, img_turn_y, - img_turn_size, img_turn_size, 0, s->img_turn, img_turn_alpha); - nvgRect(s->vg, img_turn_x, img_turn_y, img_turn_size, img_turn_size); - nvgFillPaint(s->vg, imgPaint); - nvgFill(s->vg); + ui_draw_image(s->vg, viz_event_x - (img_turn_size / 4), viz_event_y + bdr_s - 25, img_turn_size, img_turn_size, s->img_turn, 1.0f); } else { // draw steering wheel const int bg_wheel_size = 96; @@ -660,19 +627,14 @@ static void ui_draw_vision_event(UIState *s) { if (is_engaged) { nvgFillColor(s->vg, nvgRGBA(23, 134, 68, 255)); } else if (is_warning) { - nvgFillColor(s->vg, nvgRGBA(218, 111, 37, 255)); + nvgFillColor(s->vg, COLOR_OCHRE); } else if (is_engageable) { nvgFillColor(s->vg, nvgRGBA(23, 51, 73, 255)); } nvgFill(s->vg); img_wheel_alpha = 1.0f; } - nvgBeginPath(s->vg); - NVGpaint imgPaint = nvgImagePattern(s->vg, img_wheel_x, img_wheel_y, - img_wheel_size, img_wheel_size, 0, s->img_wheel, img_wheel_alpha); - nvgRect(s->vg, img_wheel_x, img_wheel_y, img_wheel_size, img_wheel_size); - nvgFillPaint(s->vg, imgPaint); - nvgFill(s->vg); + ui_draw_image(s->vg, img_wheel_x, img_wheel_y, img_wheel_size, img_wheel_size, s->img_wheel, img_wheel_alpha); } } @@ -689,18 +651,13 @@ static void ui_draw_vision_map(UIState *s) { float map_img_alpha = map_valid ? 1.0f : 0.15f; float map_bg_alpha = map_valid ? 0.3f : 0.1f; NVGcolor map_bg = nvgRGBA(0, 0, 0, (255 * map_bg_alpha)); - NVGpaint map_img = nvgImagePattern(s->vg, map_img_x, map_img_y, - map_img_size, map_img_size, 0, s->img_map, map_img_alpha); nvgBeginPath(s->vg); nvgCircle(s->vg, map_x, (map_y + (bdr_s * 1.5)), map_size); nvgFillColor(s->vg, map_bg); nvgFill(s->vg); - nvgBeginPath(s->vg); - nvgRect(s->vg, map_img_x, map_img_y, map_img_size, map_img_size); - nvgFillPaint(s->vg, map_img); - nvgFill(s->vg); + ui_draw_image(s->vg, map_img_x, map_img_y, map_img_size, map_img_size, s->img_map, map_img_alpha); } static void ui_draw_vision_face(UIState *s) { @@ -714,6 +671,94 @@ static void ui_draw_vision_face(UIState *s) { float face_img_alpha = scene->monitoring_active ? 1.0f : 0.15f; float face_bg_alpha = scene->monitoring_active ? 0.3f : 0.1f; NVGcolor face_bg = nvgRGBA(0, 0, 0, (255 * face_bg_alpha)); + + nvgBeginPath(s->vg); + nvgCircle(s->vg, face_x, (face_y + (bdr_s * 1.5)), face_size); + nvgFillColor(s->vg, face_bg); + nvgFill(s->vg); + + ui_draw_image(s->vg, face_img_x, face_img_y, face_img_size, face_img_size, s->img_face, face_img_alpha); +} + +static void ui_draw_driver_view(UIState *s) { + const UIScene *scene = &s->scene; + s->scene.uilayout_sidebarcollapsed = true; + const int frame_x = scene->ui_viz_rx; + const int frame_w = scene->ui_viz_rw; + const int valid_frame_w = 4 * box_h / 3; + const int valid_frame_x = frame_x + (frame_w - valid_frame_w) / 2 + ff_xoffset; + + // blackout + if (!scene->is_rhd) { + nvgBeginPath(s->vg); + NVGpaint gradient = nvgLinearGradient(s->vg, valid_frame_x + valid_frame_w, + box_y, + valid_frame_x + box_h / 2, box_y, + nvgRGBAf(0,0,0,1), nvgRGBAf(0,0,0,0)); + nvgFillPaint(s->vg, gradient); + nvgRect(s->vg, valid_frame_x + box_h / 2, box_y, valid_frame_w - box_h / 2, box_h); + nvgFill(s->vg); + } else { + nvgBeginPath(s->vg); + NVGpaint gradient = nvgLinearGradient(s->vg, valid_frame_x, + box_y, + valid_frame_w - box_h / 2, box_y, + nvgRGBAf(0,0,0,1), nvgRGBAf(0,0,0,0)); + nvgFillPaint(s->vg, gradient); + nvgRect(s->vg, valid_frame_x, box_y, valid_frame_w - box_h / 2, box_h); + nvgFill(s->vg); + } + nvgBeginPath(s->vg); + nvgRect(s->vg, scene->is_rhd ? valid_frame_x:valid_frame_x + box_h / 2, box_y, valid_frame_w - box_h / 2, box_h); + nvgFillColor(s->vg, COLOR_BLACK_ALPHA(144)); + nvgFill(s->vg); + + // borders + nvgBeginPath(s->vg); + nvgRect(s->vg, frame_x, box_y, valid_frame_x - frame_x, box_h); + nvgFillColor(s->vg, nvgRGBA(23,51,73,255)); + nvgFill(s->vg); + nvgBeginPath(s->vg); + nvgRect(s->vg, valid_frame_x + valid_frame_w, box_y, frame_w - valid_frame_w - (valid_frame_x - frame_x), box_h); + nvgFillColor(s->vg, nvgRGBA(23,51,73,255)); + nvgFill(s->vg); + + // draw face box + if (scene->face_prob > 0.4) { + int fbox_x; + int fbox_y = box_y + (scene->face_y + 0.5) * box_h - 0.5 * 0.6 * box_h / 2;; + if (!scene->is_rhd) { + fbox_x = valid_frame_x + (1 - (scene->face_x + 0.5)) * (box_h / 2) - 0.5 * 0.6 * box_h / 2; + } else { + fbox_x = valid_frame_x + valid_frame_w - box_h / 2 + (scene->face_x + 0.5) * (box_h / 2) - 0.5 * 0.6 * box_h / 2; + } + if (abs(scene->face_x) <= 0.35 && abs(scene->face_y) <= 0.4) { + nvgBeginPath(s->vg); + nvgRoundedRect(s->vg, fbox_x, fbox_y, 0.6 * box_h / 2, 0.6 * box_h / 2, 35); + nvgStrokeColor(s->vg, nvgRGBAf(1.0, 1.0, 1.0, 0.8 - ((abs(scene->face_x) > abs(scene->face_y) ? abs(scene->face_x):abs(scene->face_y))) * 0.6 / 0.375)); + nvgStrokeWidth(s->vg, 10); + nvgStroke(s->vg); + } else { + nvgBeginPath(s->vg); + nvgRoundedRect(s->vg, fbox_x, fbox_y, 0.6 * box_h / 2, 0.6 * box_h / 2, 35); + nvgStrokeColor(s->vg, nvgRGBAf(1.0, 1.0, 1.0, 0.2)); + nvgStrokeWidth(s->vg, 10); + nvgStroke(s->vg); + } + } else { + ; + } + + // draw face icon + const int face_size = 85; + const int face_x = (valid_frame_x + face_size + (bdr_s * 2)) + (scene->is_rhd ? valid_frame_w - box_h / 2:0); + const int face_y = (box_y + box_h - face_size - bdr_s - (bdr_s * 1.5)); + const int face_img_size = (face_size * 1.5); + const int face_img_x = (face_x - (face_img_size / 2)); + const int face_img_y = (face_y - (face_size / 4)); + float face_img_alpha = scene->face_prob > 0.4 ? 1.0f : 0.15f; + float face_bg_alpha = scene->face_prob > 0.4 ? 0.3f : 0.1f; + NVGcolor face_bg = nvgRGBA(0, 0, 0, (255 * face_bg_alpha)); NVGpaint face_img = nvgImagePattern(s->vg, face_img_x, face_img_y, face_img_size, face_img_size, 0, s->img_face, face_img_alpha); @@ -726,6 +771,7 @@ static void ui_draw_vision_face(UIState *s) { nvgRect(s->vg, face_img_x, face_img_y, face_img_size, face_img_size); nvgFillPaint(s->vg, face_img); nvgFill(s->vg); + } static void ui_draw_vision_header(UIState *s) { @@ -771,8 +817,8 @@ void ui_draw_vision_alert(UIState *s, int va_size, int va_color, const UIScene *scene = &s->scene; int ui_viz_rx = scene->ui_viz_rx; int ui_viz_rw = scene->ui_viz_rw; - bool hasSidebar = !s->scene.uilayout_sidebarcollapsed; - bool mapEnabled = s->scene.uilayout_mapenabled; + const bool hasSidebar = !scene->uilayout_sidebarcollapsed; + const bool mapEnabled = scene->uilayout_mapenabled; bool longAlert1 = strlen(va_text1) > 15; const uint8_t *color = alert_colors[va_color]; @@ -794,27 +840,27 @@ void ui_draw_vision_alert(UIState *s, int va_size, int va_color, nvgRect(s->vg, alr_x, alr_y, alr_w, alr_h); nvgFill(s->vg); - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgFillColor(s->vg, COLOR_WHITE); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); if (va_size == ALERTSIZE_SMALL) { - nvgFontFace(s->vg, "sans-semibold"); + nvgFontFaceId(s->vg, s->font_sans_semibold); nvgFontSize(s->vg, 40*2.5); nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+15, va_text1, NULL); } else if (va_size== ALERTSIZE_MID) { - nvgFontFace(s->vg, "sans-bold"); + nvgFontFaceId(s->vg, s->font_sans_bold); nvgFontSize(s->vg, 48*2.5); nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2-45, va_text1, NULL); - nvgFontFace(s->vg, "sans-regular"); + nvgFontFaceId(s->vg, s->font_sans_regular); nvgFontSize(s->vg, 36*2.5); nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+75, va_text2, NULL); } else if (va_size== ALERTSIZE_FULL) { nvgFontSize(s->vg, (longAlert1?72:96)*2.5); - nvgFontFace(s->vg, "sans-bold"); + nvgFontFaceId(s->vg, s->font_sans_bold); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); nvgTextBox(s->vg, alr_x, alr_y+(longAlert1?360:420), alr_w-60, va_text1, NULL); nvgFontSize(s->vg, 48*2.5); - nvgFontFace(s->vg, "sans-regular"); + nvgFontFaceId(s->vg, s->font_sans_regular); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM); nvgTextBox(s->vg, alr_x, alr_h-(longAlert1?300:360), alr_w-60, va_text2, NULL); } @@ -826,13 +872,22 @@ static void ui_draw_vision(UIState *s) { int ui_viz_rw = scene->ui_viz_rw; int ui_viz_ro = scene->ui_viz_ro; - glClearColor(0.0, 0.0, 0.0, 0.0); - glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); - // Draw video frames glEnable(GL_SCISSOR_TEST); - glViewport(ui_viz_rx+ui_viz_ro, s->fb_h-(box_y+box_h), viz_w, box_h); +#if defined(QCOM) || defined(QCOM2) + glViewport(ui_viz_rx+ui_viz_ro, s->fb_h-(box_y+box_h), viz_w , box_h); glScissor(ui_viz_rx, s->fb_h-(box_y+box_h), ui_viz_rw, box_h); +#else + int b0 = int(bdr_s * s->b.scr_w / 1920); + int w0 = s->b.scr_w -2 * b0; + int h0 = s->b.scr_h -2*b0; + int x0 = b0; + int y0 = s->fb_h-(b0+h0); + int x1 = (w0-ui_viz_rw)/2 - b0; + int y1 = box_h -h0 - 2 * bdr_s; + glViewport(x0,y0,w0,h0); + glScissor(x1,y1,ui_viz_rw,box_h); +#endif glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); draw_frame(s); @@ -842,6 +897,7 @@ static void ui_draw_vision(UIState *s) { glClear(GL_STENCIL_BUFFER_BIT); nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); + nvgScale(s->vg,s->b.scr_scale_x,s->b.scr_scale_y); nvgSave(s->vg); // Draw augmented elements @@ -856,14 +912,18 @@ static void ui_draw_vision(UIState *s) { nvgRestore(s->vg); // Set Speed, Current Speed, Status/Events - ui_draw_vision_header(s); + if (!scene->frontview) { + ui_draw_vision_header(s); + } else { + ui_draw_driver_view(s); + } if (s->scene.alert_size != ALERTSIZE_NONE) { // Controls Alerts ui_draw_vision_alert(s, s->scene.alert_size, s->status, s->scene.alert_text1, s->scene.alert_text2); } else { - ui_draw_vision_footer(s); + if (!scene->frontview){ui_draw_vision_footer(s);} } @@ -871,17 +931,26 @@ static void ui_draw_vision(UIState *s) { glDisable(GL_BLEND); } -static void ui_draw_blank(UIState *s) { - glClearColor(0.0, 0.0, 0.0, 0.0); +static void ui_draw_background(UIState *s) { + int bg_status = s->status; + assert(bg_status < ARRAYSIZE(bg_colors)); + const uint8_t *color = bg_colors[bg_status]; + + glClearColor(color[0]/256.0, color[1]/256.0, color[2]/256.0, 0.0); glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); } void ui_draw(UIState *s) { - if (s->vision_connected && s->active_app == cereal_UiLayoutState_App_home && s->status != STATUS_STOPPED) { + ui_draw_background(s); + if (s->started && s->active_app == cereal_UiLayoutState_App_none && s->status != STATUS_STOPPED) { ui_draw_sidebar(s); - ui_draw_vision(s); + + if (s->vision_seen){ + ui_draw_vision(s); + } } else { - ui_draw_blank(s); + nvgScale(s->vg,s->b.scr_scale_x,s->b.scr_scale_y); + if (!s->scene.uilayout_sidebarcollapsed) { ui_draw_sidebar(s); } @@ -899,6 +968,14 @@ void ui_draw(UIState *s) { } } +void ui_draw_image(NVGcontext *vg, float x, float y, float w, float h, int image, float alpha){ + nvgBeginPath(vg); + NVGpaint imgPaint = nvgImagePattern(vg, x, y, w, h, 0, image, alpha); + nvgRect(vg, x, y, w, h); + nvgFillPaint(vg, imgPaint); + nvgFill(vg); +} + #ifdef NANOVG_GL3_IMPLEMENTATION static const char frame_vertex_shader[] = "#version 150 core\n" @@ -977,35 +1054,28 @@ void ui_nvg_init(UIState *s) { s->font_sans_bold = nvgCreateFont(s->vg, "sans-bold", "../assets/fonts/opensans_bold.ttf"); assert(s->font_sans_bold >= 0); - assert(s->img_wheel >= 0); s->img_wheel = nvgCreateImage(s->vg, "../assets/img_chffr_wheel.png", 1); - - assert(s->img_turn >= 0); + assert(s->img_wheel != 0); s->img_turn = nvgCreateImage(s->vg, "../assets/img_trafficSign_turn.png", 1); - - assert(s->img_face >= 0); + assert(s->img_turn != 0); s->img_face = nvgCreateImage(s->vg, "../assets/img_driver_face.png", 1); - - assert(s->img_map >= 0); + assert(s->img_face != 0); s->img_map = nvgCreateImage(s->vg, "../assets/img_map.png", 1); - - assert(s->img_button_settings >= 0); + assert(s->img_map != 0); s->img_button_settings = nvgCreateImage(s->vg, "../assets/images/button_settings.png", 1); - - assert(s->img_button_home >= 0); + assert(s->img_button_settings != 0); s->img_button_home = nvgCreateImage(s->vg, "../assets/images/button_home.png", 1); - - assert(s->img_battery >= 0); + assert(s->img_button_home != 0); s->img_battery = nvgCreateImage(s->vg, "../assets/images/battery.png", 1); - - assert(s->img_battery_charging >= 0); + assert(s->img_battery != 0); s->img_battery_charging = nvgCreateImage(s->vg, "../assets/images/battery_charging.png", 1); + assert(s->img_battery_charging != 0); for(int i=0;i<=5;++i) { - assert(s->img_network[i] >= 0); char network_asset[32]; snprintf(network_asset, sizeof(network_asset), "../assets/images/network_%d.png", i); s->img_network[i] = nvgCreateImage(s->vg, network_asset, 1); + assert(s->img_network[i] != 0); } // init gl diff --git a/selfdrive/ui/sidebar.cc b/selfdrive/ui/sidebar.cc index 717449dee97aac..47586104a7e88a 100644 --- a/selfdrive/ui/sidebar.cc +++ b/selfdrive/ui/sidebar.cc @@ -3,59 +3,44 @@ #include #include "ui.hpp" -static void ui_draw_sidebar_background(UIState *s, bool hasSidebar) { - int sbr_x = hasSidebar ? 0 : -(sbr_w) + bdr_s * 2; +static void ui_draw_sidebar_background(UIState *s) { + int sbr_x = !s->scene.uilayout_sidebarcollapsed ? 0 : -(sbr_w) + bdr_s * 2; nvgBeginPath(s->vg); nvgRect(s->vg, sbr_x, 0, sbr_w, vwp_h); - nvgFillColor(s->vg, COLOR_BLACK_ALPHA); + nvgFillColor(s->vg, COLOR_BLACK_ALPHA(85)); nvgFill(s->vg); } -static void ui_draw_sidebar_settings_button(UIState *s, bool hasSidebar) { +static void ui_draw_sidebar_settings_button(UIState *s) { bool settingsActive = s->active_app == cereal_UiLayoutState_App_settings; - const int settings_btn_xr = hasSidebar ? settings_btn_x : -(sbr_w); + const int settings_btn_xr = !s->scene.uilayout_sidebarcollapsed ? settings_btn_x : -(sbr_w); - nvgBeginPath(s->vg); - NVGpaint imgPaint = nvgImagePattern(s->vg, settings_btn_xr, settings_btn_y, - settings_btn_w, settings_btn_h, 0, s->img_button_settings, settingsActive ? 1.0f : 0.65f); - nvgRect(s->vg, settings_btn_xr, settings_btn_y, settings_btn_w, settings_btn_h); - nvgFillPaint(s->vg, imgPaint); - nvgFill(s->vg); + ui_draw_image(s->vg, settings_btn_xr, settings_btn_y, settings_btn_w, settings_btn_h, s->img_button_settings, settingsActive ? 1.0f : 0.65f); } -static void ui_draw_sidebar_home_button(UIState *s, bool hasSidebar) { +static void ui_draw_sidebar_home_button(UIState *s) { bool homeActive = s->active_app == cereal_UiLayoutState_App_home; - const int home_btn_xr = hasSidebar ? home_btn_x : -(sbr_w); + const int home_btn_xr = !s->scene.uilayout_sidebarcollapsed ? home_btn_x : -(sbr_w); - nvgBeginPath(s->vg); - NVGpaint imgPaint = nvgImagePattern(s->vg, home_btn_xr, home_btn_y, - home_btn_w, home_btn_h, 0, s->img_button_home, homeActive ? 1.0f : 0.65f); - nvgRect(s->vg, home_btn_xr, home_btn_y, home_btn_w, home_btn_h); - nvgFillPaint(s->vg, imgPaint); - nvgFill(s->vg); + ui_draw_image(s->vg, home_btn_xr, home_btn_y, home_btn_w, home_btn_h, s->img_button_home, homeActive ? 1.0f : 0.65f); } -static void ui_draw_sidebar_network_strength(UIState *s, bool hasSidebar) { +static void ui_draw_sidebar_network_strength(UIState *s) { const int network_img_h = 27; const int network_img_w = 176; - const int network_img_x = hasSidebar ? 58 : -(sbr_w); + const int network_img_x = !s->scene.uilayout_sidebarcollapsed ? 58 : -(sbr_w); const int network_img_y = 196; const int network_img = s->scene.networkType == cereal_ThermalData_NetworkType_none ? s->img_network[0] : s->img_network[s->scene.networkStrength + 1]; - nvgBeginPath(s->vg); - NVGpaint imgPaint = nvgImagePattern(s->vg, network_img_x, network_img_y, - network_img_w, network_img_h, 0, network_img, 1.0f); - nvgRect(s->vg, network_img_x, network_img_y, network_img_w, network_img_h); - nvgFillPaint(s->vg, imgPaint); - nvgFill(s->vg); + ui_draw_image(s->vg, network_img_x, network_img_y, network_img_w, network_img_h, network_img, 1.0f); } -static void ui_draw_sidebar_battery_icon(UIState *s, bool hasSidebar) { +static void ui_draw_sidebar_battery_icon(UIState *s) { const int battery_img_h = 36; const int battery_img_w = 76; - const int battery_img_x = hasSidebar ? 160 : -(sbr_w); + const int battery_img_x = !s->scene.uilayout_sidebarcollapsed ? 160 : -(sbr_w); const int battery_img_y = 255; int battery_img = strcmp(s->scene.batteryStatus, "Charging") == 0 ? @@ -67,16 +52,11 @@ static void ui_draw_sidebar_battery_icon(UIState *s, bool hasSidebar) { nvgFillColor(s->vg, COLOR_WHITE); nvgFill(s->vg); - nvgBeginPath(s->vg); - NVGpaint imgPaint = nvgImagePattern(s->vg, battery_img_x, battery_img_y, - battery_img_w, battery_img_h, 0, battery_img, 1.0f); - nvgRect(s->vg, battery_img_x, battery_img_y, battery_img_w, battery_img_h); - nvgFillPaint(s->vg, imgPaint); - nvgFill(s->vg); + ui_draw_image(s->vg, battery_img_x, battery_img_y, battery_img_w, battery_img_h, battery_img, 1.0f); } -static void ui_draw_sidebar_network_type(UIState *s, bool hasSidebar) { - const int network_x = hasSidebar ? 50 : -(sbr_w); +static void ui_draw_sidebar_network_type(UIState *s) { + const int network_x = !s->scene.uilayout_sidebarcollapsed ? 50 : -(sbr_w); const int network_y = 273; const int network_w = 100; const int network_h = 100; @@ -89,16 +69,17 @@ static void ui_draw_sidebar_network_type(UIState *s, bool hasSidebar) { nvgFillColor(s->vg, COLOR_WHITE); nvgFontSize(s->vg, 48); - nvgFontFace(s->vg, "sans-regular"); + nvgFontFaceId(s->vg, s->font_sans_regular); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); nvgTextBox(s->vg, network_x, network_y, network_w, network_type_str, NULL); } -static void ui_draw_sidebar_metric(UIState *s, const char* label_str, const char* value_str, const int severity, const int y_offset, const char* message_str, bool hasSidebar) { - const int metric_x = hasSidebar ? 30 : -(sbr_w); +static void ui_draw_sidebar_metric(UIState *s, const char* label_str, const char* value_str, const int severity, const int y_offset, const char* message_str) { + const int metric_x = !s->scene.uilayout_sidebarcollapsed ? 30 : -(sbr_w); const int metric_y = 338 + y_offset; const int metric_w = 240; - const int metric_h = message_str ? strlen(message_str) > 8 ? 124 : 100 : 148; + const int metric_h = message_str ? strchr(message_str, '\n') ? 124 : 100 : 148; + NVGcolor status_color; if (severity == 0) { @@ -111,7 +92,7 @@ static void ui_draw_sidebar_metric(UIState *s, const char* label_str, const char nvgBeginPath(s->vg); nvgRoundedRect(s->vg, metric_x, metric_y, metric_w, metric_h, 20); - nvgStrokeColor(s->vg, severity > 0 ? COLOR_WHITE : COLOR_WHITE_ALPHA); + nvgStrokeColor(s->vg, severity > 0 ? COLOR_WHITE : COLOR_WHITE_ALPHA(85)); nvgStrokeWidth(s->vg, 2); nvgStroke(s->vg); @@ -123,54 +104,30 @@ static void ui_draw_sidebar_metric(UIState *s, const char* label_str, const char if (!message_str) { nvgFillColor(s->vg, COLOR_WHITE); nvgFontSize(s->vg, 78); - nvgFontFace(s->vg, "sans-bold"); + nvgFontFaceId(s->vg, s->font_sans_bold); nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_MIDDLE); nvgTextBox(s->vg, metric_x + 50, metric_y + 50, metric_w - 60, value_str, NULL); nvgFillColor(s->vg, COLOR_WHITE); nvgFontSize(s->vg, 48); - nvgFontFace(s->vg, "sans-regular"); + nvgFontFaceId(s->vg, s->font_sans_regular); nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_MIDDLE); nvgTextBox(s->vg, metric_x + 50, metric_y + 50 + 66, metric_w - 60, label_str, NULL); } else { nvgFillColor(s->vg, COLOR_WHITE); nvgFontSize(s->vg, 48); - nvgFontFace(s->vg, "sans-bold"); + nvgFontFaceId(s->vg, s->font_sans_bold); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); - nvgTextBox(s->vg, metric_x + 35, metric_y + (strlen(message_str) > 8 ? 40 : 50), metric_w - 50, message_str, NULL); + nvgTextBox(s->vg, metric_x + 35, metric_y + (strchr(message_str, '\n') ? 40 : 50), metric_w - 50, message_str, NULL); } } -static void ui_draw_sidebar_storage_metric(UIState *s, bool hasSidebar) { - int storage_severity; - char storage_label_str[32]; - char storage_value_str[32]; - char storage_value_unit[32]; - const int storage_y_offset = 0; - const float storage_pct = ceilf((1.0 - s->scene.freeSpace) * 100); - - if (storage_pct < 75.0) { - storage_severity = 0; - } else if (storage_pct >= 75.0 && storage_pct < 87.0) { - storage_severity = 1; - } else if (storage_pct >= 87.0) { - storage_severity = 2; - } - - snprintf(storage_value_str, sizeof(storage_value_str), "%d", (int)storage_pct); - snprintf(storage_value_unit, sizeof(storage_value_unit), "%s", "%"); - snprintf(storage_label_str, sizeof(storage_label_str), "%s", "STORAGE"); - strcat(storage_value_str, storage_value_unit); - - ui_draw_sidebar_metric(s, storage_label_str, storage_value_str, storage_severity, storage_y_offset, NULL, hasSidebar); -} - -static void ui_draw_sidebar_temp_metric(UIState *s, bool hasSidebar) { +static void ui_draw_sidebar_temp_metric(UIState *s) { int temp_severity; char temp_label_str[32]; char temp_value_str[32]; char temp_value_unit[32]; - const int temp_y_offset = 148 + 32; + const int temp_y_offset = 0; if (s->scene.thermalStatus == cereal_ThermalData_ThermalStatus_green) { temp_severity = 0; @@ -187,48 +144,48 @@ static void ui_draw_sidebar_temp_metric(UIState *s, bool hasSidebar) { snprintf(temp_label_str, sizeof(temp_label_str), "%s", "TEMP"); strcat(temp_value_str, temp_value_unit); - ui_draw_sidebar_metric(s, temp_label_str, temp_value_str, temp_severity, temp_y_offset, NULL, hasSidebar); + ui_draw_sidebar_metric(s, temp_label_str, temp_value_str, temp_severity, temp_y_offset, NULL); } -static void ui_draw_sidebar_panda_metric(UIState *s, bool hasSidebar) { +static void ui_draw_sidebar_panda_metric(UIState *s) { int panda_severity; char panda_message_str[32]; - const int panda_y_offset = (148 + 32) * 2; + const int panda_y_offset = 32 + 148; if (s->scene.hwType == cereal_HealthData_HwType_unknown) { panda_severity = 2; - snprintf(panda_message_str, sizeof(panda_message_str), "%s", "NO PANDA"); - } else if (s->scene.hwType == cereal_HealthData_HwType_whitePanda) { - panda_severity = 0; - snprintf(panda_message_str, sizeof(panda_message_str), "%s", "PANDA ACTIVE"); - } else if ( - (s->scene.hwType == cereal_HealthData_HwType_greyPanda) || - (s->scene.hwType == cereal_HealthData_HwType_blackPanda) || - (s->scene.hwType == cereal_HealthData_HwType_uno)) { - if (s->scene.satelliteCount == -1) { - panda_severity = 0; - snprintf(panda_message_str, sizeof(panda_message_str), "%s", "PANDA ACTIVE"); - } else if (s->scene.satelliteCount < 6) { - panda_severity = 1; - snprintf(panda_message_str, sizeof(panda_message_str), "%s", "PANDA\nNO GPS"); - } else if (s->scene.satelliteCount >= 6) { - panda_severity = 0; - snprintf(panda_message_str, sizeof(panda_message_str), "%s", "PANDA GOOD GPS"); - } + snprintf(panda_message_str, sizeof(panda_message_str), "%s", "NO\nVEHICLE"); + } else { + if (s->scene.satelliteCount < 6) { + panda_severity = 1; + snprintf(panda_message_str, sizeof(panda_message_str), "%s", "VEHICLE\nNO GPS"); + } else if (s->scene.satelliteCount >= 6) { + panda_severity = 0; + snprintf(panda_message_str, sizeof(panda_message_str), "%s", "VEHICLE\nGOOD GPS"); + } } - ui_draw_sidebar_metric(s, NULL, NULL, panda_severity, panda_y_offset, panda_message_str, hasSidebar); + ui_draw_sidebar_metric(s, NULL, NULL, panda_severity, panda_y_offset, panda_message_str); +} + +static void ui_draw_sidebar_connectivity(UIState *s) { + if (s->scene.athenaStatus == NET_DISCONNECTED) { + ui_draw_sidebar_metric(s, NULL, NULL, 1, 180+158, "CONNECT\nOFFLINE"); + } else if (s->scene.athenaStatus == NET_CONNECTED) { + ui_draw_sidebar_metric(s, NULL, NULL, 0, 180+158, "CONNECT\nONLINE"); + } else { + ui_draw_sidebar_metric(s, NULL, NULL, 2, 180+158, "CONNECT\nERROR"); + } } void ui_draw_sidebar(UIState *s) { - bool hasSidebar = !s->scene.uilayout_sidebarcollapsed; - ui_draw_sidebar_background(s, hasSidebar); - ui_draw_sidebar_settings_button(s, hasSidebar); - ui_draw_sidebar_home_button(s, hasSidebar); - ui_draw_sidebar_network_strength(s, hasSidebar); - ui_draw_sidebar_battery_icon(s, hasSidebar); - ui_draw_sidebar_network_type(s, hasSidebar); - ui_draw_sidebar_storage_metric(s, hasSidebar); - ui_draw_sidebar_temp_metric(s, hasSidebar); - ui_draw_sidebar_panda_metric(s, hasSidebar); + ui_draw_sidebar_background(s); + ui_draw_sidebar_settings_button(s); + ui_draw_sidebar_home_button(s); + ui_draw_sidebar_network_strength(s); + ui_draw_sidebar_battery_icon(s); + ui_draw_sidebar_network_type(s); + ui_draw_sidebar_temp_metric(s); + ui_draw_sidebar_panda_metric(s); + ui_draw_sidebar_connectivity(s); } diff --git a/selfdrive/ui/sound.cc b/selfdrive/ui/sound.cc index 701fa56b4ce11b..87b8cdbaceeef3 100644 --- a/selfdrive/ui/sound.cc +++ b/selfdrive/ui/sound.cc @@ -13,12 +13,17 @@ extern "C"{ #include "slplay.h" } +int last_volume = 0; + void set_volume(int volume) { - char volume_change_cmd[64]; - sprintf(volume_change_cmd, "service call audio 3 i32 3 i32 %d i32 1 &", volume); + if (last_volume != volume) { + char volume_change_cmd[64]; + sprintf(volume_change_cmd, "service call audio 3 i32 3 i32 %d i32 1 &", volume); - // 5 second timeout at 60fps - int volume_changed = system(volume_change_cmd); + // 5 second timeout at 60fps + int volume_changed = system(volume_change_cmd); + last_volume = volume; + } } @@ -27,6 +32,7 @@ sound_file sound_table[] = { { cereal_CarControl_HUDControl_AudibleAlert_chimeEngage, "../assets/sounds/engaged.wav", false }, { cereal_CarControl_HUDControl_AudibleAlert_chimeWarning1, "../assets/sounds/warning_1.wav", false }, { cereal_CarControl_HUDControl_AudibleAlert_chimeWarning2, "../assets/sounds/warning_2.wav", false }, + { cereal_CarControl_HUDControl_AudibleAlert_chimeWarning2Repeat, "../assets/sounds/warning_2.wav", true }, { cereal_CarControl_HUDControl_AudibleAlert_chimeWarningRepeat, "../assets/sounds/warning_repeat.wav", true }, { cereal_CarControl_HUDControl_AudibleAlert_chimeError, "../assets/sounds/error.wav", false }, { cereal_CarControl_HUDControl_AudibleAlert_chimePrompt, "../assets/sounds/error.wav", false }, diff --git a/selfdrive/ui/spinner/spinner b/selfdrive/ui/spinner/spinner index c8f8e68ecf4ae3..17b6491b142281 100755 Binary files a/selfdrive/ui/spinner/spinner and b/selfdrive/ui/spinner/spinner differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index d212c92da72524..c7eb6f9d210b69 100755 --- a/selfdrive/ui/ui +++ b/selfdrive/ui/ui @@ -1,4 +1,9 @@ #!/bin/sh export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH" -exec ./_ui - +. /data/openpilot/selfdrive/car/tesla/readconfig.sh +TBP=/data/tinkla_buddy_pro +if test -f "$TBP"; then + exec /usr/bin/startx ./_ui $jetson_monitor_forced_resolution +else + exec ./_ui +fi diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 12a469a4a2977f..dafd32fbe8d605 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -14,10 +14,18 @@ #include "common/touch.h" #include "common/visionimg.h" #include "common/params.h" - #include "ui.hpp" #include "sound.hpp" +#if !defined(QCOM) && !defined(QCOM2) +#include +int force_rez = 0; +int force_rez_w = 0; +int force_rez_h = 0; +#endif + +#include "bbui.h" + static int last_brightness = -1; static void set_brightness(UIState *s, int brightness) { if (last_brightness != brightness && (s->awake || brightness == 0)) { @@ -30,11 +38,26 @@ static void set_brightness(UIState *s, int brightness) { } } +int event_processing_enabled = -1; +static void enable_event_processing(bool yes) { + if (event_processing_enabled != 1 && yes) { + system("service call window 18 i32 1"); // enable event processing + event_processing_enabled = 1; + } else if (event_processing_enabled != 0 && !yes) { + system("service call window 18 i32 0"); // disable event processing + event_processing_enabled = 0; + } +} + static void set_awake(UIState *s, bool awake) { #ifdef QCOM if (awake) { // 30 second timeout at 30 fps - s->awake_timeout = 30*30; + if (((s->b.tri_state_switch == 3) || (s->b.keepEonOff)) && !s->b.recording) { + s->awake_timeout = 3*30; + } else { + s->awake_timeout = 30*30; + } } if (s->awake != awake) { s->awake = awake; @@ -42,13 +65,13 @@ static void set_awake(UIState *s, bool awake) { // TODO: replace command_awake and command_sleep with direct calls to android if (awake) { LOGW("awake normal"); - system("service call window 18 i32 1"); // enable event processing framebuffer_set_power(s->fb, HWC_POWER_MODE_NORMAL); + enable_event_processing(true); } else { LOGW("awake off"); set_brightness(s, 0); - system("service call window 18 i32 0"); // disable event processing framebuffer_set_power(s->fb, HWC_POWER_MODE_OFF); + enable_event_processing(false); } } #else @@ -57,9 +80,40 @@ static void set_awake(UIState *s, bool awake) { #endif } +static void update_offroad_layout_state(UIState *s) { + struct capn rc; + capn_init_malloc(&rc); + struct capn_segment *cs = capn_root(&rc).seg; + + cereal_UiLayoutState_ptr layoutp = cereal_new_UiLayoutState(cs); + struct cereal_UiLayoutState layoutd = { + .activeApp = (cereal_UiLayoutState_App)s->active_app, + .sidebarCollapsed = s->scene.uilayout_sidebarcollapsed, + }; + cereal_write_UiLayoutState(&layoutd, layoutp); + LOGD("setting active app to %d with sidebar %d", layoutd.activeApp, layoutd.sidebarCollapsed); + + cereal_Event_ptr eventp = cereal_new_Event(cs); + struct cereal_Event event = { + .logMonoTime = nanos_since_boot(), + .which = cereal_Event_uiLayoutState, + .uiLayoutState = layoutp, + }; + cereal_write_Event(&event, eventp); + capn_setp(capn_root(&rc), 0, eventp.p); + uint8_t buf[4096]; + ssize_t rs = capn_write_mem(&rc, buf, sizeof(buf), 0); + s->offroad_sock->send((char*)buf, rs); + capn_free(&rc); +} + +#include "dashcam.h" +#include "tbui.h" + static void navigate_to_settings(UIState *s) { #ifdef QCOM - system("am broadcast -a 'ai.comma.plus.SidebarSettingsTouchUpInside'"); + s->active_app = cereal_UiLayoutState_App_settings; + update_offroad_layout_state(s); #else // computer UI doesn't have offroad settings #endif @@ -67,7 +121,12 @@ static void navigate_to_settings(UIState *s) { static void navigate_to_home(UIState *s) { #ifdef QCOM - system("am broadcast -a 'ai.comma.plus.HomeButtonTouchUpInside'"); + if (s->started) { + s->active_app = cereal_UiLayoutState_App_none; + } else { + s->active_app = cereal_UiLayoutState_App_home; + } + update_offroad_layout_state(s); #else // computer UI doesn't have offroad home #endif @@ -82,17 +141,27 @@ static void handle_sidebar_touch(UIState *s, int touch_x, int touch_y) { if (touch_x >= home_btn_x && touch_x < (home_btn_x + home_btn_w) && touch_y >= home_btn_y && touch_y < (home_btn_y + home_btn_h)) { navigate_to_home(s); - if (s->vision_connected) { + if (s->started) { s->scene.uilayout_sidebarcollapsed = true; + update_offroad_layout_state(s); } } } } +static void handle_driver_view_touch(UIState *s, int touch_x, int touch_y) { + int err = write_db_value(NULL, "IsDriverViewEnabled", "0", 1); +} + static void handle_vision_touch(UIState *s, int touch_x, int touch_y) { - if (s->vision_connected && (touch_x >= s->scene.ui_viz_rx - bdr_s) + if (s->started && (touch_x >= s->scene.ui_viz_rx - bdr_s) && (s->active_app != cereal_UiLayoutState_App_settings)) { - s->scene.uilayout_sidebarcollapsed = !s->scene.uilayout_sidebarcollapsed; + if (!s->scene.frontview) { + s->scene.uilayout_sidebarcollapsed = !s->scene.uilayout_sidebarcollapsed; + } else { + handle_driver_view_touch(s, touch_x, touch_y); + } + update_offroad_layout_state(s); } } @@ -119,6 +188,16 @@ static void read_param_float(float* param, const char* param_name) { } } +static int read_param_uint64(uint64_t* dest, const char* param_name) { + char *s; + const int result = read_db_value(NULL, param_name, &s, NULL); + if (result == 0) { + *dest = strtoull(s, NULL, 0); + free(s); + } + return result; +} + static void read_param_bool_timeout(bool* param, const char* param_name, int* timeout) { if (*timeout > 0){ (*timeout)--; @@ -137,11 +216,29 @@ static void read_param_float_timeout(float* param, const char* param_name, int* } } +static int read_param_uint64_timeout(uint64_t* dest, const char* param_name, int* timeout) { + if (*timeout > 0){ + (*timeout)--; + return 0; + } else { + return read_param_uint64(dest, param_name); + *timeout = 2 * UI_FREQ; // 0.5Hz + } +} + +static void update_offroad_layout_timeout(UIState *s, int* timeout) { + if (*timeout > 0) { + (*timeout)--; + } else { + update_offroad_layout_state(s); + *timeout = 2 * UI_FREQ; + } +} + static void ui_init(UIState *s) { memset(s, 0, sizeof(UIState)); pthread_mutex_init(&s->lock, NULL); - pthread_cond_init(&s->bg_cond, NULL); s->ctx = Context::create(); s->model_sock = SubSocket::create(s->ctx, "model"); @@ -152,6 +249,9 @@ static void ui_init(UIState *s) { s->thermal_sock = SubSocket::create(s->ctx, "thermal"); s->health_sock = SubSocket::create(s->ctx, "health"); s->ubloxgnss_sock = SubSocket::create(s->ctx, "ubloxGnss"); + s->driverstate_sock = SubSocket::create(s->ctx, "driverState"); + s->dmonitoring_sock = SubSocket::create(s->ctx, "dMonitoringState"); + s->offroad_sock = PubSocket::create(s->ctx, "offroadLayout"); assert(s->model_sock != NULL); assert(s->controlsstate_sock != NULL); @@ -161,6 +261,9 @@ static void ui_init(UIState *s) { assert(s->thermal_sock != NULL); assert(s->health_sock != NULL); assert(s->ubloxgnss_sock != NULL); + assert(s->driverstate_sock != NULL); + assert(s->dmonitoring_sock != NULL); + assert(s->offroad_sock != NULL); s->poller = Poller::create({ s->model_sock, @@ -170,7 +273,9 @@ static void ui_init(UIState *s) { s->radarstate_sock, s->thermal_sock, s->health_sock, - s->ubloxgnss_sock + s->ubloxgnss_sock, + s->driverstate_sock, + s->dmonitoring_sock }); #ifdef SHOW_SPEEDLIMIT @@ -182,14 +287,43 @@ static void ui_init(UIState *s) { s->ipc_fd = -1; // init display +#if defined(QCOM) || defined(QCOM2) s->fb = framebuffer_init("ui", 0x00010000, true, &s->fb_w, &s->fb_h); +#else + s->fb = framebuffer_init_linux("ui", 0x00010000, true, &s->fb_w, &s->fb_h,bb_mouse_event_handler); +#endif assert(s->fb); set_awake(s, true); s->model_changed = false; s->livempc_or_radarstate_changed = false; +#if defined(QCOM) || defined(QCOM2) + s->b.scr_w = vwp_w; + s->b.scr_h = vwp_h; + s->b.scr_scale_x = 1.0f; + s->b.scr_scale_y = 1.0f; + s->b.scr_device_factor = 1.0f; + s->b.scr_scissor_offset = 0.0f; +#else + //scale + if (force_rez == 1) { + s->b.scr_w = force_rez_w; + s->b.scr_h = force_rez_h; + printf("Forcing rezolution to %d x %d \n",force_rez_w,force_rez_h); + } else { + s->b.scr_display = XOpenDisplay(NULL); + Screen* xorg_s = DefaultScreenOfDisplay(s->b.scr_display); + s->b.scr_w = xorg_s->width; + s->b.scr_h = xorg_s->height; + } + s->b.scr_scale_x = (float)(s->b.scr_w) / (float)(vwp_w); + s->b.scr_scale_y = (float)(s->b.scr_h) / (float)(vwp_h); + s->b.scr_device_factor = (float)(1164) / (float)(s->b.scr_w); + s->b.scr_scissor_offset = (float)(s->b.scr_w * 3) / 4.0f - (float)(s->b.scr_h); + mouse_ui_state = s; +#endif ui_nvg_init(s); } @@ -292,10 +426,10 @@ static ModelData read_model(cereal_ModelData_ptr modelp) { } static void update_status(UIState *s, int status) { + //BB Variable for the old status + int old_status = s->status; if (s->status != status) { s->status = status; - // wake up bg thread to change - pthread_cond_signal(&s->bg_cond); } } @@ -309,12 +443,14 @@ void handle_message(UIState *s, Message * msg) { struct cereal_Event eventd; cereal_read_Event(&eventd, eventp); + int bts = bb_get_button_status(s,(char *)"sound"); if (eventd.which == cereal_Event_controlsState) { struct cereal_ControlsState datad; cereal_read_ControlsState(&datad, eventd.controlsState); s->controls_timeout = 1 * UI_FREQ; - s->controls_seen = true; + s->scene.frontview = datad.rearViewCam; + if (!s->scene.frontview){s->controls_seen = true;} if (datad.vCruise != s->scene.v_cruise) { s->scene.v_cruise_update_ts = eventd.logMonoTime; @@ -322,20 +458,24 @@ void handle_message(UIState *s, Message * msg) { s->scene.v_cruise = datad.vCruise; s->scene.v_ego = datad.vEgo; s->scene.curvature = datad.curvature; + //BB get angles + s->b.angleSteers = datad.angleSteers; + s->b.angleSteersDes = datad.angleSteersDes; + //BB END s->scene.engaged = datad.enabled; s->scene.engageable = datad.engageable; s->scene.gps_planner_active = datad.gpsPlannerActive; s->scene.monitoring_active = datad.driverMonitoringOn; - s->scene.frontview = datad.rearViewCam; - s->scene.decel_for_model = datad.decelForModel; if (datad.alertSound != cereal_CarControl_HUDControl_AudibleAlert_none && datad.alertSound != s->alert_sound) { if (s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) { stop_alert_sound(s->alert_sound); } - play_alert_sound(datad.alertSound); + if (bts !=0) { + play_alert_sound(datad.alertSound); + } s->alert_sound = datad.alertSound; snprintf(s->alert_type, sizeof(s->alert_type), "%s", datad.alertType.str); @@ -356,7 +496,6 @@ void handle_message(UIState *s, Message * msg) { } else { s->scene.alert_text2[0] = '\0'; } - s->scene.awareness_status = datad.awarenessStatus; s->scene.alert_ts = eventd.logMonoTime; @@ -371,16 +510,14 @@ void handle_message(UIState *s, Message * msg) { s->alert_size = ALERTSIZE_FULL; } - if (s->status != STATUS_STOPPED) { - if (datad.alertStatus == cereal_ControlsState_AlertStatus_userPrompt) { - update_status(s, STATUS_WARNING); - } else if (datad.alertStatus == cereal_ControlsState_AlertStatus_critical) { - update_status(s, STATUS_ALERT); - } else if (datad.enabled) { - update_status(s, STATUS_ENGAGED); - } else { - update_status(s, STATUS_DISENGAGED); - } + if (datad.alertStatus == cereal_ControlsState_AlertStatus_userPrompt) { + update_status(s, STATUS_WARNING); + } else if (datad.alertStatus == cereal_ControlsState_AlertStatus_critical) { + update_status(s, STATUS_ALERT); + } else if (datad.enabled) { + update_status(s, STATUS_ENGAGED); + } else { + update_status(s, STATUS_DISENGAGED); } s->scene.alert_blinkingrate = datad.alertBlinkingRate; @@ -452,6 +589,9 @@ void handle_message(UIState *s, Message * msg) { cereal_read_UiLayoutState(&datad, eventd.uiLayoutState); s->active_app = datad.activeApp; s->scene.uilayout_sidebarcollapsed = datad.sidebarCollapsed; + if (datad.mockEngaged != s->scene.uilayout_mockengaged) { + s->scene.uilayout_mockengaged = datad.mockEngaged; + } } else if (eventd.which == cereal_Event_liveMapData) { struct cereal_LiveMapData datad; cereal_read_LiveMapData(&datad, eventd.liveMapData); @@ -467,6 +607,29 @@ void handle_message(UIState *s, Message * msg) { s->scene.freeSpace = datad.freeSpace; s->scene.thermalStatus = datad.thermalStatus; s->scene.paTemp = datad.pa0; + + s->thermal_started = datad.started; + + //BB CPU TEMP + s->b.maxCpuTemp=datad.cpu0; + if (s->b.maxCpuTempb.maxCpuTemp=datad.cpu1; + } + else if (s->b.maxCpuTempb.maxCpuTemp=datad.cpu2; + } + else if (s->b.maxCpuTempb.maxCpuTemp=datad.cpu3; + } + s->b.maxBatTemp=datad.bat; + s->b.freeSpace=datad.freeSpace; + s->b.batteryPercent=datad.batteryPercent; + s->b.chargingEnabled=!datad.chargingDisabled; + s->b.fanSpeed=datad.fanSpeed; + //BB END CPU TEMP } else if (eventd.which == cereal_Event_ubloxGnss) { struct cereal_UbloxGnss datad; cereal_read_UbloxGnss(&datad, eventd.ubloxGnss); @@ -481,7 +644,43 @@ void handle_message(UIState *s, Message * msg) { s->scene.hwType = datad.hwType; s->hardware_timeout = 5*30; // 5 seconds at 30 fps + } else if (eventd.which == cereal_Event_driverState) { + struct cereal_DriverState datad; + cereal_read_DriverState(&datad, eventd.driverState); + + s->scene.face_prob = datad.faceProb; + + capn_list32 fxy_list = datad.facePosition; + capn_resolve(&fxy_list.p); + s->scene.face_x = capn_to_f32(capn_get32(fxy_list, 0)); + s->scene.face_y = capn_to_f32(capn_get32(fxy_list, 1)); + } else if (eventd.which == cereal_Event_dMonitoringState) { + struct cereal_DMonitoringState datad; + cereal_read_DMonitoringState(&datad, eventd.dMonitoringState); + + s->scene.is_rhd = datad.isRHD; + s->scene.awareness_status = datad.awarenessStatus; + s->preview_started = datad.isPreview; } + + s->started = s->thermal_started || s->preview_started ; + // Handle onroad/offroad transition + if (!s->started) { + if (s->status != STATUS_STOPPED) { + update_status(s, STATUS_STOPPED); + s->alert_sound_timeout = 0; + s->vision_seen = false; + s->controls_seen = false; + s->active_app = cereal_UiLayoutState_App_home; + update_offroad_layout_state(s); + } + } else if (s->status == STATUS_STOPPED) { + update_status(s, STATUS_DISENGAGED); + + s->active_app = cereal_UiLayoutState_App_none; + update_offroad_layout_state(s); + } + capn_free(&ctx); } @@ -507,6 +706,7 @@ static void ui_update(UIState *s) { int err; if (s->vision_connect_firstrun) { + set_awake(s, true); // cant run this in connector thread because opengl. // do this here for now in lieu of a run_on_main_thread event @@ -573,6 +773,7 @@ static void ui_update(UIState *s) { assert(glGetError() == GL_NO_ERROR); s->scene.uilayout_sidebarcollapsed = true; + update_offroad_layout_state(s); s->scene.ui_viz_rx = (box_x-sbr_w+bdr_s*2); s->scene.ui_viz_rw = (box_w+sbr_w-(bdr_s*2)); s->scene.ui_viz_ro = 0; @@ -647,13 +848,15 @@ static void ui_update(UIState *s) { s->cur_vision_idx = idx; // printf("v %d\n", ((uint8_t*)s->bufs[idx].addr)[0]); } + //BB merge: not sure where this will go for now + //if (((awake) && (s->b.tri_state_switch != 3) && (!s->b.keepEonOff)) || (s->b.recording)){ + // set_awake(s, true); + //} } else { assert(false); } break; } - // peek and consume all events in the zmq queue, then return. - check_messages(s); } static int vision_subscribe(int fd, VisionPacket *rp, VisionStreamType type) { @@ -721,6 +924,7 @@ static void* vision_connect_thread(void *args) { front_rp.d.stream_bufs, front_rp.num_fds, front_rp.fds); s->vision_connected = true; + s->vision_seen = true; s->vision_connect_firstrun = true; // Drain sockets @@ -785,7 +989,7 @@ static void* light_sensor_thread(void *args) { s->light_sensor = buffer[0].light; } } - + sensors_close(device); return NULL; fail: @@ -794,34 +998,6 @@ static void* light_sensor_thread(void *args) { return NULL; } -static void* bg_thread(void* args) { - UIState *s = (UIState*)args; - set_thread_name("bg"); - - FramebufferState *bg_fb = framebuffer_init("bg", 0x00001000, false, NULL, NULL); - assert(bg_fb); - - int bg_status = -1; - while(!do_exit) { - pthread_mutex_lock(&s->lock); - if (bg_status == s->status) { - // will always be signaled if it changes? - pthread_cond_wait(&s->bg_cond, &s->lock); - } - bg_status = s->status; - pthread_mutex_unlock(&s->lock); - - assert(bg_status < ARRAYSIZE(bg_colors)); - const uint8_t *color = bg_colors[bg_status]; - - glClearColor(color[0]/256.0, color[1]/256.0, color[2]/256.0, 0.0); - glClear(GL_COLOR_BUFFER_BIT); - - framebuffer_swap(bg_fb); - } - - return NULL; -} #endif @@ -849,10 +1025,23 @@ int main(int argc, char* argv[]) { zsys_handler_set(NULL); signal(SIGINT, (sighandler_t)set_do_exit); +#if !defined(QCOM) && !defined(QCOM2) + if (argc == 4) { + if (strcmp(argv[1],"-rez")==0) { + force_rez = 1; + force_rez_w = atoi(argv[2]); + force_rez_h = atoi(argv[3]); + } + } +#endif UIState uistate; UIState *s = &uistate; ui_init(s); + //BB init our UI + bb_ui_init(s); + + enable_event_processing(true); pthread_t connect_thread_handle; err = pthread_create(&connect_thread_handle, NULL, @@ -864,13 +1053,8 @@ int main(int argc, char* argv[]) { err = pthread_create(&light_sensor_thread_handle, NULL, light_sensor_thread, s); assert(err == 0); - - pthread_t bg_thread_handle; - err = pthread_create(&bg_thread_handle, NULL, - bg_thread, s); - assert(err == 0); #endif - + s->b.touch_last_width = s->scene.ui_viz_rw; TouchState touch = {0}; touch_init(&touch); s->touch_fd = touch.fd; @@ -892,10 +1076,12 @@ int main(int argc, char* argv[]) { int draws = 0; s->scene.satelliteCount = -1; + s->started = false; + s->vision_seen = false; while (!do_exit) { bool should_swap = false; - if (!s->vision_connected) { + if (!s->started) { // Delay a while to avoid 9% cpu usage while car is not started and user is keeping touching on the screen. // Don't hold the lock while sleeping, so that vision_connect_thread have chances to get the lock. usleep(30 * 1000); @@ -918,12 +1104,36 @@ int main(int argc, char* argv[]) { // poll for touch events int touch_x = -1, touch_y = -1; + int dc_touch_x = -1, dc_touch_y = -1; + s->b.touch_timeout --; + if (s->b.touch_timeout < 0) { + s->b.touch_timeout = 0; + } int touched = touch_poll(&touch, &touch_x, &touch_y, 0); if (touched == 1) { set_awake(s, true); handle_sidebar_touch(s, touch_x, touch_y); handle_vision_touch(s, touch_x, touch_y); + s->b.touch_last = true; + s->b.touch_last_x = touch_x; + s->b.touch_last_y = touch_y; + s->b.touch_timeout = touch_timeout; } + //BB check touch + if ((s->b.touch_last) && (s->b.touch_last_width != s->scene.ui_viz_rw)) { + s->b.touch_last_width=s->scene.ui_viz_rw; + bb_handle_ui_touch(s,s->b.touch_last_x,s->b.touch_last_y); + dc_touch_x = s->b.touch_last_x; + dc_touch_y = s->b.touch_last_y; + s->b.touch_last = false; + s->b.touch_last_x = 0; + s->b.touch_last_y = 0; + } + + + //s->b.touch_last_width = s->scene.ui_viz_rw; + //BB Update our cereal polls + bb_ui_poll_update(s); if (!s->vision_connected) { if (s->status != STATUS_STOPPED) { @@ -931,16 +1141,19 @@ int main(int argc, char* argv[]) { } check_messages(s); } else { - set_awake(s, true); - if (s->status == STATUS_STOPPED) { - update_status(s, STATUS_DISENGAGED); + //set_awake(s, true); + // Car started, fetch a new rgb image from ipc + if (s->vision_connected){ + ui_update(s); } - // Car started, fetch a new rgb image from ipc and peek for zmq events. - ui_update(s); - if (!s->vision_connected) { - // Visiond process is just stopped, force a redraw to make screen blank again. + + check_messages(s); + + // Visiond process is just stopped, force a redraw to make screen blank again. + if (!s->started) { s->scene.satelliteCount = -1; s->scene.uilayout_sidebarcollapsed = false; + update_offroad_layout_state(s); ui_draw(s); glFinish(); should_swap = true; @@ -964,6 +1177,12 @@ int main(int argc, char* argv[]) { // Don't waste resources on drawing in case screen is off if (s->awake) { ui_draw(s); + if (s->vision_connected) { + nvgScale(s->vg,s->b.scr_scale_x,s->b.scr_scale_y); + dashcam(s, dc_touch_x, dc_touch_y); + bb_ui_draw_UI(s) ; + ui_draw_infobar(s); + } glFinish(); should_swap = true; } @@ -976,40 +1195,48 @@ int main(int argc, char* argv[]) { s->volume_timeout = 5 * UI_FREQ; } + // If car is started and controlsState times out, display an alert if (s->controls_timeout > 0) { s->controls_timeout--; } else { - // stop playing alert sound - if ((!s->vision_connected || (s->vision_connected && s->alert_sound_timeout == 0)) && - s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) { - stop_alert_sound(s->alert_sound); - s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none; - } - - // if visiond is still running and controlsState times out, display an alert - // TODO: refactor this to not be here - if (s->controls_seen && s->vision_connected && strcmp(s->scene.alert_text2, "Controls Unresponsive") != 0) { + if (s->started && s->controls_seen && strcmp(s->scene.alert_text2, "Controls Unresponsive") != 0) { + LOGE("Controls unresponsive"); s->scene.alert_size = ALERTSIZE_FULL; - if (s->status != STATUS_STOPPED) { - update_status(s, STATUS_ALERT); - } + update_status(s, STATUS_ALERT); + snprintf(s->scene.alert_text1, sizeof(s->scene.alert_text1), "%s", "TAKE CONTROL IMMEDIATELY"); snprintf(s->scene.alert_text2, sizeof(s->scene.alert_text2), "%s", "Controls Unresponsive"); ui_draw_vision_alert(s, s->scene.alert_size, s->status, s->scene.alert_text1, s->scene.alert_text2); s->alert_sound_timeout = 2 * UI_FREQ; - s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_chimeWarningRepeat; play_alert_sound(s->alert_sound); } + s->alert_sound_timeout--; s->controls_seen = false; } + // stop playing alert sound + if ((!s->started || (s->started && s->alert_sound_timeout == 0)) && + s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) { + stop_alert_sound(s->alert_sound); + s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none; + } + read_param_bool_timeout(&s->is_metric, "IsMetric", &s->is_metric_timeout); read_param_bool_timeout(&s->longitudinal_control, "LongitudinalControl", &s->longitudinal_control_timeout); read_param_bool_timeout(&s->limit_set_speed, "LimitSetSpeed", &s->limit_set_speed_timeout); read_param_float_timeout(&s->speed_lim_off, "SpeedLimitOffset", &s->limit_set_speed_timeout); + int param_read = read_param_uint64_timeout(&s->last_athena_ping, "LastAthenaPingTime", &s->last_athena_ping_timeout); + if (param_read != 0) { + s->scene.athenaStatus = NET_DISCONNECTED; + } else if (nanos_since_boot() - s->last_athena_ping < 70e9) { + s->scene.athenaStatus = NET_CONNECTED; + } else { + s->scene.athenaStatus = NET_ERROR; + } + update_offroad_layout_timeout(s, &s->offroad_layout_timeout); pthread_mutex_unlock(&s->lock); @@ -1031,14 +1258,10 @@ int main(int argc, char* argv[]) { // wake up bg thread to exit pthread_mutex_lock(&s->lock); - pthread_cond_signal(&s->bg_cond); pthread_mutex_unlock(&s->lock); #ifdef QCOM // join light_sensor_thread? - - err = pthread_join(bg_thread_handle, NULL); - assert(err == 0); #endif err = pthread_join(connect_thread_handle, NULL); diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index ee431a4496650b..71402bc5e48a4a 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -1,3 +1,4 @@ +#pragma once #ifndef _UI_H #define _UI_H @@ -12,6 +13,7 @@ #define nvgCreate nvgCreateGLES3 #endif +#include #include #include "nanovg.h" @@ -22,27 +24,33 @@ #include "common/framebuffer.h" #include "common/modeldata.h" #include "messaging.hpp" - #include "cereal/gen/c/log.capnp.h" - +#include "bbuistate.h" #include "sound.hpp" + #define STATUS_STOPPED 0 #define STATUS_DISENGAGED 1 #define STATUS_ENGAGED 2 #define STATUS_WARNING 3 #define STATUS_ALERT 4 +#define NET_CONNECTED 0 +#define NET_DISCONNECTED 1 +#define NET_ERROR 2 + #define ALERTSIZE_NONE 0 #define ALERTSIZE_SMALL 1 #define ALERTSIZE_MID 2 #define ALERTSIZE_FULL 3 -#define COLOR_BLACK_ALPHA nvgRGBA(0, 0, 0, 85) +#define COLOR_BLACK nvgRGBA(0, 0, 0, 255) +#define COLOR_BLACK_ALPHA(x) nvgRGBA(0, 0, 0, x) #define COLOR_WHITE nvgRGBA(255, 255, 255, 255) -#define COLOR_WHITE_ALPHA nvgRGBA(255, 255, 255, 85) +#define COLOR_WHITE_ALPHA(x) nvgRGBA(255, 255, 255, x) #define COLOR_YELLOW nvgRGBA(218, 202, 37, 255) #define COLOR_RED nvgRGBA(201, 34, 49, 255) +#define COLOR_OCHRE nvgRGBA(218, 111, 37, 255) #ifndef QCOM #define UI_60FPS @@ -52,6 +60,7 @@ //#define SHOW_SPEEDLIMIT 1 //#define DEBUG_TURN + const int vwp_w = 1920; const int vwp_h = 1080; const int nav_w = 640; @@ -63,6 +72,7 @@ const int box_y = bdr_s; const int box_w = vwp_w-sbr_w-(bdr_s*2); const int box_h = vwp_h-(bdr_s*2); const int viz_w = vwp_w-(bdr_s*2); +const int ff_xoffset = 32; const int header_h = 420; const int footer_h = 280; const int footer_y = vwp_h-bdr_s-footer_h; @@ -122,6 +132,7 @@ typedef struct UIScene { bool uilayout_sidebarcollapsed; bool uilayout_mapenabled; + bool uilayout_mockengaged; // responsive layout int ui_viz_rx; int ui_viz_rw; @@ -133,6 +144,10 @@ typedef struct UIScene { int lead_status2; float lead_d_rel2, lead_y_rel2, lead_v_rel2; + float face_prob; + bool is_rhd; + float face_x, face_y; + int front_box_x, front_box_y, front_box_width, front_box_height; uint64_t alert_ts; @@ -155,6 +170,7 @@ typedef struct UIScene { int paTemp; int hwType; int satelliteCount; + uint8_t athenaStatus; } UIScene; typedef struct { @@ -172,9 +188,15 @@ typedef struct { } track_vertices_data; + typedef struct UIState { + + //BB define BBUIState + BBUIState b; + int plus_state; + //BB end + pthread_mutex_t lock; - pthread_cond_t bg_cond; // framebuffer FramebufferState *fb; @@ -209,6 +231,9 @@ typedef struct UIState { SubSocket *thermal_sock; SubSocket *health_sock; SubSocket *ubloxgnss_sock; + SubSocket *driverstate_sock; + SubSocket *dmonitoring_sock; + PubSocket *offroad_sock; Poller * poller; Poller * ublox_poller; @@ -255,9 +280,12 @@ typedef struct UIState { int longitudinal_control_timeout; int limit_set_speed_timeout; int hardware_timeout; + int last_athena_ping_timeout; + int offroad_layout_timeout; bool controls_seen; + uint64_t last_athena_ping; int status; bool is_metric; bool longitudinal_control; @@ -269,6 +297,9 @@ typedef struct UIState { int alert_size; float alert_blinking_alpha; bool alert_blinked; + bool started; + bool thermal_started, preview_started; + bool vision_seen; float light_sensor; @@ -291,6 +322,36 @@ void ui_draw_vision_alert(UIState *s, int va_size, int va_color, const char* va_text1, const char* va_text2); void ui_draw(UIState *s); void ui_draw_sidebar(UIState *s); +void ui_draw_image(NVGcontext *vg, float x, float y, float w, float h, int image, float alpha); void ui_nvg_init(UIState *s); +static void set_awake(UIState *s, bool awake); +#if !defined(QCOM) && !defined(QCOM2) +#include "GLFW/glfw3.h" +FramebufferState* framebuffer_init_linux( + const char* name, int32_t layer, int alpha, + int *out_w, int *out_h, GLFWmousebuttonfun mouse_event_handler); #endif +#endif +// TODO: this is also hardcoded in common/transformations/camera.py +const mat3 intrinsic_matrix = (mat3){{ + 910., 0., 582., + 0., 910., 437., + 0., 0., 1. +}}; + +const uint8_t alert_colors[][4] = { + [STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xf1}, + [STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0xc8}, + [STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0xf1}, + [STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0xf1}, + [STATUS_ALERT] = {0xC9, 0x22, 0x31, 0xf1}, +}; + +const int alert_sizes[] = { + [ALERTSIZE_NONE] = 0, + [ALERTSIZE_SMALL] = 241, + [ALERTSIZE_MID] = 390, + [ALERTSIZE_FULL] = vwp_h, +}; + diff --git a/selfdrive/updated.py b/selfdrive/updated.py index acc4603f9c7e81..5f82c89e6d61ee 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -37,6 +37,7 @@ from common.basedir import BASEDIR from common.params import Params from selfdrive.swaglog import cloudlog +from selfdrive.car.tesla.readconfig import CarSettings STAGING_ROOT = "/data/safe_staging" @@ -112,7 +113,7 @@ def set_consistent_flag(): def set_update_available_params(new_version=False): params = Params() - t = datetime.datetime.now().isoformat() + t = datetime.datetime.utcnow().isoformat() params.put("LastUpdateTime", t.encode('utf8')) if new_version: @@ -297,6 +298,8 @@ def main(): overlay_init_done = False wait_helper = WaitTimeHelper() params = Params() + carSettings = CarSettings() + doAutoUpdate = carSettings.doAutoUpdate if not os.geteuid() == 0: raise RuntimeError("updated must be launched as root!") @@ -314,45 +317,45 @@ def main(): while True: update_failed_count += 1 - time_wrong = datetime.datetime.now().year < 2019 + time_wrong = datetime.datetime.utcnow().year < 2019 ping_failed = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"]) - - # Wait until we have a valid datetime to initialize the overlay - if not (ping_failed or time_wrong): - try: - # If the git directory has modifcations after we created the overlay - # we need to recreate the overlay - if overlay_init_done: - overlay_init_fn = os.path.join(BASEDIR, ".overlay_init") - git_dir_path = os.path.join(BASEDIR, ".git") - new_files = run(["find", git_dir_path, "-newer", overlay_init_fn]) - - if len(new_files.splitlines()): - cloudlog.info(".git directory changed, recreating overlay") - overlay_init_done = False - - if not overlay_init_done: - init_ovfs() - overlay_init_done = True - - if params.get("IsOffroad") == b"1": - attempt_update() - update_failed_count = 0 - else: - cloudlog.info("not running updater, openpilot running") - - except subprocess.CalledProcessError as e: - cloudlog.event( - "update process failed", - cmd=e.cmd, - output=e.output, - returncode=e.returncode - ) - overlay_init_done = False - except Exception: - cloudlog.exception("uncaught updated exception, shouldn't happen") - - params.put("UpdateFailedCount", str(update_failed_count)) + if doAutoUpdate: + # Wait until we have a valid datetime to initialize the overlay + if not (ping_failed or time_wrong): + try: + # If the git directory has modifcations after we created the overlay + # we need to recreate the overlay + if overlay_init_done: + overlay_init_fn = os.path.join(BASEDIR, ".overlay_init") + git_dir_path = os.path.join(BASEDIR, ".git") + new_files = run(["find", git_dir_path, "-newer", overlay_init_fn]) + + if len(new_files.splitlines()): + cloudlog.info(".git directory changed, recreating overlay") + overlay_init_done = False + + if not overlay_init_done: + init_ovfs() + overlay_init_done = True + + if params.get("IsOffroad") == b"1": + attempt_update() + update_failed_count = 0 + else: + cloudlog.info("not running updater, openpilot running") + + except subprocess.CalledProcessError as e: + cloudlog.event( + "update process failed", + cmd=e.cmd, + output=e.output, + returncode=e.returncode + ) + overlay_init_done = False + except Exception: + cloudlog.exception("uncaught updated exception, shouldn't happen") + + params.put("UpdateFailedCount", str(update_failed_count)) wait_between_updates(wait_helper.ready_event) if wait_helper.shutdown: break