diff --git a/.gitignore b/.gitignore index 789f2728..c6efe5cf 100644 --- a/.gitignore +++ b/.gitignore @@ -20,7 +20,9 @@ a.out .*.un~ *.tmp *.o +*.o-* *.os +*.os-* *.so *.a *.clb @@ -54,6 +56,7 @@ xx panda_jungle .coverage* +coverage.xml htmlcov pandaextra diff --git a/README.md b/README.md index 1ed8ca60..e871613c 100644 --- a/README.md +++ b/README.md @@ -57,106 +57,125 @@ 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 | Highlander Hybrid 2020 | All | openpilot | 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 | +| Genesis | G80 20182 | All | Stock | 0mph | 0mph | +| Genesis | G90 20182 | All | Stock | 0mph | 0mph | +| 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 | Forte 20182 | SCC + LKAS | Stock | 0mph | 0mph | +| Kia | Optima 20172 | SCC + LKAS/LDWS | Stock | 0mph | 32mph | +| Kia | Optima 20192 | SCC + LKAS | Stock | 0mph | 0mph | +| Kia | Sorento 20182 | SCC + LKAS | 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 | -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).***
+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)
+ +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 +193,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 +213,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 +231,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 +259,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 +286,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 +308,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`. @@ -308,3 +327,8 @@ NO WARRANTY EXPRESSED OR IMPLIED.** --- + +[![Total alerts](https://img.shields.io/lgtm/alerts/g/commaai/openpilot.svg?logo=lgtm&logoWidth=18)](https://lgtm.com/projects/g/commaai/openpilot/alerts/) +[![Language grade: Python](https://img.shields.io/lgtm/grade/python/g/commaai/openpilot.svg?logo=lgtm&logoWidth=18)](https://lgtm.com/projects/g/commaai/openpilot/context:python) +[![Language grade: C/C++](https://img.shields.io/lgtm/grade/cpp/g/commaai/openpilot.svg?logo=lgtm&logoWidth=18)](https://lgtm.com/projects/g/commaai/openpilot/context:cpp) +[![codecov](https://codecov.io/gh/commaai/openpilot/branch/master/graph/badge.svg)](https://codecov.io/gh/commaai/openpilot) diff --git a/RELEASES.md b/RELEASES.md index 542c7a27..67beec77 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,13 @@ +Version 0.7.5 (2020-05-13) +======================== +* 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 +* Added support for many Hyundai, Kia, Genesis models thanks to xx979xx! +* 2019 Nissan X-Trail and 2018 Nissan Leaf support thanks to avolmensky! +* Improved lateral tuning for 2020 Toyota Rav 4 (hybrid) + Version 0.7.4 (2020-03-20) ======================== * New driving model: improved lane changes and lead car detection diff --git a/SConstruct b/SConstruct index 5944541b..aac002d1 100644 --- a/SConstruct +++ b/SConstruct @@ -14,39 +14,55 @@ AddOption('--asan', arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() 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", ] + 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", ] - cflags = ["-DQCOM", "-mcpu=cortex-a57"] - cxxflags = ["-DQCOM", "-mcpu=cortex-a57"] + if arch == "larch64": + cpppath += ["#phonelibs/capnp-cpp/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"] - rpath = ["/system/vendor/lib64"] else: lenv = { "PATH": "#external/bin:" + os.environ['PATH'], } cpppath = [ "#phonelibs/capnp-cpp/include", - "#phonelibs/capnp-c/include", "#phonelibs/zmq/x64/include", "#external/tensorflow/include", ] @@ -54,7 +70,6 @@ else: if arch == "Darwin": libpath = [ "#phonelibs/capnp-cpp/mac/lib", - "#phonelibs/capnp-c/mac/lib", "#phonelibs/libyuv/mac/lib", "#cereal", "#selfdrive/common", @@ -64,7 +79,6 @@ else: else: libpath = [ "#phonelibs/capnp-cpp/x64/lib", - "#phonelibs/capnp-c/x64/lib", "#phonelibs/snpe/x86_64-linux-clang", "#phonelibs/zmq/x64/lib", "#phonelibs/libyuv/x64/lib", @@ -113,11 +127,10 @@ env = Environment( "#phonelibs/bzip2", "#phonelibs/libyuv/include", "#phonelibs/openmax/include", - "#phonelibs/json/src", "#phonelibs/json11", "#phonelibs/eigen", "#phonelibs/curl/include", - "#phonelibs/opencv/include", + #"#phonelibs/opencv/include", # use opencv4 instead "#phonelibs/libgralloc/include", "#phonelibs/android_frameworks_native/include", "#phonelibs/android_hardware_libhardware/include", @@ -175,10 +188,12 @@ 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) +Export('env', 'arch', 'zmq', 'SHARED', 'webcam') # cereal and messaging are shared with the system SConscript(['cereal/SConscript']) @@ -196,7 +211,7 @@ Import('_common', '_visionipc', '_gpucommon', '_gpu_libs') if SHARED: common, visionipc, gpucommon = abspath(common), abspath(visionipc), abspath(gpucommon) else: - common = [_common, 'json'] + common = [_common, 'json11'] visionipc = _visionipc gpucommon = [_gpucommon] + _gpu_libs @@ -215,6 +230,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']) @@ -229,5 +245,4 @@ if arch == "aarch64": SConscript(['selfdrive/locationd/SConscript']) SConscript(['selfdrive/locationd/kalman/SConscript']) - -# TODO: finish cereal, dbcbuilder, MPC +SConscript(['tools/lib/index_log/SConscript']) diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index 573a6e69..c6a6af11 100644 Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ diff --git a/codecov.yml b/codecov.yml new file mode 100644 index 00000000..c63d5fec --- /dev/null +++ b/codecov.yml @@ -0,0 +1,7 @@ +coverage: + status: + project: + default: + informational: true + patch: off + diff --git a/common/android.py b/common/android.py index 5e26924f..4342a425 100644 --- a/common/android.py +++ b/common/android.py @@ -135,8 +135,6 @@ def get_network_strength(network_type): # from SignalStrength.java def get_lte_level(rsrp, rssnr): INT_MAX = 2147483647 - lvl_rsrp = NetworkStrength.unknown - lvl_rssnr = NetworkStrength.unknown if rsrp == INT_MAX: lvl_rsrp = NetworkStrength.unknown elif rsrp >= -95: @@ -174,7 +172,6 @@ def get_network_strength(network_type): return lvl def get_gsm_level(asu): - lvl = NetworkStrength.unknown if asu <= 2 or asu == 99: lvl = NetworkStrength.unknown elif asu >= 12: @@ -232,7 +229,7 @@ def get_network_strength(network_type): if network_type == NetworkType.none: return network_strength if network_type == NetworkType.wifi: - out = subprocess.check_output('dumpsys connectivity', shell=True).decode('ascii') + out = subprocess.check_output('dumpsys connectivity', shell=True).decode('utf-8') network_strength = NetworkStrength.unknown for line in out.split('\n'): signal_str = "SignalStrength: " @@ -251,7 +248,7 @@ def get_network_strength(network_type): return network_strength else: # check cell strength - out = subprocess.check_output('dumpsys telephony.registry', shell=True).decode('ascii') + out = subprocess.check_output('dumpsys telephony.registry', shell=True).decode('utf-8') for line in out.split('\n'): if "mSignalStrength" in line: arr = line.split(' ') diff --git a/common/filter_simple.py b/common/filter_simple.py index a3206db1..fa291bcb 100644 --- a/common/filter_simple.py +++ b/common/filter_simple.py @@ -6,5 +6,4 @@ class FirstOrderFilter(): 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 22ee0aa5..787235bf 100755 --- a/common/params.py +++ b/common/params.py @@ -52,7 +52,7 @@ class UnknownKeyName(Exception): keys = { - "AccessToken": [TxType.PERSISTENT], + "AccessToken": [TxType.CLEAR_ON_MANAGER_START], "AthenadPid": [TxType.PERSISTENT], "CalibrationParams": [TxType.PERSISTENT], "CarParams": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], @@ -70,6 +70,7 @@ keys = { "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 +79,7 @@ keys = { "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], diff --git a/common/profiler.py b/common/profiler.py index eab091c7..f8262dd8 100644 --- a/common/profiler.py +++ b/common/profiler.py @@ -39,8 +39,8 @@ class Profiler(): print("******* Profiling *******") for n, ms in sorted(self.cp.items(), key=lambda x: -x[1]): if n in self.cp_ignored: - print("%30s: %7.2f percent: %3.0f IGNORED" % (n, ms*1000.0, ms/self.tot*100)) + print("%30s: %9.2f percent: %3.0f IGNORED" % (n, ms*1000.0, ms/self.tot*100)) else: - print("%30s: %7.2f percent: %3.0f" % (n, ms*1000.0, ms/self.tot*100)) + print("%30s: %9.2f percent: %3.0f" % (n, ms*1000.0, ms/self.tot*100)) print("Iter clock: %2.6f TOTAL: %2.2f" % (self.tot/self.iter, self.tot)) diff --git a/common/spinner.py b/common/spinner.py index 3582d3fe..da808403 100644 --- a/common/spinner.py +++ b/common/spinner.py @@ -50,6 +50,9 @@ class FakeSpinner(): 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 f392956e..e528c3de 100644 --- a/common/stat_live.py +++ b/common/stat_live.py @@ -53,7 +53,7 @@ class RunningStat(): 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 00000000..0d94bc74 --- /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 d8729e2f..5ca9eb57 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 c13cc7e3..c47c9e98 100644 --- a/common/transformations/model.py +++ b/common/transformations/model.py @@ -44,24 +44,15 @@ medmodel_intrinsics = np.array( # 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 @@ medmodel_frame_from_road_frame = np.dot(medmodel_intrinsics, 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 00000000..fa61b9e0 --- /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/launch_chffrplus.sh b/launch_chffrplus.sh index a33deaaf..50effbf7 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -62,6 +62,11 @@ function launch { echo 0-3 > /dev/cpuset/foreground/cpus echo 0-3 > /dev/cpuset/android/cpus + # change interrupt affinity + echo 3 > /proc/irq/6/smp_affinity_list # MDSS + echo 1 > /proc/irq/78/smp_affinity_list # Modem, can potentially lock up + echo 2 > /proc/irq/733/smp_affinity_list # USB + DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" # Remove old NEOS update file diff --git a/lgtm.yml b/lgtm.yml new file mode 100644 index 00000000..341a7880 --- /dev/null +++ b/lgtm.yml @@ -0,0 +1,14 @@ +path_classifiers: + library: + - external + - phonelibs + - pyextra + - tools/lib/mkvparse +extraction: + cpp: + after_prepare: + - "pip3 install jinja2 pyyaml cython pycapnp numpy sympy tqdm cffi logentries zmq" + - "export PATH=$PWD/external/bin:$PATH" + index: + build_command: "python3 $(which scons)" + diff --git a/models/dmonitoring_model_q.dlc b/models/dmonitoring_model_q.dlc index 9dbe0927..44ddc8f0 100644 Binary files a/models/dmonitoring_model_q.dlc and b/models/dmonitoring_model_q.dlc differ diff --git a/models/supercombo.dlc b/models/supercombo.dlc index ea6d0a06..32e66ed2 100644 Binary files a/models/supercombo.dlc and b/models/supercombo.dlc differ diff --git a/phonelibs/SConscript b/phonelibs/SConscript index c50d9a50..8d10c20d 100644 --- a/phonelibs/SConscript +++ b/phonelibs/SConscript @@ -4,9 +4,6 @@ def static_library(lib_dir, header_dir): env.Append(LIBPATH=[Dir(lib_dir)]) env.Append(CPPPATH=[Dir(header_dir)]) -env.Library('json', ['json/src/json.c']) -env.Append(CPPPATH=[Dir('json/src')]) - env.Library('json11', ['json11/json11.cpp']) env.Append(CPPPATH=[Dir('json11')]) diff --git a/phonelibs/json/src/json.c b/phonelibs/json/src/json.c deleted file mode 100644 index 2f0452ae..00000000 --- a/phonelibs/json/src/json.c +++ /dev/null @@ -1,1381 +0,0 @@ -/* - Copyright (C) 2011 Joseph A. Adams (joeyadams3.14159@gmail.com) - All rights reserved. - - 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. -*/ - -#include "json.h" - -#include -#include -#include -#include -#include - -#define out_of_memory() do { \ - fprintf(stderr, "Out of memory.\n"); \ - exit(EXIT_FAILURE); \ - } while (0) - -/* Sadly, strdup is not portable. */ -static char *json_strdup(const char *str) -{ - char *ret = (char*) malloc(strlen(str) + 1); - if (ret == NULL) - out_of_memory(); - strcpy(ret, str); - return ret; -} - -/* String buffer */ - -typedef struct -{ - char *cur; - char *end; - char *start; -} SB; - -static void sb_init(SB *sb) -{ - sb->start = (char*) malloc(17); - if (sb->start == NULL) - out_of_memory(); - sb->cur = sb->start; - sb->end = sb->start + 16; -} - -/* sb and need may be evaluated multiple times. */ -#define sb_need(sb, need) do { \ - if ((sb)->end - (sb)->cur < (need)) \ - sb_grow(sb, need); \ - } while (0) - -static void sb_grow(SB *sb, int need) -{ - size_t length = sb->cur - sb->start; - size_t alloc = sb->end - sb->start; - - do { - alloc *= 2; - } while (alloc < length + need); - - sb->start = (char*) realloc(sb->start, alloc + 1); - if (sb->start == NULL) - out_of_memory(); - sb->cur = sb->start + length; - sb->end = sb->start + alloc; -} - -static void sb_put(SB *sb, const char *bytes, int count) -{ - sb_need(sb, count); - memcpy(sb->cur, bytes, count); - sb->cur += count; -} - -#define sb_putc(sb, c) do { \ - if ((sb)->cur >= (sb)->end) \ - sb_grow(sb, 1); \ - *(sb)->cur++ = (c); \ - } while (0) - -static void sb_puts(SB *sb, const char *str) -{ - sb_put(sb, str, strlen(str)); -} - -static char *sb_finish(SB *sb) -{ - *sb->cur = 0; - assert(sb->start <= sb->cur && strlen(sb->start) == (size_t)(sb->cur - sb->start)); - return sb->start; -} - -static void sb_free(SB *sb) -{ - free(sb->start); -} - -/* - * Unicode helper functions - * - * These are taken from the ccan/charset module and customized a bit. - * Putting them here means the compiler can (choose to) inline them, - * and it keeps ccan/json from having a dependency. - */ - -/* - * Type for Unicode codepoints. - * We need our own because wchar_t might be 16 bits. - */ -typedef uint32_t uchar_t; - -/* - * Validate a single UTF-8 character starting at @s. - * The string must be null-terminated. - * - * If it's valid, return its length (1 thru 4). - * If it's invalid or clipped, return 0. - * - * This function implements the syntax given in RFC3629, which is - * the same as that given in The Unicode Standard, Version 6.0. - * - * It has the following properties: - * - * * All codepoints U+0000..U+10FFFF may be encoded, - * except for U+D800..U+DFFF, which are reserved - * for UTF-16 surrogate pair encoding. - * * UTF-8 byte sequences longer than 4 bytes are not permitted, - * as they exceed the range of Unicode. - * * The sixty-six Unicode "non-characters" are permitted - * (namely, U+FDD0..U+FDEF, U+xxFFFE, and U+xxFFFF). - */ -static int utf8_validate_cz(const char *s) -{ - unsigned char c = *s++; - - if (c <= 0x7F) { /* 00..7F */ - return 1; - } else if (c <= 0xC1) { /* 80..C1 */ - /* Disallow overlong 2-byte sequence. */ - return 0; - } else if (c <= 0xDF) { /* C2..DF */ - /* Make sure subsequent byte is in the range 0x80..0xBF. */ - if (((unsigned char)*s++ & 0xC0) != 0x80) - return 0; - - return 2; - } else if (c <= 0xEF) { /* E0..EF */ - /* Disallow overlong 3-byte sequence. */ - if (c == 0xE0 && (unsigned char)*s < 0xA0) - return 0; - - /* Disallow U+D800..U+DFFF. */ - if (c == 0xED && (unsigned char)*s > 0x9F) - return 0; - - /* Make sure subsequent bytes are in the range 0x80..0xBF. */ - if (((unsigned char)*s++ & 0xC0) != 0x80) - return 0; - if (((unsigned char)*s++ & 0xC0) != 0x80) - return 0; - - return 3; - } else if (c <= 0xF4) { /* F0..F4 */ - /* Disallow overlong 4-byte sequence. */ - if (c == 0xF0 && (unsigned char)*s < 0x90) - return 0; - - /* Disallow codepoints beyond U+10FFFF. */ - if (c == 0xF4 && (unsigned char)*s > 0x8F) - return 0; - - /* Make sure subsequent bytes are in the range 0x80..0xBF. */ - if (((unsigned char)*s++ & 0xC0) != 0x80) - return 0; - if (((unsigned char)*s++ & 0xC0) != 0x80) - return 0; - if (((unsigned char)*s++ & 0xC0) != 0x80) - return 0; - - return 4; - } else { /* F5..FF */ - return 0; - } -} - -/* Validate a null-terminated UTF-8 string. */ -static bool utf8_validate(const char *s) -{ - int len; - - for (; *s != 0; s += len) { - len = utf8_validate_cz(s); - if (len == 0) - return false; - } - - return true; -} - -/* - * Read a single UTF-8 character starting at @s, - * returning the length, in bytes, of the character read. - * - * This function assumes input is valid UTF-8, - * and that there are enough characters in front of @s. - */ -static int utf8_read_char(const char *s, uchar_t *out) -{ - const unsigned char *c = (const unsigned char*) s; - - assert(utf8_validate_cz(s)); - - if (c[0] <= 0x7F) { - /* 00..7F */ - *out = c[0]; - return 1; - } else if (c[0] <= 0xDF) { - /* C2..DF (unless input is invalid) */ - *out = ((uchar_t)c[0] & 0x1F) << 6 | - ((uchar_t)c[1] & 0x3F); - return 2; - } else if (c[0] <= 0xEF) { - /* E0..EF */ - *out = ((uchar_t)c[0] & 0xF) << 12 | - ((uchar_t)c[1] & 0x3F) << 6 | - ((uchar_t)c[2] & 0x3F); - return 3; - } else { - /* F0..F4 (unless input is invalid) */ - *out = ((uchar_t)c[0] & 0x7) << 18 | - ((uchar_t)c[1] & 0x3F) << 12 | - ((uchar_t)c[2] & 0x3F) << 6 | - ((uchar_t)c[3] & 0x3F); - return 4; - } -} - -/* - * Write a single UTF-8 character to @s, - * returning the length, in bytes, of the character written. - * - * @unicode must be U+0000..U+10FFFF, but not U+D800..U+DFFF. - * - * This function will write up to 4 bytes to @out. - */ -static int utf8_write_char(uchar_t unicode, char *out) -{ - unsigned char *o = (unsigned char*) out; - - assert(unicode <= 0x10FFFF && !(unicode >= 0xD800 && unicode <= 0xDFFF)); - - if (unicode <= 0x7F) { - /* U+0000..U+007F */ - *o++ = unicode; - return 1; - } else if (unicode <= 0x7FF) { - /* U+0080..U+07FF */ - *o++ = 0xC0 | unicode >> 6; - *o++ = 0x80 | (unicode & 0x3F); - return 2; - } else if (unicode <= 0xFFFF) { - /* U+0800..U+FFFF */ - *o++ = 0xE0 | unicode >> 12; - *o++ = 0x80 | (unicode >> 6 & 0x3F); - *o++ = 0x80 | (unicode & 0x3F); - return 3; - } else { - /* U+10000..U+10FFFF */ - *o++ = 0xF0 | unicode >> 18; - *o++ = 0x80 | (unicode >> 12 & 0x3F); - *o++ = 0x80 | (unicode >> 6 & 0x3F); - *o++ = 0x80 | (unicode & 0x3F); - return 4; - } -} - -/* - * Compute the Unicode codepoint of a UTF-16 surrogate pair. - * - * @uc should be 0xD800..0xDBFF, and @lc should be 0xDC00..0xDFFF. - * If they aren't, this function returns false. - */ -static bool from_surrogate_pair(uint16_t uc, uint16_t lc, uchar_t *unicode) -{ - if (uc >= 0xD800 && uc <= 0xDBFF && lc >= 0xDC00 && lc <= 0xDFFF) { - *unicode = 0x10000 + ((((uchar_t)uc & 0x3FF) << 10) | (lc & 0x3FF)); - return true; - } else { - return false; - } -} - -/* - * Construct a UTF-16 surrogate pair given a Unicode codepoint. - * - * @unicode must be U+10000..U+10FFFF. - */ -static void to_surrogate_pair(uchar_t unicode, uint16_t *uc, uint16_t *lc) -{ - uchar_t n; - - assert(unicode >= 0x10000 && unicode <= 0x10FFFF); - - n = unicode - 0x10000; - *uc = ((n >> 10) & 0x3FF) | 0xD800; - *lc = (n & 0x3FF) | 0xDC00; -} - -#define is_space(c) ((c) == '\t' || (c) == '\n' || (c) == '\r' || (c) == ' ') -#define is_digit(c) ((c) >= '0' && (c) <= '9') - -static bool parse_value (const char **sp, JsonNode **out); -static bool parse_string (const char **sp, char **out); -static bool parse_number (const char **sp, double *out); -static bool parse_array (const char **sp, JsonNode **out); -static bool parse_object (const char **sp, JsonNode **out); -static bool parse_hex16 (const char **sp, uint16_t *out); - -static bool expect_literal (const char **sp, const char *str); -static void skip_space (const char **sp); - -static void emit_value (SB *out, const JsonNode *node); -static void emit_value_indented (SB *out, const JsonNode *node, const char *space, int indent_level); -static void emit_string (SB *out, const char *str); -static void emit_number (SB *out, double num); -static void emit_array (SB *out, const JsonNode *array); -static void emit_array_indented (SB *out, const JsonNode *array, const char *space, int indent_level); -static void emit_object (SB *out, const JsonNode *object); -static void emit_object_indented (SB *out, const JsonNode *object, const char *space, int indent_level); - -static int write_hex16(char *out, uint16_t val); - -static JsonNode *mknode(JsonTag tag); -static void append_node(JsonNode *parent, JsonNode *child); -static void prepend_node(JsonNode *parent, JsonNode *child); -static void append_member(JsonNode *object, char *key, JsonNode *value); - -/* Assertion-friendly validity checks */ -static bool tag_is_valid(unsigned int tag); -static bool number_is_valid(const char *num); - -JsonNode *json_decode(const char *json) -{ - const char *s = json; - JsonNode *ret; - - skip_space(&s); - if (!parse_value(&s, &ret)) - return NULL; - - skip_space(&s); - if (*s != 0) { - json_delete(ret); - return NULL; - } - - return ret; -} - -char *json_encode(const JsonNode *node) -{ - return json_stringify(node, NULL); -} - -char *json_encode_string(const char *str) -{ - SB sb; - sb_init(&sb); - - emit_string(&sb, str); - - return sb_finish(&sb); -} - -char *json_stringify(const JsonNode *node, const char *space) -{ - SB sb; - sb_init(&sb); - - if (space != NULL) - emit_value_indented(&sb, node, space, 0); - else - emit_value(&sb, node); - - return sb_finish(&sb); -} - -void json_delete(JsonNode *node) -{ - if (node != NULL) { - json_remove_from_parent(node); - - switch (node->tag) { - case JSON_STRING: - free(node->string_); - break; - case JSON_ARRAY: - case JSON_OBJECT: - { - JsonNode *child, *next; - for (child = node->children.head; child != NULL; child = next) { - next = child->next; - json_delete(child); - } - break; - } - default:; - } - - free(node); - } -} - -bool json_validate(const char *json) -{ - const char *s = json; - - skip_space(&s); - if (!parse_value(&s, NULL)) - return false; - - skip_space(&s); - if (*s != 0) - return false; - - return true; -} - -JsonNode *json_find_element(JsonNode *array, int index) -{ - JsonNode *element; - int i = 0; - - if (array == NULL || array->tag != JSON_ARRAY) - return NULL; - - json_foreach(element, array) { - if (i == index) - return element; - i++; - } - - return NULL; -} - -JsonNode *json_find_member(JsonNode *object, const char *name) -{ - JsonNode *member; - - if (object == NULL || object->tag != JSON_OBJECT) - return NULL; - - json_foreach(member, object) - if (strcmp(member->key, name) == 0) - return member; - - return NULL; -} - -JsonNode *json_first_child(const JsonNode *node) -{ - if (node != NULL && (node->tag == JSON_ARRAY || node->tag == JSON_OBJECT)) - return node->children.head; - return NULL; -} - -static JsonNode *mknode(JsonTag tag) -{ - JsonNode *ret = (JsonNode*) calloc(1, sizeof(JsonNode)); - if (ret == NULL) - out_of_memory(); - ret->tag = tag; - return ret; -} - -JsonNode *json_mknull(void) -{ - return mknode(JSON_NULL); -} - -JsonNode *json_mkbool(bool b) -{ - JsonNode *ret = mknode(JSON_BOOL); - ret->bool_ = b; - return ret; -} - -static JsonNode *mkstring(char *s) -{ - JsonNode *ret = mknode(JSON_STRING); - ret->string_ = s; - return ret; -} - -JsonNode *json_mkstring(const char *s) -{ - return mkstring(json_strdup(s)); -} - -JsonNode *json_mknumber(double n) -{ - JsonNode *node = mknode(JSON_NUMBER); - node->number_ = n; - return node; -} - -JsonNode *json_mkarray(void) -{ - return mknode(JSON_ARRAY); -} - -JsonNode *json_mkobject(void) -{ - return mknode(JSON_OBJECT); -} - -static void append_node(JsonNode *parent, JsonNode *child) -{ - child->parent = parent; - child->prev = parent->children.tail; - child->next = NULL; - - if (parent->children.tail != NULL) - parent->children.tail->next = child; - else - parent->children.head = child; - parent->children.tail = child; -} - -static void prepend_node(JsonNode *parent, JsonNode *child) -{ - child->parent = parent; - child->prev = NULL; - child->next = parent->children.head; - - if (parent->children.head != NULL) - parent->children.head->prev = child; - else - parent->children.tail = child; - parent->children.head = child; -} - -static void append_member(JsonNode *object, char *key, JsonNode *value) -{ - value->key = key; - append_node(object, value); -} - -void json_append_element(JsonNode *array, JsonNode *element) -{ - assert(array->tag == JSON_ARRAY); - assert(element->parent == NULL); - - append_node(array, element); -} - -void json_prepend_element(JsonNode *array, JsonNode *element) -{ - assert(array->tag == JSON_ARRAY); - assert(element->parent == NULL); - - prepend_node(array, element); -} - -void json_append_member(JsonNode *object, const char *key, JsonNode *value) -{ - assert(object->tag == JSON_OBJECT); - assert(value->parent == NULL); - - append_member(object, json_strdup(key), value); -} - -void json_prepend_member(JsonNode *object, const char *key, JsonNode *value) -{ - assert(object->tag == JSON_OBJECT); - assert(value->parent == NULL); - - value->key = json_strdup(key); - prepend_node(object, value); -} - -void json_remove_from_parent(JsonNode *node) -{ - JsonNode *parent = node->parent; - - if (parent != NULL) { - if (node->prev != NULL) - node->prev->next = node->next; - else - parent->children.head = node->next; - if (node->next != NULL) - node->next->prev = node->prev; - else - parent->children.tail = node->prev; - - free(node->key); - - node->parent = NULL; - node->prev = node->next = NULL; - node->key = NULL; - } -} - -static bool parse_value(const char **sp, JsonNode **out) -{ - const char *s = *sp; - - switch (*s) { - case 'n': - if (expect_literal(&s, "null")) { - if (out) - *out = json_mknull(); - *sp = s; - return true; - } - return false; - - case 'f': - if (expect_literal(&s, "false")) { - if (out) - *out = json_mkbool(false); - *sp = s; - return true; - } - return false; - - case 't': - if (expect_literal(&s, "true")) { - if (out) - *out = json_mkbool(true); - *sp = s; - return true; - } - return false; - - case '"': { - char *str; - if (parse_string(&s, out ? &str : NULL)) { - if (out) - *out = mkstring(str); - *sp = s; - return true; - } - return false; - } - - case '[': - if (parse_array(&s, out)) { - *sp = s; - return true; - } - return false; - - case '{': - if (parse_object(&s, out)) { - *sp = s; - return true; - } - return false; - - default: { - double num; - if (parse_number(&s, out ? &num : NULL)) { - if (out) - *out = json_mknumber(num); - *sp = s; - return true; - } - return false; - } - } -} - -static bool parse_array(const char **sp, JsonNode **out) -{ - const char *s = *sp; - JsonNode *ret = out ? json_mkarray() : NULL; - JsonNode *element; - - if (*s++ != '[') - goto failure; - skip_space(&s); - - if (*s == ']') { - s++; - goto success; - } - - for (;;) { - if (!parse_value(&s, out ? &element : NULL)) - goto failure; - skip_space(&s); - - if (out) - json_append_element(ret, element); - - if (*s == ']') { - s++; - goto success; - } - - if (*s++ != ',') - goto failure; - skip_space(&s); - } - -success: - *sp = s; - if (out) - *out = ret; - return true; - -failure: - json_delete(ret); - return false; -} - -static bool parse_object(const char **sp, JsonNode **out) -{ - const char *s = *sp; - JsonNode *ret = out ? json_mkobject() : NULL; - char *key; - JsonNode *value; - - if (*s++ != '{') - goto failure; - skip_space(&s); - - if (*s == '}') { - s++; - goto success; - } - - for (;;) { - if (!parse_string(&s, out ? &key : NULL)) - goto failure; - skip_space(&s); - - if (*s++ != ':') - goto failure_free_key; - skip_space(&s); - - if (!parse_value(&s, out ? &value : NULL)) - goto failure_free_key; - skip_space(&s); - - if (out) - append_member(ret, key, value); - - if (*s == '}') { - s++; - goto success; - } - - if (*s++ != ',') - goto failure; - skip_space(&s); - } - -success: - *sp = s; - if (out) - *out = ret; - return true; - -failure_free_key: - if (out) - free(key); -failure: - json_delete(ret); - return false; -} - -bool parse_string(const char **sp, char **out) -{ - const char *s = *sp; - SB sb; - char throwaway_buffer[4]; - /* enough space for a UTF-8 character */ - char *b; - - if (*s++ != '"') - return false; - - if (out) { - sb_init(&sb); - sb_need(&sb, 4); - b = sb.cur; - } else { - b = throwaway_buffer; - } - - while (*s != '"') { - unsigned char c = *s++; - - /* Parse next character, and write it to b. */ - if (c == '\\') { - c = *s++; - switch (c) { - case '"': - case '\\': - case '/': - *b++ = c; - break; - case 'b': - *b++ = '\b'; - break; - case 'f': - *b++ = '\f'; - break; - case 'n': - *b++ = '\n'; - break; - case 'r': - *b++ = '\r'; - break; - case 't': - *b++ = '\t'; - break; - case 'u': - { - uint16_t uc, lc; - uchar_t unicode; - - if (!parse_hex16(&s, &uc)) - goto failed; - - if (uc >= 0xD800 && uc <= 0xDFFF) { - /* Handle UTF-16 surrogate pair. */ - if (*s++ != '\\' || *s++ != 'u' || !parse_hex16(&s, &lc)) - goto failed; /* Incomplete surrogate pair. */ - if (!from_surrogate_pair(uc, lc, &unicode)) - goto failed; /* Invalid surrogate pair. */ - } else if (uc == 0) { - /* Disallow "\u0000". */ - goto failed; - } else { - unicode = uc; - } - - b += utf8_write_char(unicode, b); - break; - } - default: - /* Invalid escape */ - goto failed; - } - } else if (c <= 0x1F) { - /* Control characters are not allowed in string literals. */ - goto failed; - } else { - /* Validate and echo a UTF-8 character. */ - int len; - - s--; - len = utf8_validate_cz(s); - if (len == 0) - goto failed; /* Invalid UTF-8 character. */ - - while (len--) - *b++ = *s++; - } - - /* - * Update sb to know about the new bytes, - * and set up b to write another character. - */ - if (out) { - sb.cur = b; - sb_need(&sb, 4); - b = sb.cur; - } else { - b = throwaway_buffer; - } - } - s++; - - if (out) - *out = sb_finish(&sb); - *sp = s; - return true; - -failed: - if (out) - sb_free(&sb); - return false; -} - -/* - * The JSON spec says that a number shall follow this precise pattern - * (spaces and quotes added for readability): - * '-'? (0 | [1-9][0-9]*) ('.' [0-9]+)? ([Ee] [+-]? [0-9]+)? - * - * However, some JSON parsers are more liberal. For instance, PHP accepts - * '.5' and '1.'. JSON.parse accepts '+3'. - * - * This function takes the strict approach. - */ -bool parse_number(const char **sp, double *out) -{ - const char *s = *sp; - - /* '-'? */ - if (*s == '-') - s++; - - /* (0 | [1-9][0-9]*) */ - if (*s == '0') { - s++; - } else { - if (!is_digit(*s)) - return false; - do { - s++; - } while (is_digit(*s)); - } - - /* ('.' [0-9]+)? */ - if (*s == '.') { - s++; - if (!is_digit(*s)) - return false; - do { - s++; - } while (is_digit(*s)); - } - - /* ([Ee] [+-]? [0-9]+)? */ - if (*s == 'E' || *s == 'e') { - s++; - if (*s == '+' || *s == '-') - s++; - if (!is_digit(*s)) - return false; - do { - s++; - } while (is_digit(*s)); - } - - if (out) - *out = strtod(*sp, NULL); - - *sp = s; - return true; -} - -static void skip_space(const char **sp) -{ - const char *s = *sp; - while (is_space(*s)) - s++; - *sp = s; -} - -static void emit_value(SB *out, const JsonNode *node) -{ - assert(tag_is_valid(node->tag)); - switch (node->tag) { - case JSON_NULL: - sb_puts(out, "null"); - break; - case JSON_BOOL: - sb_puts(out, node->bool_ ? "true" : "false"); - break; - case JSON_STRING: - emit_string(out, node->string_); - break; - case JSON_NUMBER: - emit_number(out, node->number_); - break; - case JSON_ARRAY: - emit_array(out, node); - break; - case JSON_OBJECT: - emit_object(out, node); - break; - default: - assert(false); - } -} - -void emit_value_indented(SB *out, const JsonNode *node, const char *space, int indent_level) -{ - assert(tag_is_valid(node->tag)); - switch (node->tag) { - case JSON_NULL: - sb_puts(out, "null"); - break; - case JSON_BOOL: - sb_puts(out, node->bool_ ? "true" : "false"); - break; - case JSON_STRING: - emit_string(out, node->string_); - break; - case JSON_NUMBER: - emit_number(out, node->number_); - break; - case JSON_ARRAY: - emit_array_indented(out, node, space, indent_level); - break; - case JSON_OBJECT: - emit_object_indented(out, node, space, indent_level); - break; - default: - assert(false); - } -} - -static void emit_array(SB *out, const JsonNode *array) -{ - const JsonNode *element; - - sb_putc(out, '['); - json_foreach(element, array) { - emit_value(out, element); - if (element->next != NULL) - sb_putc(out, ','); - } - sb_putc(out, ']'); -} - -static void emit_array_indented(SB *out, const JsonNode *array, const char *space, int indent_level) -{ - const JsonNode *element = array->children.head; - int i; - - if (element == NULL) { - sb_puts(out, "[]"); - return; - } - - sb_puts(out, "[\n"); - while (element != NULL) { - for (i = 0; i < indent_level + 1; i++) - sb_puts(out, space); - emit_value_indented(out, element, space, indent_level + 1); - - element = element->next; - sb_puts(out, element != NULL ? ",\n" : "\n"); - } - for (i = 0; i < indent_level; i++) - sb_puts(out, space); - sb_putc(out, ']'); -} - -static void emit_object(SB *out, const JsonNode *object) -{ - const JsonNode *member; - - sb_putc(out, '{'); - json_foreach(member, object) { - emit_string(out, member->key); - sb_putc(out, ':'); - emit_value(out, member); - if (member->next != NULL) - sb_putc(out, ','); - } - sb_putc(out, '}'); -} - -static void emit_object_indented(SB *out, const JsonNode *object, const char *space, int indent_level) -{ - const JsonNode *member = object->children.head; - int i; - - if (member == NULL) { - sb_puts(out, "{}"); - return; - } - - sb_puts(out, "{\n"); - while (member != NULL) { - for (i = 0; i < indent_level + 1; i++) - sb_puts(out, space); - emit_string(out, member->key); - sb_puts(out, ": "); - emit_value_indented(out, member, space, indent_level + 1); - - member = member->next; - sb_puts(out, member != NULL ? ",\n" : "\n"); - } - for (i = 0; i < indent_level; i++) - sb_puts(out, space); - sb_putc(out, '}'); -} - -void emit_string(SB *out, const char *str) -{ - bool escape_unicode = false; - const char *s = str; - char *b; - - assert(utf8_validate(str)); - - /* - * 14 bytes is enough space to write up to two - * \uXXXX escapes and two quotation marks. - */ - sb_need(out, 14); - b = out->cur; - - *b++ = '"'; - while (*s != 0) { - unsigned char c = *s++; - - /* Encode the next character, and write it to b. */ - switch (c) { - case '"': - *b++ = '\\'; - *b++ = '"'; - break; - case '\\': - *b++ = '\\'; - *b++ = '\\'; - break; - case '\b': - *b++ = '\\'; - *b++ = 'b'; - break; - case '\f': - *b++ = '\\'; - *b++ = 'f'; - break; - case '\n': - *b++ = '\\'; - *b++ = 'n'; - break; - case '\r': - *b++ = '\\'; - *b++ = 'r'; - break; - case '\t': - *b++ = '\\'; - *b++ = 't'; - break; - default: { - int len; - - s--; - len = utf8_validate_cz(s); - - if (len == 0) { - /* - * Handle invalid UTF-8 character gracefully in production - * by writing a replacement character (U+FFFD) - * and skipping a single byte. - * - * This should never happen when assertions are enabled - * due to the assertion at the beginning of this function. - */ - assert(false); - if (escape_unicode) { - strcpy(b, "\\uFFFD"); - b += 6; - } else { - *b++ = 0xEF; - *b++ = 0xBF; - *b++ = 0xBD; - } - s++; - } else if (c < 0x1F || (c >= 0x80 && escape_unicode)) { - /* Encode using \u.... */ - uint32_t unicode; - - s += utf8_read_char(s, &unicode); - - if (unicode <= 0xFFFF) { - *b++ = '\\'; - *b++ = 'u'; - b += write_hex16(b, unicode); - } else { - /* Produce a surrogate pair. */ - uint16_t uc, lc; - assert(unicode <= 0x10FFFF); - to_surrogate_pair(unicode, &uc, &lc); - *b++ = '\\'; - *b++ = 'u'; - b += write_hex16(b, uc); - *b++ = '\\'; - *b++ = 'u'; - b += write_hex16(b, lc); - } - } else { - /* Write the character directly. */ - while (len--) - *b++ = *s++; - } - - break; - } - } - - /* - * Update *out to know about the new bytes, - * and set up b to write another encoded character. - */ - out->cur = b; - sb_need(out, 14); - b = out->cur; - } - *b++ = '"'; - - out->cur = b; -} - -static void emit_number(SB *out, double num) -{ - /* - * This isn't exactly how JavaScript renders numbers, - * but it should produce valid JSON for reasonable numbers - * preserve precision well enough, and avoid some oddities - * like 0.3 -> 0.299999999999999988898 . - */ - char buf[64]; - sprintf(buf, "%.16g", num); - - if (number_is_valid(buf)) - sb_puts(out, buf); - else - sb_puts(out, "null"); -} - -static bool tag_is_valid(unsigned int tag) -{ - return (/* tag >= JSON_NULL && */ tag <= JSON_OBJECT); -} - -static bool number_is_valid(const char *num) -{ - return (parse_number(&num, NULL) && *num == '\0'); -} - -static bool expect_literal(const char **sp, const char *str) -{ - const char *s = *sp; - - while (*str != '\0') - if (*s++ != *str++) - return false; - - *sp = s; - return true; -} - -/* - * Parses exactly 4 hex characters (capital or lowercase). - * Fails if any input chars are not [0-9A-Fa-f]. - */ -static bool parse_hex16(const char **sp, uint16_t *out) -{ - const char *s = *sp; - uint16_t ret = 0; - uint16_t i; - uint16_t tmp; - char c; - - for (i = 0; i < 4; i++) { - c = *s++; - if (c >= '0' && c <= '9') - tmp = c - '0'; - else if (c >= 'A' && c <= 'F') - tmp = c - 'A' + 10; - else if (c >= 'a' && c <= 'f') - tmp = c - 'a' + 10; - else - return false; - - ret <<= 4; - ret += tmp; - } - - if (out) - *out = ret; - *sp = s; - return true; -} - -/* - * Encodes a 16-bit number into hexadecimal, - * writing exactly 4 hex chars. - */ -static int write_hex16(char *out, uint16_t val) -{ - const char *hex = "0123456789ABCDEF"; - - *out++ = hex[(val >> 12) & 0xF]; - *out++ = hex[(val >> 8) & 0xF]; - *out++ = hex[(val >> 4) & 0xF]; - *out++ = hex[ val & 0xF]; - - return 4; -} - -bool json_check(const JsonNode *node, char errmsg[256]) -{ - #define problem(...) do { \ - if (errmsg != NULL) \ - snprintf(errmsg, 256, __VA_ARGS__); \ - return false; \ - } while (0) - - if (node->key != NULL && !utf8_validate(node->key)) - problem("key contains invalid UTF-8"); - - if (!tag_is_valid(node->tag)) - problem("tag is invalid (%u)", node->tag); - - if (node->tag == JSON_BOOL) { - if (node->bool_ != false && node->bool_ != true) - problem("bool_ is neither false (%d) nor true (%d)", (int)false, (int)true); - } else if (node->tag == JSON_STRING) { - if (node->string_ == NULL) - problem("string_ is NULL"); - if (!utf8_validate(node->string_)) - problem("string_ contains invalid UTF-8"); - } else if (node->tag == JSON_ARRAY || node->tag == JSON_OBJECT) { - JsonNode *head = node->children.head; - JsonNode *tail = node->children.tail; - - if (head == NULL || tail == NULL) { - if (head != NULL) - problem("tail is NULL, but head is not"); - if (tail != NULL) - problem("head is NULL, but tail is not"); - } else { - JsonNode *child; - JsonNode *last = NULL; - - if (head->prev != NULL) - problem("First child's prev pointer is not NULL"); - - for (child = head; child != NULL; last = child, child = child->next) { - if (child == node) - problem("node is its own child"); - if (child->next == child) - problem("child->next == child (cycle)"); - if (child->next == head) - problem("child->next == head (cycle)"); - - if (child->parent != node) - problem("child does not point back to parent"); - if (child->next != NULL && child->next->prev != child) - problem("child->next does not point back to child"); - - if (node->tag == JSON_ARRAY && child->key != NULL) - problem("Array element's key is not NULL"); - if (node->tag == JSON_OBJECT && child->key == NULL) - problem("Object member's key is NULL"); - - if (!json_check(child, errmsg)) - return false; - } - - if (last != tail) - problem("tail does not match pointer found by starting at head and following next links"); - } - } - - return true; - - #undef problem -} diff --git a/phonelibs/json/src/json.h b/phonelibs/json/src/json.h deleted file mode 100644 index ed5255e6..00000000 --- a/phonelibs/json/src/json.h +++ /dev/null @@ -1,117 +0,0 @@ -/* - Copyright (C) 2011 Joseph A. Adams (joeyadams3.14159@gmail.com) - All rights reserved. - - 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. -*/ - -#ifndef CCAN_JSON_H -#define CCAN_JSON_H - -#include -#include - -typedef enum { - JSON_NULL, - JSON_BOOL, - JSON_STRING, - JSON_NUMBER, - JSON_ARRAY, - JSON_OBJECT, -} JsonTag; - -typedef struct JsonNode JsonNode; - -struct JsonNode -{ - /* only if parent is an object or array (NULL otherwise) */ - JsonNode *parent; - JsonNode *prev, *next; - - /* only if parent is an object (NULL otherwise) */ - char *key; /* Must be valid UTF-8. */ - - JsonTag tag; - union { - /* JSON_BOOL */ - bool bool_; - - /* JSON_STRING */ - char *string_; /* Must be valid UTF-8. */ - - /* JSON_NUMBER */ - double number_; - - /* JSON_ARRAY */ - /* JSON_OBJECT */ - struct { - JsonNode *head, *tail; - } children; - }; -}; - -/*** Encoding, decoding, and validation ***/ - -JsonNode *json_decode (const char *json); -char *json_encode (const JsonNode *node); -char *json_encode_string (const char *str); -char *json_stringify (const JsonNode *node, const char *space); -void json_delete (JsonNode *node); - -bool json_validate (const char *json); - -/*** Lookup and traversal ***/ - -JsonNode *json_find_element (JsonNode *array, int index); -JsonNode *json_find_member (JsonNode *object, const char *key); - -JsonNode *json_first_child (const JsonNode *node); - -#define json_foreach(i, object_or_array) \ - for ((i) = json_first_child(object_or_array); \ - (i) != NULL; \ - (i) = (i)->next) - -/*** Construction and manipulation ***/ - -JsonNode *json_mknull(void); -JsonNode *json_mkbool(bool b); -JsonNode *json_mkstring(const char *s); -JsonNode *json_mknumber(double n); -JsonNode *json_mkarray(void); -JsonNode *json_mkobject(void); - -void json_append_element(JsonNode *array, JsonNode *element); -void json_prepend_element(JsonNode *array, JsonNode *element); -void json_append_member(JsonNode *object, const char *key, JsonNode *value); -void json_prepend_member(JsonNode *object, const char *key, JsonNode *value); - -void json_remove_from_parent(JsonNode *node); - -/*** Debugging ***/ - -/* - * Look for structure and encoding problems in a JsonNode or its descendents. - * - * If a problem is detected, return false, writing a description of the problem - * to errmsg (unless errmsg is NULL). - */ -bool json_check(const JsonNode *node, char errmsg[256]); - -#endif diff --git a/phonelibs/linux/include/msm_ion.h b/phonelibs/linux/include/msm_ion.h index e41b3170..d5caed5e 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 bfb1841f..62595a15 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 @@ error: 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; } -// 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 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) { - int i,j,k, return_value = 1; + 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, const stbtt_fontinfo *info, stbtt_pack_range *ranges, int num_ranges, stbrp_rect *rects) +{ + 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 00000000..baf4e9cb --- /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-android-clang3.8/libSNPE.so b/phonelibs/snpe/aarch64-android-clang3.8/libSNPE.so deleted file mode 100644 index b2e5b007..00000000 Binary files a/phonelibs/snpe/aarch64-android-clang3.8/libSNPE.so and /dev/null differ diff --git a/phonelibs/snpe/aarch64-android-clang3.8/libsnpe_dsp_skel.so b/phonelibs/snpe/aarch64-android-clang3.8/libsnpe_dsp_skel.so deleted file mode 100644 index f63f2fae..00000000 Binary files a/phonelibs/snpe/aarch64-android-clang3.8/libsnpe_dsp_skel.so and /dev/null differ diff --git a/phonelibs/snpe/aarch64-android-clang3.8/libsymphony-cpu.so b/phonelibs/snpe/aarch64-android-clang3.8/libsymphony-cpu.so deleted file mode 100755 index ca05458e..00000000 Binary files a/phonelibs/snpe/aarch64-android-clang3.8/libsymphony-cpu.so and /dev/null differ diff --git a/phonelibs/snpe/aarch64-android-clang3.8/libsymphonypower.so b/phonelibs/snpe/aarch64-android-clang3.8/libsymphonypower.so deleted file mode 100755 index 30f676d7..00000000 Binary files a/phonelibs/snpe/aarch64-android-clang3.8/libsymphonypower.so and /dev/null differ diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libSNPE.so b/phonelibs/snpe/aarch64-android-clang6.0/libSNPE.so new file mode 100644 index 00000000..f8338a6e Binary files /dev/null and b/phonelibs/snpe/aarch64-android-clang6.0/libSNPE.so differ diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libc++_shared.so b/phonelibs/snpe/aarch64-android-clang6.0/libc++_shared.so new file mode 100644 index 00000000..5b9a9cff Binary files /dev/null and b/phonelibs/snpe/aarch64-android-clang6.0/libc++_shared.so differ diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_adsp.so b/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_adsp.so new file mode 100644 index 00000000..98b8c2d8 Binary files /dev/null and b/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_adsp.so differ diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_skel.so b/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_skel.so new file mode 100644 index 00000000..eecf4574 Binary files /dev/null and b/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_skel.so differ diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libsymphony-cpu.so b/phonelibs/snpe/aarch64-android-clang6.0/libsymphony-cpu.so new file mode 100755 index 00000000..8dd26fb2 Binary files /dev/null and b/phonelibs/snpe/aarch64-android-clang6.0/libsymphony-cpu.so differ diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libSNPE.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libSNPE.so new file mode 100644 index 00000000..45cf75d4 Binary files /dev/null and b/phonelibs/snpe/aarch64-linux-gcc4.9/libSNPE.so differ 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 00000000..809fd4ef Binary files /dev/null and b/phonelibs/snpe/aarch64-linux-gcc4.9/libatomic.so.1 differ diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator.so new file mode 100644 index 00000000..9452b0e4 Binary files /dev/null and b/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator.so differ diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator_domains.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator_domains.so new file mode 100644 index 00000000..eb3d336e Binary files /dev/null and b/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator_domains.so differ diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_adsp.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_adsp.so new file mode 100644 index 00000000..61a910ff Binary files /dev/null and b/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_adsp.so differ diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_dsp_domains_v2.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_dsp_domains_v2.so new file mode 100644 index 00000000..c12328c7 Binary files /dev/null and b/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_dsp_domains_v2.so differ diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libsymphony-cpu.so b/phonelibs/snpe/aarch64-linux-gcc4.9/libsymphony-cpu.so new file mode 100755 index 00000000..202d7e97 Binary files /dev/null and b/phonelibs/snpe/aarch64-linux-gcc4.9/libsymphony-cpu.so differ diff --git a/phonelibs/snpe/include/DiagLog/Options.hpp b/phonelibs/snpe/include/DiagLog/Options.hpp index ba0c8211..ea247e95 100644 --- a/phonelibs/snpe/include/DiagLog/Options.hpp +++ b/phonelibs/snpe/include/DiagLog/Options.hpp @@ -32,7 +32,8 @@ public: 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 @@ public: /// 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 c4b465fd..f2022ca9 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 @@ public: /** * 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 @@ public: /** * 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 @@ public: /** * 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 @@ public: /** * 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 @@ public: */ 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 fde252ca..5b8b6b35 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 383403b5..a8b0b4ea 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 2c41b6b6..35416b42 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 e80f6c13..f893e0a2 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 @@ public: */ 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 70f6b33c..aefa0eda 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 @@ public: 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 @@ public: * 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 @@ public: 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 @@ private: 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 @@ public: */ 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 df452dc4..3f15f177 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 @@ public: PlatformConfigInfo(){}; }; - PlatformConfig() : m_PlatformType(PlatformType_t::UNKNOWN) {}; + PlatformConfig() : m_PlatformType(PlatformType_t::UNKNOWN), + m_PlatformOptions("") {}; /** * @brief Retrieves the platform type @@ -156,9 +158,42 @@ public: 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 00000000..1088a5b3 --- /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 00000000..380a3e0e --- /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 00000000..732b7aa4 --- /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 00000000..ecd438b9 --- /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 ba84265b..e0821b90 100644 --- a/phonelibs/snpe/include/SNPE/SNPE.hpp +++ b/phonelibs/snpe/include/SNPE/SNPE.hpp @@ -83,6 +83,14 @@ public: 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 13e80e8c..f314d88b 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 @@ public: ~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 @@ public: 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 @@ public: 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 @@ public: 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 @@ public: */ 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 @@ public: */ 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 bb9921af..7bef45d8 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 @@ public: */ 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 00000000..a660bca0 --- /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/selfdrive/assets/sounds/warning_2.wav b/selfdrive/assets/sounds/warning_2.wav index e9709d9f..4909f119 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 90043bfe..b9e8d79e 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python3.7 +#!/usr/bin/env python3 import json import os import hashlib @@ -22,6 +22,7 @@ from common import android 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/athena/manage_athenad.py b/selfdrive/athena/manage_athenad.py index 2954c70b..49120d97 100755 --- a/selfdrive/athena/manage_athenad.py +++ b/selfdrive/athena/manage_athenad.py @@ -27,10 +27,10 @@ def main(): proc.join() cloudlog.event("athenad exited", exitcode=proc.exitcode) time.sleep(5) - except: + except Exception: cloudlog.exception("manage_athenad.exception") finally: params.delete(ATHENA_MGR_PID_PARAM) if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 0955be10..d5990bc1 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -6,7 +6,6 @@ #include #include #include -#include #include #include #include @@ -29,6 +28,7 @@ #include "messaging.hpp" #include +#include // double the FIFO size #define RECV_SIZE (0x1000) @@ -56,7 +56,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; @@ -95,7 +95,7 @@ void *safety_setter_thread(void *s) { // switch to SILENT when CarVin param is read while (1) { if (do_exit) return NULL; - const int result = read_db_value(NULL, "CarVin", &value_vin, &value_vin_sz); + const int result = read_db_value("CarVin", &value_vin, &value_vin_sz); if (value_vin_sz > 0) { // sanity check VIN format assert(value_vin_sz == 17); @@ -104,6 +104,7 @@ 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); @@ -117,7 +118,7 @@ void *safety_setter_thread(void *s) { while (1) { if (do_exit) return NULL; - const int result = read_db_value(NULL, "CarParams", &value, &value_sz); + const int result = read_db_value("CarParams", &value, &value_sz); if (value_sz > 0) break; usleep(100*1000); } @@ -159,6 +160,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; } @@ -177,13 +183,13 @@ bool usb_connect() { err2 = libusb_control_transfer(dev_handle, 0xc0, 0xd4, 0, 0, fw_sig_buf + 64, 64, TIMEOUT); if ((err == 64) && (err2 == 64)) { printf("FW signature read\n"); - write_db_value(NULL, "PandaFirmware", (const char *)fw_sig_buf, 128); + write_db_value("PandaFirmware", (const char *)fw_sig_buf, 128); for (size_t i = 0; i < 8; i++){ fw_sig_hex_buf[2*i] = NIBBLE_TO_HEX(fw_sig_buf[i] >> 4); fw_sig_hex_buf[2*i+1] = NIBBLE_TO_HEX(fw_sig_buf[i] & 0xF); } - write_db_value(NULL, "PandaFirmwareHex", (const char *)fw_sig_hex_buf, 16); + write_db_value("PandaFirmwareHex", (const char *)fw_sig_hex_buf, 16); } else { goto fail; } @@ -193,7 +199,7 @@ bool usb_connect() { if (err > 0) { serial = (const char *)serial_buf; serial_sz = strnlen(serial, err); - write_db_value(NULL, "PandaDongleId", serial, serial_sz); + write_db_value("PandaDongleId", serial, serial_sz); printf("panda serial: %.*s\n", serial_sz, serial); } else { goto fail; } @@ -259,6 +265,7 @@ fail: 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 +364,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; @@ -394,7 +418,7 @@ void can_health(PubSocket *publisher) { if ((no_ignition_exp || (voltage_f < VBATT_PAUSE_CHARGING)) && cdp_mode && !ignition) { char *disable_power_down = NULL; size_t disable_power_down_sz = 0; - const int result = read_db_value(NULL, "DisablePowerDown", &disable_power_down, &disable_power_down_sz); + const int result = read_db_value("DisablePowerDown", &disable_power_down, &disable_power_down_sz); if (disable_power_down_sz != 1 || disable_power_down[0] != '1') { printf("TURN OFF CHARGING!\n"); pthread_mutex_lock(&usb_lock); @@ -432,9 +456,9 @@ void can_health(PubSocket *publisher) { // clear VIN, CarParams, and set new safety on car start if (ignition && !ignition_last) { - int result = delete_db_value(NULL, "CarVin"); + int result = delete_db_value("CarVin"); assert((result == 0) || (result == ERR_NO_VALUE)); - result = delete_db_value(NULL, "CarParams"); + result = delete_db_value("CarParams"); assert((result == 0) || (result == ERR_NO_VALUE)); if (!safety_setter_thread_initialized) { @@ -475,12 +499,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 +519,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 +585,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 +631,9 @@ void *can_send_thread(void *crap) { while (!do_exit) { can_send(subscriber); } + + delete subscriber; + delete context; return NULL; } @@ -624,6 +664,9 @@ void *can_recv_thread(void *crap) { next_frame_time += dt; } + + delete publisher; + delete c; return NULL; } @@ -639,6 +682,9 @@ void *can_health_thread(void *crap) { can_health(publisher); usleep(500*1000); } + + delete publisher; + delete c; return NULL; } @@ -795,7 +841,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 +919,8 @@ void *pigeon_thread(void *crap) { cnt++; } + delete publisher; + delete context; return NULL; } @@ -899,21 +947,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); + +#if LIBUSB_API_VERSION >= 0x01000106 + libusb_set_option(ctx, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_INFO); +#else libusb_set_debug(ctx, 3); +#endif - // connect to the board - usb_retry_connect(); - - - // 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 1da81997..5956c11a 100644 --- a/selfdrive/boardd/boardd_setup.py +++ b/selfdrive/boardd/boardd_setup.py @@ -21,7 +21,10 @@ if ARCH == "x86_64": 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}, @@ -32,7 +35,6 @@ setup(name='Boardd API Implementation', library_dirs=[ './', PHONELIBS + '/capnp-cpp/' + ARCH_DIR + '/lib/', - PHONELIBS + '/capnp-c/' + ARCH_DIR + '/lib/' ], sources=['boardd_api_impl.pyx'], language="c++", diff --git a/selfdrive/boardd/tests/replay_many.py b/selfdrive/boardd/tests/replay_many.py index 02486776..7596b47a 100755 --- a/selfdrive/boardd/tests/replay_many.py +++ b/selfdrive/boardd/tests/replay_many.py @@ -4,7 +4,8 @@ import sys 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) - can_sock = messaging.sub_sock('can') + sender.set_can_loopback(False) + can_sock = messaging.sub_sock('can') - while True: - # Send messages one bus 0 and 1 - tsc = messaging.recv_one(can_sock) - snd = can_capnp_to_can_list(tsc.can) - snd = list(filter(lambda x: x[-1] <= 2, snd)) - sender.can_send_many(snd) + 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)) - # Drain panda message buffer - sender.can_recv() - except Exception: - traceback.print_exc() + try: + sender.can_send_many(snd) + except usb1.USBErrorTimeout: + pass + + # Drain panda message buffer + sender.can_recv() + except Exception: + traceback.print_exc() + time.sleep(1) if __name__ == "__main__": if jungle: @@ -50,12 +56,22 @@ if __name__ == "__main__": 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 12e1afe5..e1930a83 100644 --- a/selfdrive/camerad/SConscript +++ b/selfdrive/camerad/SConscript @@ -1,13 +1,24 @@ -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] +libs = ['m', 'pthread', common, 'jpeg', cereal, 'OpenCL', messaging, 'czmq', 'zmq', 'capnp', 'kj', visionipc, gpucommon] if arch == "aarch64": libs += ['gsl', 'CB', 'adreno_utils', 'EGL', 'GLESv3', 'cutils', 'ui'] - cameras = ['cameras/camera_qcom.c'] -else: + cameras = ['cameras/camera_qcom.cc'] +elif arch == "larch64": libs += [] - cameras = ['cameras/camera_frame_stream.cc'] + cameras = ['cameras/camera_qcom2.c'] +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"]) @@ -15,5 +26,6 @@ env.SharedLibrary('snapshot/visionipc', env.Program('camerad', [ 'main.cc', 'transforms/rgb_to_yuv.c', + 'imgproc/utils.cc', cameras, ], LIBS=libs) diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index 187952e2..1bd8ac6c 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -10,7 +10,10 @@ #define CAMERA_ID_OV8865 3 #define CAMERA_ID_IMX298_FLIPPED 4 #define CAMERA_ID_OV10640 5 -#define CAMERA_ID_MAX 6 +#define CAMERA_ID_LGC920 6 +#define CAMERA_ID_LGC615 7 +#define CAMERA_ID_AR0231 8 +#define CAMERA_ID_MAX 9 #ifdef __cplusplus extern "C" { diff --git a/selfdrive/camerad/cameras/camera_frame_stream.cc b/selfdrive/camerad/cameras/camera_frame_stream.cc index 1a9f8a93..0061fe61 100644 --- a/selfdrive/camerad/cameras/camera_frame_stream.cc +++ b/selfdrive/camerad/cameras/camera_frame_stream.cc @@ -115,7 +115,7 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = { .frame_width = 1632, .frame_height = 1224, .frame_stride = 2040, // seems right - .bayer = false, + .bayer = true, .bayer_flip = 3, .hdr = false }, diff --git a/selfdrive/camerad/cameras/camera_frame_stream.h b/selfdrive/camerad/cameras/camera_frame_stream.h index 5cb7e27c..80ff54b5 100644 --- a/selfdrive/camerad/cameras/camera_frame_stream.h +++ b/selfdrive/camerad/cameras/camera_frame_stream.h @@ -3,6 +3,7 @@ #include +#define CL_USE_DEPRECATED_OPENCL_1_2_APIS #ifdef __APPLE__ #include #else diff --git a/selfdrive/camerad/cameras/camera_qcom.c b/selfdrive/camerad/cameras/camera_qcom.cc similarity index 87% rename from selfdrive/camerad/cameras/camera_qcom.c rename to selfdrive/camerad/cameras/camera_qcom.cc index 1d1396d7..1c7af57b 100644 --- a/selfdrive/camerad/cameras/camera_qcom.c +++ b/selfdrive/camerad/cameras/camera_qcom.cc @@ -13,7 +13,7 @@ #include #include - +#include #include "msmb_isp.h" #include "msmb_ispif.h" #include "msmb_camera.h" @@ -24,7 +24,7 @@ #include "common/swaglog.h" #include "common/params.h" -#include "cereal/gen/c/log.capnp.h" +#include "cereal/gen/cpp/log.capnp.h" #include "sensor_i2c.h" @@ -99,7 +99,7 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = { }; static void camera_release_buffer(void* cookie, int buf_idx) { - CameraState *s = cookie; + CameraState *s = (CameraState *)cookie; // printf("camera_release_buffer %d\n", buf_idx); s->ss[0].qbuf_info[buf_idx].dirty_buf = 1; ioctl(s->isp_fd, VIDIOC_MSM_ISP_ENQUEUE_BUF, &s->ss[0].qbuf_info[buf_idx]); @@ -134,10 +134,10 @@ static void camera_init(CameraState *s, int camera_id, int camera_num, } -int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size_t size, int data_type) { +int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size_t size, msm_camera_i2c_data_type data_type) { struct msm_camera_i2c_reg_setting out_settings = { .reg_setting = arr, - .size = size, + .size = (uint16_t)size, .addr_type = MSM_CAMERA_I2C_WORD_ADDR, .data_type = data_type, .delay = 0, @@ -151,7 +151,7 @@ int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size static int imx298_apply_exposure(CameraState *s, int gain, int integ_lines, int frame_length) { int err; - int analog_gain = min(gain, 448); + int analog_gain = std::min(gain, 448); if (gain > 448) { s->digital_gain = (512.0/(512-(gain))) / 8.0; @@ -177,12 +177,12 @@ static int imx298_apply_exposure(CameraState *s, int gain, int integ_lines, int {0x3002,0x0,0}, // long autoexposure off // FRM_LENGTH - {0x340, frame_length >> 8, 0}, {0x341, frame_length & 0xff, 0}, + {0x340, (uint16_t)(frame_length >> 8), 0}, {0x341, (uint16_t)(frame_length & 0xff), 0}, // INTEG_TIME aka coarse_int_time_addr aka shutter speed - {0x202, integ_lines >> 8, 0}, {0x203, integ_lines & 0xff,0}, + {0x202, (uint16_t)(integ_lines >> 8), 0}, {0x203, (uint16_t)(integ_lines & 0xff),0}, // global_gain_addr // if you assume 1x gain is 32, 448 is 14x gain, aka 2^14=16384 - {0x204, analog_gain >> 8, 0}, {0x205, analog_gain & 0xff,0}, + {0x204, (uint16_t)(analog_gain >> 8), 0}, {0x205, (uint16_t)(analog_gain & 0xff),0}, // digital gain for colors: gain_greenR, gain_red, gain_blue, gain_greenB /*{0x20e, digital_gain_gr >> 8, 0}, {0x20f,digital_gain_gr & 0xFF,0}, @@ -222,13 +222,13 @@ static int ov8865_apply_exposure(CameraState *s, int gain, int integ_lines, int //{0x104,0x1,0}, // FRM_LENGTH - {0x380e, frame_length >> 8, 0}, {0x380f, frame_length & 0xff, 0}, + {0x380e, (uint16_t)(frame_length >> 8), 0}, {0x380f, (uint16_t)(frame_length & 0xff), 0}, // AEC EXPO - {0x3500, integ_lines >> 16, 0}, {0x3501, integ_lines >> 8, 0}, {0x3502, integ_lines & 0xff,0}, + {0x3500, (uint16_t)(integ_lines >> 16), 0}, {0x3501, (uint16_t)(integ_lines >> 8), 0}, {0x3502, (uint16_t)(integ_lines & 0xff),0}, // AEC MANUAL {0x3503, 0x4, 0}, // AEC GAIN - {0x3508, gain_bitmap, 0}, {0x3509, 0xf8, 0}, + {0x3508, (uint16_t)(gain_bitmap), 0}, {0x3509, 0xf8, 0}, //{0x104,0x0,0}, }; @@ -253,11 +253,11 @@ static int imx179_s5k3p8sp_apply_exposure(CameraState *s, int gain, int integ_li {0x104,0x1,0}, // FRM_LENGTH - {0x340, frame_length >> 8, 0}, {0x341, frame_length & 0xff, 0}, + {0x340, (uint16_t)(frame_length >> 8), 0}, {0x341, (uint16_t)(frame_length & 0xff), 0}, // coarse_int_time - {0x202, integ_lines >> 8, 0}, {0x203, integ_lines & 0xff,0}, + {0x202, (uint16_t)(integ_lines >> 8), 0}, {0x203, (uint16_t)(integ_lines & 0xff),0}, // global_gain - {0x204, gain >> 8, 0}, {0x205, gain & 0xff,0}, + {0x204, (uint16_t)(gain >> 8), 0}, {0x205, (uint16_t)(gain & 0xff),0}, {0x104,0x0,0}, }; @@ -365,7 +365,7 @@ static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) { integ_lines = frame_length * exposure_frac; // See page 79 of the datasheet, this is the max allowed (-1 for phase adjust) - integ_lines = min(integ_lines, frame_length-11); + integ_lines = std::min(integ_lines, frame_length-11); } if (gain_frac >= 0) { @@ -437,7 +437,8 @@ void camera_autoexposure(CameraState *s, float grey_frac) { static uint8_t* get_eeprom(int eeprom_fd, size_t *out_len) { int err; - struct msm_eeprom_cfg_data cfg = {0}; + struct msm_eeprom_cfg_data cfg; + memset(&cfg, 0, sizeof(struct msm_eeprom_cfg_data)); cfg.cfgtype = CFG_EEPROM_GET_CAL_DATA; err = ioctl(eeprom_fd, VIDIOC_MSM_EEPROM_CFG, &cfg); assert(err >= 0); @@ -445,7 +446,7 @@ static uint8_t* get_eeprom(int eeprom_fd, size_t *out_len) { uint32_t num_bytes = cfg.cfg.get_data.num_bytes; assert(num_bytes > 100); - uint8_t* buffer = malloc(num_bytes); + uint8_t* buffer = (uint8_t*)malloc(num_bytes); assert(buffer); memset(buffer, 0, num_bytes); @@ -537,7 +538,8 @@ static void sensors_init(DualCameraState *s) { } assert(sensorinit_fd >= 0); - struct sensor_init_cfg_data sensor_init_cfg = {0}; + struct sensor_init_cfg_data sensor_init_cfg; + memset(&sensor_init_cfg, 0, sizeof(struct sensor_init_cfg_data)); // init rear sensor @@ -549,10 +551,10 @@ static void sensors_init(DualCameraState *s) { .actuator_name = "dw9800w", .ois_name = "", .flash_name = "pmic", - .camera_id = 0, + .camera_id = CAMERA_0, .slave_addr = 32, - .i2c_freq_mode = 1, - .addr_type = 2, + .i2c_freq_mode = I2C_FAST_MODE, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, .sensor_id_info = { .sensor_id_reg_addr = 22, .sensor_id = 664, @@ -563,37 +565,37 @@ static void sensors_init(DualCameraState *s) { .power_setting_array = { .power_setting_a = { { - .seq_type = 1, + .seq_type = SENSOR_GPIO, .seq_val = 0, .config_val = 0, .delay = 1, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 2, .config_val = 0, .delay = 0, },{ - .seq_type = 1, + .seq_type = SENSOR_GPIO, .seq_val = 5, .config_val = 2, .delay = 0, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 1, .config_val = 0, .delay = 0, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 3, .config_val = 0, .delay = 1, },{ - .seq_type = 0, + .seq_type = SENSOR_CLK, .seq_val = 0, .config_val = 24000000, .delay = 1, },{ - .seq_type = 1, + .seq_type = SENSOR_GPIO, .seq_val = 0, .config_val = 2, .delay = 10, @@ -602,32 +604,32 @@ static void sensors_init(DualCameraState *s) { .size = 7, .power_down_setting_a = { { - .seq_type = 0, + .seq_type = SENSOR_CLK, .seq_val = 0, .config_val = 0, .delay = 1, },{ - .seq_type = 1, + .seq_type = SENSOR_GPIO, .seq_val = 0, .config_val = 0, .delay = 1, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 1, .config_val = 0, .delay = 0, },{ - .seq_type = 1, + .seq_type = SENSOR_GPIO, .seq_val = 5, .config_val = 0, .delay = 0, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 2, .config_val = 0, .delay = 0, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 3, .config_val = 0, .delay = 1, @@ -638,10 +640,10 @@ static void sensors_init(DualCameraState *s) { .is_init_params_valid = 0, .sensor_init_params = { .modes_supported = 1, - .position = 0, + .position = BACK_CAMERA_B, .sensor_mount_angle = 90, }, - .output_format = 0, + .output_format = MSM_SENSOR_BAYER, }; } else { slave_info = (struct msm_camera_sensor_slave_info){ @@ -649,10 +651,10 @@ static void sensors_init(DualCameraState *s) { .eeprom_name = "sony_imx298", .actuator_name = "rohm_bu63165gwl", .ois_name = "rohm_bu63165gwl", - .camera_id = 0, + .camera_id = CAMERA_0, .slave_addr = 52, - .i2c_freq_mode = 2, - .addr_type = 2, + .i2c_freq_mode = I2C_CUSTOM_MODE, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, .sensor_id_info = { .sensor_id_reg_addr = 22, .sensor_id = 664, @@ -661,47 +663,47 @@ static void sensors_init(DualCameraState *s) { .power_setting_array = { .power_setting_a = { { - .seq_type = 1, + .seq_type = SENSOR_GPIO, .seq_val = 0, .config_val = 0, .delay = 2, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 2, .config_val = 0, .delay = 2, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 0, .config_val = 0, .delay = 2, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 1, .config_val = 0, .delay = 2, },{ - .seq_type = 1, + .seq_type = SENSOR_GPIO, .seq_val = 6, .config_val = 2, .delay = 0, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 3, .config_val = 0, .delay = 5, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 4, .config_val = 0, .delay = 5, },{ - .seq_type = 0, + .seq_type = SENSOR_CLK, .seq_val = 0, .config_val = 24000000, .delay = 2, },{ - .seq_type = 1, + .seq_type = SENSOR_GPIO, .seq_val = 0, .config_val = 2, .delay = 2, @@ -710,42 +712,42 @@ static void sensors_init(DualCameraState *s) { .size = 9, .power_down_setting_a = { { - .seq_type = 1, + .seq_type = SENSOR_GPIO, .seq_val = 0, .config_val = 0, .delay = 10, },{ - .seq_type = 0, + .seq_type = SENSOR_CLK, .seq_val = 0, .config_val = 0, .delay = 1, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 4, .config_val = 0, .delay = 0, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 3, .config_val = 0, .delay = 1, },{ - .seq_type = 1, + .seq_type = SENSOR_GPIO, .seq_val = 6, .config_val = 0, .delay = 0, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 1, .config_val = 0, .delay = 0, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 0, .config_val = 0, .delay = 0, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 2, .config_val = 0, .delay = 0, @@ -756,10 +758,10 @@ static void sensors_init(DualCameraState *s) { .is_init_params_valid = 0, .sensor_init_params = { .modes_supported = 1, - .position = 0, + .position = BACK_CAMERA_B, .sensor_mount_angle = 360, }, - .output_format = 0, + .output_format = MSM_SENSOR_BAYER, }; } slave_info.power_setting_array.power_setting = @@ -781,10 +783,10 @@ static void sensors_init(DualCameraState *s) { .actuator_name = "", .ois_name = "", .flash_name = "", - .camera_id = 2, + .camera_id = CAMERA_2, .slave_addr = 108, - .i2c_freq_mode = 1, - .addr_type = 2, + .i2c_freq_mode = I2C_FAST_MODE, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, .sensor_id_info = { .sensor_id_reg_addr = 12299, .sensor_id = 34917, @@ -795,32 +797,32 @@ static void sensors_init(DualCameraState *s) { .power_setting_array = { .power_setting_a = { { - .seq_type = 1, + .seq_type = SENSOR_GPIO, .seq_val = 0, .config_val = 0, .delay = 5, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 1, .config_val = 0, .delay = 0, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 2, .config_val = 0, .delay = 0, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 0, .config_val = 0, .delay = 0, },{ - .seq_type = 0, + .seq_type = SENSOR_CLK, .seq_val = 0, .config_val = 24000000, .delay = 1, },{ - .seq_type = 1, + .seq_type = SENSOR_GPIO, .seq_val = 0, .config_val = 2, .delay = 1, @@ -829,27 +831,27 @@ static void sensors_init(DualCameraState *s) { .size = 6, .power_down_setting_a = { { - .seq_type = 1, + .seq_type = SENSOR_GPIO, .seq_val = 0, .config_val = 0, .delay = 5, },{ - .seq_type = 0, + .seq_type = SENSOR_CLK, .seq_val = 0, .config_val = 0, .delay = 1, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 0, .config_val = 0, .delay = 0, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 1, .config_val = 0, .delay = 0, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 2, .config_val = 0, .delay = 1, @@ -860,10 +862,10 @@ static void sensors_init(DualCameraState *s) { .is_init_params_valid = 0, .sensor_init_params = { .modes_supported = 1, - .position = 1, + .position = FRONT_CAMERA_B, .sensor_mount_angle = 270, }, - .output_format = 0, + .output_format = MSM_SENSOR_BAYER, }; } else if (s->front.camera_id == CAMERA_ID_S5K3P8SP) { // init front camera @@ -872,10 +874,10 @@ static void sensors_init(DualCameraState *s) { .eeprom_name = "s5k3p8sp_m24c64s", .actuator_name = "", .ois_name = "", - .camera_id = 1, + .camera_id = CAMERA_1, .slave_addr = 32, - .i2c_freq_mode = 1, - .addr_type = 2, + .i2c_freq_mode = I2C_FAST_MODE, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, .sensor_id_info = { .sensor_id_reg_addr = 0, .sensor_id = 12552, @@ -884,32 +886,32 @@ static void sensors_init(DualCameraState *s) { .power_setting_array = { .power_setting_a = { { - .seq_type = 1, + .seq_type = SENSOR_GPIO, .seq_val = 0, .config_val = 0, .delay = 1, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 2, .config_val = 0, .delay = 1, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 1, .config_val = 0, .delay = 1, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 0, .config_val = 0, .delay = 1, },{ - .seq_type = 0, + .seq_type = SENSOR_CLK, .seq_val = 0, .config_val = 24000000, .delay = 1, },{ - .seq_type = 1, + .seq_type = SENSOR_GPIO, .seq_val = 0, .config_val = 2, .delay = 1, @@ -918,27 +920,27 @@ static void sensors_init(DualCameraState *s) { .size = 6, .power_down_setting_a = { { - .seq_type = 0, + .seq_type = SENSOR_CLK, .seq_val = 0, .config_val = 0, .delay = 1, },{ - .seq_type = 1, + .seq_type = SENSOR_GPIO, .seq_val = 0, .config_val = 0, .delay = 1, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 0, .config_val = 0, .delay = 1, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 1, .config_val = 0, .delay = 1, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 2, .config_val = 0, .delay = 1, @@ -949,10 +951,10 @@ static void sensors_init(DualCameraState *s) { .is_init_params_valid = 0, .sensor_init_params = { .modes_supported = 1, - .position = 1, + .position = FRONT_CAMERA_B, .sensor_mount_angle = 270, }, - .output_format = 0, + .output_format = MSM_SENSOR_BAYER, }; } else { // init front camera @@ -961,10 +963,10 @@ static void sensors_init(DualCameraState *s) { .eeprom_name = "sony_imx179", .actuator_name = "", .ois_name = "", - .camera_id = 1, + .camera_id = CAMERA_1, .slave_addr = 32, - .i2c_freq_mode = 1, - .addr_type = 2, + .i2c_freq_mode = I2C_FAST_MODE, + .addr_type = MSM_CAMERA_I2C_WORD_ADDR, .sensor_id_info = { .sensor_id_reg_addr = 2, .sensor_id = 377, @@ -973,27 +975,27 @@ static void sensors_init(DualCameraState *s) { .power_setting_array = { .power_setting_a = { { - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 2, .config_val = 0, .delay = 0, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 1, .config_val = 0, .delay = 0, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 0, .config_val = 0, .delay = 0, },{ - .seq_type = 1, + .seq_type = SENSOR_GPIO, .seq_val = 0, .config_val = 2, .delay = 0, },{ - .seq_type = 0, + .seq_type = SENSOR_CLK, .seq_val = 0, .config_val = 24000000, .delay = 0, @@ -1002,27 +1004,27 @@ static void sensors_init(DualCameraState *s) { .size = 5, .power_down_setting_a = { { - .seq_type = 0, + .seq_type = SENSOR_CLK, .seq_val = 0, .config_val = 0, .delay = 0, },{ - .seq_type = 1, + .seq_type = SENSOR_GPIO, .seq_val = 0, .config_val = 0, .delay = 1, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 0, .config_val = 0, .delay = 2, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 1, .config_val = 0, .delay = 0, },{ - .seq_type = 2, + .seq_type = SENSOR_VREG, .seq_val = 2, .config_val = 0, .delay = 0, @@ -1033,10 +1035,10 @@ static void sensors_init(DualCameraState *s) { .is_init_params_valid = 0, .sensor_init_params = { .modes_supported = 1, - .position = 1, + .position = FRONT_CAMERA_B, .sensor_mount_angle = 270, }, - .output_format = 0, + .output_format = MSM_SENSOR_BAYER, }; } slave_info2.power_setting_array.power_setting = @@ -1053,22 +1055,34 @@ static void sensors_init(DualCameraState *s) { static void camera_open(CameraState *s, bool rear) { int err; - struct sensorb_cfg_data sensorb_cfg_data = {0}; - struct csid_cfg_data csid_cfg_data = {0}; - struct csiphy_cfg_data csiphy_cfg_data = {0}; - struct msm_camera_csiphy_params csiphy_params = {0}; - struct msm_camera_csid_params csid_params = {0}; - struct msm_vfe_input_cfg input_cfg = {0}; - struct msm_vfe_axi_stream_update_cmd update_cmd = {0}; - struct v4l2_event_subscription sub = {0}; - struct ispif_cfg_data ispif_cfg_data = {0}; - struct msm_vfe_cfg_cmd_list cfg_cmd_list = {0}; + struct sensorb_cfg_data sensorb_cfg_data; + memset(&sensorb_cfg_data, 0, sizeof(struct sensorb_cfg_data)); + struct csid_cfg_data csid_cfg_data; + memset(&csid_cfg_data, 0, sizeof(struct csid_cfg_data)); + struct csiphy_cfg_data csiphy_cfg_data; + memset(&csiphy_cfg_data, 0, sizeof(struct csiphy_cfg_data)); + struct msm_camera_csiphy_params csiphy_params; + memset(&csiphy_params, 0, sizeof(struct msm_camera_csiphy_params)); + struct msm_camera_csid_params csid_params; + memset(&csid_params, 0, sizeof(struct msm_camera_csid_params)); + struct msm_vfe_input_cfg input_cfg; + memset(&input_cfg, 0, sizeof(struct msm_vfe_input_cfg)); + struct msm_vfe_axi_stream_update_cmd update_cmd; + memset(&update_cmd, 0, sizeof(struct msm_vfe_axi_stream_update_cmd)); + struct v4l2_event_subscription sub; + memset(&sub, 0, sizeof(struct v4l2_event_subscription)); + struct ispif_cfg_data ispif_cfg_data; + memset(&ispif_cfg_data, 0, sizeof(struct ispif_cfg_data)); + struct msm_vfe_cfg_cmd_list cfg_cmd_list; + memset(&cfg_cmd_list, 0, sizeof(struct msm_vfe_cfg_cmd_list)); - struct msm_actuator_cfg_data actuator_cfg_data = {0}; - struct msm_ois_cfg_data ois_cfg_data = {0}; + struct msm_actuator_cfg_data actuator_cfg_data; + memset(&actuator_cfg_data, 0, sizeof(struct msm_actuator_cfg_data)); + struct msm_ois_cfg_data ois_cfg_data; + memset(&ois_cfg_data, 0, sizeof(struct msm_ois_cfg_data)); // open devices - char *sensor_dev; + const char *sensor_dev; if (rear) { s->csid_fd = open("/dev/v4l-subdev3", O_RDWR | O_NONBLOCK); assert(s->csid_fd >= 0); @@ -1333,7 +1347,7 @@ static void camera_open(CameraState *s, bool rear) { } }, .af_tuning_params = { - .initial_code = s->infinity_dac, + .initial_code = (int16_t)s->infinity_dac, .pwd_step = 0, .region_size = 1, .total_steps = 512, @@ -1419,7 +1433,7 @@ static void camera_open(CameraState *s, bool rear) { } }, .af_tuning_params = { - .initial_code = s->infinity_dac, + .initial_code = (int16_t)s->infinity_dac, .pwd_step = 0, .region_size = 1, .total_steps = 238, @@ -1512,7 +1526,7 @@ static void camera_open(CameraState *s, bool rear) { StreamState *ss = &s->ss[i]; memset(&input_cfg, 0, sizeof(struct msm_vfe_input_cfg)); - input_cfg.input_src = VFE_RAW_0+i; + input_cfg.input_src = (msm_vfe_input_src)(VFE_RAW_0+i); input_cfg.input_pix_clk = s->pixel_clock; input_cfg.d.rdi_cfg.cid = i; input_cfg.d.rdi_cfg.frame_based = 1; @@ -1534,7 +1548,7 @@ static void camera_open(CameraState *s, bool rear) { } else { ss->stream_req.output_format = v4l2_fourcc('Q', 'M', 'E', 'T'); } - ss->stream_req.stream_src = RDI_INTF_0+i; + ss->stream_req.stream_src = (msm_vfe_axi_stream_src)(RDI_INTF_0+i); #ifdef HIGH_FPS if (rear) { @@ -1645,7 +1659,7 @@ static void rear_start(CameraState *s) { actuator_cfg_data.cfgtype = CFG_SET_POSITION; actuator_cfg_data.cfg.setpos = (struct msm_actuator_set_position_t){ .number_of_steps = 1, - .hw_params = (s->device != DEVICE_LP3) ? 0x0000e424 : 7, + .hw_params = (uint32_t)((s->device != DEVICE_LP3) ? 0x0000e424 : 7), .pos = {s->infinity_dac, 0}, .delay = {0,} }; @@ -1690,9 +1704,9 @@ void actuator_move(CameraState *s, uint16_t target) { struct msm_actuator_cfg_data actuator_cfg_data = {0}; actuator_cfg_data.cfgtype = CFG_MOVE_FOCUS; actuator_cfg_data.cfg.move = (struct msm_actuator_move_params_t){ - .dir = (step > 0) ? 0 : 1, - .sign_dir = (step > 0) ? 1 : -1, - .dest_step_pos = dest_step_pos, + .dir = (int8_t)((step > 0) ? 0 : 1), + .sign_dir = (int8_t)((step > 0) ? 1 : -1), + .dest_step_pos = (int16_t)dest_step_pos, .num_steps = abs(step), .curr_lens_pos = s->cur_lens_pos, .ringing_params = &actuator_ringing_params, @@ -1719,13 +1733,15 @@ static void parse_autofocus(CameraState *s, uint8_t *d) { for (int i = 0; i < NUM_FOCUS; i++) { int doff = i*5+5; s->confidence[i] = d[doff]; + // this should just be a 10-bit signed int instead of 11 + // TODO: write it in a nicer way int16_t focus_t = (d[doff+1] << 3) | (d[doff+2] >> 5); if (focus_t >= 1024) focus_t = -(2048-focus_t); s->focus[i] = focus_t; //printf("%x->%d ", d[doff], focus_t); if (s->confidence[i] > 0x20) { good_count++; - max_focus = max(max_focus, s->focus[i]); + max_focus = std::max(max_focus, s->focus[i]); avg_focus += s->focus[i]; } } @@ -1792,34 +1808,36 @@ static void front_start(CameraState *s) { void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front) { int err; - struct ispif_cfg_data ispif_cfg_data = {0}; + struct ispif_cfg_data ispif_cfg_data; + memset(&ispif_cfg_data, 0, sizeof(struct ispif_cfg_data)); - struct msm_ispif_param_data ispif_params = {0}; + struct msm_ispif_param_data ispif_params; + memset(&ispif_params, 0, sizeof(struct msm_ispif_param_data)); ispif_params.num = 4; // rear camera - ispif_params.entries[0].vfe_intf = 0; + ispif_params.entries[0].vfe_intf = VFE0; ispif_params.entries[0].intftype = RDI0; ispif_params.entries[0].num_cids = 1; - ispif_params.entries[0].cids[0] = 0; - ispif_params.entries[0].csid = 0; + ispif_params.entries[0].cids[0] = CID0; + ispif_params.entries[0].csid = CSID0; // front camera - ispif_params.entries[1].vfe_intf = 1; + ispif_params.entries[1].vfe_intf = VFE1; ispif_params.entries[1].intftype = RDI0; ispif_params.entries[1].num_cids = 1; - ispif_params.entries[1].cids[0] = 0; - ispif_params.entries[1].csid = 2; + ispif_params.entries[1].cids[0] = CID0; + ispif_params.entries[1].csid = CSID2; // rear camera (focus) - ispif_params.entries[2].vfe_intf = 0; + ispif_params.entries[2].vfe_intf = VFE0; ispif_params.entries[2].intftype = RDI1; - ispif_params.entries[2].num_cids = 1; - ispif_params.entries[2].cids[0] = 1; - ispif_params.entries[2].csid = 0; + ispif_params.entries[2].num_cids = CID1; + ispif_params.entries[2].cids[0] = CID1; + ispif_params.entries[2].csid = CSID0; // rear camera (stats, for AE) - ispif_params.entries[3].vfe_intf = 0; + ispif_params.entries[3].vfe_intf = VFE0; ispif_params.entries[3].intftype = RDI2; ispif_params.entries[3].num_cids = 1; - ispif_params.entries[3].cids[0] = 2; - ispif_params.entries[3].csid = 0; + ispif_params.entries[3].cids[0] = CID2; + ispif_params.entries[3].csid = CSID0; assert(camera_bufs_rear); assert(camera_bufs_front); @@ -1958,13 +1976,13 @@ static FrameMetadata get_frame_metadata(CameraState *s, uint32_t frame_id) { // should never happen return (FrameMetadata){ - .frame_id = -1, + .frame_id = (uint32_t)-1, }; } -static bool acceleration_from_sensor_sock(void* sock, float* vs) { +static bool acceleration_from_sensor_sock(void *sock, float *vs) { int err; - + bool ret = false; zmq_msg_t msg; err = zmq_msg_init(&msg); assert(err == 0); @@ -1972,43 +1990,30 @@ static bool acceleration_from_sensor_sock(void* sock, float* vs) { err = zmq_msg_recv(&msg, sock, 0); assert(err >= 0); - struct capn ctx; - capn_init_mem(&ctx, zmq_msg_data(&msg), zmq_msg_size(&msg), 0); + void *data = zmq_msg_data(&msg); + size_t size = zmq_msg_size(&msg); - cereal_Event_ptr eventp; - eventp.p = capn_getp(capn_root(&ctx), 0, 1); - struct cereal_Event eventd; - cereal_read_Event(&eventd, eventp); - - bool ret = false; - - if (eventd.which == cereal_Event_sensorEvents) { - cereal_SensorEventData_list lst = eventd.sensorEvents; - int len = capn_len(lst); - for (int i=0; i(size / sizeof(capnp::word) + 1); + memcpy(amsg.begin(), data, size); + capnp::FlatArrayMessageReader cmsg(amsg); + auto event = cmsg.getRoot(); + if (event.which() == cereal::Event::SENSOR_EVENTS) { + auto sensor_events = event.getSensorEvents(); + for (auto sensor_event : sensor_events) { + if (sensor_event.which() == cereal::SensorEventData::ACCELERATION) { + auto v = sensor_event.getAcceleration().getV(); + if (v.size() < 3) { + continue; //wtf } - for (int j=0; j<3; j++) { - vs[j] = capn_to_f32(capn_get32(vecd.v, j)); + for (int j = 0; j < 3; j++) { + vs[j] = v[j]; } ret = true; break; } } } - - capn_free(&ctx); zmq_msg_close(&msg); - return ret; } @@ -2163,7 +2168,7 @@ void cameras_run(DualCameraState *s) { c->camera_bufs_metadata[buf_idx] = get_frame_metadata(c, isp_event_data->frame_id); tbuffer_dispatch(&c->camera_tb, buf_idx); } else { - uint8_t *d = c->ss[buffer].bufs[buf_idx].addr; + uint8_t *d = (uint8_t*)(c->ss[buffer].bufs[buf_idx].addr); if (buffer == 1) { parse_autofocus(c, d); } @@ -2179,9 +2184,9 @@ void cameras_run(DualCameraState *s) { c->frame_metadata[c->frame_metadata_idx] = (FrameMetadata){ .frame_id = isp_event_data->frame_id, .timestamp_eof = timestamp, - .frame_length = c->cur_frame_length, - .integ_lines = c->cur_integ_lines, - .global_gain = c->cur_gain, + .frame_length = (unsigned int)c->cur_frame_length, + .integ_lines = (unsigned int)c->cur_integ_lines, + .global_gain = (unsigned int)c->cur_gain, .lens_pos = c->cur_lens_pos, .lens_sag = c->last_sag_acc_z, .lens_err = c->focus_err, diff --git a/selfdrive/camerad/cameras/debayer.cl b/selfdrive/camerad/cameras/debayer.cl index 344ead03..c9e3716d 100644 --- a/selfdrive/camerad/cameras/debayer.cl +++ b/selfdrive/camerad/cameras/debayer.cl @@ -81,17 +81,28 @@ __kernel void debayer10(__global uchar const * const in, float4 p = convert_float4(pint); +#if NEW == 1 + // now it's 0x2a + const float black_level = 42.0f; + // max on black level + p = max(0.0, p - black_level); +#else // 64 is the black level of the sensor, remove // (changed to 56 for HDR) const float black_level = 56.0f; + // TODO: switch to max here? p = (p - black_level); +#endif + +#if NEW == 0 // correct vignetting (no pow function?) // see https://www.eecis.udel.edu/~jye/lab_research/09/JiUp.pdf the A (4th order) const float r = ((oy - RGB_HEIGHT/2)*(oy - RGB_HEIGHT/2) + (ox - RGB_WIDTH/2)*(ox - RGB_WIDTH/2)); const float fake_f = 700.0f; // should be 910, but this fits... const float lil_a = (1.0f + r/(fake_f*fake_f)); p = p * lil_a * lil_a; +#endif // rescale to 1.0 #if HDR @@ -114,8 +125,12 @@ __kernel void debayer10(__global uchar const * const in, float3 c1 = (float3)(p.s0, (p.s1+p.s2)/2.0f, p.s3); #endif +#if NEW + // TODO: new color correction +#else // color correction c1 = color_correct(c1); +#endif #if HDR // srgb gamma isn't right for YUV, so it's disabled for now diff --git a/selfdrive/camerad/imgproc/conv.cl b/selfdrive/camerad/imgproc/conv.cl new file mode 100644 index 00000000..f92d3567 --- /dev/null +++ b/selfdrive/camerad/imgproc/conv.cl @@ -0,0 +1,110 @@ +// const __constant float3 rgb_weights = (0.299, 0.587, 0.114); // opencv rgb2gray weights +// const __constant float3 bgr_weights = (0.114, 0.587, 0.299); // bgr2gray weights + +// convert input rgb image to single channel then conv +__kernel void rgb2gray_conv2d( + const __global uchar * input, + __global short * output, + __constant short * filter, + __local uchar3 * cached +) +{ + const int rowOffset = get_global_id(1) * IMAGE_W; + const int my = get_global_id(0) + rowOffset; + + const int localRowLen = TWICE_HALF_FILTER_SIZE + get_local_size(0); + const int localRowOffset = ( get_local_id(1) + HALF_FILTER_SIZE ) * localRowLen; + const int myLocal = localRowOffset + get_local_id(0) + HALF_FILTER_SIZE; + + // cache local pixels + cached[ myLocal ].x = input[ my * 3 ]; // r + cached[ myLocal ].y = input[ my * 3 + 1]; // g + cached[ myLocal ].z = input[ my * 3 + 2]; // b + + // pad + if ( + get_global_id(0) < HALF_FILTER_SIZE || + get_global_id(0) > IMAGE_W - HALF_FILTER_SIZE - 1 || + get_global_id(1) < HALF_FILTER_SIZE || + get_global_id(1) > IMAGE_H - HALF_FILTER_SIZE - 1 + ) + { + barrier(CLK_LOCAL_MEM_FENCE); + return; + } + else + { + int localColOffset = -1; + int globalColOffset = -1; + + // cache extra + if ( get_local_id(0) < HALF_FILTER_SIZE ) + { + localColOffset = get_local_id(0); + globalColOffset = -HALF_FILTER_SIZE; + + cached[ localRowOffset + get_local_id(0) ].x = input[ my * 3 - HALF_FILTER_SIZE * 3 ]; + cached[ localRowOffset + get_local_id(0) ].y = input[ my * 3 - HALF_FILTER_SIZE * 3 + 1]; + cached[ localRowOffset + get_local_id(0) ].z = input[ my * 3 - HALF_FILTER_SIZE * 3 + 2]; + } + else if ( get_local_id(0) >= get_local_size(0) - HALF_FILTER_SIZE ) + { + localColOffset = get_local_id(0) + TWICE_HALF_FILTER_SIZE; + globalColOffset = HALF_FILTER_SIZE; + + cached[ myLocal + HALF_FILTER_SIZE ].x = input[ my * 3 + HALF_FILTER_SIZE * 3 ]; + cached[ myLocal + HALF_FILTER_SIZE ].y = input[ my * 3 + HALF_FILTER_SIZE * 3 + 1]; + cached[ myLocal + HALF_FILTER_SIZE ].z = input[ my * 3 + HALF_FILTER_SIZE * 3 + 2]; + } + + + if ( get_local_id(1) < HALF_FILTER_SIZE ) + { + cached[ get_local_id(1) * localRowLen + get_local_id(0) + HALF_FILTER_SIZE ].x = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 ]; + cached[ get_local_id(1) * localRowLen + get_local_id(0) + HALF_FILTER_SIZE ].y = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + 1]; + cached[ get_local_id(1) * localRowLen + get_local_id(0) + HALF_FILTER_SIZE ].z = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + 2]; + if (localColOffset > 0) + { + cached[ get_local_id(1) * localRowLen + localColOffset ].x = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3]; + cached[ get_local_id(1) * localRowLen + localColOffset ].y = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 1]; + cached[ get_local_id(1) * localRowLen + localColOffset ].z = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 2]; + } + } + else if ( get_local_id(1) >= get_local_size(1) -HALF_FILTER_SIZE ) + { + int offset = ( get_local_id(1) + TWICE_HALF_FILTER_SIZE ) * localRowLen; + cached[ offset + get_local_id(0) + HALF_FILTER_SIZE ].x = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 ]; + cached[ offset + get_local_id(0) + HALF_FILTER_SIZE ].y = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + 1]; + cached[ offset + get_local_id(0) + HALF_FILTER_SIZE ].z = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + 2]; + if (localColOffset > 0) + { + cached[ offset + localColOffset ].x = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3]; + cached[ offset + localColOffset ].y = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 1]; + cached[ offset + localColOffset ].z = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 2]; + } + } + + // sync + barrier(CLK_LOCAL_MEM_FENCE); + + // perform convolution + int fIndex = 0; + short sum = 0; + + for (int r = -HALF_FILTER_SIZE; r <= HALF_FILTER_SIZE; r++) + { + int curRow = r * localRowLen; + for (int c = -HALF_FILTER_SIZE; c <= HALF_FILTER_SIZE; c++, fIndex++) + { + if (!FLIP_RB){ + // sum += dot(rgb_weights, cached[ myLocal + curRow + c ]) * filter[ fIndex ]; + sum += (cached[ myLocal + curRow + c ].x / 3 + cached[ myLocal + curRow + c ].y / 2 + cached[ myLocal + curRow + c ].z / 9) * filter[ fIndex ]; + } else { + // sum += dot(bgr_weights, cached[ myLocal + curRow + c ]) * filter[ fIndex ]; + sum += (cached[ myLocal + curRow + c ].x / 9 + cached[ myLocal + curRow + c ].y / 2 + cached[ myLocal + curRow + c ].z / 3) * filter[ fIndex ]; + } + } + } + output[my] = sum; + } +} \ No newline at end of file diff --git a/selfdrive/camerad/imgproc/pool.cl b/selfdrive/camerad/imgproc/pool.cl new file mode 100644 index 00000000..3ba86ae2 --- /dev/null +++ b/selfdrive/camerad/imgproc/pool.cl @@ -0,0 +1,34 @@ +// calculate variance in each subregion +__kernel void var_pool( + const __global char * input, + __global ushort * output // should not be larger than 128*128 so uint16 +) +{ + const int xidx = get_global_id(0) + ROI_X_MIN; + const int yidx = get_global_id(1) + ROI_Y_MIN; + + const int size = X_PITCH * Y_PITCH; + + float fsum = 0; + char mean, max; + + for (int i = 0; i < size; i++) { + int x_offset = i % X_PITCH; + int y_offset = i / X_PITCH; + fsum += input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X]; + max = input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X]>max ? input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X]:max; + } + + mean = convert_char_rte(fsum / size); + + float fvar = 0; + for (int i = 0; i < size; i++) { + int x_offset = i % X_PITCH; + int y_offset = i / X_PITCH; + fvar += (input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X] - mean) * (input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X] - mean); + } + + fvar = fvar / size; + + output[(xidx-ROI_X_MIN)+(yidx-ROI_Y_MIN)*(ROI_X_MAX-ROI_X_MIN+1)] = convert_ushort_rte(5 * fvar + convert_float_rte(max)); +} \ No newline at end of file diff --git a/selfdrive/camerad/imgproc/utils.cc b/selfdrive/camerad/imgproc/utils.cc new file mode 100644 index 00000000..0b36d686 --- /dev/null +++ b/selfdrive/camerad/imgproc/utils.cc @@ -0,0 +1,44 @@ +#include "utils.h" +#include +#include + +// calculate score based on laplacians in one area +void get_lapmap_one(int16_t *lap, uint16_t *res, int x_pitch, int y_pitch) { + int size = x_pitch * y_pitch; + // avg and max of roi + float fsum = 0; + int16_t mean, max; + max = 0; + + for (int i = 0; i < size; i++) { + int x_offset = i % x_pitch; + int y_offset = i / x_pitch; + fsum += lap[x_offset + y_offset*x_pitch]; + max = std::max(lap[x_offset + y_offset*x_pitch], max); + } + + mean = fsum / size; + + // var of roi + float fvar = 0; + for (int i = 0; i < size; i++) { + int x_offset = i % x_pitch; + int y_offset = i / x_pitch; + fvar += (float)((lap[x_offset + y_offset*x_pitch] - mean) * (lap[x_offset + y_offset*x_pitch] - mean)); + } + + fvar = fvar / size; + + *res = std::min(5 * fvar + max, (float)65535); +} + +bool is_blur(uint16_t *lapmap) { + int n_roi = (ROI_X_MAX - ROI_X_MIN + 1) * (ROI_Y_MAX - ROI_Y_MIN + 1); + float bad_sum = 0; + for (int i = 0; i < n_roi; i++) { + if (*(lapmap + i) < LM_THRESH) { + bad_sum += 1/(float)n_roi; + } + } + return (bad_sum > LM_PREC_THRESH); +} \ No newline at end of file diff --git a/selfdrive/camerad/imgproc/utils.h b/selfdrive/camerad/imgproc/utils.h new file mode 100644 index 00000000..f77a4569 --- /dev/null +++ b/selfdrive/camerad/imgproc/utils.h @@ -0,0 +1,30 @@ +#ifndef IMGPROC_UTILS +#define IMGPROC_UTILS + +#include + +#define NUM_SEGMENTS_X 8 +#define NUM_SEGMENTS_Y 6 + +#define ROI_X_MIN 1 +#define ROI_X_MAX 6 +#define ROI_Y_MIN 2 +#define ROI_Y_MAX 3 + +#define LM_THRESH 222 +#define LM_PREC_THRESH 0.9 + +// only apply to QCOM +#define FULL_STRIDE_X 1280 +#define FULL_STRIDE_Y 896 + +#define CONV_LOCAL_WORKSIZE 16 + +const int16_t lapl_conv_krnl[9] = {0, 1, 0, + 1, -4, 1, + 0, 1, 0}; + +void get_lapmap_one(int16_t *lap, uint16_t *res, int x_pitch, int y_pitch); +bool is_blur(uint16_t *lapmap); + +#endif \ No newline at end of file diff --git a/selfdrive/camerad/include/media/cam_cpas.h b/selfdrive/camerad/include/media/cam_cpas.h new file mode 100644 index 00000000..c5cbac82 --- /dev/null +++ b/selfdrive/camerad/include/media/cam_cpas.h @@ -0,0 +1,25 @@ +#ifndef __UAPI_CAM_CPAS_H__ +#define __UAPI_CAM_CPAS_H__ + +#include "cam_defs.h" + +#define CAM_FAMILY_CAMERA_SS 1 +#define CAM_FAMILY_CPAS_SS 2 + +/** + * struct cam_cpas_query_cap - CPAS query device capability payload + * + * @camera_family : Camera family type + * @reserved : Reserved field for alignment + * @camera_version : Camera platform version + * @cpas_version : Camera CPAS version within camera platform + * + */ +struct cam_cpas_query_cap { + uint32_t camera_family; + uint32_t reserved; + struct cam_hw_version camera_version; + struct cam_hw_version cpas_version; +}; + +#endif /* __UAPI_CAM_CPAS_H__ */ diff --git a/selfdrive/camerad/include/media/cam_defs.h b/selfdrive/camerad/include/media/cam_defs.h new file mode 100644 index 00000000..e006463d --- /dev/null +++ b/selfdrive/camerad/include/media/cam_defs.h @@ -0,0 +1,477 @@ +#ifndef __UAPI_CAM_DEFS_H__ +#define __UAPI_CAM_DEFS_H__ + +#include +#include +#include + + +/* camera op codes */ +#define CAM_COMMON_OPCODE_BASE 0x100 +#define CAM_QUERY_CAP (CAM_COMMON_OPCODE_BASE + 0x1) +#define CAM_ACQUIRE_DEV (CAM_COMMON_OPCODE_BASE + 0x2) +#define CAM_START_DEV (CAM_COMMON_OPCODE_BASE + 0x3) +#define CAM_STOP_DEV (CAM_COMMON_OPCODE_BASE + 0x4) +#define CAM_CONFIG_DEV (CAM_COMMON_OPCODE_BASE + 0x5) +#define CAM_RELEASE_DEV (CAM_COMMON_OPCODE_BASE + 0x6) +#define CAM_SD_SHUTDOWN (CAM_COMMON_OPCODE_BASE + 0x7) +#define CAM_FLUSH_REQ (CAM_COMMON_OPCODE_BASE + 0x8) +#define CAM_COMMON_OPCODE_MAX (CAM_COMMON_OPCODE_BASE + 0x9) + +#define CAM_EXT_OPCODE_BASE 0x200 +#define CAM_CONFIG_DEV_EXTERNAL (CAM_EXT_OPCODE_BASE + 0x1) + +/* camera handle type */ +#define CAM_HANDLE_USER_POINTER 1 +#define CAM_HANDLE_MEM_HANDLE 2 + +/* Generic Blob CmdBuffer header properties */ +#define CAM_GENERIC_BLOB_CMDBUFFER_SIZE_MASK 0xFFFFFF00 +#define CAM_GENERIC_BLOB_CMDBUFFER_SIZE_SHIFT 8 +#define CAM_GENERIC_BLOB_CMDBUFFER_TYPE_MASK 0xFF +#define CAM_GENERIC_BLOB_CMDBUFFER_TYPE_SHIFT 0 + +/* Command Buffer Types */ +#define CAM_CMD_BUF_DMI 0x1 +#define CAM_CMD_BUF_DMI16 0x2 +#define CAM_CMD_BUF_DMI32 0x3 +#define CAM_CMD_BUF_DMI64 0x4 +#define CAM_CMD_BUF_DIRECT 0x5 +#define CAM_CMD_BUF_INDIRECT 0x6 +#define CAM_CMD_BUF_I2C 0x7 +#define CAM_CMD_BUF_FW 0x8 +#define CAM_CMD_BUF_GENERIC 0x9 +#define CAM_CMD_BUF_LEGACY 0xA + +/** + * enum flush_type_t - Identifies the various flush types + * + * @CAM_FLUSH_TYPE_REQ: Flush specific request + * @CAM_FLUSH_TYPE_ALL: Flush all requests belonging to a context + * @CAM_FLUSH_TYPE_MAX: Max enum to validate flush type + * + */ +enum flush_type_t { + CAM_FLUSH_TYPE_REQ, + CAM_FLUSH_TYPE_ALL, + CAM_FLUSH_TYPE_MAX +}; + +/** + * struct cam_control - Structure used by ioctl control for camera + * + * @op_code: This is the op code for camera control + * @size: Control command size + * @handle_type: User pointer or shared memory handle + * @reserved: Reserved field for 64 bit alignment + * @handle: Control command payload + */ +struct cam_control { + uint32_t op_code; + uint32_t size; + uint32_t handle_type; + uint32_t reserved; + uint64_t handle; +}; + +/* camera IOCTL */ +#define VIDIOC_CAM_CONTROL \ + _IOWR('V', BASE_VIDIOC_PRIVATE, struct cam_control) + +/** + * struct cam_hw_version - Structure for HW version of camera devices + * + * @major : Hardware version major + * @minor : Hardware version minor + * @incr : Hardware version increment + * @reserved : Reserved for 64 bit aligngment + */ +struct cam_hw_version { + uint32_t major; + uint32_t minor; + uint32_t incr; + uint32_t reserved; +}; + +/** + * struct cam_iommu_handle - Structure for IOMMU handles of camera hw devices + * + * @non_secure: Device Non Secure IOMMU handle + * @secure: Device Secure IOMMU handle + * + */ +struct cam_iommu_handle { + int32_t non_secure; + int32_t secure; +}; + +/* camera secure mode */ +#define CAM_SECURE_MODE_NON_SECURE 0 +#define CAM_SECURE_MODE_SECURE 1 + +/* Camera Format Type */ +#define CAM_FORMAT_BASE 0 +#define CAM_FORMAT_MIPI_RAW_6 1 +#define CAM_FORMAT_MIPI_RAW_8 2 +#define CAM_FORMAT_MIPI_RAW_10 3 +#define CAM_FORMAT_MIPI_RAW_12 4 +#define CAM_FORMAT_MIPI_RAW_14 5 +#define CAM_FORMAT_MIPI_RAW_16 6 +#define CAM_FORMAT_MIPI_RAW_20 7 +#define CAM_FORMAT_QTI_RAW_8 8 +#define CAM_FORMAT_QTI_RAW_10 9 +#define CAM_FORMAT_QTI_RAW_12 10 +#define CAM_FORMAT_QTI_RAW_14 11 +#define CAM_FORMAT_PLAIN8 12 +#define CAM_FORMAT_PLAIN16_8 13 +#define CAM_FORMAT_PLAIN16_10 14 +#define CAM_FORMAT_PLAIN16_12 15 +#define CAM_FORMAT_PLAIN16_14 16 +#define CAM_FORMAT_PLAIN16_16 17 +#define CAM_FORMAT_PLAIN32_20 18 +#define CAM_FORMAT_PLAIN64 19 +#define CAM_FORMAT_PLAIN128 20 +#define CAM_FORMAT_ARGB 21 +#define CAM_FORMAT_ARGB_10 22 +#define CAM_FORMAT_ARGB_12 23 +#define CAM_FORMAT_ARGB_14 24 +#define CAM_FORMAT_DPCM_10_6_10 25 +#define CAM_FORMAT_DPCM_10_8_10 26 +#define CAM_FORMAT_DPCM_12_6_12 27 +#define CAM_FORMAT_DPCM_12_8_12 28 +#define CAM_FORMAT_DPCM_14_8_14 29 +#define CAM_FORMAT_DPCM_14_10_14 30 +#define CAM_FORMAT_NV21 31 +#define CAM_FORMAT_NV12 32 +#define CAM_FORMAT_TP10 33 +#define CAM_FORMAT_YUV422 34 +#define CAM_FORMAT_PD8 35 +#define CAM_FORMAT_PD10 36 +#define CAM_FORMAT_UBWC_NV12 37 +#define CAM_FORMAT_UBWC_NV12_4R 38 +#define CAM_FORMAT_UBWC_TP10 39 +#define CAM_FORMAT_UBWC_P010 40 +#define CAM_FORMAT_PLAIN8_SWAP 41 +#define CAM_FORMAT_PLAIN8_10 42 +#define CAM_FORMAT_PLAIN8_10_SWAP 43 +#define CAM_FORMAT_YV12 44 +#define CAM_FORMAT_Y_ONLY 45 +#define CAM_FORMAT_MAX 46 + +/* camera rotaion */ +#define CAM_ROTATE_CW_0_DEGREE 0 +#define CAM_ROTATE_CW_90_DEGREE 1 +#define CAM_RORATE_CW_180_DEGREE 2 +#define CAM_ROTATE_CW_270_DEGREE 3 + +/* camera Color Space */ +#define CAM_COLOR_SPACE_BASE 0 +#define CAM_COLOR_SPACE_BT601_FULL 1 +#define CAM_COLOR_SPACE_BT601625 2 +#define CAM_COLOR_SPACE_BT601525 3 +#define CAM_COLOR_SPACE_BT709 4 +#define CAM_COLOR_SPACE_DEPTH 5 +#define CAM_COLOR_SPACE_MAX 6 + +/* camera buffer direction */ +#define CAM_BUF_INPUT 1 +#define CAM_BUF_OUTPUT 2 +#define CAM_BUF_IN_OUT 3 + +/* camera packet device Type */ +#define CAM_PACKET_DEV_BASE 0 +#define CAM_PACKET_DEV_IMG_SENSOR 1 +#define CAM_PACKET_DEV_ACTUATOR 2 +#define CAM_PACKET_DEV_COMPANION 3 +#define CAM_PACKET_DEV_EEPOM 4 +#define CAM_PACKET_DEV_CSIPHY 5 +#define CAM_PACKET_DEV_OIS 6 +#define CAM_PACKET_DEV_FLASH 7 +#define CAM_PACKET_DEV_FD 8 +#define CAM_PACKET_DEV_JPEG_ENC 9 +#define CAM_PACKET_DEV_JPEG_DEC 10 +#define CAM_PACKET_DEV_VFE 11 +#define CAM_PACKET_DEV_CPP 12 +#define CAM_PACKET_DEV_CSID 13 +#define CAM_PACKET_DEV_ISPIF 14 +#define CAM_PACKET_DEV_IFE 15 +#define CAM_PACKET_DEV_ICP 16 +#define CAM_PACKET_DEV_LRME 17 +#define CAM_PACKET_DEV_MAX 18 + + +/* constants */ +#define CAM_PACKET_MAX_PLANES 3 + +/** + * struct cam_plane_cfg - Plane configuration info + * + * @width: Plane width in pixels + * @height: Plane height in lines + * @plane_stride: Plane stride in pixel + * @slice_height: Slice height in line (not used by ISP) + * @meta_stride: UBWC metadata stride + * @meta_size: UBWC metadata plane size + * @meta_offset: UBWC metadata offset + * @packer_config: UBWC packer config + * @mode_config: UBWC mode config + * @tile_config: UBWC tile config + * @h_init: UBWC horizontal initial coordinate in pixels + * @v_init: UBWC vertical initial coordinate in lines + * + */ +struct cam_plane_cfg { + uint32_t width; + uint32_t height; + uint32_t plane_stride; + uint32_t slice_height; + uint32_t meta_stride; + uint32_t meta_size; + uint32_t meta_offset; + uint32_t packer_config; + uint32_t mode_config; + uint32_t tile_config; + uint32_t h_init; + uint32_t v_init; +}; + +/** + * struct cam_cmd_buf_desc - Command buffer descriptor + * + * @mem_handle: Command buffer handle + * @offset: Command start offset + * @size: Size of the command buffer in bytes + * @length: Used memory in command buffer in bytes + * @type: Type of the command buffer + * @meta_data: Data type for private command buffer + * Between UMD and KMD + * + */ +struct cam_cmd_buf_desc { + int32_t mem_handle; + uint32_t offset; + uint32_t size; + uint32_t length; + uint32_t type; + uint32_t meta_data; +}; + +/** + * struct cam_buf_io_cfg - Buffer io configuration for buffers + * + * @mem_handle: Mem_handle array for the buffers. + * @offsets: Offsets for each planes in the buffer + * @planes: Per plane information + * @width: Main plane width in pixel + * @height: Main plane height in lines + * @format: Format of the buffer + * @color_space: Color space for the buffer + * @color_pattern: Color pattern in the buffer + * @bpp: Bit per pixel + * @rotation: Rotation information for the buffer + * @resource_type: Resource type associated with the buffer + * @fence: Fence handle + * @early_fence: Fence handle for early signal + * @aux_cmd_buf: An auxiliary command buffer that may be + * used for programming the IO + * @direction: Direction of the config + * @batch_size: Batch size in HFR mode + * @subsample_pattern: Subsample pattern. Used in HFR mode. It + * should be consistent with batchSize and + * CAMIF programming. + * @subsample_period: Subsample period. Used in HFR mode. It + * should be consistent with batchSize and + * CAMIF programming. + * @framedrop_pattern: Framedrop pattern + * @framedrop_period: Framedrop period + * @flag: Flags for extra information + * @direction: Buffer direction: input or output + * @padding: Padding for the structure + * + */ +struct cam_buf_io_cfg { + int32_t mem_handle[CAM_PACKET_MAX_PLANES]; + uint32_t offsets[CAM_PACKET_MAX_PLANES]; + struct cam_plane_cfg planes[CAM_PACKET_MAX_PLANES]; + uint32_t format; + uint32_t color_space; + uint32_t color_pattern; + uint32_t bpp; + uint32_t rotation; + uint32_t resource_type; + int32_t fence; + int32_t early_fence; + struct cam_cmd_buf_desc aux_cmd_buf; + uint32_t direction; + uint32_t batch_size; + uint32_t subsample_pattern; + uint32_t subsample_period; + uint32_t framedrop_pattern; + uint32_t framedrop_period; + uint32_t flag; + uint32_t padding; +}; + +/** + * struct cam_packet_header - Camera packet header + * + * @op_code: Camera packet opcode + * @size: Size of the camera packet in bytes + * @request_id: Request id for this camera packet + * @flags: Flags for the camera packet + * @padding: Padding + * + */ +struct cam_packet_header { + uint32_t op_code; + uint32_t size; + uint64_t request_id; + uint32_t flags; + uint32_t padding; +}; + +/** + * struct cam_patch_desc - Patch structure + * + * @dst_buf_hdl: Memory handle for the dest buffer + * @dst_offset: Offset byte in the dest buffer + * @src_buf_hdl: Memory handle for the source buffer + * @src_offset: Offset byte in the source buffer + * + */ +struct cam_patch_desc { + int32_t dst_buf_hdl; + uint32_t dst_offset; + int32_t src_buf_hdl; + uint32_t src_offset; +}; + +/** + * struct cam_packet - Camera packet structure + * + * @header: Camera packet header + * @cmd_buf_offset: Command buffer start offset + * @num_cmd_buf: Number of the command buffer in the packet + * @io_config_offset: Buffer io configuration start offset + * @num_io_configs: Number of the buffer io configurations + * @patch_offset: Patch offset for the patch structure + * @num_patches: Number of the patch structure + * @kmd_cmd_buf_index: Command buffer index which contains extra + * space for the KMD buffer + * @kmd_cmd_buf_offset: Offset from the beginning of the command + * buffer for KMD usage. + * @payload: Camera packet payload + * + */ +struct cam_packet { + struct cam_packet_header header; + uint32_t cmd_buf_offset; + uint32_t num_cmd_buf; + uint32_t io_configs_offset; + uint32_t num_io_configs; + uint32_t patch_offset; + uint32_t num_patches; + uint32_t kmd_cmd_buf_index; + uint32_t kmd_cmd_buf_offset; + uint64_t payload[1]; + +}; + +/** + * struct cam_release_dev_cmd - Control payload for release devices + * + * @session_handle: Session handle for the release + * @dev_handle: Device handle for the release + */ +struct cam_release_dev_cmd { + int32_t session_handle; + int32_t dev_handle; +}; + +/** + * struct cam_start_stop_dev_cmd - Control payload for start/stop device + * + * @session_handle: Session handle for the start/stop command + * @dev_handle: Device handle for the start/stop command + * + */ +struct cam_start_stop_dev_cmd { + int32_t session_handle; + int32_t dev_handle; +}; + +/** + * struct cam_config_dev_cmd - Command payload for configure device + * + * @session_handle: Session handle for the command + * @dev_handle: Device handle for the command + * @offset: Offset byte in the packet handle. + * @packet_handle: Packet memory handle for the actual packet: + * struct cam_packet. + * + */ +struct cam_config_dev_cmd { + int32_t session_handle; + int32_t dev_handle; + uint64_t offset; + uint64_t packet_handle; +}; + +/** + * struct cam_query_cap_cmd - Payload for query device capability + * + * @size: Handle size + * @handle_type: User pointer or shared memory handle + * @caps_handle: Device specific query command payload + * + */ +struct cam_query_cap_cmd { + uint32_t size; + uint32_t handle_type; + uint64_t caps_handle; +}; + +/** + * struct cam_acquire_dev_cmd - Control payload for acquire devices + * + * @session_handle: Session handle for the acquire command + * @dev_handle: Device handle to be returned + * @handle_type: Resource handle type: + * 1 = user pointer, 2 = mem handle + * @num_resources: Number of the resources to be acquired + * @resources_hdl: Resource handle that refers to the actual + * resource array. Each item in this + * array is device specific resource structure + * + */ +struct cam_acquire_dev_cmd { + int32_t session_handle; + int32_t dev_handle; + uint32_t handle_type; + uint32_t num_resources; + uint64_t resource_hdl; +}; + +/** + * struct cam_flush_dev_cmd - Control payload for flush devices + * + * @version: Version + * @session_handle: Session handle for the acquire command + * @dev_handle: Device handle to be returned + * @flush_type: Flush type: + * 0 = flush specific request + * 1 = flush all + * @reserved: Reserved for 64 bit aligngment + * @req_id: Request id that needs to cancel + * + */ +struct cam_flush_dev_cmd { + uint64_t version; + int32_t session_handle; + int32_t dev_handle; + uint32_t flush_type; + uint32_t reserved; + int64_t req_id; +}; + +#endif /* __UAPI_CAM_DEFS_H__ */ diff --git a/selfdrive/camerad/include/media/cam_fd.h b/selfdrive/camerad/include/media/cam_fd.h new file mode 100644 index 00000000..8feb6e4d --- /dev/null +++ b/selfdrive/camerad/include/media/cam_fd.h @@ -0,0 +1,127 @@ +#ifndef __UAPI_CAM_FD_H__ +#define __UAPI_CAM_FD_H__ + +#include "cam_defs.h" + +#define CAM_FD_MAX_FACES 35 +#define CAM_FD_RAW_RESULT_ENTRIES 512 + +/* FD Op Codes */ +#define CAM_PACKET_OPCODES_FD_FRAME_UPDATE 0x0 + +/* FD Command Buffer identifiers */ +#define CAM_FD_CMD_BUFFER_ID_GENERIC 0x0 +#define CAM_FD_CMD_BUFFER_ID_CDM 0x1 +#define CAM_FD_CMD_BUFFER_ID_MAX 0x2 + +/* FD Blob types */ +#define CAM_FD_BLOB_TYPE_SOC_CLOCK_BW_REQUEST 0x0 +#define CAM_FD_BLOB_TYPE_RAW_RESULTS_REQUIRED 0x1 + +/* FD Resource IDs */ +#define CAM_FD_INPUT_PORT_ID_IMAGE 0x0 +#define CAM_FD_INPUT_PORT_ID_MAX 0x1 + +#define CAM_FD_OUTPUT_PORT_ID_RESULTS 0x0 +#define CAM_FD_OUTPUT_PORT_ID_RAW_RESULTS 0x1 +#define CAM_FD_OUTPUT_PORT_ID_WORK_BUFFER 0x2 +#define CAM_FD_OUTPUT_PORT_ID_MAX 0x3 + +/** + * struct cam_fd_soc_clock_bw_request - SOC clock, bandwidth request info + * + * @clock_rate : Clock rate required while processing frame + * @bandwidth : Bandwidth required while processing frame + * @reserved : Reserved for future use + */ +struct cam_fd_soc_clock_bw_request { + uint64_t clock_rate; + uint64_t bandwidth; + uint64_t reserved[4]; +}; + +/** + * struct cam_fd_face - Face properties + * + * @prop1 : Property 1 of face + * @prop2 : Property 2 of face + * @prop3 : Property 3 of face + * @prop4 : Property 4 of face + * + * Do not change this layout, this is inline with how HW writes + * these values directly when the buffer is programmed to HW + */ +struct cam_fd_face { + uint32_t prop1; + uint32_t prop2; + uint32_t prop3; + uint32_t prop4; +}; + +/** + * struct cam_fd_results - FD results layout + * + * @faces : Array of faces with face properties + * @face_count : Number of faces detected + * @reserved : Reserved for alignment + * + * Do not change this layout, this is inline with how HW writes + * these values directly when the buffer is programmed to HW + */ +struct cam_fd_results { + struct cam_fd_face faces[CAM_FD_MAX_FACES]; + uint32_t face_count; + uint32_t reserved[3]; +}; + +/** + * struct cam_fd_hw_caps - Face properties + * + * @core_version : FD core version + * @wrapper_version : FD wrapper version + * @raw_results_available : Whether raw results are available on this HW + * @supported_modes : Modes supported by this HW. + * @reserved : Reserved for future use + */ +struct cam_fd_hw_caps { + struct cam_hw_version core_version; + struct cam_hw_version wrapper_version; + uint32_t raw_results_available; + uint32_t supported_modes; + uint64_t reserved; +}; + +/** + * struct cam_fd_query_cap_cmd - FD Query capabilities information + * + * @device_iommu : FD IOMMU handles + * @cdm_iommu : CDM iommu handles + * @hw_caps : FD HW capabilities + * @reserved : Reserved for alignment + */ +struct cam_fd_query_cap_cmd { + struct cam_iommu_handle device_iommu; + struct cam_iommu_handle cdm_iommu; + struct cam_fd_hw_caps hw_caps; + uint64_t reserved; +}; + +/** + * struct cam_fd_acquire_dev_info - FD acquire device information + * + * @clk_bw_request : SOC clock, bandwidth request + * @priority : Priority for this acquire + * @mode : Mode in which to run FD HW. + * @get_raw_results : Whether this acquire needs face raw results + * while frame processing + * @reserved : Reserved field for 64 bit alignment + */ +struct cam_fd_acquire_dev_info { + struct cam_fd_soc_clock_bw_request clk_bw_request; + uint32_t priority; + uint32_t mode; + uint32_t get_raw_results; + uint32_t reserved[13]; +}; + +#endif /* __UAPI_CAM_FD_H__ */ diff --git a/selfdrive/camerad/include/media/cam_icp.h b/selfdrive/camerad/include/media/cam_icp.h new file mode 100644 index 00000000..680d05b6 --- /dev/null +++ b/selfdrive/camerad/include/media/cam_icp.h @@ -0,0 +1,179 @@ +#ifndef __UAPI_CAM_ICP_H__ +#define __UAPI_CAM_ICP_H__ + +#include "cam_defs.h" + +/* icp, ipe, bps, cdm(ipe/bps) are used in querycap */ +#define CAM_ICP_DEV_TYPE_A5 1 +#define CAM_ICP_DEV_TYPE_IPE 2 +#define CAM_ICP_DEV_TYPE_BPS 3 +#define CAM_ICP_DEV_TYPE_IPE_CDM 4 +#define CAM_ICP_DEV_TYPE_BPS_CDM 5 +#define CAM_ICP_DEV_TYPE_MAX 5 + +/* definitions needed for icp aquire device */ +#define CAM_ICP_RES_TYPE_BPS 1 +#define CAM_ICP_RES_TYPE_IPE_RT 2 +#define CAM_ICP_RES_TYPE_IPE 3 +#define CAM_ICP_RES_TYPE_MAX 4 + +/* packet opcode types */ +#define CAM_ICP_OPCODE_IPE_UPDATE 0 +#define CAM_ICP_OPCODE_BPS_UPDATE 1 + +/* IPE input port resource type */ +#define CAM_ICP_IPE_INPUT_IMAGE_FULL 0x0 +#define CAM_ICP_IPE_INPUT_IMAGE_DS4 0x1 +#define CAM_ICP_IPE_INPUT_IMAGE_DS16 0x2 +#define CAM_ICP_IPE_INPUT_IMAGE_DS64 0x3 +#define CAM_ICP_IPE_INPUT_IMAGE_FULL_REF 0x4 +#define CAM_ICP_IPE_INPUT_IMAGE_DS4_REF 0x5 +#define CAM_ICP_IPE_INPUT_IMAGE_DS16_REF 0x6 +#define CAM_ICP_IPE_INPUT_IMAGE_DS64_REF 0x7 + +/* IPE output port resource type */ +#define CAM_ICP_IPE_OUTPUT_IMAGE_DISPLAY 0x8 +#define CAM_ICP_IPE_OUTPUT_IMAGE_VIDEO 0x9 +#define CAM_ICP_IPE_OUTPUT_IMAGE_FULL_REF 0xA +#define CAM_ICP_IPE_OUTPUT_IMAGE_DS4_REF 0xB +#define CAM_ICP_IPE_OUTPUT_IMAGE_DS16_REF 0xC +#define CAM_ICP_IPE_OUTPUT_IMAGE_DS64_REF 0xD + +#define CAM_ICP_IPE_IMAGE_MAX 0xE + +/* BPS input port resource type */ +#define CAM_ICP_BPS_INPUT_IMAGE 0x0 + +/* BPS output port resource type */ +#define CAM_ICP_BPS_OUTPUT_IMAGE_FULL 0x1 +#define CAM_ICP_BPS_OUTPUT_IMAGE_DS4 0x2 +#define CAM_ICP_BPS_OUTPUT_IMAGE_DS16 0x3 +#define CAM_ICP_BPS_OUTPUT_IMAGE_DS64 0x4 +#define CAM_ICP_BPS_OUTPUT_IMAGE_STATS_BG 0x5 +#define CAM_ICP_BPS_OUTPUT_IMAGE_STATS_BHIST 0x6 +#define CAM_ICP_BPS_OUTPUT_IMAGE_REG1 0x7 +#define CAM_ICP_BPS_OUTPUT_IMAGE_REG2 0x8 + +#define CAM_ICP_BPS_IO_IMAGES_MAX 0x9 + +/* Command meta types */ +#define CAM_ICP_CMD_META_GENERIC_BLOB 0x1 + +/* Generic blob types */ +#define CAM_ICP_CMD_GENERIC_BLOB_CLK 0x1 +#define CAM_ICP_CMD_GENERIC_BLOB_CFG_IO 0x2 + +/** + * struct cam_icp_clk_bw_request + * + * @budget_ns: Time required to process frame + * @frame_cycles: Frame cycles needed to process the frame + * @rt_flag: Flag to indicate real time stream + * @uncompressed_bw: Bandwidth required to process frame + * @compressed_bw: Compressed bandwidth to process frame + */ +struct cam_icp_clk_bw_request { + uint64_t budget_ns; + uint32_t frame_cycles; + uint32_t rt_flag; + uint64_t uncompressed_bw; + uint64_t compressed_bw; +}; + +/** + * struct cam_icp_dev_ver - Device information for particular hw type + * + * This is used to get device version info of + * ICP, IPE, BPS and CDM related IPE and BPS from firmware + * and use this info in CAM_QUERY_CAP IOCTL + * + * @dev_type: hardware type for the cap info(icp, ipe, bps, cdm(ipe/bps)) + * @reserved: reserved field + * @hw_ver: major, minor and incr values of a device version + */ +struct cam_icp_dev_ver { + uint32_t dev_type; + uint32_t reserved; + struct cam_hw_version hw_ver; +}; + +/** + * struct cam_icp_ver - ICP version info + * + * This strcuture is used for fw and api version + * this is used to get firmware version and api version from firmware + * and use this info in CAM_QUERY_CAP IOCTL + * + * @major: FW version major + * @minor: FW version minor + * @revision: FW version increment + */ +struct cam_icp_ver { + uint32_t major; + uint32_t minor; + uint32_t revision; + uint32_t reserved; +}; + +/** + * struct cam_icp_query_cap_cmd - ICP query device capability payload + * + * @dev_iommu_handle: icp iommu handles for secure/non secure modes + * @cdm_iommu_handle: iommu handles for secure/non secure modes + * @fw_version: firmware version info + * @api_version: api version info + * @num_ipe: number of ipes + * @num_bps: number of bps + * @dev_ver: returned device capability array + */ +struct cam_icp_query_cap_cmd { + struct cam_iommu_handle dev_iommu_handle; + struct cam_iommu_handle cdm_iommu_handle; + struct cam_icp_ver fw_version; + struct cam_icp_ver api_version; + uint32_t num_ipe; + uint32_t num_bps; + struct cam_icp_dev_ver dev_ver[CAM_ICP_DEV_TYPE_MAX]; +}; + +/** + * struct cam_icp_res_info - ICP output resource info + * + * @format: format of the resource + * @width: width in pixels + * @height: height in lines + * @fps: fps + */ +struct cam_icp_res_info { + uint32_t format; + uint32_t width; + uint32_t height; + uint32_t fps; +}; + +/** + * struct cam_icp_acquire_dev_info - An ICP device info + * + * @scratch_mem_size: Output param - size of scratch memory + * @dev_type: device type (IPE_RT/IPE_NON_RT/BPS) + * @io_config_cmd_size: size of IO config command + * @io_config_cmd_handle: IO config command for each acquire + * @secure_mode: camera mode (secure/non secure) + * @chain_info: chaining info of FW device handles + * @in_res: resource info used for clock and bandwidth calculation + * @num_out_res: number of output resources + * @out_res: output resource + */ +struct cam_icp_acquire_dev_info { + uint32_t scratch_mem_size; + uint32_t dev_type; + uint32_t io_config_cmd_size; + int32_t io_config_cmd_handle; + uint32_t secure_mode; + int32_t chain_info; + struct cam_icp_res_info in_res; + uint32_t num_out_res; + struct cam_icp_res_info out_res[1]; +} __attribute__((__packed__)); + +#endif /* __UAPI_CAM_ICP_H__ */ diff --git a/selfdrive/camerad/include/media/cam_isp.h b/selfdrive/camerad/include/media/cam_isp.h new file mode 100644 index 00000000..266840d3 --- /dev/null +++ b/selfdrive/camerad/include/media/cam_isp.h @@ -0,0 +1,379 @@ +#ifndef __UAPI_CAM_ISP_H__ +#define __UAPI_CAM_ISP_H__ + +#include "cam_defs.h" +#include "cam_isp_vfe.h" +#include "cam_isp_ife.h" + + +/* ISP driver name */ +#define CAM_ISP_DEV_NAME "cam-isp" + +/* HW type */ +#define CAM_ISP_HW_BASE 0 +#define CAM_ISP_HW_CSID 1 +#define CAM_ISP_HW_VFE 2 +#define CAM_ISP_HW_IFE 3 +#define CAM_ISP_HW_ISPIF 4 +#define CAM_ISP_HW_MAX 5 + +/* Color Pattern */ +#define CAM_ISP_PATTERN_BAYER_RGRGRG 0 +#define CAM_ISP_PATTERN_BAYER_GRGRGR 1 +#define CAM_ISP_PATTERN_BAYER_BGBGBG 2 +#define CAM_ISP_PATTERN_BAYER_GBGBGB 3 +#define CAM_ISP_PATTERN_YUV_YCBYCR 4 +#define CAM_ISP_PATTERN_YUV_YCRYCB 5 +#define CAM_ISP_PATTERN_YUV_CBYCRY 6 +#define CAM_ISP_PATTERN_YUV_CRYCBY 7 +#define CAM_ISP_PATTERN_MAX 8 + +/* Usage Type */ +#define CAM_ISP_RES_USAGE_SINGLE 0 +#define CAM_ISP_RES_USAGE_DUAL 1 +#define CAM_ISP_RES_USAGE_MAX 2 + +/* Resource ID */ +#define CAM_ISP_RES_ID_PORT 0 +#define CAM_ISP_RES_ID_CLK 1 +#define CAM_ISP_RES_ID_MAX 2 + +/* Resource Type - Type of resource for the resource id + * defined in cam_isp_vfe.h, cam_isp_ife.h + */ + +/* Lane Type in input resource for Port */ +#define CAM_ISP_LANE_TYPE_DPHY 0 +#define CAM_ISP_LANE_TYPE_CPHY 1 +#define CAM_ISP_LANE_TYPE_MAX 2 + +/* ISP Resurce Composite Group ID */ +#define CAM_ISP_RES_COMP_GROUP_NONE 0 +#define CAM_ISP_RES_COMP_GROUP_ID_0 1 +#define CAM_ISP_RES_COMP_GROUP_ID_1 2 +#define CAM_ISP_RES_COMP_GROUP_ID_2 3 +#define CAM_ISP_RES_COMP_GROUP_ID_3 4 +#define CAM_ISP_RES_COMP_GROUP_ID_4 5 +#define CAM_ISP_RES_COMP_GROUP_ID_5 6 +#define CAM_ISP_RES_COMP_GROUP_ID_MAX 6 + +/* ISP packet opcode for ISP */ +#define CAM_ISP_PACKET_OP_BASE 0 +#define CAM_ISP_PACKET_INIT_DEV 1 +#define CAM_ISP_PACKET_UPDATE_DEV 2 +#define CAM_ISP_PACKET_OP_MAX 3 + +/* ISP packet meta_data type for command buffer */ +#define CAM_ISP_PACKET_META_BASE 0 +#define CAM_ISP_PACKET_META_LEFT 1 +#define CAM_ISP_PACKET_META_RIGHT 2 +#define CAM_ISP_PACKET_META_COMMON 3 +#define CAM_ISP_PACKET_META_DMI_LEFT 4 +#define CAM_ISP_PACKET_META_DMI_RIGHT 5 +#define CAM_ISP_PACKET_META_DMI_COMMON 6 +#define CAM_ISP_PACKET_META_CLOCK 7 +#define CAM_ISP_PACKET_META_CSID 8 +#define CAM_ISP_PACKET_META_DUAL_CONFIG 9 +#define CAM_ISP_PACKET_META_GENERIC_BLOB_LEFT 10 +#define CAM_ISP_PACKET_META_GENERIC_BLOB_RIGHT 11 +#define CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON 12 + +/* DSP mode */ +#define CAM_ISP_DSP_MODE_NONE 0 +#define CAM_ISP_DSP_MODE_ONE_WAY 1 +#define CAM_ISP_DSP_MODE_ROUND 2 + +/* ISP Generic Cmd Buffer Blob types */ +#define CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG 0 +#define CAM_ISP_GENERIC_BLOB_TYPE_CLOCK_CONFIG 1 +#define CAM_ISP_GENERIC_BLOB_TYPE_BW_CONFIG 2 + +/* Query devices */ +/** + * struct cam_isp_dev_cap_info - A cap info for particular hw type + * + * @hw_type: Hardware type for the cap info + * @reserved: reserved field for alignment + * @hw_version: Hardware version + * + */ +struct cam_isp_dev_cap_info { + uint32_t hw_type; + uint32_t reserved; + struct cam_hw_version hw_version; +}; + +/** + * struct cam_isp_query_cap_cmd - ISP query device capability payload + * + * @device_iommu: returned iommu handles for device + * @cdm_iommu: returned iommu handles for cdm + * @num_dev: returned number of device capabilities + * @reserved: reserved field for alignment + * @dev_caps: returned device capability array + * + */ +struct cam_isp_query_cap_cmd { + struct cam_iommu_handle device_iommu; + struct cam_iommu_handle cdm_iommu; + int32_t num_dev; + uint32_t reserved; + struct cam_isp_dev_cap_info dev_caps[CAM_ISP_HW_MAX]; +}; + +/* Acquire Device */ +/** + * struct cam_isp_out_port_info - An output port resource info + * + * @res_type: output resource type defined in file + * cam_isp_vfe.h or cam_isp_ife.h + * @format: output format of the resource + * @wdith: output width in pixels + * @height: output height in lines + * @comp_grp_id: composite group id for the resource. + * @split_point: split point in pixels for the dual VFE. + * @secure_mode: flag to tell if output should be run in secure + * mode or not. See cam_defs.h for definition + * @reserved: reserved field for alignment + * + */ +struct cam_isp_out_port_info { + uint32_t res_type; + uint32_t format; + uint32_t width; + uint32_t height; + uint32_t comp_grp_id; + uint32_t split_point; + uint32_t secure_mode; + uint32_t reserved; +}; + +/** + * struct cam_isp_in_port_info - An input port resource info + * + * @res_type: input resource type define in file + * cam_isp_vfe.h or cam_isp_ife.h + * @lane_type: lane type: c-phy or d-phy. + * @lane_num: active lane number + * @lane_cfg: lane configurations: 4 bits per lane + * @vc: input virtual channel number + * @dt: input data type number + * @format: input format + * @test_pattern: test pattern for the testgen + * @usage_type: whether dual vfe is required + * @left_start: left input start offset in pixels + * @left_stop: left input stop offset in pixels + * @left_width: left input width in pixels + * @right_start: right input start offset in pixels. + * Only for Dual VFE + * @right_stop: right input stop offset in pixels. + * Only for Dual VFE + * @right_width: right input width in pixels. + * Only for dual VFE + * @line_start: top of the line number + * @line_stop: bottome of the line number + * @height: input height in lines + * @pixel_clk; sensor output clock + * @batch_size: batch size for HFR mode + * @dsp_mode: DSP stream mode (Defines as CAM_ISP_DSP_MODE_*) + * @hbi_cnt: HBI count for the camif input + * @reserved: Reserved field for alignment + * @num_out_res: number of the output resource associated + * @data: payload that contains the output resources + * + */ +struct cam_isp_in_port_info { + uint32_t res_type; + uint32_t lane_type; + uint32_t lane_num; + uint32_t lane_cfg; + uint32_t vc; + uint32_t dt; + uint32_t format; + uint32_t test_pattern; + uint32_t usage_type; + uint32_t left_start; + uint32_t left_stop; + uint32_t left_width; + uint32_t right_start; + uint32_t right_stop; + uint32_t right_width; + uint32_t line_start; + uint32_t line_stop; + uint32_t height; + uint32_t pixel_clk; + uint32_t batch_size; + uint32_t dsp_mode; + uint32_t hbi_cnt; + uint32_t custom_csid; + uint32_t reserved; + uint32_t num_out_res; + struct cam_isp_out_port_info data[1]; +}; + +/** + * struct cam_isp_resource - A resource bundle + * + * @resoruce_id: resource id for the resource bundle + * @length: length of the while resource blob + * @handle_type: type of the resource handle + * @reserved: reserved field for alignment + * @res_hdl: resource handle that points to the + * resource array; + * + */ +struct cam_isp_resource { + uint32_t resource_id; + uint32_t length; + uint32_t handle_type; + uint32_t reserved; + uint64_t res_hdl; +}; + +/** + * struct cam_isp_port_hfr_config - HFR configuration for this port + * + * @resource_type: Resource type + * @subsample_pattern: Subsample pattern. Used in HFR mode. It + * should be consistent with batchSize and + * CAMIF programming. + * @subsample_period: Subsample period. Used in HFR mode. It + * should be consistent with batchSize and + * CAMIF programming. + * @framedrop_pattern: Framedrop pattern + * @framedrop_period: Framedrop period + * @reserved: Reserved for alignment + */ +struct cam_isp_port_hfr_config { + uint32_t resource_type; + uint32_t subsample_pattern; + uint32_t subsample_period; + uint32_t framedrop_pattern; + uint32_t framedrop_period; + uint32_t reserved; +} __attribute__((packed)); + +/** + * struct cam_isp_resource_hfr_config - Resource HFR configuration + * + * @num_ports: Number of ports + * @reserved: Reserved for alignment + * @port_hfr_config: HFR configuration for each IO port + */ +struct cam_isp_resource_hfr_config { + uint32_t num_ports; + uint32_t reserved; + struct cam_isp_port_hfr_config port_hfr_config[1]; +} __attribute__((packed)); + +/** + * struct cam_isp_dual_split_params - dual isp spilt parameters + * + * @split_point: Split point information x, where (0 < x < width) + * left ISP's input ends at x + righ padding and + * Right ISP's input starts at x - left padding + * @right_padding: Padding added past the split point for left + * ISP's input + * @left_padding: Padding added before split point for right + * ISP's input + * @reserved: Reserved filed for alignment + * + */ +struct cam_isp_dual_split_params { + uint32_t split_point; + uint32_t right_padding; + uint32_t left_padding; + uint32_t reserved; +}; + +/** + * struct cam_isp_dual_stripe_config - stripe config per bus client + * + * @offset: Start horizontal offset relative to + * output buffer + * In UBWC mode, this value indicates the H_INIT + * value in pixel + * @width: Width of the stripe in bytes + * @tileconfig Ubwc meta tile config. Contain the partial + * tile info + * @port_id: port id of ISP output + * + */ +struct cam_isp_dual_stripe_config { + uint32_t offset; + uint32_t width; + uint32_t tileconfig; + uint32_t port_id; +}; + +/** + * struct cam_isp_dual_config - dual isp configuration + * + * @num_ports Number of isp output ports + * @reserved Reserved field for alignment + * @split_params: Inpput split parameters + * @stripes: Stripe information + * + */ +struct cam_isp_dual_config { + uint32_t num_ports; + uint32_t reserved; + struct cam_isp_dual_split_params split_params; + struct cam_isp_dual_stripe_config stripes[1]; +} __attribute__((packed)); + +/** + * struct cam_isp_clock_config - Clock configuration + * + * @usage_type: Usage type (Single/Dual) + * @num_rdi: Number of RDI votes + * @left_pix_hz: Pixel Clock for Left ISP + * @right_pix_hz: Pixel Clock for Right ISP, valid only if Dual + * @rdi_hz: RDI Clock. ISP clock will be max of RDI and + * PIX clocks. For a particular context which ISP + * HW the RDI is allocated to is not known to UMD. + * Hence pass the clock and let KMD decide. + */ +struct cam_isp_clock_config { + uint32_t usage_type; + uint32_t num_rdi; + uint64_t left_pix_hz; + uint64_t right_pix_hz; + uint64_t rdi_hz[1]; +} __attribute__((packed)); + +/** + * struct cam_isp_bw_vote - Bandwidth vote information + * + * @resource_id: Resource ID + * @reserved: Reserved field for alignment + * @cam_bw_bps: Bandwidth vote for CAMNOC + * @ext_bw_bps: Bandwidth vote for path-to-DDR after CAMNOC + */ + +struct cam_isp_bw_vote { + uint32_t resource_id; + uint32_t reserved; + uint64_t cam_bw_bps; + uint64_t ext_bw_bps; +} __attribute__((packed)); + +/** + * struct cam_isp_bw_config - Bandwidth configuration + * + * @usage_type: Usage type (Single/Dual) + * @num_rdi: Number of RDI votes + * @left_pix_vote: Bandwidth vote for left ISP + * @right_pix_vote: Bandwidth vote for right ISP + * @rdi_vote: RDI bandwidth requirements + */ + +struct cam_isp_bw_config { + uint32_t usage_type; + uint32_t num_rdi; + struct cam_isp_bw_vote left_pix_vote; + struct cam_isp_bw_vote right_pix_vote; + struct cam_isp_bw_vote rdi_vote[1]; +} __attribute__((packed)); + +#endif /* __UAPI_CAM_ISP_H__ */ diff --git a/selfdrive/camerad/include/media/cam_isp_ife.h b/selfdrive/camerad/include/media/cam_isp_ife.h new file mode 100644 index 00000000..f5e72813 --- /dev/null +++ b/selfdrive/camerad/include/media/cam_isp_ife.h @@ -0,0 +1,39 @@ +#ifndef __UAPI_CAM_ISP_IFE_H__ +#define __UAPI_CAM_ISP_IFE_H__ + +/* IFE output port resource type (global unique)*/ +#define CAM_ISP_IFE_OUT_RES_BASE 0x3000 + +#define CAM_ISP_IFE_OUT_RES_FULL (CAM_ISP_IFE_OUT_RES_BASE + 0) +#define CAM_ISP_IFE_OUT_RES_DS4 (CAM_ISP_IFE_OUT_RES_BASE + 1) +#define CAM_ISP_IFE_OUT_RES_DS16 (CAM_ISP_IFE_OUT_RES_BASE + 2) +#define CAM_ISP_IFE_OUT_RES_RAW_DUMP (CAM_ISP_IFE_OUT_RES_BASE + 3) +#define CAM_ISP_IFE_OUT_RES_FD (CAM_ISP_IFE_OUT_RES_BASE + 4) +#define CAM_ISP_IFE_OUT_RES_PDAF (CAM_ISP_IFE_OUT_RES_BASE + 5) +#define CAM_ISP_IFE_OUT_RES_RDI_0 (CAM_ISP_IFE_OUT_RES_BASE + 6) +#define CAM_ISP_IFE_OUT_RES_RDI_1 (CAM_ISP_IFE_OUT_RES_BASE + 7) +#define CAM_ISP_IFE_OUT_RES_RDI_2 (CAM_ISP_IFE_OUT_RES_BASE + 8) +#define CAM_ISP_IFE_OUT_RES_RDI_3 (CAM_ISP_IFE_OUT_RES_BASE + 9) +#define CAM_ISP_IFE_OUT_RES_STATS_HDR_BE (CAM_ISP_IFE_OUT_RES_BASE + 10) +#define CAM_ISP_IFE_OUT_RES_STATS_HDR_BHIST (CAM_ISP_IFE_OUT_RES_BASE + 11) +#define CAM_ISP_IFE_OUT_RES_STATS_TL_BG (CAM_ISP_IFE_OUT_RES_BASE + 12) +#define CAM_ISP_IFE_OUT_RES_STATS_BF (CAM_ISP_IFE_OUT_RES_BASE + 13) +#define CAM_ISP_IFE_OUT_RES_STATS_AWB_BG (CAM_ISP_IFE_OUT_RES_BASE + 14) +#define CAM_ISP_IFE_OUT_RES_STATS_BHIST (CAM_ISP_IFE_OUT_RES_BASE + 15) +#define CAM_ISP_IFE_OUT_RES_STATS_RS (CAM_ISP_IFE_OUT_RES_BASE + 16) +#define CAM_ISP_IFE_OUT_RES_STATS_CS (CAM_ISP_IFE_OUT_RES_BASE + 17) +#define CAM_ISP_IFE_OUT_RES_STATS_IHIST (CAM_ISP_IFE_OUT_RES_BASE + 18) +#define CAM_ISP_IFE_OUT_RES_MAX (CAM_ISP_IFE_OUT_RES_BASE + 19) + + +/* IFE input port resource type (global unique) */ +#define CAM_ISP_IFE_IN_RES_BASE 0x4000 + +#define CAM_ISP_IFE_IN_RES_TPG (CAM_ISP_IFE_IN_RES_BASE + 0) +#define CAM_ISP_IFE_IN_RES_PHY_0 (CAM_ISP_IFE_IN_RES_BASE + 1) +#define CAM_ISP_IFE_IN_RES_PHY_1 (CAM_ISP_IFE_IN_RES_BASE + 2) +#define CAM_ISP_IFE_IN_RES_PHY_2 (CAM_ISP_IFE_IN_RES_BASE + 3) +#define CAM_ISP_IFE_IN_RES_PHY_3 (CAM_ISP_IFE_IN_RES_BASE + 4) +#define CAM_ISP_IFE_IN_RES_MAX (CAM_ISP_IFE_IN_RES_BASE + 5) + +#endif /* __UAPI_CAM_ISP_IFE_H__ */ diff --git a/selfdrive/camerad/include/media/cam_isp_vfe.h b/selfdrive/camerad/include/media/cam_isp_vfe.h new file mode 100644 index 00000000..e48db2f9 --- /dev/null +++ b/selfdrive/camerad/include/media/cam_isp_vfe.h @@ -0,0 +1,44 @@ +#ifndef __UAPI_CAM_ISP_VFE_H__ +#define __UAPI_CAM_ISP_VFE_H__ + +/* VFE output port resource type (global unique) */ +#define CAM_ISP_VFE_OUT_RES_BASE 0x1000 + +#define CAM_ISP_VFE_OUT_RES_ENC (CAM_ISP_VFE_OUT_RES_BASE + 0) +#define CAM_ISP_VFE_OUT_RES_VIEW (CAM_ISP_VFE_OUT_RES_BASE + 1) +#define CAM_ISP_VFE_OUT_RES_VID (CAM_ISP_VFE_OUT_RES_BASE + 2) +#define CAM_ISP_VFE_OUT_RES_RDI_0 (CAM_ISP_VFE_OUT_RES_BASE + 3) +#define CAM_ISP_VFE_OUT_RES_RDI_1 (CAM_ISP_VFE_OUT_RES_BASE + 4) +#define CAM_ISP_VFE_OUT_RES_RDI_2 (CAM_ISP_VFE_OUT_RES_BASE + 5) +#define CAM_ISP_VFE_OUT_RES_RDI_3 (CAM_ISP_VFE_OUT_RES_BASE + 6) +#define CAM_ISP_VFE_OUT_RES_STATS_AEC (CAM_ISP_VFE_OUT_RES_BASE + 7) +#define CAM_ISP_VFE_OUT_RES_STATS_AF (CAM_ISP_VFE_OUT_RES_BASE + 8) +#define CAM_ISP_VFE_OUT_RES_STATS_AWB (CAM_ISP_VFE_OUT_RES_BASE + 9) +#define CAM_ISP_VFE_OUT_RES_STATS_RS (CAM_ISP_VFE_OUT_RES_BASE + 10) +#define CAM_ISP_VFE_OUT_RES_STATS_CS (CAM_ISP_VFE_OUT_RES_BASE + 11) +#define CAM_ISP_VFE_OUT_RES_STATS_IHIST (CAM_ISP_VFE_OUT_RES_BASE + 12) +#define CAM_ISP_VFE_OUT_RES_STATS_SKIN (CAM_ISP_VFE_OUT_RES_BASE + 13) +#define CAM_ISP_VFE_OUT_RES_STATS_BG (CAM_ISP_VFE_OUT_RES_BASE + 14) +#define CAM_ISP_VFE_OUT_RES_STATS_BF (CAM_ISP_VFE_OUT_RES_BASE + 15) +#define CAM_ISP_VFE_OUT_RES_STATS_BE (CAM_ISP_VFE_OUT_RES_BASE + 16) +#define CAM_ISP_VFE_OUT_RES_STATS_BHIST (CAM_ISP_VFE_OUT_RES_BASE + 17) +#define CAM_ISP_VFE_OUT_RES_STATS_BF_SCALE (CAM_ISP_VFE_OUT_RES_BASE + 18) +#define CAM_ISP_VFE_OUT_RES_STATS_HDR_BE (CAM_ISP_VFE_OUT_RES_BASE + 19) +#define CAM_ISP_VFE_OUT_RES_STATS_HDR_BHIST (CAM_ISP_VFE_OUT_RES_BASE + 20) +#define CAM_ISP_VFE_OUT_RES_STATS_AEC_BG (CAM_ISP_VFE_OUT_RES_BASE + 21) +#define CAM_ISP_VFE_OUT_RES_CAMIF_RAW (CAM_ISP_VFE_OUT_RES_BASE + 22) +#define CAM_ISP_VFE_OUT_RES_IDEAL_RAW (CAM_ISP_VFE_OUT_RES_BASE + 23) +#define CAM_ISP_VFE_OUT_RES_MAX (CAM_ISP_VFE_OUT_RES_BASE + 24) + +/* VFE input port_ resource type (global unique) */ +#define CAM_ISP_VFE_IN_RES_BASE 0x2000 + +#define CAM_ISP_VFE_IN_RES_TPG (CAM_ISP_VFE_IN_RES_BASE + 0) +#define CAM_ISP_VFE_IN_RES_PHY_0 (CAM_ISP_VFE_IN_RES_BASE + 1) +#define CAM_ISP_VFE_IN_RES_PHY_1 (CAM_ISP_VFE_IN_RES_BASE + 2) +#define CAM_ISP_VFE_IN_RES_PHY_2 (CAM_ISP_VFE_IN_RES_BASE + 3) +#define CAM_ISP_VFE_IN_RES_PHY_3 (CAM_ISP_VFE_IN_RES_BASE + 4) +#define CAM_ISP_VFE_IN_RES_FE (CAM_ISP_VFE_IN_RES_BASE + 5) +#define CAM_ISP_VFE_IN_RES_MAX (CAM_ISP_VFE_IN_RES_BASE + 6) + +#endif /* __UAPI_CAM_ISP_VFE_H__ */ diff --git a/selfdrive/camerad/include/media/cam_jpeg.h b/selfdrive/camerad/include/media/cam_jpeg.h new file mode 100644 index 00000000..f3082f3b --- /dev/null +++ b/selfdrive/camerad/include/media/cam_jpeg.h @@ -0,0 +1,117 @@ +#ifndef __UAPI_CAM_JPEG_H__ +#define __UAPI_CAM_JPEG_H__ + +#include "cam_defs.h" + +/* enc, dma, cdm(enc/dma) are used in querycap */ +#define CAM_JPEG_DEV_TYPE_ENC 0 +#define CAM_JPEG_DEV_TYPE_DMA 1 +#define CAM_JPEG_DEV_TYPE_MAX 2 + +#define CAM_JPEG_NUM_DEV_PER_RES_MAX 1 + +/* definitions needed for jpeg aquire device */ +#define CAM_JPEG_RES_TYPE_ENC 0 +#define CAM_JPEG_RES_TYPE_DMA 1 +#define CAM_JPEG_RES_TYPE_MAX 2 + +/* packet opcode types */ +#define CAM_JPEG_OPCODE_ENC_UPDATE 0 +#define CAM_JPEG_OPCODE_DMA_UPDATE 1 + +/* ENC input port resource type */ +#define CAM_JPEG_ENC_INPUT_IMAGE 0x0 + +/* ENC output port resource type */ +#define CAM_JPEG_ENC_OUTPUT_IMAGE 0x1 + +#define CAM_JPEG_ENC_IO_IMAGES_MAX 0x2 + +/* DMA input port resource type */ +#define CAM_JPEG_DMA_INPUT_IMAGE 0x0 + +/* DMA output port resource type */ +#define CAM_JPEG_DMA_OUTPUT_IMAGE 0x1 + +#define CAM_JPEG_DMA_IO_IMAGES_MAX 0x2 + +#define CAM_JPEG_IMAGE_MAX 0x2 + +/** + * struct cam_jpeg_dev_ver - Device information for particular hw type + * + * This is used to get device version info of JPEG ENC, JPEG DMA + * from hardware and use this info in CAM_QUERY_CAP IOCTL + * + * @size : Size of struct passed + * @dev_type: Hardware type for the cap info(jpeg enc, jpeg dma) + * @hw_ver: Major, minor and incr values of a device version + */ +struct cam_jpeg_dev_ver { + uint32_t size; + uint32_t dev_type; + struct cam_hw_version hw_ver; +}; + +/** + * struct cam_jpeg_query_cap_cmd - JPEG query device capability payload + * + * @dev_iommu_handle: Jpeg iommu handles for secure/non secure + * modes + * @cdm_iommu_handle: Iommu handles for secure/non secure modes + * @num_enc: Number of encoder + * @num_dma: Number of dma + * @dev_ver: Returned device capability array + */ +struct cam_jpeg_query_cap_cmd { + struct cam_iommu_handle dev_iommu_handle; + struct cam_iommu_handle cdm_iommu_handle; + uint32_t num_enc; + uint32_t num_dma; + struct cam_jpeg_dev_ver dev_ver[CAM_JPEG_DEV_TYPE_MAX]; +}; + +/** + * struct cam_jpeg_res_info - JPEG output resource info + * + * @format: Format of the resource + * @width: Width in pixels + * @height: Height in lines + * @fps: Fps + */ +struct cam_jpeg_res_info { + uint32_t format; + uint32_t width; + uint32_t height; + uint32_t fps; +}; + +/** + * struct cam_jpeg_acquire_dev_info - An JPEG device info + * + * @dev_type: Device type (ENC/DMA) + * @reserved: Reserved Bytes + * @in_res: In resource info + * @in_res: Iut resource info + */ +struct cam_jpeg_acquire_dev_info { + uint32_t dev_type; + uint32_t reserved; + struct cam_jpeg_res_info in_res; + struct cam_jpeg_res_info out_res; +}; + +/** + * struct cam_jpeg_config_inout_param_info - JPEG Config time + * input output params + * + * @clk_index: Input Param- clock selection index.(-1 default) + * @output_size: Output Param - jpeg encode/dma output size in + * bytes + */ +struct cam_jpeg_config_inout_param_info { + int32_t clk_index; + int32_t output_size; +}; + +#endif /* __UAPI_CAM_JPEG_H__ */ diff --git a/selfdrive/camerad/include/media/cam_lrme.h b/selfdrive/camerad/include/media/cam_lrme.h new file mode 100644 index 00000000..97d95783 --- /dev/null +++ b/selfdrive/camerad/include/media/cam_lrme.h @@ -0,0 +1,65 @@ +#ifndef __UAPI_CAM_LRME_H__ +#define __UAPI_CAM_LRME_H__ + +#include "cam_defs.h" + +/* LRME Resource Types */ + +enum CAM_LRME_IO_TYPE { + CAM_LRME_IO_TYPE_TAR, + CAM_LRME_IO_TYPE_REF, + CAM_LRME_IO_TYPE_RES, + CAM_LRME_IO_TYPE_DS2, +}; + +#define CAM_LRME_INPUT_PORT_TYPE_TAR (1 << 0) +#define CAM_LRME_INPUT_PORT_TYPE_REF (1 << 1) + +#define CAM_LRME_OUTPUT_PORT_TYPE_DS2 (1 << 0) +#define CAM_LRME_OUTPUT_PORT_TYPE_RES (1 << 1) + +#define CAM_LRME_DEV_MAX 1 + + +struct cam_lrme_hw_version { + uint32_t gen; + uint32_t rev; + uint32_t step; +}; + +struct cam_lrme_dev_cap { + struct cam_lrme_hw_version clc_hw_version; + struct cam_lrme_hw_version bus_rd_hw_version; + struct cam_lrme_hw_version bus_wr_hw_version; + struct cam_lrme_hw_version top_hw_version; + struct cam_lrme_hw_version top_titan_version; +}; + +/** + * struct cam_lrme_query_cap_cmd - LRME query device capability payload + * + * @dev_iommu_handle: LRME iommu handles for secure/non secure + * modes + * @cdm_iommu_handle: Iommu handles for secure/non secure modes + * @num_devices: number of hardware devices + * @dev_caps: Returned device capability array + */ +struct cam_lrme_query_cap_cmd { + struct cam_iommu_handle device_iommu; + struct cam_iommu_handle cdm_iommu; + uint32_t num_devices; + struct cam_lrme_dev_cap dev_caps[CAM_LRME_DEV_MAX]; +}; + +struct cam_lrme_soc_info { + uint64_t clock_rate; + uint64_t bandwidth; + uint64_t reserved[4]; +}; + +struct cam_lrme_acquire_args { + struct cam_lrme_soc_info lrme_soc_info; +}; + +#endif /* __UAPI_CAM_LRME_H__ */ + diff --git a/selfdrive/camerad/include/media/cam_req_mgr.h b/selfdrive/camerad/include/media/cam_req_mgr.h new file mode 100644 index 00000000..ae656499 --- /dev/null +++ b/selfdrive/camerad/include/media/cam_req_mgr.h @@ -0,0 +1,436 @@ +#ifndef __UAPI_LINUX_CAM_REQ_MGR_H +#define __UAPI_LINUX_CAM_REQ_MGR_H + +#include +#include +#include +#include +#include + +#define CAM_REQ_MGR_VNODE_NAME "cam-req-mgr-devnode" + +#define CAM_DEVICE_TYPE_BASE (MEDIA_ENT_F_OLD_BASE) +#define CAM_VNODE_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE) +#define CAM_SENSOR_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 1) +#define CAM_IFE_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 2) +#define CAM_ICP_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 3) +#define CAM_LRME_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 4) +#define CAM_JPEG_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 5) +#define CAM_FD_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 6) +#define CAM_CPAS_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 7) +#define CAM_CSIPHY_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 8) +#define CAM_ACTUATOR_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 9) +#define CAM_CCI_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 10) +#define CAM_FLASH_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 11) +#define CAM_EEPROM_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 12) +#define CAM_OIS_DEVICE_TYPE (CAM_DEVICE_TYPE_BASE + 13) + +/* cam_req_mgr hdl info */ +#define CAM_REQ_MGR_HDL_IDX_POS 8 +#define CAM_REQ_MGR_HDL_IDX_MASK ((1 << CAM_REQ_MGR_HDL_IDX_POS) - 1) +#define CAM_REQ_MGR_GET_HDL_IDX(hdl) (hdl & CAM_REQ_MGR_HDL_IDX_MASK) + +/** + * Max handles supported by cam_req_mgr + * It includes both session and device handles + */ +#define CAM_REQ_MGR_MAX_HANDLES 64 +#define MAX_LINKS_PER_SESSION 2 + +/* V4L event type which user space will subscribe to */ +#define V4L_EVENT_CAM_REQ_MGR_EVENT (V4L2_EVENT_PRIVATE_START + 0) + +/* Specific event ids to get notified in user space */ +#define V4L_EVENT_CAM_REQ_MGR_SOF 0 +#define V4L_EVENT_CAM_REQ_MGR_ERROR 1 +#define V4L_EVENT_CAM_REQ_MGR_SOF_BOOT_TS 2 + +/* SOF Event status */ +#define CAM_REQ_MGR_SOF_EVENT_SUCCESS 0 +#define CAM_REQ_MGR_SOF_EVENT_ERROR 1 + +/* Link control operations */ +#define CAM_REQ_MGR_LINK_ACTIVATE 0 +#define CAM_REQ_MGR_LINK_DEACTIVATE 1 + +/** + * Request Manager : flush_type + * @CAM_REQ_MGR_FLUSH_TYPE_ALL: Req mgr will remove all the pending + * requests from input/processing queue. + * @CAM_REQ_MGR_FLUSH_TYPE_CANCEL_REQ: Req mgr will remove only particular + * request id from input/processing queue. + * @CAM_REQ_MGR_FLUSH_TYPE_MAX: Max number of the flush type + * @opcode: CAM_REQ_MGR_FLUSH_REQ + */ +#define CAM_REQ_MGR_FLUSH_TYPE_ALL 0 +#define CAM_REQ_MGR_FLUSH_TYPE_CANCEL_REQ 1 +#define CAM_REQ_MGR_FLUSH_TYPE_MAX 2 + +/** + * Request Manager : Sync Mode type + * @CAM_REQ_MGR_SYNC_MODE_NO_SYNC: Req mgr will apply non-sync mode for this + * request. + * @CAM_REQ_MGR_SYNC_MODE_SYNC: Req mgr will apply sync mode for this request. + */ +#define CAM_REQ_MGR_SYNC_MODE_NO_SYNC 0 +#define CAM_REQ_MGR_SYNC_MODE_SYNC 1 + +/** + * struct cam_req_mgr_event_data + * @session_hdl: session handle + * @link_hdl: link handle + * @frame_id: frame id + * @reserved: reserved for 64 bit aligngment + * @req_id: request id + * @tv_sec: timestamp in seconds + * @tv_usec: timestamp in micro seconds + */ +struct cam_req_mgr_event_data { + int32_t session_hdl; + int32_t link_hdl; + int32_t frame_id; + int32_t reserved; + int64_t req_id; + uint64_t tv_sec; + uint64_t tv_usec; +}; + +/** + * struct cam_req_mgr_session_info + * @session_hdl: In/Output param - session_handle + * @opcode1: CAM_REQ_MGR_CREATE_SESSION + * @opcode2: CAM_REQ_MGR_DESTROY_SESSION + */ +struct cam_req_mgr_session_info { + int32_t session_hdl; + int32_t reserved; +}; + +/** + * struct cam_req_mgr_link_info + * @session_hdl: Input param - Identifier for CSL session + * @num_devices: Input Param - Num of devices to be linked + * @dev_hdls: Input param - List of device handles to be linked + * @link_hdl: Output Param -Identifier for link + * @opcode: CAM_REQ_MGR_LINK + */ +struct cam_req_mgr_link_info { + int32_t session_hdl; + uint32_t num_devices; + int32_t dev_hdls[CAM_REQ_MGR_MAX_HANDLES]; + int32_t link_hdl; +}; + +/** + * struct cam_req_mgr_unlink_info + * @session_hdl: input param - session handle + * @link_hdl: input param - link handle + * @opcode: CAM_REQ_MGR_UNLINK + */ +struct cam_req_mgr_unlink_info { + int32_t session_hdl; + int32_t link_hdl; +}; + +/** + * struct cam_req_mgr_flush_info + * @brief: User can tell drivers to flush a particular request id or + * flush all requests from its pending processing queue. Flush is a + * blocking call and driver shall ensure all requests are flushed + * before returning. + * @session_hdl: Input param - Identifier for CSL session + * @link_hdl: Input Param -Identifier for link + * @flush_type: User can cancel a particular req id or can flush + * all requests in queue + * @reserved: reserved for 64 bit aligngment + * @req_id: field is valid only if flush type is cancel request + * for flush all this field value is not considered. + * @opcode: CAM_REQ_MGR_FLUSH_REQ + */ +struct cam_req_mgr_flush_info { + int32_t session_hdl; + int32_t link_hdl; + uint32_t flush_type; + uint32_t reserved; + int64_t req_id; +}; + +/** struct cam_req_mgr_sched_info + * @session_hdl: Input param - Identifier for CSL session + * @link_hdl: Input Param -Identifier for link + * inluding itself. + * @bubble_enable: Input Param - Cam req mgr will do bubble recovery if this + * flag is set. + * @sync_mode: Type of Sync mode for this request + * @req_id: Input Param - Request Id from which all requests will be flushed + */ +struct cam_req_mgr_sched_request { + int32_t session_hdl; + int32_t link_hdl; + int32_t bubble_enable; + int32_t sync_mode; + int64_t req_id; +}; + +/** + * struct cam_req_mgr_sync_mode + * @session_hdl: Input param - Identifier for CSL session + * @sync_mode: Input Param - Type of sync mode + * @num_links: Input Param - Num of links in sync mode (Valid only + * when sync_mode is one of SYNC enabled modes) + * @link_hdls: Input Param - Array of link handles to be in sync mode + * (Valid only when sync_mode is one of SYNC + * enabled modes) + * @master_link_hdl: Input Param - To dictate which link's SOF drives system + * (Valid only when sync_mode is one of SYNC + * enabled modes) + * + * @opcode: CAM_REQ_MGR_SYNC_MODE + */ +struct cam_req_mgr_sync_mode { + int32_t session_hdl; + int32_t sync_mode; + int32_t num_links; + int32_t link_hdls[MAX_LINKS_PER_SESSION]; + int32_t master_link_hdl; + int32_t reserved; +}; + +/** + * struct cam_req_mgr_link_control + * @ops: Link operations: activate/deactive + * @session_hdl: Input param - Identifier for CSL session + * @num_links: Input Param - Num of links + * @reserved: reserved field + * @link_hdls: Input Param - Links to be activated/deactivated + * + * @opcode: CAM_REQ_MGR_LINK_CONTROL + */ +struct cam_req_mgr_link_control { + int32_t ops; + int32_t session_hdl; + int32_t num_links; + int32_t reserved; + int32_t link_hdls[MAX_LINKS_PER_SESSION]; +}; + +/** + * cam_req_mgr specific opcode ids + */ +#define CAM_REQ_MGR_CREATE_DEV_NODES (CAM_COMMON_OPCODE_MAX + 1) +#define CAM_REQ_MGR_CREATE_SESSION (CAM_COMMON_OPCODE_MAX + 2) +#define CAM_REQ_MGR_DESTROY_SESSION (CAM_COMMON_OPCODE_MAX + 3) +#define CAM_REQ_MGR_LINK (CAM_COMMON_OPCODE_MAX + 4) +#define CAM_REQ_MGR_UNLINK (CAM_COMMON_OPCODE_MAX + 5) +#define CAM_REQ_MGR_SCHED_REQ (CAM_COMMON_OPCODE_MAX + 6) +#define CAM_REQ_MGR_FLUSH_REQ (CAM_COMMON_OPCODE_MAX + 7) +#define CAM_REQ_MGR_SYNC_MODE (CAM_COMMON_OPCODE_MAX + 8) +#define CAM_REQ_MGR_ALLOC_BUF (CAM_COMMON_OPCODE_MAX + 9) +#define CAM_REQ_MGR_MAP_BUF (CAM_COMMON_OPCODE_MAX + 10) +#define CAM_REQ_MGR_RELEASE_BUF (CAM_COMMON_OPCODE_MAX + 11) +#define CAM_REQ_MGR_CACHE_OPS (CAM_COMMON_OPCODE_MAX + 12) +#define CAM_REQ_MGR_LINK_CONTROL (CAM_COMMON_OPCODE_MAX + 13) +/* end of cam_req_mgr opcodes */ + +#define CAM_MEM_FLAG_HW_READ_WRITE (1<<0) +#define CAM_MEM_FLAG_HW_READ_ONLY (1<<1) +#define CAM_MEM_FLAG_HW_WRITE_ONLY (1<<2) +#define CAM_MEM_FLAG_KMD_ACCESS (1<<3) +#define CAM_MEM_FLAG_UMD_ACCESS (1<<4) +#define CAM_MEM_FLAG_PROTECTED_MODE (1<<5) +#define CAM_MEM_FLAG_CMD_BUF_TYPE (1<<6) +#define CAM_MEM_FLAG_PIXEL_BUF_TYPE (1<<7) +#define CAM_MEM_FLAG_STATS_BUF_TYPE (1<<8) +#define CAM_MEM_FLAG_PACKET_BUF_TYPE (1<<9) +#define CAM_MEM_FLAG_CACHE (1<<10) +#define CAM_MEM_FLAG_HW_SHARED_ACCESS (1<<11) + +#define CAM_MEM_MMU_MAX_HANDLE 16 + +/* Maximum allowed buffers in existence */ +#define CAM_MEM_BUFQ_MAX 1024 + +#define CAM_MEM_MGR_SECURE_BIT_POS 15 +#define CAM_MEM_MGR_HDL_IDX_SIZE 15 +#define CAM_MEM_MGR_HDL_FD_SIZE 16 +#define CAM_MEM_MGR_HDL_IDX_END_POS 16 +#define CAM_MEM_MGR_HDL_FD_END_POS 32 + +#define CAM_MEM_MGR_HDL_IDX_MASK ((1 << CAM_MEM_MGR_HDL_IDX_SIZE) - 1) + +#define GET_MEM_HANDLE(idx, fd) \ + ((idx & CAM_MEM_MGR_HDL_IDX_MASK) | \ + (fd << (CAM_MEM_MGR_HDL_FD_END_POS - CAM_MEM_MGR_HDL_FD_SIZE))) \ + +#define GET_FD_FROM_HANDLE(hdl) \ + (hdl >> (CAM_MEM_MGR_HDL_FD_END_POS - CAM_MEM_MGR_HDL_FD_SIZE)) \ + +#define CAM_MEM_MGR_GET_HDL_IDX(hdl) (hdl & CAM_MEM_MGR_HDL_IDX_MASK) + +#define CAM_MEM_MGR_SET_SECURE_HDL(hdl, flag) \ + ((flag) ? (hdl |= (1 << CAM_MEM_MGR_SECURE_BIT_POS)) : \ + ((hdl) &= ~(1 << CAM_MEM_MGR_SECURE_BIT_POS))) + +#define CAM_MEM_MGR_IS_SECURE_HDL(hdl) \ + (((hdl) & \ + (1<> CAM_MEM_MGR_SECURE_BIT_POS) + +/** + * memory allocation type + */ +#define CAM_MEM_DMA_NONE 0 +#define CAM_MEM_DMA_BIDIRECTIONAL 1 +#define CAM_MEM_DMA_TO_DEVICE 2 +#define CAM_MEM_DMA_FROM_DEVICE 3 + + +/** + * memory cache operation + */ +#define CAM_MEM_CLEAN_CACHE 1 +#define CAM_MEM_INV_CACHE 2 +#define CAM_MEM_CLEAN_INV_CACHE 3 + + +/** + * struct cam_mem_alloc_out_params + * @buf_handle: buffer handle + * @fd: output buffer file descriptor + * @vaddr: virtual address pointer + */ +struct cam_mem_alloc_out_params { + uint32_t buf_handle; + int32_t fd; + uint64_t vaddr; +}; + +/** + * struct cam_mem_map_out_params + * @buf_handle: buffer handle + * @reserved: reserved for future + * @vaddr: virtual address pointer + */ +struct cam_mem_map_out_params { + uint32_t buf_handle; + uint32_t reserved; + uint64_t vaddr; +}; + +/** + * struct cam_mem_mgr_alloc_cmd + * @len: size of buffer to allocate + * @align: alignment of the buffer + * @mmu_hdls: array of mmu handles + * @num_hdl: number of handles + * @flags: flags of the buffer + * @out: out params + */ +/* CAM_REQ_MGR_ALLOC_BUF */ +struct cam_mem_mgr_alloc_cmd { + uint64_t len; + uint64_t align; + int32_t mmu_hdls[CAM_MEM_MMU_MAX_HANDLE]; + uint32_t num_hdl; + uint32_t flags; + struct cam_mem_alloc_out_params out; +}; + +/** + * struct cam_mem_mgr_map_cmd + * @mmu_hdls: array of mmu handles + * @num_hdl: number of handles + * @flags: flags of the buffer + * @fd: output buffer file descriptor + * @reserved: reserved field + * @out: out params + */ + +/* CAM_REQ_MGR_MAP_BUF */ +struct cam_mem_mgr_map_cmd { + int32_t mmu_hdls[CAM_MEM_MMU_MAX_HANDLE]; + uint32_t num_hdl; + uint32_t flags; + int32_t fd; + uint32_t reserved; + struct cam_mem_map_out_params out; +}; + +/** + * struct cam_mem_mgr_map_cmd + * @buf_handle: buffer handle + * @reserved: reserved field + */ +/* CAM_REQ_MGR_RELEASE_BUF */ +struct cam_mem_mgr_release_cmd { + int32_t buf_handle; + uint32_t reserved; +}; + +/** + * struct cam_mem_mgr_map_cmd + * @buf_handle: buffer handle + * @ops: cache operations + */ +/* CAM_REQ_MGR_CACHE_OPS */ +struct cam_mem_cache_ops_cmd { + int32_t buf_handle; + uint32_t mem_cache_ops; +}; + +/** + * Request Manager : error message type + * @CAM_REQ_MGR_ERROR_TYPE_DEVICE: Device error message, fatal to session + * @CAM_REQ_MGR_ERROR_TYPE_REQUEST: Error on a single request, not fatal + * @CAM_REQ_MGR_ERROR_TYPE_BUFFER: Buffer was not filled, not fatal + */ +#define CAM_REQ_MGR_ERROR_TYPE_DEVICE 0 +#define CAM_REQ_MGR_ERROR_TYPE_REQUEST 1 +#define CAM_REQ_MGR_ERROR_TYPE_BUFFER 2 + +/** + * struct cam_req_mgr_error_msg + * @error_type: type of error + * @request_id: request id of frame + * @device_hdl: device handle + * @linke_hdl: link_hdl + * @resource_size: size of the resource + */ +struct cam_req_mgr_error_msg { + uint32_t error_type; + uint32_t request_id; + int32_t device_hdl; + int32_t link_hdl; + uint64_t resource_size; +}; + +/** + * struct cam_req_mgr_frame_msg + * @request_id: request id of the frame + * @frame_id: frame id of the frame + * @timestamp: timestamp of the frame + * @link_hdl: link handle associated with this message + * @sof_status: sof status success or fail + */ +struct cam_req_mgr_frame_msg { + uint64_t request_id; + uint64_t frame_id; + uint64_t timestamp; + int32_t link_hdl; + uint32_t sof_status; +}; + +/** + * struct cam_req_mgr_message + * @session_hdl: session to which the frame belongs to + * @reserved: reserved field + * @u: union which can either be error or frame message + */ +struct cam_req_mgr_message { + int32_t session_hdl; + int32_t reserved; + union { + struct cam_req_mgr_error_msg err_msg; + struct cam_req_mgr_frame_msg frame_msg; + } u; +}; +#endif /* __UAPI_LINUX_CAM_REQ_MGR_H */ diff --git a/selfdrive/camerad/include/media/cam_sensor.h b/selfdrive/camerad/include/media/cam_sensor.h new file mode 100644 index 00000000..f5af6047 --- /dev/null +++ b/selfdrive/camerad/include/media/cam_sensor.h @@ -0,0 +1,477 @@ +#ifndef __UAPI_CAM_SENSOR_H__ +#define __UAPI_CAM_SENSOR_H__ + +#include +#include +#include + +#define CAM_SENSOR_PROBE_CMD (CAM_COMMON_OPCODE_MAX + 1) +#define CAM_FLASH_MAX_LED_TRIGGERS 3 +#define MAX_OIS_NAME_SIZE 32 +#define CAM_CSIPHY_SECURE_MODE_ENABLED 1 +/** + * struct cam_sensor_query_cap - capabilities info for sensor + * + * @slot_info : Indicates about the slotId or cell Index + * @secure_camera : Camera is in secure/Non-secure mode + * @pos_pitch : Sensor position pitch + * @pos_roll : Sensor position roll + * @pos_yaw : Sensor position yaw + * @actuator_slot_id : Actuator slot id which connected to sensor + * @eeprom_slot_id : EEPROM slot id which connected to sensor + * @ois_slot_id : OIS slot id which connected to sensor + * @flash_slot_id : Flash slot id which connected to sensor + * @csiphy_slot_id : CSIphy slot id which connected to sensor + * + */ +struct cam_sensor_query_cap { + uint32_t slot_info; + uint32_t secure_camera; + uint32_t pos_pitch; + uint32_t pos_roll; + uint32_t pos_yaw; + uint32_t actuator_slot_id; + uint32_t eeprom_slot_id; + uint32_t ois_slot_id; + uint32_t flash_slot_id; + uint32_t csiphy_slot_id; +} __attribute__((packed)); + +/** + * struct cam_csiphy_query_cap - capabilities info for csiphy + * + * @slot_info : Indicates about the slotId or cell Index + * @version : CSIphy version + * @clk lane : Of the 5 lanes, informs lane configured + * as clock lane + * @reserved + */ +struct cam_csiphy_query_cap { + uint32_t slot_info; + uint32_t version; + uint32_t clk_lane; + uint32_t reserved; +} __attribute__((packed)); + +/** + * struct cam_actuator_query_cap - capabilities info for actuator + * + * @slot_info : Indicates about the slotId or cell Index + * @reserved + */ +struct cam_actuator_query_cap { + uint32_t slot_info; + uint32_t reserved; +} __attribute__((packed)); + +/** + * struct cam_eeprom_query_cap_t - capabilities info for eeprom + * + * @slot_info : Indicates about the slotId or cell Index + * @eeprom_kernel_probe : Indicates about the kernel or userspace probe + */ +struct cam_eeprom_query_cap_t { + uint32_t slot_info; + uint16_t eeprom_kernel_probe; + uint16_t reserved; +} __attribute__((packed)); + +/** + * struct cam_ois_query_cap_t - capabilities info for ois + * + * @slot_info : Indicates about the slotId or cell Index + */ +struct cam_ois_query_cap_t { + uint32_t slot_info; + uint16_t reserved; +} __attribute__((packed)); + +/** + * struct cam_cmd_i2c_info - Contains slave I2C related info + * + * @slave_addr : Slave address + * @i2c_freq_mode : 4 bits are used for I2c freq mode + * @cmd_type : Explains type of command + */ +struct cam_cmd_i2c_info { + uint16_t slave_addr; + uint8_t i2c_freq_mode; + uint8_t cmd_type; +} __attribute__((packed)); + +/** + * struct cam_ois_opcode - Contains OIS opcode + * + * @prog : OIS FW prog register address + * @coeff : OIS FW coeff register address + * @pheripheral : OIS pheripheral + * @memory : OIS memory + */ +struct cam_ois_opcode { + uint32_t prog; + uint32_t coeff; + uint32_t pheripheral; + uint32_t memory; +} __attribute__((packed)); + +/** + * struct cam_cmd_ois_info - Contains OIS slave info + * + * @slave_addr : OIS i2c slave address + * @i2c_freq_mode : i2c frequency mode + * @cmd_type : Explains type of command + * @ois_fw_flag : indicates if fw is present or not + * @is_ois_calib : indicates the calibration data is available + * @ois_name : OIS name + * @opcode : opcode + */ +struct cam_cmd_ois_info { + uint16_t slave_addr; + uint8_t i2c_freq_mode; + uint8_t cmd_type; + uint8_t ois_fw_flag; + uint8_t is_ois_calib; + char ois_name[MAX_OIS_NAME_SIZE]; + struct cam_ois_opcode opcode; +} __attribute__((packed)); + +/** + * struct cam_cmd_probe - Contains sensor slave info + * + * @data_type : Slave register data type + * @addr_type : Slave register address type + * @op_code : Don't Care + * @cmd_type : Explains type of command + * @reg_addr : Slave register address + * @expected_data : Data expected at slave register address + * @data_mask : Data mask if only few bits are valid + * @camera_id : Indicates the slot to which camera + * needs to be probed + * @reserved + */ +struct cam_cmd_probe { + uint8_t data_type; + uint8_t addr_type; + uint8_t op_code; + uint8_t cmd_type; + uint32_t reg_addr; + uint32_t expected_data; + uint32_t data_mask; + uint16_t camera_id; + uint16_t reserved; +} __attribute__((packed)); + +/** + * struct cam_power_settings - Contains sensor power setting info + * + * @power_seq_type : Type of power sequence + * @reserved + * @config_val_low : Lower 32 bit value configuration value + * @config_val_high : Higher 32 bit value configuration value + * + */ +struct cam_power_settings { + uint16_t power_seq_type; + uint16_t reserved; + uint32_t config_val_low; + uint32_t config_val_high; +} __attribute__((packed)); + +/** + * struct cam_cmd_power - Explains about the power settings + * + * @count : Number of power settings follows + * @reserved + * @cmd_type : Explains type of command + * @power_settings : Contains power setting info + */ +struct cam_cmd_power { + uint16_t count; + uint8_t reserved; + uint8_t cmd_type; + struct cam_power_settings power_settings[1]; +} __attribute__((packed)); + +/** + * struct i2c_rdwr_header - header of READ/WRITE I2C command + * + * @ count : Number of registers / data / reg-data pairs + * @ op_code : Operation code + * @ cmd_type : Command buffer type + * @ data_type : I2C data type + * @ addr_type : I2C address type + * @ reserved + */ +struct i2c_rdwr_header { + uint16_t count; + uint8_t op_code; + uint8_t cmd_type; + uint8_t data_type; + uint8_t addr_type; + uint16_t reserved; +} __attribute__((packed)); + +/** + * struct i2c_random_wr_payload - payload for I2C random write + * + * @ reg_addr : Register address + * @ reg_data : Register data + * + */ +struct i2c_random_wr_payload { + uint32_t reg_addr; + uint32_t reg_data; +} __attribute__((packed)); + +/** + * struct cam_cmd_i2c_random_wr - I2C random write command + * @ header : header of READ/WRITE I2C command + * @ random_wr_payload : payload for I2C random write + */ +struct cam_cmd_i2c_random_wr { + struct i2c_rdwr_header header; + struct i2c_random_wr_payload random_wr_payload[1]; +} __attribute__((packed)); + +/** + * struct cam_cmd_read - I2C read command + * @ reg_data : Register data + * @ reserved + */ +struct cam_cmd_read { + uint32_t reg_data; + uint32_t reserved; +} __attribute__((packed)); + +/** + * struct cam_cmd_i2c_continuous_wr - I2C continuous write command + * @ header : header of READ/WRITE I2C command + * @ reg_addr : Register address + * @ data_read : I2C read command + */ +struct cam_cmd_i2c_continuous_wr { + struct i2c_rdwr_header header; + uint32_t reg_addr; + struct cam_cmd_read data_read[1]; +} __attribute__((packed)); + +/** + * struct cam_cmd_i2c_random_rd - I2C random read command + * @ header : header of READ/WRITE I2C command + * @ data_read : I2C read command + */ +struct cam_cmd_i2c_random_rd { + struct i2c_rdwr_header header; + struct cam_cmd_read data_read[1]; +} __attribute__((packed)); + +/** + * struct cam_cmd_i2c_continuous_rd - I2C continuous continuous read command + * @ header : header of READ/WRITE I2C command + * @ reg_addr : Register address + * + */ +struct cam_cmd_i2c_continuous_rd { + struct i2c_rdwr_header header; + uint32_t reg_addr; +} __attribute__((packed)); + +/** + * struct cam_cmd_conditional_wait - Conditional wait command + * @data_type : Data type + * @addr_type : Address type + * @op_code : Opcode + * @cmd_type : Explains type of command + * @timeout : Timeout for retries + * @reserved + * @reg_addr : Register Address + * @reg_data : Register data + * @data_mask : Data mask if only few bits are valid + * @camera_id : Indicates the slot to which camera + * needs to be probed + * + */ +struct cam_cmd_conditional_wait { + uint8_t data_type; + uint8_t addr_type; + uint8_t op_code; + uint8_t cmd_type; + uint16_t timeout; + uint16_t reserved; + uint32_t reg_addr; + uint32_t reg_data; + uint32_t data_mask; +} __attribute__((packed)); + +/** + * struct cam_cmd_unconditional_wait - Un-conditional wait command + * @delay : Delay + * @op_code : Opcode + * @cmd_type : Explains type of command + */ +struct cam_cmd_unconditional_wait { + int16_t delay; + uint8_t op_code; + uint8_t cmd_type; +} __attribute__((packed)); + +/** + * cam_csiphy_info: Provides cmdbuffer structre + * @lane_mask : Lane mask details + * @lane_assign : Lane sensor will be using + * @csiphy_3phase : Total number of lanes + * @combo_mode : Info regarding combo_mode is enable / disable + * @lane_cnt : Total number of lanes + * @secure_mode : Secure mode flag to enable / disable + * @3phase : Details whether 3Phase / 2Phase operation + * @settle_time : Settling time in ms + * @data_rate : Data rate + * + */ +struct cam_csiphy_info { + uint16_t lane_mask; + uint16_t lane_assign; + uint8_t csiphy_3phase; + uint8_t combo_mode; + uint8_t lane_cnt; + uint8_t secure_mode; + uint64_t settle_time; + uint64_t data_rate; +} __attribute__((packed)); + +/** + * cam_csiphy_acquire_dev_info : Information needed for + * csiphy at the time of acquire + * @combo_mode : Indicates the device mode of operation + * @reserved + * + */ +struct cam_csiphy_acquire_dev_info { + uint32_t combo_mode; + uint32_t reserved; +} __attribute__((packed)); + +/** + * cam_sensor_acquire_dev : Updates sensor acuire cmd + * @device_handle : Updates device handle + * @session_handle : Session handle for acquiring device + * @handle_type : Resource handle type + * @reserved + * @info_handle : Handle to additional info + * needed for sensor sub modules + * + */ +struct cam_sensor_acquire_dev { + uint32_t session_handle; + uint32_t device_handle; + uint32_t handle_type; + uint32_t reserved; + uint64_t info_handle; +} __attribute__((packed)); + +/** + * cam_sensor_streamon_dev : StreamOn command for the sensor + * @session_handle : Session handle for acquiring device + * @device_handle : Updates device handle + * @handle_type : Resource handle type + * @reserved + * @info_handle : Information Needed at the time of streamOn + * + */ +struct cam_sensor_streamon_dev { + uint32_t session_handle; + uint32_t device_handle; + uint32_t handle_type; + uint32_t reserved; + uint64_t info_handle; +} __attribute__((packed)); + +/** + * struct cam_flash_init : Init command for the flash + * @flash_type : flash hw type + * @reserved + * @cmd_type : command buffer type + */ +struct cam_flash_init { + uint8_t flash_type; + uint16_t reserved; + uint8_t cmd_type; +} __attribute__((packed)); + +/** + * struct cam_flash_set_rer : RedEyeReduction command buffer + * + * @count : Number of flash leds + * @opcode : Command buffer opcode + * CAM_FLASH_FIRE_RER + * @cmd_type : command buffer operation type + * @num_iteration : Number of led turn on/off sequence + * @reserved + * @led_on_delay_ms : flash led turn on time in ms + * @led_off_delay_ms : flash led turn off time in ms + * @led_current_ma : flash led current in ma + * + */ +struct cam_flash_set_rer { + uint16_t count; + uint8_t opcode; + uint8_t cmd_type; + uint16_t num_iteration; + uint16_t reserved; + uint32_t led_on_delay_ms; + uint32_t led_off_delay_ms; + uint32_t led_current_ma[CAM_FLASH_MAX_LED_TRIGGERS]; +} __attribute__((packed)); + +/** + * struct cam_flash_set_on_off : led turn on/off command buffer + * + * @count : Number of Flash leds + * @opcode : command buffer opcodes + * CAM_FLASH_FIRE_LOW + * CAM_FLASH_FIRE_HIGH + * CAM_FLASH_OFF + * @cmd_type : command buffer operation type + * @led_current_ma : flash led current in ma + * + */ +struct cam_flash_set_on_off { + uint16_t count; + uint8_t opcode; + uint8_t cmd_type; + uint32_t led_current_ma[CAM_FLASH_MAX_LED_TRIGGERS]; +} __attribute__((packed)); + +/** + * struct cam_flash_query_curr : query current command buffer + * + * @reserved + * @opcode : command buffer opcode + * @cmd_type : command buffer operation type + * @query_current_ma : battery current in ma + * + */ +struct cam_flash_query_curr { + uint16_t reserved; + uint8_t opcode; + uint8_t cmd_type; + uint32_t query_current_ma; +} __attribute__ ((packed)); + +/** + * struct cam_flash_query_cap : capabilities info for flash + * + * @slot_info : Indicates about the slotId or cell Index + * @max_current_flash : max supported current for flash + * @max_duration_flash : max flash turn on duration + * @max_current_torch : max supported current for torch + * + */ +struct cam_flash_query_cap_info { + uint32_t slot_info; + uint32_t max_current_flash[CAM_FLASH_MAX_LED_TRIGGERS]; + uint32_t max_duration_flash[CAM_FLASH_MAX_LED_TRIGGERS]; + uint32_t max_current_torch[CAM_FLASH_MAX_LED_TRIGGERS]; +} __attribute__ ((packed)); + +#endif diff --git a/selfdrive/camerad/include/media/cam_sensor_cmn_header.h b/selfdrive/camerad/include/media/cam_sensor_cmn_header.h new file mode 100644 index 00000000..24617b39 --- /dev/null +++ b/selfdrive/camerad/include/media/cam_sensor_cmn_header.h @@ -0,0 +1,391 @@ +/* Copyright (c) 2017-2018, The Linux Foundation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 and + * only version 2 as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#ifndef _CAM_SENSOR_CMN_HEADER_ +#define _CAM_SENSOR_CMN_HEADER_ + +#include +#include +#include + +#define MAX_REGULATOR 5 +#define MAX_POWER_CONFIG 12 + +#define MAX_PER_FRAME_ARRAY 32 +#define BATCH_SIZE_MAX 16 + +#define CAM_SENSOR_NAME "cam-sensor" +#define CAM_ACTUATOR_NAME "cam-actuator" +#define CAM_CSIPHY_NAME "cam-csiphy" +#define CAM_FLASH_NAME "cam-flash" +#define CAM_EEPROM_NAME "cam-eeprom" +#define CAM_OIS_NAME "cam-ois" + +#define MAX_SYSTEM_PIPELINE_DELAY 2 + +#define CAM_PKT_NOP_OPCODE 127 + +enum camera_sensor_cmd_type { + CAMERA_SENSOR_CMD_TYPE_INVALID, + CAMERA_SENSOR_CMD_TYPE_PROBE, + CAMERA_SENSOR_CMD_TYPE_PWR_UP, + CAMERA_SENSOR_CMD_TYPE_PWR_DOWN, + CAMERA_SENSOR_CMD_TYPE_I2C_INFO, + CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR, + CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_RD, + CAMERA_SENSOR_CMD_TYPE_I2C_CONT_WR, + CAMERA_SENSOR_CMD_TYPE_I2C_CONT_RD, + CAMERA_SENSOR_CMD_TYPE_WAIT, + CAMERA_SENSOR_FLASH_CMD_TYPE_INIT_INFO, + CAMERA_SENSOR_FLASH_CMD_TYPE_FIRE, + CAMERA_SENSOR_FLASH_CMD_TYPE_RER, + CAMERA_SENSOR_FLASH_CMD_TYPE_QUERYCURR, + CAMERA_SENSOR_FLASH_CMD_TYPE_WIDGET, + CAMERA_SENSOR_CMD_TYPE_RD_DATA, + CAMERA_SENSOR_FLASH_CMD_TYPE_INIT_FIRE, + CAMERA_SENSOR_CMD_TYPE_MAX, +}; + +enum camera_sensor_i2c_op_code { + CAMERA_SENSOR_I2C_OP_INVALID, + CAMERA_SENSOR_I2C_OP_RNDM_WR, + CAMERA_SENSOR_I2C_OP_RNDM_WR_VERF, + CAMERA_SENSOR_I2C_OP_CONT_WR_BRST, + CAMERA_SENSOR_I2C_OP_CONT_WR_BRST_VERF, + CAMERA_SENSOR_I2C_OP_CONT_WR_SEQN, + CAMERA_SENSOR_I2C_OP_CONT_WR_SEQN_VERF, + CAMERA_SENSOR_I2C_OP_MAX, +}; + +enum camera_sensor_wait_op_code { + CAMERA_SENSOR_WAIT_OP_INVALID, + CAMERA_SENSOR_WAIT_OP_COND, + CAMERA_SENSOR_WAIT_OP_HW_UCND, + CAMERA_SENSOR_WAIT_OP_SW_UCND, + CAMERA_SENSOR_WAIT_OP_MAX, +}; + +enum camera_flash_opcode { + CAMERA_SENSOR_FLASH_OP_INVALID, + CAMERA_SENSOR_FLASH_OP_OFF, + CAMERA_SENSOR_FLASH_OP_FIRELOW, + CAMERA_SENSOR_FLASH_OP_FIREHIGH, + CAMERA_SENSOR_FLASH_OP_MAX, +}; + +enum camera_sensor_i2c_type { + CAMERA_SENSOR_I2C_TYPE_INVALID, + CAMERA_SENSOR_I2C_TYPE_BYTE, + CAMERA_SENSOR_I2C_TYPE_WORD, + CAMERA_SENSOR_I2C_TYPE_3B, + CAMERA_SENSOR_I2C_TYPE_DWORD, + CAMERA_SENSOR_I2C_TYPE_MAX, +}; + +enum i2c_freq_mode { + I2C_STANDARD_MODE, + I2C_FAST_MODE, + I2C_CUSTOM_MODE, + I2C_FAST_PLUS_MODE, + I2C_MAX_MODES, +}; + +enum position_roll { + ROLL_0 = 0, + ROLL_90 = 90, + ROLL_180 = 180, + ROLL_270 = 270, + ROLL_INVALID = 360, +}; + +enum position_yaw { + FRONT_CAMERA_YAW = 0, + REAR_CAMERA_YAW = 180, + INVALID_YAW = 360, +}; + +enum position_pitch { + LEVEL_PITCH = 0, + INVALID_PITCH = 360, +}; + +enum sensor_sub_module { + SUB_MODULE_SENSOR, + SUB_MODULE_ACTUATOR, + SUB_MODULE_EEPROM, + SUB_MODULE_LED_FLASH, + SUB_MODULE_CSID, + SUB_MODULE_CSIPHY, + SUB_MODULE_OIS, + SUB_MODULE_EXT, + SUB_MODULE_MAX, +}; + +enum msm_camera_power_seq_type { + SENSOR_MCLK, + SENSOR_VANA, + SENSOR_VDIG, + SENSOR_VIO, + SENSOR_VAF, + SENSOR_VAF_PWDM, + SENSOR_CUSTOM_REG1, + SENSOR_CUSTOM_REG2, + SENSOR_RESET, + SENSOR_STANDBY, + SENSOR_CUSTOM_GPIO1, + SENSOR_CUSTOM_GPIO2, + SENSOR_SEQ_TYPE_MAX, +}; + +enum cam_sensor_packet_opcodes { + CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON, + CAM_SENSOR_PACKET_OPCODE_SENSOR_UPDATE, + CAM_SENSOR_PACKET_OPCODE_SENSOR_INITIAL_CONFIG, + CAM_SENSOR_PACKET_OPCODE_SENSOR_PROBE, + CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, + CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF, + CAM_SENSOR_PACKET_OPCODE_SENSOR_NOP = 127 +}; + +enum cam_actuator_packet_opcodes { + CAM_ACTUATOR_PACKET_OPCODE_INIT, + CAM_ACTUATOR_PACKET_AUTO_MOVE_LENS, + CAM_ACTUATOR_PACKET_MANUAL_MOVE_LENS +}; + +enum cam_eeprom_packet_opcodes { + CAM_EEPROM_PACKET_OPCODE_INIT +}; + +enum cam_ois_packet_opcodes { + CAM_OIS_PACKET_OPCODE_INIT, + CAM_OIS_PACKET_OPCODE_OIS_CONTROL +}; + +enum msm_bus_perf_setting { + S_INIT, + S_PREVIEW, + S_VIDEO, + S_CAPTURE, + S_ZSL, + S_STEREO_VIDEO, + S_STEREO_CAPTURE, + S_DEFAULT, + S_LIVESHOT, + S_DUAL, + S_EXIT +}; + +enum msm_camera_device_type_t { + MSM_CAMERA_I2C_DEVICE, + MSM_CAMERA_PLATFORM_DEVICE, + MSM_CAMERA_SPI_DEVICE, +}; + +enum cam_flash_device_type { + CAMERA_FLASH_DEVICE_TYPE_PMIC = 0, + CAMERA_FLASH_DEVICE_TYPE_I2C, + CAMERA_FLASH_DEVICE_TYPE_GPIO, +}; + +enum cci_i2c_master_t { + MASTER_0, + MASTER_1, + MASTER_MAX, +}; + +enum camera_vreg_type { + VREG_TYPE_DEFAULT, + VREG_TYPE_CUSTOM, +}; + +enum cam_sensor_i2c_cmd_type { + CAM_SENSOR_I2C_WRITE_RANDOM, + CAM_SENSOR_I2C_WRITE_BURST, + CAM_SENSOR_I2C_WRITE_SEQ, + CAM_SENSOR_I2C_READ, + CAM_SENSOR_I2C_POLL +}; + +struct common_header { + uint16_t first_word; + uint8_t third_byte; + uint8_t cmd_type; +}; + +struct camera_vreg_t { + const char *reg_name; + int min_voltage; + int max_voltage; + int op_mode; + uint32_t delay; + const char *custom_vreg_name; + enum camera_vreg_type type; +}; + +struct msm_camera_gpio_num_info { + uint16_t gpio_num[SENSOR_SEQ_TYPE_MAX]; + uint8_t valid[SENSOR_SEQ_TYPE_MAX]; +}; + +struct msm_cam_clk_info { + const char *clk_name; + long clk_rate; + uint32_t delay; +}; + +struct msm_pinctrl_info { + struct pinctrl *pinctrl; + struct pinctrl_state *gpio_state_active; + struct pinctrl_state *gpio_state_suspend; + bool use_pinctrl; +}; + +struct cam_sensor_i2c_reg_array { + uint32_t reg_addr; + uint32_t reg_data; + uint32_t delay; + uint32_t data_mask; +}; + +struct cam_sensor_i2c_reg_setting { + struct cam_sensor_i2c_reg_array *reg_setting; + unsigned short size; + enum camera_sensor_i2c_type addr_type; + enum camera_sensor_i2c_type data_type; + unsigned short delay; +}; + +/*struct i2c_settings_list { + struct cam_sensor_i2c_reg_setting i2c_settings; + enum cam_sensor_i2c_cmd_type op_code; + struct list_head list; +}; + +struct i2c_settings_array { + struct list_head list_head; + int32_t is_settings_valid; + int64_t request_id; +}; + +struct i2c_data_settings { + struct i2c_settings_array init_settings; + struct i2c_settings_array config_settings; + struct i2c_settings_array streamon_settings; + struct i2c_settings_array streamoff_settings; + struct i2c_settings_array *per_frame; +};*/ + +struct cam_sensor_power_ctrl_t { + struct device *dev; + struct cam_sensor_power_setting *power_setting; + uint16_t power_setting_size; + struct cam_sensor_power_setting *power_down_setting; + uint16_t power_down_setting_size; + struct msm_camera_gpio_num_info *gpio_num_info; + struct msm_pinctrl_info pinctrl_info; + uint8_t cam_pinctrl_status; +}; + +struct cam_camera_slave_info { + uint16_t sensor_slave_addr; + uint16_t sensor_id_reg_addr; + uint16_t sensor_id; + uint16_t sensor_id_mask; +}; + +struct msm_sensor_init_params { + int modes_supported; + unsigned int sensor_mount_angle; +}; + +enum msm_sensor_camera_id_t { + CAMERA_0, + CAMERA_1, + CAMERA_2, + CAMERA_3, + CAMERA_4, + CAMERA_5, + CAMERA_6, + MAX_CAMERAS, +}; + +struct msm_sensor_id_info_t { + unsigned short sensor_id_reg_addr; + unsigned short sensor_id; + unsigned short sensor_id_mask; +}; + +enum msm_sensor_output_format_t { + MSM_SENSOR_BAYER, + MSM_SENSOR_YCBCR, + MSM_SENSOR_META, +}; + +struct cam_sensor_power_setting { + enum msm_camera_power_seq_type seq_type; + unsigned short seq_val; + long config_val; + unsigned short delay; + void *data[10]; +}; + +struct cam_sensor_board_info { + struct cam_camera_slave_info slave_info; + int32_t sensor_mount_angle; + int32_t secure_mode; + int modes_supported; + int32_t pos_roll; + int32_t pos_yaw; + int32_t pos_pitch; + int32_t subdev_id[SUB_MODULE_MAX]; + int32_t subdev_intf[SUB_MODULE_MAX]; + const char *misc_regulator; + struct cam_sensor_power_ctrl_t power_info; +}; + +enum msm_camera_vreg_name_t { + CAM_VDIG, + CAM_VIO, + CAM_VANA, + CAM_VAF, + CAM_V_CUSTOM1, + CAM_V_CUSTOM2, + CAM_VREG_MAX, +}; + +struct msm_camera_gpio_conf { + void *cam_gpiomux_conf_tbl; + uint8_t cam_gpiomux_conf_tbl_size; + struct gpio *cam_gpio_common_tbl; + uint8_t cam_gpio_common_tbl_size; + struct gpio *cam_gpio_req_tbl; + uint8_t cam_gpio_req_tbl_size; + uint32_t gpio_no_mux; + uint32_t *camera_off_table; + uint8_t camera_off_table_size; + uint32_t *camera_on_table; + uint8_t camera_on_table_size; + struct msm_camera_gpio_num_info *gpio_num_info; +}; + +/*for tof camera Begin*/ +enum EEPROM_DATA_OP_T{ + EEPROM_DEFAULT_DATA = 0, + EEPROM_INIT_DATA, + EEPROM_CONFIG_DATA, + EEPROM_STREAMON_DATA, + EEPROM_STREAMOFF_DATA, + EEPROM_OTHER_DATA, +}; +/*for tof camera End*/ +#endif /* _CAM_SENSOR_CMN_HEADER_ */ diff --git a/selfdrive/camerad/include/media/cam_sync.h b/selfdrive/camerad/include/media/cam_sync.h new file mode 100644 index 00000000..4a8781fc --- /dev/null +++ b/selfdrive/camerad/include/media/cam_sync.h @@ -0,0 +1,134 @@ +#ifndef __UAPI_CAM_SYNC_H__ +#define __UAPI_CAM_SYNC_H__ + +#include +#include +#include +#include + +#define CAM_SYNC_DEVICE_NAME "cam_sync_device" + +/* V4L event which user space will subscribe to */ +#define CAM_SYNC_V4L_EVENT (V4L2_EVENT_PRIVATE_START + 0) + +/* Specific event ids to get notified in user space */ +#define CAM_SYNC_V4L_EVENT_ID_CB_TRIG 0 + +/* Size of opaque payload sent to kernel for safekeeping until signal time */ +#define CAM_SYNC_USER_PAYLOAD_SIZE 2 + +/* Device type for sync device needed for device discovery */ +#define CAM_SYNC_DEVICE_TYPE (MEDIA_ENT_F_OLD_BASE) + +#define CAM_SYNC_GET_PAYLOAD_PTR(ev, type) \ + (type *)((char *)ev.u.data + sizeof(struct cam_sync_ev_header)) + +#define CAM_SYNC_GET_HEADER_PTR(ev) \ + ((struct cam_sync_ev_header *)ev.u.data) + +#define CAM_SYNC_STATE_INVALID 0 +#define CAM_SYNC_STATE_ACTIVE 1 +#define CAM_SYNC_STATE_SIGNALED_SUCCESS 2 +#define CAM_SYNC_STATE_SIGNALED_ERROR 3 + +/** + * struct cam_sync_ev_header - Event header for sync event notification + * + * @sync_obj: Sync object + * @status: Status of the object + */ +struct cam_sync_ev_header { + int32_t sync_obj; + int32_t status; +}; + +/** + * struct cam_sync_info - Sync object creation information + * + * @name: Optional string representation of the sync object + * @sync_obj: Sync object returned after creation in kernel + */ +struct cam_sync_info { + char name[64]; + int32_t sync_obj; +}; + +/** + * struct cam_sync_signal - Sync object signaling struct + * + * @sync_obj: Sync object to be signaled + * @sync_state: State of the sync object to which it should be signaled + */ +struct cam_sync_signal { + int32_t sync_obj; + uint32_t sync_state; +}; + +/** + * struct cam_sync_merge - Merge information for sync objects + * + * @sync_objs: Pointer to sync objects + * @num_objs: Number of objects in the array + * @merged: Merged sync object + */ +struct cam_sync_merge { + __u64 sync_objs; + uint32_t num_objs; + int32_t merged; +}; + +/** + * struct cam_sync_userpayload_info - Payload info from user space + * + * @sync_obj: Sync object for which payload has to be registered for + * @reserved: Reserved + * @payload: Pointer to user payload + */ +struct cam_sync_userpayload_info { + int32_t sync_obj; + uint32_t reserved; + __u64 payload[CAM_SYNC_USER_PAYLOAD_SIZE]; +}; + +/** + * struct cam_sync_wait - Sync object wait information + * + * @sync_obj: Sync object to wait on + * @reserved: Reserved + * @timeout_ms: Timeout in milliseconds + */ +struct cam_sync_wait { + int32_t sync_obj; + uint32_t reserved; + uint64_t timeout_ms; +}; + +/** + * struct cam_private_ioctl_arg - Sync driver ioctl argument + * + * @id: IOCTL command id + * @size: Size of command payload + * @result: Result of command execution + * @reserved: Reserved + * @ioctl_ptr: Pointer to user data + */ +struct cam_private_ioctl_arg { + __u32 id; + __u32 size; + __u32 result; + __u32 reserved; + __u64 ioctl_ptr; +}; + +#define CAM_PRIVATE_IOCTL_CMD \ + _IOWR('V', BASE_VIDIOC_PRIVATE, struct cam_private_ioctl_arg) + +#define CAM_SYNC_CREATE 0 +#define CAM_SYNC_DESTROY 1 +#define CAM_SYNC_SIGNAL 2 +#define CAM_SYNC_MERGE 3 +#define CAM_SYNC_REGISTER_PAYLOAD 4 +#define CAM_SYNC_DEREGISTER_PAYLOAD 5 +#define CAM_SYNC_WAIT 6 + +#endif /* __UAPI_CAM_SYNC_H__ */ diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index 15611681..28697801 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -4,6 +4,10 @@ #ifdef QCOM #include "cameras/camera_qcom.h" +#elif QCOM2 +#include "cameras/camera_qcom2.h" +#elif WEBCAM +#include "cameras/camera_webcam.h" #else #include "cameras/camera_frame_stream.h" #endif @@ -18,6 +22,7 @@ #include "messaging.hpp" #include "transforms/rgb_to_yuv.h" +#include "imgproc/utils.h" #include "clutil.h" #include "bufs.h" @@ -73,6 +78,14 @@ struct VisionState { cl_kernel krnl_debayer_rear; cl_kernel krnl_debayer_front; + cl_program prg_rgb_laplacian; + cl_kernel krnl_rgb_laplacian; + + int conv_cl_localMemSize; + size_t conv_cl_globalWorkSize[2]; + size_t conv_cl_localWorkSize[2]; + size_t pool_cl_globalWorkSize[2]; + // processing TBuffer ui_tb; TBuffer ui_front_tb; @@ -105,11 +118,15 @@ struct VisionState { int rgb_width, rgb_height, rgb_stride; VisionBuf rgb_bufs[UI_BUF_COUNT]; cl_mem rgb_bufs_cl[UI_BUF_COUNT]; + cl_mem rgb_conv_roi_cl, rgb_conv_result_cl, rgb_conv_filter_cl; + uint16_t lapres[(ROI_X_MAX-ROI_X_MIN+1)*(ROI_Y_MAX-ROI_Y_MIN+1)]; size_t rgb_front_buf_size; int rgb_front_width, rgb_front_height, rgb_front_stride; VisionBuf rgb_front_bufs[UI_BUF_COUNT]; cl_mem rgb_front_bufs_cl[UI_BUF_COUNT]; + bool rhd_front; + bool rhd_front_checked; int front_meteringbox_xmin, front_meteringbox_xmax; int front_meteringbox_ymin, front_meteringbox_ymax; @@ -148,7 +165,9 @@ void* frontview_thread(void *arg) { // TODO: the loop is bad, ideally models shouldn't affect sensors Context *msg_context = Context::create(); SubSocket *monitoring_sock = SubSocket::create(msg_context, "driverState", "127.0.0.1", true); + SubSocket *dmonstate_sock = SubSocket::create(msg_context, "dMonitoringState", "127.0.0.1", true); assert(monitoring_sock != NULL); + assert(dmonstate_sock != NULL); cl_command_queue q = clCreateCommandQueue(s->context, s->device_id, 0, &err); assert(err == 0); @@ -160,32 +179,55 @@ void* frontview_thread(void *arg) { } int ui_idx = tbuffer_select(&s->ui_front_tb); + int rgb_idx = ui_idx; FrameMetadata frame_data = s->cameras.front.camera_bufs_metadata[buf_idx]; double t1 = millis_since_boot(); - err = clSetKernelArg(s->krnl_debayer_front, 0, sizeof(cl_mem), &s->front_camera_bufs_cl[buf_idx]); - assert(err == 0); - err = clSetKernelArg(s->krnl_debayer_front, 1, sizeof(cl_mem), &s->rgb_front_bufs_cl[ui_idx]); - assert(err == 0); - float digital_gain = 1.0; - err = clSetKernelArg(s->krnl_debayer_front, 2, sizeof(float), &digital_gain); - assert(err == 0); - cl_event debayer_event; - const size_t debayer_work_size = s->rgb_front_height; - const size_t debayer_local_work_size = 128; - err = clEnqueueNDRangeKernel(q, s->krnl_debayer_front, 1, NULL, - &debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event); - assert(err == 0); + if (s->cameras.front.ci.bayer) { + err = clSetKernelArg(s->krnl_debayer_front, 0, sizeof(cl_mem), &s->front_camera_bufs_cl[buf_idx]); + assert(err == 0); + err = clSetKernelArg(s->krnl_debayer_front, 1, sizeof(cl_mem), &s->rgb_front_bufs_cl[rgb_idx]); + assert(err == 0); + float digital_gain = 1.0; + err = clSetKernelArg(s->krnl_debayer_front, 2, sizeof(float), &digital_gain); + assert(err == 0); + + const size_t debayer_work_size = s->rgb_front_height; // doesn't divide evenly, is this okay? + const size_t debayer_local_work_size = 128; + err = clEnqueueNDRangeKernel(q, s->krnl_debayer_front, 1, NULL, + &debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event); + assert(err == 0); + } else { + assert(s->rgb_front_buf_size >= s->cameras.front.frame_size); + assert(s->rgb_front_stride == s->cameras.front.ci.frame_stride); + err = clEnqueueCopyBuffer(q, s->front_camera_bufs_cl[buf_idx], s->rgb_front_bufs_cl[rgb_idx], + 0, 0, s->rgb_front_buf_size, 0, 0, &debayer_event); + assert(err == 0); + } clWaitForEvents(1, &debayer_event); clReleaseEvent(debayer_event); - tbuffer_release(&s->cameras.front.camera_tb, buf_idx); - visionbuf_sync(&s->rgb_front_bufs[ui_idx], VISIONBUF_SYNC_FROM_DEVICE); - // set front camera metering target + // no more check after gps check + if (!s->rhd_front_checked) { + Message *msg_dmon = dmonstate_sock->receive(true); + if (msg_dmon != NULL) { + auto amsg = kj::heapArray((msg_dmon->getSize() / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), msg_dmon->getData(), msg_dmon->getSize()); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + + s->rhd_front = event.getDMonitoringState().getIsRHD(); + s->rhd_front_checked = event.getDMonitoringState().getRhdChecked(); + + delete msg_dmon; + } + } + Message *msg = monitoring_sock->receive(true); if (msg != NULL) { auto amsg = kj::heapArray((msg->getSize() / sizeof(capnp::word)) + 1); @@ -202,7 +244,7 @@ void* frontview_thread(void *arg) { // set front camera metering target if (face_prob > 0.4) { - int x_offset = s->rgb_front_width - 0.5 * s->rgb_front_height; + int x_offset = s->rhd_front ? 0:s->rgb_front_width - 0.5 * s->rgb_front_height; s->front_meteringbox_xmin = x_offset + (face_position[0] + 0.5) * (0.5 * s->rgb_front_height) - 72; s->front_meteringbox_xmax = x_offset + (face_position[0] + 0.5) * (0.5 * s->rgb_front_height) + 72; s->front_meteringbox_ymin = (face_position[1] + 0.5) * (s->rgb_front_height) - 72; @@ -212,8 +254,8 @@ void* frontview_thread(void *arg) { { s->front_meteringbox_ymin = s->rgb_front_height * 1 / 3; s->front_meteringbox_ymax = s->rgb_front_height * 1; - s->front_meteringbox_xmin = s->rgb_front_width * 3 / 5; - s->front_meteringbox_xmax = s->rgb_front_width; + s->front_meteringbox_xmin = s->rhd_front ? 0:s->rgb_front_width * 3 / 5; + s->front_meteringbox_xmax = s->rhd_front ? s->rgb_front_width * 2 / 5:s->rgb_front_width; } delete msg; @@ -242,8 +284,8 @@ void* frontview_thread(void *arg) { { y_start = s->rgb_front_height * 1 / 3; y_end = s->rgb_front_height * 1; - x_start = s->rgb_front_width * 3 / 5; - x_end = s->rgb_front_width; + x_start = s->rhd_front ? 0:s->rgb_front_width * 3 / 5; + x_end = s->rhd_front ? s->rgb_front_width * 2 / 5:s->rgb_front_width; } uint32_t lum_binning[256] = {0,}; @@ -288,27 +330,29 @@ void* frontview_thread(void *arg) { // send frame event { - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); + if (s->front_frame_sock != NULL) { + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); - auto framed = event.initFrontFrame(); - framed.setFrameId(frame_data.frame_id); - framed.setEncodeId(cnt); - framed.setTimestampEof(frame_data.timestamp_eof); - framed.setFrameLength(frame_data.frame_length); - framed.setIntegLines(frame_data.integ_lines); - framed.setGlobalGain(frame_data.global_gain); - framed.setLensPos(frame_data.lens_pos); - framed.setLensSag(frame_data.lens_sag); - framed.setLensErr(frame_data.lens_err); - framed.setLensTruePos(frame_data.lens_true_pos); - framed.setGainFrac(frame_data.gain_frac); - framed.setFrameType(cereal::FrameData::FrameType::FRONT); + auto framed = event.initFrontFrame(); + framed.setFrameId(frame_data.frame_id); + framed.setEncodeId(cnt); + framed.setTimestampEof(frame_data.timestamp_eof); + framed.setFrameLength(frame_data.frame_length); + framed.setIntegLines(frame_data.integ_lines); + framed.setGlobalGain(frame_data.global_gain); + framed.setLensPos(frame_data.lens_pos); + framed.setLensSag(frame_data.lens_sag); + framed.setLensErr(frame_data.lens_err); + framed.setLensTruePos(frame_data.lens_true_pos); + framed.setGainFrac(frame_data.gain_frac); + framed.setFrameType(cereal::FrameData::FrameType::FRONT); - auto words = capnp::messageToFlatArray(msg); - auto bytes = words.asBytes(); - s->front_frame_sock->send((char*)bytes.begin(), bytes.size()); + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + s->front_frame_sock->send((char*)bytes.begin(), bytes.size()); + } } /*FILE *f = fopen("/tmp/test2", "wb"); @@ -324,6 +368,7 @@ void* frontview_thread(void *arg) { } delete monitoring_sock; + delete dmonstate_sock; return NULL; } @@ -369,9 +414,9 @@ void* processing_thread(void *arg) { cl_event debayer_event; if (s->cameras.rear.ci.bayer) { err = clSetKernelArg(s->krnl_debayer_rear, 0, sizeof(cl_mem), &s->camera_bufs_cl[buf_idx]); - cl_check_error(err); + assert(err == 0); err = clSetKernelArg(s->krnl_debayer_rear, 1, sizeof(cl_mem), &s->rgb_bufs_cl[rgb_idx]); - cl_check_error(err); + assert(err == 0); err = clSetKernelArg(s->krnl_debayer_rear, 2, sizeof(float), &s->cameras.rear.digital_gain); assert(err == 0); @@ -395,6 +440,74 @@ void* processing_thread(void *arg) { visionbuf_sync(&s->rgb_bufs[rgb_idx], VISIONBUF_SYNC_FROM_DEVICE); +#ifdef QCOM + /*FILE *dump_rgb_file = fopen("/tmp/process_dump.rgb", "wb"); + fwrite(s->rgb_bufs[rgb_idx].addr, s->rgb_bufs[rgb_idx].len, sizeof(uint8_t), dump_rgb_file); + fclose(dump_rgb_file); + printf("ORIGINAL SAVED!!\n");*/ + + /*double t10 = millis_since_boot();*/ + + // cache rgb roi and write to cl + uint8_t *rgb_roi_buf = new uint8_t[(s->rgb_width/NUM_SEGMENTS_X)*(s->rgb_height/NUM_SEGMENTS_Y)*3]; + int roi_id = cnt % ((ROI_X_MAX-ROI_X_MIN+1)*(ROI_Y_MAX-ROI_Y_MIN+1)); // rolling roi + int roi_x_offset = roi_id % (ROI_X_MAX-ROI_X_MIN+1); + int roi_y_offset = roi_id / (ROI_X_MAX-ROI_X_MIN+1); + + for (int r=0;r<(s->rgb_height/NUM_SEGMENTS_Y);r++) { + memcpy(rgb_roi_buf + r * (s->rgb_width/NUM_SEGMENTS_X) * 3, + (uint8_t *) s->rgb_bufs[rgb_idx].addr + \ + (ROI_Y_MIN + roi_y_offset) * s->rgb_height/NUM_SEGMENTS_Y * FULL_STRIDE_X * 3 + \ + (ROI_X_MIN + roi_x_offset) * s->rgb_width/NUM_SEGMENTS_X * 3 + r * FULL_STRIDE_X * 3, + s->rgb_width/NUM_SEGMENTS_X * 3); + } + + err = clEnqueueWriteBuffer (q, s->rgb_conv_roi_cl, true, 0, + s->rgb_width/NUM_SEGMENTS_X * s->rgb_height/NUM_SEGMENTS_Y * 3 * sizeof(uint8_t), rgb_roi_buf, 0, 0, 0); + assert(err == 0); + + /*double t11 = millis_since_boot(); + printf("cache time: %f ms\n", t11 - t10); + t10 = millis_since_boot();*/ + + err = clSetKernelArg(s->krnl_rgb_laplacian, 0, sizeof(cl_mem), (void *) &s->rgb_conv_roi_cl); + assert(err == 0); + err = clSetKernelArg(s->krnl_rgb_laplacian, 1, sizeof(cl_mem), (void *) &s->rgb_conv_result_cl); + assert(err == 0); + err = clSetKernelArg(s->krnl_rgb_laplacian, 2, sizeof(cl_mem), (void *) &s->rgb_conv_filter_cl); + assert(err == 0); + err = clSetKernelArg(s->krnl_rgb_laplacian, 3, s->conv_cl_localMemSize, 0); + assert(err == 0); + + cl_event conv_event; + err = clEnqueueNDRangeKernel(q, s->krnl_rgb_laplacian, 2, NULL, + s->conv_cl_globalWorkSize, s->conv_cl_localWorkSize, 0, 0, &conv_event); + assert(err == 0); + clWaitForEvents(1, &conv_event); + clReleaseEvent(conv_event); + + int16_t *conv_result = new int16_t[(s->rgb_width/NUM_SEGMENTS_X)*(s->rgb_height/NUM_SEGMENTS_Y)]; + err = clEnqueueReadBuffer(q, s->rgb_conv_result_cl, true, 0, + s->rgb_width/NUM_SEGMENTS_X * s->rgb_height/NUM_SEGMENTS_Y * sizeof(int16_t), conv_result, 0, 0, 0); + assert(err == 0); + + /*t11 = millis_since_boot(); + printf("conv time: %f ms\n", t11 - t10); + t10 = millis_since_boot();*/ + + get_lapmap_one(conv_result, &s->lapres[roi_id], s->rgb_width/NUM_SEGMENTS_X, s->rgb_height/NUM_SEGMENTS_Y); + + /*t11 = millis_since_boot(); + printf("pool time: %f ms\n", t11 - t10); + t10 = millis_since_boot();*/ + + delete [] rgb_roi_buf; + delete [] conv_result; + + /*t11 = millis_since_boot(); + printf("process time: %f ms\n ----- \n", t11 - t10); + t10 = millis_since_boot();*/ +#endif double t2 = millis_since_boot(); @@ -421,43 +534,50 @@ void* processing_thread(void *arg) { // send frame event { - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); - - auto framed = event.initFrame(); - framed.setFrameId(frame_data.frame_id); - framed.setEncodeId(cnt); - framed.setTimestampEof(frame_data.timestamp_eof); - framed.setFrameLength(frame_data.frame_length); - framed.setIntegLines(frame_data.integ_lines); - framed.setGlobalGain(frame_data.global_gain); - framed.setLensPos(frame_data.lens_pos); - framed.setLensSag(frame_data.lens_sag); - framed.setLensErr(frame_data.lens_err); - framed.setLensTruePos(frame_data.lens_true_pos); - framed.setGainFrac(frame_data.gain_frac); -#ifdef QCOM - kj::ArrayPtr focus_vals(&s->cameras.rear.focus[0], NUM_FOCUS); - kj::ArrayPtr focus_confs(&s->cameras.rear.confidence[0], NUM_FOCUS); - framed.setFocusVal(focus_vals); - framed.setFocusConf(focus_confs); -#endif - -#ifndef QCOM - framed.setImage(kj::arrayPtr((const uint8_t*)s->yuv_ion[yuv_idx].addr, s->yuv_buf_size)); -#endif - - kj::ArrayPtr transform_vs(&s->yuv_transform.v[0], 9); - framed.setTransform(transform_vs); - if (s->frame_sock != NULL) { + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + auto framed = event.initFrame(); + framed.setFrameId(frame_data.frame_id); + framed.setEncodeId(cnt); + framed.setTimestampEof(frame_data.timestamp_eof); + framed.setFrameLength(frame_data.frame_length); + framed.setIntegLines(frame_data.integ_lines); + framed.setGlobalGain(frame_data.global_gain); + framed.setLensPos(frame_data.lens_pos); + framed.setLensSag(frame_data.lens_sag); + framed.setLensErr(frame_data.lens_err); + framed.setLensTruePos(frame_data.lens_true_pos); + framed.setGainFrac(frame_data.gain_frac); + +#ifdef QCOM + kj::ArrayPtr focus_vals(&s->cameras.rear.focus[0], NUM_FOCUS); + kj::ArrayPtr focus_confs(&s->cameras.rear.confidence[0], NUM_FOCUS); + framed.setFocusVal(focus_vals); + framed.setFocusConf(focus_confs); + kj::ArrayPtr sharpness_score(&s->lapres[0], (ROI_X_MAX-ROI_X_MIN+1)*(ROI_Y_MAX-ROI_Y_MIN+1)); + framed.setSharpnessScore(sharpness_score); +#endif + +// TODO: add this back +#if !defined(QCOM) && !defined(QCOM2) +//#ifndef QCOM + framed.setImage(kj::arrayPtr((const uint8_t*)s->yuv_ion[yuv_idx].addr, s->yuv_buf_size)); +#endif + + kj::ArrayPtr transform_vs(&s->yuv_transform.v[0], 9); + framed.setTransform(transform_vs); + auto words = capnp::messageToFlatArray(msg); auto bytes = words.asBytes(); s->frame_sock->send((char*)bytes.begin(), bytes.size()); } } +#ifndef QCOM2 + // TODO: fix on QCOM2, giving scanline error // one thumbnail per 5 seconds (instead of %5 == 0 posenet) if (cnt % 100 == 3) { uint8_t* thumbnail_buffer = NULL; @@ -521,6 +641,7 @@ void* processing_thread(void *arg) { free(thumbnail_buffer); } +#endif tbuffer_dispatch(&s->ui_tb, ui_idx); @@ -862,18 +983,52 @@ cl_program build_debayer_program(VisionState *s, assert(rgb_width == frame_width/2); assert(rgb_height == frame_height/2); + #ifdef QCOM2 + int dnew = 1; + #else + int dnew = 0; + #endif + char args[4096]; snprintf(args, sizeof(args), "-cl-fast-relaxed-math -cl-denorms-are-zero " "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d " "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d " - "-DBAYER_FLIP=%d -DHDR=%d", + "-DBAYER_FLIP=%d -DHDR=%d -DNEW=%d", frame_width, frame_height, frame_stride, rgb_width, rgb_height, rgb_stride, - bayer_flip, hdr); + bayer_flip, hdr, dnew); return CLU_LOAD_FROM_FILE(s->context, s->device_id, "cameras/debayer.cl", args); } +cl_program build_conv_program(VisionState *s, + int image_w, int image_h, + int filter_size) { + char args[4096]; + snprintf(args, sizeof(args), + "-cl-fast-relaxed-math -cl-denorms-are-zero " + "-DIMAGE_W=%d -DIMAGE_H=%d -DFLIP_RB=%d " + "-DFILTER_SIZE=%d -DHALF_FILTER_SIZE=%d -DTWICE_HALF_FILTER_SIZE=%d -DHALF_FILTER_SIZE_IMAGE_W=%d", + image_w, image_h, 1, + filter_size, filter_size/2, (filter_size/2)*2, (filter_size/2)*image_w); + return CLU_LOAD_FROM_FILE(s->context, s->device_id, "imgproc/conv.cl", args); +} + +cl_program build_pool_program(VisionState *s, + int full_stride_x, + int x_pitch, int y_pitch, + int roi_x_min, int roi_x_max, + int roi_y_min, int roi_y_max) { + char args[4096]; + snprintf(args, sizeof(args), + "-cl-fast-relaxed-math -cl-denorms-are-zero " + "-DFULL_STRIDE_X=%d -DX_PITCH=%d -DY_PITCH=%d " + "-DROI_X_MIN=%d -DROI_X_MAX=%d -DROI_Y_MIN=%d -DROI_Y_MAX=%d", + full_stride_x, x_pitch, y_pitch, + roi_x_min, roi_x_max, roi_y_min, roi_y_max); + return CLU_LOAD_FROM_FILE(s->context, s->device_id, "imgproc/pool.cl", args); +} + void cl_init(VisionState *s) { int err; cl_platform_id platform_id = NULL; @@ -908,9 +1063,11 @@ void init_buffers(VisionState *s) { for (int i=0; icamera_bufs[i] = visionbuf_allocate_cl(s->frame_size, s->device_id, s->context, &s->camera_bufs_cl[i]); - // TODO: make lengths correct - s->focus_bufs[i] = visionbuf_allocate(0xb80); - s->stats_bufs[i] = visionbuf_allocate(0xb80); + #ifndef QCOM2 + // TODO: make lengths correct + s->focus_bufs[i] = visionbuf_allocate(0xb80); + s->stats_bufs[i] = visionbuf_allocate(0xb80); + #endif } for (int i=0; iui_tb, UI_BUF_COUNT, "rgb"); //assert(s->cameras.front.ci.bayer); - s->rgb_front_width = s->cameras.front.ci.frame_width/2; - s->rgb_front_height = s->cameras.front.ci.frame_height/2; + if (s->cameras.front.ci.bayer) { + s->rgb_front_width = s->cameras.front.ci.frame_width/2; + s->rgb_front_height = s->cameras.front.ci.frame_height/2; + } else { + s->rgb_front_width = s->cameras.front.ci.frame_width; + s->rgb_front_height = s->cameras.front.ci.frame_height; + } + for (int i=0; irgb_front_width, s->rgb_front_height, &s->rgb_front_bufs[i]); @@ -1008,6 +1171,25 @@ void init_buffers(VisionState *s) { assert(err == 0); } + s->prg_rgb_laplacian = build_conv_program(s, s->rgb_width/NUM_SEGMENTS_X, s->rgb_height/NUM_SEGMENTS_Y, + 3); + s->krnl_rgb_laplacian = clCreateKernel(s->prg_rgb_laplacian, "rgb2gray_conv2d", &err); + assert(err == 0); + s->rgb_conv_roi_cl = clCreateBuffer(s->context, CL_MEM_READ_WRITE | CL_MEM_SVM_FINE_GRAIN_BUFFER, + s->rgb_width/NUM_SEGMENTS_X * s->rgb_height/NUM_SEGMENTS_Y * 3 * sizeof(uint8_t), NULL, NULL); + s->rgb_conv_result_cl = clCreateBuffer(s->context, CL_MEM_READ_WRITE | CL_MEM_SVM_FINE_GRAIN_BUFFER, + s->rgb_width/NUM_SEGMENTS_X * s->rgb_height/NUM_SEGMENTS_Y * sizeof(int16_t), NULL, NULL); + s->rgb_conv_filter_cl = clCreateBuffer(s->context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, + 9 * sizeof(int16_t), (void*)&lapl_conv_krnl, NULL); + s->conv_cl_localMemSize = ( CONV_LOCAL_WORKSIZE + 2 * (3 / 2) ) * ( CONV_LOCAL_WORKSIZE + 2 * (3 / 2) ); + s->conv_cl_localMemSize *= 3 * sizeof(uint8_t); + s->conv_cl_globalWorkSize[0] = s->rgb_width/NUM_SEGMENTS_X; + s->conv_cl_globalWorkSize[1] = s->rgb_height/NUM_SEGMENTS_Y; + s->conv_cl_localWorkSize[0] = CONV_LOCAL_WORKSIZE; + s->conv_cl_localWorkSize[1] = CONV_LOCAL_WORKSIZE; + + for (int i=0; i<(ROI_X_MAX-ROI_X_MIN+1)*(ROI_Y_MAX-ROI_Y_MIN+1); i++) {s->lapres[i] = 16160;} + rgb_to_yuv_init(&s->rgb_to_yuv_state, s->context, s->device_id, s->yuv_width, s->yuv_height, s->rgb_stride); rgb_to_yuv_init(&s->front_rgb_to_yuv_state, s->context, s->device_id, s->yuv_front_width, s->yuv_front_height, s->rgb_front_stride); } @@ -1053,10 +1235,13 @@ void party(VisionState *s) { processing_thread, s); assert(err == 0); +#ifndef QCOM2 + // TODO: fix front camera on qcom2 pthread_t frontview_thread_handle; err = pthread_create(&frontview_thread_handle, NULL, frontview_thread, s); assert(err == 0); +#endif // priority for cameras err = set_realtime_priority(1); @@ -1071,9 +1256,11 @@ void party(VisionState *s) { zsock_signal(s->terminate_pub, 0); +#ifndef QCOM2 LOG("joining frontview_thread"); err = pthread_join(frontview_thread_handle, NULL); assert(err == 0); +#endif LOG("joining visionserver_thread"); err = pthread_join(visionserver_thread_handle, NULL); @@ -1109,7 +1296,7 @@ int main(int argc, char *argv[]) { init_buffers(s); -#ifdef QCOM +#if defined(QCOM) || defined(QCOM2) s->msg_context = Context::create(); s->frame_sock = PubSocket::create(s->msg_context, "frame"); s->front_frame_sock = PubSocket::create(s->msg_context, "frontFrame"); @@ -1123,7 +1310,7 @@ int main(int argc, char *argv[]) { party(s); -#ifdef QCOM +#if defined(QCOM) || defined(QCOM2) delete s->frame_sock; delete s->front_frame_sock; delete s->thumbnail_sock; diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py index 0da8d679..4ba10bf4 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 2626e6ee..b9e66b36 100644 --- a/selfdrive/camerad/transforms/rgb_to_yuv.c +++ b/selfdrive/camerad/transforms/rgb_to_yuv.c @@ -8,6 +8,7 @@ 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; diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index e99d61e4..9c11feb6 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -27,12 +27,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('.', '/') + '/carcontroller.py'): - CarController = __import__(path + '.carcontroller', fromlist=['CarController']).CarController + + if os.path.exists(BASEDIR + '/' + path.replace('.', '/') + '/carstate.py'): CarState = __import__(path + '.carstate', fromlist=['CarState']).CarState else: - CarController = None CarState = None + + if os.path.exists(BASEDIR + '/' + path.replace('.', '/') + '/carcontroller.py'): + CarController = __import__(path + '.carcontroller', fromlist=['CarController']).CarController + else: + CarController = None + for model_name in brand_names[brand_name]: ret[model_name] = (CarInterface, CarController, CarState) return ret @@ -66,7 +71,9 @@ def only_toyota_left(candidate_cars): # **** for use live only **** def fingerprint(logcan, sendcan, has_relay): - if has_relay: + fixed_fingerprint = os.environ.get('FINGERPRINT', "") + + if has_relay and not fixed_fingerprint: # Vin query only reliably works thorugh OBDII bus = 1 @@ -141,8 +148,7 @@ def fingerprint(logcan, sendcan, has_relay): car_fingerprint = list(fw_candidates)[0] source = car.CarParams.FingerprintSource.fw - fixed_fingerprint = os.environ.get('FINGERPRINT', "") - if len(fixed_fingerprint): + if fixed_fingerprint: car_fingerprint = fixed_fingerprint source = car.CarParams.FingerprintSource.fixed diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 8c58d3db..c4db7303 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -6,7 +6,6 @@ from opendbc.can.packer import CANPacker class CarController(): def __init__(self, dbc_name, CP, VM): - self.braking = False self.apply_steer_last = 0 self.ccframe = 0 self.prev_frame = -1 @@ -51,7 +50,7 @@ class CarController(): if pcm_cancel_cmd: # TODO: would be better to start from frame_2b3 - new_msg = create_wheel_buttons(self.ccframe) + new_msg = create_wheel_buttons(self.packer, self.ccframe, cancel=True) can_sends.append(new_msg) # LKAS_HEARTBIT is forwarded by Panda so no need to send it here. diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 99e09f00..1a4b1e65 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -54,7 +54,7 @@ class CarState(CarStateBase): ret.steeringTorqueEps = cp.vl["EPS_STATUS"]["TORQUE_MOTOR"] ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD steer_state = cp.vl["EPS_STATUS"]["LKAS_STATE"] - self.steer_error = steer_state == 4 or (steer_state == 0 and ret.vEgo > self.CP.minSteerSpeed) + ret.steerError = steer_state == 4 or (steer_state == 0 and ret.vEgo > self.CP.minSteerSpeed) ret.genericToggle = bool(cp.vl["STEERING_LEVERS"]['HIGH_BEAM_FLASH']) diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py index c06c27e9..bf9f3731 100644 --- a/selfdrive/car/chrysler/chryslercan.py +++ b/selfdrive/car/chrysler/chryslercan.py @@ -5,34 +5,6 @@ from selfdrive.car import make_can_msg GearShifter = car.CarState.GearShifter VisualAlert = car.CarControl.HUDControl.VisualAlert -def calc_checksum(data): - """This function does not want the checksum byte in the input data. - - jeep chrysler canbus checksum from http://illmatics.com/Remote%20Car%20Hacking.pdf - """ - checksum = 0xFF - for curr in data[:-1]: - shift = 0x80 - for i in range(0, 8): - bit_sum = curr & shift - temp_chk = checksum & 0x80 - if (bit_sum != 0): - bit_sum = 0x1C - if (temp_chk != 0): - bit_sum = 1 - checksum = checksum << 1 - temp_chk = checksum | 1 - bit_sum ^= temp_chk - else: - if (temp_chk != 0): - bit_sum = 0x1D - checksum = checksum << 1 - bit_sum ^= checksum - checksum = bit_sum - shift = shift >> 1 - return ~checksum & 0xFF - - def create_lkas_hud(packer, gear, lkas_active, hud_alert, hud_count, lkas_car_model): # LKAS_HUD 0x2a6 (678) Controls what lane-keeping icon is displayed. @@ -73,18 +45,13 @@ def create_lkas_command(packer, apply_steer, moving_fast, frame): "LKAS_HIGH_TORQUE": int(moving_fast), "COUNTER": frame % 0x10, } - - dat = packer.make_can_msg("LKAS_COMMAND", 0, values)[2] - checksum = calc_checksum(dat) - - values["CHECKSUM"] = checksum return packer.make_can_msg("LKAS_COMMAND", 0, values) -def create_wheel_buttons(frame): +def create_wheel_buttons(packer, frame, cancel=False): # WHEEL_BUTTONS (571) Message sent to cancel ACC. - start = b"\x01" # acc cancel set - counter = (frame % 10) << 4 - dat = start + counter.to_bytes(1, 'little') + b"\x00" - dat = dat[:-1] + calc_checksum(dat).to_bytes(1, 'little') - return make_can_msg(0x23b, dat, 0) + values = { + "ACC_CANCEL": cancel, + "COUNTER": frame % 10 + } + return packer.make_can_msg("WHEEL_BUTTONS", 0, values) diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 428b7f21..3bc1db87 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -1,13 +1,12 @@ #!/usr/bin/env python3 from cereal import car -from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event from selfdrive.car.chrysler.values import Ecu, ECU_FINGERPRINT, CAR, FINGERPRINTS from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase -class CarInterface(CarInterfaceBase): +class CarInterface(CarInterfaceBase): @staticmethod def compute_gb(accel, speed): return float(accel) / 3.0 @@ -18,6 +17,9 @@ class CarInterface(CarInterfaceBase): ret.carName = "chrysler" ret.safetyModel = car.CarParams.SafetyModel.chrysler + # Chrysler port is a community feature, since we don't own one to test + ret.communityFeature = True + # Speed conversion: 20, 45 mph ret.wheelbase = 3.089 # in meters for Pacifica Hybrid 2017 ret.steerRatio = 16.2 # Pacifica Hybrid 2017 @@ -37,7 +39,7 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.44 ret.minSteerSpeed = 3.8 # m/s - if candidate in (CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE_2019): + if candidate in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019): # TODO allow 2019 cars to steer down to 13 m/s if already engaged. ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged. @@ -64,28 +66,18 @@ class CarInterface(CarInterfaceBase): ret.canValid = self.cp.can_valid and self.cp_cam.can_valid # speeds - ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo) ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False ret.buttonEvents = [] # events - events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low], gas_resume_speed = 2.) - - if ret.cruiseState.enabled and not self.cruise_enabled_prev: - events.append(create_event('pcmEnable', [ET.ENABLE])) - elif not ret.cruiseState.enabled: - events.append(create_event('pcmDisable', [ET.USER_DISABLE])) + events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low], gas_resume_speed=2.) if ret.vEgo < self.CP.minSteerSpeed: events.append(create_event('belowSteerSpeed', [ET.WARNING])) ret.events = events - self.gas_pressed_prev = ret.gasPressed - self.brake_pressed_prev = ret.brakePressed - self.cruise_enabled_prev = ret.cruiseState.enabled - # copy back carState packet to CS self.CS.out = ret.as_reader() diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py index 0713bf1d..40a7c5a7 100755 --- a/selfdrive/car/chrysler/radar_interface.py +++ b/selfdrive/car/chrysler/radar_interface.py @@ -48,8 +48,7 @@ def _address_to_track(address): class RadarInterface(RadarInterfaceBase): def __init__(self, CP): - self.pts = {} - self.delay = 0 # Delay of radar #TUNE + super().__init__(CP) self.rcp = _create_radar_can_parser() self.updated_messages = set() self.trigger_msg = LAST_MSG diff --git a/selfdrive/car/chrysler/test_chryslercan.py b/selfdrive/car/chrysler/test_chryslercan.py index d4d41057..5b15666c 100644 --- a/selfdrive/car/chrysler/test_chryslercan.py +++ b/selfdrive/car/chrysler/test_chryslercan.py @@ -11,10 +11,6 @@ GearShifter = car.CarState.GearShifter class TestChryslerCan(unittest.TestCase): - def test_checksum(self): - self.assertEqual(0x75, chryslercan.calc_checksum(b"\x01\x20\x00")) - self.assertEqual(0xcc, chryslercan.calc_checksum(b"\x14\x00\x00\x00\x20\x00")) - def test_hud(self): packer = CANPacker('chrysler_pacifica_2017_hybrid') self.assertEqual( diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index efbcc67b..ac1ede5f 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -16,6 +16,7 @@ class CAR: PACIFICA_2018_HYBRID = "CHRYSLER PACIFICA HYBRID 2018" PACIFICA_2019_HYBRID = "CHRYSLER PACIFICA HYBRID 2019" PACIFICA_2018 = "CHRYSLER PACIFICA 2018" # Also covers Pacifica 2017. + PACIFICA_2020 = "CHRYSLER PACIFICA 2020" JEEP_CHEROKEE = "JEEP GRAND CHEROKEE V6 2018" # Also covers Tailhawk 2017. JEEP_CHEROKEE_2019 = "JEEP GRAND CHEROKEE 2019" @@ -38,6 +39,11 @@ FINGERPRINTS = { {55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 8, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8}, {55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8}, ], + CAR.PACIFICA_2020: [ + { + 55: 8, 179: 8, 181: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 926: 3, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 7, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1284: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 2016: 8, 2024: 8 + } + ], CAR.PACIFICA_2018_HYBRID: [ {68: 8, 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8}, # based on 9ae7821dc4e92455|2019-07-01--16-42-55 @@ -68,9 +74,9 @@ FINGERPRINTS = { {257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 658: 6, 660: 8, 671: 8, 672: 8, 680: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 783: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8}, ], CAR.JEEP_CHEROKEE_2019: [ - # Jeep Grand Cherokee 2019 from Switzerland + # Jeep Grand Cherokee 2019 # 530: 8 is so far only in this Jeep. - {55: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 530: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 792: 8, 799: 8, 804: 8, 806: 2, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8}, + {55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 530: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1543: 8, 2015: 8, 2016: 8, 2024: 8}, ], } @@ -82,6 +88,9 @@ DBC = { CAR.PACIFICA_2018: dbc_dict( # Same DBC file works. 'chrysler_pacifica_2017_hybrid', # 'pt' 'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar' + CAR.PACIFICA_2020: dbc_dict( # Same DBC file works. + 'chrysler_pacifica_2017_hybrid', # 'pt' + 'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar' CAR.PACIFICA_2018_HYBRID: dbc_dict( # Same DBC file works. 'chrysler_pacifica_2017_hybrid', # 'pt' 'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar' diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 20341ac9..7f34acc9 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -19,6 +19,7 @@ class CarState(CarStateBase): ret.standstill = not ret.vEgoRaw > 0.001 ret.steeringAngle = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns'] ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"]['LaHandsOff_B_Actl'] + ret.steerError = cp.vl["Lane_Keep_Assist_Status"]['LaActDeny_B_Actl'] == 1 ret.cruiseState.speed = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS ret.cruiseState.enabled = not (cp.vl["Cruise_Status"]['Cruise_State'] in [0, 3]) ret.cruiseState.available = cp.vl["Cruise_Status"]['Cruise_State'] != 0 @@ -29,7 +30,6 @@ class CarState(CarStateBase): ret.genericToggle = bool(cp.vl["Steering_Buttons"]["Dist_Incr"]) # TODO: we also need raw driver torque, needed for Assisted Lane Change self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]['LaActAvail_D_Actl'] - self.steer_error = cp.vl["Lane_Keep_Assist_Status"]['LaActDeny_B_Actl'] return ret diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 6a599240..9ebaad36 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -61,23 +61,12 @@ class CarInterface(CarInterfaceBase): # events events = self.create_common_events(ret) - # enable request in prius is simple, as we activate when Toyota is active (rising edge) - if ret.cruiseState.enabled and not self.cruise_enabled_prev: - events.append(create_event('pcmEnable', [ET.ENABLE])) - elif not ret.cruiseState.enabled: - events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - if self.CS.lkas_state not in [2, 3] and ret.vEgo > 13.* CV.MPH_TO_MS and ret.cruiseState.enabled: events.append(create_event('steerTempUnavailableMute', [ET.WARNING])) ret.events = events - self.gas_pressed_prev = ret.gasPressed - self.brake_pressed_prev = ret.brakePressed - self.cruise_enabled_prev = ret.cruiseState.enabled - self.CS.out = ret.as_reader() - return self.CS.out # pass in a car.CarControl diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index 048f556f..3733ddce 100755 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -19,13 +19,10 @@ def _create_radar_can_parser(car_fingerprint): class RadarInterface(RadarInterfaceBase): def __init__(self, CP): - # radar - self.pts = {} + super().__init__(CP) self.validCnt = {key: 0 for key in RADAR_MSGS} self.track_id = 0 - self.delay = 0 # Delay of radar - self.rcp = _create_radar_can_parser(CP.carFingerprint) self.trigger_msg = 0x53f self.updated_messages = set() diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 4fe0e5a1..bda8ef22 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -4,27 +4,19 @@ from common.numpy_fast import interp from selfdrive.config import Conversions as CV from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.gm import gmcan -from selfdrive.car.gm.values import DBC, SUPERCRUISE_CARS, CanBus +from selfdrive.car.gm.values import DBC, CanBus from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert class CarControllerParams(): - def __init__(self, car_fingerprint): - if car_fingerprint in SUPERCRUISE_CARS: - self.STEER_MAX = 150 - self.STEER_STEP = 1 # how often we update the steer cmd - self.STEER_DELTA_UP = 2 # 0.75s time to peak torque - self.STEER_DELTA_DOWN = 5 # 0.3s from peak torque to zero - self.MIN_STEER_SPEED = -1. # can steer down to zero - else: - self.STEER_MAX = 300 - self.STEER_STEP = 2 # how often we update the steer cmd - self.STEER_DELTA_UP = 7 # ~0.75s time to peak torque (255/50hz/0.75s) - self.STEER_DELTA_DOWN = 17 # ~0.3s from peak torque to zero - self.MIN_STEER_SPEED = 3. - + def __init__(self): + self.STEER_MAX = 300 + self.STEER_STEP = 2 # how often we update the steer cmd + self.STEER_DELTA_UP = 7 # ~0.75s time to peak torque (255/50hz/0.75s) + self.STEER_DELTA_DOWN = 17 # ~0.3s from peak torque to zero + self.MIN_STEER_SPEED = 3. self.STEER_DRIVER_ALLOWANCE = 50 # allowed driver torque before start limiting self.STEER_DRIVER_MULTIPLIER = 4 # weight driver torque heavily self.STEER_DRIVER_FACTOR = 100 # from dbc @@ -61,23 +53,15 @@ def actuator_hystereses(final_pedal, pedal_steady): return final_pedal, pedal_steady -def process_hud_alert(hud_alert): - # initialize to no alert - steer = 0 - if hud_alert == VisualAlert.steerRequired: - steer = 1 - return steer - class CarController(): def __init__(self, dbc_name, CP, VM): self.pedal_steady = 0. self.start_time = 0. self.apply_steer_last = 0 - self.car_fingerprint = CP.carFingerprint self.lka_icon_status_last = (False, False) self.steer_rate_limited = False - self.params = CarControllerParams(CP.carFingerprint) + self.params = CarControllerParams() self.packer_pt = CANPacker(DBC[CP.carFingerprint]['pt']) self.packer_ch = CANPacker(DBC[CP.carFingerprint]['chassis']) @@ -90,13 +74,10 @@ class CarController(): # Send CAN commands. can_sends = [] - alert_out = process_hud_alert(hud_alert) - steer = alert_out - ### STEER ### if (frame % P.STEER_STEP) == 0: - lkas_enabled = enabled and not CS.steer_warning and CS.out.vEgo > P.MIN_STEER_SPEED + lkas_enabled = enabled and not CS.out.steerWarning and CS.out.vEgo > P.MIN_STEER_SPEED if lkas_enabled: new_steer = actuators.steer * P.STEER_MAX apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P) @@ -107,76 +88,72 @@ class CarController(): self.apply_steer_last = apply_steer idx = (frame // P.STEER_STEP) % 4 - if self.car_fingerprint in SUPERCRUISE_CARS: - can_sends += gmcan.create_steering_control_ct6(self.packer_pt, - CanBus, apply_steer, CS.out.vEgo, idx, lkas_enabled) - else: - can_sends.append(gmcan.create_steering_control(self.packer_pt, - CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled)) + can_sends.append(gmcan.create_steering_control(self.packer_pt, + CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled)) ### GAS/BRAKE ### - if self.car_fingerprint not in SUPERCRUISE_CARS: - # no output if not enabled, but keep sending keepalive messages - # treat pedals as one - final_pedal = actuators.gas - actuators.brake + # no output if not enabled, but keep sending keepalive messages + # treat pedals as one + final_pedal = actuators.gas - actuators.brake - # *** apply pedal hysteresis *** - final_brake, self.brake_steady = actuator_hystereses( - final_pedal, self.pedal_steady) + # *** apply pedal hysteresis *** + final_brake, self.brake_steady = actuator_hystereses( + final_pedal, self.pedal_steady) - if not enabled: - # Stock ECU sends max regen when not enabled. - apply_gas = P.MAX_ACC_REGEN - apply_brake = 0 - else: - apply_gas = int(round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V))) - apply_brake = int(round(interp(final_pedal, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V))) + if not enabled: + # Stock ECU sends max regen when not enabled. + apply_gas = P.MAX_ACC_REGEN + apply_brake = 0 + else: + apply_gas = int(round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V))) + apply_brake = int(round(interp(final_pedal, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V))) - # Gas/regen and brakes - all at 25Hz - if (frame % 4) == 0: - idx = (frame // 4) % 4 + # Gas/regen and brakes - all at 25Hz + if (frame % 4) == 0: + idx = (frame // 4) % 4 - at_full_stop = enabled and CS.out.standstill - near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) - can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, apply_brake, idx, near_stop, at_full_stop)) + at_full_stop = enabled and CS.out.standstill + near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) + can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, apply_brake, idx, near_stop, at_full_stop)) - at_full_stop = enabled and CS.out.standstill - can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, apply_gas, idx, enabled, at_full_stop)) + at_full_stop = enabled and CS.out.standstill + can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, apply_gas, idx, enabled, at_full_stop)) - # Send dashboard UI commands (ACC status), 25hz - if (frame % 4) == 0: - can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car)) + # Send dashboard UI commands (ACC status), 25hz + if (frame % 4) == 0: + can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car)) - # Radar needs to know current speed and yaw rate (50hz), - # and that ADAS is alive (10hz) - time_and_headlights_step = 10 - tt = frame * DT_CTRL + # Radar needs to know current speed and yaw rate (50hz), + # and that ADAS is alive (10hz) + time_and_headlights_step = 10 + tt = frame * DT_CTRL - if frame % time_and_headlights_step == 0: - idx = (frame // time_and_headlights_step) % 4 - can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx)) - can_sends.append(gmcan.create_adas_headlights_status(CanBus.OBSTACLE)) + if frame % time_and_headlights_step == 0: + idx = (frame // time_and_headlights_step) % 4 + can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx)) + can_sends.append(gmcan.create_adas_headlights_status(CanBus.OBSTACLE)) - speed_and_accelerometer_step = 2 - if frame % speed_and_accelerometer_step == 0: - idx = (frame // speed_and_accelerometer_step) % 4 - can_sends.append(gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx)) - can_sends.append(gmcan.create_adas_accelerometer_speed_status(CanBus.OBSTACLE, CS.out.vEgo, idx)) + speed_and_accelerometer_step = 2 + if frame % speed_and_accelerometer_step == 0: + idx = (frame // speed_and_accelerometer_step) % 4 + can_sends.append(gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx)) + can_sends.append(gmcan.create_adas_accelerometer_speed_status(CanBus.OBSTACLE, CS.out.vEgo, idx)) - if frame % P.ADAS_KEEPALIVE_STEP == 0: - can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN) + if frame % P.ADAS_KEEPALIVE_STEP == 0: + can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN) - # Show green icon when LKA torque is applied, and - # alarming orange icon when approaching torque limit. - # If not sent again, LKA icon disappears in about 5 seconds. - # Conveniently, sending camera message periodically also works as a keepalive. - lka_active = CS.lkas_status == 1 - lka_critical = lka_active and abs(actuators.steer) > 0.9 - lka_icon_status = (lka_active, lka_critical) - if frame % P.CAMERA_KEEPALIVE_STEP == 0 \ - or lka_icon_status != self.lka_icon_status_last: - can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer)) - self.lka_icon_status_last = lka_icon_status + # Show green icon when LKA torque is applied, and + # alarming orange icon when approaching torque limit. + # If not sent again, LKA icon disappears in about 5 seconds. + # Conveniently, sending camera message periodically also works as a keepalive. + lka_active = CS.lkas_status == 1 + lka_critical = lka_active and abs(actuators.steer) > 0.9 + lka_icon_status = (lka_active, lka_critical) + if frame % P.CAMERA_KEEPALIVE_STEP == 0 \ + or lka_icon_status != self.lka_icon_status_last: + steer_alert = hud_alert == VisualAlert.steerRequired + can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert)) + self.lka_icon_status_last = lka_icon_status return can_sends diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index cb849c0d..ac866ba2 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -6,7 +6,7 @@ from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase from selfdrive.car.gm.values import DBC, CAR, AccState, CanBus, \ CruiseButtons, is_eps_status_ok, \ - STEER_THRESHOLD, SUPERCRUISE_CARS + STEER_THRESHOLD class CarState(CarStateBase): @@ -53,21 +53,14 @@ class CarState(CarStateBase): ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1 ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2 - if self.car_fingerprint in SUPERCRUISE_CARS: - self.park_brake = False - ret.cruiseState.available = False - ret.espDisabled = False - regen_pressed = False - self.pcm_acc_status = int(pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive']) - else: - self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed'] - ret.cruiseState.available = bool(pt_cp.vl["ECMEngineStatus"]['CruiseMainOn']) - ret.espDisabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1 - self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState'] - if self.car_fingerprint == CAR.VOLT: - regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle']) - else: - regen_pressed = False + self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed'] + ret.cruiseState.available = bool(pt_cp.vl["ECMEngineStatus"]['CruiseMainOn']) + ret.espDisabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1 + self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState'] + + regen_pressed = False + if self.car_fingerprint == CAR.VOLT: + regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle']) # Regen braking is braking ret.brakePressed = ret.brake > 1e-5 or regen_pressed @@ -76,7 +69,7 @@ class CarState(CarStateBase): # 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus'] - self.steer_warning = not is_eps_status_ok(self.lkas_status, self.car_fingerprint) + ret.steerWarning = not is_eps_status_ok(self.lkas_status, self.car_fingerprint) return ret @@ -94,6 +87,7 @@ class CarState(CarStateBase): ("RightSeatBelt", "BCMDoorBeltStatus", 0), ("TurnSignals", "BCMTurnSignals", 0), ("AcceleratorPedal", "AcceleratorPedal", 0), + ("CruiseState", "AcceleratorPedal2", 0), ("ACCButtons", "ASCMSteeringButton", CruiseButtons.UNPRESS), ("SteeringWheelAngle", "PSCMSteeringAngle", 0), ("FLWheelSpd", "EBCMWheelSpdFront", 0), @@ -103,22 +97,14 @@ class CarState(CarStateBase): ("PRNDL", "ECMPRDNL", 0), ("LKADriverAppldTrq", "PSCMStatus", 0), ("LKATorqueDeliveredStatus", "PSCMStatus", 0), + ("TractionControlOn", "ESPStatus", 0), + ("EPBClosed", "EPBStatus", 0), + ("CruiseMainOn", "ECMEngineStatus", 0), ] if CP.carFingerprint == CAR.VOLT: signals += [ ("RegenPaddle", "EBCMRegenPaddle", 0), ] - if CP.carFingerprint in SUPERCRUISE_CARS: - signals += [ - ("ACCCmdActive", "ASCMActiveCruiseControlStatus", 0) - ] - else: - signals += [ - ("TractionControlOn", "ESPStatus", 0), - ("EPBClosed", "EPBStatus", 0), - ("CruiseMainOn", "ECMEngineStatus", 0), - ("CruiseState", "AcceleratorPedal2", 0), - ] return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], CanBus.POWERTRAIN) diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index d1e47935..23b521f5 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -11,32 +11,6 @@ def create_steering_control(packer, bus, apply_steer, idx, lkas_active): return packer.make_can_msg("ASCMLKASteeringCmd", bus, values) -def create_steering_control_ct6(packer, CanBus, apply_steer, v_ego, idx, enabled): - - values = { - "LKASteeringCmdActive": 1 if enabled else 0, - "LKASteeringCmd": apply_steer, - "RollingCounter": idx, - "SetMe1": 1, - "LKASVehicleSpeed": abs(v_ego * 3.6), - "LKASMode": 2 if enabled else 0, - "LKASteeringCmdChecksum": 0 # assume zero and then manually compute it - } - - dat = packer.make_can_msg("ASCMLKASteeringCmd", 0, values)[2] - # the checksum logic is weird - values['LKASteeringCmdChecksum'] = (0x2a + - sum(dat[:4]) + - values['LKASMode']) & 0x3ff - # pack again with checksum - dat = packer.make_can_msg("ASCMLKASteeringCmd", 0, values)[2] - - return [0x152, 0, dat, CanBus.POWERTRAIN], \ - [0x154, 0, dat, CanBus.POWERTRAIN], \ - [0x151, 0, dat, CanBus.CHASSIS], \ - [0x153, 0, dat, CanBus.CHASSIS] - - def create_adas_keepalive(bus): dat = b"\x00\x00\x00\x00\x00\x00\x00" return [make_can_msg(0x409, dat, bus), make_can_msg(0x40a, dat, bus)] @@ -147,47 +121,3 @@ def create_lka_icon_command(bus, active, critical, steer): dat = b"\x00\x00\x00" return make_can_msg(0x104c006c, dat, bus) -# TODO: WIP -''' -def create_friction_brake_command_ct6(packer, bus, apply_brake, idx, near_stop, at_full_stop): - - # counters loops across [0, 29, 42, 55] but checksum only considers 0, 1, 2, 3 - cntrs = [0, 29, 42, 55] - if apply_brake == 0: - mode = 0x1 - else: - mode = 0xa - - if at_full_stop: - mode = 0xd - elif near_stop: - mode = 0xb - - brake = (0x1000 - apply_brake) & 0xfff - checksum = (0x10000 - (mode << 12) - brake - idx) & 0xffff - - values = { - "RollingCounter" : cntrs[idx], - "FrictionBrakeMode" : mode, - "FrictionBrakeChecksum": checksum, - "FrictionBrakeCmd" : -apply_brake - } - - dat = packer.make_can_msg("EBCMFrictionBrakeCmd", 0, values)[2] - # msg is 0x315 but we are doing the panda forwarding - return make_can_msg(0x314, dat, 2) - -def create_gas_regen_command_ct6(bus, throttle, idx, acc_engaged, at_full_stop): - cntrs = [0, 7, 10, 13] - eng_bit = 1 if acc_engaged else 0 - gas_high = (throttle >> 8) | 0x80 - gas_low = (throttle) & 0xff - full_stop = 0x20 if at_full_stop else 0 - - chk1 = (0x100 - gas_high - 1) & 0xff - chk2 = (0x100 - gas_low - idx) & 0xff - dat = [(idx << 6) | eng_bit, 0xc2 | full_stop, gas_high, gas_low, - (1 - eng_bit) | (cntrs[idx] << 1), 0x5d - full_stop, chk1, chk2] - return make_can_msg(0x2cb, "".join(map(chr, dat)), bus) - -''' diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 48b744a9..ee4ef55d 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -3,7 +3,7 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from selfdrive.car.gm.values import CAR, Ecu, ECU_FINGERPRINT, CruiseButtons, \ - SUPERCRUISE_CARS, AccState, FINGERPRINTS + AccState, FINGERPRINTS from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase @@ -29,9 +29,7 @@ class CarInterface(CarInterfaceBase): # Presence of a camera on the object bus is ok. # Have to go to read_only if ASCM is online (ACC-enabled cars), # or camera is on powertrain bus (LKA cars without ACC). - ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or \ - has_relay or \ - candidate == CAR.CADILLAC_CT6 + ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay ret.openpilotLongitudinalControl = ret.enableCamera tire_stiffness_factor = 0.444 # not optimized yet @@ -93,16 +91,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.49 - elif candidate == CAR.CADILLAC_CT6: - # engage speed is decided by pcm - ret.minEnableSpeed = -1. - ret.mass = 4016. * CV.LB_TO_KG + STD_CARGO_KG - ret.safetyModel = car.CarParams.SafetyModel.cadillac - ret.wheelbase = 3.11 - ret.steerRatio = 14.6 # it's 16.3 without rear active steering - ret.steerRatioRear = 0. # TODO: there is RAS on this car! - ret.centerToFront = ret.wheelbase * 0.465 - # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) @@ -132,7 +120,6 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp) ret.canValid = self.cp.can_valid - ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo) ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False buttonEvents = [] @@ -159,41 +146,28 @@ class CarInterface(CarInterfaceBase): ret.buttonEvents = buttonEvents - events = self.create_common_events(ret) + events = self.create_common_events(ret, pcm_enable=False) - if self.CS.car_fingerprint in SUPERCRUISE_CARS: - if ret.cruiseState.enabled and not self.cruise_enabled_prev: - events.append(create_event('pcmEnable', [ET.ENABLE])) - if not ret.cruiseState.enabled: - events.append(create_event('pcmDisable', [ET.USER_DISABLE])) + if ret.vEgo < self.CP.minEnableSpeed: + events.append(create_event('speedTooLow', [ET.NO_ENTRY])) + if self.CS.park_brake: + events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) + if ret.cruiseState.standstill: + events.append(create_event('resumeRequired', [ET.WARNING])) + if self.CS.pcm_acc_status == AccState.FAULTED: + events.append(create_event('controlsFailed', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - else: - # TODO: why is this only not supercruise? ignore supercruise? - if ret.vEgo < self.CP.minEnableSpeed: - events.append(create_event('speedTooLow', [ET.NO_ENTRY])) - if self.CS.park_brake: - events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) - if ret.cruiseState.standstill: - events.append(create_event('resumeRequired', [ET.WARNING])) - if self.CS.pcm_acc_status == AccState.FAULTED: - events.append(create_event('controlsFailed', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - - # handle button presses - for b in ret.buttonEvents: - # do enable on both accel and decel buttons - if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed: - events.append(create_event('buttonEnable', [ET.ENABLE])) - # do disable on button down - if b.type == ButtonType.cancel and b.pressed: - events.append(create_event('buttonCancel', [ET.USER_DISABLE])) + # handle button presses + for b in ret.buttonEvents: + # do enable on both accel and decel buttons + if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed: + events.append(create_event('buttonEnable', [ET.ENABLE])) + # do disable on button down + if b.type == ButtonType.cancel and b.pressed: + events.append(create_event('buttonCancel', [ET.USER_DISABLE])) ret.events = events - # update previous brake/gas pressed - self.cruise_enabled_prev = ret.cruiseState.enabled - self.gas_pressed_prev = ret.gasPressed - self.brake_pressed_prev = ret.brakePressed - # copy back carState packet to CS self.CS.out = ret.as_reader() diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index b20706a0..8a2f9a2e 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -43,10 +43,7 @@ def create_radar_can_parser(car_fingerprint): class RadarInterface(RadarInterfaceBase): def __init__(self, CP): - # radar - self.pts = {} - - self.delay = 0 # Delay of radar + super().__init__(CP) self.rcp = create_radar_can_parser(CP.carFingerprint) diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index c48ed4e1..6a35c17c 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -6,13 +6,10 @@ class CAR: HOLDEN_ASTRA = "HOLDEN ASTRA RS-V BK 2017" VOLT = "CHEVROLET VOLT PREMIER 2017" CADILLAC_ATS = "CADILLAC ATS Premium Performance 2018" - CADILLAC_CT6 = "CADILLAC CT6 SUPERCRUISE 2018" MALIBU = "CHEVROLET MALIBU PREMIER 2017" ACADIA = "GMC ACADIA DENALI 2018" BUICK_REGAL = "BUICK REGAL ESSENCE 2018" -SUPERCRUISE_CARS = [CAR.CADILLAC_CT6] - class CruiseButtons: INIT = 0 UNPRESS = 1 @@ -34,12 +31,7 @@ class CanBus: SW_GMLAN = 3 def is_eps_status_ok(eps_status, car_fingerprint): - valid_eps_status = [] - if car_fingerprint in SUPERCRUISE_CARS: - valid_eps_status += [0, 1, 4, 5, 6] - else: - valid_eps_status += [0, 1] - return eps_status in valid_eps_status + return eps_status in [0, 1] FINGERPRINTS = { # Astra BK MY17, ASCM unplugged @@ -65,9 +57,6 @@ FINGERPRINTS = { { 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 368: 3, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 462: 4, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 491: 2, 493: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 723: 2, 753: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 882: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1241: 3, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1904: 7, 1906: 7, 1907: 7, 1912: 7, 1916: 7, 1917: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8 }], - CAR.CADILLAC_CT6: [{ - 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 313: 8, 320: 3, 322: 7, 328: 1, 336: 1, 338: 6, 340: 6, 352: 5, 354: 5, 356: 8, 368: 3, 372: 5, 381: 8, 386: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 458: 5, 460: 5, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 569: 3, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 723: 2, 753: 5, 761: 7, 800: 6, 801: 8, 804: 3, 810: 8, 832: 8, 833: 8, 834: 8, 835: 6, 836: 5, 837: 8, 838: 8, 839: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 884: 8, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 1, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1918: 7, 1919: 7, 1934: 7, 2016: 8, 2024: 8 - }], CAR.MALIBU: [ # Malibu Premier w/ ACC 2017 { @@ -93,5 +82,4 @@ DBC = { CAR.ACADIA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), CAR.CADILLAC_ATS: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), CAR.BUICK_REGAL: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), - CAR.CADILLAC_CT6: dbc_dict('cadillac_ct6_powertrain', 'cadillac_ct6_object', chassis_dbc='cadillac_ct6_chassis'), } diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 3da8a7b3..a09dffce 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -155,6 +155,9 @@ class CarController(): can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.isPandaBlack, CS.stock_hud)) if CS.CP.radarOffCan: + if (frame % 2) == 0: + idx = frame // 2 + can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack)) # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 08e44c02..91124564 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -107,6 +107,9 @@ def get_can_signals(CP): signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)] elif CP.carFingerprint == CAR.ODYSSEY_CHN: signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS", 1)] + elif CP.carFingerprint == CAR.HRV: + signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS", 1), + ("WHEELS_MOVING", "STANDSTILL", 1)] else: signals += [("DOOR_OPEN_FL", "DOORS_STATUS", 1), ("DOOR_OPEN_FR", "DOORS_STATUS", 1), @@ -125,7 +128,7 @@ def get_can_signals(CP): ("MAIN_ON", "SCM_BUTTONS", 0)] elif CP.carFingerprint in (CAR.CRV, CAR.CRV_EU, CAR.ACURA_RDX, CAR.PILOT_2019, CAR.RIDGELINE): signals += [("MAIN_ON", "SCM_BUTTONS", 0)] - elif CP.carFingerprint == CAR.FIT: + elif CP.carFingerprint in (CAR.FIT, CAR.HRV): signals += [("CAR_GAS", "GAS_PEDAL_2", 0), ("MAIN_ON", "SCM_BUTTONS", 0), ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0)] @@ -182,6 +185,8 @@ class CarState(CarStateBase): elif self.CP.carFingerprint == CAR.ODYSSEY_CHN: ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1 ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN']) + elif self.CP.carFingerprint == CAR.HRV: + ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN']) else: ret.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING'] ret.doorOpen = any([cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'], @@ -189,11 +194,11 @@ class CarState(CarStateBase): ret.seatbeltUnlatched = bool(cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP'] or not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED']) steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]['STEER_STATUS']] - self.steer_error = steer_status not in ['NORMAL', 'NO_TORQUE_ALERT_1', 'NO_TORQUE_ALERT_2', 'LOW_SPEED_LOCKOUT', 'TMP_FAULT'] + ret.steerError = steer_status not in ['NORMAL', 'NO_TORQUE_ALERT_1', 'NO_TORQUE_ALERT_2', 'LOW_SPEED_LOCKOUT', 'TMP_FAULT'] # NO_TORQUE_ALERT_2 can be caused by bump OR steering nudge from driver self.steer_not_allowed = steer_status not in ['NORMAL', 'NO_TORQUE_ALERT_2'] # LOW_SPEED_LOCKOUT is not worth a warning - self.steer_warning = steer_status not in ['NORMAL', 'LOW_SPEED_LOCKOUT', 'NO_TORQUE_ALERT_2'] + ret.steerWarning = steer_status not in ['NORMAL', 'LOW_SPEED_LOCKOUT', 'NO_TORQUE_ALERT_2'] if self.CP.radarOffCan: self.brake_error = 0 diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 38d099fd..178a452e 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -44,6 +44,17 @@ def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, i return packer.make_can_msg("STEERING_CONTROL", bus, values, idx) +def create_bosch_supplemental_1(packer, car_fingerprint, idx, has_relay): + # non-active params + values = { + "SET_ME_X04": 0x04, + "SET_ME_X80": 0x80, + "SET_ME_X10": 0x10, + } + bus = get_lkas_cmd_bus(car_fingerprint, has_relay) + return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values, idx) + + def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay, stock_hud): commands = [] bus_pt = get_pt_bus(car_fingerprint, has_relay) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index de43c4ca..d8d5cba7 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -284,6 +284,20 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] + elif candidate == CAR.HRV: + stop_and_go = False + ret.mass = 3125 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.61 + ret.centerToFront = ret.wheelbase * 0.41 + ret.steerRatio = 15.2 + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] + tire_stiffness_factor = 0.5 + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.18, 0.12] + elif candidate == CAR.ACURA_RDX: stop_and_go = False ret.mass = 3935. * CV.LB_TO_KG + STD_CARGO_KG @@ -372,8 +386,6 @@ class CarInterface(CarInterfaceBase): else: raise ValueError("unsupported car %s" % candidate) - ret.steerControlType = car.CarParams.SteerControlType.torque - # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not # conflict with PCM acc @@ -454,7 +466,7 @@ class CarInterface(CarInterfaceBase): ret.buttonEvents = buttonEvents # events - events = self.create_common_events(ret) + events = self.create_common_events(ret, pcm_enable=False) if self.CS.brake_error: events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if self.CS.brake_hold and self.CS.CP.carFingerprint not in HONDA_BOSCH: @@ -506,10 +518,6 @@ class CarInterface(CarInterfaceBase): ret.events = events - # update previous brake/gas pressed - self.gas_pressed_prev = ret.gasPressed - self.brake_pressed_prev = ret.brakePressed - self.CS.out = ret.as_reader() return self.CS.out diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py index 3c07cd74..b1606e64 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -20,8 +20,7 @@ def _create_nidec_can_parser(): class RadarInterface(RadarInterfaceBase): def __init__(self, CP): - # radar - self.pts = {} + super().__init__(CP) self.track_id = 0 self.radar_fault = False self.radar_wrong_config = False diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 9cf63a69..ca1008e2 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -34,6 +34,7 @@ class CAR: CRV_EU = "HONDA CR-V 2016 EXECUTIVE" CRV_HYBRID = "HONDA CR-V 2019 HYBRID" FIT = "HONDA FIT 2018 EX" + HRV = "HONDA HRV 2019 TOURING" ODYSSEY = "HONDA ODYSSEY 2018 EX-L" ODYSSEY_CHN = "HONDA ODYSSEY 2019 EXCLUSIVE CHN" ACURA_RDX = "ACURA RDX 2018 ACURAWATCH PLUS" @@ -93,6 +94,9 @@ FINGERPRINTS = { CAR.FIT: [{ 57: 3, 145: 8, 228: 5, 304: 8, 342: 6, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 422: 8, 427: 3, 428: 8, 432: 7, 464: 8, 487: 4, 490: 8, 506: 8, 597: 8, 660: 8, 661: 4, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 829: 5, 862: 8, 884: 7, 892: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8 }], + CAR.HRV: [{ + 57: 3, 145: 8, 228: 5, 316: 8, 340: 8, 342: 6, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 422: 8, 423: 2, 426: 8, 427: 3, 432: 7, 441: 5, 450: 8, 464: 8, 474: 8, 490: 8, 493: 3, 506: 8, 538: 5, 578: 2, 597: 8, 660: 8, 661: 4, 773: 7, 777: 8, 780: 8, 804: 8, 808: 8, 829: 5, 862: 8, 882: 2, 884: 7, 892: 8, 929: 8, 985: 3, 1030: 5, 1033: 5, 1108: 8, 1137: 8, 1348: 5, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1618: 5 + }], # 2018 Odyssey w/ Added Comma Pedal Support (512L & 513L) CAR.ODYSSEY: [{ 57: 3, 148: 8, 228: 5, 229: 4, 316: 8, 342: 6, 344: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 427: 3, 432: 7, 450: 8, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 862: 8, 871: 8, 881: 8, 882: 4, 884: 8, 891: 8, 892: 8, 905: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1029: 8, 1036: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1302: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1615: 8, 1616: 5, 1619: 5, 1623: 5, 1668: 5 @@ -122,7 +126,7 @@ FINGERPRINTS = { }, # 2019 Ridgeline { - 57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422:8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 476: 4, 490: 8, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 871: 8, 882: 2, 884: 7, 892: 8, 923: 2, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 424: 5, 1613: 5, 1616: 5, 1618: 5, 1623: 5, 1668: 5 + 57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422:8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 476: 4, 490: 8, 512: 6, 513: 6, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 871: 8, 882: 2, 884: 7, 892: 8, 923: 2, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 424: 5, 1613: 5, 1616: 5, 1618: 5, 1623: 5, 1668: 5 }], # 2019 Insight CAR.INSIGHT: [{ @@ -131,7 +135,7 @@ FINGERPRINTS = { } # Don't use theses fingerprints for fingerprinting, they are still needed for ECU detection -IGNORED_FINGERPRINTS = [CAR.INSIGHT, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_EU] +IGNORED_FINGERPRINTS = [CAR.INSIGHT, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_EU, CAR.HRV] # add DIAG_MSGS to fingerprints for c in FINGERPRINTS: @@ -206,11 +210,13 @@ FW_VERSIONS = { b'37805-6A0-A740\x00\x00', b'37805-6A0-A840\x00\x00', b'37805-6A0-A850\x00\x00', + b'37805-6A1-H650\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ b'28101-6A7-A220\x00\x00', b'28101-6A7-A320\x00\x00', b'28101-6A7-A510\x00\x00', + b'28101-6A9-H140\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TVA-A230\x00\x00', @@ -219,6 +225,7 @@ FW_VERSIONS = { b'46114-TVA-A050\x00\x00', b'46114-TVA-A060\x00\x00', b'46114-TVA-A120\x00\x00', + b'46114-TVE-H550\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TVA-A010\x00\x00', @@ -226,29 +233,35 @@ FW_VERSIONS = { b'78109-TVA-A220\x00\x00', b'78109-TVA-A310\x00\x00', b'78109-TWA-A210\x00\x00', + b'78109-TVE-H610\x00\x00', ], (Ecu.hud, 0x18da61f1, None): [ b'78209-TVA-A010\x00\x00', ], (Ecu.fwdCamera, 0x18dab5f1, None): [ b'36161-TVA-A060\x00\x00', + b'36161-TVE-H050\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TVA-A460\x00\x00', + b'77959-TVA-H230\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ b'57114-TVA-B050\x00\x00', b'57114-TVA-B040\x00\x00', + b'57114-TVE-H250\x00\x00', ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TVA-A150\x00\x00', b'36802-TVA-A160\x00\x00', b'36802-TVA-A170\x00\x00', + b'36802-TVE-H070\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TVA-A140\x00\x00', b'39990-TVA-A150\x00\x00', # Are these two different steerRatio? b'39990-TVA-A160\x00\x00', # Sport, Sport 2.0T and Touring 2.0T have different ratios + b'39990-TVE-H130\x00\x00', ], }, CAR.ACCORDH: { @@ -353,23 +366,28 @@ FW_VERSIONS = { b'37805-5AA-L950\x00\x00', b'37805-5AN-A750\x00\x00', b'37805-5AN-A830\x00\x00', + b'37805-5AN-A840\x00\x00', b'37805-5AN-A930\x00\x00', b'37805-5AN-A950\x00\x00', + b'37805-5AN-AH20\x00\x00', b'37805-5AN-L940\x00\x00', + b'37805-5AN-LF20\x00\x00', b'37805-5AN-LH20\x00\x00', b'37805-5AN-LJ20\x00\x00', + b'37805-5AW-G720\x00\x00', b'37805-5AZ-E850\x00\x00', b'37805-5BB-L640\x00\x00', - b'37805-5AN-AH20\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ b'28101-5CG-A920\x00\x00', b'28101-5CG-C220\x00\x00', b'28101-5CG-C320\x00\x00', + b'28101-5CG-G020\x00\x00', b'28101-5CK-A130\x00\x00', b'28101-5CK-A140\x00\x00', b'28101-5CK-A150\x00\x00', b'28101-5CK-C130\x00\x00', + b'28101-5CK-C140\x00\x00', b'28101-5DJ-A710\x00\x00', b'28101-5DV-E330\x00\x00', ], @@ -382,6 +400,7 @@ FW_VERSIONS = { (Ecu.eps, 0x18da30f1, None): [ b'39990-TBA-C020\x00\x00', b'39990-TBA-C120\x00\x00', + b'39990-TEZ-T020\x00\x00', b'39990-TGG-A020\x00\x00', b'39990-TGG-A120\x00\x00', b'39990-TGL-E130\x00\x00', @@ -389,14 +408,16 @@ FW_VERSIONS = { ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TBA-A060\x00\x00', + b'77959-TEA-G020\x00\x00', b'77959-TGG-A020\x00\x00', b'77959-TGG-G010\x00\x00', - b'77959-TGG-A020\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TBA-A910\x00\x00', b'78109-TBC-A740\x00\x00', + b'78109-TFJ-G020\x00\x00', b'78109-TGG-A210\x00\x00', + b'78109-TGG-A220\x00\x00', b'78109-TGG-A310\x00\x00', b'78109-TGG-A320\x00\x00', b'78109-TGG-A810\x00\x00', @@ -405,12 +426,14 @@ FW_VERSIONS = { ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TBA-A150\x00\x00', + b'36802-TFJ-G060\x00\x00', b'36802-TGG-A050\x00\x00', b'36802-TGL-G040\x00\x00', b'36802-TGG-A060\x00\x00', ], (Ecu.fwdCamera, 0x18dab5f1, None): [ b'36161-TBA-A130\x00\x00', + b'36161-TFJ-G070\x00\x00', b'36161-TGG-A060\x00\x00', b'36161-TGL-G050\x00\x00', b'36161-TGG-A080\x00\x00', @@ -418,7 +441,6 @@ FW_VERSIONS = { (Ecu.unknown, 0x18daeff1, None): [ b'38897-TBA-A110\x00\x00', b'38897-TBA-A020\x00\x00', - b'38897-TBA-A020\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TBA-A110\x00\x00', @@ -434,6 +456,8 @@ FW_VERSIONS = { (Ecu.combinationMeter, 0x18da60f1, None): [b'78109-TFK-G020\x00\x00'], (Ecu.fwdRadar, 0x18dab0f1, None): [b'36802-TFK-G130\x00\x00'], (Ecu.shiftByWire, 0x18da0bf1, None): [b'54008-TGN-E010\x00\x00'], + (Ecu.fwdCamera, 0x18dab5f1, None): [b'36161-TFK-G130\x00\x00'], + (Ecu.gateway, 0x18daeff1, None): [b'38897-TBA-A020\x00\x00'], }, CAR.CRV_5G: { (Ecu.programmedFuelInjection, 0x18da10f1, None): [ @@ -447,6 +471,7 @@ FW_VERSIONS = { b'37805-5PA-A870\x00\x00', b'37805-5PA-A880\x00\x00', b'37805-5PA-A890\x00\x00', + b'37805-5PD-Q630\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ b'28101-5RG-A020\x00\x00', @@ -456,19 +481,23 @@ FW_VERSIONS = { b'28101-5RH-A030\x00\x00', b'28101-5RH-A040\x00\x00', b'28101-5RH-A120\x00\x00', + b'28101-5RL-Q010\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ b'57114-TLA-A040\x00\x00', b'57114-TLA-A050\x00\x00', b'57114-TLA-A060\x00\x00', + b'57114-TMC-Z050\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ - b'39990-TLA-A040\x00\x00', b'39990-TLA,A040\x00\x00', + b'39990-TLA-A040\x00\x00', + b'39990-TMT-T010\x00\x00', ], (Ecu.electricBrakeBooster, 0x18da2bf1, None): [ b'46114-TLA-A040\x00\x00', b'46114-TLA-A050\x00\x00', + b'46114-TMC-U020\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TLA-A110\x00\x00', @@ -476,6 +505,7 @@ FW_VERSIONS = { b'78109-TLA-C210\x00\x00', b'78109-TLB-A110\x00\x00', b'78109-TLB-A210\x00\x00', + b'78109-TMC-Q210\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TLA-A010\x00\x00', @@ -485,16 +515,19 @@ FW_VERSIONS = { b'36802-TLA-A040\x00\x00', b'36802-TLA-A050\x00\x00', b'36802-TLA-A060\x00\x00', + b'36802-TMC-Q070\x00\x00', ], (Ecu.fwdCamera, 0x18dab5f1, None): [ b'36161-TLA-A060\x00\x00', b'36161-TLA-A070\x00\x00', b'36161-TLA-A080\x00\x00', + b'36161-TMC-Q040\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TLA-A240\x00\x00', b'77959-TLA-A250\x00\x00', b'77959-TLA-A320\x00\x00', + b'77959-TLA-Q040\x00\x00', ], }, CAR.CRV_EU: { @@ -634,21 +667,26 @@ FW_VERSIONS = { CAR.RIDGELINE: { (Ecu.eps, 0x18da30f1, None): [ b'39990-T6Z-A020\x00\x00', + b'39990-T6Z-A030\x00\x00', ], (Ecu.fwdCamera, 0x18dab0f1, None): [ b'36161-T6Z-A310\x00\x00', + b'36161-T6Z-A520\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-T6Z-A010\x00\x00', + b'38897-T6Z-A110\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-T6Z-A420\x00\x00', + b'78109-T6Z-A510\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ b'77959-T6Z-A020\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ b'57114-T6Z-A130\x00\x00', + b'57114-T6Z-A520\x00\x00', ], }, CAR.INSIGHT: { @@ -665,6 +703,13 @@ FW_VERSIONS = { b'77959-TXM-A230\x00\x00', ], }, + CAR.HRV: { + (Ecu.gateway, 0x18daeff1, None): [b'38897-T7A-A010\x00\x00'], + (Ecu.eps, 0x18da30f1, None): [b'39990-THX-A020\x00\x00'], + (Ecu.fwdRadar, 0x18dab0f1, None): [b'36161-T7A-A240\x00\x00'], + (Ecu.srs, 0x18da53f1, None): [b'77959-T7A-A230\x00\x00'], + (Ecu.combinationMeter, 0x18da60f1, None): [b'78109-THX-A210\x00\x00'], + }, } DBC = { @@ -681,6 +726,7 @@ DBC = { CAR.CRV_EU: dbc_dict('honda_crv_executive_2016_can_generated', 'acura_ilx_2016_nidec'), CAR.CRV_HYBRID: dbc_dict('honda_crv_hybrid_2019_can_generated', None), CAR.FIT: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), + CAR.HRV: dbc_dict('honda_hrv_touring_2019_can_generated', 'acura_ilx_2016_nidec'), CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'), CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'), CAR.PILOT: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'), @@ -703,6 +749,7 @@ STEER_THRESHOLD = { CAR.CRV_EU: 400, CAR.CRV_HYBRID: 1200, CAR.FIT: 1200, + CAR.HRV: 1200, CAR.ODYSSEY: 1200, CAR.ODYSSEY_CHN: 1200, CAR.PILOT: 1200, @@ -725,6 +772,7 @@ SPEED_FACTOR = { CAR.CRV_EU: 1.025, CAR.CRV_HYBRID: 1.025, CAR.FIT: 1., + CAR.HRV: 1.025, CAR.ODYSSEY: 1., CAR.ODYSSEY_CHN: 1., CAR.PILOT: 1., diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index efd8796e..59fef40d 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -1,47 +1,102 @@ +from cereal import car from selfdrive.car import apply_std_steer_torque_limits -from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11 -from selfdrive.car.hyundai.values import Buttons, SteerLimitParams +from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_lfa_mfa +from selfdrive.car.hyundai.values import Buttons, SteerLimitParams, CAR from opendbc.can.packer import CANPacker +VisualAlert = car.CarControl.HUDControl.VisualAlert + + +def process_hud_alert(enabled, fingerprint, visual_alert, left_lane, + right_lane, left_lane_depart, right_lane_depart): + sys_warning = (visual_alert == VisualAlert.steerRequired) + + # initialize to no line visible + sys_state = 1 + if left_lane and right_lane or sys_warning: #HUD alert only display when LKAS status is active + if enabled or sys_warning: + sys_state = 3 + else: + sys_state = 4 + elif left_lane: + sys_state = 5 + elif right_lane: + sys_state = 6 + + # initialize to no warnings + left_lane_warning = 0 + right_lane_warning = 0 + if left_lane_depart: + left_lane_warning = 1 if fingerprint in [CAR.GENESIS_G90, CAR.GENESIS_G80] else 2 + if right_lane_depart: + right_lane_warning = 1 if fingerprint in [CAR.GENESIS_G90, CAR.GENESIS_G80] else 2 + + return sys_warning, sys_state, left_lane_warning, right_lane_warning + class CarController(): def __init__(self, dbc_name, CP, VM): self.apply_steer_last = 0 self.car_fingerprint = CP.carFingerprint - self.lkas11_cnt = 0 - self.cnt = 0 - self.last_resume_cnt = 0 self.packer = CANPacker(dbc_name) self.steer_rate_limited = False + self.resume_cnt = 0 + self.last_resume_frame = 0 + self.last_lead_distance = 0 - def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): - - ### Steering Torque + def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, + left_lane, right_lane, left_lane_depart, right_lane_depart): + # Steering Torque new_steer = actuators.steer * SteerLimitParams.STEER_MAX apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, SteerLimitParams) self.steer_rate_limited = new_steer != apply_steer - if not enabled: - apply_steer = 0 + # disable if steer angle reach 90 deg, otherwise mdps fault in some models + lkas_active = enabled and abs(CS.out.steeringAngle) < 90. - steer_req = 1 if enabled else 0 + # fix for Genesis hard fault at low speed + if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS: + lkas_active = 0 + + if not lkas_active: + apply_steer = 0 self.apply_steer_last = apply_steer + sys_warning, sys_state, left_lane_warning, right_lane_warning =\ + process_hud_alert(enabled, self.car_fingerprint, visual_alert, + left_lane, right_lane, left_lane_depart, right_lane_depart) + can_sends = [] - - self.lkas11_cnt = self.cnt % 0x10 - self.clu11_cnt = self.cnt % 0x10 - - can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt, - enabled, CS.lkas11, hud_alert, keep_stock=True)) + can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active, + CS.lkas11, sys_warning, sys_state, enabled, + left_lane, right_lane, + left_lane_warning, right_lane_warning)) if pcm_cancel_cmd: - can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL)) - elif CS.out.cruiseState.standstill and (self.cnt - self.last_resume_cnt) > 5: - self.last_resume_cnt = self.cnt - can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL)) + can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL)) - self.cnt += 1 + elif CS.out.cruiseState.standstill: + # run only first time when the car stopped + if self.last_lead_distance == 0: + # get the lead distance from the Radar + self.last_lead_distance = CS.lead_distance + self.resume_cnt = 0 + # when lead car starts moving, create 6 RES msgs + elif CS.lead_distance != self.last_lead_distance and (frame - self.last_resume_frame) > 5: + can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)) + self.resume_cnt += 1 + # interval after 6 msgs + if self.resume_cnt > 5: + self.last_resume_frame = frame + self.clu11_cnt = 0 + # reset lead distnce after the car starts moving + elif self.last_lead_distance != 0: + self.last_lead_distance = 0 + + + # 20 Hz LFA MFA message + if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE]: + can_sends.append(create_lfa_mfa(self.packer, frame, enabled)) return can_sends diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 59c825f4..d6f7d30e 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -6,12 +6,14 @@ from selfdrive.config import Conversions as CV GearShifter = car.CarState.GearShifter -class CarState(CarStateBase): +class CarState(CarStateBase): def update(self, cp, cp_cam): ret = car.CarState.new_message() - ret.doorOpen = False # FIXME + ret.doorOpen = any([cp.vl["CGW1"]['CF_Gway_DrvDrSw'],cp.vl["CGW1"]['CF_Gway_AstDrSw'], + cp.vl["CGW2"]['CF_Gway_RLDrSw'], cp.vl["CGW2"]['CF_Gway_RRDrSw']]) + ret.seatbeltUnlatched = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw'] == 0 ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS @@ -28,14 +30,16 @@ class CarState(CarStateBase): ret.yawRate = cp.vl["ESP12"]['YAW_RATE'] ret.leftBlinker = cp.vl["CGW1"]['CF_Gway_TSigLHSw'] != 0 ret.rightBlinker = cp.vl["CGW1"]['CF_Gway_TSigRHSw'] != 0 - ret.steeringTorque = cp.vl["MDPS11"]['CR_Mdps_DrvTq'] + ret.steeringTorque = cp.vl["MDPS12"]['CR_Mdps_StrColTq'] ret.steeringTorqueEps = cp.vl["MDPS12"]['CR_Mdps_OutTq'] ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD + ret.steerWarning = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail'] != 0 # cruise state - ret.cruiseState.enabled = cp.vl["SCC12"]['ACCMode'] != 0 ret.cruiseState.available = True + ret.cruiseState.enabled = cp.vl["SCC12"]['ACCMode'] != 0 ret.cruiseState.standstill = cp.vl["SCC11"]['SCCInfoDisplay'] == 4. + if ret.cruiseState.enabled: is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"]) speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS @@ -43,70 +47,79 @@ class CarState(CarStateBase): else: ret.cruiseState.speed = 0 - ret.brake = 0 # FIXME + # TODO: Find brake pressure + ret.brake = 0 ret.brakePressed = cp.vl["TCS13"]['DriverBraking'] != 0 - ret.brakeLights = ret.brakePressed + + # TODO: Check this + ret.brakeLights = bool(cp.vl["TCS13"]['BrakeLight'] or ret.brakePressed) + + #TODO: find pedal signal for EV/HYBRID Cars ret.gas = cp.vl["EMS12"]['PV_AV_CAN'] / 100 - ret.gasPressed = cp.vl["EMS16"]["CF_Ems_AclAct"] != 0 - ret.espDisabled = cp.vl["TCS15"]['ESC_Off_Step'] != 0 - - # Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with - gear = cp.vl["LVR12"]["CF_Lvr_Gear"] - if gear == 5: - gear_shifter = GearShifter.drive - elif gear == 6: - gear_shifter = GearShifter.neutral - elif gear == 0: - gear_shifter = GearShifter.park - elif gear == 7: - gear_shifter = GearShifter.reverse - else: - gear_shifter = GearShifter.unknown + ret.gasPressed = bool(cp.vl["EMS16"]["CF_Ems_AclAct"]) + # TODO: refactor gear parsing in function # Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection, as this seems to be standard over all cars, but is not the preferred method. - if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1: - gear_shifter_cluster = GearShifter.drive - elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1: - gear_shifter_cluster = GearShifter.neutral - elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1: - gear_shifter_cluster = GearShifter.park - elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1: - gear_shifter_cluster = GearShifter.reverse - else: - gear_shifter_cluster = GearShifter.unknown - - # Gear Selecton via TCU12 - gear2 = cp.vl["TCU12"]["CUR_GR"] - if gear2 == 0: - gear_tcu = GearShifter.park - elif gear2 == 14: - gear_tcu = GearShifter.reverse - elif gear2 > 0 and gear2 < 9: # unaware of anything over 8 currently - gear_tcu = GearShifter.drive - else: - gear_tcu = GearShifter.unknown - - # gear shifter if self.CP.carFingerprint in FEATURES["use_cluster_gears"]: - ret.gearShifter = gear_shifter_cluster + if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1: + ret.gearShifter = GearShifter.drive + elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1: + ret.gearShifter = GearShifter.neutral + elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1: + ret.gearShifter = GearShifter.park + elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1: + ret.gearShifter = GearShifter.reverse + else: + ret.gearShifter = GearShifter.unknown + # Gear Selecton via TCU12 elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]: - ret.gearShifter = gear_tcu + gear = cp.vl["TCU12"]["CUR_GR"] + if gear == 0: + ret.gearShifter = GearShifter.park + elif gear == 14: + ret.gearShifter = GearShifter.reverse + elif gear > 0 and gear < 9: # unaware of anything over 8 currently + ret.gearShifter = GearShifter.drive + else: + ret.gearShifter = GearShifter.unknown + # Gear Selecton - This is only compatible with optima hybrid 2017 + elif self.CP.carFingerprint in FEATURES["use_elect_gears"]: + gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"] + if gear in (5, 8): # 5: D, 8: sport mode + ret.gearShifter = GearShifter.drive + elif gear == 6: + ret.gearShifter = GearShifter.neutral + elif gear == 0: + ret.gearShifter = GearShifter.park + elif gear == 7: + ret.gearShifter = GearShifter.reverse + else: + ret.gearShifter = GearShifter.unknown + # Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with else: - ret.gearShifter = gear_shifter + gear = cp.vl["LVR12"]["CF_Lvr_Gear"] + if gear in (5, 8): # 5: D, 8: sport mode + ret.gearShifter = GearShifter.drive + elif gear == 6: + ret.gearShifter = GearShifter.neutral + elif gear == 0: + ret.gearShifter = GearShifter.park + elif gear == 7: + ret.gearShifter = GearShifter.reverse + else: + ret.gearShifter = GearShifter.unknown # save the entire LKAS11 and CLU11 self.lkas11 = cp_cam.vl["LKAS11"] self.clu11 = cp.vl["CLU11"] self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw'] self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE - self.steer_warning = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail'] - self.brake_error = 0 + self.lead_distance = cp.vl["SCC11"]['ACC_ObjDist'] return ret @staticmethod def get_can_parser(CP): - signals = [ # sig_name, sig_address, default ("WHL_SPD_FL", "WHL_SPD11", 0), @@ -119,16 +132,16 @@ class CarState(CarStateBase): ("CF_Gway_DrvSeatBeltInd", "CGW4", 1), ("CF_Gway_DrvSeatBeltSw", "CGW1", 0), + ("CF_Gway_DrvDrSw", "CGW1", 0), # Driver Door + ("CF_Gway_AstDrSw", "CGW1", 0), # Passenger door + ("CF_Gway_RLDrSw", "CGW2", 0), # Rear reft door + ("CF_Gway_RRDrSw", "CGW2", 0), # Rear right door ("CF_Gway_TSigLHSw", "CGW1", 0), ("CF_Gway_TurnSigLh", "CGW1", 0), ("CF_Gway_TSigRHSw", "CGW1", 0), ("CF_Gway_TurnSigRh", "CGW1", 0), ("CF_Gway_ParkBrakeSw", "CGW1", 0), - ("BRAKE_ACT", "EMS12", 0), - ("PV_AV_CAN", "EMS12", 0), - ("CF_Ems_AclAct", "EMS16", 0), - ("CYL_PRES", "ESP12", 0), ("CF_Clu_CruiseSwState", "CLU11", 0), @@ -144,54 +157,76 @@ class CarState(CarStateBase): ("CF_Clu_AmpInfo", "CLU11", 0), ("CF_Clu_AliveCnt1", "CLU11", 0), - ("CF_Clu_InhibitD", "CLU15", 0), - ("CF_Clu_InhibitP", "CLU15", 0), - ("CF_Clu_InhibitN", "CLU15", 0), - ("CF_Clu_InhibitR", "CLU15", 0), - - ("CF_Lvr_Gear", "LVR12",0), - ("CUR_GR", "TCU12",0), - ("ACCEnable", "TCS13", 0), + ("BrakeLight", "TCS13", 0), ("DriverBraking", "TCS13", 0), ("ESC_Off_Step", "TCS15", 0), ("CF_Lvr_GearInf", "LVR11", 0), #Transmission Gear (0 = N or P, 1-8 = Fwd, 14 = Rev) - ("CR_Mdps_DrvTq", "MDPS11", 0), - ("CR_Mdps_StrColTq", "MDPS12", 0), ("CF_Mdps_ToiActive", "MDPS12", 0), ("CF_Mdps_ToiUnavail", "MDPS12", 0), ("CF_Mdps_FailStat", "MDPS12", 0), ("CR_Mdps_OutTq", "MDPS12", 0), - ("VSetDis", "SCC11", 0), - ("SCCInfoDisplay", "SCC11", 0), - ("ACCMode", "SCC12", 1), - ("SAS_Angle", "SAS11", 0), ("SAS_Speed", "SAS11", 0), + + ("MainMode_ACC", "SCC11", 0), + ("VSetDis", "SCC11", 0), + ("SCCInfoDisplay", "SCC11", 0), + ("ACC_ObjDist", "SCC11", 0), + ("ACCMode", "SCC12", 1), + + ("PV_AV_CAN", "EMS12", 0), + ("CF_Ems_AclAct", "EMS16", 0), ] checks = [ # address, frequency ("MDPS12", 50), - ("MDPS11", 100), - ("TCS15", 10), ("TCS13", 50), + ("TCS15", 10), ("CLU11", 50), ("ESP12", 100), - ("EMS12", 100), - ("EMS16", 100), ("CGW1", 10), ("CGW4", 5), ("WHL_SPD11", 50), + ("SAS11", 100), ("SCC11", 50), ("SCC12", 50), - ("SAS11", 100) + ("EMS12", 100), + ("EMS16", 100), ] + if CP.carFingerprint in FEATURES["use_cluster_gears"]: + signals += [ + ("CF_Clu_InhibitD", "CLU15", 0), + ("CF_Clu_InhibitP", "CLU15", 0), + ("CF_Clu_InhibitN", "CLU15", 0), + ("CF_Clu_InhibitR", "CLU15", 0), + ] + checks += [ + ("CLU15", 5) + ] + elif CP.carFingerprint in FEATURES["use_tcu_gears"]: + signals += [ + ("CUR_GR", "TCU12",0) + ] + checks += [ + ("TCU12", 100) + ] + elif CP.carFingerprint in FEATURES["use_elect_gears"]: + signals += [("Elect_Gear_Shifter", "ELECT_GEAR", 0)] + checks += [("ELECT_GEAR", 20)] + else: + signals += [ + ("CF_Lvr_Gear","LVR12",0) + ] + checks += [ + ("LVR12", 100) + ] return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) @@ -200,13 +235,13 @@ class CarState(CarStateBase): signals = [ # sig_name, sig_address, default + ("CF_Lkas_Bca_R", "LKAS11", 0), ("CF_Lkas_LdwsSysState", "LKAS11", 0), ("CF_Lkas_SysWarning", "LKAS11", 0), ("CF_Lkas_LdwsLHWarning", "LKAS11", 0), ("CF_Lkas_LdwsRHWarning", "LKAS11", 0), ("CF_Lkas_HbaLamp", "LKAS11", 0), ("CF_Lkas_FcwBasReq", "LKAS11", 0), - ("CF_Lkas_ToiFlt", "LKAS11", 0), ("CF_Lkas_HbaSysState", "LKAS11", 0), ("CF_Lkas_FcwOpt", "LKAS11", 0), ("CF_Lkas_HbaOpt", "LKAS11", 0), diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index 910e05ad..32aefc85 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -1,31 +1,48 @@ import crcmod -from selfdrive.car.hyundai.values import CHECKSUM +from selfdrive.car.hyundai.values import CAR, CHECKSUM hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf) -def create_lkas11(packer, car_fingerprint, apply_steer, steer_req, cnt, enabled, lkas11, hud_alert, keep_stock=False): - values = { - "CF_Lkas_Bca_R": 3 if enabled else 0, - "CF_Lkas_LdwsSysState": 3 if steer_req else 1, - "CF_Lkas_SysWarning": hud_alert, - "CF_Lkas_LdwsLHWarning": lkas11["CF_Lkas_LdwsLHWarning"] if keep_stock else 0, - "CF_Lkas_LdwsRHWarning": lkas11["CF_Lkas_LdwsRHWarning"] if keep_stock else 0, - "CF_Lkas_HbaLamp": lkas11["CF_Lkas_HbaLamp"] if keep_stock else 0, - "CF_Lkas_FcwBasReq": lkas11["CF_Lkas_FcwBasReq"] if keep_stock else 0, - "CR_Lkas_StrToqReq": apply_steer, - "CF_Lkas_ActToi": steer_req, - "CF_Lkas_ToiFlt": 0, - "CF_Lkas_HbaSysState": lkas11["CF_Lkas_HbaSysState"] if keep_stock else 1, - "CF_Lkas_FcwOpt": lkas11["CF_Lkas_FcwOpt"] if keep_stock else 0, - "CF_Lkas_HbaOpt": lkas11["CF_Lkas_HbaOpt"] if keep_stock else 3, - "CF_Lkas_MsgCount": cnt, - "CF_Lkas_FcwSysState": lkas11["CF_Lkas_FcwSysState"] if keep_stock else 0, - "CF_Lkas_FcwCollisionWarning": lkas11["CF_Lkas_FcwCollisionWarning"] if keep_stock else 0, - "CF_Lkas_FusionState": lkas11["CF_Lkas_FusionState"] if keep_stock else 0, - "CF_Lkas_Chksum": 0, - "CF_Lkas_FcwOpt_USM": 2 if enabled else 1, - "CF_Lkas_LdwsOpt_USM": lkas11["CF_Lkas_LdwsOpt_USM"] if keep_stock else 3, - } + +def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, + lkas11, sys_warning, sys_state, enabled, + left_lane, right_lane, + left_lane_depart, right_lane_depart): + values = lkas11 + values["CF_Lkas_LdwsSysState"] = sys_state + values["CF_Lkas_SysWarning"] = 3 if sys_warning else 0 + values["CF_Lkas_LdwsLHWarning"] = left_lane_depart + values["CF_Lkas_LdwsRHWarning"] = right_lane_depart + values["CR_Lkas_StrToqReq"] = apply_steer + values["CF_Lkas_ActToi"] = steer_req + values["CF_Lkas_ToiFlt"] = 0 + values["CF_Lkas_MsgCount"] = frame % 0x10 + values["CF_Lkas_Chksum"] = 0 + + if car_fingerprint in [CAR.SONATA, CAR.PALISADE]: + values["CF_Lkas_Bca_R"] = int(left_lane) + (int(right_lane) << 1) + values["CF_Lkas_LdwsOpt_USM"] = 2 + + # FcwOpt_USM 5 = Orange blinking car + lanes + # FcwOpt_USM 4 = Orange car + lanes + # FcwOpt_USM 3 = Green blinking car + lanes + # FcwOpt_USM 2 = Green car + lanes + # FcwOpt_USM 1 = White car + lanes + # FcwOpt_USM 0 = No car + lanes + values["CF_Lkas_FcwOpt_USM"] = 2 if enabled else 1 + + # SysWarning 4 = keep hands on wheel + # SysWarning 5 = keep hands on wheel (red) + # SysWarning 6 = keep hands on wheel (red) + beep + # Note: the warning is hidden while the blinkers are on + values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0 + + elif car_fingerprint == CAR.HYUNDAI_GENESIS: + # This field is actually LdwsActivemode + # Genesis and Optima fault when forwarding while engaged + values["CF_Lkas_Bca_R"] = 2 + elif car_fingerprint == CAR.KIA_OPTIMA: + values["CF_Lkas_Bca_R"] = 0 dat = packer.make_can_msg("LKAS11", 0, values)[2] @@ -36,7 +53,7 @@ def create_lkas11(packer, car_fingerprint, apply_steer, steer_req, cnt, enabled, elif car_fingerprint in CHECKSUM["6B"]: # Checksum of first 6 Bytes, as seen on 2018 Kia Sorento checksum = sum(dat[:6]) % 256 - elif car_fingerprint in CHECKSUM["7B"]: + else: # Checksum of first 6 Bytes and last Byte as seen on 2018 Kia Stinger checksum = (sum(dat[:6]) + dat[7]) % 256 @@ -44,20 +61,29 @@ def create_lkas11(packer, car_fingerprint, apply_steer, steer_req, cnt, enabled, return packer.make_can_msg("LKAS11", 0, values) -def create_clu11(packer, clu11, button): + +def create_clu11(packer, frame, clu11, button): + values = clu11 + values["CF_Clu_CruiseSwState"] = button + values["CF_Clu_CruiseSwState"] = frame % 0x10 + return packer.make_can_msg("CLU11", 0, values) + + +def create_lfa_mfa(packer, frame, enabled): values = { - "CF_Clu_CruiseSwState": button, - "CF_Clu_CruiseSwMain": clu11["CF_Clu_CruiseSwMain"], - "CF_Clu_SldMainSW": clu11["CF_Clu_SldMainSW"], - "CF_Clu_ParityBit1": clu11["CF_Clu_ParityBit1"], - "CF_Clu_VanzDecimal": clu11["CF_Clu_VanzDecimal"], - "CF_Clu_Vanz": clu11["CF_Clu_Vanz"], - "CF_Clu_SPEED_UNIT": clu11["CF_Clu_SPEED_UNIT"], - "CF_Clu_DetentOut": clu11["CF_Clu_DetentOut"], - "CF_Clu_RheostatLevel": clu11["CF_Clu_RheostatLevel"], - "CF_Clu_CluInfo": clu11["CF_Clu_CluInfo"], - "CF_Clu_AmpInfo": clu11["CF_Clu_AmpInfo"], - "CF_Clu_AliveCnt1": 0, + "ACTIVE": enabled, } - return packer.make_can_msg("CLU11", 0, values) + # ACTIVE 1 = Green steering wheel icon + + # LFA_USM 2 & 3 = LFA cancelled, fast loud beeping + # LFA_USM 0 & 1 = No mesage + + # LFA_SysWarning 1 = "Switching to HDA", short beep + # LFA_SysWarning 2 = "Switching to Smart Cruise control", short beep + # LFA_SysWarning 3 = LFA error + + # ACTIVE2: nothing + # HDA_USM: nothing + + return packer.make_can_msg("LFAHDA_MFC", 0, values) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 2ec2d57a..649d181c 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -2,7 +2,7 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event -from selfdrive.car.hyundai.values import Ecu, ECU_FINGERPRINT, CAR, get_hud_alerts, FINGERPRINTS +from selfdrive.car.hyundai.values import Ecu, ECU_FINGERPRINT, CAR, FINGERPRINTS from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase @@ -20,6 +20,9 @@ class CarInterface(CarInterfaceBase): ret.safetyModel = car.CarParams.SafetyModel.hyundai ret.radarOffCan = True + # Hyundai port is a community feature for now + ret.communityFeature = True + ret.steerActuatorDelay = 0.1 # Default delay ret.steerRateCost = 0.5 ret.steerLimitTimer = 0.4 @@ -29,14 +32,25 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.766 - # Values from optimizer ret.steerRatio = 16.55 # 13.8 is spec end-to-end tire_stiffness_factor = 0.82 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[9., 22.], [9., 22.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.35], [0.05, 0.09]] - ret.minSteerSpeed = 0. + elif candidate == CAR.SONATA: + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 1513. + STD_CARGO_KG + ret.wheelbase = 2.84 + ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + elif candidate == CAR.PALISADE: + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 1999. + STD_CARGO_KG + ret.wheelbase = 2.90 + ret.steerRatio = 13.75 * 1.15 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.KIA_SORENTO: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1985. + STD_CARGO_KG @@ -44,25 +58,30 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] - ret.minSteerSpeed = 0. - elif candidate == CAR.ELANTRA: + elif candidate in [CAR.ELANTRA, CAR.ELANTRA_GT_I30]: ret.lateralTuning.pid.kf = 0.00006 ret.mass = 1275. + STD_CARGO_KG ret.wheelbase = 2.7 - ret.steerRatio = 13.73 #Spec - tire_stiffness_factor = 0.385 + ret.steerRatio = 15.4 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535 + tire_stiffness_factor = 0.385 # stiffnessFactor settled on 1.0081302973865127 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.minSteerSpeed = 32 * CV.MPH_TO_MS - elif candidate == CAR.GENESIS: + elif candidate == CAR.HYUNDAI_GENESIS: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 2060. + STD_CARGO_KG ret.wheelbase = 3.01 ret.steerRatio = 16.5 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] - ret.minSteerSpeed = 35 * CV.MPH_TO_MS - elif candidate == CAR.KIA_OPTIMA: + ret.minSteerSpeed = 60 * CV.KPH_TO_MS + elif candidate in [CAR.GENESIS_G90, CAR.GENESIS_G80]: + ret.mass = 2200 + ret.wheelbase = 3.15 + ret.steerRatio = 12.069 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] + elif candidate in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3558. * CV.LB_TO_KG ret.wheelbase = 2.80 @@ -77,7 +96,48 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] - ret.minSteerSpeed = 0. + elif candidate == CAR.KONA: + ret.lateralTuning.pid.kf = 0.00006 + ret.mass = 1275. + STD_CARGO_KG + ret.wheelbase = 2.7 + ret.steerRatio = 13.73 #Spec + tire_stiffness_factor = 0.385 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + elif candidate == CAR.IONIQ: + ret.lateralTuning.pid.kf = 0.00006 + ret.mass = 1275. + STD_CARGO_KG + ret.wheelbase = 2.7 + ret.steerRatio = 13.73 #Spec + tire_stiffness_factor = 0.385 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + ret.minSteerSpeed = 32 * CV.MPH_TO_MS + elif candidate == CAR.KONA_EV: + ret.lateralTuning.pid.kf = 0.00006 + ret.mass = 1685. + STD_CARGO_KG + ret.wheelbase = 2.7 + ret.steerRatio = 13.73 #Spec + tire_stiffness_factor = 0.385 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + elif candidate == CAR.IONIQ_EV_LTD: + ret.lateralTuning.pid.kf = 0.00006 + ret.mass = 1490. + STD_CARGO_KG #weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx + ret.wheelbase = 2.7 + ret.steerRatio = 13.73 #Spec + tire_stiffness_factor = 0.385 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + ret.minSteerSpeed = 32 * CV.MPH_TO_MS + elif candidate == CAR.KIA_FORTE: + ret.lateralTuning.pid.kf = 0.00005 + ret.mass = 3558. * CV.LB_TO_KG + ret.wheelbase = 2.80 + ret.steerRatio = 13.75 + tire_stiffness_factor = 0.5 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.centerToFront = ret.wheelbase * 0.4 @@ -105,11 +165,7 @@ class CarInterface(CarInterfaceBase): ret.buttonEvents = [] events = self.create_common_events(ret) - - if ret.cruiseState.enabled and not self.cruise_enabled_prev: - events.append(create_event('pcmEnable', [ET.ENABLE])) - elif not ret.cruiseState.enabled: - events.append(create_event('pcmDisable', [ET.USER_DISABLE])) + #TODO: addd abs(self.CS.angle_steers) > 90 to 'steerTempUnavailable' event # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.: @@ -121,18 +177,12 @@ class CarInterface(CarInterfaceBase): ret.events = events - self.gas_pressed_prev = ret.gasPressed - self.brake_pressed_prev = ret.brakePressed - self.cruise_enabled_prev = ret.cruiseState.enabled - self.CS.out = ret.as_reader() return self.CS.out def apply(self, c): - - hud_alert = get_hud_alerts(c.hudControl.visualAlert) - - can_sends = self.CC.update(c.enabled, self.CS, c.actuators, - c.cruiseControl.cancel, hud_alert) - + can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, + c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.leftLaneVisible, + c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) + self.frame += 1 return can_sends diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index b2f76511..b5bdcd83 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -1,5 +1,75 @@ #!/usr/bin/env python3 +import os +import time +from cereal import car +from opendbc.can.parser import CANParser from selfdrive.car.interfaces import RadarInterfaceBase +from selfdrive.car.hyundai.values import DBC + + +def get_radar_can_parser(CP): + signals = [ + # sig_name, sig_address, default + ("ACC_ObjStatus", "SCC11", 0), + ("ACC_ObjLatPos", "SCC11", 0), + ("ACC_ObjDist", "SCC11", 0), + ("ACC_ObjRelSpd", "SCC11", 0), + ] + checks = [ + # address, frequency + ("SCC11", 50), + ] + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) + class RadarInterface(RadarInterfaceBase): - pass + def __init__(self, CP): + super().__init__(CP) + self.rcp = get_radar_can_parser(CP) + self.updated_messages = set() + self.trigger_msg = 0x420 + self.track_id = 0 + self.radar_off_can = CP.radarOffCan + + def update(self, can_strings): + if self.radar_off_can: + if 'NO_RADAR_SLEEP' not in os.environ: + time.sleep(0.05) # radard runs on RI updates + + return car.RadarData.new_message() + + vls = self.rcp.update_strings(can_strings) + self.updated_messages.update(vls) + + if self.trigger_msg not in self.updated_messages: + return None + + rr = self._update(self.updated_messages) + self.updated_messages.clear() + + return rr + + def _update(self, updated_messages): + ret = car.RadarData.new_message() + cpt = self.rcp.vl + errors = [] + if not self.rcp.can_valid: + errors.append("canError") + ret.errors = errors + + valid = cpt["SCC11"]['ACC_ObjStatus'] + if valid: + for ii in range(2): + if ii not in self.pts: + self.pts[ii] = car.RadarData.RadarPoint.new_message() + self.pts[ii].trackId = self.track_id + self.track_id += 1 + self.pts[ii].dRel = cpt["SCC11"]['ACC_ObjDist'] # from front of car + self.pts[ii].yRel = -cpt["SCC11"]['ACC_ObjLatPos'] # in car frame's y axis, left is negative + self.pts[ii].vRel = cpt["SCC11"]['ACC_ObjRelSpd'] + self.pts[ii].aRel = float('nan') + self.pts[ii].yvRel = float('nan') + self.pts[ii].measured = True + + ret.points = list(self.pts.values()) + return ret diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 7fe689c6..480ed6f2 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -2,16 +2,7 @@ from cereal import car from selfdrive.car import dbc_dict Ecu = car.CarParams.Ecu -VisualAlert = car.CarControl.HUDControl.VisualAlert - -def get_hud_alerts(visual_alert): - if visual_alert == VisualAlert.steerRequired: - return 5 - else: - return 0 - # Steer torque limits - class SteerLimitParams: STEER_MAX = 255 # 409 is the max, 255 is stock STEER_DELTA_UP = 3 @@ -20,13 +11,27 @@ class SteerLimitParams: STEER_DRIVER_MULTIPLIER = 2 STEER_DRIVER_FACTOR = 1 + class CAR: ELANTRA = "HYUNDAI ELANTRA LIMITED ULTIMATE 2017" - GENESIS = "HYUNDAI GENESIS 2018" - KIA_OPTIMA = "KIA OPTIMA SX 2019" + ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT" + GENESIS_G80 = "GENESIS G80 2017" + GENESIS_G90 = "GENESIS G90 2017" + HYUNDAI_GENESIS = "HYUNDAI GENESIS 2015-2016" + IONIQ = "HYUNDAI IONIQ HYBRID PREMIUM 2017" + IONIQ_EV_LTD = "HYUNDAI IONIQ ELECTRIC LIMITED 2019" + KIA_FORTE = "KIA FORTE E 2018" + KIA_OPTIMA = "KIA OPTIMA SX 2019 & 2016" + KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019" KIA_SORENTO = "KIA SORENTO GT LINE 2018" KIA_STINGER = "KIA STINGER GT2 2018" + KONA = "HYUNDAI KONA 2019" + KONA_EV = "HYUNDAI KONA ELECTRIC 2019" SANTA_FE = "HYUNDAI SANTA FE LIMITED 2019" + SANTA_FE_1 = "HYUNDAI SANTA FE has no scc" + SONATA = "HYUNDAI SONATA 2020" + PALISADE = "HYUNDAI PALISADE 2020" + class Buttons: NONE = 0 @@ -38,12 +43,42 @@ FINGERPRINTS = { CAR.ELANTRA: [{ 66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 897: 8, 832: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1170: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1345: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2001: 8, 2003: 8, 2004: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 }], - CAR.GENESIS: [{ + CAR.ELANTRA_GT_I30: [{ + 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1193: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1952: 8, 1960: 8, 1988: 8, 2000: 8, 2001: 8, 2005: 8, 2008: 8, 2009: 8, 2013: 8, 2017: 8, 2025: 8 + }, + { + 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 897: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1440: 8, 1456: 4, 1470: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8 + }, + { + 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 897: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1486: 8, 1487: 8, 1491: 8, 1960: 8, 1990: 8, 1998: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2008: 8, 2009: 8, 2012: 8, 2013: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + }], + CAR.HYUNDAI_GENESIS: [{ 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1342: 6, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4 }, { 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1378: 4, 1379: 8, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4 + }, + { + 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1268: 8, 1280: 1, 1281: 3, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1437: 8, 1456: 4 + }, + { + 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1378: 4, 1379: 8, 1384: 5, 1407: 8, 1425: 2, 1427: 6, 1437: 8, 1456: 4 + }, + { + 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1437: 8, 1456: 4 }], + CAR.SANTA_FE: [{ + 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1379: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8 + }, + { + 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 764: 8, 809: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1180: 8, 1183: 8, 1186: 2, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8 + }, + { + 67: 8, 68: 8, 80: 4, 160: 8, 161: 8, 272: 8, 288: 4, 339: 8, 356: 8, 357: 8, 399: 8, 544: 8, 608: 8, 672: 8, 688: 5, 704: 1, 790: 8, 809: 8, 848: 8, 880: 8, 898: 8, 900: 8, 901: 8, 904: 8, 1056: 8, 1064: 8, 1065: 8, 1072: 8, 1075: 8, 1087: 8, 1088: 8, 1151: 8, 1200: 8, 1201: 8, 1232: 4, 1264: 8, 1265: 8, 1266: 8, 1296: 8, 1306: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1348: 8, 1349: 8, 1369: 8, 1370: 8, 1371: 8, 1407: 8, 1415: 8, 1419: 8, 1440: 8, 1442: 4, 1461: 8, 1470: 8 + }], + CAR.SONATA: [ + {67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 545: 8, 546: 8, 547: 8, 548: 8, 549: 8, 550: 8, 576: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 8, 865: 8, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 908: 8, 909: 8, 912: 7, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1089: 5, 1107: 5, 1108: 8, 1114: 8, 1136: 8, 1145: 8, 1151: 8, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 8, 1170: 8, 1173: 8, 1180: 8, 1183: 8, 1184: 8, 1186: 2, 1191: 2, 1193: 8, 1210: 8, 1225: 8, 1227: 8, 1265: 4, 1268: 8, 1280: 8, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1330: 8, 1339: 8, 1342: 6, 1343: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1394: 8, 1407: 8, 1419: 8, 1427: 6, 1446: 8, 1456: 4, 1460: 8, 1470: 8, 1485: 8, 1504: 3}, + ], CAR.KIA_OPTIMA: [ { 64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1186: 2, 1191: 2, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 1952: 8, 1960: 8, 1988: 8, 1996: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 @@ -58,13 +93,42 @@ FINGERPRINTS = { CAR.KIA_STINGER: [{ 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 359: 8, 544: 8, 576: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1378: 4, 1379: 8, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1456: 4, 1470: 8 }], - CAR.SANTA_FE: [{ - 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1379: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8 + CAR.GENESIS_G80: [{ + 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1024: 2, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1191: 2, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1434: 2, 1456: 4, 1470: 8 }, { - 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 764: 8, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1180: 8, 1183: 8, 1186: 2, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8 - } - ], + 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 359: 8, 544: 8, 546: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1157: 4, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1434: 2, 1437: 8, 1456: 4, 1470: 8 + }, + { + 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1157: 4, 1162: 8, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1193: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1437: 8, 1456: 4, 1470: 8 + }], + CAR.GENESIS_G90: [{ + 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 359: 8, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1162: 4, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1434: 2, 1456: 4, 1470: 8, 1988: 8, 2000: 8, 2003: 8, 2004: 8, 2005: 8, 2008: 8, 2011: 8, 2012: 8, 2013: 8 + }], + CAR.IONIQ: [{ + 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 576: 8, 593: 8, 688: 5, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 6, 1173: 8, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1322: 8, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1407: 8, 1419: 8, 1427: 6, 1429: 8, 1430: 8, 1448: 8, 1456: 4, 1470:8, 1476: 8, 1535: 8 + }], + CAR.IONIQ_EV_LTD: [{ + 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 7, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1168: 7, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1407: 8, 1419: 8, 1425: 2, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1507: 8, 1535: 8 + }], + CAR.KONA: [{ + 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 354: 3, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 909: 8, 916: 8, 1040: 8, 1078: 4, 1107: 5, 1136: 8, 1156: 8, 1170: 8, 1173: 8, 1191: 2, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1384: 8, 1394: 8,1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 2004: 8, 2009: 8, 2012: 8 + }], + CAR.KONA_EV: [{ + 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 549: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1307: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1378: 4, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8 + }], + CAR.KIA_FORTE: [{ + 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1078: 4, 1107: 5, 1136: 8, 1156: 8, 1170: 8, 1173: 8, 1191: 2, 1225: 8, 1265: 4, 1280: 4, 1287: 4, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1384: 8, 1394: 8, 1407: 8, 1427: 6, 1456: 4, 1470: 8 + }], + CAR.KIA_OPTIMA_H: [{ + 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 6, 1151: 6, 1168: 7, 1173: 8, 1236: 2, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1371: 8, 1407: 8, 1419: 8, 1427: 6, 1429: 8, 1430: 8, 1448: 8, 1456: 4, 1470: 8, 1476: 8, 1535: 8 + }, + { + 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 576: 8, 593: 8, 688: 5, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 909: 8, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 6, 1151: 6, 1168: 7, 1173: 8, 1180: 8, 1186: 2, 1191: 2, 1265: 4, 1268: 8, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1371: 8, 1407: 8, 1419: 8, 1420: 8, 1425: 2, 1427: 6, 1429: 8, 1430: 8, 1448: 8, 1456: 4, 1470: 8, 1476: 8, 1535: 8 + }], + CAR.PALISADE: [{ + 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 549: 8, 576: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1123: 8, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1180: 8, 1186: 2, 1191: 2, 1193: 8, 1210: 8, 1225: 8, 1227: 8, 1265: 4, 1280: 8, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1407: 8, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 2000: 8, 2005: 8, 2008: 8 + }], } ECU_FINGERPRINT = { @@ -72,23 +136,34 @@ ECU_FINGERPRINT = { } CHECKSUM = { - "crc8": [CAR.SANTA_FE], - "6B": [CAR.KIA_SORENTO, CAR.GENESIS], - "7B": [CAR.KIA_STINGER, CAR.ELANTRA, CAR.KIA_OPTIMA], + "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE], + "6B": [CAR.KIA_SORENTO, CAR.HYUNDAI_GENESIS], } FEATURES = { - "use_cluster_gears": [CAR.ELANTRA], # Use Cluster for Gear Selection, rather than Transmission - "use_tcu_gears": [CAR.KIA_OPTIMA], # Use TCU Message for Gear Selection + "use_cluster_gears": [CAR.ELANTRA, CAR.KONA, CAR.ELANTRA_GT_I30], # Use Cluster for Gear Selection, rather than Transmission + "use_tcu_gears": [CAR.KIA_OPTIMA], # Use TCU Message for Gear Selection + "use_elect_gears": [CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV], # Use TCU Message for Gear Selection } DBC = { CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None), - CAR.GENESIS: dbc_dict('hyundai_kia_generic', None), + CAR.ELANTRA_GT_I30: dbc_dict('hyundai_kia_generic', None), + CAR.GENESIS_G80: dbc_dict('hyundai_kia_generic', None), + CAR.GENESIS_G90: dbc_dict('hyundai_kia_generic', None), + CAR.HYUNDAI_GENESIS: dbc_dict('hyundai_kia_generic', None), + CAR.IONIQ: dbc_dict('hyundai_kia_generic', None), + CAR.IONIQ_EV_LTD: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_FORTE: dbc_dict('hyundai_kia_generic', None), CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None), CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None), + CAR.KONA: dbc_dict('hyundai_kia_generic', None), + CAR.KONA_EV: dbc_dict('hyundai_kia_generic', None), CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', None), + CAR.SONATA: dbc_dict('hyundai_kia_generic', None), + CAR.PALISADE: dbc_dict('hyundai_kia_generic', None), } -STEER_THRESHOLD = 100 +STEER_THRESHOLD = 150 diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index a25df91f..41a8c5ee 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -17,9 +17,6 @@ class CarInterfaceBase(): 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) @@ -53,6 +50,7 @@ class CarInterfaceBase(): ret.steerControlType = car.CarParams.SteerControlType.torque ret.steerMaxBP = [0.] ret.steerMaxV = [1.] + ret.minSteerSpeed = 0. # stock ACC by default ret.enableCruise = True @@ -81,7 +79,7 @@ class CarInterfaceBase(): 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: @@ -99,19 +97,25 @@ class CarInterfaceBase(): if cs_out.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) - # TODO: move this stuff to the capnp strut - if getattr(self.CS, "steer_error", False): + if cs_out.steerError: events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) - elif getattr(self.CS, "steer_warning", False): + elif cs_out.steerWarning: events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) # Disable on rising edge of gas or brake. Also disable on brake when speed > 0. # Optionally allow to press gas at zero speed to resume. # e.g. Chrysler does not spam the resume button yet, so resuming with gas is handy. FIXME! - if (cs_out.gasPressed and (not self.gas_pressed_prev) and cs_out.vEgo > gas_resume_speed) or \ - (cs_out.brakePressed and (not self.brake_pressed_prev or not cs_out.standstill)): + 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 @@ class CarStateBase: 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/mock/interface.py b/selfdrive/car/mock/interface.py index dc972440..3c97fb30 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -73,13 +73,13 @@ class CarInterface(CarInterfaceBase): ret.aEgo = a ret.brakePressed = a < -0.5 - self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate - ret.yawRate = self.yaw_rate ret.standstill = self.speed < 0.01 ret.wheelSpeeds.fl = self.speed ret.wheelSpeeds.fr = self.speed ret.wheelSpeeds.rl = self.speed ret.wheelSpeeds.rr = self.speed + + self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate curvature = self.yaw_rate / max(self.speed, 1.) ret.steeringAngle = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 335e1893..7f3fc3d8 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -6,7 +6,7 @@ from opendbc.can.packer import CANPacker class CarControllerParams(): - def __init__(self, car_fingerprint): + def __init__(self): self.STEER_MAX = 2047 # max_steer 4095 self.STEER_STEP = 2 # how often we update the steer cmd self.STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max @@ -27,7 +27,7 @@ class CarController(): # Setup detection helper. Routes commands to # an appropriate CAN bus number. - self.params = CarControllerParams(CP.carFingerprint) + self.params = CarControllerParams() self.packer = CANPacker(DBC[CP.carFingerprint]['pt']) def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line): @@ -56,7 +56,7 @@ class CarController(): if not lkas_enabled: apply_steer = 0 - can_sends.append(subarucan.create_steering_control(self.packer, CS.CP.carFingerprint, apply_steer, frame, P.STEER_STEP)) + can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, P.STEER_STEP)) self.apply_steer_last = apply_steer diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index a6cd6e9f..ebfbaaa3 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -1,5 +1,6 @@ import copy from cereal import car +from opendbc.can.can_define import CANDefine from selfdrive.config import Conversions as CV from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser @@ -11,6 +12,8 @@ class CarState(CarStateBase): super().__init__(CP) self.left_blinker_cnt = 0 self.right_blinker_cnt = 0 + can_define = CANDefine(DBC[CP.carFingerprint]['pt']) + self.shifter_values = can_define.dv["Transmission"]['Gear'] def update(self, cp, cp_cam): ret = car.CarState.new_message() @@ -35,6 +38,9 @@ class CarState(CarStateBase): self.right_blinker_cnt = 50 if cp.vl["Dashlights"]['RIGHT_BLINKER'] else max(self.right_blinker_cnt - 1, 0) ret.rightBlinker = self.right_blinker_cnt > 0 + can_gear = int(cp.vl["Transmission"]['Gear']) + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + ret.steeringAngle = cp.vl["Steering_Torque"]['Steering_Angle'] ret.steeringTorque = cp.vl["Steering_Torque"]['Steer_Torque_Sensor'] ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.car_fingerprint] @@ -80,6 +86,7 @@ class CarState(CarStateBase): ("DOOR_OPEN_RR", "BodyInfo", 1), ("DOOR_OPEN_RL", "BodyInfo", 1), ("Units", "Dash_State", 1), + ("Gear", "Transmission", 0), ] checks = [ @@ -104,7 +111,6 @@ class CarState(CarStateBase): ("Main", "ES_Distance", 0), ("Signal3", "ES_Distance", 0), - ("Checksum", "ES_LKAS_State", 0), ("Counter", "ES_LKAS_State", 0), ("Keep_Hands_On_Wheel", "ES_LKAS_State", 0), ("Empty_Box", "ES_LKAS_State", 0), diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 0f2f4176..e205c7cc 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -1,7 +1,5 @@ #!/usr/bin/env python3 from cereal import car -from selfdrive.config import Conversions as CV -from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from selfdrive.car.subaru.values import CAR from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase @@ -20,6 +18,9 @@ class CarInterface(CarInterfaceBase): ret.radarOffCan = True ret.safetyModel = car.CarParams.SafetyModel.subaru + # Subaru port is a community feature, since we don't own one to test + ret.communityFeature = True + # force openpilot to fake the stock camera, since car harness is not supported yet and old style giraffe (with switches) # was never released ret.enableCamera = True @@ -56,25 +57,13 @@ class CarInterface(CarInterfaceBase): ret.canValid = self.cp.can_valid and self.cp_cam.can_valid ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False - ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo) buttonEvents = [] be = car.CarState.ButtonEvent.new_message() be.type = car.CarState.ButtonEvent.Type.accelCruise buttonEvents.append(be) - events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.unknown]) - - if ret.cruiseState.enabled and not self.cruise_enabled_prev: - events.append(create_event('pcmEnable', [ET.ENABLE])) - if not ret.cruiseState.enabled: - events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - - ret.events = events - - self.gas_pressed_prev = ret.gasPressed - self.brake_pressed_prev = ret.brakePressed - self.cruise_enabled_prev = ret.cruiseState.enabled + ret.events = self.create_common_events(ret) self.CS.out = ret.as_reader() return self.CS.out diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index 96abfaaf..d0850afc 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -1,36 +1,24 @@ import copy from cereal import car -from selfdrive.car.subaru.values import CAR VisualAlert = car.CarControl.HUDControl.VisualAlert -def subaru_checksum(packer, values, addr): - dat = packer.make_can_msg(addr, 0, values)[2] - return (sum(dat[1:]) + (addr >> 8) + addr) & 0xff +def create_steering_control(packer, apply_steer, frame, steer_step): -def create_steering_control(packer, car_fingerprint, apply_steer, frame, steer_step): + # counts from 0 to 15 then back to 0 + 16 for enable bit + idx = ((frame // steer_step) % 16) - if car_fingerprint == CAR.IMPREZA: - #counts from 0 to 15 then back to 0 + 16 for enable bit - idx = ((frame // steer_step) % 16) - - values = { - "Counter": idx, - "LKAS_Output": apply_steer, - "LKAS_Request": 1 if apply_steer != 0 else 0, - "SET_1": 1 - } - values["Checksum"] = subaru_checksum(packer, values, 0x122) + values = { + "Counter": idx, + "LKAS_Output": apply_steer, + "LKAS_Request": 1 if apply_steer != 0 else 0, + "SET_1": 1 + } return packer.make_can_msg("ES_LKAS", 0, values) -def create_steering_status(packer, car_fingerprint, apply_steer, frame, steer_step): - - if car_fingerprint == CAR.IMPREZA: - values = {} - values["Checksum"] = subaru_checksum(packer, {}, 0x322) - - return packer.make_can_msg("ES_LKAS_State", 0, values) +def create_steering_status(packer, apply_steer, frame, steer_step): + return packer.make_can_msg("ES_LKAS_State", 0, {}) def create_es_distance(packer, es_distance_msg, pcm_cancel_cmd): @@ -38,8 +26,6 @@ def create_es_distance(packer, es_distance_msg, pcm_cancel_cmd): if pcm_cancel_cmd: values["Main"] = 1 - values["Checksum"] = subaru_checksum(packer, values, 545) - return packer.make_can_msg("ES_Distance", 0, values) def create_es_lkas(packer, es_lkas_msg, visual_alert, left_line, right_line): @@ -51,6 +37,4 @@ def create_es_lkas(packer, es_lkas_msg, visual_alert, left_line, right_line): values["LKAS_Left_Line_Visible"] = int(left_line) values["LKAS_Right_Line_Visible"] = int(right_line) - values["Checksum"] = subaru_checksum(packer, values, 802) - return packer.make_can_msg("ES_LKAS_State", 0, values) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 6f21fec4..58a970f7 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -29,25 +29,10 @@ def accel_hysteresis(accel, accel_steady, enabled): return accel, accel_steady -def process_hud_alert(hud_alert): - # initialize to no alert - steer = 0 - fcw = 0 - - if hud_alert == VisualAlert.fcw: - fcw = 1 - elif hud_alert == VisualAlert.steerRequired: - steer = 1 - - return steer, fcw - - class CarController(): def __init__(self, dbc_name, CP, VM): - self.braking = False self.last_steer = 0 self.accel_steady = 0. - self.car_fingerprint = CP.carFingerprint self.alert_active = False self.last_standstill = False self.standstill_req = False @@ -135,37 +120,35 @@ class CarController(): can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead)) if (frame % 2 == 0) and (CS.CP.enableGasInterceptor): - # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. - # This prevents unexpected pedal range rescaling - can_sends.append(create_gas_command(self.packer, apply_gas, frame//2)) + # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. + # This prevents unexpected pedal range rescaling + can_sends.append(create_gas_command(self.packer, apply_gas, frame//2)) # ui mesg is at 100Hz but we send asap if: # - there is something to display # - there is something to stop displaying - alert_out = process_hud_alert(hud_alert) - steer, fcw = alert_out + fcw_alert = hud_alert == VisualAlert.fcw + steer_alert = hud_alert == VisualAlert.steerRequired - if (any(alert_out) and not self.alert_active) or \ - (not any(alert_out) and self.alert_active): + send_ui = False + if ((fcw_alert or steer_alert) and not self.alert_active) or \ + (not (fcw_alert or steer_alert) and self.alert_active): send_ui = True self.alert_active = not self.alert_active - else: - send_ui = False - - # disengage msg causes a bad fault sound so play a good sound instead - if pcm_cancel_cmd: + elif pcm_cancel_cmd: + # forcing the pcm to disengage causes a bad fault sound so play a good sound instead send_ui = True if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus: - can_sends.append(create_ui_command(self.packer, steer, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart)) + can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart)) if frame % 100 == 0 and Ecu.dsu in self.fake_ecus: - can_sends.append(create_fcw_command(self.packer, fcw)) + can_sends.append(create_fcw_command(self.packer, fcw_alert)) #*** static msgs *** for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS: - if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars: + if frame % fr_step == 0 and ecu in self.fake_ecus and CS.CP.carFingerprint in cars: can_sends.append(make_can_msg(addr, vl, bus)) return can_sends diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index e5d58f0d..c9970bea 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -36,7 +36,7 @@ class CarState(CarStateBase): ret.gasPressed = ret.gas > 15 else: ret.gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL'] - ret.gasPressed = ret.gas > 1e-5 + ret.gasPressed = cp.vl["PCM_CRUISE"]['GAS_RELEASED'] == 0 ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS @@ -72,6 +72,7 @@ class CarState(CarStateBase): ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS'] # we could use the override bit from dbc, but it's triggered at too high torque values ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD + ret.steerWarning = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5] if self.CP.carFingerprint == CAR.LEXUS_IS: ret.cruiseState.available = cp.vl["DSU_CRUISE"]['MAIN_ON'] != 0 @@ -99,7 +100,10 @@ class CarState(CarStateBase): ret.espDisabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] != 0 # 2 is standby, 10 is active. TODO: check that everything else is really a faulty state self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE'] - self.steer_warning = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5] + + if self.CP.carFingerprint in TSS2_CAR: + ret.leftBlindspot = cp.vl["BSM"]['L_ADJACENT'] == 1 + ret.rightBlindspot = cp.vl["BSM"]['R_ADJACENT'] == 1 return ret @@ -126,6 +130,7 @@ class CarState(CarStateBase): ("STEER_RATE", "STEER_ANGLE_SENSOR", 0), ("CRUISE_ACTIVE", "PCM_CRUISE", 0), ("CRUISE_STATE", "PCM_CRUISE", 0), + ("GAS_RELEASED", "PCM_CRUISE", 1), ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0), ("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0), @@ -165,6 +170,10 @@ class CarState(CarStateBase): signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0)) checks.append(("GAS_SENSOR", 50)) + if CP.carFingerprint in TSS2_CAR: + signals += [("L_ADJACENT", "BSM", 0)] + signals += [("R_ADJACENT", "BSM", 0)] + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) @staticmethod diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index c8e6da55..e24c38f6 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -7,8 +7,8 @@ from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, from selfdrive.swaglog import cloudlog from selfdrive.car.interfaces import CarInterfaceBase -class CarInterface(CarInterfaceBase): +class CarInterface(CarInterfaceBase): @staticmethod def compute_gb(accel, speed): return float(accel) / 3.0 @@ -121,7 +121,7 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00006 - elif candidate == CAR.HIGHLANDER_TSS2: + elif candidate in [CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.84988 # 112.2 in = 2.84988 m @@ -157,9 +157,9 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.68986 ret.steerRatio = 14.3 tire_stiffness_factor = 0.7933 - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15], [0.05]] ret.mass = 3370. * CV.LB_TO_KG + STD_CARGO_KG - ret.lateralTuning.pid.kf = 0.00007818594 + ret.lateralTuning.pid.kf = 0.00004 elif candidate == CAR.RAV4H_TSS2: stop_and_go = True @@ -167,9 +167,9 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.68986 ret.steerRatio = 14.3 tire_stiffness_factor = 0.7933 - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15], [0.05]] ret.mass = 3800. * CV.LB_TO_KG + STD_CARGO_KG - ret.lateralTuning.pid.kf = 0.00007818594 + ret.lateralTuning.pid.kf = 0.00004 elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2]: stop_and_go = True @@ -195,10 +195,10 @@ class CarInterface(CarInterfaceBase): stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 3.03 - ret.steerRatio = 16.0 + ret.steerRatio = 15.5 tire_stiffness_factor = 0.444 ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.02]] ret.lateralTuning.pid.kf = 0.00007818594 elif candidate == CAR.LEXUS_IS: @@ -290,7 +290,6 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam) ret.canValid = self.cp.can_valid and self.cp_cam.can_valid - ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo) ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False ret.buttonEvents = [] @@ -310,18 +309,8 @@ class CarInterface(CarInterfaceBase): # while in standstill, send a user alert events.append(create_event('manualRestart', [ET.WARNING])) - # enable request in prius is simple, as we activate when Toyota is active (rising edge) - if ret.cruiseState.enabled and not self.cruise_enabled_prev: - events.append(create_event('pcmEnable', [ET.ENABLE])) - elif not ret.cruiseState.enabled: - events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - ret.events = events - self.gas_pressed_prev = ret.gasPressed - self.brake_pressed_prev = ret.brakePressed - self.cruise_enabled_prev = ret.cruiseState.enabled - self.CS.out = ret.as_reader() return self.CS.out diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index 80977b70..12387d48 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -30,11 +30,8 @@ def _create_radar_can_parser(car_fingerprint): class RadarInterface(RadarInterfaceBase): def __init__(self, CP): - # radar - self.pts = {} + super().__init__(CP) self.track_id = 0 - - self.delay = 0 # Delay of radar self.radar_ts = CP.radarTimeStep if CP.carFingerprint in TSS2_CAR: diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 98065aa2..ff530ad5 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -24,6 +24,7 @@ class CAR: HIGHLANDER = "TOYOTA HIGHLANDER 2017" HIGHLANDER_TSS2 = "TOYOTA HIGHLANDER 2020" HIGHLANDERH = "TOYOTA HIGHLANDER HYBRID 2018" + HIGHLANDERH_TSS2 = "TOYOTA HIGHLANDER HYBRID 2020" AVALON = "TOYOTA AVALON 2016" RAV4_TSS2 = "TOYOTA RAV4 2019" COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019" @@ -97,7 +98,7 @@ FINGERPRINTS = { }, #2017 German Prius { - 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296:8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8,740: 5, 742: 8, 743: 8, 767:4, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1792: 8, 1767:4, 800: 8, 1863:8, 1904: 8, 1912: 8, 1984: 8, 1988: 8, 1990: 8, 1992: 8, 1996:8, 1998: 8, 2002: 8, 2010: 8, 2015: 8, 2016: 8, 2018: 8, 2024: 8, 2026: 8, 2030: 8 + 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296:8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8,740: 5, 742: 8, 743: 8, 767:4, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1792: 8, 1767:4, 1863:8, 1904: 8, 1912: 8, 1984: 8, 1988: 8, 1990: 8, 1992: 8, 1996:8, 1998: 8, 2002: 8, 2010: 8, 2015: 8, 2016: 8, 2018: 8, 2024: 8, 2026: 8, 2030: 8 }], #Corolla w/ added Pedal Support (512L and 513L) CAR.COROLLA: [{ @@ -150,7 +151,7 @@ FINGERPRINTS = { }, { # 2019 XSE - 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 942: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1792: 8, 1767:4, 800: 8, 1808: 8, 1816: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1937: 8, 1945: 8, 1953: 8, 1961: 8, 1968: 8, 1976: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8 + 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 942: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1792: 8, 1767:4, 1808: 8, 1816: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1937: 8, 1945: 8, 1953: 8, 1961: 8, 1968: 8, 1976: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8 }], CAR.CAMRYH: [ #SE, LE and LE with Blindspot Monitor @@ -191,6 +192,9 @@ FINGERPRINTS = { # 2019 Highlander Hybrid Limited Platinum 36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }], + CAR.HIGHLANDERH_TSS2: [{ + 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 761: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 881: 8, 885: 8, 891: 8, 896: 8, 898: 8, 918: 8, 942: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 971: 7, 975: 5, 987: 8, 993: 8, 1014: 8, 1017: 8, 1020: 8, 1059: 1, 1063: 8, 1071: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1552: 8, 1553: 8, 1556: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8 + }], CAR.AVALON: [{ 36: 8, 37: 8, 170: 8, 180: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 905: 8, 911: 1, 916: 2, 921: 8, 933: 6, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 1005: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1206: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1558: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }], @@ -257,7 +261,7 @@ FINGERPRINTS = { } # Don't use theses fingerprints for fingerprinting, they are still needed for ECU detection -IGNORED_FINGERPRINTS = [CAR.RAV4H_TSS2] +IGNORED_FINGERPRINTS = [CAR.RAV4H_TSS2, CAR.HIGHLANDERH_TSS2] FW_VERSIONS = { CAR.AVALON: { @@ -277,11 +281,13 @@ FW_VERSIONS = { b'\x018966333P4500\x00\x00\x00\x00', b'\x018966333P4700\x00\x00\x00\x00', b'\x018966333Q6200\x00\x00\x00\x00', + b'\x018966306Q4100\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ b'8821F0607200 ', b'8821F0601300 ', b'8821F0603300 ', + b'8821F0608000 ', ], (Ecu.esp, 0x7b0, None): [ b'F152606210\x00\x00\x00\x00\x00\x00', @@ -292,17 +298,20 @@ FW_VERSIONS = { (Ecu.eps, 0x7a1, None): [ b'8965B33540\x00\x00\x00\x00\x00\x00', b'8965B33542\x00\x00\x00\x00\x00\x00', + b'8965B33580\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ # Same as 0x791 b'8821F0601300 ', b'8821F0603300 ', b'8821F0607200 ', + b'8821F0608000 ', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'8646F0601200 ', b'8646F0601300 ', b'8646F0603400 ', b'8646F0605000 ', + b'8646F0606000 ', ], }, CAR.CAMRYH: { @@ -311,6 +320,7 @@ FW_VERSIONS = { b'\x028966306N8200\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', b'\x028966306R5000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', b'\x028966306R6000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', + b'\x028966306S0100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152633712\x00\x00\x00\x00\x00\x00', @@ -321,12 +331,17 @@ FW_VERSIONS = { b'8821F0601200 ', b'8821F0601300 ', b'8821F0607200 ', + b'8821F0608000 ', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B33540\x00\x00\x00\x00\x00\x00', + b'8965B33580\x00\x00\x00\x00\x00\x00', ], - (Ecu.eps, 0x7a1, None): [b'8965B33540\x00\x00\x00\x00\x00\x00'], (Ecu.fwdRadar, 0x750, 0xf): [ # Same as 0x791 b'8821F0601200 ', b'8821F0601300 ', b'8821F0607200 ', + b'8821F0608000 ', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'8646F0601200 ', @@ -480,6 +495,7 @@ FW_VERSIONS = { b'F152642540\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301100\x00\x00\x00\x00', b'\x018821F3301200\x00\x00\x00\x00', b'\x018821F3301300\x00\x00\x00\x00', b'\x018821F3301400\x00\x00\x00\x00', @@ -498,10 +514,12 @@ FW_VERSIONS = { b'\x01896630E84000\x00\x00\x00\x00', b'\x01896630E85000\x00\x00\x00\x00', b'\x01896630E88000\x00\x00\x00\x00', + b'\x01896630E09000\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B48140\x00\x00\x00\x00\x00\x00', b'8965B48150\x00\x00\x00\x00\x00\x00', + b'8965B48210\x00\x00\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [b'F15260E011\x00\x00\x00\x00\x00\x00'], (Ecu.dsu, 0x791, None): [ @@ -524,6 +542,13 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x750, 0xf): [b'8821F4702100\x00\x00\x00\x00'], (Ecu.fwdCamera, 0x750, 0x6d): [b'8646F0E01200\x00\x00\x00\x00'], }, + CAR.HIGHLANDERH_TSS2: { + (Ecu.eps, 0x7a1, None): [b'8965B48241\x00\x00\x00\x00\x00\x00',], + (Ecu.esp, 0x7b0, None): [b'\x01F15264872300\x00\x00\x00\x00',], + (Ecu.engine, 0x700, None): [b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',], + (Ecu.fwdRadar, 0x750, 0xf): [b'\x018821F3301400\x00\x00\x00\x00',], + (Ecu.fwdCamera, 0x750, 0x6d): [b'\x028646F0E02100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',], + }, CAR.LEXUS_IS: { (Ecu.engine, 0x700, None): [b'\x018966353Q2300\x00\x00\x00\x00'], (Ecu.esp, 0x7b0, None): [b'F152653330\x00\x00\x00\x00\x00\x00'], @@ -543,6 +568,8 @@ FW_VERSIONS = { b'\x02896634774200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896634782000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896634784000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x028966347A5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x03896634759200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701004\x00\x00\x00\x00', b'\x03896634759200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00', b'\x03896634759300\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701004\x00\x00\x00\x00', b'\x03896634760000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701002\x00\x00\x00\x00', @@ -563,7 +590,7 @@ FW_VERSIONS = { b'8965B47022\x00\x00\x00\x00\x00\x00', b'8965B47023\x00\x00\x00\x00\x00\x00', b'8965B47050\x00\x00\x00\x00\x00\x00', - b'8965B47060\x00\x00\x00\x00\x00\x00', # Think this the EPS with good angle sensor + b'8965B47060\x00\x00\x00\x00\x00\x00', # This is the EPS with good angle sensor ], (Ecu.esp, 0x7b0, None): [ b'F152647290\x00\x00\x00\x00\x00\x00', @@ -578,6 +605,7 @@ FW_VERSIONS = { b'F152647863\x00\x00\x00\x00\x00\x00', b'F152647864\x00\x00\x00\x00\x00\x00', b'F152647865\x00\x00\x00\x00\x00\x00', + b'F152647470\x00\x00\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ b'881514702300\x00\x00\x00\x00', @@ -673,6 +701,7 @@ FW_VERSIONS = { b'\x01896634A05000\x00\x00\x00\x00', b'\x01896634A19000\x00\x00\x00\x00', b'\x01896634A22000\x00\x00\x00\x00', + b'\x018966342U4000\x00\x00\x00\x00', b'\x01F152642551\x00\x00\x00\x00\x00\x00', b'\x028966342Y8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', b'\x02896634A18000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', @@ -715,6 +744,7 @@ FW_VERSIONS = { b'F152642330\x00\x00\x00\x00\x00\x00', b'F152642531\x00\x00\x00\x00\x00\x00', b'F152642532\x00\x00\x00\x00\x00\x00', + b'F152642521\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B42170\x00\x00\x00\x00\x00\x00', @@ -740,13 +770,6 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x750, 0xf): [b'\x018821F3301200\x00\x00\x00\x00'], (Ecu.fwdCamera, 0x750, 0x6d): [b'\x028646F3303200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00'], }, - CAR.LEXUS_IS: { - (Ecu.engine, 0x700, None): [b'\x018966353Q2300\x00\x00\x00\x00'], - (Ecu.dsu, 0x791, None): [b'881515306400\x00\x00\x00\x00'], - (Ecu.eps, 0x7a1, None): [b'8965B53271\x00\x00\x00\x00\x00\x00'], - (Ecu.fwdRadar, 0x750, 0xf): [b'8821F4702300\x00\x00\x00\x00'], - (Ecu.fwdCamera, 0x750, 0x6d): [b'8646F5301400\x00\x00\x00\x00'], - }, CAR.SIENNA: { (Ecu.engine, 0x700, None): [ b'\x01896630832100\x00\x00\x00\x00', @@ -783,6 +806,14 @@ FW_VERSIONS = { b'\x028646F3304100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', ], }, + CAR.LEXUS_RX: { + (Ecu.engine, 0x700, None): [b'\x01896630E41200\x00\x00\x00\x00'], + (Ecu.esp, 0x7b0, None): [b'F152648473\x00\x00\x00\x00\x00\x00'], + (Ecu.dsu, 0x791, None): [b'881514810500\x00\x00\x00\x00'], + (Ecu.eps, 0x7a1, None): [b'8965B0E012\x00\x00\x00\x00\x00\x00'], + (Ecu.fwdRadar, 0x750, 0xf): [b'8821F4701100\x00\x00\x00\x00'], + (Ecu.fwdCamera, 0x750, 0x6d): [b'8646F4802001\x00\x00\x00\x00'], + }, CAR.LEXUS_RXH: { (Ecu.engine, 0x7e0, None): [ b'\x02348Q4000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', @@ -832,6 +863,7 @@ DBC = { CAR.HIGHLANDER: dbc_dict('toyota_highlander_2017_pt_generated', 'toyota_adas'), CAR.HIGHLANDER_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.HIGHLANDERH: dbc_dict('toyota_highlander_hybrid_2018_pt_generated', 'toyota_adas'), + CAR.HIGHLANDERH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), CAR.AVALON: dbc_dict('toyota_avalon_2017_pt_generated', 'toyota_adas'), CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), @@ -845,6 +877,6 @@ DBC = { CAR.LEXUS_NXH: dbc_dict('lexus_nx300h_2018_pt_generated', 'toyota_adas'), } -NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.HIGHLANDER_TSS2] -TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.HIGHLANDER_TSS2] -NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.HIGHLANDER_TSS2] # no resume button press required +NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2] +TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2] +NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2] # no resume button press required diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 4a2f7cb7..cc4e978c 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -2,7 +2,7 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES -from common.params import Params +from common.params import put_nonblocking from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase @@ -23,6 +23,9 @@ class CarInterface(CarInterfaceBase): def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) + # VW port is a community feature, since we don't own one to test + ret.communityFeature = True + if candidate == CAR.GOLF: # Set common MQB parameters that will apply globally ret.carName = "volkswagen" @@ -76,7 +79,6 @@ class CarInterface(CarInterfaceBase): def update(self, c, can_strings): canMonoTimes = [] buttonEvents = [] - params = Params() # Process the most recent CAN message traffic, and check for validity # The camera CAN has no signals we use at this time, but we process it @@ -91,7 +93,7 @@ class CarInterface(CarInterfaceBase): # Update the EON metric configuration to match the car at first startup, # or if there's been a change. if self.CS.displayMetricUnits != self.displayMetricUnitsPrev: - params.put("IsMetric", "1" if self.CS.displayMetricUnits else "0") + put_nonblocking("IsMetric", "1" if self.CS.displayMetricUnits else "0") # Check for and process state-change events (button press or release) from # the turn stalk switch or ACC steering wheel/control stalk buttons. @@ -110,22 +112,11 @@ class CarInterface(CarInterfaceBase): if self.CS.steeringFault: events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) - # Engagement and longitudinal control using stock ACC. Make sure OP is - # disengaged if stock ACC is disengaged. - if not ret.cruiseState.enabled: - events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - # Attempt OP engagement only on rising edge of stock ACC engagement. - elif not self.cruise_enabled_prev: - events.append(create_event('pcmEnable', [ET.ENABLE])) - ret.events = events ret.buttonEvents = buttonEvents ret.canMonoTimes = canMonoTimes # update previous car states - self.gas_pressed_prev = ret.gasPressed - self.brake_pressed_prev = ret.brakePressed - self.cruise_enabled_prev = ret.cruiseState.enabled self.displayMetricUnitsPrev = self.CS.displayMetricUnits self.buttonStatesPrev = self.CS.buttonStates.copy() diff --git a/selfdrive/clocksd/clocksd.cc b/selfdrive/clocksd/clocksd.cc index 0dba6259..d289387a 100644 --- a/selfdrive/clocksd/clocksd.cc +++ b/selfdrive/clocksd/clocksd.cc @@ -67,6 +67,6 @@ int main() { close(timerfd); delete clock_publisher; - + delete context; return 0; } \ No newline at end of file diff --git a/selfdrive/common/SConscript b/selfdrive/common/SConscript index 6f40e6a8..18f9fdf4 100644 --- a/selfdrive/common/SConscript +++ b/selfdrive/common/SConscript @@ -5,7 +5,7 @@ if SHARED: else: fxn = env.Library -_common = fxn('common', ['params.cc', 'swaglog.c', 'util.c', 'cqueue.c'], LIBS="json") +_common = fxn('common', ['params.cc', 'swaglog.cc', 'util.c', 'cqueue.c'], LIBS="json11") _visionipc = fxn('visionipc', ['visionipc.c', 'ipc.c']) files = [ @@ -24,6 +24,12 @@ if arch == "aarch64": 'visionbuf_ion.c', ] _gpu_libs = ['gui', 'adreno_utils'] +elif arch == "larch64": + defines = {"CLU_NO_CACHE": None} + files += [ + 'visionbuf_ion.c', + ] + _gpu_libs = ['GL'] else: defines = {"CLU_NO_CACHE": None} files += [ diff --git a/selfdrive/common/clutil.c b/selfdrive/common/clutil.c index 0040e4dd..22c9b45d 100644 --- a/selfdrive/common/clutil.c +++ b/selfdrive/common/clutil.c @@ -85,7 +85,7 @@ void cl_print_info(cl_platform_id platform, cl_device_id device) { size_t sz; clGetDeviceInfo(device, CL_DEVICE_MAX_WORK_GROUP_SIZE, sizeof(sz), &sz, NULL); - printf("max work group size: %u\n", sz); + printf("max work group size: %zu\n", sz); cl_device_type type; clGetDeviceInfo(device, CL_DEVICE_TYPE, sizeof(type), &type, NULL); diff --git a/selfdrive/common/framebuffer.cc b/selfdrive/common/framebuffer.cc index 788b8129..7bea565d 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 79bc5d91..5ab2eae3 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -25,10 +25,16 @@ T* null_coalesce(T* a, T* b) { return a != NULL ? a : b; } -static const char* default_params_path = null_coalesce( - const_cast(getenv("PARAMS_PATH")), "/data/params"); +static const char* default_params_path = null_coalesce(const_cast(getenv("PARAMS_PATH")), "/data/params"); + +#ifdef QCOM +static const char* persistent_params_path = null_coalesce(const_cast(getenv("PERSISTENT_PARAMS_PATH")), "/persist/comma/params"); +#else +static const char* persistent_params_path = default_params_path; +#endif + +} //namespace -} // namespace static int fsync_dir(const char* path){ int result = 0; @@ -57,8 +63,15 @@ static int fsync_dir(const char* path){ } } -int write_db_value(const char* params_path, const char* key, const char* value, - size_t value_size) { +static int ensure_dir_exists(const char* path) { + struct stat st; + if (stat(path, &st) == -1) { + return mkdir(path, 0700); + } + return 0; +} + +int write_db_value(const char* key, const char* value, size_t value_size, bool persistent_param) { // Information about safely and atomically writing a file: https://lwn.net/Articles/457667/ // 1) Create temp file // 2) Write data to temp file @@ -71,10 +84,59 @@ int write_db_value(const char* params_path, const char* key, const char* value, int result; char tmp_path[1024]; char path[1024]; + char *tmp_dir; ssize_t bytes_written; + const char* params_path = persistent_param ? persistent_params_path : default_params_path; - if (params_path == NULL) { - params_path = default_params_path; + // Make sure params path exists + result = ensure_dir_exists(params_path); + if (result < 0) { + goto cleanup; + } + + result = snprintf(path, sizeof(path), "%s/d", params_path); + if (result < 0) { + goto cleanup; + } + + // See if the symlink exists, otherwise create it + struct stat st; + if (stat(path, &st) == -1) { + // Create temp folder + result = snprintf(path, sizeof(path), "%s/.tmp_XXXXXX", params_path); + if (result < 0) { + goto cleanup; + } + tmp_dir = mkdtemp(path); + if (tmp_dir == NULL){ + goto cleanup; + } + + // Set permissions + result = chmod(tmp_dir, 0777); + if (result < 0) { + goto cleanup; + } + + // Symlink it to temp link + result = snprintf(tmp_path, sizeof(tmp_path), "%s.link", tmp_dir); + if (result < 0) { + goto cleanup; + } + result = symlink(tmp_dir, tmp_path); + if (result < 0) { + goto cleanup; + } + + // Move symlink to /d + result = snprintf(path, sizeof(path), "%s/d", params_path); + if (result < 0) { + goto cleanup; + } + result = rename(tmp_path, path); + if (result < 0) { + goto cleanup; + } } // Write value to temp. @@ -96,7 +158,7 @@ int write_db_value(const char* params_path, const char* key, const char* value, if (result < 0) { goto cleanup; } - lock_fd = open(path, 0); + lock_fd = open(path, O_CREAT); // Build key path result = snprintf(path, sizeof(path), "%s/d/%s", params_path, key); @@ -153,21 +215,18 @@ cleanup: return result; } -int delete_db_value(const char* params_path, const char* key) { +int delete_db_value(const char* key, bool persistent_param) { int lock_fd = -1; int result; char path[1024]; - - if (params_path == NULL) { - params_path = default_params_path; - } + const char* params_path = persistent_param ? persistent_params_path : default_params_path; // Build lock path, and open lockfile result = snprintf(path, sizeof(path), "%s/.lock", params_path); if (result < 0) { goto cleanup; } - lock_fd = open(path, 0); + lock_fd = open(path, O_CREAT); // Take lock. result = flock(lock_fd, LOCK_EX); @@ -207,15 +266,11 @@ cleanup: return result; } -int read_db_value(const char* params_path, const char* key, char** value, - size_t* value_sz) { +int read_db_value(const char* key, char** value, size_t* value_sz, bool persistent_param) { int lock_fd = -1; int result; char path[1024]; - - if (params_path == NULL) { - params_path = default_params_path; - } + const char* params_path = persistent_param ? persistent_params_path : default_params_path; result = snprintf(path, sizeof(path), "%s/.lock", params_path); if (result < 0) { @@ -253,10 +308,9 @@ cleanup: return result; } -void read_db_value_blocking(const char* params_path, const char* key, - char** value, size_t* value_sz) { +void read_db_value_blocking(const char* key, char** value, size_t* value_sz, bool persistent_param) { while (1) { - const int result = read_db_value(params_path, key, value, value_sz); + const int result = read_db_value(key, value, value_sz, persistent_param); if (result == 0) { return; } else { @@ -266,12 +320,9 @@ void read_db_value_blocking(const char* params_path, const char* key, } } -int read_db_all(const char* params_path, std::map *params) { +int read_db_all(std::map *params, bool persistent_param) { int err = 0; - - if (params_path == NULL) { - params_path = default_params_path; - } + const char* params_path = persistent_param ? persistent_params_path : default_params_path; std::string lock_path = util::string_format("%s/.lock", params_path); @@ -292,9 +343,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/params.h b/selfdrive/common/params.h index 6dcd3614..ea002eb0 100644 --- a/selfdrive/common/params.h +++ b/selfdrive/common/params.h @@ -9,30 +9,28 @@ extern "C" { #define ERR_NO_VALUE -33 -int write_db_value(const char* params_path, const char* key, const char* value, - size_t value_size); +int write_db_value(const char* key, const char* value, size_t value_size, bool persistent_param = false); // Reads a value from the params database. // Inputs: -// params_path: The path of the database, or NULL to use the default. // key: The key to read. // value: A pointer where a newly allocated string containing the db value will // be written. // value_sz: A pointer where the size of value will be written. Does not // include the NULL terminator. +// persistent_param: Boolean indicating if the param store in the /persist partition is to be used. +// e.g. for sensor calibration files. Will not be cleared after wipe or re-install. // // Returns: Negative on failure, otherwise 0. -int read_db_value(const char* params_path, const char* key, char** value, - size_t* value_sz); +int read_db_value(const char* key, char** value, size_t* value_sz, bool persistent_param = false); // Delete a value from the params database. // Inputs are the same as read_db_value, without value and value_sz. -int delete_db_value(const char* params_path, const char* key); +int delete_db_value(const char* key, bool persistent_param = false); // Reads a value from the params database, blocking until successful. // Inputs are the same as read_db_value. -void read_db_value_blocking(const char* params_path, const char* key, - char** value, size_t* value_sz); +void read_db_value_blocking(const char* key, char** value, size_t* value_sz, bool persistent_param = false); #ifdef __cplusplus } // extern "C" @@ -41,7 +39,7 @@ void read_db_value_blocking(const char* params_path, const char* key, #ifdef __cplusplus #include #include -int read_db_all(const char* params_path, std::map *params); +int read_db_all(std::map *params, bool persistent_param = false); #endif #endif // _SELFDRIVE_COMMON_PARAMS_H_ diff --git a/selfdrive/common/swaglog.c b/selfdrive/common/swaglog.cc similarity index 69% rename from selfdrive/common/swaglog.c rename to selfdrive/common/swaglog.cc index 70cba781..41b6358d 100644 --- a/selfdrive/common/swaglog.c +++ b/selfdrive/common/swaglog.cc @@ -1,15 +1,15 @@ +#ifndef _GNU_SOURCE #define _GNU_SOURCE +#endif -#include -#include -#include -#include +#include #include #include #include #include -#include + +#include "json11.hpp" #include "common/timing.h" #include "common/version.h" @@ -19,7 +19,7 @@ typedef struct LogState { pthread_mutex_t lock; bool inited; - JsonNode *ctx_j; + json11::Json::object ctx_j; void *zctx; void *sock; int print_level; @@ -30,12 +30,12 @@ static LogState s = { }; static void cloudlog_bind_locked(const char* k, const char* v) { - json_append_member(s.ctx_j, k, json_mkstring(v)); + s.ctx_j[k] = v; } static void cloudlog_init() { if (s.inited) return; - s.ctx_j = json_mkobject(); + s.ctx_j = json11::Json::object {}; s.zctx = zmq_ctx_new(); s.sock = zmq_socket(s.zctx, ZMQ_PUSH); zmq_connect(s.sock, "ipc:///tmp/logmessage"); @@ -58,8 +58,7 @@ static void cloudlog_init() { cloudlog_bind_locked("dongle_id", dongle_id); } cloudlog_bind_locked("version", COMMA_VERSION); - bool dirty = !getenv("CLEAN"); - json_append_member(s.ctx_j, "dirty", json_mkbool(dirty)); + s.ctx_j["dirty"] = !getenv("CLEAN"); s.inited = true; } @@ -84,23 +83,19 @@ void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func printf("%s: %s\n", filename, msg_buf); } - JsonNode *log_j = json_mkobject(); - assert(log_j); + json11::Json log_j = json11::Json::object { + {"msg", msg_buf}, + {"ctx", s.ctx_j}, + {"levelnum", levelnum}, + {"filename", filename}, + {"lineno", lineno}, + {"funcname", func}, + {"created", seconds_since_epoch()} + }; - json_append_member(log_j, "msg", json_mkstring(msg_buf)); - json_append_member(log_j, "ctx", s.ctx_j); - json_append_member(log_j, "levelnum", json_mknumber(levelnum)); - json_append_member(log_j, "filename", json_mkstring(filename)); - json_append_member(log_j, "lineno", json_mknumber(lineno)); - json_append_member(log_j, "funcname", json_mkstring(func)); - json_append_member(log_j, "created", json_mknumber(seconds_since_epoch())); - - char* log_s = json_encode(log_j); + char* log_s = strdup(log_j.dump().c_str()); assert(log_s); - json_remove_from_parent(s.ctx_j); - - json_delete(log_j); free(msg_buf); char levelnum_c = levelnum; diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 714b71d8..7e491e61 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-release" diff --git a/selfdrive/common/visionbuf.h b/selfdrive/common/visionbuf.h index 3457f6df..8958bdd1 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 333bed55..5e946498 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 724e75e9..e5dddfad 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/common/visionipc.c b/selfdrive/common/visionipc.c index b0536589..a9c4e8a1 100644 --- a/selfdrive/common/visionipc.c +++ b/selfdrive/common/visionipc.c @@ -99,7 +99,7 @@ int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, Visi close(s->ipc_fd); return -1; } - assert(rp.type = VIPC_STREAM_BUFS); + assert(rp.type == VIPC_STREAM_BUFS); assert(rp.d.stream_bufs.type == type); s->bufs_info = rp.d.stream_bufs; diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8c135782..f6c2e9d1 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -6,7 +6,7 @@ from cereal import car, log 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 @@ -27,6 +27,8 @@ from selfdrive.controls.lib.planner import LON_MPC_STEP from selfdrive.locationd.calibration_helpers import Calibration, Filter LANE_DEPARTURE_THRESHOLD = 0.1 +STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL +STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees ThermalStatus = log.ThermalData.ThermalStatus State = log.ControlsState.OpenpilotState @@ -114,6 +116,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 > 92 * 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 +223,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 +271,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 +297,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 +355,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') @@ -443,6 +454,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 +466,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 +489,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 +515,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 +532,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 @@ -536,7 +550,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): events.append(create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) 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])) @@ -556,6 +570,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 +585,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 86afb9dc..c9c37044 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 88fb8fbb..2b2b78aa 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -110,7 +110,7 @@ ALERTS = [ 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 @@ ALERTS = [ "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 @@ ALERTS = [ "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", @@ -187,7 +187,7 @@ ALERTS = [ Alert( "startupNoCar", - "Dashcam mode with unsupported car", + "Dashcam mode for unsupported car", "Always keep hands on wheel and eyes on road", AlertStatus.normal, AlertSize.mid, Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), @@ -272,7 +272,7 @@ ALERTS = [ 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 @@ ALERTS = [ 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 @@ ALERTS = [ 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 @@ ALERTS = [ 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 @@ ALERTS = [ 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 @@ ALERTS = [ 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 @@ ALERTS = [ 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", @@ -772,7 +815,6 @@ ALERTS = [ AlertStatus.normal, AlertSize.small, Priority.LOWEST, VisualAlert.steerRequired, AudibleAlert.none, .0, .0, .1), - # offroad alerts Alert( "ldwPermanent", "TAKE CONTROL", diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index 8f0dbda7..73ef0777 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -5,6 +5,12 @@ from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET 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 @@ _HI_STD_FALLBACK_TIME = 10 # fall back to wheel touch if model is uncertain for _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 @@ class DriverStatus(): 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 @@ class DriverStatus(): 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 00000000..298858bd --- /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/gps_helpers.py b/selfdrive/controls/lib/gps_helpers.py deleted file mode 100755 index e9e7d6c9..00000000 --- a/selfdrive/controls/lib/gps_helpers.py +++ /dev/null @@ -1,19 +0,0 @@ -_RHD_REGION_MAP = [ ['AU', -54.76, -9.23, 112.91, 159.11], \ - ['IN1', 6.75, 28.10, 68.17, 97.4], \ - ['IN2', 28.09, 35.99, 72.18, 80.87], \ - ['IE', 51.42, 55.38, -10.58, -5.99], \ - ['JP1', 32.66, 45.52, 137.27, 146.02], \ - ['JP2', 32.79, 37.60, 131.41, 137.28], \ - ['JP3', 24.04, 34.78, 122.93, 131.42], \ - ['MU', -20.53, -19.98, 57.30, 57.81], \ - ['MY', 0.86, 7.36, 99.64, 119.27], \ - ['NZ', -52.61, -29.24, 166, 178.84], \ - ['UK', 49.9, 60.84, -8.62, 1.77], \ - ['ZA', -34.83, -22.13, 16.47, 32.88] ] - -def is_rhd_region(latitude, longitude): - for region in _RHD_REGION_MAP: - if region[1] <= latitude <= region[2] and \ - region[3] <= longitude <= region[4]: - return True - return False diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index ff090bcd..5b9485a2 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -4,6 +4,7 @@ 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 +17,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 @@ -82,7 +94,7 @@ class LanePlanner(): 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) + 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): self.parse_model(md) diff --git a/selfdrive/controls/lib/long_mpc_model.py b/selfdrive/controls/lib/long_mpc_model.py new file mode 100644 index 00000000..b3a55b8f --- /dev/null +++ b/selfdrive/controls/lib/long_mpc_model.py @@ -0,0 +1,73 @@ +import numpy as np +import math + +from selfdrive.swaglog import cloudlog +from common.realtime import sec_since_boot +from selfdrive.controls.lib.longitudinal_mpc_model import libmpc_py + + +class LongitudinalMpcModel(): + def __init__(self): + + self.setup_mpc() + self.v_mpc = 0.0 + self.v_mpc_future = 0.0 + self.a_mpc = 0.0 + self.last_cloudlog_t = 0.0 + self.ts = list(range(10)) + + self.valid = False + + def setup_mpc(self, v_ego=0.0): + self.libmpc = libmpc_py.libmpc + self.libmpc.init(1.0, 1.0, 1.0, 1.0, 1.0) + self.libmpc.init_with_simulation(v_ego) + + self.mpc_solution = libmpc_py.ffi.new("log_t *") + self.cur_state = libmpc_py.ffi.new("state_t *") + + self.cur_state[0].x_ego = 0 + self.cur_state[0].v_ego = 0 + self.cur_state[0].a_ego = 0 + + def set_cur_state(self, v, a): + self.cur_state[0].x_ego = 0.0 + self.cur_state[0].v_ego = v + self.cur_state[0].a_ego = a + + def update(self, v_ego, a_ego, poss, speeds, accels): + if len(poss) == 0: + self.valid = False + return + + x_poly = list(map(float, np.polyfit(self.ts, poss, 3))) + v_poly = list(map(float, np.polyfit(self.ts, speeds, 3))) + a_poly = list(map(float, np.polyfit(self.ts, accels, 3))) + + # Calculate mpc + self.libmpc.run_mpc(self.cur_state, self.mpc_solution, x_poly, v_poly, a_poly) + + # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed + self.v_mpc = self.mpc_solution[0].v_ego[1] + self.a_mpc = self.mpc_solution[0].a_ego[1] + self.v_mpc_future = self.mpc_solution[0].v_ego[10] + self.valid = True + + # Reset if NaN or goes through lead car + nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego) + + t = sec_since_boot() + if nans: + if t > self.last_cloudlog_t + 5.0: + self.last_cloudlog_t = t + cloudlog.warning("Longitudinal model mpc reset - backwards") + + self.libmpc.init(1.0, 1.0, 1.0, 1.0, 1.0) + self.libmpc.init_with_simulation(v_ego) + + self.cur_state[0].v_ego = v_ego + self.cur_state[0].a_ego = 0.0 + + self.v_mpc = v_ego + self.a_mpc = a_ego + self.valid = False diff --git a/selfdrive/controls/lib/longitudinal_mpc_model/.gitignore b/selfdrive/controls/lib/longitudinal_mpc_model/.gitignore new file mode 100644 index 00000000..f61a939c --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc_model/.gitignore @@ -0,0 +1,2 @@ +generator +lib_qp/ diff --git a/selfdrive/controls/lib/longitudinal_mpc_model/SConscript b/selfdrive/controls/lib/longitudinal_mpc_model/SConscript new file mode 100644 index 00000000..f474bf79 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc_model/SConscript @@ -0,0 +1,31 @@ +Import('env', 'arch') + + +cpp_path = [ + "#phonelibs/acado/include", + "#phonelibs/acado/include/acado", + "#phonelibs/qpoases/INCLUDE", + "#phonelibs/qpoases/INCLUDE/EXTRAS", + "#phonelibs/qpoases/SRC/", + "#phonelibs/qpoases", + "lib_mpc_export" + +] + +mpc_files = [ + "longitudinal_mpc.c", + Glob("lib_mpc_export/*.c"), + Glob("lib_mpc_export/*.cpp"), +] + +interface_dir = Dir('lib_mpc_export') + +SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir']) + +env.SharedLibrary('mpc', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) + +# if arch != "aarch64": +# acado_libs = [File("#phonelibs/acado/x64/lib/libacado_toolkit.a"), +# File("#phonelibs/acado/x64/lib/libacado_casadi.a"), +# File("#phonelibs/acado/x64/lib/libacado_csparse.a")] +# env.Program('generator', 'generator.cpp', LIBS=acado_libs, CPPPATH=cpp_path) diff --git a/selfdrive/controls/lib/longitudinal_mpc_model/__init__.py b/selfdrive/controls/lib/longitudinal_mpc_model/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/selfdrive/controls/lib/longitudinal_mpc_model/generator.cpp b/selfdrive/controls/lib/longitudinal_mpc_model/generator.cpp new file mode 100644 index 00000000..129b52a7 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc_model/generator.cpp @@ -0,0 +1,99 @@ +#include + +const int controlHorizon = 50; + +using namespace std; + + +int main( ) +{ + USING_NAMESPACE_ACADO + + + DifferentialEquation f; + + DifferentialState x_ego, v_ego, a_ego, t; + + OnlineData x_poly_r0, x_poly_r1, x_poly_r2, x_poly_r3; + OnlineData v_poly_r0, v_poly_r1, v_poly_r2, v_poly_r3; + OnlineData a_poly_r0, a_poly_r1, a_poly_r2, a_poly_r3; + + Control j_ego; + + // Equations of motion + f << dot(x_ego) == v_ego; + f << dot(v_ego) == a_ego; + f << dot(a_ego) == j_ego; + f << dot(t) == 1; + + auto poly_x = x_poly_r0*(t*t*t) + x_poly_r1*(t*t) + x_poly_r2*t + x_poly_r3; + auto poly_v = v_poly_r0*(t*t*t) + v_poly_r1*(t*t) + v_poly_r2*t + v_poly_r3; + auto poly_a = a_poly_r0*(t*t*t) + a_poly_r1*(t*t) + a_poly_r2*t + a_poly_r3; + + // Running cost + Function h; + h << x_ego - poly_x; + h << v_ego - poly_v; + h << a_ego - poly_a; + h << a_ego * (0.1 * v_ego + 1.0); + h << j_ego * (0.1 * v_ego + 1.0); + + // Weights are defined in mpc. + BMatrix Q(5,5); Q.setAll(true); + + // Terminal cost + Function hN; + hN << x_ego - poly_x; + hN << v_ego - poly_v; + hN << a_ego - poly_a; + hN << a_ego * (0.1 * v_ego + 1.0); + + // Weights are defined in mpc. + BMatrix QN(4,4); QN.setAll(true); + + // Non uniform time grid + // First 5 timesteps are 0.2, after that it's 0.6 + DMatrix numSteps(20, 1); + for (int i = 0; i < 5; i++){ + numSteps(i) = 1; + } + for (int i = 5; i < 20; i++){ + numSteps(i) = 3; + } + + // Setup Optimal Control Problem + const double tStart = 0.0; + const double tEnd = 10.0; + + OCP ocp( tStart, tEnd, numSteps); + ocp.subjectTo(f); + + ocp.minimizeLSQ(Q, h); + ocp.minimizeLSQEndTerm(QN, hN); + + //ocp.subjectTo( 0.0 <= v_ego); + ocp.setNOD(12); + + OCPexport mpc(ocp); + mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON ); + mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING ); + mpc.set( INTEGRATOR_TYPE, INT_RK4 ); + mpc.set( NUM_INTEGRATOR_STEPS, controlHorizon); + mpc.set( MAX_NUM_QP_ITERATIONS, 500); + mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES); + + mpc.set( SPARSE_QP_SOLUTION, CONDENSING ); + mpc.set( QP_SOLVER, QP_QPOASES ); + mpc.set( HOTSTART_QP, YES ); + mpc.set( GENERATE_TEST_FILE, NO); + mpc.set( GENERATE_MAKE_FILE, NO ); + mpc.set( GENERATE_MATLAB_INTERFACE, NO ); + mpc.set( GENERATE_SIMULINK_INTERFACE, NO ); + + if (mpc.exportCode( "lib_mpc_export" ) != SUCCESSFUL_RETURN) + exit( EXIT_FAILURE ); + + mpc.printDimensionsQP( ); + + return EXIT_SUCCESS; +} diff --git a/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_auxiliary_functions.c b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_auxiliary_functions.c new file mode 100644 index 00000000..6f0bb705 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_auxiliary_functions.c @@ -0,0 +1,212 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#include "acado_auxiliary_functions.h" + +#include + +real_t* acado_getVariablesX( ) +{ + return acadoVariables.x; +} + +real_t* acado_getVariablesU( ) +{ + return acadoVariables.u; +} + +#if ACADO_NY > 0 +real_t* acado_getVariablesY( ) +{ + return acadoVariables.y; +} +#endif + +#if ACADO_NYN > 0 +real_t* acado_getVariablesYN( ) +{ + return acadoVariables.yN; +} +#endif + +real_t* acado_getVariablesX0( ) +{ +#if ACADO_INITIAL_VALUE_FIXED + return acadoVariables.x0; +#else + return 0; +#endif +} + +/** Print differential variables. */ +void acado_printDifferentialVariables( ) +{ + int i, j; + printf("\nDifferential variables:\n[\n"); + for (i = 0; i < ACADO_N + 1; ++i) + { + for (j = 0; j < ACADO_NX; ++j) + printf("\t%e", acadoVariables.x[i * ACADO_NX + j]); + printf("\n"); + } + printf("]\n\n"); +} + +/** Print control variables. */ +void acado_printControlVariables( ) +{ + int i, j; + printf("\nControl variables:\n[\n"); + for (i = 0; i < ACADO_N; ++i) + { + for (j = 0; j < ACADO_NU; ++j) + printf("\t%e", acadoVariables.u[i * ACADO_NU + j]); + printf("\n"); + } + printf("]\n\n"); +} + +/** Print ACADO code generation notice. */ +void acado_printHeader( ) +{ + printf( + "\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\n" + "Copyright (C) 2008-2015 by Boris Houska, Hans Joachim Ferreau,\n" + "Milan Vukov and Rien Quirynen, KU Leuven.\n" + ); + + printf( + "Developed within the Optimization in Engineering Center (OPTEC) under\n" + "supervision of Moritz Diehl. All rights reserved.\n\n" + "ACADO Toolkit is distributed under the terms of the GNU Lesser\n" + "General Public License 3 in the hope that it will be useful,\n" + "but WITHOUT ANY WARRANTY; without even the implied warranty of\n" + "MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n" + "GNU Lesser General Public License for more details.\n\n" + ); +} + +#if !(defined _DSPACE) +#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__) + +void acado_tic( acado_timer* t ) +{ + QueryPerformanceFrequency(&t->freq); + QueryPerformanceCounter(&t->tic); +} + +real_t acado_toc( acado_timer* t ) +{ + QueryPerformanceCounter(&t->toc); + return ((t->toc.QuadPart - t->tic.QuadPart) / (real_t)t->freq.QuadPart); +} + + +#elif (defined __APPLE__) + +void acado_tic( acado_timer* t ) +{ + /* read current clock cycles */ + t->tic = mach_absolute_time(); +} + +real_t acado_toc( acado_timer* t ) +{ + + uint64_t duration; /* elapsed time in clock cycles*/ + + t->toc = mach_absolute_time(); + duration = t->toc - t->tic; + + /*conversion from clock cycles to nanoseconds*/ + mach_timebase_info(&(t->tinfo)); + duration *= t->tinfo.numer; + duration /= t->tinfo.denom; + + return (real_t)duration / 1e9; +} + +#else + +#if __STDC_VERSION__ >= 199901L +/* C99 mode */ + +/* read current time */ +void acado_tic( acado_timer* t ) +{ + gettimeofday(&t->tic, 0); +} + +/* return time passed since last call to tic on this timer */ +real_t acado_toc( acado_timer* t ) +{ + struct timeval temp; + + gettimeofday(&t->toc, 0); + + if ((t->toc.tv_usec - t->tic.tv_usec) < 0) + { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1; + temp.tv_usec = 1000000 + t->toc.tv_usec - t->tic.tv_usec; + } + else + { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec; + temp.tv_usec = t->toc.tv_usec - t->tic.tv_usec; + } + + return (real_t)temp.tv_sec + (real_t)temp.tv_usec / 1e6; +} + +#else +/* ANSI */ + +/* read current time */ +void acado_tic( acado_timer* t ) +{ + clock_gettime(CLOCK_MONOTONIC, &t->tic); +} + + +/* return time passed since last call to tic on this timer */ +real_t acado_toc( acado_timer* t ) +{ + struct timespec temp; + + clock_gettime(CLOCK_MONOTONIC, &t->toc); + + if ((t->toc.tv_nsec - t->tic.tv_nsec) < 0) + { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1; + temp.tv_nsec = 1000000000+t->toc.tv_nsec - t->tic.tv_nsec; + } + else + { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec; + temp.tv_nsec = t->toc.tv_nsec - t->tic.tv_nsec; + } + + return (real_t)temp.tv_sec + (real_t)temp.tv_nsec / 1e9; +} + +#endif /* __STDC_VERSION__ >= 199901L */ + +#endif /* (defined _WIN32 || _WIN64) */ + +#endif diff --git a/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_auxiliary_functions.h b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_auxiliary_functions.h new file mode 100644 index 00000000..b1be0a66 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_auxiliary_functions.h @@ -0,0 +1,138 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#ifndef ACADO_AUXILIARY_FUNCTIONS_H +#define ACADO_AUXILIARY_FUNCTIONS_H + +#include "acado_common.h" + +#ifndef __MATLAB__ +#ifdef __cplusplus +extern "C" +{ +#endif /* __cplusplus */ +#endif /* __MATLAB__ */ + +/** Get pointer to the matrix with differential variables. */ +real_t* acado_getVariablesX( ); + +/** Get pointer to the matrix with control variables. */ +real_t* acado_getVariablesU( ); + +#if ACADO_NY > 0 +/** Get pointer to the matrix with references/measurements. */ +real_t* acado_getVariablesY( ); +#endif + +#if ACADO_NYN > 0 +/** Get pointer to the vector with references/measurement on the last node. */ +real_t* acado_getVariablesYN( ); +#endif + +/** Get pointer to the current state feedback vector. Only applicable for NMPC. */ +real_t* acado_getVariablesX0( ); + +/** Print differential variables. */ +void acado_printDifferentialVariables( ); + +/** Print control variables. */ +void acado_printControlVariables( ); + +/** Print ACADO code generation notice. */ +void acado_printHeader( ); + +/* + * A huge thanks goes to Alexander Domahidi from ETHZ, Switzerland, for + * providing us with the following timing routines. + */ + +#if !(defined _DSPACE) +#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__) + +/* Use Windows QueryPerformanceCounter for timing. */ +#include + +/** A structure for keeping internal timer data. */ +typedef struct acado_timer_ +{ + LARGE_INTEGER tic; + LARGE_INTEGER toc; + LARGE_INTEGER freq; +} acado_timer; + + +#elif (defined __APPLE__) + +#include "unistd.h" +#include + +/** A structure for keeping internal timer data. */ +typedef struct acado_timer_ +{ + uint64_t tic; + uint64_t toc; + mach_timebase_info_data_t tinfo; +} acado_timer; + +#else + +/* Use POSIX clock_gettime() for timing on non-Windows machines. */ +#include + +#if __STDC_VERSION__ >= 199901L +/* C99 mode of operation. */ + +#include +#include + +typedef struct acado_timer_ +{ + struct timeval tic; + struct timeval toc; +} acado_timer; + +#else +/* ANSI C */ + +/** A structure for keeping internal timer data. */ +typedef struct acado_timer_ +{ + struct timespec tic; + struct timespec toc; +} acado_timer; + +#endif /* __STDC_VERSION__ >= 199901L */ + +#endif /* (defined _WIN32 || defined _WIN64) */ + +/** A function for measurement of the current time. */ +void acado_tic( acado_timer* t ); + +/** A function which returns the elapsed time. */ +real_t acado_toc( acado_timer* t ); + +#endif + +#ifndef __MATLAB__ +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ +#endif /* __MATLAB__ */ + +#endif /* ACADO_AUXILIARY_FUNCTIONS_H */ diff --git a/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_common.h b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_common.h new file mode 100644 index 00000000..cf8c4247 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_common.h @@ -0,0 +1,342 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#ifndef ACADO_COMMON_H +#define ACADO_COMMON_H + +#include +#include + +#ifndef __MATLAB__ +#ifdef __cplusplus +extern "C" +{ +#endif /* __cplusplus */ +#endif /* __MATLAB__ */ + +/** \defgroup ACADO ACADO CGT generated module. */ +/** @{ */ + +/** qpOASES QP solver indicator. */ +#define ACADO_QPOASES 0 +#define ACADO_QPOASES3 1 +/** FORCES QP solver indicator.*/ +#define ACADO_FORCES 2 +/** qpDUNES QP solver indicator.*/ +#define ACADO_QPDUNES 3 +/** HPMPC QP solver indicator. */ +#define ACADO_HPMPC 4 +#define ACADO_GENERIC 5 + +/** Indicator for determining the QP solver used by the ACADO solver code. */ +#define ACADO_QP_SOLVER ACADO_QPOASES + +#include "acado_qpoases_interface.hpp" + + +/* + * Common definitions + */ +/** User defined block based condensing. */ +#define ACADO_BLOCK_CONDENSING 0 +/** Compute covariance matrix of the last state estimate. */ +#define ACADO_COMPUTE_COVARIANCE_MATRIX 0 +/** Flag indicating whether constraint values are hard-coded or not. */ +#define ACADO_HARDCODED_CONSTRAINT_VALUES 1 +/** Indicator for fixed initial state. */ +#define ACADO_INITIAL_STATE_FIXED 1 +/** Number of control/estimation intervals. */ +#define ACADO_N 20 +/** Number of online data values. */ +#define ACADO_NOD 12 +/** Number of path constraints. */ +#define ACADO_NPAC 0 +/** Number of control variables. */ +#define ACADO_NU 1 +/** Number of differential variables. */ +#define ACADO_NX 4 +/** Number of algebraic variables. */ +#define ACADO_NXA 0 +/** Number of differential derivative variables. */ +#define ACADO_NXD 0 +/** Number of references/measurements per node on the first N nodes. */ +#define ACADO_NY 5 +/** Number of references/measurements on the last (N + 1)st node. */ +#define ACADO_NYN 4 +/** Total number of QP optimization variables. */ +#define ACADO_QP_NV 24 +/** Number of Runge-Kutta stages per integration step. */ +#define ACADO_RK_NSTAGES 4 +/** Providing interface for arrival cost. */ +#define ACADO_USE_ARRIVAL_COST 0 +/** Indicator for usage of non-hard-coded linear terms in the objective. */ +#define ACADO_USE_LINEAR_TERMS 0 +/** Indicator for type of fixed weighting matrices. */ +#define ACADO_WEIGHTING_MATRICES_TYPE 2 + + +/* + * Globally used structure definitions + */ + +/** The structure containing the user data. + * + * Via this structure the user "communicates" with the solver code. + */ +typedef struct ACADOvariables_ +{ +int dummy; +/** Matrix of size: 21 x 4 (row major format) + * + * Matrix containing 21 differential variable vectors. + */ +real_t x[ 84 ]; + +/** Column vector of size: 20 + * + * Matrix containing 20 control variable vectors. + */ +real_t u[ 20 ]; + +/** Matrix of size: 21 x 12 (row major format) + * + * Matrix containing 21 online data vectors. + */ +real_t od[ 252 ]; + +/** Column vector of size: 100 + * + * Matrix containing 20 reference/measurement vectors of size 5 for first 20 nodes. + */ +real_t y[ 100 ]; + +/** Column vector of size: 4 + * + * Reference/measurement vector for the 21. node. + */ +real_t yN[ 4 ]; + +/** Matrix of size: 100 x 5 (row major format) */ +real_t W[ 500 ]; + +/** Matrix of size: 4 x 4 (row major format) */ +real_t WN[ 16 ]; + +/** Column vector of size: 4 + * + * Current state feedback vector. + */ +real_t x0[ 4 ]; + + +} ACADOvariables; + +/** Private workspace used by the auto-generated code. + * + * Data members of this structure are private to the solver. + * In other words, the user code should not modify values of this + * structure. + */ +typedef struct ACADOworkspace_ +{ +real_t rk_ttt; + +/** Row vector of size: 37 */ +real_t rk_xxx[ 37 ]; + +/** Matrix of size: 4 x 24 (row major format) */ +real_t rk_kkk[ 96 ]; + +/** Row vector of size: 37 */ +real_t state[ 37 ]; + +/** Column vector of size: 80 */ +real_t d[ 80 ]; + +/** Column vector of size: 100 */ +real_t Dy[ 100 ]; + +/** Column vector of size: 4 */ +real_t DyN[ 4 ]; + +/** Matrix of size: 80 x 4 (row major format) */ +real_t evGx[ 320 ]; + +/** Column vector of size: 80 */ +real_t evGu[ 80 ]; + +/** Row vector of size: 17 */ +real_t objValueIn[ 17 ]; + +/** Row vector of size: 30 */ +real_t objValueOut[ 30 ]; + +/** Matrix of size: 80 x 4 (row major format) */ +real_t Q1[ 320 ]; + +/** Matrix of size: 80 x 5 (row major format) */ +real_t Q2[ 400 ]; + +/** Column vector of size: 20 */ +real_t R1[ 20 ]; + +/** Matrix of size: 20 x 5 (row major format) */ +real_t R2[ 100 ]; + +/** Column vector of size: 80 */ +real_t S1[ 80 ]; + +/** Matrix of size: 4 x 4 (row major format) */ +real_t QN1[ 16 ]; + +/** Matrix of size: 4 x 4 (row major format) */ +real_t QN2[ 16 ]; + +/** Column vector of size: 4 */ +real_t Dx0[ 4 ]; + +/** Matrix of size: 4 x 4 (row major format) */ +real_t T[ 16 ]; + +/** Column vector of size: 840 */ +real_t E[ 840 ]; + +/** Column vector of size: 840 */ +real_t QE[ 840 ]; + +/** Matrix of size: 80 x 4 (row major format) */ +real_t QGx[ 320 ]; + +/** Column vector of size: 80 */ +real_t Qd[ 80 ]; + +/** Column vector of size: 84 */ +real_t QDy[ 84 ]; + +/** Matrix of size: 20 x 4 (row major format) */ +real_t H10[ 80 ]; + +/** Matrix of size: 24 x 24 (row major format) */ +real_t H[ 576 ]; + +/** Column vector of size: 24 */ +real_t g[ 24 ]; + +/** Column vector of size: 24 */ +real_t lb[ 24 ]; + +/** Column vector of size: 24 */ +real_t ub[ 24 ]; + +/** Column vector of size: 24 */ +real_t x[ 24 ]; + +/** Column vector of size: 24 */ +real_t y[ 24 ]; + + +} ACADOworkspace; + +/* + * Forward function declarations. + */ + + +/** Performs the integration and sensitivity propagation for one shooting interval. + * + * \param rk_eta Working array to pass the input values and return the results. + * \param resetIntegrator The internal memory of the integrator can be reset. + * \param rk_index Number of the shooting interval. + * + * \return Status code of the integrator. + */ +int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index ); + +/** Export of an ACADO symbolic function. + * + * \param in Input to the exported function. + * \param out Output of the exported function. + */ +void acado_rhs_forw(const real_t* in, real_t* out); + +/** Preparation step of the RTI scheme. + * + * \return Status of the integration module. =0: OK, otherwise the error code. + */ +int acado_preparationStep( ); + +/** Feedback/estimation step of the RTI scheme. + * + * \return Status code of the qpOASES QP solver. + */ +int acado_feedbackStep( ); + +/** Solver initialization. Must be called once before any other function call. + * + * \return =0: OK, otherwise an error code of a QP solver. + */ +int acado_initializeSolver( ); + +/** Initialize shooting nodes by a forward simulation starting from the first node. + */ +void acado_initializeNodesByForwardSimulation( ); + +/** Shift differential variables vector by one interval. + * + * \param strategy Shifting strategy: 1. Initialize node 21 with xEnd. 2. Initialize node 21 by forward simulation. + * \param xEnd Value for the x vector on the last node. If =0 the old value is used. + * \param uEnd Value for the u vector on the second to last node. If =0 the old value is used. + */ +void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd ); + +/** Shift controls vector by one interval. + * + * \param uEnd Value for the u vector on the second to last node. If =0 the old value is used. + */ +void acado_shiftControls( real_t* const uEnd ); + +/** Get the KKT tolerance of the current iterate. + * + * \return The KKT tolerance value. + */ +real_t acado_getKKT( ); + +/** Calculate the objective value. + * + * \return Value of the objective function. + */ +real_t acado_getObjective( ); + + +/* + * Extern declarations. + */ + +extern ACADOworkspace acadoWorkspace; +extern ACADOvariables acadoVariables; + +/** @} */ + +#ifndef __MATLAB__ +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ +#endif /* __MATLAB__ */ + +#endif /* ACADO_COMMON_H */ diff --git a/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_integrator.c b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_integrator.c new file mode 100644 index 00000000..7212295f --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_integrator.c @@ -0,0 +1,231 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#include "acado_common.h" + + +void acado_rhs_forw(const real_t* in, real_t* out) +{ +const real_t* xd = in; +const real_t* u = in + 24; + +/* Compute outputs: */ +out[0] = xd[1]; +out[1] = xd[2]; +out[2] = u[0]; +out[3] = (real_t)(1.0000000000000000e+00); +out[4] = xd[8]; +out[5] = xd[9]; +out[6] = xd[10]; +out[7] = xd[11]; +out[8] = xd[12]; +out[9] = xd[13]; +out[10] = xd[14]; +out[11] = xd[15]; +out[12] = (real_t)(0.0000000000000000e+00); +out[13] = (real_t)(0.0000000000000000e+00); +out[14] = (real_t)(0.0000000000000000e+00); +out[15] = (real_t)(0.0000000000000000e+00); +out[16] = (real_t)(0.0000000000000000e+00); +out[17] = (real_t)(0.0000000000000000e+00); +out[18] = (real_t)(0.0000000000000000e+00); +out[19] = (real_t)(0.0000000000000000e+00); +out[20] = xd[21]; +out[21] = xd[22]; +out[22] = (real_t)(1.0000000000000000e+00); +out[23] = (real_t)(0.0000000000000000e+00); +} + +/* Fixed step size:0.2 */ +int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index ) +{ +int error; + +int run1; +int numSteps[20] = {1, 1, 1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3}; +int numInts = numSteps[rk_index]; +acadoWorkspace.rk_ttt = 0.0000000000000000e+00; +rk_eta[4] = 1.0000000000000000e+00; +rk_eta[5] = 0.0000000000000000e+00; +rk_eta[6] = 0.0000000000000000e+00; +rk_eta[7] = 0.0000000000000000e+00; +rk_eta[8] = 0.0000000000000000e+00; +rk_eta[9] = 1.0000000000000000e+00; +rk_eta[10] = 0.0000000000000000e+00; +rk_eta[11] = 0.0000000000000000e+00; +rk_eta[12] = 0.0000000000000000e+00; +rk_eta[13] = 0.0000000000000000e+00; +rk_eta[14] = 1.0000000000000000e+00; +rk_eta[15] = 0.0000000000000000e+00; +rk_eta[16] = 0.0000000000000000e+00; +rk_eta[17] = 0.0000000000000000e+00; +rk_eta[18] = 0.0000000000000000e+00; +rk_eta[19] = 1.0000000000000000e+00; +rk_eta[20] = 0.0000000000000000e+00; +rk_eta[21] = 0.0000000000000000e+00; +rk_eta[22] = 0.0000000000000000e+00; +rk_eta[23] = 0.0000000000000000e+00; +acadoWorkspace.rk_xxx[24] = rk_eta[24]; +acadoWorkspace.rk_xxx[25] = rk_eta[25]; +acadoWorkspace.rk_xxx[26] = rk_eta[26]; +acadoWorkspace.rk_xxx[27] = rk_eta[27]; +acadoWorkspace.rk_xxx[28] = rk_eta[28]; +acadoWorkspace.rk_xxx[29] = rk_eta[29]; +acadoWorkspace.rk_xxx[30] = rk_eta[30]; +acadoWorkspace.rk_xxx[31] = rk_eta[31]; +acadoWorkspace.rk_xxx[32] = rk_eta[32]; +acadoWorkspace.rk_xxx[33] = rk_eta[33]; +acadoWorkspace.rk_xxx[34] = rk_eta[34]; +acadoWorkspace.rk_xxx[35] = rk_eta[35]; +acadoWorkspace.rk_xxx[36] = rk_eta[36]; + +for (run1 = 0; run1 < 1; ++run1) +{ +for(run1 = 0; run1 < numInts; run1++ ) { +acadoWorkspace.rk_xxx[0] = + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + rk_eta[23]; +acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk ); +acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[15] + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[16] + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[17] + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[18] + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[19] + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[20] + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[21] + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[22] + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[23] + rk_eta[23]; +acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 24 ]) ); +acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[24] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[25] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[26] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[27] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[28] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[29] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[30] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[31] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[32] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[33] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[34] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[35] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[36] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[37] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[38] + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[39] + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[40] + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[41] + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[42] + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[43] + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[44] + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[45] + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[46] + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[47] + rk_eta[23]; +acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) ); +acadoWorkspace.rk_xxx[0] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[48] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[49] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[50] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[51] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[52] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[53] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[54] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[55] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[56] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[57] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[58] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[59] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[60] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[61] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[62] + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[63] + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[64] + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[65] + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[66] + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[67] + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[68] + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[69] + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[70] + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[71] + rk_eta[23]; +acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 72 ]) ); +rk_eta[0] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[0] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[24] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[48] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[72]; +rk_eta[1] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[1] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[25] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[49] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[73]; +rk_eta[2] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[2] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[26] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[50] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[74]; +rk_eta[3] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[3] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[27] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[51] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[75]; +rk_eta[4] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[4] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[28] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[52] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[76]; +rk_eta[5] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[5] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[29] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[53] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[77]; +rk_eta[6] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[6] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[30] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[54] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[78]; +rk_eta[7] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[7] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[31] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[55] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[79]; +rk_eta[8] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[8] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[32] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[56] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[80]; +rk_eta[9] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[9] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[33] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[57] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[81]; +rk_eta[10] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[10] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[34] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[58] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[82]; +rk_eta[11] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[11] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[35] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[59] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[83]; +rk_eta[12] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[12] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[36] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[60] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[84]; +rk_eta[13] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[13] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[37] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[61] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[85]; +rk_eta[14] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[14] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[38] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[62] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[86]; +rk_eta[15] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[15] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[39] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[63] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[87]; +rk_eta[16] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[16] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[40] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[64] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[88]; +rk_eta[17] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[17] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[41] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[65] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[89]; +rk_eta[18] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[18] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[42] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[66] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[90]; +rk_eta[19] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[19] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[43] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[67] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[91]; +rk_eta[20] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[20] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[44] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[68] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[92]; +rk_eta[21] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[21] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[45] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[69] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[93]; +rk_eta[22] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[22] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[46] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[70] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[94]; +rk_eta[23] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[23] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[47] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[71] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[95]; +acadoWorkspace.rk_ttt += 1.0000000000000000e+00; +} +} +error = 0; +return error; +} + diff --git a/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_qpoases_interface.cpp b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_qpoases_interface.cpp new file mode 100644 index 00000000..dd6fada4 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_qpoases_interface.cpp @@ -0,0 +1,70 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +extern "C" +{ +#include "acado_common.h" +} + +#include "INCLUDE/QProblemB.hpp" + +#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1 +#include "INCLUDE/EXTRAS/SolutionAnalysis.hpp" +#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ + +static int acado_nWSR; + + + +#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1 +static SolutionAnalysis acado_sa; +#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ + +int acado_solve( void ) +{ + acado_nWSR = QPOASES_NWSRMAX; + + QProblemB qp( 24 ); + + returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.lb, acadoWorkspace.ub, acado_nWSR, acadoWorkspace.y); + + qp.getPrimalSolution( acadoWorkspace.x ); + qp.getDualSolution( acadoWorkspace.y ); + +#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1 + + if (retVal != SUCCESSFUL_RETURN) + return (int)retVal; + + retVal = acado_sa.getHessianInverse( &qp,var ); + +#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ + + return (int)retVal; +} + +int acado_getNWSR( void ) +{ + return acado_nWSR; +} + +const char* acado_getErrorString( int error ) +{ + return MessageHandling::getErrorString( error ); +} diff --git a/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_qpoases_interface.hpp b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_qpoases_interface.hpp new file mode 100644 index 00000000..cc7d1e1c --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_qpoases_interface.hpp @@ -0,0 +1,65 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#ifndef QPOASES_HEADER +#define QPOASES_HEADER + +#ifdef PC_DEBUG +#include +#endif /* PC_DEBUG */ + +#include + +#ifdef __cplusplus +#define EXTERNC extern "C" +#else +#define EXTERNC +#endif + +/* + * A set of options for qpOASES + */ + +/** Maximum number of optimization variables. */ +#define QPOASES_NVMAX 24 +/** Maximum number of constraints. */ +#define QPOASES_NCMAX 0 +/** Maximum number of working set recalculations. */ +#define QPOASES_NWSRMAX 500 +/** Print level for qpOASES. */ +#define QPOASES_PRINTLEVEL PL_NONE +/** The value of EPS */ +#define QPOASES_EPS 2.221e-16 +/** Internally used floating point type */ +typedef double real_t; + +/* + * Forward function declarations + */ + +/** A function that calls the QP solver */ +EXTERNC int acado_solve( void ); + +/** Get the number of active set changes */ +EXTERNC int acado_getNWSR( void ); + +/** Get the error string. */ +const char* acado_getErrorString( int error ); + +#endif /* QPOASES_HEADER */ diff --git a/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_solver.c new file mode 100644 index 00000000..411d47b6 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_solver.c @@ -0,0 +1,4944 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#include "acado_common.h" + + + + +/******************************************************************************/ +/* */ +/* ACADO code generation */ +/* */ +/******************************************************************************/ + + +int acado_modelSimulation( ) +{ +int ret; + +int lRun1; +ret = 0; +for (lRun1 = 0; lRun1 < 20; ++lRun1) +{ +acadoWorkspace.state[0] = acadoVariables.x[lRun1 * 4]; +acadoWorkspace.state[1] = acadoVariables.x[lRun1 * 4 + 1]; +acadoWorkspace.state[2] = acadoVariables.x[lRun1 * 4 + 2]; +acadoWorkspace.state[3] = acadoVariables.x[lRun1 * 4 + 3]; + +acadoWorkspace.state[24] = acadoVariables.u[lRun1]; +acadoWorkspace.state[25] = acadoVariables.od[lRun1 * 12]; +acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 12 + 1]; +acadoWorkspace.state[27] = acadoVariables.od[lRun1 * 12 + 2]; +acadoWorkspace.state[28] = acadoVariables.od[lRun1 * 12 + 3]; +acadoWorkspace.state[29] = acadoVariables.od[lRun1 * 12 + 4]; +acadoWorkspace.state[30] = acadoVariables.od[lRun1 * 12 + 5]; +acadoWorkspace.state[31] = acadoVariables.od[lRun1 * 12 + 6]; +acadoWorkspace.state[32] = acadoVariables.od[lRun1 * 12 + 7]; +acadoWorkspace.state[33] = acadoVariables.od[lRun1 * 12 + 8]; +acadoWorkspace.state[34] = acadoVariables.od[lRun1 * 12 + 9]; +acadoWorkspace.state[35] = acadoVariables.od[lRun1 * 12 + 10]; +acadoWorkspace.state[36] = acadoVariables.od[lRun1 * 12 + 11]; + +ret = acado_integrate(acadoWorkspace.state, 1, lRun1); + +acadoWorkspace.d[lRun1 * 4] = acadoWorkspace.state[0] - acadoVariables.x[lRun1 * 4 + 4]; +acadoWorkspace.d[lRun1 * 4 + 1] = acadoWorkspace.state[1] - acadoVariables.x[lRun1 * 4 + 5]; +acadoWorkspace.d[lRun1 * 4 + 2] = acadoWorkspace.state[2] - acadoVariables.x[lRun1 * 4 + 6]; +acadoWorkspace.d[lRun1 * 4 + 3] = acadoWorkspace.state[3] - acadoVariables.x[lRun1 * 4 + 7]; + +acadoWorkspace.evGx[lRun1 * 16] = acadoWorkspace.state[4]; +acadoWorkspace.evGx[lRun1 * 16 + 1] = acadoWorkspace.state[5]; +acadoWorkspace.evGx[lRun1 * 16 + 2] = acadoWorkspace.state[6]; +acadoWorkspace.evGx[lRun1 * 16 + 3] = acadoWorkspace.state[7]; +acadoWorkspace.evGx[lRun1 * 16 + 4] = acadoWorkspace.state[8]; +acadoWorkspace.evGx[lRun1 * 16 + 5] = acadoWorkspace.state[9]; +acadoWorkspace.evGx[lRun1 * 16 + 6] = acadoWorkspace.state[10]; +acadoWorkspace.evGx[lRun1 * 16 + 7] = acadoWorkspace.state[11]; +acadoWorkspace.evGx[lRun1 * 16 + 8] = acadoWorkspace.state[12]; +acadoWorkspace.evGx[lRun1 * 16 + 9] = acadoWorkspace.state[13]; +acadoWorkspace.evGx[lRun1 * 16 + 10] = acadoWorkspace.state[14]; +acadoWorkspace.evGx[lRun1 * 16 + 11] = acadoWorkspace.state[15]; +acadoWorkspace.evGx[lRun1 * 16 + 12] = acadoWorkspace.state[16]; +acadoWorkspace.evGx[lRun1 * 16 + 13] = acadoWorkspace.state[17]; +acadoWorkspace.evGx[lRun1 * 16 + 14] = acadoWorkspace.state[18]; +acadoWorkspace.evGx[lRun1 * 16 + 15] = acadoWorkspace.state[19]; + +acadoWorkspace.evGu[lRun1 * 4] = acadoWorkspace.state[20]; +acadoWorkspace.evGu[lRun1 * 4 + 1] = acadoWorkspace.state[21]; +acadoWorkspace.evGu[lRun1 * 4 + 2] = acadoWorkspace.state[22]; +acadoWorkspace.evGu[lRun1 * 4 + 3] = acadoWorkspace.state[23]; +} +return ret; +} + +void acado_evaluateLSQ(const real_t* in, real_t* out) +{ +const real_t* xd = in; +const real_t* u = in + 4; +const real_t* od = in + 5; + +/* Compute outputs: */ +out[0] = (xd[0]-((((od[0]*((xd[3]*xd[3])*xd[3]))+(od[1]*(xd[3]*xd[3])))+(od[2]*xd[3]))+od[3])); +out[1] = (xd[1]-((((od[4]*((xd[3]*xd[3])*xd[3]))+(od[5]*(xd[3]*xd[3])))+(od[6]*xd[3]))+od[7])); +out[2] = (xd[2]-((((od[8]*((xd[3]*xd[3])*xd[3]))+(od[9]*(xd[3]*xd[3])))+(od[10]*xd[3]))+od[11])); +out[3] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); +out[4] = (u[0]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); +out[5] = (real_t)(1.0000000000000000e+00); +out[6] = (real_t)(0.0000000000000000e+00); +out[7] = (real_t)(0.0000000000000000e+00); +out[8] = ((real_t)(0.0000000000000000e+00)-(((od[0]*(((xd[3]+xd[3])*xd[3])+(xd[3]*xd[3])))+(od[1]*(xd[3]+xd[3])))+od[2])); +out[9] = (real_t)(0.0000000000000000e+00); +out[10] = (real_t)(1.0000000000000000e+00); +out[11] = (real_t)(0.0000000000000000e+00); +out[12] = ((real_t)(0.0000000000000000e+00)-(((od[4]*(((xd[3]+xd[3])*xd[3])+(xd[3]*xd[3])))+(od[5]*(xd[3]+xd[3])))+od[6])); +out[13] = (real_t)(0.0000000000000000e+00); +out[14] = (real_t)(0.0000000000000000e+00); +out[15] = (real_t)(1.0000000000000000e+00); +out[16] = ((real_t)(0.0000000000000000e+00)-(((od[8]*(((xd[3]+xd[3])*xd[3])+(xd[3]*xd[3])))+(od[9]*(xd[3]+xd[3])))+od[10])); +out[17] = (real_t)(0.0000000000000000e+00); +out[18] = (xd[2]*(real_t)(1.0000000000000001e-01)); +out[19] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)); +out[20] = (real_t)(0.0000000000000000e+00); +out[21] = (real_t)(0.0000000000000000e+00); +out[22] = (u[0]*(real_t)(1.0000000000000001e-01)); +out[23] = (real_t)(0.0000000000000000e+00); +out[24] = (real_t)(0.0000000000000000e+00); +out[25] = (real_t)(0.0000000000000000e+00); +out[26] = (real_t)(0.0000000000000000e+00); +out[27] = (real_t)(0.0000000000000000e+00); +out[28] = (real_t)(0.0000000000000000e+00); +out[29] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)); +} + +void acado_evaluateLSQEndTerm(const real_t* in, real_t* out) +{ +const real_t* xd = in; +const real_t* od = in + 4; + +/* Compute outputs: */ +out[0] = (xd[0]-((((od[0]*((xd[3]*xd[3])*xd[3]))+(od[1]*(xd[3]*xd[3])))+(od[2]*xd[3]))+od[3])); +out[1] = (xd[1]-((((od[4]*((xd[3]*xd[3])*xd[3]))+(od[5]*(xd[3]*xd[3])))+(od[6]*xd[3]))+od[7])); +out[2] = (xd[2]-((((od[8]*((xd[3]*xd[3])*xd[3]))+(od[9]*(xd[3]*xd[3])))+(od[10]*xd[3]))+od[11])); +out[3] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); +out[4] = (real_t)(1.0000000000000000e+00); +out[5] = (real_t)(0.0000000000000000e+00); +out[6] = (real_t)(0.0000000000000000e+00); +out[7] = ((real_t)(0.0000000000000000e+00)-(((od[0]*(((xd[3]+xd[3])*xd[3])+(xd[3]*xd[3])))+(od[1]*(xd[3]+xd[3])))+od[2])); +out[8] = (real_t)(0.0000000000000000e+00); +out[9] = (real_t)(1.0000000000000000e+00); +out[10] = (real_t)(0.0000000000000000e+00); +out[11] = ((real_t)(0.0000000000000000e+00)-(((od[4]*(((xd[3]+xd[3])*xd[3])+(xd[3]*xd[3])))+(od[5]*(xd[3]+xd[3])))+od[6])); +out[12] = (real_t)(0.0000000000000000e+00); +out[13] = (real_t)(0.0000000000000000e+00); +out[14] = (real_t)(1.0000000000000000e+00); +out[15] = ((real_t)(0.0000000000000000e+00)-(((od[8]*(((xd[3]+xd[3])*xd[3])+(xd[3]*xd[3])))+(od[9]*(xd[3]+xd[3])))+od[10])); +out[16] = (real_t)(0.0000000000000000e+00); +out[17] = (xd[2]*(real_t)(1.0000000000000001e-01)); +out[18] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)); +out[19] = (real_t)(0.0000000000000000e+00); +} + +void acado_setObjQ1Q2( real_t* const tmpFx, real_t* const tmpObjS, real_t* const tmpQ1, real_t* const tmpQ2 ) +{ +tmpQ2[0] = + tmpFx[0]*tmpObjS[0] + tmpFx[4]*tmpObjS[5] + tmpFx[8]*tmpObjS[10] + tmpFx[12]*tmpObjS[15] + tmpFx[16]*tmpObjS[20]; +tmpQ2[1] = + tmpFx[0]*tmpObjS[1] + tmpFx[4]*tmpObjS[6] + tmpFx[8]*tmpObjS[11] + tmpFx[12]*tmpObjS[16] + tmpFx[16]*tmpObjS[21]; +tmpQ2[2] = + tmpFx[0]*tmpObjS[2] + tmpFx[4]*tmpObjS[7] + tmpFx[8]*tmpObjS[12] + tmpFx[12]*tmpObjS[17] + tmpFx[16]*tmpObjS[22]; +tmpQ2[3] = + tmpFx[0]*tmpObjS[3] + tmpFx[4]*tmpObjS[8] + tmpFx[8]*tmpObjS[13] + tmpFx[12]*tmpObjS[18] + tmpFx[16]*tmpObjS[23]; +tmpQ2[4] = + tmpFx[0]*tmpObjS[4] + tmpFx[4]*tmpObjS[9] + tmpFx[8]*tmpObjS[14] + tmpFx[12]*tmpObjS[19] + tmpFx[16]*tmpObjS[24]; +tmpQ2[5] = + tmpFx[1]*tmpObjS[0] + tmpFx[5]*tmpObjS[5] + tmpFx[9]*tmpObjS[10] + tmpFx[13]*tmpObjS[15] + tmpFx[17]*tmpObjS[20]; +tmpQ2[6] = + tmpFx[1]*tmpObjS[1] + tmpFx[5]*tmpObjS[6] + tmpFx[9]*tmpObjS[11] + tmpFx[13]*tmpObjS[16] + tmpFx[17]*tmpObjS[21]; +tmpQ2[7] = + tmpFx[1]*tmpObjS[2] + tmpFx[5]*tmpObjS[7] + tmpFx[9]*tmpObjS[12] + tmpFx[13]*tmpObjS[17] + tmpFx[17]*tmpObjS[22]; +tmpQ2[8] = + tmpFx[1]*tmpObjS[3] + tmpFx[5]*tmpObjS[8] + tmpFx[9]*tmpObjS[13] + tmpFx[13]*tmpObjS[18] + tmpFx[17]*tmpObjS[23]; +tmpQ2[9] = + tmpFx[1]*tmpObjS[4] + tmpFx[5]*tmpObjS[9] + tmpFx[9]*tmpObjS[14] + tmpFx[13]*tmpObjS[19] + tmpFx[17]*tmpObjS[24]; +tmpQ2[10] = + tmpFx[2]*tmpObjS[0] + tmpFx[6]*tmpObjS[5] + tmpFx[10]*tmpObjS[10] + tmpFx[14]*tmpObjS[15] + tmpFx[18]*tmpObjS[20]; +tmpQ2[11] = + tmpFx[2]*tmpObjS[1] + tmpFx[6]*tmpObjS[6] + tmpFx[10]*tmpObjS[11] + tmpFx[14]*tmpObjS[16] + tmpFx[18]*tmpObjS[21]; +tmpQ2[12] = + tmpFx[2]*tmpObjS[2] + tmpFx[6]*tmpObjS[7] + tmpFx[10]*tmpObjS[12] + tmpFx[14]*tmpObjS[17] + tmpFx[18]*tmpObjS[22]; +tmpQ2[13] = + tmpFx[2]*tmpObjS[3] + tmpFx[6]*tmpObjS[8] + tmpFx[10]*tmpObjS[13] + tmpFx[14]*tmpObjS[18] + tmpFx[18]*tmpObjS[23]; +tmpQ2[14] = + tmpFx[2]*tmpObjS[4] + tmpFx[6]*tmpObjS[9] + tmpFx[10]*tmpObjS[14] + tmpFx[14]*tmpObjS[19] + tmpFx[18]*tmpObjS[24]; +tmpQ2[15] = + tmpFx[3]*tmpObjS[0] + tmpFx[7]*tmpObjS[5] + tmpFx[11]*tmpObjS[10] + tmpFx[15]*tmpObjS[15] + tmpFx[19]*tmpObjS[20]; +tmpQ2[16] = + tmpFx[3]*tmpObjS[1] + tmpFx[7]*tmpObjS[6] + tmpFx[11]*tmpObjS[11] + tmpFx[15]*tmpObjS[16] + tmpFx[19]*tmpObjS[21]; +tmpQ2[17] = + tmpFx[3]*tmpObjS[2] + tmpFx[7]*tmpObjS[7] + tmpFx[11]*tmpObjS[12] + tmpFx[15]*tmpObjS[17] + tmpFx[19]*tmpObjS[22]; +tmpQ2[18] = + tmpFx[3]*tmpObjS[3] + tmpFx[7]*tmpObjS[8] + tmpFx[11]*tmpObjS[13] + tmpFx[15]*tmpObjS[18] + tmpFx[19]*tmpObjS[23]; +tmpQ2[19] = + tmpFx[3]*tmpObjS[4] + tmpFx[7]*tmpObjS[9] + tmpFx[11]*tmpObjS[14] + tmpFx[15]*tmpObjS[19] + tmpFx[19]*tmpObjS[24]; +tmpQ1[0] = + tmpQ2[0]*tmpFx[0] + tmpQ2[1]*tmpFx[4] + tmpQ2[2]*tmpFx[8] + tmpQ2[3]*tmpFx[12] + tmpQ2[4]*tmpFx[16]; +tmpQ1[1] = + tmpQ2[0]*tmpFx[1] + tmpQ2[1]*tmpFx[5] + tmpQ2[2]*tmpFx[9] + tmpQ2[3]*tmpFx[13] + tmpQ2[4]*tmpFx[17]; +tmpQ1[2] = + tmpQ2[0]*tmpFx[2] + tmpQ2[1]*tmpFx[6] + tmpQ2[2]*tmpFx[10] + tmpQ2[3]*tmpFx[14] + tmpQ2[4]*tmpFx[18]; +tmpQ1[3] = + tmpQ2[0]*tmpFx[3] + tmpQ2[1]*tmpFx[7] + tmpQ2[2]*tmpFx[11] + tmpQ2[3]*tmpFx[15] + tmpQ2[4]*tmpFx[19]; +tmpQ1[4] = + tmpQ2[5]*tmpFx[0] + tmpQ2[6]*tmpFx[4] + tmpQ2[7]*tmpFx[8] + tmpQ2[8]*tmpFx[12] + tmpQ2[9]*tmpFx[16]; +tmpQ1[5] = + tmpQ2[5]*tmpFx[1] + tmpQ2[6]*tmpFx[5] + tmpQ2[7]*tmpFx[9] + tmpQ2[8]*tmpFx[13] + tmpQ2[9]*tmpFx[17]; +tmpQ1[6] = + tmpQ2[5]*tmpFx[2] + tmpQ2[6]*tmpFx[6] + tmpQ2[7]*tmpFx[10] + tmpQ2[8]*tmpFx[14] + tmpQ2[9]*tmpFx[18]; +tmpQ1[7] = + tmpQ2[5]*tmpFx[3] + tmpQ2[6]*tmpFx[7] + tmpQ2[7]*tmpFx[11] + tmpQ2[8]*tmpFx[15] + tmpQ2[9]*tmpFx[19]; +tmpQ1[8] = + tmpQ2[10]*tmpFx[0] + tmpQ2[11]*tmpFx[4] + tmpQ2[12]*tmpFx[8] + tmpQ2[13]*tmpFx[12] + tmpQ2[14]*tmpFx[16]; +tmpQ1[9] = + tmpQ2[10]*tmpFx[1] + tmpQ2[11]*tmpFx[5] + tmpQ2[12]*tmpFx[9] + tmpQ2[13]*tmpFx[13] + tmpQ2[14]*tmpFx[17]; +tmpQ1[10] = + tmpQ2[10]*tmpFx[2] + tmpQ2[11]*tmpFx[6] + tmpQ2[12]*tmpFx[10] + tmpQ2[13]*tmpFx[14] + tmpQ2[14]*tmpFx[18]; +tmpQ1[11] = + tmpQ2[10]*tmpFx[3] + tmpQ2[11]*tmpFx[7] + tmpQ2[12]*tmpFx[11] + tmpQ2[13]*tmpFx[15] + tmpQ2[14]*tmpFx[19]; +tmpQ1[12] = + tmpQ2[15]*tmpFx[0] + tmpQ2[16]*tmpFx[4] + tmpQ2[17]*tmpFx[8] + tmpQ2[18]*tmpFx[12] + tmpQ2[19]*tmpFx[16]; +tmpQ1[13] = + tmpQ2[15]*tmpFx[1] + tmpQ2[16]*tmpFx[5] + tmpQ2[17]*tmpFx[9] + tmpQ2[18]*tmpFx[13] + tmpQ2[19]*tmpFx[17]; +tmpQ1[14] = + tmpQ2[15]*tmpFx[2] + tmpQ2[16]*tmpFx[6] + tmpQ2[17]*tmpFx[10] + tmpQ2[18]*tmpFx[14] + tmpQ2[19]*tmpFx[18]; +tmpQ1[15] = + tmpQ2[15]*tmpFx[3] + tmpQ2[16]*tmpFx[7] + tmpQ2[17]*tmpFx[11] + tmpQ2[18]*tmpFx[15] + tmpQ2[19]*tmpFx[19]; +} + +void acado_setObjR1R2( real_t* const tmpFu, real_t* const tmpObjS, real_t* const tmpR1, real_t* const tmpR2 ) +{ +tmpR2[0] = + tmpFu[0]*tmpObjS[0] + tmpFu[1]*tmpObjS[5] + tmpFu[2]*tmpObjS[10] + tmpFu[3]*tmpObjS[15] + tmpFu[4]*tmpObjS[20]; +tmpR2[1] = + tmpFu[0]*tmpObjS[1] + tmpFu[1]*tmpObjS[6] + tmpFu[2]*tmpObjS[11] + tmpFu[3]*tmpObjS[16] + tmpFu[4]*tmpObjS[21]; +tmpR2[2] = + tmpFu[0]*tmpObjS[2] + tmpFu[1]*tmpObjS[7] + tmpFu[2]*tmpObjS[12] + tmpFu[3]*tmpObjS[17] + tmpFu[4]*tmpObjS[22]; +tmpR2[3] = + tmpFu[0]*tmpObjS[3] + tmpFu[1]*tmpObjS[8] + tmpFu[2]*tmpObjS[13] + tmpFu[3]*tmpObjS[18] + tmpFu[4]*tmpObjS[23]; +tmpR2[4] = + tmpFu[0]*tmpObjS[4] + tmpFu[1]*tmpObjS[9] + tmpFu[2]*tmpObjS[14] + tmpFu[3]*tmpObjS[19] + tmpFu[4]*tmpObjS[24]; +tmpR1[0] = + tmpR2[0]*tmpFu[0] + tmpR2[1]*tmpFu[1] + tmpR2[2]*tmpFu[2] + tmpR2[3]*tmpFu[3] + tmpR2[4]*tmpFu[4]; +} + +void acado_setObjQN1QN2( real_t* const tmpFx, real_t* const tmpObjSEndTerm, real_t* const tmpQN1, real_t* const tmpQN2 ) +{ +tmpQN2[0] = + tmpFx[0]*tmpObjSEndTerm[0] + tmpFx[4]*tmpObjSEndTerm[4] + tmpFx[8]*tmpObjSEndTerm[8] + tmpFx[12]*tmpObjSEndTerm[12]; +tmpQN2[1] = + tmpFx[0]*tmpObjSEndTerm[1] + tmpFx[4]*tmpObjSEndTerm[5] + tmpFx[8]*tmpObjSEndTerm[9] + tmpFx[12]*tmpObjSEndTerm[13]; +tmpQN2[2] = + tmpFx[0]*tmpObjSEndTerm[2] + tmpFx[4]*tmpObjSEndTerm[6] + tmpFx[8]*tmpObjSEndTerm[10] + tmpFx[12]*tmpObjSEndTerm[14]; +tmpQN2[3] = + tmpFx[0]*tmpObjSEndTerm[3] + tmpFx[4]*tmpObjSEndTerm[7] + tmpFx[8]*tmpObjSEndTerm[11] + tmpFx[12]*tmpObjSEndTerm[15]; +tmpQN2[4] = + tmpFx[1]*tmpObjSEndTerm[0] + tmpFx[5]*tmpObjSEndTerm[4] + tmpFx[9]*tmpObjSEndTerm[8] + tmpFx[13]*tmpObjSEndTerm[12]; +tmpQN2[5] = + tmpFx[1]*tmpObjSEndTerm[1] + tmpFx[5]*tmpObjSEndTerm[5] + tmpFx[9]*tmpObjSEndTerm[9] + tmpFx[13]*tmpObjSEndTerm[13]; +tmpQN2[6] = + tmpFx[1]*tmpObjSEndTerm[2] + tmpFx[5]*tmpObjSEndTerm[6] + tmpFx[9]*tmpObjSEndTerm[10] + tmpFx[13]*tmpObjSEndTerm[14]; +tmpQN2[7] = + tmpFx[1]*tmpObjSEndTerm[3] + tmpFx[5]*tmpObjSEndTerm[7] + tmpFx[9]*tmpObjSEndTerm[11] + tmpFx[13]*tmpObjSEndTerm[15]; +tmpQN2[8] = + tmpFx[2]*tmpObjSEndTerm[0] + tmpFx[6]*tmpObjSEndTerm[4] + tmpFx[10]*tmpObjSEndTerm[8] + tmpFx[14]*tmpObjSEndTerm[12]; +tmpQN2[9] = + tmpFx[2]*tmpObjSEndTerm[1] + tmpFx[6]*tmpObjSEndTerm[5] + tmpFx[10]*tmpObjSEndTerm[9] + tmpFx[14]*tmpObjSEndTerm[13]; +tmpQN2[10] = + tmpFx[2]*tmpObjSEndTerm[2] + tmpFx[6]*tmpObjSEndTerm[6] + tmpFx[10]*tmpObjSEndTerm[10] + tmpFx[14]*tmpObjSEndTerm[14]; +tmpQN2[11] = + tmpFx[2]*tmpObjSEndTerm[3] + tmpFx[6]*tmpObjSEndTerm[7] + tmpFx[10]*tmpObjSEndTerm[11] + tmpFx[14]*tmpObjSEndTerm[15]; +tmpQN2[12] = + tmpFx[3]*tmpObjSEndTerm[0] + tmpFx[7]*tmpObjSEndTerm[4] + tmpFx[11]*tmpObjSEndTerm[8] + tmpFx[15]*tmpObjSEndTerm[12]; +tmpQN2[13] = + tmpFx[3]*tmpObjSEndTerm[1] + tmpFx[7]*tmpObjSEndTerm[5] + tmpFx[11]*tmpObjSEndTerm[9] + tmpFx[15]*tmpObjSEndTerm[13]; +tmpQN2[14] = + tmpFx[3]*tmpObjSEndTerm[2] + tmpFx[7]*tmpObjSEndTerm[6] + tmpFx[11]*tmpObjSEndTerm[10] + tmpFx[15]*tmpObjSEndTerm[14]; +tmpQN2[15] = + tmpFx[3]*tmpObjSEndTerm[3] + tmpFx[7]*tmpObjSEndTerm[7] + tmpFx[11]*tmpObjSEndTerm[11] + tmpFx[15]*tmpObjSEndTerm[15]; +tmpQN1[0] = + tmpQN2[0]*tmpFx[0] + tmpQN2[1]*tmpFx[4] + tmpQN2[2]*tmpFx[8] + tmpQN2[3]*tmpFx[12]; +tmpQN1[1] = + tmpQN2[0]*tmpFx[1] + tmpQN2[1]*tmpFx[5] + tmpQN2[2]*tmpFx[9] + tmpQN2[3]*tmpFx[13]; +tmpQN1[2] = + tmpQN2[0]*tmpFx[2] + tmpQN2[1]*tmpFx[6] + tmpQN2[2]*tmpFx[10] + tmpQN2[3]*tmpFx[14]; +tmpQN1[3] = + tmpQN2[0]*tmpFx[3] + tmpQN2[1]*tmpFx[7] + tmpQN2[2]*tmpFx[11] + tmpQN2[3]*tmpFx[15]; +tmpQN1[4] = + tmpQN2[4]*tmpFx[0] + tmpQN2[5]*tmpFx[4] + tmpQN2[6]*tmpFx[8] + tmpQN2[7]*tmpFx[12]; +tmpQN1[5] = + tmpQN2[4]*tmpFx[1] + tmpQN2[5]*tmpFx[5] + tmpQN2[6]*tmpFx[9] + tmpQN2[7]*tmpFx[13]; +tmpQN1[6] = + tmpQN2[4]*tmpFx[2] + tmpQN2[5]*tmpFx[6] + tmpQN2[6]*tmpFx[10] + tmpQN2[7]*tmpFx[14]; +tmpQN1[7] = + tmpQN2[4]*tmpFx[3] + tmpQN2[5]*tmpFx[7] + tmpQN2[6]*tmpFx[11] + tmpQN2[7]*tmpFx[15]; +tmpQN1[8] = + tmpQN2[8]*tmpFx[0] + tmpQN2[9]*tmpFx[4] + tmpQN2[10]*tmpFx[8] + tmpQN2[11]*tmpFx[12]; +tmpQN1[9] = + tmpQN2[8]*tmpFx[1] + tmpQN2[9]*tmpFx[5] + tmpQN2[10]*tmpFx[9] + tmpQN2[11]*tmpFx[13]; +tmpQN1[10] = + tmpQN2[8]*tmpFx[2] + tmpQN2[9]*tmpFx[6] + tmpQN2[10]*tmpFx[10] + tmpQN2[11]*tmpFx[14]; +tmpQN1[11] = + tmpQN2[8]*tmpFx[3] + tmpQN2[9]*tmpFx[7] + tmpQN2[10]*tmpFx[11] + tmpQN2[11]*tmpFx[15]; +tmpQN1[12] = + tmpQN2[12]*tmpFx[0] + tmpQN2[13]*tmpFx[4] + tmpQN2[14]*tmpFx[8] + tmpQN2[15]*tmpFx[12]; +tmpQN1[13] = + tmpQN2[12]*tmpFx[1] + tmpQN2[13]*tmpFx[5] + tmpQN2[14]*tmpFx[9] + tmpQN2[15]*tmpFx[13]; +tmpQN1[14] = + tmpQN2[12]*tmpFx[2] + tmpQN2[13]*tmpFx[6] + tmpQN2[14]*tmpFx[10] + tmpQN2[15]*tmpFx[14]; +tmpQN1[15] = + tmpQN2[12]*tmpFx[3] + tmpQN2[13]*tmpFx[7] + tmpQN2[14]*tmpFx[11] + tmpQN2[15]*tmpFx[15]; +} + +void acado_evaluateObjective( ) +{ +int runObj; +for (runObj = 0; runObj < 20; ++runObj) +{ +acadoWorkspace.objValueIn[0] = acadoVariables.x[runObj * 4]; +acadoWorkspace.objValueIn[1] = acadoVariables.x[runObj * 4 + 1]; +acadoWorkspace.objValueIn[2] = acadoVariables.x[runObj * 4 + 2]; +acadoWorkspace.objValueIn[3] = acadoVariables.x[runObj * 4 + 3]; +acadoWorkspace.objValueIn[4] = acadoVariables.u[runObj]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 12]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 12 + 1]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 12 + 2]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 12 + 3]; +acadoWorkspace.objValueIn[9] = acadoVariables.od[runObj * 12 + 4]; +acadoWorkspace.objValueIn[10] = acadoVariables.od[runObj * 12 + 5]; +acadoWorkspace.objValueIn[11] = acadoVariables.od[runObj * 12 + 6]; +acadoWorkspace.objValueIn[12] = acadoVariables.od[runObj * 12 + 7]; +acadoWorkspace.objValueIn[13] = acadoVariables.od[runObj * 12 + 8]; +acadoWorkspace.objValueIn[14] = acadoVariables.od[runObj * 12 + 9]; +acadoWorkspace.objValueIn[15] = acadoVariables.od[runObj * 12 + 10]; +acadoWorkspace.objValueIn[16] = acadoVariables.od[runObj * 12 + 11]; + +acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acadoWorkspace.Dy[runObj * 5] = acadoWorkspace.objValueOut[0]; +acadoWorkspace.Dy[runObj * 5 + 1] = acadoWorkspace.objValueOut[1]; +acadoWorkspace.Dy[runObj * 5 + 2] = acadoWorkspace.objValueOut[2]; +acadoWorkspace.Dy[runObj * 5 + 3] = acadoWorkspace.objValueOut[3]; +acadoWorkspace.Dy[runObj * 5 + 4] = acadoWorkspace.objValueOut[4]; + +acado_setObjQ1Q2( &(acadoWorkspace.objValueOut[ 5 ]), &(acadoVariables.W[ runObj * 25 ]), &(acadoWorkspace.Q1[ runObj * 16 ]), &(acadoWorkspace.Q2[ runObj * 20 ]) ); + +acado_setObjR1R2( &(acadoWorkspace.objValueOut[ 25 ]), &(acadoVariables.W[ runObj * 25 ]), &(acadoWorkspace.R1[ runObj ]), &(acadoWorkspace.R2[ runObj * 5 ]) ); + +} +acadoWorkspace.objValueIn[0] = acadoVariables.x[80]; +acadoWorkspace.objValueIn[1] = acadoVariables.x[81]; +acadoWorkspace.objValueIn[2] = acadoVariables.x[82]; +acadoWorkspace.objValueIn[3] = acadoVariables.x[83]; +acadoWorkspace.objValueIn[4] = acadoVariables.od[240]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[241]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[242]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[243]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[244]; +acadoWorkspace.objValueIn[9] = acadoVariables.od[245]; +acadoWorkspace.objValueIn[10] = acadoVariables.od[246]; +acadoWorkspace.objValueIn[11] = acadoVariables.od[247]; +acadoWorkspace.objValueIn[12] = acadoVariables.od[248]; +acadoWorkspace.objValueIn[13] = acadoVariables.od[249]; +acadoWorkspace.objValueIn[14] = acadoVariables.od[250]; +acadoWorkspace.objValueIn[15] = acadoVariables.od[251]; +acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); + +acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0]; +acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1]; +acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2]; +acadoWorkspace.DyN[3] = acadoWorkspace.objValueOut[3]; + +acado_setObjQN1QN2( &(acadoWorkspace.objValueOut[ 4 ]), acadoVariables.WN, acadoWorkspace.QN1, acadoWorkspace.QN2 ); + +} + +void acado_multGxd( real_t* const dOld, real_t* const Gx1, real_t* const dNew ) +{ +dNew[0] += + Gx1[0]*dOld[0] + Gx1[1]*dOld[1] + Gx1[2]*dOld[2] + Gx1[3]*dOld[3]; +dNew[1] += + Gx1[4]*dOld[0] + Gx1[5]*dOld[1] + Gx1[6]*dOld[2] + Gx1[7]*dOld[3]; +dNew[2] += + Gx1[8]*dOld[0] + Gx1[9]*dOld[1] + Gx1[10]*dOld[2] + Gx1[11]*dOld[3]; +dNew[3] += + Gx1[12]*dOld[0] + Gx1[13]*dOld[1] + Gx1[14]*dOld[2] + Gx1[15]*dOld[3]; +} + +void acado_moveGxT( real_t* const Gx1, real_t* const Gx2 ) +{ +Gx2[0] = Gx1[0]; +Gx2[1] = Gx1[1]; +Gx2[2] = Gx1[2]; +Gx2[3] = Gx1[3]; +Gx2[4] = Gx1[4]; +Gx2[5] = Gx1[5]; +Gx2[6] = Gx1[6]; +Gx2[7] = Gx1[7]; +Gx2[8] = Gx1[8]; +Gx2[9] = Gx1[9]; +Gx2[10] = Gx1[10]; +Gx2[11] = Gx1[11]; +Gx2[12] = Gx1[12]; +Gx2[13] = Gx1[13]; +Gx2[14] = Gx1[14]; +Gx2[15] = Gx1[15]; +} + +void acado_multGxGx( real_t* const Gx1, real_t* const Gx2, real_t* const Gx3 ) +{ +Gx3[0] = + Gx1[0]*Gx2[0] + Gx1[1]*Gx2[4] + Gx1[2]*Gx2[8] + Gx1[3]*Gx2[12]; +Gx3[1] = + Gx1[0]*Gx2[1] + Gx1[1]*Gx2[5] + Gx1[2]*Gx2[9] + Gx1[3]*Gx2[13]; +Gx3[2] = + Gx1[0]*Gx2[2] + Gx1[1]*Gx2[6] + Gx1[2]*Gx2[10] + Gx1[3]*Gx2[14]; +Gx3[3] = + Gx1[0]*Gx2[3] + Gx1[1]*Gx2[7] + Gx1[2]*Gx2[11] + Gx1[3]*Gx2[15]; +Gx3[4] = + Gx1[4]*Gx2[0] + Gx1[5]*Gx2[4] + Gx1[6]*Gx2[8] + Gx1[7]*Gx2[12]; +Gx3[5] = + Gx1[4]*Gx2[1] + Gx1[5]*Gx2[5] + Gx1[6]*Gx2[9] + Gx1[7]*Gx2[13]; +Gx3[6] = + Gx1[4]*Gx2[2] + Gx1[5]*Gx2[6] + Gx1[6]*Gx2[10] + Gx1[7]*Gx2[14]; +Gx3[7] = + Gx1[4]*Gx2[3] + Gx1[5]*Gx2[7] + Gx1[6]*Gx2[11] + Gx1[7]*Gx2[15]; +Gx3[8] = + Gx1[8]*Gx2[0] + Gx1[9]*Gx2[4] + Gx1[10]*Gx2[8] + Gx1[11]*Gx2[12]; +Gx3[9] = + Gx1[8]*Gx2[1] + Gx1[9]*Gx2[5] + Gx1[10]*Gx2[9] + Gx1[11]*Gx2[13]; +Gx3[10] = + Gx1[8]*Gx2[2] + Gx1[9]*Gx2[6] + Gx1[10]*Gx2[10] + Gx1[11]*Gx2[14]; +Gx3[11] = + Gx1[8]*Gx2[3] + Gx1[9]*Gx2[7] + Gx1[10]*Gx2[11] + Gx1[11]*Gx2[15]; +Gx3[12] = + Gx1[12]*Gx2[0] + Gx1[13]*Gx2[4] + Gx1[14]*Gx2[8] + Gx1[15]*Gx2[12]; +Gx3[13] = + Gx1[12]*Gx2[1] + Gx1[13]*Gx2[5] + Gx1[14]*Gx2[9] + Gx1[15]*Gx2[13]; +Gx3[14] = + Gx1[12]*Gx2[2] + Gx1[13]*Gx2[6] + Gx1[14]*Gx2[10] + Gx1[15]*Gx2[14]; +Gx3[15] = + Gx1[12]*Gx2[3] + Gx1[13]*Gx2[7] + Gx1[14]*Gx2[11] + Gx1[15]*Gx2[15]; +} + +void acado_multGxGu( real_t* const Gx1, real_t* const Gu1, real_t* const Gu2 ) +{ +Gu2[0] = + Gx1[0]*Gu1[0] + Gx1[1]*Gu1[1] + Gx1[2]*Gu1[2] + Gx1[3]*Gu1[3]; +Gu2[1] = + Gx1[4]*Gu1[0] + Gx1[5]*Gu1[1] + Gx1[6]*Gu1[2] + Gx1[7]*Gu1[3]; +Gu2[2] = + Gx1[8]*Gu1[0] + Gx1[9]*Gu1[1] + Gx1[10]*Gu1[2] + Gx1[11]*Gu1[3]; +Gu2[3] = + Gx1[12]*Gu1[0] + Gx1[13]*Gu1[1] + Gx1[14]*Gu1[2] + Gx1[15]*Gu1[3]; +} + +void acado_moveGuE( real_t* const Gu1, real_t* const Gu2 ) +{ +Gu2[0] = Gu1[0]; +Gu2[1] = Gu1[1]; +Gu2[2] = Gu1[2]; +Gu2[3] = Gu1[3]; +} + +void acado_setBlockH11( int iRow, int iCol, real_t* const Gu1, real_t* const Gu2 ) +{ +acadoWorkspace.H[(iRow * 24 + 96) + (iCol + 4)] += + Gu1[0]*Gu2[0] + Gu1[1]*Gu2[1] + Gu1[2]*Gu2[2] + Gu1[3]*Gu2[3]; +} + +void acado_setBlockH11_R1( int iRow, int iCol, real_t* const R11 ) +{ +acadoWorkspace.H[(iRow * 24 + 96) + (iCol + 4)] = R11[0]; +} + +void acado_zeroBlockH11( int iRow, int iCol ) +{ +acadoWorkspace.H[(iRow * 24 + 96) + (iCol + 4)] = 0.0000000000000000e+00; +} + +void acado_copyHTH( int iRow, int iCol ) +{ +acadoWorkspace.H[(iRow * 24 + 96) + (iCol + 4)] = acadoWorkspace.H[(iCol * 24 + 96) + (iRow + 4)]; +} + +void acado_multQ1d( real_t* const Gx1, real_t* const dOld, real_t* const dNew ) +{ +dNew[0] = + Gx1[0]*dOld[0] + Gx1[1]*dOld[1] + Gx1[2]*dOld[2] + Gx1[3]*dOld[3]; +dNew[1] = + Gx1[4]*dOld[0] + Gx1[5]*dOld[1] + Gx1[6]*dOld[2] + Gx1[7]*dOld[3]; +dNew[2] = + Gx1[8]*dOld[0] + Gx1[9]*dOld[1] + Gx1[10]*dOld[2] + Gx1[11]*dOld[3]; +dNew[3] = + Gx1[12]*dOld[0] + Gx1[13]*dOld[1] + Gx1[14]*dOld[2] + Gx1[15]*dOld[3]; +} + +void acado_multQN1d( real_t* const QN1, real_t* const dOld, real_t* const dNew ) +{ +dNew[0] = + acadoWorkspace.QN1[0]*dOld[0] + acadoWorkspace.QN1[1]*dOld[1] + acadoWorkspace.QN1[2]*dOld[2] + acadoWorkspace.QN1[3]*dOld[3]; +dNew[1] = + acadoWorkspace.QN1[4]*dOld[0] + acadoWorkspace.QN1[5]*dOld[1] + acadoWorkspace.QN1[6]*dOld[2] + acadoWorkspace.QN1[7]*dOld[3]; +dNew[2] = + acadoWorkspace.QN1[8]*dOld[0] + acadoWorkspace.QN1[9]*dOld[1] + acadoWorkspace.QN1[10]*dOld[2] + acadoWorkspace.QN1[11]*dOld[3]; +dNew[3] = + acadoWorkspace.QN1[12]*dOld[0] + acadoWorkspace.QN1[13]*dOld[1] + acadoWorkspace.QN1[14]*dOld[2] + acadoWorkspace.QN1[15]*dOld[3]; +} + +void acado_multRDy( real_t* const R2, real_t* const Dy1, real_t* const RDy1 ) +{ +RDy1[0] = + R2[0]*Dy1[0] + R2[1]*Dy1[1] + R2[2]*Dy1[2] + R2[3]*Dy1[3] + R2[4]*Dy1[4]; +} + +void acado_multQDy( real_t* const Q2, real_t* const Dy1, real_t* const QDy1 ) +{ +QDy1[0] = + Q2[0]*Dy1[0] + Q2[1]*Dy1[1] + Q2[2]*Dy1[2] + Q2[3]*Dy1[3] + Q2[4]*Dy1[4]; +QDy1[1] = + Q2[5]*Dy1[0] + Q2[6]*Dy1[1] + Q2[7]*Dy1[2] + Q2[8]*Dy1[3] + Q2[9]*Dy1[4]; +QDy1[2] = + Q2[10]*Dy1[0] + Q2[11]*Dy1[1] + Q2[12]*Dy1[2] + Q2[13]*Dy1[3] + Q2[14]*Dy1[4]; +QDy1[3] = + Q2[15]*Dy1[0] + Q2[16]*Dy1[1] + Q2[17]*Dy1[2] + Q2[18]*Dy1[3] + Q2[19]*Dy1[4]; +} + +void acado_multEQDy( real_t* const E1, real_t* const QDy1, real_t* const U1 ) +{ +U1[0] += + E1[0]*QDy1[0] + E1[1]*QDy1[1] + E1[2]*QDy1[2] + E1[3]*QDy1[3]; +} + +void acado_multQETGx( real_t* const E1, real_t* const Gx1, real_t* const H101 ) +{ +H101[0] += + E1[0]*Gx1[0] + E1[1]*Gx1[4] + E1[2]*Gx1[8] + E1[3]*Gx1[12]; +H101[1] += + E1[0]*Gx1[1] + E1[1]*Gx1[5] + E1[2]*Gx1[9] + E1[3]*Gx1[13]; +H101[2] += + E1[0]*Gx1[2] + E1[1]*Gx1[6] + E1[2]*Gx1[10] + E1[3]*Gx1[14]; +H101[3] += + E1[0]*Gx1[3] + E1[1]*Gx1[7] + E1[2]*Gx1[11] + E1[3]*Gx1[15]; +} + +void acado_zeroBlockH10( real_t* const H101 ) +{ +{ int lCopy; for (lCopy = 0; lCopy < 4; lCopy++) H101[ lCopy ] = 0; } +} + +void acado_multEDu( real_t* const E1, real_t* const U1, real_t* const dNew ) +{ +dNew[0] += + E1[0]*U1[0]; +dNew[1] += + E1[1]*U1[0]; +dNew[2] += + E1[2]*U1[0]; +dNew[3] += + E1[3]*U1[0]; +} + +void acado_zeroBlockH00( ) +{ +acadoWorkspace.H[0] = 0.0000000000000000e+00; +acadoWorkspace.H[1] = 0.0000000000000000e+00; +acadoWorkspace.H[2] = 0.0000000000000000e+00; +acadoWorkspace.H[3] = 0.0000000000000000e+00; +acadoWorkspace.H[24] = 0.0000000000000000e+00; +acadoWorkspace.H[25] = 0.0000000000000000e+00; +acadoWorkspace.H[26] = 0.0000000000000000e+00; +acadoWorkspace.H[27] = 0.0000000000000000e+00; +acadoWorkspace.H[48] = 0.0000000000000000e+00; +acadoWorkspace.H[49] = 0.0000000000000000e+00; +acadoWorkspace.H[50] = 0.0000000000000000e+00; +acadoWorkspace.H[51] = 0.0000000000000000e+00; +acadoWorkspace.H[72] = 0.0000000000000000e+00; +acadoWorkspace.H[73] = 0.0000000000000000e+00; +acadoWorkspace.H[74] = 0.0000000000000000e+00; +acadoWorkspace.H[75] = 0.0000000000000000e+00; +} + +void acado_multCTQC( real_t* const Gx1, real_t* const Gx2 ) +{ +acadoWorkspace.H[0] += + Gx1[0]*Gx2[0] + Gx1[4]*Gx2[4] + Gx1[8]*Gx2[8] + Gx1[12]*Gx2[12]; +acadoWorkspace.H[1] += + Gx1[0]*Gx2[1] + Gx1[4]*Gx2[5] + Gx1[8]*Gx2[9] + Gx1[12]*Gx2[13]; +acadoWorkspace.H[2] += + Gx1[0]*Gx2[2] + Gx1[4]*Gx2[6] + Gx1[8]*Gx2[10] + Gx1[12]*Gx2[14]; +acadoWorkspace.H[3] += + Gx1[0]*Gx2[3] + Gx1[4]*Gx2[7] + Gx1[8]*Gx2[11] + Gx1[12]*Gx2[15]; +acadoWorkspace.H[24] += + Gx1[1]*Gx2[0] + Gx1[5]*Gx2[4] + Gx1[9]*Gx2[8] + Gx1[13]*Gx2[12]; +acadoWorkspace.H[25] += + Gx1[1]*Gx2[1] + Gx1[5]*Gx2[5] + Gx1[9]*Gx2[9] + Gx1[13]*Gx2[13]; +acadoWorkspace.H[26] += + Gx1[1]*Gx2[2] + Gx1[5]*Gx2[6] + Gx1[9]*Gx2[10] + Gx1[13]*Gx2[14]; +acadoWorkspace.H[27] += + Gx1[1]*Gx2[3] + Gx1[5]*Gx2[7] + Gx1[9]*Gx2[11] + Gx1[13]*Gx2[15]; +acadoWorkspace.H[48] += + Gx1[2]*Gx2[0] + Gx1[6]*Gx2[4] + Gx1[10]*Gx2[8] + Gx1[14]*Gx2[12]; +acadoWorkspace.H[49] += + Gx1[2]*Gx2[1] + Gx1[6]*Gx2[5] + Gx1[10]*Gx2[9] + Gx1[14]*Gx2[13]; +acadoWorkspace.H[50] += + Gx1[2]*Gx2[2] + Gx1[6]*Gx2[6] + Gx1[10]*Gx2[10] + Gx1[14]*Gx2[14]; +acadoWorkspace.H[51] += + Gx1[2]*Gx2[3] + Gx1[6]*Gx2[7] + Gx1[10]*Gx2[11] + Gx1[14]*Gx2[15]; +acadoWorkspace.H[72] += + Gx1[3]*Gx2[0] + Gx1[7]*Gx2[4] + Gx1[11]*Gx2[8] + Gx1[15]*Gx2[12]; +acadoWorkspace.H[73] += + Gx1[3]*Gx2[1] + Gx1[7]*Gx2[5] + Gx1[11]*Gx2[9] + Gx1[15]*Gx2[13]; +acadoWorkspace.H[74] += + Gx1[3]*Gx2[2] + Gx1[7]*Gx2[6] + Gx1[11]*Gx2[10] + Gx1[15]*Gx2[14]; +acadoWorkspace.H[75] += + Gx1[3]*Gx2[3] + Gx1[7]*Gx2[7] + Gx1[11]*Gx2[11] + Gx1[15]*Gx2[15]; +} + +void acado_macCTSlx( real_t* const C0, real_t* const g0 ) +{ +g0[0] += 0.0; +; +g0[1] += 0.0; +; +g0[2] += 0.0; +; +g0[3] += 0.0; +; +} + +void acado_macETSlu( real_t* const E0, real_t* const g1 ) +{ +g1[0] += 0.0; +; +} + +void acado_condensePrep( ) +{ +acado_moveGuE( acadoWorkspace.evGu, acadoWorkspace.E ); +acado_moveGxT( &(acadoWorkspace.evGx[ 16 ]), acadoWorkspace.T ); +acado_multGxd( acadoWorkspace.d, &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.d[ 4 ]) ); +acado_multGxGx( acadoWorkspace.T, acadoWorkspace.evGx, &(acadoWorkspace.evGx[ 16 ]) ); + +acado_multGxGu( acadoWorkspace.T, acadoWorkspace.E, &(acadoWorkspace.E[ 4 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 4 ]), &(acadoWorkspace.E[ 8 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 32 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 4 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.d[ 8 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.evGx[ 32 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.E[ 12 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.E[ 16 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 8 ]), &(acadoWorkspace.E[ 20 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 48 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 8 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.d[ 12 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.evGx[ 48 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.E[ 24 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.E[ 28 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.E[ 32 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 12 ]), &(acadoWorkspace.E[ 36 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 64 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.d[ 16 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.evGx[ 64 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.E[ 40 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.E[ 44 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.E[ 48 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.E[ 52 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 16 ]), &(acadoWorkspace.E[ 56 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 80 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 16 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.d[ 20 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.evGx[ 80 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.E[ 60 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.E[ 64 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.E[ 68 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.E[ 72 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.E[ 76 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 20 ]), &(acadoWorkspace.E[ 80 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 96 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 20 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.d[ 24 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.evGx[ 96 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.E[ 84 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.E[ 88 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.E[ 92 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.E[ 96 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.E[ 100 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.E[ 104 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 24 ]), &(acadoWorkspace.E[ 108 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 112 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.d[ 28 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.evGx[ 112 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.E[ 112 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.E[ 116 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.E[ 120 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.E[ 124 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.E[ 128 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.E[ 132 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.E[ 136 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 28 ]), &(acadoWorkspace.E[ 140 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 128 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 28 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.d[ 32 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.evGx[ 128 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.E[ 144 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.E[ 148 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.E[ 152 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.E[ 156 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.E[ 160 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.E[ 164 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.E[ 168 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.E[ 172 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 32 ]), &(acadoWorkspace.E[ 176 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 32 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.d[ 36 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.evGx[ 144 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.E[ 180 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.E[ 184 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.E[ 188 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.E[ 192 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.E[ 196 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.E[ 200 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.E[ 204 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.E[ 208 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.E[ 212 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 36 ]), &(acadoWorkspace.E[ 216 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 160 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.d[ 40 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.evGx[ 160 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.E[ 220 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.E[ 224 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.E[ 228 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.E[ 232 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.E[ 236 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.E[ 240 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.E[ 244 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.E[ 248 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.E[ 252 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.E[ 256 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 40 ]), &(acadoWorkspace.E[ 260 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 176 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 40 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.d[ 44 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.evGx[ 176 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.E[ 264 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.E[ 268 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.E[ 272 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.E[ 276 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.E[ 280 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.E[ 284 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.E[ 288 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.E[ 292 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.E[ 296 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.E[ 300 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.E[ 304 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 44 ]), &(acadoWorkspace.E[ 308 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 192 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 44 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.d[ 48 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.evGx[ 192 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.E[ 312 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.E[ 316 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.E[ 320 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.E[ 324 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.E[ 328 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.E[ 332 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.E[ 336 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.E[ 340 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.E[ 344 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.E[ 348 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.E[ 352 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.E[ 356 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 48 ]), &(acadoWorkspace.E[ 360 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 208 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.d[ 52 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.evGx[ 208 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.E[ 364 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.E[ 368 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.E[ 372 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.E[ 376 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.E[ 380 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.E[ 384 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.E[ 388 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.E[ 392 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.E[ 396 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.E[ 400 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.E[ 404 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.E[ 408 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.E[ 412 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 52 ]), &(acadoWorkspace.E[ 416 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 224 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 52 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.d[ 56 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.evGx[ 224 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.E[ 420 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.E[ 424 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.E[ 428 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.E[ 432 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.E[ 436 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.E[ 440 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.E[ 444 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.E[ 448 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.E[ 452 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.E[ 456 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.E[ 460 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.E[ 464 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.E[ 468 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.E[ 472 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 56 ]), &(acadoWorkspace.E[ 476 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 240 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 56 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.d[ 60 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.evGx[ 240 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.E[ 480 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.E[ 484 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.E[ 488 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.E[ 492 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.E[ 496 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.E[ 500 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.E[ 504 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.E[ 508 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.E[ 512 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.E[ 516 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.E[ 520 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.E[ 524 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.E[ 528 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.E[ 532 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.E[ 536 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 60 ]), &(acadoWorkspace.E[ 540 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 256 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 60 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.d[ 64 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.evGx[ 256 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.E[ 544 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.E[ 548 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.E[ 552 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.E[ 556 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.E[ 560 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.E[ 564 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.E[ 568 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.E[ 572 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.E[ 576 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.E[ 580 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.E[ 584 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.E[ 588 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.E[ 592 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.E[ 596 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.E[ 600 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.E[ 604 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 64 ]), &(acadoWorkspace.E[ 608 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 272 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 64 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.d[ 68 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.evGx[ 272 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.E[ 612 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.E[ 616 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.E[ 620 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.E[ 624 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.E[ 628 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.E[ 632 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.E[ 636 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.E[ 640 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.E[ 644 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.E[ 648 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.E[ 652 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.E[ 656 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.E[ 660 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.E[ 664 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.E[ 668 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 604 ]), &(acadoWorkspace.E[ 672 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 608 ]), &(acadoWorkspace.E[ 676 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 68 ]), &(acadoWorkspace.E[ 680 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 288 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 68 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.d[ 72 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.evGx[ 288 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.E[ 684 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.E[ 688 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.E[ 692 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.E[ 696 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.E[ 700 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.E[ 704 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.E[ 708 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.E[ 712 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.E[ 716 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.E[ 720 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.E[ 724 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.E[ 728 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.E[ 732 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.E[ 736 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.E[ 740 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.E[ 744 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 676 ]), &(acadoWorkspace.E[ 748 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 680 ]), &(acadoWorkspace.E[ 752 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 72 ]), &(acadoWorkspace.E[ 756 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 304 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 72 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.d[ 76 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.evGx[ 304 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.E[ 760 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.E[ 764 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.E[ 768 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.E[ 772 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.E[ 776 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.E[ 780 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.E[ 784 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.E[ 788 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.E[ 792 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.E[ 796 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.E[ 800 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.E[ 804 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.E[ 808 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.E[ 812 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.E[ 816 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.E[ 820 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.E[ 824 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 752 ]), &(acadoWorkspace.E[ 828 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.E[ 832 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 76 ]), &(acadoWorkspace.E[ 836 ]) ); + +acado_multGxGx( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.evGx, acadoWorkspace.QGx ); +acado_multGxGx( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.QGx[ 16 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.QGx[ 32 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.QGx[ 48 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.QGx[ 64 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.QGx[ 80 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.QGx[ 96 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.QGx[ 112 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.QGx[ 128 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.QGx[ 144 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.QGx[ 160 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.QGx[ 176 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.QGx[ 192 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.QGx[ 208 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.QGx[ 224 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.QGx[ 240 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.QGx[ 256 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.QGx[ 272 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.QGx[ 288 ]) ); +acado_multGxGx( acadoWorkspace.QN1, &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.QGx[ 304 ]) ); + +acado_multGxGu( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.E, acadoWorkspace.QE ); +acado_multGxGu( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.QE[ 4 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.QE[ 8 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 12 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.QE[ 16 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.QE[ 20 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 24 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QE[ 28 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.QE[ 32 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 36 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 40 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 44 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.QE[ 52 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.QE[ 56 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 60 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 64 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 68 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.QE[ 76 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.QE[ 80 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 84 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 88 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 92 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QE[ 100 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 108 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 112 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 116 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 124 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 128 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.QE[ 140 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 144 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 148 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 152 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 160 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.QE[ 176 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 180 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 184 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 188 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 196 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 220 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 224 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 232 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 236 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.QE[ 260 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 268 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 272 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 276 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 280 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 284 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 292 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 296 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.QE[ 304 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.QE[ 308 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 316 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 320 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 328 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 332 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 340 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 344 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QE[ 352 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.QE[ 356 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 364 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 368 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 376 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 380 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 388 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 392 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 400 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 404 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.QE[ 412 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.QE[ 416 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 420 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 424 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 428 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 436 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 440 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 448 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 452 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 460 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 464 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.QE[ 472 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.QE[ 476 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 484 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 488 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 496 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 500 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 508 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 512 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 520 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 524 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QE[ 532 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.QE[ 536 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 544 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 548 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 556 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 560 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 568 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 572 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 576 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 580 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 584 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QE[ 592 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.QE[ 596 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 604 ]), &(acadoWorkspace.QE[ 604 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 608 ]), &(acadoWorkspace.QE[ 608 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 612 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 616 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 620 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 628 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 632 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 636 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 640 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 644 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 648 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 652 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 656 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QE[ 664 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.QE[ 668 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 676 ]), &(acadoWorkspace.QE[ 676 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 680 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 684 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 688 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 692 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 696 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 700 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 704 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 712 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 716 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 720 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 724 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 728 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 732 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 736 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QE[ 740 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.QE[ 748 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 752 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 760 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 764 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 768 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 772 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 776 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 780 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 784 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 788 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 792 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 796 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 800 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 804 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 808 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 812 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 816 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QE[ 820 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.QE[ 824 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 828 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 832 ]), &(acadoWorkspace.QE[ 832 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 836 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_zeroBlockH00( ); +acado_multCTQC( acadoWorkspace.evGx, acadoWorkspace.QGx ); +acado_multCTQC( &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.QGx[ 16 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.QGx[ 32 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.QGx[ 48 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.QGx[ 64 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.QGx[ 80 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.QGx[ 96 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.QGx[ 112 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.QGx[ 128 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.QGx[ 144 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.QGx[ 160 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.QGx[ 176 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.QGx[ 192 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.QGx[ 208 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.QGx[ 224 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.QGx[ 240 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.QGx[ 256 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.QGx[ 272 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.QGx[ 288 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.QGx[ 304 ]) ); + +acado_zeroBlockH10( acadoWorkspace.H10 ); +acado_multQETGx( acadoWorkspace.QE, acadoWorkspace.evGx, acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 4 ]), &(acadoWorkspace.evGx[ 16 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.evGx[ 32 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 24 ]), &(acadoWorkspace.evGx[ 48 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 40 ]), &(acadoWorkspace.evGx[ 64 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 60 ]), &(acadoWorkspace.evGx[ 80 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 84 ]), &(acadoWorkspace.evGx[ 96 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 112 ]), &(acadoWorkspace.evGx[ 112 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 144 ]), &(acadoWorkspace.evGx[ 128 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 180 ]), &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 220 ]), &(acadoWorkspace.evGx[ 160 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 264 ]), &(acadoWorkspace.evGx[ 176 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.evGx[ 192 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 364 ]), &(acadoWorkspace.evGx[ 208 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 420 ]), &(acadoWorkspace.evGx[ 224 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 480 ]), &(acadoWorkspace.evGx[ 240 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 544 ]), &(acadoWorkspace.evGx[ 256 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 612 ]), &(acadoWorkspace.evGx[ 272 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 684 ]), &(acadoWorkspace.evGx[ 288 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 760 ]), &(acadoWorkspace.evGx[ 304 ]), acadoWorkspace.H10 ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 8 ]), &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 16 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 28 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 44 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 64 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 88 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 116 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 148 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 184 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 224 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 268 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 316 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 368 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 424 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 484 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 548 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 616 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 688 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 764 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 20 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 32 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 48 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 68 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 92 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 120 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 152 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 188 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 228 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 272 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 320 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 372 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 428 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 488 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 552 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 620 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 692 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 768 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 36 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 52 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 72 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 96 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 124 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 156 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 192 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 232 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 276 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 376 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 432 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 492 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 556 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 624 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 696 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 772 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 56 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 76 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 100 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 128 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 160 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 196 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 236 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 280 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 328 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 380 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 436 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 496 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 560 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 628 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 700 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 776 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 80 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 104 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 132 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 164 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 200 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 240 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 284 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 332 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 384 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 440 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 500 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 564 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 632 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 704 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 780 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 108 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 136 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 168 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 204 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 244 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 288 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 388 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 444 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 504 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 568 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 636 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 708 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 784 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 140 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 172 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 208 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 248 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 292 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 340 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 392 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 448 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 508 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 572 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 640 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 712 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 788 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 32 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 176 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 32 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 212 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 32 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 252 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 32 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 296 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 32 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 344 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 32 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 396 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 32 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 452 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 32 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 512 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 32 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 576 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 32 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 644 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 32 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 716 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 32 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 792 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 32 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 216 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 256 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 300 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 400 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 456 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 516 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 580 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 648 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 720 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 796 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 40 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 260 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 40 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 304 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 40 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 352 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 40 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 404 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 40 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 460 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 40 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 520 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 40 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 584 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 40 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 652 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 40 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 724 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 40 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 800 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 40 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 44 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 308 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 44 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 356 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 44 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 408 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 44 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 464 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 44 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 524 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 44 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 588 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 44 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 656 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 44 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 728 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 44 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 804 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 44 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 360 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 412 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 468 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 528 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 592 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 660 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 732 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 808 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 52 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 416 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 52 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 472 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 52 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 532 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 52 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 596 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 52 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 664 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 52 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 736 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 52 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 812 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 52 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 56 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 476 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 56 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 536 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 56 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 600 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 56 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 668 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 56 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 740 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 56 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 816 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 56 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 60 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 540 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 60 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 604 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 60 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 672 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 60 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 744 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 60 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 820 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 60 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 64 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 608 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 64 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 676 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 64 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 748 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 64 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 824 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 64 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 68 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 680 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 68 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 752 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 68 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 828 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 68 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 72 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 756 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 72 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 832 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 72 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 76 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 836 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 76 ]) ); + +acadoWorkspace.H[4] = acadoWorkspace.H10[0]; +acadoWorkspace.H[5] = acadoWorkspace.H10[4]; +acadoWorkspace.H[6] = acadoWorkspace.H10[8]; +acadoWorkspace.H[7] = acadoWorkspace.H10[12]; +acadoWorkspace.H[8] = acadoWorkspace.H10[16]; +acadoWorkspace.H[9] = acadoWorkspace.H10[20]; +acadoWorkspace.H[10] = acadoWorkspace.H10[24]; +acadoWorkspace.H[11] = acadoWorkspace.H10[28]; +acadoWorkspace.H[12] = acadoWorkspace.H10[32]; +acadoWorkspace.H[13] = acadoWorkspace.H10[36]; +acadoWorkspace.H[14] = acadoWorkspace.H10[40]; +acadoWorkspace.H[15] = acadoWorkspace.H10[44]; +acadoWorkspace.H[16] = acadoWorkspace.H10[48]; +acadoWorkspace.H[17] = acadoWorkspace.H10[52]; +acadoWorkspace.H[18] = acadoWorkspace.H10[56]; +acadoWorkspace.H[19] = acadoWorkspace.H10[60]; +acadoWorkspace.H[20] = acadoWorkspace.H10[64]; +acadoWorkspace.H[21] = acadoWorkspace.H10[68]; +acadoWorkspace.H[22] = acadoWorkspace.H10[72]; +acadoWorkspace.H[23] = acadoWorkspace.H10[76]; +acadoWorkspace.H[28] = acadoWorkspace.H10[1]; +acadoWorkspace.H[29] = acadoWorkspace.H10[5]; +acadoWorkspace.H[30] = acadoWorkspace.H10[9]; +acadoWorkspace.H[31] = acadoWorkspace.H10[13]; +acadoWorkspace.H[32] = acadoWorkspace.H10[17]; +acadoWorkspace.H[33] = acadoWorkspace.H10[21]; +acadoWorkspace.H[34] = acadoWorkspace.H10[25]; +acadoWorkspace.H[35] = acadoWorkspace.H10[29]; +acadoWorkspace.H[36] = acadoWorkspace.H10[33]; +acadoWorkspace.H[37] = acadoWorkspace.H10[37]; +acadoWorkspace.H[38] = acadoWorkspace.H10[41]; +acadoWorkspace.H[39] = acadoWorkspace.H10[45]; +acadoWorkspace.H[40] = acadoWorkspace.H10[49]; +acadoWorkspace.H[41] = acadoWorkspace.H10[53]; +acadoWorkspace.H[42] = acadoWorkspace.H10[57]; +acadoWorkspace.H[43] = acadoWorkspace.H10[61]; +acadoWorkspace.H[44] = acadoWorkspace.H10[65]; +acadoWorkspace.H[45] = acadoWorkspace.H10[69]; +acadoWorkspace.H[46] = acadoWorkspace.H10[73]; +acadoWorkspace.H[47] = acadoWorkspace.H10[77]; +acadoWorkspace.H[52] = acadoWorkspace.H10[2]; +acadoWorkspace.H[53] = acadoWorkspace.H10[6]; +acadoWorkspace.H[54] = acadoWorkspace.H10[10]; +acadoWorkspace.H[55] = acadoWorkspace.H10[14]; +acadoWorkspace.H[56] = acadoWorkspace.H10[18]; +acadoWorkspace.H[57] = acadoWorkspace.H10[22]; +acadoWorkspace.H[58] = acadoWorkspace.H10[26]; +acadoWorkspace.H[59] = acadoWorkspace.H10[30]; +acadoWorkspace.H[60] = acadoWorkspace.H10[34]; +acadoWorkspace.H[61] = acadoWorkspace.H10[38]; +acadoWorkspace.H[62] = acadoWorkspace.H10[42]; +acadoWorkspace.H[63] = acadoWorkspace.H10[46]; +acadoWorkspace.H[64] = acadoWorkspace.H10[50]; +acadoWorkspace.H[65] = acadoWorkspace.H10[54]; +acadoWorkspace.H[66] = acadoWorkspace.H10[58]; +acadoWorkspace.H[67] = acadoWorkspace.H10[62]; +acadoWorkspace.H[68] = acadoWorkspace.H10[66]; +acadoWorkspace.H[69] = acadoWorkspace.H10[70]; +acadoWorkspace.H[70] = acadoWorkspace.H10[74]; +acadoWorkspace.H[71] = acadoWorkspace.H10[78]; +acadoWorkspace.H[76] = acadoWorkspace.H10[3]; +acadoWorkspace.H[77] = acadoWorkspace.H10[7]; +acadoWorkspace.H[78] = acadoWorkspace.H10[11]; +acadoWorkspace.H[79] = acadoWorkspace.H10[15]; +acadoWorkspace.H[80] = acadoWorkspace.H10[19]; +acadoWorkspace.H[81] = acadoWorkspace.H10[23]; +acadoWorkspace.H[82] = acadoWorkspace.H10[27]; +acadoWorkspace.H[83] = acadoWorkspace.H10[31]; +acadoWorkspace.H[84] = acadoWorkspace.H10[35]; +acadoWorkspace.H[85] = acadoWorkspace.H10[39]; +acadoWorkspace.H[86] = acadoWorkspace.H10[43]; +acadoWorkspace.H[87] = acadoWorkspace.H10[47]; +acadoWorkspace.H[88] = acadoWorkspace.H10[51]; +acadoWorkspace.H[89] = acadoWorkspace.H10[55]; +acadoWorkspace.H[90] = acadoWorkspace.H10[59]; +acadoWorkspace.H[91] = acadoWorkspace.H10[63]; +acadoWorkspace.H[92] = acadoWorkspace.H10[67]; +acadoWorkspace.H[93] = acadoWorkspace.H10[71]; +acadoWorkspace.H[94] = acadoWorkspace.H10[75]; +acadoWorkspace.H[95] = acadoWorkspace.H10[79]; + +acado_setBlockH11_R1( 0, 0, acadoWorkspace.R1 ); +acado_setBlockH11( 0, 0, acadoWorkspace.E, acadoWorkspace.QE ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.QE[ 4 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 12 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 24 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 40 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 60 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 84 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 112 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 144 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 180 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 220 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 364 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 420 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 544 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 612 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 684 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 760 ]) ); + +acado_zeroBlockH11( 0, 1 ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.QE[ 8 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 16 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 28 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 44 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 64 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 88 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 116 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 148 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 184 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 224 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 268 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 316 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 368 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 424 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 484 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 548 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 616 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 688 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 764 ]) ); + +acado_zeroBlockH11( 0, 2 ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 20 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 32 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 68 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 92 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 152 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 188 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 272 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 320 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 428 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 488 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 620 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 692 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 768 ]) ); + +acado_zeroBlockH11( 0, 3 ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 36 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 52 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 124 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 232 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 276 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 376 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 556 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 696 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 772 ]) ); + +acado_zeroBlockH11( 0, 4 ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 56 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 76 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 100 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 128 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 160 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 196 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 236 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 280 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 328 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 380 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 436 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 496 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 560 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 628 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 700 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 776 ]) ); + +acado_zeroBlockH11( 0, 5 ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 80 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 284 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 332 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 440 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 500 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 632 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 704 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 780 ]) ); + +acado_zeroBlockH11( 0, 6 ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 108 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 388 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 568 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 636 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 784 ]) ); + +acado_zeroBlockH11( 0, 7 ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 140 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 292 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 340 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 392 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 448 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 508 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 572 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 640 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 712 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 788 ]) ); + +acado_zeroBlockH11( 0, 8 ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 176 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 296 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 344 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 452 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 512 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 576 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 644 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 716 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 792 ]) ); + +acado_zeroBlockH11( 0, 9 ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 400 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 580 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 648 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 720 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 796 ]) ); + +acado_zeroBlockH11( 0, 10 ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 260 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 304 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 352 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 404 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 460 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 520 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 584 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 652 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 724 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 800 ]) ); + +acado_zeroBlockH11( 0, 11 ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 308 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 356 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 464 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 524 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 656 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 728 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 804 ]) ); + +acado_zeroBlockH11( 0, 12 ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 412 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 592 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 732 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 808 ]) ); + +acado_zeroBlockH11( 0, 13 ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 416 ]) ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 472 ]) ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 532 ]) ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 596 ]) ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 664 ]) ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 736 ]) ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 812 ]) ); + +acado_zeroBlockH11( 0, 14 ); +acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 476 ]) ); +acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 536 ]) ); +acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 668 ]) ); +acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 740 ]) ); +acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 816 ]) ); + +acado_zeroBlockH11( 0, 15 ); +acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 604 ]) ); +acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 820 ]) ); + +acado_zeroBlockH11( 0, 16 ); +acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 608 ]) ); +acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 676 ]) ); +acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 748 ]) ); +acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 824 ]) ); + +acado_zeroBlockH11( 0, 17 ); +acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 828 ]) ); + +acado_zeroBlockH11( 0, 18 ); +acado_setBlockH11( 0, 18, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 0, 18, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 0, 19 ); +acado_setBlockH11( 0, 19, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 1, 1, &(acadoWorkspace.R1[ 1 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.QE[ 8 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.QE[ 16 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QE[ 28 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 44 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 64 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 88 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 116 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 148 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 184 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 224 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 268 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 316 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 368 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 424 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 484 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 548 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 616 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 688 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 764 ]) ); + +acado_zeroBlockH11( 1, 2 ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.QE[ 20 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QE[ 32 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 68 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 92 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 152 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 188 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 272 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 320 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 428 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 488 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 620 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 692 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 768 ]) ); + +acado_zeroBlockH11( 1, 3 ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QE[ 36 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 52 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 124 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 232 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 276 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 376 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 556 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 696 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 772 ]) ); + +acado_zeroBlockH11( 1, 4 ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 56 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 76 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 100 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 128 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 160 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 196 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 236 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 280 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 328 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 380 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 436 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 496 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 560 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 628 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 700 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 776 ]) ); + +acado_zeroBlockH11( 1, 5 ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 80 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 284 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 332 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 440 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 500 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 632 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 704 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 780 ]) ); + +acado_zeroBlockH11( 1, 6 ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 108 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 388 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 568 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 636 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 784 ]) ); + +acado_zeroBlockH11( 1, 7 ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 140 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 292 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 340 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 392 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 448 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 508 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 572 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 640 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 712 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 788 ]) ); + +acado_zeroBlockH11( 1, 8 ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 176 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 296 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 344 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 452 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 512 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 576 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 644 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 716 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 792 ]) ); + +acado_zeroBlockH11( 1, 9 ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 400 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 580 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 648 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 720 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 796 ]) ); + +acado_zeroBlockH11( 1, 10 ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 260 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 304 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 352 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 404 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 460 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 520 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 584 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 652 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 724 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 800 ]) ); + +acado_zeroBlockH11( 1, 11 ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 308 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 356 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 464 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 524 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 656 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 728 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 804 ]) ); + +acado_zeroBlockH11( 1, 12 ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 412 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 592 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 732 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 808 ]) ); + +acado_zeroBlockH11( 1, 13 ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 416 ]) ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 472 ]) ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 532 ]) ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 596 ]) ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 664 ]) ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 736 ]) ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 812 ]) ); + +acado_zeroBlockH11( 1, 14 ); +acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 476 ]) ); +acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 536 ]) ); +acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 668 ]) ); +acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 740 ]) ); +acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 816 ]) ); + +acado_zeroBlockH11( 1, 15 ); +acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 604 ]) ); +acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 820 ]) ); + +acado_zeroBlockH11( 1, 16 ); +acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 608 ]) ); +acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 676 ]) ); +acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 748 ]) ); +acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 824 ]) ); + +acado_zeroBlockH11( 1, 17 ); +acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 828 ]) ); + +acado_zeroBlockH11( 1, 18 ); +acado_setBlockH11( 1, 18, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 1, 18, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 1, 19 ); +acado_setBlockH11( 1, 19, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 2, 2, &(acadoWorkspace.R1[ 2 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.QE[ 20 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.QE[ 32 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 68 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 92 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 152 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 188 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 272 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 320 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 428 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 488 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 620 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 692 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 768 ]) ); + +acado_zeroBlockH11( 2, 3 ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.QE[ 36 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 52 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 124 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 232 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 276 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 376 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 556 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 696 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 772 ]) ); + +acado_zeroBlockH11( 2, 4 ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 56 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 76 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 100 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 128 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 160 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 196 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 236 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 280 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 328 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 380 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 436 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 496 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 560 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 628 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 700 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 776 ]) ); + +acado_zeroBlockH11( 2, 5 ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 80 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 284 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 332 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 440 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 500 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 632 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 704 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 780 ]) ); + +acado_zeroBlockH11( 2, 6 ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 108 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 388 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 568 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 636 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 784 ]) ); + +acado_zeroBlockH11( 2, 7 ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 140 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 292 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 340 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 392 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 448 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 508 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 572 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 640 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 712 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 788 ]) ); + +acado_zeroBlockH11( 2, 8 ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 176 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 296 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 344 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 452 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 512 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 576 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 644 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 716 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 792 ]) ); + +acado_zeroBlockH11( 2, 9 ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 400 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 580 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 648 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 720 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 796 ]) ); + +acado_zeroBlockH11( 2, 10 ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 260 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 304 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 352 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 404 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 460 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 520 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 584 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 652 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 724 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 800 ]) ); + +acado_zeroBlockH11( 2, 11 ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 308 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 356 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 464 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 524 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 656 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 728 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 804 ]) ); + +acado_zeroBlockH11( 2, 12 ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 412 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 592 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 732 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 808 ]) ); + +acado_zeroBlockH11( 2, 13 ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 416 ]) ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 472 ]) ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 532 ]) ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 596 ]) ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 664 ]) ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 736 ]) ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 812 ]) ); + +acado_zeroBlockH11( 2, 14 ); +acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 476 ]) ); +acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 536 ]) ); +acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 668 ]) ); +acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 740 ]) ); +acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 816 ]) ); + +acado_zeroBlockH11( 2, 15 ); +acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 604 ]) ); +acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 820 ]) ); + +acado_zeroBlockH11( 2, 16 ); +acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 608 ]) ); +acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 676 ]) ); +acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 748 ]) ); +acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 824 ]) ); + +acado_zeroBlockH11( 2, 17 ); +acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 828 ]) ); + +acado_zeroBlockH11( 2, 18 ); +acado_setBlockH11( 2, 18, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 2, 18, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 2, 19 ); +acado_setBlockH11( 2, 19, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 3, 3, &(acadoWorkspace.R1[ 3 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 36 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.QE[ 52 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 124 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 232 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 276 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 376 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 556 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 696 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 772 ]) ); + +acado_zeroBlockH11( 3, 4 ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.QE[ 56 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 76 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 100 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 128 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 160 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 196 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 236 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 280 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 328 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 380 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 436 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 496 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 560 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 628 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 700 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 776 ]) ); + +acado_zeroBlockH11( 3, 5 ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 80 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 284 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 332 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 440 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 500 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 632 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 704 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 780 ]) ); + +acado_zeroBlockH11( 3, 6 ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 108 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 388 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 568 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 636 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 784 ]) ); + +acado_zeroBlockH11( 3, 7 ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 140 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 292 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 340 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 392 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 448 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 508 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 572 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 640 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 712 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 788 ]) ); + +acado_zeroBlockH11( 3, 8 ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 176 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 296 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 344 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 452 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 512 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 576 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 644 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 716 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 792 ]) ); + +acado_zeroBlockH11( 3, 9 ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 400 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 580 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 648 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 720 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 796 ]) ); + +acado_zeroBlockH11( 3, 10 ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 260 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 304 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 352 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 404 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 460 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 520 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 584 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 652 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 724 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 800 ]) ); + +acado_zeroBlockH11( 3, 11 ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 308 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 356 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 464 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 524 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 656 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 728 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 804 ]) ); + +acado_zeroBlockH11( 3, 12 ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 412 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 592 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 732 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 808 ]) ); + +acado_zeroBlockH11( 3, 13 ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 416 ]) ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 472 ]) ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 532 ]) ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 596 ]) ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 664 ]) ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 736 ]) ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 812 ]) ); + +acado_zeroBlockH11( 3, 14 ); +acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 476 ]) ); +acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 536 ]) ); +acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 668 ]) ); +acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 740 ]) ); +acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 816 ]) ); + +acado_zeroBlockH11( 3, 15 ); +acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 604 ]) ); +acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 820 ]) ); + +acado_zeroBlockH11( 3, 16 ); +acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 608 ]) ); +acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 676 ]) ); +acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 748 ]) ); +acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 824 ]) ); + +acado_zeroBlockH11( 3, 17 ); +acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 828 ]) ); + +acado_zeroBlockH11( 3, 18 ); +acado_setBlockH11( 3, 18, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 3, 18, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 3, 19 ); +acado_setBlockH11( 3, 19, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 4, 4, &(acadoWorkspace.R1[ 4 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.QE[ 56 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.QE[ 76 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QE[ 100 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 128 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 160 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 196 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 236 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 280 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 328 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 380 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 436 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 496 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 560 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 628 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 700 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 776 ]) ); + +acado_zeroBlockH11( 4, 5 ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.QE[ 80 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 284 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 332 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 440 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 500 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 632 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 704 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 780 ]) ); + +acado_zeroBlockH11( 4, 6 ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QE[ 108 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 388 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 568 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 636 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 784 ]) ); + +acado_zeroBlockH11( 4, 7 ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 140 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 292 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 340 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 392 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 448 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 508 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 572 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 640 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 712 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 788 ]) ); + +acado_zeroBlockH11( 4, 8 ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 176 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 296 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 344 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 452 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 512 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 576 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 644 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 716 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 792 ]) ); + +acado_zeroBlockH11( 4, 9 ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 400 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 580 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 648 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 720 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 796 ]) ); + +acado_zeroBlockH11( 4, 10 ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 260 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 304 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 352 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 404 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 460 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 520 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 584 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 652 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 724 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 800 ]) ); + +acado_zeroBlockH11( 4, 11 ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 308 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 356 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 464 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 524 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 656 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 728 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 804 ]) ); + +acado_zeroBlockH11( 4, 12 ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 412 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 592 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 732 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 808 ]) ); + +acado_zeroBlockH11( 4, 13 ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 416 ]) ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 472 ]) ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 532 ]) ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 596 ]) ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 664 ]) ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 736 ]) ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 812 ]) ); + +acado_zeroBlockH11( 4, 14 ); +acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 476 ]) ); +acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 536 ]) ); +acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 668 ]) ); +acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 740 ]) ); +acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 816 ]) ); + +acado_zeroBlockH11( 4, 15 ); +acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 604 ]) ); +acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 820 ]) ); + +acado_zeroBlockH11( 4, 16 ); +acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 608 ]) ); +acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 676 ]) ); +acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 748 ]) ); +acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 824 ]) ); + +acado_zeroBlockH11( 4, 17 ); +acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 828 ]) ); + +acado_zeroBlockH11( 4, 18 ); +acado_setBlockH11( 4, 18, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 4, 18, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 4, 19 ); +acado_setBlockH11( 4, 19, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 5, 5, &(acadoWorkspace.R1[ 5 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.QE[ 80 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 284 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 332 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 440 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 500 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 632 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 704 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 780 ]) ); + +acado_zeroBlockH11( 5, 6 ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.QE[ 108 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 388 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 568 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 636 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 784 ]) ); + +acado_zeroBlockH11( 5, 7 ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 140 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 292 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 340 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 392 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 448 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 508 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 572 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 640 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 712 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 788 ]) ); + +acado_zeroBlockH11( 5, 8 ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 176 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 296 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 344 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 452 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 512 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 576 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 644 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 716 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 792 ]) ); + +acado_zeroBlockH11( 5, 9 ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 400 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 580 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 648 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 720 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 796 ]) ); + +acado_zeroBlockH11( 5, 10 ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 260 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 304 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 352 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 404 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 460 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 520 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 584 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 652 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 724 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 800 ]) ); + +acado_zeroBlockH11( 5, 11 ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 308 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 356 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 464 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 524 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 656 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 728 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 804 ]) ); + +acado_zeroBlockH11( 5, 12 ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 412 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 592 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 732 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 808 ]) ); + +acado_zeroBlockH11( 5, 13 ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 416 ]) ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 472 ]) ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 532 ]) ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 596 ]) ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 664 ]) ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 736 ]) ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 812 ]) ); + +acado_zeroBlockH11( 5, 14 ); +acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 476 ]) ); +acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 536 ]) ); +acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 668 ]) ); +acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 740 ]) ); +acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 816 ]) ); + +acado_zeroBlockH11( 5, 15 ); +acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 604 ]) ); +acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 820 ]) ); + +acado_zeroBlockH11( 5, 16 ); +acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 608 ]) ); +acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 676 ]) ); +acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 748 ]) ); +acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 824 ]) ); + +acado_zeroBlockH11( 5, 17 ); +acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 828 ]) ); + +acado_zeroBlockH11( 5, 18 ); +acado_setBlockH11( 5, 18, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 5, 18, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 5, 19 ); +acado_setBlockH11( 5, 19, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 6, 6, &(acadoWorkspace.R1[ 6 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 108 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 388 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 568 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 636 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 784 ]) ); + +acado_zeroBlockH11( 6, 7 ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.QE[ 140 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 292 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 340 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 392 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 448 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 508 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 572 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 640 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 712 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 788 ]) ); + +acado_zeroBlockH11( 6, 8 ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 176 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 296 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 344 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 452 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 512 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 576 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 644 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 716 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 792 ]) ); + +acado_zeroBlockH11( 6, 9 ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 400 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 580 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 648 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 720 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 796 ]) ); + +acado_zeroBlockH11( 6, 10 ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 260 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 304 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 352 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 404 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 460 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 520 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 584 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 652 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 724 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 800 ]) ); + +acado_zeroBlockH11( 6, 11 ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 308 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 356 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 464 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 524 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 656 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 728 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 804 ]) ); + +acado_zeroBlockH11( 6, 12 ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 412 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 592 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 732 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 808 ]) ); + +acado_zeroBlockH11( 6, 13 ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 416 ]) ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 472 ]) ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 532 ]) ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 596 ]) ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 664 ]) ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 736 ]) ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 812 ]) ); + +acado_zeroBlockH11( 6, 14 ); +acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 476 ]) ); +acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 536 ]) ); +acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 668 ]) ); +acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 740 ]) ); +acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 816 ]) ); + +acado_zeroBlockH11( 6, 15 ); +acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 604 ]) ); +acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 820 ]) ); + +acado_zeroBlockH11( 6, 16 ); +acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 608 ]) ); +acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 676 ]) ); +acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 748 ]) ); +acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 824 ]) ); + +acado_zeroBlockH11( 6, 17 ); +acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 828 ]) ); + +acado_zeroBlockH11( 6, 18 ); +acado_setBlockH11( 6, 18, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 6, 18, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 6, 19 ); +acado_setBlockH11( 6, 19, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 7, 7, &(acadoWorkspace.R1[ 7 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.QE[ 140 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 292 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 340 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 392 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 448 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 508 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 572 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 640 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 712 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 788 ]) ); + +acado_zeroBlockH11( 7, 8 ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.QE[ 176 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 296 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 344 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 452 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 512 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 576 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 644 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 716 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 792 ]) ); + +acado_zeroBlockH11( 7, 9 ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 400 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 580 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 648 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 720 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 796 ]) ); + +acado_zeroBlockH11( 7, 10 ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 260 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 304 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 352 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 404 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 460 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 520 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 584 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 652 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 724 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 800 ]) ); + +acado_zeroBlockH11( 7, 11 ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 308 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 356 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 464 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 524 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 656 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 728 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 804 ]) ); + +acado_zeroBlockH11( 7, 12 ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 412 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 592 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 732 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 808 ]) ); + +acado_zeroBlockH11( 7, 13 ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 416 ]) ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 472 ]) ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 532 ]) ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 596 ]) ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 664 ]) ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 736 ]) ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 812 ]) ); + +acado_zeroBlockH11( 7, 14 ); +acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 476 ]) ); +acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 536 ]) ); +acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 668 ]) ); +acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 740 ]) ); +acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 816 ]) ); + +acado_zeroBlockH11( 7, 15 ); +acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 604 ]) ); +acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 820 ]) ); + +acado_zeroBlockH11( 7, 16 ); +acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 608 ]) ); +acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 676 ]) ); +acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 748 ]) ); +acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 824 ]) ); + +acado_zeroBlockH11( 7, 17 ); +acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 828 ]) ); + +acado_zeroBlockH11( 7, 18 ); +acado_setBlockH11( 7, 18, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 7, 18, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 7, 19 ); +acado_setBlockH11( 7, 19, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 8, 8, &(acadoWorkspace.R1[ 8 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.QE[ 176 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 296 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 344 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 452 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 512 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 576 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 644 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 716 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 792 ]) ); + +acado_zeroBlockH11( 8, 9 ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 400 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 580 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 648 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 720 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 796 ]) ); + +acado_zeroBlockH11( 8, 10 ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 260 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 304 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 352 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 404 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 460 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 520 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 584 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 652 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 724 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 800 ]) ); + +acado_zeroBlockH11( 8, 11 ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 308 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 356 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 464 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 524 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 656 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 728 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 804 ]) ); + +acado_zeroBlockH11( 8, 12 ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 412 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 592 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 732 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 808 ]) ); + +acado_zeroBlockH11( 8, 13 ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 416 ]) ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 472 ]) ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 532 ]) ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 596 ]) ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 664 ]) ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 736 ]) ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 812 ]) ); + +acado_zeroBlockH11( 8, 14 ); +acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 476 ]) ); +acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 536 ]) ); +acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 668 ]) ); +acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 740 ]) ); +acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 816 ]) ); + +acado_zeroBlockH11( 8, 15 ); +acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 604 ]) ); +acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 820 ]) ); + +acado_zeroBlockH11( 8, 16 ); +acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 608 ]) ); +acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 676 ]) ); +acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 748 ]) ); +acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 824 ]) ); + +acado_zeroBlockH11( 8, 17 ); +acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 828 ]) ); + +acado_zeroBlockH11( 8, 18 ); +acado_setBlockH11( 8, 18, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 8, 18, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 8, 19 ); +acado_setBlockH11( 8, 19, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 9, 9, &(acadoWorkspace.R1[ 9 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 400 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 580 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 648 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 720 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 796 ]) ); + +acado_zeroBlockH11( 9, 10 ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.QE[ 260 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 304 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 352 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 404 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 460 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 520 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 584 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 652 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 724 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 800 ]) ); + +acado_zeroBlockH11( 9, 11 ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 308 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 356 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 464 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 524 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 656 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 728 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 804 ]) ); + +acado_zeroBlockH11( 9, 12 ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 412 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 592 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 732 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 808 ]) ); + +acado_zeroBlockH11( 9, 13 ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 416 ]) ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 472 ]) ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 532 ]) ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 596 ]) ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 664 ]) ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 736 ]) ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 812 ]) ); + +acado_zeroBlockH11( 9, 14 ); +acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 476 ]) ); +acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 536 ]) ); +acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 668 ]) ); +acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 740 ]) ); +acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 816 ]) ); + +acado_zeroBlockH11( 9, 15 ); +acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 604 ]) ); +acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 820 ]) ); + +acado_zeroBlockH11( 9, 16 ); +acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 608 ]) ); +acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 676 ]) ); +acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 748 ]) ); +acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 824 ]) ); + +acado_zeroBlockH11( 9, 17 ); +acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 828 ]) ); + +acado_zeroBlockH11( 9, 18 ); +acado_setBlockH11( 9, 18, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 9, 18, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 9, 19 ); +acado_setBlockH11( 9, 19, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 10, 10, &(acadoWorkspace.R1[ 10 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.QE[ 260 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.QE[ 304 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QE[ 352 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 404 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 460 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 520 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 584 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 652 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 724 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 800 ]) ); + +acado_zeroBlockH11( 10, 11 ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.QE[ 308 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QE[ 356 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 464 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 524 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 656 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 728 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 804 ]) ); + +acado_zeroBlockH11( 10, 12 ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 412 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 592 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 732 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 808 ]) ); + +acado_zeroBlockH11( 10, 13 ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 416 ]) ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 472 ]) ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 532 ]) ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 596 ]) ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 664 ]) ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 736 ]) ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 812 ]) ); + +acado_zeroBlockH11( 10, 14 ); +acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 476 ]) ); +acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 536 ]) ); +acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 668 ]) ); +acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 740 ]) ); +acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 816 ]) ); + +acado_zeroBlockH11( 10, 15 ); +acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 604 ]) ); +acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 820 ]) ); + +acado_zeroBlockH11( 10, 16 ); +acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 608 ]) ); +acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 676 ]) ); +acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 748 ]) ); +acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 824 ]) ); + +acado_zeroBlockH11( 10, 17 ); +acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 828 ]) ); + +acado_zeroBlockH11( 10, 18 ); +acado_setBlockH11( 10, 18, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 10, 18, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 10, 19 ); +acado_setBlockH11( 10, 19, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 11, 11, &(acadoWorkspace.R1[ 11 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.QE[ 308 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.QE[ 356 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 464 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 524 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 656 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 728 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 804 ]) ); + +acado_zeroBlockH11( 11, 12 ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 412 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 592 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 732 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 808 ]) ); + +acado_zeroBlockH11( 11, 13 ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 416 ]) ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 472 ]) ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 532 ]) ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 596 ]) ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 664 ]) ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 736 ]) ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 812 ]) ); + +acado_zeroBlockH11( 11, 14 ); +acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 476 ]) ); +acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 536 ]) ); +acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 668 ]) ); +acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 740 ]) ); +acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 816 ]) ); + +acado_zeroBlockH11( 11, 15 ); +acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 604 ]) ); +acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 820 ]) ); + +acado_zeroBlockH11( 11, 16 ); +acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 608 ]) ); +acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 676 ]) ); +acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 748 ]) ); +acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 824 ]) ); + +acado_zeroBlockH11( 11, 17 ); +acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 828 ]) ); + +acado_zeroBlockH11( 11, 18 ); +acado_setBlockH11( 11, 18, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 11, 18, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 11, 19 ); +acado_setBlockH11( 11, 19, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 12, 12, &(acadoWorkspace.R1[ 12 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.QE[ 412 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QE[ 592 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 732 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 808 ]) ); + +acado_zeroBlockH11( 12, 13 ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.QE[ 416 ]) ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 472 ]) ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 532 ]) ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QE[ 596 ]) ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 664 ]) ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 736 ]) ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 812 ]) ); + +acado_zeroBlockH11( 12, 14 ); +acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 476 ]) ); +acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 536 ]) ); +acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 668 ]) ); +acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 740 ]) ); +acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 816 ]) ); + +acado_zeroBlockH11( 12, 15 ); +acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QE[ 604 ]) ); +acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 820 ]) ); + +acado_zeroBlockH11( 12, 16 ); +acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QE[ 608 ]) ); +acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 676 ]) ); +acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 748 ]) ); +acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 824 ]) ); + +acado_zeroBlockH11( 12, 17 ); +acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 828 ]) ); + +acado_zeroBlockH11( 12, 18 ); +acado_setBlockH11( 12, 18, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 12, 18, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 12, 19 ); +acado_setBlockH11( 12, 19, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 13, 13, &(acadoWorkspace.R1[ 13 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.QE[ 416 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.QE[ 472 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QE[ 532 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.QE[ 596 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QE[ 664 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 736 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 812 ]) ); + +acado_zeroBlockH11( 13, 14 ); +acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.QE[ 476 ]) ); +acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QE[ 536 ]) ); +acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QE[ 668 ]) ); +acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 740 ]) ); +acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 816 ]) ); + +acado_zeroBlockH11( 13, 15 ); +acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.QE[ 604 ]) ); +acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 820 ]) ); + +acado_zeroBlockH11( 13, 16 ); +acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.QE[ 608 ]) ); +acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QE[ 676 ]) ); +acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 748 ]) ); +acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 824 ]) ); + +acado_zeroBlockH11( 13, 17 ); +acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 828 ]) ); + +acado_zeroBlockH11( 13, 18 ); +acado_setBlockH11( 13, 18, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 13, 18, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 13, 19 ); +acado_setBlockH11( 13, 19, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 14, 14, &(acadoWorkspace.R1[ 14 ]) ); +acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.QE[ 476 ]) ); +acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.QE[ 536 ]) ); +acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.QE[ 668 ]) ); +acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QE[ 740 ]) ); +acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 816 ]) ); + +acado_zeroBlockH11( 14, 15 ); +acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 604 ]) ); +acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 820 ]) ); + +acado_zeroBlockH11( 14, 16 ); +acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 608 ]) ); +acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.QE[ 676 ]) ); +acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QE[ 748 ]) ); +acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 824 ]) ); + +acado_zeroBlockH11( 14, 17 ); +acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 828 ]) ); + +acado_zeroBlockH11( 14, 18 ); +acado_setBlockH11( 14, 18, &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 14, 18, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 14, 19 ); +acado_setBlockH11( 14, 19, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 15, 15, &(acadoWorkspace.R1[ 15 ]) ); +acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 604 ]), &(acadoWorkspace.QE[ 604 ]) ); +acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QE[ 820 ]) ); + +acado_zeroBlockH11( 15, 16 ); +acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 604 ]), &(acadoWorkspace.QE[ 608 ]) ); +acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 676 ]) ); +acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 748 ]) ); +acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QE[ 824 ]) ); + +acado_zeroBlockH11( 15, 17 ); +acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QE[ 828 ]) ); + +acado_zeroBlockH11( 15, 18 ); +acado_setBlockH11( 15, 18, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 15, 18, &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 15, 19 ); +acado_setBlockH11( 15, 19, &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 16, 16, &(acadoWorkspace.R1[ 16 ]) ); +acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 608 ]), &(acadoWorkspace.QE[ 608 ]) ); +acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 676 ]), &(acadoWorkspace.QE[ 676 ]) ); +acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.QE[ 748 ]) ); +acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.QE[ 824 ]) ); + +acado_zeroBlockH11( 16, 17 ); +acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 676 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.QE[ 828 ]) ); + +acado_zeroBlockH11( 16, 18 ); +acado_setBlockH11( 16, 18, &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 16, 18, &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 16, 19 ); +acado_setBlockH11( 16, 19, &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 17, 17, &(acadoWorkspace.R1[ 17 ]) ); +acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 680 ]), &(acadoWorkspace.QE[ 680 ]) ); +acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 752 ]), &(acadoWorkspace.QE[ 752 ]) ); +acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 828 ]) ); + +acado_zeroBlockH11( 17, 18 ); +acado_setBlockH11( 17, 18, &(acadoWorkspace.E[ 752 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 17, 18, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 17, 19 ); +acado_setBlockH11( 17, 19, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 18, 18, &(acadoWorkspace.R1[ 18 ]) ); +acado_setBlockH11( 18, 18, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 18, 18, &(acadoWorkspace.E[ 832 ]), &(acadoWorkspace.QE[ 832 ]) ); + +acado_zeroBlockH11( 18, 19 ); +acado_setBlockH11( 18, 19, &(acadoWorkspace.E[ 832 ]), &(acadoWorkspace.QE[ 836 ]) ); + +acado_setBlockH11_R1( 19, 19, &(acadoWorkspace.R1[ 19 ]) ); +acado_setBlockH11( 19, 19, &(acadoWorkspace.E[ 836 ]), &(acadoWorkspace.QE[ 836 ]) ); + + +acado_copyHTH( 1, 0 ); +acado_copyHTH( 2, 0 ); +acado_copyHTH( 2, 1 ); +acado_copyHTH( 3, 0 ); +acado_copyHTH( 3, 1 ); +acado_copyHTH( 3, 2 ); +acado_copyHTH( 4, 0 ); +acado_copyHTH( 4, 1 ); +acado_copyHTH( 4, 2 ); +acado_copyHTH( 4, 3 ); +acado_copyHTH( 5, 0 ); +acado_copyHTH( 5, 1 ); +acado_copyHTH( 5, 2 ); +acado_copyHTH( 5, 3 ); +acado_copyHTH( 5, 4 ); +acado_copyHTH( 6, 0 ); +acado_copyHTH( 6, 1 ); +acado_copyHTH( 6, 2 ); +acado_copyHTH( 6, 3 ); +acado_copyHTH( 6, 4 ); +acado_copyHTH( 6, 5 ); +acado_copyHTH( 7, 0 ); +acado_copyHTH( 7, 1 ); +acado_copyHTH( 7, 2 ); +acado_copyHTH( 7, 3 ); +acado_copyHTH( 7, 4 ); +acado_copyHTH( 7, 5 ); +acado_copyHTH( 7, 6 ); +acado_copyHTH( 8, 0 ); +acado_copyHTH( 8, 1 ); +acado_copyHTH( 8, 2 ); +acado_copyHTH( 8, 3 ); +acado_copyHTH( 8, 4 ); +acado_copyHTH( 8, 5 ); +acado_copyHTH( 8, 6 ); +acado_copyHTH( 8, 7 ); +acado_copyHTH( 9, 0 ); +acado_copyHTH( 9, 1 ); +acado_copyHTH( 9, 2 ); +acado_copyHTH( 9, 3 ); +acado_copyHTH( 9, 4 ); +acado_copyHTH( 9, 5 ); +acado_copyHTH( 9, 6 ); +acado_copyHTH( 9, 7 ); +acado_copyHTH( 9, 8 ); +acado_copyHTH( 10, 0 ); +acado_copyHTH( 10, 1 ); +acado_copyHTH( 10, 2 ); +acado_copyHTH( 10, 3 ); +acado_copyHTH( 10, 4 ); +acado_copyHTH( 10, 5 ); +acado_copyHTH( 10, 6 ); +acado_copyHTH( 10, 7 ); +acado_copyHTH( 10, 8 ); +acado_copyHTH( 10, 9 ); +acado_copyHTH( 11, 0 ); +acado_copyHTH( 11, 1 ); +acado_copyHTH( 11, 2 ); +acado_copyHTH( 11, 3 ); +acado_copyHTH( 11, 4 ); +acado_copyHTH( 11, 5 ); +acado_copyHTH( 11, 6 ); +acado_copyHTH( 11, 7 ); +acado_copyHTH( 11, 8 ); +acado_copyHTH( 11, 9 ); +acado_copyHTH( 11, 10 ); +acado_copyHTH( 12, 0 ); +acado_copyHTH( 12, 1 ); +acado_copyHTH( 12, 2 ); +acado_copyHTH( 12, 3 ); +acado_copyHTH( 12, 4 ); +acado_copyHTH( 12, 5 ); +acado_copyHTH( 12, 6 ); +acado_copyHTH( 12, 7 ); +acado_copyHTH( 12, 8 ); +acado_copyHTH( 12, 9 ); +acado_copyHTH( 12, 10 ); +acado_copyHTH( 12, 11 ); +acado_copyHTH( 13, 0 ); +acado_copyHTH( 13, 1 ); +acado_copyHTH( 13, 2 ); +acado_copyHTH( 13, 3 ); +acado_copyHTH( 13, 4 ); +acado_copyHTH( 13, 5 ); +acado_copyHTH( 13, 6 ); +acado_copyHTH( 13, 7 ); +acado_copyHTH( 13, 8 ); +acado_copyHTH( 13, 9 ); +acado_copyHTH( 13, 10 ); +acado_copyHTH( 13, 11 ); +acado_copyHTH( 13, 12 ); +acado_copyHTH( 14, 0 ); +acado_copyHTH( 14, 1 ); +acado_copyHTH( 14, 2 ); +acado_copyHTH( 14, 3 ); +acado_copyHTH( 14, 4 ); +acado_copyHTH( 14, 5 ); +acado_copyHTH( 14, 6 ); +acado_copyHTH( 14, 7 ); +acado_copyHTH( 14, 8 ); +acado_copyHTH( 14, 9 ); +acado_copyHTH( 14, 10 ); +acado_copyHTH( 14, 11 ); +acado_copyHTH( 14, 12 ); +acado_copyHTH( 14, 13 ); +acado_copyHTH( 15, 0 ); +acado_copyHTH( 15, 1 ); +acado_copyHTH( 15, 2 ); +acado_copyHTH( 15, 3 ); +acado_copyHTH( 15, 4 ); +acado_copyHTH( 15, 5 ); +acado_copyHTH( 15, 6 ); +acado_copyHTH( 15, 7 ); +acado_copyHTH( 15, 8 ); +acado_copyHTH( 15, 9 ); +acado_copyHTH( 15, 10 ); +acado_copyHTH( 15, 11 ); +acado_copyHTH( 15, 12 ); +acado_copyHTH( 15, 13 ); +acado_copyHTH( 15, 14 ); +acado_copyHTH( 16, 0 ); +acado_copyHTH( 16, 1 ); +acado_copyHTH( 16, 2 ); +acado_copyHTH( 16, 3 ); +acado_copyHTH( 16, 4 ); +acado_copyHTH( 16, 5 ); +acado_copyHTH( 16, 6 ); +acado_copyHTH( 16, 7 ); +acado_copyHTH( 16, 8 ); +acado_copyHTH( 16, 9 ); +acado_copyHTH( 16, 10 ); +acado_copyHTH( 16, 11 ); +acado_copyHTH( 16, 12 ); +acado_copyHTH( 16, 13 ); +acado_copyHTH( 16, 14 ); +acado_copyHTH( 16, 15 ); +acado_copyHTH( 17, 0 ); +acado_copyHTH( 17, 1 ); +acado_copyHTH( 17, 2 ); +acado_copyHTH( 17, 3 ); +acado_copyHTH( 17, 4 ); +acado_copyHTH( 17, 5 ); +acado_copyHTH( 17, 6 ); +acado_copyHTH( 17, 7 ); +acado_copyHTH( 17, 8 ); +acado_copyHTH( 17, 9 ); +acado_copyHTH( 17, 10 ); +acado_copyHTH( 17, 11 ); +acado_copyHTH( 17, 12 ); +acado_copyHTH( 17, 13 ); +acado_copyHTH( 17, 14 ); +acado_copyHTH( 17, 15 ); +acado_copyHTH( 17, 16 ); +acado_copyHTH( 18, 0 ); +acado_copyHTH( 18, 1 ); +acado_copyHTH( 18, 2 ); +acado_copyHTH( 18, 3 ); +acado_copyHTH( 18, 4 ); +acado_copyHTH( 18, 5 ); +acado_copyHTH( 18, 6 ); +acado_copyHTH( 18, 7 ); +acado_copyHTH( 18, 8 ); +acado_copyHTH( 18, 9 ); +acado_copyHTH( 18, 10 ); +acado_copyHTH( 18, 11 ); +acado_copyHTH( 18, 12 ); +acado_copyHTH( 18, 13 ); +acado_copyHTH( 18, 14 ); +acado_copyHTH( 18, 15 ); +acado_copyHTH( 18, 16 ); +acado_copyHTH( 18, 17 ); +acado_copyHTH( 19, 0 ); +acado_copyHTH( 19, 1 ); +acado_copyHTH( 19, 2 ); +acado_copyHTH( 19, 3 ); +acado_copyHTH( 19, 4 ); +acado_copyHTH( 19, 5 ); +acado_copyHTH( 19, 6 ); +acado_copyHTH( 19, 7 ); +acado_copyHTH( 19, 8 ); +acado_copyHTH( 19, 9 ); +acado_copyHTH( 19, 10 ); +acado_copyHTH( 19, 11 ); +acado_copyHTH( 19, 12 ); +acado_copyHTH( 19, 13 ); +acado_copyHTH( 19, 14 ); +acado_copyHTH( 19, 15 ); +acado_copyHTH( 19, 16 ); +acado_copyHTH( 19, 17 ); +acado_copyHTH( 19, 18 ); + +acadoWorkspace.H[96] = acadoWorkspace.H10[0]; +acadoWorkspace.H[97] = acadoWorkspace.H10[1]; +acadoWorkspace.H[98] = acadoWorkspace.H10[2]; +acadoWorkspace.H[99] = acadoWorkspace.H10[3]; +acadoWorkspace.H[120] = acadoWorkspace.H10[4]; +acadoWorkspace.H[121] = acadoWorkspace.H10[5]; +acadoWorkspace.H[122] = acadoWorkspace.H10[6]; +acadoWorkspace.H[123] = acadoWorkspace.H10[7]; +acadoWorkspace.H[144] = acadoWorkspace.H10[8]; +acadoWorkspace.H[145] = acadoWorkspace.H10[9]; +acadoWorkspace.H[146] = acadoWorkspace.H10[10]; +acadoWorkspace.H[147] = acadoWorkspace.H10[11]; +acadoWorkspace.H[168] = acadoWorkspace.H10[12]; +acadoWorkspace.H[169] = acadoWorkspace.H10[13]; +acadoWorkspace.H[170] = acadoWorkspace.H10[14]; +acadoWorkspace.H[171] = acadoWorkspace.H10[15]; +acadoWorkspace.H[192] = acadoWorkspace.H10[16]; +acadoWorkspace.H[193] = acadoWorkspace.H10[17]; +acadoWorkspace.H[194] = acadoWorkspace.H10[18]; +acadoWorkspace.H[195] = acadoWorkspace.H10[19]; +acadoWorkspace.H[216] = acadoWorkspace.H10[20]; +acadoWorkspace.H[217] = acadoWorkspace.H10[21]; +acadoWorkspace.H[218] = acadoWorkspace.H10[22]; +acadoWorkspace.H[219] = acadoWorkspace.H10[23]; +acadoWorkspace.H[240] = acadoWorkspace.H10[24]; +acadoWorkspace.H[241] = acadoWorkspace.H10[25]; +acadoWorkspace.H[242] = acadoWorkspace.H10[26]; +acadoWorkspace.H[243] = acadoWorkspace.H10[27]; +acadoWorkspace.H[264] = acadoWorkspace.H10[28]; +acadoWorkspace.H[265] = acadoWorkspace.H10[29]; +acadoWorkspace.H[266] = acadoWorkspace.H10[30]; +acadoWorkspace.H[267] = acadoWorkspace.H10[31]; +acadoWorkspace.H[288] = acadoWorkspace.H10[32]; +acadoWorkspace.H[289] = acadoWorkspace.H10[33]; +acadoWorkspace.H[290] = acadoWorkspace.H10[34]; +acadoWorkspace.H[291] = acadoWorkspace.H10[35]; +acadoWorkspace.H[312] = acadoWorkspace.H10[36]; +acadoWorkspace.H[313] = acadoWorkspace.H10[37]; +acadoWorkspace.H[314] = acadoWorkspace.H10[38]; +acadoWorkspace.H[315] = acadoWorkspace.H10[39]; +acadoWorkspace.H[336] = acadoWorkspace.H10[40]; +acadoWorkspace.H[337] = acadoWorkspace.H10[41]; +acadoWorkspace.H[338] = acadoWorkspace.H10[42]; +acadoWorkspace.H[339] = acadoWorkspace.H10[43]; +acadoWorkspace.H[360] = acadoWorkspace.H10[44]; +acadoWorkspace.H[361] = acadoWorkspace.H10[45]; +acadoWorkspace.H[362] = acadoWorkspace.H10[46]; +acadoWorkspace.H[363] = acadoWorkspace.H10[47]; +acadoWorkspace.H[384] = acadoWorkspace.H10[48]; +acadoWorkspace.H[385] = acadoWorkspace.H10[49]; +acadoWorkspace.H[386] = acadoWorkspace.H10[50]; +acadoWorkspace.H[387] = acadoWorkspace.H10[51]; +acadoWorkspace.H[408] = acadoWorkspace.H10[52]; +acadoWorkspace.H[409] = acadoWorkspace.H10[53]; +acadoWorkspace.H[410] = acadoWorkspace.H10[54]; +acadoWorkspace.H[411] = acadoWorkspace.H10[55]; +acadoWorkspace.H[432] = acadoWorkspace.H10[56]; +acadoWorkspace.H[433] = acadoWorkspace.H10[57]; +acadoWorkspace.H[434] = acadoWorkspace.H10[58]; +acadoWorkspace.H[435] = acadoWorkspace.H10[59]; +acadoWorkspace.H[456] = acadoWorkspace.H10[60]; +acadoWorkspace.H[457] = acadoWorkspace.H10[61]; +acadoWorkspace.H[458] = acadoWorkspace.H10[62]; +acadoWorkspace.H[459] = acadoWorkspace.H10[63]; +acadoWorkspace.H[480] = acadoWorkspace.H10[64]; +acadoWorkspace.H[481] = acadoWorkspace.H10[65]; +acadoWorkspace.H[482] = acadoWorkspace.H10[66]; +acadoWorkspace.H[483] = acadoWorkspace.H10[67]; +acadoWorkspace.H[504] = acadoWorkspace.H10[68]; +acadoWorkspace.H[505] = acadoWorkspace.H10[69]; +acadoWorkspace.H[506] = acadoWorkspace.H10[70]; +acadoWorkspace.H[507] = acadoWorkspace.H10[71]; +acadoWorkspace.H[528] = acadoWorkspace.H10[72]; +acadoWorkspace.H[529] = acadoWorkspace.H10[73]; +acadoWorkspace.H[530] = acadoWorkspace.H10[74]; +acadoWorkspace.H[531] = acadoWorkspace.H10[75]; +acadoWorkspace.H[552] = acadoWorkspace.H10[76]; +acadoWorkspace.H[553] = acadoWorkspace.H10[77]; +acadoWorkspace.H[554] = acadoWorkspace.H10[78]; +acadoWorkspace.H[555] = acadoWorkspace.H10[79]; + +acado_multQ1d( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.d, acadoWorkspace.Qd ); +acado_multQ1d( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.d[ 4 ]), &(acadoWorkspace.Qd[ 4 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.d[ 8 ]), &(acadoWorkspace.Qd[ 8 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.Qd[ 12 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.d[ 16 ]), &(acadoWorkspace.Qd[ 16 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.d[ 20 ]), &(acadoWorkspace.Qd[ 20 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.Qd[ 24 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.d[ 28 ]), &(acadoWorkspace.Qd[ 28 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.d[ 32 ]), &(acadoWorkspace.Qd[ 32 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.Qd[ 36 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.d[ 40 ]), &(acadoWorkspace.Qd[ 40 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.d[ 44 ]), &(acadoWorkspace.Qd[ 44 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.Qd[ 48 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.d[ 52 ]), &(acadoWorkspace.Qd[ 52 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.d[ 56 ]), &(acadoWorkspace.Qd[ 56 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.d[ 60 ]), &(acadoWorkspace.Qd[ 60 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.d[ 64 ]), &(acadoWorkspace.Qd[ 64 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.d[ 68 ]), &(acadoWorkspace.Qd[ 68 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.d[ 72 ]), &(acadoWorkspace.Qd[ 72 ]) ); +acado_multQN1d( acadoWorkspace.QN1, &(acadoWorkspace.d[ 76 ]), &(acadoWorkspace.Qd[ 76 ]) ); + +acado_macCTSlx( acadoWorkspace.evGx, acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 16 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 32 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 48 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 64 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 80 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 96 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 112 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 128 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 160 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 176 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 192 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 208 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 224 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 240 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 256 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 272 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 288 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 304 ]), acadoWorkspace.g ); +acado_macETSlu( acadoWorkspace.QE, &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 4 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 24 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 40 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 60 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 84 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 112 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 144 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 180 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 220 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 264 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 364 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 420 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 480 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 544 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 612 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 684 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 760 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 8 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 16 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 28 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 44 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 64 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 88 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 116 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 148 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 184 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 224 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 268 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 316 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 368 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 424 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 484 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 548 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 616 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 688 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 764 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 20 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 32 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 48 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 68 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 92 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 120 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 152 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 188 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 228 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 272 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 320 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 372 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 428 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 488 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 552 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 620 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 692 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 768 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 36 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 52 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 72 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 96 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 124 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 156 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 192 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 232 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 276 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 376 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 432 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 492 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 556 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 624 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 696 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 772 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 56 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 76 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 100 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 128 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 160 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 196 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 236 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 280 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 328 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 380 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 436 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 496 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 560 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 628 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 700 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 776 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 80 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 104 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 132 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 164 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 200 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 240 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 284 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 332 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 384 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 440 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 500 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 564 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 632 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 704 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 780 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 108 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 136 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 168 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 204 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 244 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 288 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 388 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 444 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 504 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 568 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 636 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 708 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 784 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 140 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 172 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 208 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 248 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 292 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 340 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 392 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 448 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 508 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 572 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 640 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 712 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 788 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 176 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 212 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 252 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 296 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 344 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 396 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 452 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 512 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 576 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 644 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 716 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 792 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 216 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 256 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 300 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 400 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 456 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 516 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 580 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 648 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 720 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 796 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 260 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 304 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 352 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 404 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 460 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 520 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 584 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 652 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 724 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 800 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 308 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 356 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 408 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 464 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 524 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 588 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 656 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 728 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 804 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 360 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 412 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 468 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 528 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 592 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 660 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 732 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 808 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 416 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 472 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 532 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 596 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 664 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 736 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 812 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 476 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 536 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 600 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 668 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 740 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 816 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 540 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 604 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 672 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 744 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 820 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 608 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 676 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 748 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 824 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 680 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 752 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 828 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 756 ]), &(acadoWorkspace.g[ 22 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 832 ]), &(acadoWorkspace.g[ 22 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 836 ]), &(acadoWorkspace.g[ 23 ]) ); +acadoWorkspace.lb[4] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[0]; +acadoWorkspace.lb[5] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[1]; +acadoWorkspace.lb[6] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[2]; +acadoWorkspace.lb[7] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[3]; +acadoWorkspace.lb[8] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[4]; +acadoWorkspace.lb[9] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[5]; +acadoWorkspace.lb[10] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[6]; +acadoWorkspace.lb[11] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[7]; +acadoWorkspace.lb[12] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[8]; +acadoWorkspace.lb[13] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[9]; +acadoWorkspace.lb[14] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[10]; +acadoWorkspace.lb[15] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[11]; +acadoWorkspace.lb[16] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[12]; +acadoWorkspace.lb[17] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[13]; +acadoWorkspace.lb[18] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[14]; +acadoWorkspace.lb[19] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[15]; +acadoWorkspace.lb[20] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[16]; +acadoWorkspace.lb[21] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[17]; +acadoWorkspace.lb[22] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[18]; +acadoWorkspace.lb[23] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[19]; +acadoWorkspace.ub[4] = (real_t)1.0000000000000000e+12 - acadoVariables.u[0]; +acadoWorkspace.ub[5] = (real_t)1.0000000000000000e+12 - acadoVariables.u[1]; +acadoWorkspace.ub[6] = (real_t)1.0000000000000000e+12 - acadoVariables.u[2]; +acadoWorkspace.ub[7] = (real_t)1.0000000000000000e+12 - acadoVariables.u[3]; +acadoWorkspace.ub[8] = (real_t)1.0000000000000000e+12 - acadoVariables.u[4]; +acadoWorkspace.ub[9] = (real_t)1.0000000000000000e+12 - acadoVariables.u[5]; +acadoWorkspace.ub[10] = (real_t)1.0000000000000000e+12 - acadoVariables.u[6]; +acadoWorkspace.ub[11] = (real_t)1.0000000000000000e+12 - acadoVariables.u[7]; +acadoWorkspace.ub[12] = (real_t)1.0000000000000000e+12 - acadoVariables.u[8]; +acadoWorkspace.ub[13] = (real_t)1.0000000000000000e+12 - acadoVariables.u[9]; +acadoWorkspace.ub[14] = (real_t)1.0000000000000000e+12 - acadoVariables.u[10]; +acadoWorkspace.ub[15] = (real_t)1.0000000000000000e+12 - acadoVariables.u[11]; +acadoWorkspace.ub[16] = (real_t)1.0000000000000000e+12 - acadoVariables.u[12]; +acadoWorkspace.ub[17] = (real_t)1.0000000000000000e+12 - acadoVariables.u[13]; +acadoWorkspace.ub[18] = (real_t)1.0000000000000000e+12 - acadoVariables.u[14]; +acadoWorkspace.ub[19] = (real_t)1.0000000000000000e+12 - acadoVariables.u[15]; +acadoWorkspace.ub[20] = (real_t)1.0000000000000000e+12 - acadoVariables.u[16]; +acadoWorkspace.ub[21] = (real_t)1.0000000000000000e+12 - acadoVariables.u[17]; +acadoWorkspace.ub[22] = (real_t)1.0000000000000000e+12 - acadoVariables.u[18]; +acadoWorkspace.ub[23] = (real_t)1.0000000000000000e+12 - acadoVariables.u[19]; + +} + +void acado_condenseFdb( ) +{ +acadoWorkspace.Dx0[0] = acadoVariables.x0[0] - acadoVariables.x[0]; +acadoWorkspace.Dx0[1] = acadoVariables.x0[1] - acadoVariables.x[1]; +acadoWorkspace.Dx0[2] = acadoVariables.x0[2] - acadoVariables.x[2]; +acadoWorkspace.Dx0[3] = acadoVariables.x0[3] - acadoVariables.x[3]; + +acadoWorkspace.Dy[0] -= acadoVariables.y[0]; +acadoWorkspace.Dy[1] -= acadoVariables.y[1]; +acadoWorkspace.Dy[2] -= acadoVariables.y[2]; +acadoWorkspace.Dy[3] -= acadoVariables.y[3]; +acadoWorkspace.Dy[4] -= acadoVariables.y[4]; +acadoWorkspace.Dy[5] -= acadoVariables.y[5]; +acadoWorkspace.Dy[6] -= acadoVariables.y[6]; +acadoWorkspace.Dy[7] -= acadoVariables.y[7]; +acadoWorkspace.Dy[8] -= acadoVariables.y[8]; +acadoWorkspace.Dy[9] -= acadoVariables.y[9]; +acadoWorkspace.Dy[10] -= acadoVariables.y[10]; +acadoWorkspace.Dy[11] -= acadoVariables.y[11]; +acadoWorkspace.Dy[12] -= acadoVariables.y[12]; +acadoWorkspace.Dy[13] -= acadoVariables.y[13]; +acadoWorkspace.Dy[14] -= acadoVariables.y[14]; +acadoWorkspace.Dy[15] -= acadoVariables.y[15]; +acadoWorkspace.Dy[16] -= acadoVariables.y[16]; +acadoWorkspace.Dy[17] -= acadoVariables.y[17]; +acadoWorkspace.Dy[18] -= acadoVariables.y[18]; +acadoWorkspace.Dy[19] -= acadoVariables.y[19]; +acadoWorkspace.Dy[20] -= acadoVariables.y[20]; +acadoWorkspace.Dy[21] -= acadoVariables.y[21]; +acadoWorkspace.Dy[22] -= acadoVariables.y[22]; +acadoWorkspace.Dy[23] -= acadoVariables.y[23]; +acadoWorkspace.Dy[24] -= acadoVariables.y[24]; +acadoWorkspace.Dy[25] -= acadoVariables.y[25]; +acadoWorkspace.Dy[26] -= acadoVariables.y[26]; +acadoWorkspace.Dy[27] -= acadoVariables.y[27]; +acadoWorkspace.Dy[28] -= acadoVariables.y[28]; +acadoWorkspace.Dy[29] -= acadoVariables.y[29]; +acadoWorkspace.Dy[30] -= acadoVariables.y[30]; +acadoWorkspace.Dy[31] -= acadoVariables.y[31]; +acadoWorkspace.Dy[32] -= acadoVariables.y[32]; +acadoWorkspace.Dy[33] -= acadoVariables.y[33]; +acadoWorkspace.Dy[34] -= acadoVariables.y[34]; +acadoWorkspace.Dy[35] -= acadoVariables.y[35]; +acadoWorkspace.Dy[36] -= acadoVariables.y[36]; +acadoWorkspace.Dy[37] -= acadoVariables.y[37]; +acadoWorkspace.Dy[38] -= acadoVariables.y[38]; +acadoWorkspace.Dy[39] -= acadoVariables.y[39]; +acadoWorkspace.Dy[40] -= acadoVariables.y[40]; +acadoWorkspace.Dy[41] -= acadoVariables.y[41]; +acadoWorkspace.Dy[42] -= acadoVariables.y[42]; +acadoWorkspace.Dy[43] -= acadoVariables.y[43]; +acadoWorkspace.Dy[44] -= acadoVariables.y[44]; +acadoWorkspace.Dy[45] -= acadoVariables.y[45]; +acadoWorkspace.Dy[46] -= acadoVariables.y[46]; +acadoWorkspace.Dy[47] -= acadoVariables.y[47]; +acadoWorkspace.Dy[48] -= acadoVariables.y[48]; +acadoWorkspace.Dy[49] -= acadoVariables.y[49]; +acadoWorkspace.Dy[50] -= acadoVariables.y[50]; +acadoWorkspace.Dy[51] -= acadoVariables.y[51]; +acadoWorkspace.Dy[52] -= acadoVariables.y[52]; +acadoWorkspace.Dy[53] -= acadoVariables.y[53]; +acadoWorkspace.Dy[54] -= acadoVariables.y[54]; +acadoWorkspace.Dy[55] -= acadoVariables.y[55]; +acadoWorkspace.Dy[56] -= acadoVariables.y[56]; +acadoWorkspace.Dy[57] -= acadoVariables.y[57]; +acadoWorkspace.Dy[58] -= acadoVariables.y[58]; +acadoWorkspace.Dy[59] -= acadoVariables.y[59]; +acadoWorkspace.Dy[60] -= acadoVariables.y[60]; +acadoWorkspace.Dy[61] -= acadoVariables.y[61]; +acadoWorkspace.Dy[62] -= acadoVariables.y[62]; +acadoWorkspace.Dy[63] -= acadoVariables.y[63]; +acadoWorkspace.Dy[64] -= acadoVariables.y[64]; +acadoWorkspace.Dy[65] -= acadoVariables.y[65]; +acadoWorkspace.Dy[66] -= acadoVariables.y[66]; +acadoWorkspace.Dy[67] -= acadoVariables.y[67]; +acadoWorkspace.Dy[68] -= acadoVariables.y[68]; +acadoWorkspace.Dy[69] -= acadoVariables.y[69]; +acadoWorkspace.Dy[70] -= acadoVariables.y[70]; +acadoWorkspace.Dy[71] -= acadoVariables.y[71]; +acadoWorkspace.Dy[72] -= acadoVariables.y[72]; +acadoWorkspace.Dy[73] -= acadoVariables.y[73]; +acadoWorkspace.Dy[74] -= acadoVariables.y[74]; +acadoWorkspace.Dy[75] -= acadoVariables.y[75]; +acadoWorkspace.Dy[76] -= acadoVariables.y[76]; +acadoWorkspace.Dy[77] -= acadoVariables.y[77]; +acadoWorkspace.Dy[78] -= acadoVariables.y[78]; +acadoWorkspace.Dy[79] -= acadoVariables.y[79]; +acadoWorkspace.Dy[80] -= acadoVariables.y[80]; +acadoWorkspace.Dy[81] -= acadoVariables.y[81]; +acadoWorkspace.Dy[82] -= acadoVariables.y[82]; +acadoWorkspace.Dy[83] -= acadoVariables.y[83]; +acadoWorkspace.Dy[84] -= acadoVariables.y[84]; +acadoWorkspace.Dy[85] -= acadoVariables.y[85]; +acadoWorkspace.Dy[86] -= acadoVariables.y[86]; +acadoWorkspace.Dy[87] -= acadoVariables.y[87]; +acadoWorkspace.Dy[88] -= acadoVariables.y[88]; +acadoWorkspace.Dy[89] -= acadoVariables.y[89]; +acadoWorkspace.Dy[90] -= acadoVariables.y[90]; +acadoWorkspace.Dy[91] -= acadoVariables.y[91]; +acadoWorkspace.Dy[92] -= acadoVariables.y[92]; +acadoWorkspace.Dy[93] -= acadoVariables.y[93]; +acadoWorkspace.Dy[94] -= acadoVariables.y[94]; +acadoWorkspace.Dy[95] -= acadoVariables.y[95]; +acadoWorkspace.Dy[96] -= acadoVariables.y[96]; +acadoWorkspace.Dy[97] -= acadoVariables.y[97]; +acadoWorkspace.Dy[98] -= acadoVariables.y[98]; +acadoWorkspace.Dy[99] -= acadoVariables.y[99]; +acadoWorkspace.DyN[0] -= acadoVariables.yN[0]; +acadoWorkspace.DyN[1] -= acadoVariables.yN[1]; +acadoWorkspace.DyN[2] -= acadoVariables.yN[2]; +acadoWorkspace.DyN[3] -= acadoVariables.yN[3]; + +acado_multRDy( acadoWorkspace.R2, acadoWorkspace.Dy, &(acadoWorkspace.g[ 4 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 5 ]), &(acadoWorkspace.Dy[ 5 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 10 ]), &(acadoWorkspace.Dy[ 10 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 15 ]), &(acadoWorkspace.Dy[ 15 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 20 ]), &(acadoWorkspace.Dy[ 20 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 25 ]), &(acadoWorkspace.Dy[ 25 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 30 ]), &(acadoWorkspace.Dy[ 30 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 35 ]), &(acadoWorkspace.Dy[ 35 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 40 ]), &(acadoWorkspace.Dy[ 40 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 45 ]), &(acadoWorkspace.Dy[ 45 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 50 ]), &(acadoWorkspace.Dy[ 50 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 55 ]), &(acadoWorkspace.Dy[ 55 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 60 ]), &(acadoWorkspace.Dy[ 60 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 65 ]), &(acadoWorkspace.Dy[ 65 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 70 ]), &(acadoWorkspace.Dy[ 70 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 75 ]), &(acadoWorkspace.Dy[ 75 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 80 ]), &(acadoWorkspace.Dy[ 80 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 85 ]), &(acadoWorkspace.Dy[ 85 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 90 ]), &(acadoWorkspace.Dy[ 90 ]), &(acadoWorkspace.g[ 22 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 95 ]), &(acadoWorkspace.Dy[ 95 ]), &(acadoWorkspace.g[ 23 ]) ); + +acado_multQDy( acadoWorkspace.Q2, acadoWorkspace.Dy, acadoWorkspace.QDy ); +acado_multQDy( &(acadoWorkspace.Q2[ 20 ]), &(acadoWorkspace.Dy[ 5 ]), &(acadoWorkspace.QDy[ 4 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 40 ]), &(acadoWorkspace.Dy[ 10 ]), &(acadoWorkspace.QDy[ 8 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 60 ]), &(acadoWorkspace.Dy[ 15 ]), &(acadoWorkspace.QDy[ 12 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 80 ]), &(acadoWorkspace.Dy[ 20 ]), &(acadoWorkspace.QDy[ 16 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 100 ]), &(acadoWorkspace.Dy[ 25 ]), &(acadoWorkspace.QDy[ 20 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 120 ]), &(acadoWorkspace.Dy[ 30 ]), &(acadoWorkspace.QDy[ 24 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 140 ]), &(acadoWorkspace.Dy[ 35 ]), &(acadoWorkspace.QDy[ 28 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 160 ]), &(acadoWorkspace.Dy[ 40 ]), &(acadoWorkspace.QDy[ 32 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 180 ]), &(acadoWorkspace.Dy[ 45 ]), &(acadoWorkspace.QDy[ 36 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 200 ]), &(acadoWorkspace.Dy[ 50 ]), &(acadoWorkspace.QDy[ 40 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 220 ]), &(acadoWorkspace.Dy[ 55 ]), &(acadoWorkspace.QDy[ 44 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 240 ]), &(acadoWorkspace.Dy[ 60 ]), &(acadoWorkspace.QDy[ 48 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 260 ]), &(acadoWorkspace.Dy[ 65 ]), &(acadoWorkspace.QDy[ 52 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 280 ]), &(acadoWorkspace.Dy[ 70 ]), &(acadoWorkspace.QDy[ 56 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 300 ]), &(acadoWorkspace.Dy[ 75 ]), &(acadoWorkspace.QDy[ 60 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 320 ]), &(acadoWorkspace.Dy[ 80 ]), &(acadoWorkspace.QDy[ 64 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 340 ]), &(acadoWorkspace.Dy[ 85 ]), &(acadoWorkspace.QDy[ 68 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 360 ]), &(acadoWorkspace.Dy[ 90 ]), &(acadoWorkspace.QDy[ 72 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 380 ]), &(acadoWorkspace.Dy[ 95 ]), &(acadoWorkspace.QDy[ 76 ]) ); + +acadoWorkspace.QDy[80] = + acadoWorkspace.QN2[0]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[1]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[2]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[3]*acadoWorkspace.DyN[3]; +acadoWorkspace.QDy[81] = + acadoWorkspace.QN2[4]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[5]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[6]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[7]*acadoWorkspace.DyN[3]; +acadoWorkspace.QDy[82] = + acadoWorkspace.QN2[8]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[9]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[10]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[11]*acadoWorkspace.DyN[3]; +acadoWorkspace.QDy[83] = + acadoWorkspace.QN2[12]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[13]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[14]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[15]*acadoWorkspace.DyN[3]; + +acadoWorkspace.QDy[4] += acadoWorkspace.Qd[0]; +acadoWorkspace.QDy[5] += acadoWorkspace.Qd[1]; +acadoWorkspace.QDy[6] += acadoWorkspace.Qd[2]; +acadoWorkspace.QDy[7] += acadoWorkspace.Qd[3]; +acadoWorkspace.QDy[8] += acadoWorkspace.Qd[4]; +acadoWorkspace.QDy[9] += acadoWorkspace.Qd[5]; +acadoWorkspace.QDy[10] += acadoWorkspace.Qd[6]; +acadoWorkspace.QDy[11] += acadoWorkspace.Qd[7]; +acadoWorkspace.QDy[12] += acadoWorkspace.Qd[8]; +acadoWorkspace.QDy[13] += acadoWorkspace.Qd[9]; +acadoWorkspace.QDy[14] += acadoWorkspace.Qd[10]; +acadoWorkspace.QDy[15] += acadoWorkspace.Qd[11]; +acadoWorkspace.QDy[16] += acadoWorkspace.Qd[12]; +acadoWorkspace.QDy[17] += acadoWorkspace.Qd[13]; +acadoWorkspace.QDy[18] += acadoWorkspace.Qd[14]; +acadoWorkspace.QDy[19] += acadoWorkspace.Qd[15]; +acadoWorkspace.QDy[20] += acadoWorkspace.Qd[16]; +acadoWorkspace.QDy[21] += acadoWorkspace.Qd[17]; +acadoWorkspace.QDy[22] += acadoWorkspace.Qd[18]; +acadoWorkspace.QDy[23] += acadoWorkspace.Qd[19]; +acadoWorkspace.QDy[24] += acadoWorkspace.Qd[20]; +acadoWorkspace.QDy[25] += acadoWorkspace.Qd[21]; +acadoWorkspace.QDy[26] += acadoWorkspace.Qd[22]; +acadoWorkspace.QDy[27] += acadoWorkspace.Qd[23]; +acadoWorkspace.QDy[28] += acadoWorkspace.Qd[24]; +acadoWorkspace.QDy[29] += acadoWorkspace.Qd[25]; +acadoWorkspace.QDy[30] += acadoWorkspace.Qd[26]; +acadoWorkspace.QDy[31] += acadoWorkspace.Qd[27]; +acadoWorkspace.QDy[32] += acadoWorkspace.Qd[28]; +acadoWorkspace.QDy[33] += acadoWorkspace.Qd[29]; +acadoWorkspace.QDy[34] += acadoWorkspace.Qd[30]; +acadoWorkspace.QDy[35] += acadoWorkspace.Qd[31]; +acadoWorkspace.QDy[36] += acadoWorkspace.Qd[32]; +acadoWorkspace.QDy[37] += acadoWorkspace.Qd[33]; +acadoWorkspace.QDy[38] += acadoWorkspace.Qd[34]; +acadoWorkspace.QDy[39] += acadoWorkspace.Qd[35]; +acadoWorkspace.QDy[40] += acadoWorkspace.Qd[36]; +acadoWorkspace.QDy[41] += acadoWorkspace.Qd[37]; +acadoWorkspace.QDy[42] += acadoWorkspace.Qd[38]; +acadoWorkspace.QDy[43] += acadoWorkspace.Qd[39]; +acadoWorkspace.QDy[44] += acadoWorkspace.Qd[40]; +acadoWorkspace.QDy[45] += acadoWorkspace.Qd[41]; +acadoWorkspace.QDy[46] += acadoWorkspace.Qd[42]; +acadoWorkspace.QDy[47] += acadoWorkspace.Qd[43]; +acadoWorkspace.QDy[48] += acadoWorkspace.Qd[44]; +acadoWorkspace.QDy[49] += acadoWorkspace.Qd[45]; +acadoWorkspace.QDy[50] += acadoWorkspace.Qd[46]; +acadoWorkspace.QDy[51] += acadoWorkspace.Qd[47]; +acadoWorkspace.QDy[52] += acadoWorkspace.Qd[48]; +acadoWorkspace.QDy[53] += acadoWorkspace.Qd[49]; +acadoWorkspace.QDy[54] += acadoWorkspace.Qd[50]; +acadoWorkspace.QDy[55] += acadoWorkspace.Qd[51]; +acadoWorkspace.QDy[56] += acadoWorkspace.Qd[52]; +acadoWorkspace.QDy[57] += acadoWorkspace.Qd[53]; +acadoWorkspace.QDy[58] += acadoWorkspace.Qd[54]; +acadoWorkspace.QDy[59] += acadoWorkspace.Qd[55]; +acadoWorkspace.QDy[60] += acadoWorkspace.Qd[56]; +acadoWorkspace.QDy[61] += acadoWorkspace.Qd[57]; +acadoWorkspace.QDy[62] += acadoWorkspace.Qd[58]; +acadoWorkspace.QDy[63] += acadoWorkspace.Qd[59]; +acadoWorkspace.QDy[64] += acadoWorkspace.Qd[60]; +acadoWorkspace.QDy[65] += acadoWorkspace.Qd[61]; +acadoWorkspace.QDy[66] += acadoWorkspace.Qd[62]; +acadoWorkspace.QDy[67] += acadoWorkspace.Qd[63]; +acadoWorkspace.QDy[68] += acadoWorkspace.Qd[64]; +acadoWorkspace.QDy[69] += acadoWorkspace.Qd[65]; +acadoWorkspace.QDy[70] += acadoWorkspace.Qd[66]; +acadoWorkspace.QDy[71] += acadoWorkspace.Qd[67]; +acadoWorkspace.QDy[72] += acadoWorkspace.Qd[68]; +acadoWorkspace.QDy[73] += acadoWorkspace.Qd[69]; +acadoWorkspace.QDy[74] += acadoWorkspace.Qd[70]; +acadoWorkspace.QDy[75] += acadoWorkspace.Qd[71]; +acadoWorkspace.QDy[76] += acadoWorkspace.Qd[72]; +acadoWorkspace.QDy[77] += acadoWorkspace.Qd[73]; +acadoWorkspace.QDy[78] += acadoWorkspace.Qd[74]; +acadoWorkspace.QDy[79] += acadoWorkspace.Qd[75]; +acadoWorkspace.QDy[80] += acadoWorkspace.Qd[76]; +acadoWorkspace.QDy[81] += acadoWorkspace.Qd[77]; +acadoWorkspace.QDy[82] += acadoWorkspace.Qd[78]; +acadoWorkspace.QDy[83] += acadoWorkspace.Qd[79]; + +acadoWorkspace.g[0] = + acadoWorkspace.evGx[0]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[4]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[8]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[12]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[16]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[20]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[24]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[28]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[32]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[36]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[40]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[44]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[48]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[52]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[56]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[60]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[64]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[68]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[72]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[76]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[80]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[84]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[88]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[92]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[96]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[100]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[104]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[108]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[112]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[116]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[120]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[124]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[128]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[132]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[136]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[140]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[144]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[148]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[152]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[156]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[160]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[164]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[168]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[172]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[176]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[180]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[184]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[188]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[192]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[196]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[200]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[204]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[208]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[212]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[216]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[220]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[224]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[228]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[232]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[236]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[240]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[244]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[248]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[252]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[256]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[260]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[264]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[268]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[272]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[276]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[280]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[284]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[288]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[292]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[296]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[300]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[304]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[308]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[312]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[316]*acadoWorkspace.QDy[83]; +acadoWorkspace.g[1] = + acadoWorkspace.evGx[1]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[5]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[9]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[13]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[17]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[21]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[25]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[29]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[33]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[37]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[41]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[45]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[49]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[53]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[57]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[61]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[65]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[69]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[73]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[77]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[81]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[85]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[89]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[93]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[97]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[101]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[105]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[109]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[113]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[117]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[121]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[125]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[129]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[133]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[137]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[141]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[145]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[149]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[153]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[157]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[161]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[165]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[169]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[173]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[177]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[181]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[185]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[189]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[193]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[197]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[201]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[205]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[209]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[213]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[217]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[221]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[225]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[229]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[233]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[237]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[241]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[245]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[249]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[253]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[257]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[261]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[265]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[269]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[273]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[277]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[281]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[285]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[289]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[293]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[297]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[301]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[305]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[309]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[313]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[317]*acadoWorkspace.QDy[83]; +acadoWorkspace.g[2] = + acadoWorkspace.evGx[2]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[6]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[10]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[14]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[18]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[22]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[26]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[30]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[34]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[38]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[42]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[46]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[50]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[54]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[58]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[62]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[66]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[70]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[74]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[78]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[82]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[86]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[90]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[94]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[98]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[102]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[106]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[110]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[114]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[118]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[122]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[126]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[130]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[134]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[138]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[142]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[146]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[150]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[154]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[158]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[162]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[166]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[170]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[174]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[178]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[182]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[186]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[190]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[194]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[198]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[202]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[206]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[210]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[214]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[218]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[222]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[226]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[230]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[234]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[238]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[242]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[246]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[250]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[254]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[258]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[262]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[266]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[270]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[274]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[278]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[282]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[286]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[290]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[294]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[298]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[302]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[306]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[310]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[314]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[318]*acadoWorkspace.QDy[83]; +acadoWorkspace.g[3] = + acadoWorkspace.evGx[3]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[7]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[11]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[15]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[19]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[23]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[27]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[31]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[35]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[39]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[43]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[47]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[51]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[55]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[59]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[63]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[67]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[71]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[75]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[79]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[83]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[87]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[91]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[95]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[99]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[103]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[107]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[111]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[115]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[119]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[123]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[127]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[131]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[135]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[139]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[143]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[147]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[151]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[155]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[159]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[163]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[167]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[171]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[175]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[179]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[183]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[187]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[191]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[195]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[199]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[203]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[207]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[211]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[215]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[219]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[223]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[227]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[231]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[235]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[239]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[243]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[247]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[251]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[255]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[259]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[263]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[267]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[271]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[275]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[279]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[283]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[287]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[291]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[295]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[299]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[303]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[307]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[311]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[315]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[319]*acadoWorkspace.QDy[83]; + + +acado_multEQDy( acadoWorkspace.E, &(acadoWorkspace.QDy[ 4 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.QDy[ 8 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QDy[ 16 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.QDy[ 8 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QDy[ 16 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.QDy[ 16 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QDy[ 16 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 604 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 608 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 676 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 680 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 752 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 22 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 832 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 22 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 836 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 23 ]) ); + +acadoWorkspace.lb[0] = acadoWorkspace.Dx0[0]; +acadoWorkspace.lb[1] = acadoWorkspace.Dx0[1]; +acadoWorkspace.lb[2] = acadoWorkspace.Dx0[2]; +acadoWorkspace.lb[3] = acadoWorkspace.Dx0[3]; +acadoWorkspace.ub[0] = acadoWorkspace.Dx0[0]; +acadoWorkspace.ub[1] = acadoWorkspace.Dx0[1]; +acadoWorkspace.ub[2] = acadoWorkspace.Dx0[2]; +acadoWorkspace.ub[3] = acadoWorkspace.Dx0[3]; +} + +void acado_expand( ) +{ +acadoVariables.x[0] += acadoWorkspace.x[0]; +acadoVariables.x[1] += acadoWorkspace.x[1]; +acadoVariables.x[2] += acadoWorkspace.x[2]; +acadoVariables.x[3] += acadoWorkspace.x[3]; + +acadoVariables.u[0] += acadoWorkspace.x[4]; +acadoVariables.u[1] += acadoWorkspace.x[5]; +acadoVariables.u[2] += acadoWorkspace.x[6]; +acadoVariables.u[3] += acadoWorkspace.x[7]; +acadoVariables.u[4] += acadoWorkspace.x[8]; +acadoVariables.u[5] += acadoWorkspace.x[9]; +acadoVariables.u[6] += acadoWorkspace.x[10]; +acadoVariables.u[7] += acadoWorkspace.x[11]; +acadoVariables.u[8] += acadoWorkspace.x[12]; +acadoVariables.u[9] += acadoWorkspace.x[13]; +acadoVariables.u[10] += acadoWorkspace.x[14]; +acadoVariables.u[11] += acadoWorkspace.x[15]; +acadoVariables.u[12] += acadoWorkspace.x[16]; +acadoVariables.u[13] += acadoWorkspace.x[17]; +acadoVariables.u[14] += acadoWorkspace.x[18]; +acadoVariables.u[15] += acadoWorkspace.x[19]; +acadoVariables.u[16] += acadoWorkspace.x[20]; +acadoVariables.u[17] += acadoWorkspace.x[21]; +acadoVariables.u[18] += acadoWorkspace.x[22]; +acadoVariables.u[19] += acadoWorkspace.x[23]; + +acadoVariables.x[4] += + acadoWorkspace.evGx[0]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1]*acadoWorkspace.x[1] + acadoWorkspace.evGx[2]*acadoWorkspace.x[2] + acadoWorkspace.evGx[3]*acadoWorkspace.x[3] + acadoWorkspace.d[0]; +acadoVariables.x[5] += + acadoWorkspace.evGx[4]*acadoWorkspace.x[0] + acadoWorkspace.evGx[5]*acadoWorkspace.x[1] + acadoWorkspace.evGx[6]*acadoWorkspace.x[2] + acadoWorkspace.evGx[7]*acadoWorkspace.x[3] + acadoWorkspace.d[1]; +acadoVariables.x[6] += + acadoWorkspace.evGx[8]*acadoWorkspace.x[0] + acadoWorkspace.evGx[9]*acadoWorkspace.x[1] + acadoWorkspace.evGx[10]*acadoWorkspace.x[2] + acadoWorkspace.evGx[11]*acadoWorkspace.x[3] + acadoWorkspace.d[2]; +acadoVariables.x[7] += + acadoWorkspace.evGx[12]*acadoWorkspace.x[0] + acadoWorkspace.evGx[13]*acadoWorkspace.x[1] + acadoWorkspace.evGx[14]*acadoWorkspace.x[2] + acadoWorkspace.evGx[15]*acadoWorkspace.x[3] + acadoWorkspace.d[3]; +acadoVariables.x[8] += + acadoWorkspace.evGx[16]*acadoWorkspace.x[0] + acadoWorkspace.evGx[17]*acadoWorkspace.x[1] + acadoWorkspace.evGx[18]*acadoWorkspace.x[2] + acadoWorkspace.evGx[19]*acadoWorkspace.x[3] + acadoWorkspace.d[4]; +acadoVariables.x[9] += + acadoWorkspace.evGx[20]*acadoWorkspace.x[0] + acadoWorkspace.evGx[21]*acadoWorkspace.x[1] + acadoWorkspace.evGx[22]*acadoWorkspace.x[2] + acadoWorkspace.evGx[23]*acadoWorkspace.x[3] + acadoWorkspace.d[5]; +acadoVariables.x[10] += + acadoWorkspace.evGx[24]*acadoWorkspace.x[0] + acadoWorkspace.evGx[25]*acadoWorkspace.x[1] + acadoWorkspace.evGx[26]*acadoWorkspace.x[2] + acadoWorkspace.evGx[27]*acadoWorkspace.x[3] + acadoWorkspace.d[6]; +acadoVariables.x[11] += + acadoWorkspace.evGx[28]*acadoWorkspace.x[0] + acadoWorkspace.evGx[29]*acadoWorkspace.x[1] + acadoWorkspace.evGx[30]*acadoWorkspace.x[2] + acadoWorkspace.evGx[31]*acadoWorkspace.x[3] + acadoWorkspace.d[7]; +acadoVariables.x[12] += + acadoWorkspace.evGx[32]*acadoWorkspace.x[0] + acadoWorkspace.evGx[33]*acadoWorkspace.x[1] + acadoWorkspace.evGx[34]*acadoWorkspace.x[2] + acadoWorkspace.evGx[35]*acadoWorkspace.x[3] + acadoWorkspace.d[8]; +acadoVariables.x[13] += + acadoWorkspace.evGx[36]*acadoWorkspace.x[0] + acadoWorkspace.evGx[37]*acadoWorkspace.x[1] + acadoWorkspace.evGx[38]*acadoWorkspace.x[2] + acadoWorkspace.evGx[39]*acadoWorkspace.x[3] + acadoWorkspace.d[9]; +acadoVariables.x[14] += + acadoWorkspace.evGx[40]*acadoWorkspace.x[0] + acadoWorkspace.evGx[41]*acadoWorkspace.x[1] + acadoWorkspace.evGx[42]*acadoWorkspace.x[2] + acadoWorkspace.evGx[43]*acadoWorkspace.x[3] + acadoWorkspace.d[10]; +acadoVariables.x[15] += + acadoWorkspace.evGx[44]*acadoWorkspace.x[0] + acadoWorkspace.evGx[45]*acadoWorkspace.x[1] + acadoWorkspace.evGx[46]*acadoWorkspace.x[2] + acadoWorkspace.evGx[47]*acadoWorkspace.x[3] + acadoWorkspace.d[11]; +acadoVariables.x[16] += + acadoWorkspace.evGx[48]*acadoWorkspace.x[0] + acadoWorkspace.evGx[49]*acadoWorkspace.x[1] + acadoWorkspace.evGx[50]*acadoWorkspace.x[2] + acadoWorkspace.evGx[51]*acadoWorkspace.x[3] + acadoWorkspace.d[12]; +acadoVariables.x[17] += + acadoWorkspace.evGx[52]*acadoWorkspace.x[0] + acadoWorkspace.evGx[53]*acadoWorkspace.x[1] + acadoWorkspace.evGx[54]*acadoWorkspace.x[2] + acadoWorkspace.evGx[55]*acadoWorkspace.x[3] + acadoWorkspace.d[13]; +acadoVariables.x[18] += + acadoWorkspace.evGx[56]*acadoWorkspace.x[0] + acadoWorkspace.evGx[57]*acadoWorkspace.x[1] + acadoWorkspace.evGx[58]*acadoWorkspace.x[2] + acadoWorkspace.evGx[59]*acadoWorkspace.x[3] + acadoWorkspace.d[14]; +acadoVariables.x[19] += + acadoWorkspace.evGx[60]*acadoWorkspace.x[0] + acadoWorkspace.evGx[61]*acadoWorkspace.x[1] + acadoWorkspace.evGx[62]*acadoWorkspace.x[2] + acadoWorkspace.evGx[63]*acadoWorkspace.x[3] + acadoWorkspace.d[15]; +acadoVariables.x[20] += + acadoWorkspace.evGx[64]*acadoWorkspace.x[0] + acadoWorkspace.evGx[65]*acadoWorkspace.x[1] + acadoWorkspace.evGx[66]*acadoWorkspace.x[2] + acadoWorkspace.evGx[67]*acadoWorkspace.x[3] + acadoWorkspace.d[16]; +acadoVariables.x[21] += + acadoWorkspace.evGx[68]*acadoWorkspace.x[0] + acadoWorkspace.evGx[69]*acadoWorkspace.x[1] + acadoWorkspace.evGx[70]*acadoWorkspace.x[2] + acadoWorkspace.evGx[71]*acadoWorkspace.x[3] + acadoWorkspace.d[17]; +acadoVariables.x[22] += + acadoWorkspace.evGx[72]*acadoWorkspace.x[0] + acadoWorkspace.evGx[73]*acadoWorkspace.x[1] + acadoWorkspace.evGx[74]*acadoWorkspace.x[2] + acadoWorkspace.evGx[75]*acadoWorkspace.x[3] + acadoWorkspace.d[18]; +acadoVariables.x[23] += + acadoWorkspace.evGx[76]*acadoWorkspace.x[0] + acadoWorkspace.evGx[77]*acadoWorkspace.x[1] + acadoWorkspace.evGx[78]*acadoWorkspace.x[2] + acadoWorkspace.evGx[79]*acadoWorkspace.x[3] + acadoWorkspace.d[19]; +acadoVariables.x[24] += + acadoWorkspace.evGx[80]*acadoWorkspace.x[0] + acadoWorkspace.evGx[81]*acadoWorkspace.x[1] + acadoWorkspace.evGx[82]*acadoWorkspace.x[2] + acadoWorkspace.evGx[83]*acadoWorkspace.x[3] + acadoWorkspace.d[20]; +acadoVariables.x[25] += + acadoWorkspace.evGx[84]*acadoWorkspace.x[0] + acadoWorkspace.evGx[85]*acadoWorkspace.x[1] + acadoWorkspace.evGx[86]*acadoWorkspace.x[2] + acadoWorkspace.evGx[87]*acadoWorkspace.x[3] + acadoWorkspace.d[21]; +acadoVariables.x[26] += + acadoWorkspace.evGx[88]*acadoWorkspace.x[0] + acadoWorkspace.evGx[89]*acadoWorkspace.x[1] + acadoWorkspace.evGx[90]*acadoWorkspace.x[2] + acadoWorkspace.evGx[91]*acadoWorkspace.x[3] + acadoWorkspace.d[22]; +acadoVariables.x[27] += + acadoWorkspace.evGx[92]*acadoWorkspace.x[0] + acadoWorkspace.evGx[93]*acadoWorkspace.x[1] + acadoWorkspace.evGx[94]*acadoWorkspace.x[2] + acadoWorkspace.evGx[95]*acadoWorkspace.x[3] + acadoWorkspace.d[23]; +acadoVariables.x[28] += + acadoWorkspace.evGx[96]*acadoWorkspace.x[0] + acadoWorkspace.evGx[97]*acadoWorkspace.x[1] + acadoWorkspace.evGx[98]*acadoWorkspace.x[2] + acadoWorkspace.evGx[99]*acadoWorkspace.x[3] + acadoWorkspace.d[24]; +acadoVariables.x[29] += + acadoWorkspace.evGx[100]*acadoWorkspace.x[0] + acadoWorkspace.evGx[101]*acadoWorkspace.x[1] + acadoWorkspace.evGx[102]*acadoWorkspace.x[2] + acadoWorkspace.evGx[103]*acadoWorkspace.x[3] + acadoWorkspace.d[25]; +acadoVariables.x[30] += + acadoWorkspace.evGx[104]*acadoWorkspace.x[0] + acadoWorkspace.evGx[105]*acadoWorkspace.x[1] + acadoWorkspace.evGx[106]*acadoWorkspace.x[2] + acadoWorkspace.evGx[107]*acadoWorkspace.x[3] + acadoWorkspace.d[26]; +acadoVariables.x[31] += + acadoWorkspace.evGx[108]*acadoWorkspace.x[0] + acadoWorkspace.evGx[109]*acadoWorkspace.x[1] + acadoWorkspace.evGx[110]*acadoWorkspace.x[2] + acadoWorkspace.evGx[111]*acadoWorkspace.x[3] + acadoWorkspace.d[27]; +acadoVariables.x[32] += + acadoWorkspace.evGx[112]*acadoWorkspace.x[0] + acadoWorkspace.evGx[113]*acadoWorkspace.x[1] + acadoWorkspace.evGx[114]*acadoWorkspace.x[2] + acadoWorkspace.evGx[115]*acadoWorkspace.x[3] + acadoWorkspace.d[28]; +acadoVariables.x[33] += + acadoWorkspace.evGx[116]*acadoWorkspace.x[0] + acadoWorkspace.evGx[117]*acadoWorkspace.x[1] + acadoWorkspace.evGx[118]*acadoWorkspace.x[2] + acadoWorkspace.evGx[119]*acadoWorkspace.x[3] + acadoWorkspace.d[29]; +acadoVariables.x[34] += + acadoWorkspace.evGx[120]*acadoWorkspace.x[0] + acadoWorkspace.evGx[121]*acadoWorkspace.x[1] + acadoWorkspace.evGx[122]*acadoWorkspace.x[2] + acadoWorkspace.evGx[123]*acadoWorkspace.x[3] + acadoWorkspace.d[30]; +acadoVariables.x[35] += + acadoWorkspace.evGx[124]*acadoWorkspace.x[0] + acadoWorkspace.evGx[125]*acadoWorkspace.x[1] + acadoWorkspace.evGx[126]*acadoWorkspace.x[2] + acadoWorkspace.evGx[127]*acadoWorkspace.x[3] + acadoWorkspace.d[31]; +acadoVariables.x[36] += + acadoWorkspace.evGx[128]*acadoWorkspace.x[0] + acadoWorkspace.evGx[129]*acadoWorkspace.x[1] + acadoWorkspace.evGx[130]*acadoWorkspace.x[2] + acadoWorkspace.evGx[131]*acadoWorkspace.x[3] + acadoWorkspace.d[32]; +acadoVariables.x[37] += + acadoWorkspace.evGx[132]*acadoWorkspace.x[0] + acadoWorkspace.evGx[133]*acadoWorkspace.x[1] + acadoWorkspace.evGx[134]*acadoWorkspace.x[2] + acadoWorkspace.evGx[135]*acadoWorkspace.x[3] + acadoWorkspace.d[33]; +acadoVariables.x[38] += + acadoWorkspace.evGx[136]*acadoWorkspace.x[0] + acadoWorkspace.evGx[137]*acadoWorkspace.x[1] + acadoWorkspace.evGx[138]*acadoWorkspace.x[2] + acadoWorkspace.evGx[139]*acadoWorkspace.x[3] + acadoWorkspace.d[34]; +acadoVariables.x[39] += + acadoWorkspace.evGx[140]*acadoWorkspace.x[0] + acadoWorkspace.evGx[141]*acadoWorkspace.x[1] + acadoWorkspace.evGx[142]*acadoWorkspace.x[2] + acadoWorkspace.evGx[143]*acadoWorkspace.x[3] + acadoWorkspace.d[35]; +acadoVariables.x[40] += + acadoWorkspace.evGx[144]*acadoWorkspace.x[0] + acadoWorkspace.evGx[145]*acadoWorkspace.x[1] + acadoWorkspace.evGx[146]*acadoWorkspace.x[2] + acadoWorkspace.evGx[147]*acadoWorkspace.x[3] + acadoWorkspace.d[36]; +acadoVariables.x[41] += + acadoWorkspace.evGx[148]*acadoWorkspace.x[0] + acadoWorkspace.evGx[149]*acadoWorkspace.x[1] + acadoWorkspace.evGx[150]*acadoWorkspace.x[2] + acadoWorkspace.evGx[151]*acadoWorkspace.x[3] + acadoWorkspace.d[37]; +acadoVariables.x[42] += + acadoWorkspace.evGx[152]*acadoWorkspace.x[0] + acadoWorkspace.evGx[153]*acadoWorkspace.x[1] + acadoWorkspace.evGx[154]*acadoWorkspace.x[2] + acadoWorkspace.evGx[155]*acadoWorkspace.x[3] + acadoWorkspace.d[38]; +acadoVariables.x[43] += + acadoWorkspace.evGx[156]*acadoWorkspace.x[0] + acadoWorkspace.evGx[157]*acadoWorkspace.x[1] + acadoWorkspace.evGx[158]*acadoWorkspace.x[2] + acadoWorkspace.evGx[159]*acadoWorkspace.x[3] + acadoWorkspace.d[39]; +acadoVariables.x[44] += + acadoWorkspace.evGx[160]*acadoWorkspace.x[0] + acadoWorkspace.evGx[161]*acadoWorkspace.x[1] + acadoWorkspace.evGx[162]*acadoWorkspace.x[2] + acadoWorkspace.evGx[163]*acadoWorkspace.x[3] + acadoWorkspace.d[40]; +acadoVariables.x[45] += + acadoWorkspace.evGx[164]*acadoWorkspace.x[0] + acadoWorkspace.evGx[165]*acadoWorkspace.x[1] + acadoWorkspace.evGx[166]*acadoWorkspace.x[2] + acadoWorkspace.evGx[167]*acadoWorkspace.x[3] + acadoWorkspace.d[41]; +acadoVariables.x[46] += + acadoWorkspace.evGx[168]*acadoWorkspace.x[0] + acadoWorkspace.evGx[169]*acadoWorkspace.x[1] + acadoWorkspace.evGx[170]*acadoWorkspace.x[2] + acadoWorkspace.evGx[171]*acadoWorkspace.x[3] + acadoWorkspace.d[42]; +acadoVariables.x[47] += + acadoWorkspace.evGx[172]*acadoWorkspace.x[0] + acadoWorkspace.evGx[173]*acadoWorkspace.x[1] + acadoWorkspace.evGx[174]*acadoWorkspace.x[2] + acadoWorkspace.evGx[175]*acadoWorkspace.x[3] + acadoWorkspace.d[43]; +acadoVariables.x[48] += + acadoWorkspace.evGx[176]*acadoWorkspace.x[0] + acadoWorkspace.evGx[177]*acadoWorkspace.x[1] + acadoWorkspace.evGx[178]*acadoWorkspace.x[2] + acadoWorkspace.evGx[179]*acadoWorkspace.x[3] + acadoWorkspace.d[44]; +acadoVariables.x[49] += + acadoWorkspace.evGx[180]*acadoWorkspace.x[0] + acadoWorkspace.evGx[181]*acadoWorkspace.x[1] + acadoWorkspace.evGx[182]*acadoWorkspace.x[2] + acadoWorkspace.evGx[183]*acadoWorkspace.x[3] + acadoWorkspace.d[45]; +acadoVariables.x[50] += + acadoWorkspace.evGx[184]*acadoWorkspace.x[0] + acadoWorkspace.evGx[185]*acadoWorkspace.x[1] + acadoWorkspace.evGx[186]*acadoWorkspace.x[2] + acadoWorkspace.evGx[187]*acadoWorkspace.x[3] + acadoWorkspace.d[46]; +acadoVariables.x[51] += + acadoWorkspace.evGx[188]*acadoWorkspace.x[0] + acadoWorkspace.evGx[189]*acadoWorkspace.x[1] + acadoWorkspace.evGx[190]*acadoWorkspace.x[2] + acadoWorkspace.evGx[191]*acadoWorkspace.x[3] + acadoWorkspace.d[47]; +acadoVariables.x[52] += + acadoWorkspace.evGx[192]*acadoWorkspace.x[0] + acadoWorkspace.evGx[193]*acadoWorkspace.x[1] + acadoWorkspace.evGx[194]*acadoWorkspace.x[2] + acadoWorkspace.evGx[195]*acadoWorkspace.x[3] + acadoWorkspace.d[48]; +acadoVariables.x[53] += + acadoWorkspace.evGx[196]*acadoWorkspace.x[0] + acadoWorkspace.evGx[197]*acadoWorkspace.x[1] + acadoWorkspace.evGx[198]*acadoWorkspace.x[2] + acadoWorkspace.evGx[199]*acadoWorkspace.x[3] + acadoWorkspace.d[49]; +acadoVariables.x[54] += + acadoWorkspace.evGx[200]*acadoWorkspace.x[0] + acadoWorkspace.evGx[201]*acadoWorkspace.x[1] + acadoWorkspace.evGx[202]*acadoWorkspace.x[2] + acadoWorkspace.evGx[203]*acadoWorkspace.x[3] + acadoWorkspace.d[50]; +acadoVariables.x[55] += + acadoWorkspace.evGx[204]*acadoWorkspace.x[0] + acadoWorkspace.evGx[205]*acadoWorkspace.x[1] + acadoWorkspace.evGx[206]*acadoWorkspace.x[2] + acadoWorkspace.evGx[207]*acadoWorkspace.x[3] + acadoWorkspace.d[51]; +acadoVariables.x[56] += + acadoWorkspace.evGx[208]*acadoWorkspace.x[0] + acadoWorkspace.evGx[209]*acadoWorkspace.x[1] + acadoWorkspace.evGx[210]*acadoWorkspace.x[2] + acadoWorkspace.evGx[211]*acadoWorkspace.x[3] + acadoWorkspace.d[52]; +acadoVariables.x[57] += + acadoWorkspace.evGx[212]*acadoWorkspace.x[0] + acadoWorkspace.evGx[213]*acadoWorkspace.x[1] + acadoWorkspace.evGx[214]*acadoWorkspace.x[2] + acadoWorkspace.evGx[215]*acadoWorkspace.x[3] + acadoWorkspace.d[53]; +acadoVariables.x[58] += + acadoWorkspace.evGx[216]*acadoWorkspace.x[0] + acadoWorkspace.evGx[217]*acadoWorkspace.x[1] + acadoWorkspace.evGx[218]*acadoWorkspace.x[2] + acadoWorkspace.evGx[219]*acadoWorkspace.x[3] + acadoWorkspace.d[54]; +acadoVariables.x[59] += + acadoWorkspace.evGx[220]*acadoWorkspace.x[0] + acadoWorkspace.evGx[221]*acadoWorkspace.x[1] + acadoWorkspace.evGx[222]*acadoWorkspace.x[2] + acadoWorkspace.evGx[223]*acadoWorkspace.x[3] + acadoWorkspace.d[55]; +acadoVariables.x[60] += + acadoWorkspace.evGx[224]*acadoWorkspace.x[0] + acadoWorkspace.evGx[225]*acadoWorkspace.x[1] + acadoWorkspace.evGx[226]*acadoWorkspace.x[2] + acadoWorkspace.evGx[227]*acadoWorkspace.x[3] + acadoWorkspace.d[56]; +acadoVariables.x[61] += + acadoWorkspace.evGx[228]*acadoWorkspace.x[0] + acadoWorkspace.evGx[229]*acadoWorkspace.x[1] + acadoWorkspace.evGx[230]*acadoWorkspace.x[2] + acadoWorkspace.evGx[231]*acadoWorkspace.x[3] + acadoWorkspace.d[57]; +acadoVariables.x[62] += + acadoWorkspace.evGx[232]*acadoWorkspace.x[0] + acadoWorkspace.evGx[233]*acadoWorkspace.x[1] + acadoWorkspace.evGx[234]*acadoWorkspace.x[2] + acadoWorkspace.evGx[235]*acadoWorkspace.x[3] + acadoWorkspace.d[58]; +acadoVariables.x[63] += + acadoWorkspace.evGx[236]*acadoWorkspace.x[0] + acadoWorkspace.evGx[237]*acadoWorkspace.x[1] + acadoWorkspace.evGx[238]*acadoWorkspace.x[2] + acadoWorkspace.evGx[239]*acadoWorkspace.x[3] + acadoWorkspace.d[59]; +acadoVariables.x[64] += + acadoWorkspace.evGx[240]*acadoWorkspace.x[0] + acadoWorkspace.evGx[241]*acadoWorkspace.x[1] + acadoWorkspace.evGx[242]*acadoWorkspace.x[2] + acadoWorkspace.evGx[243]*acadoWorkspace.x[3] + acadoWorkspace.d[60]; +acadoVariables.x[65] += + acadoWorkspace.evGx[244]*acadoWorkspace.x[0] + acadoWorkspace.evGx[245]*acadoWorkspace.x[1] + acadoWorkspace.evGx[246]*acadoWorkspace.x[2] + acadoWorkspace.evGx[247]*acadoWorkspace.x[3] + acadoWorkspace.d[61]; +acadoVariables.x[66] += + acadoWorkspace.evGx[248]*acadoWorkspace.x[0] + acadoWorkspace.evGx[249]*acadoWorkspace.x[1] + acadoWorkspace.evGx[250]*acadoWorkspace.x[2] + acadoWorkspace.evGx[251]*acadoWorkspace.x[3] + acadoWorkspace.d[62]; +acadoVariables.x[67] += + acadoWorkspace.evGx[252]*acadoWorkspace.x[0] + acadoWorkspace.evGx[253]*acadoWorkspace.x[1] + acadoWorkspace.evGx[254]*acadoWorkspace.x[2] + acadoWorkspace.evGx[255]*acadoWorkspace.x[3] + acadoWorkspace.d[63]; +acadoVariables.x[68] += + acadoWorkspace.evGx[256]*acadoWorkspace.x[0] + acadoWorkspace.evGx[257]*acadoWorkspace.x[1] + acadoWorkspace.evGx[258]*acadoWorkspace.x[2] + acadoWorkspace.evGx[259]*acadoWorkspace.x[3] + acadoWorkspace.d[64]; +acadoVariables.x[69] += + acadoWorkspace.evGx[260]*acadoWorkspace.x[0] + acadoWorkspace.evGx[261]*acadoWorkspace.x[1] + acadoWorkspace.evGx[262]*acadoWorkspace.x[2] + acadoWorkspace.evGx[263]*acadoWorkspace.x[3] + acadoWorkspace.d[65]; +acadoVariables.x[70] += + acadoWorkspace.evGx[264]*acadoWorkspace.x[0] + acadoWorkspace.evGx[265]*acadoWorkspace.x[1] + acadoWorkspace.evGx[266]*acadoWorkspace.x[2] + acadoWorkspace.evGx[267]*acadoWorkspace.x[3] + acadoWorkspace.d[66]; +acadoVariables.x[71] += + acadoWorkspace.evGx[268]*acadoWorkspace.x[0] + acadoWorkspace.evGx[269]*acadoWorkspace.x[1] + acadoWorkspace.evGx[270]*acadoWorkspace.x[2] + acadoWorkspace.evGx[271]*acadoWorkspace.x[3] + acadoWorkspace.d[67]; +acadoVariables.x[72] += + acadoWorkspace.evGx[272]*acadoWorkspace.x[0] + acadoWorkspace.evGx[273]*acadoWorkspace.x[1] + acadoWorkspace.evGx[274]*acadoWorkspace.x[2] + acadoWorkspace.evGx[275]*acadoWorkspace.x[3] + acadoWorkspace.d[68]; +acadoVariables.x[73] += + acadoWorkspace.evGx[276]*acadoWorkspace.x[0] + acadoWorkspace.evGx[277]*acadoWorkspace.x[1] + acadoWorkspace.evGx[278]*acadoWorkspace.x[2] + acadoWorkspace.evGx[279]*acadoWorkspace.x[3] + acadoWorkspace.d[69]; +acadoVariables.x[74] += + acadoWorkspace.evGx[280]*acadoWorkspace.x[0] + acadoWorkspace.evGx[281]*acadoWorkspace.x[1] + acadoWorkspace.evGx[282]*acadoWorkspace.x[2] + acadoWorkspace.evGx[283]*acadoWorkspace.x[3] + acadoWorkspace.d[70]; +acadoVariables.x[75] += + acadoWorkspace.evGx[284]*acadoWorkspace.x[0] + acadoWorkspace.evGx[285]*acadoWorkspace.x[1] + acadoWorkspace.evGx[286]*acadoWorkspace.x[2] + acadoWorkspace.evGx[287]*acadoWorkspace.x[3] + acadoWorkspace.d[71]; +acadoVariables.x[76] += + acadoWorkspace.evGx[288]*acadoWorkspace.x[0] + acadoWorkspace.evGx[289]*acadoWorkspace.x[1] + acadoWorkspace.evGx[290]*acadoWorkspace.x[2] + acadoWorkspace.evGx[291]*acadoWorkspace.x[3] + acadoWorkspace.d[72]; +acadoVariables.x[77] += + acadoWorkspace.evGx[292]*acadoWorkspace.x[0] + acadoWorkspace.evGx[293]*acadoWorkspace.x[1] + acadoWorkspace.evGx[294]*acadoWorkspace.x[2] + acadoWorkspace.evGx[295]*acadoWorkspace.x[3] + acadoWorkspace.d[73]; +acadoVariables.x[78] += + acadoWorkspace.evGx[296]*acadoWorkspace.x[0] + acadoWorkspace.evGx[297]*acadoWorkspace.x[1] + acadoWorkspace.evGx[298]*acadoWorkspace.x[2] + acadoWorkspace.evGx[299]*acadoWorkspace.x[3] + acadoWorkspace.d[74]; +acadoVariables.x[79] += + acadoWorkspace.evGx[300]*acadoWorkspace.x[0] + acadoWorkspace.evGx[301]*acadoWorkspace.x[1] + acadoWorkspace.evGx[302]*acadoWorkspace.x[2] + acadoWorkspace.evGx[303]*acadoWorkspace.x[3] + acadoWorkspace.d[75]; +acadoVariables.x[80] += + acadoWorkspace.evGx[304]*acadoWorkspace.x[0] + acadoWorkspace.evGx[305]*acadoWorkspace.x[1] + acadoWorkspace.evGx[306]*acadoWorkspace.x[2] + acadoWorkspace.evGx[307]*acadoWorkspace.x[3] + acadoWorkspace.d[76]; +acadoVariables.x[81] += + acadoWorkspace.evGx[308]*acadoWorkspace.x[0] + acadoWorkspace.evGx[309]*acadoWorkspace.x[1] + acadoWorkspace.evGx[310]*acadoWorkspace.x[2] + acadoWorkspace.evGx[311]*acadoWorkspace.x[3] + acadoWorkspace.d[77]; +acadoVariables.x[82] += + acadoWorkspace.evGx[312]*acadoWorkspace.x[0] + acadoWorkspace.evGx[313]*acadoWorkspace.x[1] + acadoWorkspace.evGx[314]*acadoWorkspace.x[2] + acadoWorkspace.evGx[315]*acadoWorkspace.x[3] + acadoWorkspace.d[78]; +acadoVariables.x[83] += + acadoWorkspace.evGx[316]*acadoWorkspace.x[0] + acadoWorkspace.evGx[317]*acadoWorkspace.x[1] + acadoWorkspace.evGx[318]*acadoWorkspace.x[2] + acadoWorkspace.evGx[319]*acadoWorkspace.x[3] + acadoWorkspace.d[79]; + +acado_multEDu( acadoWorkspace.E, &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 4 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 8 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 8 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 12 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 12 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 12 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 16 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 16 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 16 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 16 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 20 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 20 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 20 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 20 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 20 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 68 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 68 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 68 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 68 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 68 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 68 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 68 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 68 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 68 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 68 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 68 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 68 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 68 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 68 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 68 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 604 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 68 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 608 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 68 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 676 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 680 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 752 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 76 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 832 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 80 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 836 ]), &(acadoWorkspace.x[ 23 ]), &(acadoVariables.x[ 80 ]) ); +} + +int acado_preparationStep( ) +{ +int ret; + +ret = acado_modelSimulation(); +acado_evaluateObjective( ); +acado_condensePrep( ); +return ret; +} + +int acado_feedbackStep( ) +{ +int tmp; + +acado_condenseFdb( ); + +tmp = acado_solve( ); + +acado_expand( ); +return tmp; +} + +int acado_initializeSolver( ) +{ +int ret; + +/* This is a function which must be called once before any other function call! */ + + +ret = 0; + +memset(&acadoWorkspace, 0, sizeof( acadoWorkspace )); +return ret; +} + +void acado_initializeNodesByForwardSimulation( ) +{ +int index; +for (index = 0; index < 20; ++index) +{ +acadoWorkspace.state[0] = acadoVariables.x[index * 4]; +acadoWorkspace.state[1] = acadoVariables.x[index * 4 + 1]; +acadoWorkspace.state[2] = acadoVariables.x[index * 4 + 2]; +acadoWorkspace.state[3] = acadoVariables.x[index * 4 + 3]; +acadoWorkspace.state[24] = acadoVariables.u[index]; +acadoWorkspace.state[25] = acadoVariables.od[index * 12]; +acadoWorkspace.state[26] = acadoVariables.od[index * 12 + 1]; +acadoWorkspace.state[27] = acadoVariables.od[index * 12 + 2]; +acadoWorkspace.state[28] = acadoVariables.od[index * 12 + 3]; +acadoWorkspace.state[29] = acadoVariables.od[index * 12 + 4]; +acadoWorkspace.state[30] = acadoVariables.od[index * 12 + 5]; +acadoWorkspace.state[31] = acadoVariables.od[index * 12 + 6]; +acadoWorkspace.state[32] = acadoVariables.od[index * 12 + 7]; +acadoWorkspace.state[33] = acadoVariables.od[index * 12 + 8]; +acadoWorkspace.state[34] = acadoVariables.od[index * 12 + 9]; +acadoWorkspace.state[35] = acadoVariables.od[index * 12 + 10]; +acadoWorkspace.state[36] = acadoVariables.od[index * 12 + 11]; + +acado_integrate(acadoWorkspace.state, index == 0, index); + +acadoVariables.x[index * 4 + 4] = acadoWorkspace.state[0]; +acadoVariables.x[index * 4 + 5] = acadoWorkspace.state[1]; +acadoVariables.x[index * 4 + 6] = acadoWorkspace.state[2]; +acadoVariables.x[index * 4 + 7] = acadoWorkspace.state[3]; +} +} + +void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd ) +{ +int index; +for (index = 0; index < 20; ++index) +{ +acadoVariables.x[index * 4] = acadoVariables.x[index * 4 + 4]; +acadoVariables.x[index * 4 + 1] = acadoVariables.x[index * 4 + 5]; +acadoVariables.x[index * 4 + 2] = acadoVariables.x[index * 4 + 6]; +acadoVariables.x[index * 4 + 3] = acadoVariables.x[index * 4 + 7]; +} + +if (strategy == 1 && xEnd != 0) +{ +acadoVariables.x[80] = xEnd[0]; +acadoVariables.x[81] = xEnd[1]; +acadoVariables.x[82] = xEnd[2]; +acadoVariables.x[83] = xEnd[3]; +} +else if (strategy == 2) +{ +acadoWorkspace.state[0] = acadoVariables.x[80]; +acadoWorkspace.state[1] = acadoVariables.x[81]; +acadoWorkspace.state[2] = acadoVariables.x[82]; +acadoWorkspace.state[3] = acadoVariables.x[83]; +if (uEnd != 0) +{ +acadoWorkspace.state[24] = uEnd[0]; +} +else +{ +acadoWorkspace.state[24] = acadoVariables.u[19]; +} +acadoWorkspace.state[25] = acadoVariables.od[240]; +acadoWorkspace.state[26] = acadoVariables.od[241]; +acadoWorkspace.state[27] = acadoVariables.od[242]; +acadoWorkspace.state[28] = acadoVariables.od[243]; +acadoWorkspace.state[29] = acadoVariables.od[244]; +acadoWorkspace.state[30] = acadoVariables.od[245]; +acadoWorkspace.state[31] = acadoVariables.od[246]; +acadoWorkspace.state[32] = acadoVariables.od[247]; +acadoWorkspace.state[33] = acadoVariables.od[248]; +acadoWorkspace.state[34] = acadoVariables.od[249]; +acadoWorkspace.state[35] = acadoVariables.od[250]; +acadoWorkspace.state[36] = acadoVariables.od[251]; + +acado_integrate(acadoWorkspace.state, 1, 19); + +acadoVariables.x[80] = acadoWorkspace.state[0]; +acadoVariables.x[81] = acadoWorkspace.state[1]; +acadoVariables.x[82] = acadoWorkspace.state[2]; +acadoVariables.x[83] = acadoWorkspace.state[3]; +} +} + +void acado_shiftControls( real_t* const uEnd ) +{ +int index; +for (index = 0; index < 19; ++index) +{ +acadoVariables.u[index] = acadoVariables.u[index + 1]; +} + +if (uEnd != 0) +{ +acadoVariables.u[19] = uEnd[0]; +} +} + +real_t acado_getKKT( ) +{ +real_t kkt; + +int index; +real_t prd; + +kkt = + acadoWorkspace.g[0]*acadoWorkspace.x[0] + acadoWorkspace.g[1]*acadoWorkspace.x[1] + acadoWorkspace.g[2]*acadoWorkspace.x[2] + acadoWorkspace.g[3]*acadoWorkspace.x[3] + acadoWorkspace.g[4]*acadoWorkspace.x[4] + acadoWorkspace.g[5]*acadoWorkspace.x[5] + acadoWorkspace.g[6]*acadoWorkspace.x[6] + acadoWorkspace.g[7]*acadoWorkspace.x[7] + acadoWorkspace.g[8]*acadoWorkspace.x[8] + acadoWorkspace.g[9]*acadoWorkspace.x[9] + acadoWorkspace.g[10]*acadoWorkspace.x[10] + acadoWorkspace.g[11]*acadoWorkspace.x[11] + acadoWorkspace.g[12]*acadoWorkspace.x[12] + acadoWorkspace.g[13]*acadoWorkspace.x[13] + acadoWorkspace.g[14]*acadoWorkspace.x[14] + acadoWorkspace.g[15]*acadoWorkspace.x[15] + acadoWorkspace.g[16]*acadoWorkspace.x[16] + acadoWorkspace.g[17]*acadoWorkspace.x[17] + acadoWorkspace.g[18]*acadoWorkspace.x[18] + acadoWorkspace.g[19]*acadoWorkspace.x[19] + acadoWorkspace.g[20]*acadoWorkspace.x[20] + acadoWorkspace.g[21]*acadoWorkspace.x[21] + acadoWorkspace.g[22]*acadoWorkspace.x[22] + acadoWorkspace.g[23]*acadoWorkspace.x[23]; +kkt = fabs( kkt ); +for (index = 0; index < 24; ++index) +{ +prd = acadoWorkspace.y[index]; +if (prd > 1e-12) +kkt += fabs(acadoWorkspace.lb[index] * prd); +else if (prd < -1e-12) +kkt += fabs(acadoWorkspace.ub[index] * prd); +} +return kkt; +} + +real_t acado_getObjective( ) +{ +real_t objVal; + +int lRun1; +/** Row vector of size: 5 */ +real_t tmpDy[ 5 ]; + +/** Row vector of size: 4 */ +real_t tmpDyN[ 4 ]; + +for (lRun1 = 0; lRun1 < 20; ++lRun1) +{ +acadoWorkspace.objValueIn[0] = acadoVariables.x[lRun1 * 4]; +acadoWorkspace.objValueIn[1] = acadoVariables.x[lRun1 * 4 + 1]; +acadoWorkspace.objValueIn[2] = acadoVariables.x[lRun1 * 4 + 2]; +acadoWorkspace.objValueIn[3] = acadoVariables.x[lRun1 * 4 + 3]; +acadoWorkspace.objValueIn[4] = acadoVariables.u[lRun1]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 12]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 12 + 1]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 12 + 2]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 12 + 3]; +acadoWorkspace.objValueIn[9] = acadoVariables.od[lRun1 * 12 + 4]; +acadoWorkspace.objValueIn[10] = acadoVariables.od[lRun1 * 12 + 5]; +acadoWorkspace.objValueIn[11] = acadoVariables.od[lRun1 * 12 + 6]; +acadoWorkspace.objValueIn[12] = acadoVariables.od[lRun1 * 12 + 7]; +acadoWorkspace.objValueIn[13] = acadoVariables.od[lRun1 * 12 + 8]; +acadoWorkspace.objValueIn[14] = acadoVariables.od[lRun1 * 12 + 9]; +acadoWorkspace.objValueIn[15] = acadoVariables.od[lRun1 * 12 + 10]; +acadoWorkspace.objValueIn[16] = acadoVariables.od[lRun1 * 12 + 11]; + +acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acadoWorkspace.Dy[lRun1 * 5] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 5]; +acadoWorkspace.Dy[lRun1 * 5 + 1] = acadoWorkspace.objValueOut[1] - acadoVariables.y[lRun1 * 5 + 1]; +acadoWorkspace.Dy[lRun1 * 5 + 2] = acadoWorkspace.objValueOut[2] - acadoVariables.y[lRun1 * 5 + 2]; +acadoWorkspace.Dy[lRun1 * 5 + 3] = acadoWorkspace.objValueOut[3] - acadoVariables.y[lRun1 * 5 + 3]; +acadoWorkspace.Dy[lRun1 * 5 + 4] = acadoWorkspace.objValueOut[4] - acadoVariables.y[lRun1 * 5 + 4]; +} +acadoWorkspace.objValueIn[0] = acadoVariables.x[80]; +acadoWorkspace.objValueIn[1] = acadoVariables.x[81]; +acadoWorkspace.objValueIn[2] = acadoVariables.x[82]; +acadoWorkspace.objValueIn[3] = acadoVariables.x[83]; +acadoWorkspace.objValueIn[4] = acadoVariables.od[240]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[241]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[242]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[243]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[244]; +acadoWorkspace.objValueIn[9] = acadoVariables.od[245]; +acadoWorkspace.objValueIn[10] = acadoVariables.od[246]; +acadoWorkspace.objValueIn[11] = acadoVariables.od[247]; +acadoWorkspace.objValueIn[12] = acadoVariables.od[248]; +acadoWorkspace.objValueIn[13] = acadoVariables.od[249]; +acadoWorkspace.objValueIn[14] = acadoVariables.od[250]; +acadoWorkspace.objValueIn[15] = acadoVariables.od[251]; +acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0]; +acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1]; +acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2] - acadoVariables.yN[2]; +acadoWorkspace.DyN[3] = acadoWorkspace.objValueOut[3] - acadoVariables.yN[3]; +objVal = 0.0000000000000000e+00; +for (lRun1 = 0; lRun1 < 20; ++lRun1) +{ +tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 5] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 10] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 15] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 20]; +tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25 + 1] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 6] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 11] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 16] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 21]; +tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25 + 2] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 7] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 12] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 17] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 22]; +tmpDy[3] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25 + 3] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 8] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 13] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 18] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 23]; +tmpDy[4] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25 + 4] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 9] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 14] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 19] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 24]; +objVal += + acadoWorkspace.Dy[lRun1 * 5]*tmpDy[0] + acadoWorkspace.Dy[lRun1 * 5 + 1]*tmpDy[1] + acadoWorkspace.Dy[lRun1 * 5 + 2]*tmpDy[2] + acadoWorkspace.Dy[lRun1 * 5 + 3]*tmpDy[3] + acadoWorkspace.Dy[lRun1 * 5 + 4]*tmpDy[4]; +} + +tmpDyN[0] = + acadoWorkspace.DyN[0]*acadoVariables.WN[0] + acadoWorkspace.DyN[1]*acadoVariables.WN[4] + acadoWorkspace.DyN[2]*acadoVariables.WN[8] + acadoWorkspace.DyN[3]*acadoVariables.WN[12]; +tmpDyN[1] = + acadoWorkspace.DyN[0]*acadoVariables.WN[1] + acadoWorkspace.DyN[1]*acadoVariables.WN[5] + acadoWorkspace.DyN[2]*acadoVariables.WN[9] + acadoWorkspace.DyN[3]*acadoVariables.WN[13]; +tmpDyN[2] = + acadoWorkspace.DyN[0]*acadoVariables.WN[2] + acadoWorkspace.DyN[1]*acadoVariables.WN[6] + acadoWorkspace.DyN[2]*acadoVariables.WN[10] + acadoWorkspace.DyN[3]*acadoVariables.WN[14]; +tmpDyN[3] = + acadoWorkspace.DyN[0]*acadoVariables.WN[3] + acadoWorkspace.DyN[1]*acadoVariables.WN[7] + acadoWorkspace.DyN[2]*acadoVariables.WN[11] + acadoWorkspace.DyN[3]*acadoVariables.WN[15]; +objVal += + acadoWorkspace.DyN[0]*tmpDyN[0] + acadoWorkspace.DyN[1]*tmpDyN[1] + acadoWorkspace.DyN[2]*tmpDyN[2] + acadoWorkspace.DyN[3]*tmpDyN[3]; + +objVal *= 0.5; +return objVal; +} + diff --git a/selfdrive/controls/lib/longitudinal_mpc_model/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc_model/libmpc_py.py new file mode 100644 index 00000000..a25dbb66 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc_model/libmpc_py.py @@ -0,0 +1,32 @@ +import os + +from cffi import FFI +from common.ffi_wrapper import suffix + +mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__))) +libmpc_fn = os.path.join(mpc_dir, "libmpc"+suffix()) + +ffi = FFI() +ffi.cdef(""" + +typedef struct { +double x_ego, v_ego, a_ego; +} state_t; + + +typedef struct { +double x_ego[21]; +double v_ego[21]; +double a_ego[21]; +double t[21]; +double j_ego[20]; +double cost; +} log_t; + + +void init(double xCost, double vCost, double aCost, double accelCost, double jerkCost); +void init_with_simulation(double v_ego); +int run_mpc(state_t * x0, log_t * solution, double x_poly[4], double v_poly[4], double a_poly[4]); +""") + +libmpc = ffi.dlopen(libmpc_fn) diff --git a/selfdrive/controls/lib/longitudinal_mpc_model/longitudinal_mpc.c b/selfdrive/controls/lib/longitudinal_mpc_model/longitudinal_mpc.c new file mode 100644 index 00000000..39cff8f2 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc_model/longitudinal_mpc.c @@ -0,0 +1,141 @@ +#include "acado_common.h" +#include "acado_auxiliary_functions.h" + +#include +#include + +#define NX ACADO_NX /* Number of differential state variables. */ +#define NXA ACADO_NXA /* Number of algebraic variables. */ +#define NU ACADO_NU /* Number of control inputs. */ +#define NOD ACADO_NOD /* Number of online data values. */ + +#define NY ACADO_NY /* Number of measurements/references on nodes 0..N - 1. */ +#define NYN ACADO_NYN /* Number of measurements/references on node N. */ + +#define N ACADO_N /* Number of intervals in the horizon. */ + +ACADOvariables acadoVariables; +ACADOworkspace acadoWorkspace; + +typedef struct { + double x_ego, v_ego, a_ego; +} state_t; + + +typedef struct { + double x_ego[N+1]; + double v_ego[N+1]; + double a_ego[N+1]; + double t[N+1]; + double j_ego[N]; + double cost; +} log_t; + +void init(double xCost, double vCost, double aCost, double accelCost, double jerkCost){ + acado_initializeSolver(); + int i; + const int STEP_MULTIPLIER = 3; + + /* Initialize the states and controls. */ + for (i = 0; i < NX * (N + 1); ++i) acadoVariables.x[ i ] = 0.0; + for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.0; + + /* Initialize the measurements/reference. */ + for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0; + for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0; + + /* MPC: initialize the current state feedback. */ + for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; + // Set weights + + for (i = 0; i < N; i++) { + int f = 1; + if (i > 4){ + f = STEP_MULTIPLIER; + } + // Setup diagonal entries + acadoVariables.W[NY*NY*i + (NY+1)*0] = xCost * f; + acadoVariables.W[NY*NY*i + (NY+1)*1] = vCost * f; + acadoVariables.W[NY*NY*i + (NY+1)*2] = aCost * f; + acadoVariables.W[NY*NY*i + (NY+1)*3] = accelCost * f; + acadoVariables.W[NY*NY*i + (NY+1)*4] = jerkCost * f; + } + acadoVariables.WN[(NYN+1)*0] = xCost * STEP_MULTIPLIER; + acadoVariables.WN[(NYN+1)*1] = vCost * STEP_MULTIPLIER; + acadoVariables.WN[(NYN+1)*2] = aCost * STEP_MULTIPLIER; + acadoVariables.WN[(NYN+1)*3] = accelCost * STEP_MULTIPLIER; + +} + +void init_with_simulation(double v_ego){ + int i; + + double x_ego = 0.0; + + double dt = 0.2; + double t = 0.0; + + for (i = 0; i < N + 1; ++i){ + if (i > 4){ + dt = 0.6; + } + + acadoVariables.x[i*NX] = x_ego; + acadoVariables.x[i*NX+1] = v_ego; + acadoVariables.x[i*NX+2] = 0; + acadoVariables.x[i*NX+3] = t; + + x_ego += v_ego * dt; + t += dt; + } + + for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.0; + for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0; + for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0; +} + +int run_mpc(state_t * x0, log_t * solution, + double x_poly[4], double v_poly[4], double a_poly[4]){ + int i; + + for (i = 0; i < N + 1; ++i){ + acadoVariables.od[i*NOD+0] = x_poly[0]; + acadoVariables.od[i*NOD+1] = x_poly[1]; + acadoVariables.od[i*NOD+2] = x_poly[2]; + acadoVariables.od[i*NOD+3] = x_poly[3]; + + acadoVariables.od[i*NOD+4] = v_poly[0]; + acadoVariables.od[i*NOD+5] = v_poly[1]; + acadoVariables.od[i*NOD+6] = v_poly[2]; + acadoVariables.od[i*NOD+7] = v_poly[3]; + + acadoVariables.od[i*NOD+8] = a_poly[0]; + acadoVariables.od[i*NOD+9] = a_poly[1]; + acadoVariables.od[i*NOD+10] = a_poly[2]; + acadoVariables.od[i*NOD+11] = a_poly[3]; + } + + acadoVariables.x[0] = acadoVariables.x0[0] = x0->x_ego; + acadoVariables.x[1] = acadoVariables.x0[1] = x0->v_ego; + acadoVariables.x[2] = acadoVariables.x0[2] = x0->a_ego; + acadoVariables.x[3] = acadoVariables.x0[3] = 0; + + acado_preparationStep(); + acado_feedbackStep(); + + for (i = 0; i <= N; i++){ + solution->x_ego[i] = acadoVariables.x[i*NX]; + solution->v_ego[i] = acadoVariables.x[i*NX+1]; + solution->a_ego[i] = acadoVariables.x[i*NX+2]; + solution->t[i] = acadoVariables.x[i*NX+3]; + + if (i < N){ + solution->j_ego[i] = acadoVariables.u[i]; + } + } + solution->cost = acado_getObjective(); + + // Dont shift states here. Current solution is closer to next timestep than if + // we shift by 0.1 seconds. + return acado_getNWSR(); +} diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 0e8a4eff..413d71d3 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -126,8 +126,8 @@ class PathPlanner(): # 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 over .2s + self.lane_change_ll_prob = max(self.lane_change_ll_prob - DT_MDL/5, 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,10 +154,6 @@ class PathPlanner(): 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) - 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) # account for actuation delay diff --git a/selfdrive/controls/tests/test_alerts.py b/selfdrive/controls/tests/test_alerts.py new file mode 100755 index 00000000..faea3c06 --- /dev/null +++ b/selfdrive/controls/tests/test_alerts.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 +import os +import unittest +from PIL import Image, ImageDraw, ImageFont + +from cereal import log +from common.basedir import BASEDIR +from selfdrive.controls.lib.alerts import ALERTS + +AlertSize = log.ControlsState.AlertSize + +FONT_PATH = os.path.join(BASEDIR, "selfdrive/assets/fonts") +REGULAR_FONT_PATH = os.path.join(FONT_PATH, "opensans_semibold.ttf") +BOLD_FONT_PATH = os.path.join(FONT_PATH, "opensans_semibold.ttf") +SEMIBOLD_FONT_PATH = os.path.join(FONT_PATH, "opensans_semibold.ttf") + +MAX_TEXT_WIDTH = 1920 - 300 # full screen width is useable, minus sidebar +# TODO: get exact scale factor. found this empirically, works well enough +FONT_SIZE_SCALE = 1.85 # factor to scale from nanovg units to PIL + +class TestAlerts(unittest.TestCase): + + # ensure alert text doesn't exceed allowed width + def test_alert_text_length(self): + draw = ImageDraw.Draw(Image.new('RGB', (0, 0))) + + fonts = { + AlertSize.small: [ImageFont.truetype(SEMIBOLD_FONT_PATH, int(40*FONT_SIZE_SCALE))], + AlertSize.mid: [ImageFont.truetype(BOLD_FONT_PATH, int(48*FONT_SIZE_SCALE)), + ImageFont.truetype(REGULAR_FONT_PATH, int(36*FONT_SIZE_SCALE))], + } + + for alert in ALERTS: + # for full size alerts, both text fields wrap the text, + # so it's unlikely that they would go past the max width + if alert.alert_size in [AlertSize.none, AlertSize.full]: + continue + + for i, txt in enumerate([alert.alert_text_1, alert.alert_text_2]): + if i >= len(fonts[alert.alert_size]): break + + font = fonts[alert.alert_size][i] + w, h = draw.textsize(txt, font) + msg = "type: %s msg: %s" % (alert.alert_type, txt) + self.assertLessEqual(w, MAX_TEXT_WIDTH, msg=msg) + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 8dfff81a..3c1f3932 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/controls/tests/test_monitoring.py b/selfdrive/controls/tests/test_monitoring.py index af44dc33..163766f4 100644 --- a/selfdrive/controls/tests/test_monitoring.py +++ b/selfdrive/controls/tests/test_monitoring.py @@ -6,7 +6,6 @@ from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALE _AWARENESS_PROMPT_TIME_TILL_TERMINAL, _DISTRACTED_TIME, \ _DISTRACTED_PRE_TIME_TILL_TERMINAL, _DISTRACTED_PROMPT_TIME_TILL_TERMINAL, \ _POSESTD_THRESHOLD, _HI_STD_TIMEOUT -from selfdrive.controls.lib.gps_helpers import is_rhd_region _TEST_TIMESPAN = 120 # seconds _DISTRACTED_SECONDS_TO_ORANGE = _DISTRACTED_TIME - _DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1 @@ -70,19 +69,6 @@ def run_DState_seq(driver_state_msgs, driver_car_interaction, openpilot_status, return events_from_DM, DS class TestMonitoring(unittest.TestCase): - # -1. rhd parser sanity check - def test_rhd_parser(self): - cities = [[32.7, -117.1, 0],\ - [51.5, 0.129, 1],\ - [35.7, 139.7, 1],\ - [-37.8, 144.9, 1],\ - [32.1, 41.74, 0],\ - [55.7, 12.69, 0]] - result = [] - for city in cities: - result.append(int(is_rhd_region(city[0],city[1]))) - self.assertEqual(result,[int(city[2]) for city in cities]) - # 0. op engaged, driver is doing fine all the time def test_fully_aware_driver(self): events_output = run_DState_seq(always_attentive, always_false, always_true, always_false)[0] diff --git a/selfdrive/crash.py b/selfdrive/crash.py index 74a7094a..a8cb758a 100644 --- a/selfdrive/crash.py +++ b/selfdrive/crash.py @@ -40,7 +40,7 @@ else: __excepthook__ = sys.excepthook def handle_exception(*exc_info): if exc_info[0] not in (KeyboardInterrupt, SystemExit): - capture_exception(exc_info=exc_info) + capture_exception() __excepthook__(*exc_info) sys.excepthook = handle_exception diff --git a/selfdrive/debug/check_lag.py b/selfdrive/debug/check_lag.py new file mode 100755 index 00000000..418f1f87 --- /dev/null +++ b/selfdrive/debug/check_lag.py @@ -0,0 +1,26 @@ +#!/usr/bin/env python3 +import cereal.messaging as messaging +from cereal.services import service_list + +TO_CHECK = ['carState'] + + +if __name__ == "__main__": + sm = messaging.SubMaster(TO_CHECK) + + prev_t = {} + + while True: + sm.update() + + for s in TO_CHECK: + if sm.updated[s]: + t = sm.logMonoTime[s] / 1e9 + + if s in prev_t: + expected = 1.0 / (service_list[s].frequency) + dt = t - prev_t[s] + if dt > 10 * expected: + print(t, s, dt) + + prev_t[s] = t diff --git a/selfdrive/debug/filter_log_message.py b/selfdrive/debug/filter_log_message.py index 8ddb12e2..162a6381 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 @@ LEVELS = { "CRITICAL": 50, } +ANDROID_LOG_SOURCE = { + 0: "MAIN", + 1: "RADIO", + 2: "EVENTS", + 3: "SYSTEM", + 4: "CRASH", + 5: "KERNEL", +} + if __name__ == "__main__": @@ -23,20 +31,24 @@ if __name__ == "__main__": 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 log['levelnum'] >= min_level: - print(f"{log['filename']}:{log.get('lineno', '')} - {log.get('funcname', '')}: {log['msg']}") + 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 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/debug/mpc/longitudinal_mpc_model.py b/selfdrive/debug/mpc/longitudinal_mpc_model.py new file mode 100755 index 00000000..1a1c00c6 --- /dev/null +++ b/selfdrive/debug/mpc/longitudinal_mpc_model.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python3 +import numpy as np + +import matplotlib.pyplot as plt + +from selfdrive.controls.lib.longitudinal_mpc_model import libmpc_py + +libmpc = libmpc_py.libmpc + +dt = 1 +speeds = [6.109375, 5.9765625, 6.6367188, 7.6875, 8.7578125, 9.4375, 10.21875, 11.070312, 11.679688, 12.21875] +accelerations = [0.15405273, 0.39575195, 0.36669922, 0.29248047, 0.27856445, 0.27832031, 0.29736328, 0.22705078, 0.16003418, 0.15185547] +ts = [t * dt for t in range(len(speeds))] + +# TODO: Get from actual model packet +x = 0.0 +positions = [] +for v in speeds: + positions.append(x) + x += v * dt + + +# Polyfit trajectories +x_poly = list(map(float, np.polyfit(ts, positions, 3))) +v_poly = list(map(float, np.polyfit(ts, speeds, 3))) +a_poly = list(map(float, np.polyfit(ts, accelerations, 3))) + +x_poly = libmpc_py.ffi.new("double[4]", x_poly) +v_poly = libmpc_py.ffi.new("double[4]", v_poly) +a_poly = libmpc_py.ffi.new("double[4]", a_poly) + +cur_state = libmpc_py.ffi.new("state_t *") +cur_state[0].x_ego = 0 +cur_state[0].v_ego = 10 +cur_state[0].a_ego = 0 + +libmpc.init(1.0, 1.0, 1.0, 1.0, 1.0) + +mpc_solution = libmpc_py.ffi.new("log_t *") +libmpc.init_with_simulation(cur_state[0].v_ego) + +libmpc.run_mpc(cur_state, mpc_solution, x_poly, v_poly, a_poly) + +# Converge to solution +for _ in range(10): + libmpc.run_mpc(cur_state, mpc_solution, x_poly, v_poly, a_poly) + + +ts_sol = list(mpc_solution[0].t) +x_sol = list(mpc_solution[0].x_ego) +v_sol = list(mpc_solution[0].v_ego) +a_sol = list(mpc_solution[0].a_ego) + + +plt.figure() +plt.subplot(3, 1, 1) +plt.plot(ts, positions, 'k--') +plt.plot(ts_sol, x_sol) +plt.ylabel('Position [m]') +plt.xlabel('Time [s]') + +plt.subplot(3, 1, 2) +plt.plot(ts, speeds, 'k--') +plt.plot(ts_sol, v_sol) +plt.xlabel('Time [s]') +plt.ylabel('Speed [m/s]') + +plt.subplot(3, 1, 3) +plt.plot(ts, accelerations, 'k--') +plt.plot(ts_sol, a_sol) + +plt.xlabel('Time [s]') +plt.ylabel('Acceleration [m/s^2]') + +plt.show() diff --git a/selfdrive/debug/mpc/tune_longitudinal.py b/selfdrive/debug/mpc/tune_longitudinal.py index 0af44463..ed3eb637 100755 --- a/selfdrive/debug/mpc/tune_longitudinal.py +++ b/selfdrive/debug/mpc/tune_longitudinal.py @@ -3,7 +3,6 @@ import numpy as np import matplotlib.pyplot as plt from selfdrive.controls.lib.longitudinal_mpc import libmpc_py from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG -import math # plot liongitudinal MPC trajectory by defining boundary conditions: # ego and lead vehicles state. Use this script to tune MPC costs @@ -16,7 +15,7 @@ def RW(v_ego, v_l): def NORM_RW_ERROR(v_ego, v_l, p): return (RW(v_ego, v_l) + 4.0 - p) - return (RW(v_ego, v_l) + 4.0 - p) / (np.sqrt(v_ego + 0.5) + 0.1) + #return (RW(v_ego, v_l) + 4.0 - p) / (np.sqrt(v_ego + 0.5) + 0.1) v_ego = 20.0 @@ -36,7 +35,7 @@ a_lead_tau = 0. # a_lead_tau = 2.90729817665 -min_a_lead_tau = (a_lead**2 * math.pi) / (2 * (v_lead + 0.01)**2) +#min_a_lead_tau = (a_lead**2 * math.pi) / (2 * (v_lead + 0.01)**2) min_a_lead_tau = 0.0 print(a_lead_tau, min_a_lead_tau) diff --git a/selfdrive/debug/tuner.py b/selfdrive/debug/tuner.py deleted file mode 100755 index b8a023ba..00000000 --- a/selfdrive/debug/tuner.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/env python3 -""" -This tool can be used to quickly changes the values in a JSON file used for tuning -Keys like in vim: - - h: decrease by 0.05 - - l: increase by 0.05 - - k: move pointer up - - j: move pointer down -""" - -import tty -import sys -import json -import termios -from collections import OrderedDict - -FILENAME = '/data/tuning.json' - -def read_tuning(): - while True: - try: - return json.loads(open(FILENAME).read()) - except: - pass - -def main(): - dat = json.loads(open(FILENAME, 'r').read()) - dat = OrderedDict(sorted(dat.items(), key=lambda i: i[0])) - - cur = 0 - while True: - sys.stdout.write("\x1Bc") - - for i, (k, v) in enumerate(dat.items()): - prefix = "> " if cur == i else " " - print((prefix + k).ljust(20) + "%.2f" % v) - - key = sys.stdin.read(1)[0] - - write = False - if key == "k": - cur = max(0, cur - 1) - elif key == "j": - cur = min(len(dat.keys()) - 1, cur + 1) - elif key == "l": - dat[dat.keys()[cur]] += 0.05 - write = True - elif key == "h": - dat[dat.keys()[cur]] -= 0.05 - write = True - elif key == "q": - break - - if write: - open(FILENAME, 'w').write(json.dumps(dat)) - - -if __name__ == "__main__": - orig_settings = termios.tcgetattr(sys.stdin) - tty.setcbreak(sys.stdin) - - try: - main() - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, orig_settings) - except: - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, orig_settings) - raise diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript index fca12d13..45c926ee 100644 --- a/selfdrive/locationd/SConscript +++ b/selfdrive/locationd/SConscript @@ -3,7 +3,7 @@ loc_objs = [ "locationd_yawrate.cc", "params_learner.cc", "paramsd.cc"] -loc_libs = [messaging, 'zmq', common, 'capnp', 'kj', 'json', 'json11', 'pthread'] +loc_libs = [messaging, 'zmq', common, 'capnp', 'kj', 'json11', 'pthread'] env.Program("paramsd", loc_objs, LIBS=loc_libs) env.SharedLibrary("locationd", loc_objs, LIBS=loc_libs) @@ -19,7 +19,4 @@ env.Program("ubloxd_test", [ "ublox_msg.cc", "ubloxd_main.cc"], LIBS=loc_libs) - - - diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index bc0f0a03..ffead7cf 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -10,7 +10,7 @@ from selfdrive.swaglog import cloudlog from common.params import Params, put_nonblocking from common.transformations.model import model_height from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \ - get_calib_from_vp, H, W, FOCAL + get_calib_from_vp, vp_from_rpy, H, W, FOCAL MPH_TO_MS = 0.44704 MIN_SPEED_FILTER = 15 * MPH_TO_MS @@ -59,13 +59,20 @@ class Calibrator(): 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) - self.vp = np.array(calibration_params["vanishing_point"]) + if 'calib_radians' in calibration_params: + self.vp = vp_from_rpy(calibration_params["calib_radians"]) + else: + self.vp = np.array(calibration_params["vanishing_point"]) if not np.isfinite(self.vp).all(): self.vp = copy.copy(VP_INIT) self.vps = np.tile(self.vp, (INPUTS_WANTED, 1)) @@ -88,8 +95,11 @@ class Calibrator(): 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: @@ -110,7 +120,8 @@ class Calibrator(): self.update_status() if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated): - cal_params = {"vanishing_point": list(self.vp), + calib = get_calib_from_vp(self.vp) + cal_params = {"calib_radians": list(calib), "valid_blocks": self.valid_blocks} put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8')) return new_vp @@ -132,7 +143,7 @@ class Calibrator(): 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 +154,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 2ec5ba0d..fd07abbe 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', diff --git a/selfdrive/locationd/kalman/helpers/ekf_sym.py b/selfdrive/locationd/kalman/helpers/ekf_sym.py index c9eb093a..d18feddf 100644 --- a/selfdrive/locationd/kalman/helpers/ekf_sym.py +++ b/selfdrive/locationd/kalman/helpers/ekf_sym.py @@ -291,6 +291,11 @@ class EKF_sym(): 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 aa729fc1..d6557202 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, - .0001**2, - math.radians(0.01)**2, - math.radians(0.2)**2, + (.05/100)**2, + .01**2, + math.radians(0.002)**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 @@ class CarKalman(): 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 @@ class CarKalman(): 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 @@ class CarKalman(): 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 001f6d2e..a147dde5 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 selfdrive.swaglog import cloudlog #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 @@ class Localizer(): 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 @@ class Localizer(): 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 @@ class Localizer(): 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 @@ class Localizer(): 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 @@ -144,7 +182,6 @@ class Localizer(): initial_pose_ecef_quat = quat_from_euler(initial_pose_ecef) gps_speed = log.speed quat_uncertainty = 0.2**2 - initial_pose_ecef_quat = quat_from_euler(initial_pose_ecef) initial_state = LiveKalman.initial_x initial_covs_diag = LiveKalman.initial_P_diag diff --git a/selfdrive/locationd/paramsd.cc b/selfdrive/locationd/paramsd.cc index 318a7598..8c60bc12 100644 --- a/selfdrive/locationd/paramsd.cc +++ b/selfdrive/locationd/paramsd.cc @@ -51,7 +51,7 @@ int main(int argc, char *argv[]) { LOGW("waiting for params to set vehicle model"); while (true) { - read_db_value(NULL, "CarParams", &value, &value_sz); + read_db_value("CarParams", &value, &value_sz); if (value_sz > 0) break; usleep(100*1000); } @@ -66,7 +66,7 @@ int main(int argc, char *argv[]) { cereal::CarParams::Reader car_params = cmsg.getRoot(); // Read params from previous run - const int result = read_db_value(NULL, "LiveParameters", &value, &value_sz); + const int result = read_db_value("LiveParameters", &value, &value_sz); std::string fingerprint = car_params.getCarFingerprint(); std::string vin = car_params.getCarVin(); @@ -168,7 +168,7 @@ int main(int argc, char *argv[]) { std::string out = json.dump(); std::async(std::launch::async, [out]{ - write_db_value(NULL, "LiveParameters", out.c_str(), out.length()); + write_db_value("LiveParameters", out.c_str(), out.length()); }); } } diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 93523502..aaf5017d 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 @@ class ParamsLearner: 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)])) + + self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]])) # 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]) + # 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]]])) - if not (0.5 < x[States.STIFFNESS] < 3.0): - self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, [1.0]) - - else: - self.kf.filter.filter_time = t - 0.1 + # 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 + 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") - CP = CarInterface.get_params(CAR.COROLLA_TSS2) - learner = ParamsLearner(CP) + 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/logcatd/logcatd.cc b/selfdrive/logcatd/logcatd.cc index 81fafb71..9a7f1ba5 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); @@ -64,8 +64,8 @@ int main() { android_logger_list_close(logger_list); - delete c; delete androidLog; + delete c; return 0; } diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript index 1feb899c..cb5dfa6e 100644 --- a/selfdrive/loggerd/SConscript +++ b/selfdrive/loggerd/SConscript @@ -3,10 +3,15 @@ Import('env', 'arch', 'messaging', 'common', 'visionipc') src = ['loggerd.cc', 'logger.cc'] libs = ['zmq', 'czmq', 'capnp', 'kj', 'z', 'avformat', 'avcodec', 'swscale', 'avutil', - 'yuv', 'bz2', common, 'json', messaging, visionipc] + 'yuv', 'bz2', common, messaging, visionipc] if arch == "aarch64": src += ['encoder.c', 'raw_logger.cc'] libs += ['OmxVenc', 'OmxCore', 'cutils'] +elif arch == "larch64": + src += ['encoder.c', 'raw_logger.cc'] + libs += ['OmxVenc', 'OmxCore', 'pthread'] +else: + libs += ['pthread'] env.Program(src, LIBS=libs) diff --git a/selfdrive/loggerd/encoder.c b/selfdrive/loggerd/encoder.c index 747c7a19..1509ff11 100644 --- a/selfdrive/loggerd/encoder.c +++ b/selfdrive/loggerd/encoder.c @@ -15,7 +15,7 @@ #include -#include +//#include #include @@ -24,7 +24,7 @@ #include "encoder.h" -#define ALOG(...) __android_log_print(ANDROID_LOG_VERBOSE, "omxapp", ##__VA_ARGS__) +//#define ALOG(...) __android_log_print(ANDROID_LOG_VERBOSE, "omxapp", ##__VA_ARGS__) // encoder: lossey codec using hardware hevc static void wait_for_state(EncoderState *s, OMX_STATETYPE state) { @@ -58,8 +58,6 @@ static OMX_ERRORTYPE event_handler(OMX_HANDLETYPE component, OMX_PTR app_data, O break; } - pthread_mutex_unlock(&s->state_lock); - return OMX_ErrorNone; } @@ -204,6 +202,9 @@ void encoder_init(EncoderState *s, const char* filename, int width, int height, err = OMX_GetHandle(&s->handle, (OMX_STRING)"OMX.qcom.video.encoder.avc", s, &omx_callbacks); } + if (err != OMX_ErrorNone) { + LOGE("error getting codec: %x", err); + } assert(err == OMX_ErrorNone); // printf("handle: %p\n", s->handle); @@ -283,20 +284,22 @@ void encoder_init(EncoderState *s, const char* filename, int width, int height, assert(err == OMX_ErrorNone); if (h265) { - // setup HEVC - OMX_VIDEO_PARAM_HEVCTYPE hecv_type = {0}; - hecv_type.nSize = sizeof(hecv_type); - hecv_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT; - err = OMX_GetParameter(s->handle, (OMX_INDEXTYPE)OMX_IndexParamVideoHevc, - (OMX_PTR) &hecv_type); - assert(err == OMX_ErrorNone); + #ifndef QCOM2 + // setup HEVC + OMX_VIDEO_PARAM_HEVCTYPE hecv_type = {0}; + hecv_type.nSize = sizeof(hecv_type); + hecv_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT; + err = OMX_GetParameter(s->handle, (OMX_INDEXTYPE)OMX_IndexParamVideoHevc, + (OMX_PTR) &hecv_type); + assert(err == OMX_ErrorNone); - hecv_type.eProfile = OMX_VIDEO_HEVCProfileMain; - hecv_type.eLevel = OMX_VIDEO_HEVCHighTierLevel5; + hecv_type.eProfile = OMX_VIDEO_HEVCProfileMain; + hecv_type.eLevel = OMX_VIDEO_HEVCHighTierLevel5; - err = OMX_SetParameter(s->handle, (OMX_INDEXTYPE)OMX_IndexParamVideoHevc, - (OMX_PTR) &hecv_type); - assert(err == OMX_ErrorNone); + err = OMX_SetParameter(s->handle, (OMX_INDEXTYPE)OMX_IndexParamVideoHevc, + (OMX_PTR) &hecv_type); + assert(err == OMX_ErrorNone); + #endif } else { // setup h264 OMX_VIDEO_PARAM_AVCTYPE avc = { 0 }; @@ -552,6 +555,7 @@ void encoder_open(EncoderState *s, const char* path) { pthread_mutex_lock(&s->lock); snprintf(s->vid_path, sizeof(s->vid_path), "%s/%s", path, s->filename); + LOGD("encoder_open %s remuxing:%d", s->vid_path, s->remuxing); if (s->remuxing) { avformat_alloc_output_context2(&s->ofmt_ctx, NULL, NULL, s->vid_path); diff --git a/selfdrive/loggerd/logger.cc b/selfdrive/loggerd/logger.cc index 0bb6c1b7..e0d0a736 100644 --- a/selfdrive/loggerd/logger.cc +++ b/selfdrive/loggerd/logger.cc @@ -132,8 +132,22 @@ static LoggerHandle* logger_open(LoggerState *s, const char* root_path) { return h; fail: LOGE("logger failed to open files"); - if (h->qlog_file) fclose(h->qlog_file); - if (h->log_file) fclose(h->log_file); + if (h->bz_file) { + BZ2_bzWriteClose(&bzerror, h->bz_file, 0, NULL, NULL); + h->bz_file = NULL; + } + if (h->bz_qlog) { + BZ2_bzWriteClose(&bzerror, h->bz_qlog, 0, NULL, NULL); + h->bz_qlog = NULL; + } + if (h->qlog_file) { + fclose(h->qlog_file); + h->qlog_file = NULL; + } + if (h->log_file) { + fclose(h->log_file); + h->log_file = NULL; + } return NULL; } @@ -228,8 +242,12 @@ void lh_close(LoggerHandle* h) { BZ2_bzWriteClose(&bzerror, h->bz_qlog, 0, NULL, NULL); h->bz_qlog = NULL; } - if (h->qlog_file) fclose(h->qlog_file); + if (h->qlog_file) { + fclose(h->qlog_file); + h->qlog_file = NULL; + } fclose(h->log_file); + h->log_file = NULL; unlink(h->lock_path); pthread_mutex_unlock(&h->lock); pthread_mutex_destroy(&h->lock); diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index 0e5c5457..93e80abb 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -41,7 +41,7 @@ #include "messaging.hpp" #include "services.h" -#ifndef QCOM +#if !(defined(QCOM) || defined(QCOM2)) // no encoder on PC #define DISABLE_ENCODER #endif @@ -95,7 +95,7 @@ void encoder_thread(bool is_streaming, bool raw_clips, bool front) { if (front) { char *value; - const int result = read_db_value(NULL, "RecordFront", &value, NULL); + const int result = read_db_value("RecordFront", &value, NULL); if (result != 0) return; if (value[0] != '1') { free(value); return; } free(value); @@ -137,10 +137,14 @@ void encoder_thread(bool is_streaming, bool raw_clips, bool front) { if (!encoder_inited) { LOGD("encoder init %dx%d", buf_info.width, buf_info.height); encoder_init(&encoder, front ? "dcamera.hevc" : "fcamera.hevc", buf_info.width, buf_info.height, CAMERA_FPS, front ? 2500000 : 5000000, true, false); + + #ifndef QCOM2 + // TODO: fix qcamera on tici if (!front) { encoder_init(&encoder_alt, "qcamera.ts", 480, 360, CAMERA_FPS, 128000, false, true); has_encoder_alt = true; } + #endif encoder_inited = true; if (is_streaming) { encoder.zmq_ctx = zmq_ctx_new(); @@ -164,6 +168,7 @@ void encoder_thread(bool is_streaming, bool raw_clips, bool front) { VIPCBuf* buf = visionstream_get(&stream, &extra); if (buf == NULL) { LOG("visionstream get failed"); + visionstream_destroy(&stream); break; } @@ -454,32 +459,32 @@ kj::Array gen_init_data() { char* git_commit = NULL; size_t size; - read_db_value(NULL, "GitCommit", &git_commit, &size); + read_db_value("GitCommit", &git_commit, &size); if (git_commit) { init.setGitCommit(capnp::Text::Reader(git_commit, size)); } char* git_branch = NULL; - read_db_value(NULL, "GitBranch", &git_branch, &size); + read_db_value("GitBranch", &git_branch, &size); if (git_branch) { init.setGitBranch(capnp::Text::Reader(git_branch, size)); } char* git_remote = NULL; - read_db_value(NULL, "GitRemote", &git_remote, &size); + read_db_value("GitRemote", &git_remote, &size); if (git_remote) { init.setGitRemote(capnp::Text::Reader(git_remote, size)); } char* passive = NULL; - read_db_value(NULL, "Passive", &passive, NULL); + read_db_value("Passive", &passive, NULL); init.setPassive(passive && strlen(passive) && passive[0] == '1'); { // log params std::map params; - read_db_all(NULL, ¶ms); + read_db_all(¶ms); auto lparams = init.initParams().initEntries(params.size()); int i = 0; for (auto& kv : params) { @@ -727,7 +732,8 @@ int main(int argc, char** argv) { for (auto s : socks){ delete s; } - + + delete poller; delete s.ctx; return 0; } diff --git a/selfdrive/loggerd/tests/loggerd_tests_common.py b/selfdrive/loggerd/tests/loggerd_tests_common.py index f36ef217..72bd5e3b 100644 --- a/selfdrive/loggerd/tests/loggerd_tests_common.py +++ b/selfdrive/loggerd/tests/loggerd_tests_common.py @@ -28,15 +28,26 @@ def create_random_file(file_path, size_mb, lock=False): os.remove(lock_path) class MockResponse(): - def __init__(self, text): + def __init__(self, text, status_code): self.text = text + self.status_code = status_code class MockApi(): def __init__(self, dongle_id): pass def get(self, *args, **kwargs): - return MockResponse('{"url": "http://localhost/does/not/exist", "headers": {}}') + return MockResponse('{"url": "http://localhost/does/not/exist", "headers": {}}', 200) + + def get_token(self): + return "fake-token" + +class MockApiIgnore(): + def __init__(self, dongle_id): + pass + + def get(self, *args, **kwargs): + return MockResponse('', 412) def get_token(self): return "fake-token" @@ -54,6 +65,9 @@ class MockParams(): class UploaderTestCase(unittest.TestCase): f_type = "UNKNOWN" + def set_ignore(self): + uploader.Api = MockApiIgnore + def setUp(self): self.root = tempfile.mkdtemp() uploader.ROOT = self.root # Monkey patch root dir diff --git a/selfdrive/loggerd/tests/test_uploader.py b/selfdrive/loggerd/tests/test_uploader.py index ed9a9722..96f449b0 100644 --- a/selfdrive/loggerd/tests/test_uploader.py +++ b/selfdrive/loggerd/tests/test_uploader.py @@ -1,13 +1,13 @@ -import os import time import threading +import unittest import logging import json from selfdrive.swaglog import cloudlog import selfdrive.loggerd.uploader as uploader -from common.timeout import Timeout +from common.xattr import getxattr from selfdrive.loggerd.tests.loggerd_tests_common import UploaderTestCase @@ -18,13 +18,16 @@ class TestLogHandler(logging.Handler): def reset(self): self.upload_order = list() + self.upload_ignored = list() def emit(self, record): try: j = json.loads(record.message) if j["event"] == "upload_success": self.upload_order.append(j["key"]) - except BaseException: + if j["event"] == "upload_ignored": + self.upload_ignored.append(j["key"]) + except Exception: pass log_handler = TestLogHandler() @@ -69,17 +72,35 @@ class TestUploader(UploaderTestCase): f_paths = self.gen_files(lock=False) self.start_thread() - - with Timeout(5, "Timeout waiting for file to be uploaded"): - while len(os.listdir(self.root)): - time.sleep(0.01) + # allow enough time that files could upload twice if there is a bug in the logic + time.sleep(5) self.join_thread() + self.assertTrue(len(log_handler.upload_ignored) == 0, "Some files were ignored") + self.assertFalse(len(log_handler.upload_order) < len(f_paths), "Some files failed to upload") + self.assertFalse(len(log_handler.upload_order) > len(f_paths), "Some files were uploaded twice") for f_path in f_paths: - self.assertFalse(os.path.exists(f_path), "All files not uploaded") + self.assertTrue(getxattr(f_path, uploader.UPLOAD_ATTR_NAME), "All files not uploaded") exp_order = self.gen_order([self.seg_num], []) self.assertTrue(log_handler.upload_order == exp_order, "Files uploaded in wrong order") + def test_upload_ignored(self): + self.set_ignore() + f_paths = self.gen_files(lock=False) + + self.start_thread() + # allow enough time that files could upload twice if there is a bug in the logic + time.sleep(5) + self.join_thread() + + self.assertTrue(len(log_handler.upload_order) == 0, "Some files were not ignored") + self.assertFalse(len(log_handler.upload_ignored) < len(f_paths), "Some files failed to ignore") + self.assertFalse(len(log_handler.upload_ignored) > len(f_paths), "Some files were ignored twice") + for f_path in f_paths: + self.assertTrue(getxattr(f_path, uploader.UPLOAD_ATTR_NAME), "All files not ignored") + exp_order = self.gen_order([self.seg_num], []) + self.assertTrue(log_handler.upload_ignored == exp_order, "Files ignored in wrong order") + def test_upload_files_in_create_order(self): f_paths = list() seg1_nums = [0,1,2,10,20] @@ -92,15 +113,15 @@ class TestUploader(UploaderTestCase): f_paths += self.gen_files() self.start_thread() - - with Timeout(5, "Timeout waiting for file to be upload"): - while len(os.listdir(self.root)): - time.sleep(0.01) - + # allow enough time that files could upload twice if there is a bug in the logic + time.sleep(5) self.join_thread() + self.assertTrue(len(log_handler.upload_ignored) == 0, "Some files were ignored") + self.assertFalse(len(log_handler.upload_order) < len(f_paths), "Some files failed to upload") + self.assertFalse(len(log_handler.upload_order) > len(f_paths), "Some files were uploaded twice") for f_path in f_paths: - self.assertFalse(os.path.exists(f_path), "All files not uploaded") + self.assertTrue(getxattr(f_path, uploader.UPLOAD_ATTR_NAME), "All files not uploaded") exp_order = self.gen_order(seg1_nums, seg2_nums) self.assertTrue(log_handler.upload_order == exp_order, "Files uploaded in wrong order") @@ -113,4 +134,8 @@ class TestUploader(UploaderTestCase): self.join_thread() for f_path in f_paths: - self.assertTrue(os.path.exists(f_path), "File upload when locked") + self.assertFalse(getxattr(f_path, uploader.UPLOAD_ATTR_NAME), "File upload when locked") + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index dba2287b..5c2a9431 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -17,6 +17,10 @@ from selfdrive.loggerd.config import ROOT from common import android from common.params import Params from common.api import Api +from common.xattr import getxattr, setxattr + +UPLOAD_ATTR_NAME = 'user.upload' +UPLOAD_ATTR_VALUE = b'1' fake_upload = os.getenv("FAKEUPLOAD") is not None @@ -86,7 +90,7 @@ def is_on_hotspot(): is_entune = result.startswith('10.0.2.') return (is_android or is_ios or is_entune) - except: + except Exception: return False class Uploader(): @@ -103,16 +107,6 @@ class Uploader(): self.immediate_priority = {"qlog.bz2": 0, "qcamera.ts": 1} self.high_priority = {"rlog.bz2": 0, "fcamera.hevc": 1, "dcamera.hevc": 2} - def clean_dirs(self): - try: - for logname in os.listdir(self.root): - path = os.path.join(self.root, logname) - # remove empty directories - if not os.listdir(path): - os.rmdir(path) - except OSError: - cloudlog.exception("clean_dirs failed") - def get_upload_sort(self, name): if name in self.immediate_priority: return self.immediate_priority[name] @@ -135,6 +129,14 @@ class Uploader(): for name in sorted(names, key=self.get_upload_sort): key = os.path.join(logname, name) fn = os.path.join(path, name) + # skip files already uploaded + try: + is_uploaded = getxattr(fn, UPLOAD_ATTR_NAME) + except OSError: + cloudlog.event("uploader_getxattr_failed", exc=self.last_exc, key=key, fn=fn) + is_uploaded = True # deleter could have deleted + if is_uploaded: + continue yield (name, key, fn) @@ -161,6 +163,10 @@ class Uploader(): def do_upload(self, key, fn): try: url_resp = self.api.get("v1.3/"+self.dongle_id+"/upload_url/", timeout=10, path=key, access_token=self.api.get_token()) + if url_resp.status_code == 412: + self.last_resp = url_resp + return + url_resp_json = json.loads(url_resp.text) url = url_resp_json['url'] headers = url_resp_json['headers'] @@ -202,28 +208,27 @@ class Uploader(): cloudlog.info("checking %r with size %r", key, sz) if sz == 0: - # can't upload files of 0 size - os.unlink(fn) # delete the file + try: + # tag files of 0 size as uploaded + setxattr(fn, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE) + except OSError: + cloudlog.event("uploader_setxattr_failed", exc=self.last_exc, key=key, fn=fn, sz=sz) success = True else: cloudlog.info("uploading %r", fn) stat = self.normal_upload(key, fn) - if stat is not None and stat.status_code in (200, 201): - cloudlog.event("upload_success", key=key, fn=fn, sz=sz) - - # delete the file + if stat is not None and stat.status_code in (200, 201, 412): + cloudlog.event("upload_success" if stat.status_code != 412 else "upload_ignored", key=key, fn=fn, sz=sz) try: - os.unlink(fn) + # tag file as uploaded + setxattr(fn, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE) except OSError: - cloudlog.event("delete_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz) - + cloudlog.event("uploader_setxattr_failed", exc=self.last_exc, key=key, fn=fn, sz=sz) success = True else: cloudlog.event("upload_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz) success = False - self.clean_dirs() - return success def uploader_fn(exit_event): diff --git a/selfdrive/logmessaged.py b/selfdrive/logmessaged.py index 693d259a..91dcbb7e 100755 --- a/selfdrive/logmessaged.py +++ b/selfdrive/logmessaged.py @@ -1,14 +1,12 @@ #!/usr/bin/env python3 import zmq -from logentries import LogentriesHandler import cereal.messaging as messaging +from selfdrive.swaglog import get_le_handler + def main(): - # setup logentries. we forward log messages to it - le_token = "e8549616-0798-4d7e-a2ca-2513ae81fa17" - le_handler = LogentriesHandler(le_token, use_tls=False, verbose=False) - - le_level = 20 #logging.INFO + le_handler = get_le_handler() + le_level = 20 # logging.INFO ctx = zmq.Context().instance() sock = ctx.socket(zmq.PULL) @@ -36,5 +34,6 @@ def main(): msg.logMessage = dat pub_sock.send(msg.to_bytes()) + if __name__ == "__main__": main() diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 4a63241d..c2db7582 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 @@ -8,13 +8,16 @@ import signal import shutil import subprocess 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 +67,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,25 +94,32 @@ if not prebuilt: 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): @@ -114,7 +128,19 @@ if not prebuilt: 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 +149,6 @@ import cereal.messaging as messaging 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 @@ -162,6 +187,7 @@ managed_processes = { "updated": "selfdrive.updated", "dmonitoringmodeld": ("selfdrive/modeld", ["./dmonitoringmodeld"]), "modeld": ("selfdrive/modeld", ["./modeld"]), + "driverview": "selfdrive.controls.lib.driverview", } daemon_processes = { @@ -190,6 +216,7 @@ persistent_processes = [ 'ui', 'uploader', ] + if ANDROID: persistent_processes += [ 'logcatd', @@ -209,8 +236,14 @@ car_started_processes = [ 'modeld', 'proclogd', 'ubloxd', - 'locationd', + #'locationd', ] + +if WEBCAM: + car_started_processes += [ + 'dmonitoringmodeld', + ] + if ANDROID: car_started_processes += [ 'sensord', @@ -353,7 +386,7 @@ 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: @@ -439,7 +472,7 @@ def manager_thread(): 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,6 +482,11 @@ 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] @@ -467,7 +505,6 @@ 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) @@ -478,7 +515,7 @@ def manager_prepare(spinner=None): 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: @@ -507,6 +544,7 @@ def main(): default_params = [ ("CommunityFeaturesToggle", "0"), ("CompletedTrainingVersion", "0"), + ("IsRHD", "0"), ("IsMetric", "0"), ("RecordFront", "0"), ("HasAcceptedTerms", "0"), @@ -518,9 +556,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 @@ -560,7 +599,22 @@ def main(): 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 4c58e278..0872b653 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -11,6 +11,8 @@ common_src = [ if arch == "aarch64": libs += ['gsl', 'CB', 'gnustl_shared'] +elif arch == "larch64": + libs += ['gsl', 'CB', 'symphony-cpu', 'pthread'] else: libs += ['symphony-cpu', 'pthread'] diff --git a/selfdrive/modeld/dmonitoringmodeld b/selfdrive/modeld/dmonitoringmodeld index 710e0e59..51b520d7 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 9fb67b63..d17247f9 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,7 @@ int main(int argc, char **argv) { visionstream_destroy(&stream); delete dmonitoring_sock; + delete msg_context; dmonitoring_free(&dmonitoringmodel); return 0; diff --git a/selfdrive/modeld/modeld b/selfdrive/modeld/modeld index bbe7006a..5369034d 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 fa49d1b5..309c64c1 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; } @@ -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"); diff --git a/selfdrive/modeld/models/commonmodel.c b/selfdrive/modeld/models/commonmodel.c index 143480fb..db990af0 100644 --- a/selfdrive/modeld/models/commonmodel.c +++ b/selfdrive/modeld/models/commonmodel.c @@ -1,7 +1,6 @@ #include "commonmodel.h" #include -#include "cereal/gen/c/log.capnp.h" #include "common/mat.h" #include "common/timing.h" diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 4e3ee448..0a03d281 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 787608f9..7e8d6351 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 80aae5cd..dc5d116e 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 b544a3e5..4ef8c8df 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" @@ -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("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]; @@ -112,18 +135,18 @@ void model_free(ModelState* s) { delete s->m; } -void poly_fit(float *in_pts, float *in_stds, float *out) { +void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) { // References to inputs - Eigen::Map > pts(in_pts, MODEL_PATH_DISTANCE); - Eigen::Map > std(in_stds, MODEL_PATH_DISTANCE); + Eigen::Map > pts(in_pts, valid_len); + Eigen::Map > std(in_stds, valid_len); Eigen::Map > p(out, POLYFIT_DEGREE - 1); float y0 = pts[0]; pts = pts.array() - y0; // Build Least Squares equations - Eigen::Matrix lhs = vander.array().colwise() / std.array(); - Eigen::Matrix rhs = pts.array() / std.array(); + Eigen::Matrix lhs = vander.topRows(valid_len).array().colwise() / std.array(); + Eigen::Matrix rhs = pts.array() / std.array(); // Improve numerical stability Eigen::Matrix scale = 1. / (lhs.array()*lhs.array()).sqrt().colwise().sum(); @@ -147,15 +170,11 @@ void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bo float prob; float valid_len; - valid_len = data[MODEL_PATH_DISTANCE*2]; + // clamp to 5 and 192 + valid_len = fmin(192, fmax(5, data[MODEL_PATH_DISTANCE*2])); for (int i=0; i stds(&stds_arr[0], ARRAYSIZE(stds_arr)); @@ -205,14 +224,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 +260,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 diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 742eb6ce..13669608 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, diff --git a/selfdrive/modeld/runners/runmodel.h b/selfdrive/modeld/runners/runmodel.h index 8ddc2503..d3889aaa 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 c954f9b9..4bb442d5 100644 --- a/selfdrive/modeld/runners/snpemodel.cc +++ b/selfdrive/modeld/runners/snpemodel.cc @@ -28,7 +28,7 @@ SNPEModel::SNPEModel(const char *path, float *output, size_t output_size, int ru // load model std::unique_ptr container = zdl::DlContainer::IDlContainer::open(model_data, model_size); if (!container) { PrintErrorStringAndExit(); } - printf("loaded model with size: %u\n", model_size); + printf("loaded model with size: %lu\n", model_size); // create model runner zdl::SNPE::SNPEBuilder snpeBuilder(container.get()); @@ -79,7 +79,7 @@ SNPEModel::SNPEModel(const char *path, float *output, size_t output_size, int ru stride *= bufferShape[i]; strides[i-1] = stride; } - printf("input product is %u\n", product); + printf("input product is %lu\n", product); inputBuffer = ubFactory.createUserBuffer(NULL, product*sizeof(float), strides, &userBufferEncodingFloat); inputMap.add(input_tensor_name, inputBuffer.get()); @@ -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 f99acd09..9289444b 100644 --- a/selfdrive/modeld/runners/snpemodel.h +++ b/selfdrive/modeld/runners/snpemodel.h @@ -24,6 +24,7 @@ public: 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 @@ private: // 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 e0154291..fc7655b5 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,6 @@ __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 3854be9d..14f9d236 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 bf747af0..64fde866 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") diff --git a/selfdrive/proclogd/proclogd.cc b/selfdrive/proclogd/proclogd.cc index ec8a9ad2..2e7503ca 100644 --- a/selfdrive/proclogd/proclogd.cc +++ b/selfdrive/proclogd/proclogd.cc @@ -240,8 +240,8 @@ int main() { usleep(2000000); // 2 secs } - delete c; delete publisher; + delete c; return 0; } diff --git a/selfdrive/registration.py b/selfdrive/registration.py index 5b01d144..dbedcc9e 100644 --- a/selfdrive/registration.py +++ b/selfdrive/registration.py @@ -36,7 +36,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 +52,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 b285cf97..4c9e2432 100644 --- a/selfdrive/sensord/SConscript +++ b/selfdrive/sensord/SConscript @@ -1,5 +1,5 @@ Import('env', 'common', 'messaging') -env.Program('_sensord', 'sensors.cc', LIBS=['hardware', common, 'json', messaging, 'capnp', 'zmq', 'kj']) +env.Program('_sensord', 'sensors.cc', LIBS=['hardware', common, 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', messaging, 'capnp', 'zmq', 'kj']) diff --git a/selfdrive/sensord/gpsd.cc b/selfdrive/sensord/gpsd.cc index ae2ecec0..4f3b7079 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/rawgps.cc b/selfdrive/sensord/rawgps.cc deleted file mode 100644 index 3a0d1c80..00000000 --- a/selfdrive/sensord/rawgps.cc +++ /dev/null @@ -1,1184 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "cereal/gen/cpp/log.capnp.h" - -#include "messaging.hpp" -#include "common/timing.h" -#include "common/util.h" -#include "common/swaglog.h" - -#include "libdiag.h" - -#define NV_GNSS_OEM_FEATURE_MASK 7165 -# define NV_GNSS_OEM_FEATURE_MASK_OEMDRE 1 -#define NV_CGPS_DPO_CONTROL 5596 - -#define DIAG_NV_READ_F 38 -#define DIAG_NV_WRITE_F 39 - -#define DIAG_SUBSYS_CMD 75 -#define DIAG_SUBSYS_CMD_VER_2 128 - -#define DIAG_SUBSYS_GPS 13 -#define DIAG_SUBSYS_FS 19 - -#define CGPS_DIAG_PDAPI_CMD 100 -#define CGPS_OEM_CONTROL 202 - -#define GPSDIAG_OEMFEATURE_DRE 1 -#define GPSDIAG_OEM_DRE_ON 1 - -#define FEATURE_OEMDRE_NOT_SUPPORTED 1 -#define FEATURE_OEMDRE_ON 2 -#define FEATURE_OEMDRE_ALREADY_ON 4 - -#define TM_DIAG_NAV_CONFIG_CMD 0x6E - -#define EFS2_DIAG_SYNC_NO_WAIT 48 - -struct __attribute__((packed)) NvPacket { - uint8_t cmd_code; - uint16_t nv_id; - uint8_t data[128]; - uint16_t status; -}; - -enum NvStatus { - NV_DONE, - NV_BUSY, - NV_FULL, - NV_FAIL, - NV_NOTACTIVE, - NV_BADPARAM, - NV_READONLY, - NV_BADRG, - NV_NOMEM, - NV_NOTALLOC, -}; - -struct __attribute__((packed)) Efs2DiagSyncReq { - uint8_t cmd_code; - uint8_t subsys_id; - uint16_t subsys_cmd_code; - uint16_t sequence_num; - char path[8]; -}; - - -struct __attribute__((packed)) Efs2DiagSyncResp { - uint8_t cmd_code; - uint8_t subsys_id; - uint16_t subsys_cmd_code; - uint16_t sequence_num; - uint32_t sync_token; - int32_t diag_errno; -}; - - - -struct __attribute__((packed)) GpsOemControlReq { - uint8_t cmd_code; - uint8_t subsys_id; - uint16_t subsys_cmd_code; - uint8_t gps_cmd_code; - uint8_t version; - - uint32_t oem_feature; - uint32_t oem_command; - uint32_t reserved[2]; -}; - - -struct __attribute__((packed)) GpsOemControlResp { - uint8_t cmd_code; - uint8_t subsys_id; - uint16_t subsys_cmd_code; - uint8_t gps_cmd_code; - uint8_t version; - - uint32_t oem_feature; - uint32_t oem_command; - uint32_t resp_result; - uint32_t reserved[2]; -}; - -struct __attribute__((packed)) GpsNavConfigReq { - uint8_t cmd_code; - uint8_t subsys_id; - uint16_t subsys_cmd_code; - uint32_t subsys_status; - uint16_t subsys_delayed_resp_id; - uint16_t subsys_rsp_cnt; - - uint8_t desired_config; -}; - -struct __attribute__((packed)) GpsNavConfigResp { - uint8_t cmd_code; - uint8_t subsys_id; - uint16_t subsys_cmd_code; - uint32_t subsys_status; - uint16_t subsys_delayed_resp_id; - uint16_t subsys_rsp_cnt; - - uint8_t supported_config; - uint8_t actual_config; -}; - - -#define LOG_GNSS_POSITION_REPORT 0x1476 -#define LOG_GNSS_GPS_MEASUREMENT_REPORT 0x1477 -#define LOG_GNSS_CLOCK_REPORT 0x1478 -#define LOG_GNSS_GLONASS_MEASUREMENT_REPORT 0x1480 -#define LOG_GNSS_BDS_MEASUREMENT_REPORT 0x1756 -#define LOG_GNSS_GAL_MEASUREMENT_REPORT 0x1886 - -#define LOG_GNSS_OEMDRE_MEASUREMENMT_REPORT 0x14DE -#define LOG_GNSS_OEMDRE_SVPOLY_REPORT 0x14E1 - -#define LOG_GNSS_ME_DPO_STATUS 0x1838 -#define LOG_GNSS_CD_DB_REPORT 0x147B -#define LOG_GNSS_PRX_RF_HW_STATUS_REPORT 0x147E -#define LOG_CGPS_SLOW_CLOCK_CLIB_REPORT 0x1488 -#define LOG_GNSS_CONFIGURATION_STATE 0x1516 - -struct __attribute__((packed)) log_header_type { - uint16_t len; - uint16_t code; - uint64_t ts; -}; - - - -enum SVObservationStates { - SV_IDLE, - SV_SEARCH, - SV_SEACH_VERIFY, - SV_BIT_EDGE, - SV_TRACK_VERIFY, - SV_TRACK, - SV_RESTART, - SV_DPO, - SV_GLO_10ms_BE, - SV_GLO_10ms_AT, -}; - -struct __attribute__((packed)) GNSSGpsMeasurementReportv0_SV{ - uint8_t sv_id; - uint8_t observation_state; // SVObservationStates - uint8_t observations; - uint8_t good_observations; - uint16_t parity_error_count; - uint8_t filter_stages; - uint16_t carrier_noise; - int16_t latency; - uint8_t predetect_interval; - uint16_t postdetections; - uint32_t unfiltered_measurement_integral; - float unfiltered_measurement_fraction; - float unfiltered_time_uncertainty; - float unfiltered_speed; - float unfiltered_speed_uncertainty; - uint32_t measurement_status; - uint8_t misc_status; - uint32_t multipath_estimate; - float azimuth; - float elevation; - int32_t carrier_phase_cycles_integral; - uint16_t carrier_phase_cycles_fraction; - float fine_speed; - float fine_speed_uncertainty; - uint8_t cycle_slip_count; - uint32_t pad; -}; - -_Static_assert(sizeof(GNSSGpsMeasurementReportv0_SV) == 70, "error"); - -struct __attribute__((packed)) GNSSGpsMeasurementReportv0{ - log_header_type header; - uint8_t version; - uint32_t f_count; - uint16_t week; - uint32_t milliseconds; - float time_bias; - float clock_time_uncertainty; - float clock_frequency_bias; - float clock_frequency_uncertainty; - uint8_t sv_count; - GNSSGpsMeasurementReportv0_SV sv[]; -}; - - - -struct __attribute__((packed)) GNSSGlonassMeasurementReportv0_SV { - uint8_t sv_id; - int8_t frequency_index; - uint8_t observation_state; // SVObservationStates - uint8_t observations; - uint8_t good_observations; - uint8_t hemming_error_count; - uint8_t filter_stages; - uint16_t carrier_noise; - int16_t latency; - uint8_t predetect_interval; - uint16_t postdetections; - uint32_t unfiltered_measurement_integral; - float unfiltered_measurement_fraction; - float unfiltered_time_uncertainty; - float unfiltered_speed; - float unfiltered_speed_uncertainty; - uint32_t measurement_status; - uint8_t misc_status; - uint32_t multipath_estimate; - float azimuth; - float elevation; - int32_t carrier_phase_cycles_integral; - uint16_t carrier_phase_cycles_fraction; - float fine_speed; - float fine_speed_uncertainty; - uint8_t cycle_slip_count; - uint32_t pad; -}; - -_Static_assert(sizeof(GNSSGlonassMeasurementReportv0_SV) == 70, "error"); - -struct __attribute__((packed)) GNSSGlonassMeasurementReportv0 { - log_header_type header; - uint8_t version; - uint32_t f_count; - uint8_t glonass_cycle_number; - uint16_t glonass_number_of_days; - uint32_t milliseconds; - float time_bias; - float clock_time_uncertainty; - float clock_frequency_bias; - float clock_frequency_uncertainty; - uint8_t sv_count; - GNSSGlonassMeasurementReportv0_SV sv[]; -}; - - -struct __attribute__((packed)) GNSSClockReportv2 { - log_header_type header; - uint8_t version; - uint16_t valid_flags; - - uint32_t f_count; - - uint16_t gps_week; - uint32_t gps_milliseconds; - float gps_time_bias; - float gps_clock_time_uncertainty; - uint8_t gps_clock_source; - - uint8_t glonass_year; - uint16_t glonass_day; - uint32_t glonass_milliseconds; - float glonass_time_bias; - float glonass_clock_time_uncertainty; - uint8_t glonass_clock_source; - - uint16_t bds_week; - uint32_t bds_milliseconds; - float bds_time_bias; - float bds_clock_time_uncertainty; - uint8_t bds_clock_source; - - uint16_t gal_week; - uint32_t gal_milliseconds; - float gal_time_bias; - float gal_clock_time_uncertainty; - uint8_t gal_clock_source; - - float clock_frequency_bias; - float clock_frequency_uncertainty; - uint8_t frequency_source; - uint8_t gps_leap_seconds; - uint8_t gps_leap_seconds_uncertainty; - uint8_t gps_leap_seconds_source; - - float gps_to_glonass_time_bias_milliseconds; - float gps_to_glonass_time_bias_milliseconds_uncertainty; - float gps_to_bds_time_bias_milliseconds; - float gps_to_bds_time_bias_milliseconds_uncertainty; - float bds_to_glo_time_bias_milliseconds; - float bds_to_glo_time_bias_milliseconds_uncertainty; - float gps_to_gal_time_bias_milliseconds; - float gps_to_gal_time_bias_milliseconds_uncertainty; - float gal_to_glo_time_bias_milliseconds; - float gal_to_glo_time_bias_milliseconds_uncertainty; - float gal_to_bds_time_bias_milliseconds; - float gal_to_bds_time_bias_milliseconds_uncertainty; - - uint32_t system_rtc_time; - uint32_t f_count_offset; - uint32_t lpm_rtc_count; - uint32_t clock_resets; - - uint32_t reserved[3]; - -}; - - -enum GNSSMeasurementSource { - SOURCE_GPS, - SOURCE_GLONASS, - SOURCE_BEIDOU, -}; - -struct __attribute__((packed)) GNSSOemdreMeasurement { - uint8_t sv_id; - uint8_t unkn; - int8_t glonass_frequency_index; - uint32_t observation_state; - uint8_t observations; - uint8_t good_observations; - uint8_t filter_stages; - uint8_t predetect_interval; - uint8_t cycle_slip_count; - uint16_t postdetections; - - uint32_t measurement_status; - uint32_t measurement_status2; - - uint16_t carrier_noise; - uint16_t rf_loss; - int16_t latency; - - float filtered_measurement_fraction; - uint32_t filtered_measurement_integral; - float filtered_time_uncertainty; - float filtered_speed; - float filtered_speed_uncertainty; - - float unfiltered_measurement_fraction; - uint32_t unfiltered_measurement_integral; - float unfiltered_time_uncertainty; - float unfiltered_speed; - float unfiltered_speed_uncertainty; - - uint8_t multipath_estimate_valid; - uint32_t multipath_estimate; - uint8_t direction_valid; - float azimuth; - float elevation; - float doppler_acceleration; - float fine_speed; - float fine_speed_uncertainty; - - uint64_t carrier_phase; - uint32_t f_count; - - uint16_t parity_error_count; - uint8_t good_parity; - -}; - -_Static_assert(sizeof(GNSSOemdreMeasurement) == 109, "error"); - -struct __attribute__((packed)) GNSSOemdreMeasurementReportv2 { - log_header_type header; - uint8_t version; - uint8_t reason; - uint8_t sv_count; - uint8_t seq_num; - uint8_t seq_max; - uint16_t rf_loss; - - uint8_t system_rtc_valid; - uint32_t f_count; - uint32_t clock_resets; - uint64_t system_rtc_time; - - uint8_t gps_leap_seconds; - uint8_t gps_leap_seconds_uncertainty; - float gps_to_glonass_time_bias_milliseconds; - float gps_to_glonass_time_bias_milliseconds_uncertainty; - - uint16_t gps_week; - uint32_t gps_milliseconds; - uint32_t gps_time_bias; - uint32_t gps_clock_time_uncertainty; - uint8_t gps_clock_source; - - uint8_t glonass_clock_source; - uint8_t glonass_year; - uint16_t glonass_day; - uint32_t glonass_milliseconds; - float glonass_time_bias; - float glonass_clock_time_uncertainty; - - float clock_frequency_bias; - float clock_frequency_uncertainty; - uint8_t frequency_source; - - uint32_t cdma_clock_info[5]; - - uint8_t source; - - GNSSOemdreMeasurement measurements[16]; - -}; - -_Static_assert(sizeof(GNSSOemdreMeasurementReportv2) == 1851, "error"); - - -struct __attribute__((packed)) GNSSOemdreSVPolyReportv2 { - log_header_type header; - uint8_t version; - uint16_t sv_id; - int8_t frequency_index; - uint8_t flags; - uint16_t iode; - double t0; - double xyz0[3]; - double xyzN[9]; - float other[4]; - float position_uncertainty; - float iono_delay; - float iono_dot; - float sbas_iono_delay; - float sbas_iono_dot; - float tropo_delay; - float elevation; - float elevation_dot; - float elevation_uncertainty; - double velocity_coeff[12]; -}; - -_Static_assert(sizeof(GNSSOemdreSVPolyReportv2) == 271, "error"); - - -static Context *rawgps_context; -static PubSocket *rawgps_publisher; -static int client_id = 0; - -static void hexdump(uint8_t* d, size_t len) { - for (int i=0; i= sizeof(log_header_type)+1); - log_header_type* log_header = (log_header_type*)ptr; - uint8_t* log_data = ptr + sizeof(log_header_type); - -#ifdef RAWGPS_TEST - printf("%04x\n", log_header->code); -#endif - - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); - auto qcomGnss = event.initQcomGnss(); - qcomGnss.setLogTs(log_header->ts); - - switch (log_header->code) { - - case LOG_GNSS_CLOCK_REPORT: { - uint8_t version = log_data[0]; - assert(version == 2); - - assert(len >= sizeof(GNSSClockReportv2)); - const GNSSClockReportv2* report = (const GNSSClockReportv2*)ptr; - - auto lreport = qcomGnss.initClockReport(); - lreport.setHasFCount(report->valid_flags & (1 << 0)); - lreport.setFCount(report->f_count); - - lreport.setHasGpsWeek(report->valid_flags & (1 << 2)); - lreport.setGpsWeek(report->gps_week); - lreport.setHasGpsMilliseconds(report->valid_flags & (1 << 1)); - lreport.setGpsMilliseconds(report->gps_milliseconds); - lreport.setGpsTimeBias(report->gps_time_bias); - lreport.setGpsClockTimeUncertainty(report->gps_clock_time_uncertainty); - lreport.setGpsClockSource(report->gps_clock_source); - - lreport.setHasGlonassYear(report->valid_flags & (1 << 6)); - lreport.setGlonassYear(report->glonass_year); - lreport.setHasGlonassDay(report->valid_flags & (1 << 5)); - lreport.setGlonassDay(report->glonass_day); - lreport.setHasGlonassMilliseconds(report->valid_flags & (1 << 4)); - lreport.setGlonassMilliseconds(report->glonass_milliseconds); - lreport.setGlonassTimeBias(report->glonass_time_bias); - lreport.setGlonassClockTimeUncertainty(report->glonass_clock_time_uncertainty); - lreport.setGlonassClockSource(report->glonass_clock_source); - - lreport.setBdsWeek(report->bds_week); - lreport.setBdsMilliseconds(report->bds_milliseconds); - lreport.setBdsTimeBias(report->bds_time_bias); - lreport.setBdsClockTimeUncertainty(report->bds_clock_time_uncertainty); - lreport.setBdsClockSource(report->bds_clock_source); - - lreport.setGalWeek(report->gal_week); - lreport.setGalMilliseconds(report->gal_milliseconds); - lreport.setGalTimeBias(report->gal_time_bias); - lreport.setGalClockTimeUncertainty(report->gal_clock_time_uncertainty); - lreport.setGalClockSource(report->gal_clock_source); - - lreport.setClockFrequencyBias(report->clock_frequency_bias); - lreport.setClockFrequencyUncertainty(report->clock_frequency_uncertainty); - lreport.setFrequencySource(report->frequency_source); - lreport.setGpsLeapSeconds(report->gps_leap_seconds); - lreport.setGpsLeapSecondsUncertainty(report->gps_leap_seconds_uncertainty); - lreport.setGpsLeapSecondsSource(report->gps_leap_seconds_source); - - lreport.setGpsToGlonassTimeBiasMilliseconds(report->gps_to_glonass_time_bias_milliseconds); - lreport.setGpsToGlonassTimeBiasMillisecondsUncertainty(report->gps_to_glonass_time_bias_milliseconds_uncertainty); - lreport.setGpsToBdsTimeBiasMilliseconds(report->gps_to_bds_time_bias_milliseconds); - lreport.setGpsToBdsTimeBiasMillisecondsUncertainty(report->gps_to_bds_time_bias_milliseconds_uncertainty); - lreport.setBdsToGloTimeBiasMilliseconds(report->bds_to_glo_time_bias_milliseconds); - lreport.setBdsToGloTimeBiasMillisecondsUncertainty(report->bds_to_glo_time_bias_milliseconds_uncertainty); - lreport.setGpsToGalTimeBiasMilliseconds(report->gps_to_gal_time_bias_milliseconds); - lreport.setGpsToGalTimeBiasMillisecondsUncertainty(report->gps_to_gal_time_bias_milliseconds_uncertainty); - lreport.setGalToGloTimeBiasMilliseconds(report->gal_to_glo_time_bias_milliseconds); - lreport.setGalToGloTimeBiasMillisecondsUncertainty(report->gal_to_glo_time_bias_milliseconds_uncertainty); - lreport.setGalToBdsTimeBiasMilliseconds(report->gal_to_bds_time_bias_milliseconds); - lreport.setGalToBdsTimeBiasMillisecondsUncertainty(report->gal_to_bds_time_bias_milliseconds_uncertainty); - - lreport.setHasRtcTime(report->valid_flags & (1 << 3)); - lreport.setSystemRtcTime(report->system_rtc_time); - lreport.setFCountOffset(report->f_count_offset); - lreport.setLpmRtcCount(report->lpm_rtc_count); - lreport.setClockResets(report->clock_resets); - - break; - } - case LOG_GNSS_GPS_MEASUREMENT_REPORT: { - uint8_t version = log_data[0]; - assert(version == 0); - - assert(len >= sizeof(GNSSGpsMeasurementReportv0)); - const GNSSGpsMeasurementReportv0* report = (const GNSSGpsMeasurementReportv0*)ptr; - assert(len >= sizeof(sizeof(GNSSGpsMeasurementReportv0))+sizeof(GNSSGpsMeasurementReportv0_SV) * report->sv_count); - - auto lreport = qcomGnss.initMeasurementReport(); - lreport.setSource(cereal::QcomGnss::MeasurementSource::GPS); - lreport.setFCount(report->f_count); - lreport.setGpsWeek(report->week); - lreport.setMilliseconds(report->milliseconds); - lreport.setTimeBias(report->time_bias); - lreport.setClockTimeUncertainty(report->clock_time_uncertainty); - lreport.setClockFrequencyBias(report->clock_frequency_bias); - lreport.setClockFrequencyUncertainty(report->clock_frequency_uncertainty); - - auto lsvs = lreport.initSv(report->sv_count); - for (int i=0; isv_count; i++) { - auto lsv = lsvs[i]; - const GNSSGpsMeasurementReportv0_SV *sv = &report->sv[i]; - - lsv.setSvId(sv->sv_id); - lsv.setObservationState(cereal::QcomGnss::SVObservationState(sv->observation_state)); - lsv.setObservations(sv->observations); - lsv.setGoodObservations(sv->good_observations); - lsv.setGpsParityErrorCount(sv->parity_error_count); - lsv.setFilterStages(sv->filter_stages); - lsv.setCarrierNoise(sv->carrier_noise); - lsv.setLatency(sv->latency); - lsv.setPredetectInterval(sv->predetect_interval); - lsv.setPostdetections(sv->postdetections); - lsv.setUnfilteredMeasurementIntegral(sv->unfiltered_measurement_integral); - lsv.setUnfilteredMeasurementFraction(sv->unfiltered_measurement_fraction); - lsv.setUnfilteredTimeUncertainty(sv->unfiltered_time_uncertainty); - lsv.setUnfilteredSpeed(sv->unfiltered_speed); - lsv.setUnfilteredSpeedUncertainty(sv->unfiltered_speed_uncertainty); - - lsv.setMultipathEstimate(sv->multipath_estimate); - lsv.setAzimuth(sv->azimuth); - lsv.setElevation(sv->elevation); - lsv.setCarrierPhaseCyclesIntegral(sv->carrier_phase_cycles_integral); - lsv.setCarrierPhaseCyclesFraction(sv->carrier_phase_cycles_fraction); - lsv.setFineSpeed(sv->fine_speed); - lsv.setFineSpeedUncertainty(sv->fine_speed_uncertainty); - lsv.setCycleSlipCount(sv->cycle_slip_count); - - auto status = lsv.initMeasurementStatus(); - parse_measurement_status_common(sv->measurement_status, status); - - status.setGpsRoundRobinRxDiversity(sv->measurement_status & (1 << 18)); - status.setGpsRxDiversity(sv->measurement_status & (1 << 19)); - status.setGpsLowBandwidthRxDiversityCombined(sv->measurement_status & (1 << 20)); - status.setGpsHighBandwidthNu4(sv->measurement_status & (1 << 21)); - status.setGpsHighBandwidthNu8(sv->measurement_status & (1 << 22)); - status.setGpsHighBandwidthUniform(sv->measurement_status & (1 << 23)); - - status.setMultipathEstimateIsValid(sv->misc_status & (1 << 0)); - status.setDirectionIsValid(sv->misc_status & (1 << 1)); - -#ifdef RAWGPS_TEST - // if (sv->measurement_status & (1 << 27)) printf("%d\n", sv->unfiltered_measurement_integral); - printf("GPS %03d %d %d 0x%08X o: %02x go: %02x fs: %02x cn: %02x pd: %02x cs: %02x po: %02x ms: %08x ms2: %08x me: %08x az: %08x el: %08x fc: %08x\n", - sv->sv_id, - !!(sv->measurement_status & (1 << 27)), - sv->unfiltered_measurement_integral, sv->unfiltered_measurement_integral, - sv->observations, - sv->good_observations, - sv->filter_stages, - sv->carrier_noise, - sv->predetect_interval, - sv->cycle_slip_count, - sv->postdetections, - sv->measurement_status, - sv->misc_status, - sv->multipath_estimate, - *(uint32_t*)&sv->azimuth, - *(uint32_t*)&sv->elevation, - report->f_count - ); -#endif - - } - - break; - } - case LOG_GNSS_GLONASS_MEASUREMENT_REPORT: { - uint8_t version = log_data[0]; - assert(version == 0); - - assert(len >= sizeof(GNSSGlonassMeasurementReportv0)); - const GNSSGlonassMeasurementReportv0* report = (const GNSSGlonassMeasurementReportv0*)ptr; - - auto lreport = qcomGnss.initMeasurementReport(); - lreport.setSource(cereal::QcomGnss::MeasurementSource::GLONASS); - lreport.setFCount(report->f_count); - lreport.setGlonassCycleNumber(report->glonass_cycle_number); - lreport.setGlonassNumberOfDays(report->glonass_number_of_days); - lreport.setMilliseconds(report->milliseconds); - lreport.setTimeBias(report->time_bias); - lreport.setClockTimeUncertainty(report->clock_time_uncertainty); - lreport.setClockFrequencyBias(report->clock_frequency_bias); - lreport.setClockFrequencyUncertainty(report->clock_frequency_uncertainty); - - auto lsvs = lreport.initSv(report->sv_count); - for (int i=0; isv_count; i++) { - auto lsv = lsvs[i]; - const GNSSGlonassMeasurementReportv0_SV *sv = &report->sv[i]; - - lsv.setSvId(sv->sv_id); - lsv.setObservationState(cereal::QcomGnss::SVObservationState(sv->observation_state)); - lsv.setObservations(sv->observations); - lsv.setGoodObservations(sv->good_observations); - lsv.setGlonassFrequencyIndex(sv->frequency_index); - lsv.setGlonassHemmingErrorCount(sv->hemming_error_count); - lsv.setFilterStages(sv->filter_stages); - lsv.setCarrierNoise(sv->carrier_noise); - lsv.setLatency(sv->latency); - lsv.setPredetectInterval(sv->predetect_interval); - lsv.setPostdetections(sv->postdetections); - lsv.setUnfilteredMeasurementIntegral(sv->unfiltered_measurement_integral); - lsv.setUnfilteredMeasurementFraction(sv->unfiltered_measurement_fraction); - lsv.setUnfilteredTimeUncertainty(sv->unfiltered_time_uncertainty); - lsv.setUnfilteredSpeed(sv->unfiltered_speed); - lsv.setUnfilteredSpeedUncertainty(sv->unfiltered_speed_uncertainty); - - lsv.setMultipathEstimate(sv->multipath_estimate); - lsv.setAzimuth(sv->azimuth); - lsv.setElevation(sv->elevation); - lsv.setCarrierPhaseCyclesIntegral(sv->carrier_phase_cycles_integral); - lsv.setCarrierPhaseCyclesFraction(sv->carrier_phase_cycles_fraction); - lsv.setFineSpeed(sv->fine_speed); - lsv.setFineSpeedUncertainty(sv->fine_speed_uncertainty); - lsv.setCycleSlipCount(sv->cycle_slip_count); - - auto status = lsv.initMeasurementStatus(); - parse_measurement_status_common(sv->measurement_status, status); - - status.setGlonassMeanderBitEdgeValid(sv->measurement_status & (1 << 16)); - status.setGlonassTimeMarkValid(sv->measurement_status & (1 << 17)); - - status.setMultipathEstimateIsValid(sv->misc_status & (1 << 0)); - status.setDirectionIsValid(sv->misc_status & (1 << 1)); - - -#ifdef RAWGPS_TEST - // if (sv->measurement_status & (1 << 27)) printf("%d\n", sv->unfiltered_measurement_integral); - printf("GLO %03d %02x %d %d 0x%08X o: %02x go: %02x fs: %02x cn: %02x pd: %02x cs: %02x po: %02x ms: %08x ms2: %08x me: %08x az: %08x el: %08x fc: %08x\n", - sv->sv_id, sv->frequency_index & 0xff, - !!(sv->measurement_status & (1 << 27)), - sv->unfiltered_measurement_integral, sv->unfiltered_measurement_integral, - sv->observations, - sv->good_observations, - sv->filter_stages, - sv->carrier_noise, - sv->predetect_interval, - sv->cycle_slip_count, - sv->postdetections, - sv->measurement_status, - sv->misc_status, - sv->multipath_estimate, - *(uint32_t*)&sv->azimuth, - *(uint32_t*)&sv->elevation, - report->f_count - ); -#endif - - } - break; - } - case LOG_GNSS_OEMDRE_MEASUREMENMT_REPORT: { - // hexdump(ptr, len); - - uint8_t version = log_data[0]; - assert(version == 2); - - assert(len >= sizeof(GNSSOemdreMeasurementReportv2)); - const GNSSOemdreMeasurementReportv2* report = (const GNSSOemdreMeasurementReportv2*)ptr; - - - auto lreport = qcomGnss.initDrMeasurementReport(); - - lreport.setReason(report->reason); - lreport.setSeqNum(report->seq_num); - lreport.setSeqMax(report->seq_max); - lreport.setRfLoss(report->rf_loss); - lreport.setSystemRtcValid(report->system_rtc_valid); - lreport.setFCount(report->f_count); - lreport.setClockResets(report->clock_resets); - lreport.setSystemRtcTime(report->system_rtc_time); - - lreport.setGpsLeapSeconds(report->gps_leap_seconds); - lreport.setGpsLeapSecondsUncertainty(report->gps_leap_seconds_uncertainty); - lreport.setGpsToGlonassTimeBiasMilliseconds(report->gps_to_glonass_time_bias_milliseconds); - lreport.setGpsToGlonassTimeBiasMillisecondsUncertainty(report->gps_to_glonass_time_bias_milliseconds_uncertainty); - - lreport.setGpsWeek(report->gps_week); - lreport.setGpsMilliseconds(report->gps_milliseconds); - lreport.setGpsTimeBiasMs(report->gps_time_bias); - lreport.setGpsClockTimeUncertaintyMs(report->gps_clock_time_uncertainty); - lreport.setGpsClockSource(report->gps_clock_source); - - lreport.setGlonassClockSource(report->glonass_clock_source); - lreport.setGlonassYear(report->glonass_year); - lreport.setGlonassDay(report->glonass_day); - lreport.setGlonassMilliseconds(report->glonass_milliseconds); - lreport.setGlonassTimeBias(report->glonass_time_bias); - lreport.setGlonassClockTimeUncertainty(report->glonass_clock_time_uncertainty); - - lreport.setClockFrequencyBias(report->clock_frequency_bias); - lreport.setClockFrequencyUncertainty(report->clock_frequency_uncertainty); - lreport.setFrequencySource(report->frequency_source); - - lreport.setSource(cereal::QcomGnss::MeasurementSource(report->source)); - - auto lsvs = lreport.initSv(report->sv_count); - - // for (int i=0; isv_count; i++) { - // GNSSOemdreMeasurement *sv = &report->gps[i]; - // if (!(sv->measurement_status & (1 << 27))) continue; - // printf("oemdre %03d %d %f\n", sv->sv_id, sv->unfiltered_measurement_integral, sv->unfiltered_measurement_fraction); - // } - for (int i=0; isv_count; i++) { - auto lsv = lsvs[i]; - const GNSSOemdreMeasurement *sv = &report->measurements[i]; - - lsv.setSvId(sv->sv_id); - lsv.setGlonassFrequencyIndex(sv->glonass_frequency_index); - lsv.setObservationState(cereal::QcomGnss::SVObservationState(sv->observation_state)); - lsv.setObservations(sv->observations); - lsv.setGoodObservations(sv->good_observations); - lsv.setFilterStages(sv->filter_stages); - lsv.setPredetectInterval(sv->predetect_interval); - lsv.setCycleSlipCount(sv->cycle_slip_count); - lsv.setPostdetections(sv->postdetections); - - auto status = lsv.initMeasurementStatus(); - parse_measurement_status_common(sv->measurement_status, status); - - status.setMultipathEstimateIsValid(sv->multipath_estimate_valid); - status.setDirectionIsValid(sv->direction_valid); - - lsv.setCarrierNoise(sv->carrier_noise); - lsv.setRfLoss(sv->rf_loss); - lsv.setLatency(sv->latency); - - lsv.setFilteredMeasurementFraction(sv->filtered_measurement_fraction); - lsv.setFilteredMeasurementIntegral(sv->filtered_measurement_integral); - lsv.setFilteredTimeUncertainty(sv->filtered_time_uncertainty); - lsv.setFilteredSpeed(sv->filtered_speed); - lsv.setFilteredSpeedUncertainty(sv->filtered_speed_uncertainty); - - lsv.setUnfilteredMeasurementFraction(sv->unfiltered_measurement_fraction); - lsv.setUnfilteredMeasurementIntegral(sv->unfiltered_measurement_integral); - lsv.setUnfilteredTimeUncertainty(sv->unfiltered_time_uncertainty); - lsv.setUnfilteredSpeed(sv->unfiltered_speed); - lsv.setUnfilteredSpeedUncertainty(sv->unfiltered_speed_uncertainty); - - lsv.setMultipathEstimate(sv->multipath_estimate); - lsv.setAzimuth(sv->azimuth); - lsv.setElevation(sv->elevation); - lsv.setDopplerAcceleration(sv->doppler_acceleration); - lsv.setFineSpeed(sv->fine_speed); - lsv.setFineSpeedUncertainty(sv->fine_speed_uncertainty); - - lsv.setCarrierPhase(sv->carrier_phase); - lsv.setFCount(sv->f_count); - - lsv.setParityErrorCount(sv->parity_error_count); - lsv.setGoodParity(sv->good_parity); - - } - break; - } - case LOG_GNSS_OEMDRE_SVPOLY_REPORT: { - uint8_t version = log_data[0]; - assert(version == 2); - - assert(len >= sizeof(GNSSOemdreSVPolyReportv2)); - const GNSSOemdreSVPolyReportv2* report = (const GNSSOemdreSVPolyReportv2*)ptr; - - auto lreport = qcomGnss.initDrSvPoly(); - - lreport.setSvId(report->sv_id); - lreport.setFrequencyIndex(report->frequency_index); - - lreport.setHasPosition(report->flags & 1); - lreport.setHasIono(report->flags & 2); - lreport.setHasTropo(report->flags & 4); - lreport.setHasElevation(report->flags & 8); - lreport.setPolyFromXtra(report->flags & 16); - lreport.setHasSbasIono(report->flags & 32); - - lreport.setIode(report->iode); - lreport.setT0(report->t0); - - kj::ArrayPtr xyz0(report->xyz0, 3); - lreport.setXyz0(xyz0); - - kj::ArrayPtr xyzN(report->xyzN, 9); - lreport.setXyzN(xyzN); - - kj::ArrayPtr other(report->other, 4); - lreport.setOther(other); - - lreport.setPositionUncertainty(report->position_uncertainty); - lreport.setIonoDelay(report->iono_delay); - lreport.setIonoDot(report->iono_dot); - lreport.setSbasIonoDelay(report->sbas_iono_delay); - lreport.setSbasIonoDot(report->sbas_iono_dot); - lreport.setTropoDelay(report->tropo_delay); - lreport.setElevation(report->elevation); - lreport.setElevationDot(report->elevation_dot); - lreport.setElevationUncertainty(report->elevation_uncertainty); - - kj::ArrayPtr velocity_coeff(report->velocity_coeff, 12); - lreport.setVelocityCoeff(velocity_coeff); - - break; - } - default: - // printf("%04x\n", log_header->code); - // hexdump(ptr, len); - - qcomGnss.setRawLog(kj::arrayPtr(ptr, len)); - - break; - } - - auto words = capnp::messageToFlatArray(msg); - auto bytes = words.asBytes(); - rawgps_publisher->send((char*)bytes.begin(), bytes.size()); -} - -static void handle_event(unsigned char *ptr, int len) { - // printf("EVENT\n"); -} - -static uint16_t log_codes[] = { - LOG_GNSS_CLOCK_REPORT, - LOG_GNSS_GPS_MEASUREMENT_REPORT, - LOG_GNSS_GLONASS_MEASUREMENT_REPORT, - LOG_GNSS_OEMDRE_MEASUREMENMT_REPORT, - LOG_GNSS_OEMDRE_SVPOLY_REPORT, - - // unparsed: - LOG_GNSS_POSITION_REPORT, - - LOG_GNSS_BDS_MEASUREMENT_REPORT, // these are missing by might as well try to catch them anyway - LOG_GNSS_GAL_MEASUREMENT_REPORT, - - LOG_GNSS_ME_DPO_STATUS, - LOG_GNSS_CD_DB_REPORT, - LOG_GNSS_PRX_RF_HW_STATUS_REPORT, - LOG_CGPS_SLOW_CLOCK_CLIB_REPORT, - LOG_GNSS_CONFIGURATION_STATE, -}; - - -struct SendDiagSyncState { - sem_t sem; - int len; -}; - -static void diag_send_sync_cb(unsigned char *ptr, int len, void *data_ptr) { - SendDiagSyncState *s = (SendDiagSyncState*)data_ptr; - s->len = len; - sem_post(&s->sem); -} - -static int diag_send_sync(int client_id, unsigned char* req_pkt, size_t pkt_len, - unsigned char* res_pkt, size_t res_pkt_size) { - - SendDiagSyncState s = {0}; - sem_init(&s.sem, 0, 0); - - int err = diag_send_dci_async_req(client_id, req_pkt, pkt_len, res_pkt, res_pkt_size, - diag_send_sync_cb, &s); - assert(err == DIAG_DCI_NO_ERROR); - - sem_wait(&s.sem); - return s.len; -} - -static int oemdre_on(int client_id) { - // enable OEM DR - unsigned char res_pkt[DIAG_MAX_RX_PKT_SIZ]; - - GpsOemControlReq req_pkt = { - .cmd_code = DIAG_SUBSYS_CMD, - .subsys_id = DIAG_SUBSYS_GPS, - .subsys_cmd_code = CGPS_DIAG_PDAPI_CMD, - .gps_cmd_code = CGPS_OEM_CONTROL, - .version = 1, - - .oem_feature = GPSDIAG_OEMFEATURE_DRE, - .oem_command = GPSDIAG_OEM_DRE_ON, - }; - - int res_len = diag_send_sync(client_id, (unsigned char*)&req_pkt, sizeof(req_pkt), - res_pkt, sizeof(res_pkt)); - GpsOemControlResp *resp = (GpsOemControlResp*)res_pkt; - - if (res_len != sizeof(GpsOemControlResp) - || resp->cmd_code != DIAG_SUBSYS_CMD - || resp->subsys_id != DIAG_SUBSYS_GPS - || resp->subsys_cmd_code != CGPS_DIAG_PDAPI_CMD - || resp->gps_cmd_code != CGPS_OEM_CONTROL - || resp->oem_feature != GPSDIAG_OEMFEATURE_DRE - || resp->oem_command != GPSDIAG_OEM_DRE_ON) { - LOGW("oemdre_on: bad response!"); - return -1; - } - - return resp->resp_result; -} - -static void efs_sync(int client_id) { - unsigned char res_pkt[DIAG_MAX_RX_PKT_SIZ]; - - Efs2DiagSyncReq req_pkt = { - .cmd_code = DIAG_SUBSYS_CMD, - .subsys_id = DIAG_SUBSYS_FS, - .subsys_cmd_code = EFS2_DIAG_SYNC_NO_WAIT, - .sequence_num = (uint16_t)(rand() % 100), - }; - req_pkt.path[0] = '/'; - req_pkt.path[1] = 0; - - int res_len = diag_send_sync(client_id, (unsigned char*)&req_pkt, sizeof(req_pkt), - res_pkt, sizeof(res_pkt)); - Efs2DiagSyncResp *resp = (Efs2DiagSyncResp*)res_pkt; - - if (res_len != sizeof(Efs2DiagSyncResp) - || resp->cmd_code != DIAG_SUBSYS_CMD - || resp->subsys_id != DIAG_SUBSYS_FS - || resp->subsys_cmd_code != EFS2_DIAG_SYNC_NO_WAIT) { - LOGW("efs_sync: bad response!"); - return; - } - if (resp->diag_errno != 0) { - LOGW("efs_sync: error %d", resp->diag_errno); - } -} - -static uint32_t nv_read_u32(int client_id, uint16_t nv_id) { - NvPacket req = { - .cmd_code = DIAG_NV_READ_F, - .nv_id = nv_id, - }; - NvPacket resp = {0}; - - int res_len = diag_send_sync(client_id, (unsigned char*)&req, sizeof(req), - (unsigned char*)&resp, sizeof(resp)); - - // hexdump((uint8_t*)&resp, res_len); - - if (resp.cmd_code != DIAG_NV_READ_F - || resp.nv_id != nv_id) { - LOGW("nv_read_u32: diag command failed"); - return 0; - } - - if (resp.status != NV_DONE) { - LOGW("nv_read_u32: read failed: %d", resp.status); - return 0; - } - return *(uint32_t*)resp.data; -} - -static bool nv_write_u32(int client_id, uint16_t nv_id, uint32_t val) { - NvPacket req = { - .cmd_code = DIAG_NV_WRITE_F, - .nv_id = nv_id, - }; - *(uint32_t*)req.data = val; - - NvPacket resp = {0}; - int res_len = diag_send_sync(client_id, (unsigned char*)&req, sizeof(req), - (unsigned char*)&resp, sizeof(resp)); - - // hexdump((uint8_t*)&resp, res_len); - - if (resp.cmd_code != DIAG_NV_WRITE_F - || resp.nv_id != nv_id) { - LOGW("nv_write_u32: diag command failed"); - return false; - } - - if (resp.status != NV_DONE) { - LOGW("nv_write_u32: write failed: %d", resp.status); - return false; - } - - return true; -} - -static void nav_config(int client_id, uint8_t config) { - unsigned char res_pkt[DIAG_MAX_RX_PKT_SIZ]; - - GpsNavConfigReq req_pkt = { - .cmd_code = DIAG_SUBSYS_CMD_VER_2, - .subsys_id = DIAG_SUBSYS_GPS, - .subsys_cmd_code = TM_DIAG_NAV_CONFIG_CMD, - .desired_config = config, - }; - - int res_len = diag_send_sync(client_id, (unsigned char*)&req_pkt, sizeof(req_pkt), - res_pkt, sizeof(res_pkt)); - GpsNavConfigResp *resp = (GpsNavConfigResp*)res_pkt; - - if (res_len != sizeof(GpsNavConfigResp) - || resp->cmd_code != DIAG_SUBSYS_CMD_VER_2 - || resp->subsys_id != DIAG_SUBSYS_GPS - || resp->subsys_cmd_code != TM_DIAG_NAV_CONFIG_CMD) { - LOGW("nav_config: bad response!"); - return; - } - LOG("nav config: %04x %04x", resp->supported_config, resp->actual_config); -} - -void rawgps_init() { - int err; - - rawgps_context = Context::create(); - rawgps_publisher = PubSocket::create(rawgps_context, "qcomGnss"); - assert(rawgps_publisher != NULL); - - bool init_success = Diag_LSM_Init(NULL); - assert(init_success); - - uint16_t list = DIAG_CON_APSS | DIAG_CON_MPSS; - int signal_type = SIGCONT; - err = diag_register_dci_client(&client_id, &list, 0, &signal_type); - assert(err == DIAG_DCI_NO_ERROR); - - { - uint32_t oem_features = nv_read_u32(client_id, NV_GNSS_OEM_FEATURE_MASK); - LOG("oem features: %08x", oem_features); - - if (!(oem_features & NV_GNSS_OEM_FEATURE_MASK_OEMDRE)) { - LOG("OEMDRE feature disabled, enabling..."); - nv_write_u32(client_id, NV_GNSS_OEM_FEATURE_MASK, NV_GNSS_OEM_FEATURE_MASK_OEMDRE); - efs_sync(client_id); - } - - int oemdre_status = oemdre_on(client_id); - LOG("oemdre status: %d", oemdre_status); - } - - { - // make sure GNSS duty cycling is off - uint32_t dpo = nv_read_u32(client_id, NV_CGPS_DPO_CONTROL); - LOG("dpo: %d", dpo); - if (dpo != 0) { - nv_write_u32(client_id, NV_CGPS_DPO_CONTROL, 0); - efs_sync(client_id); - } - } - - // enable beidou - // nav_config(client_id, 0x13); // 0b10011 - - err = diag_register_dci_stream(handle_log, handle_event); - assert(err == DIAG_DCI_NO_ERROR); - - err = diag_log_stream_config(client_id, true, log_codes, ARRAYSIZE(log_codes)); - assert(err == DIAG_DCI_NO_ERROR); -} - -void rawgps_destroy() { - - int err; - - err = diag_log_stream_config(client_id, false, log_codes, ARRAYSIZE(log_codes)); - assert(err == DIAG_DCI_NO_ERROR); - - err = diag_release_dci_client(&client_id); - assert(err == DIAG_DCI_NO_ERROR); - - Diag_LSM_DeInit(); - - delete rawgps_publisher; - delete rawgps_context; -} - - -#ifdef RAWGPS_TEST -int main() { - int err = 0; - rawgps_init(); - - while(1) { - usleep(100000); - } - - rawgps_destroy(); - return 0; -} -#endif diff --git a/selfdrive/sensord/rawgps.h b/selfdrive/sensord/rawgps.h deleted file mode 100644 index 13d23cd2..00000000 --- a/selfdrive/sensord/rawgps.h +++ /dev/null @@ -1,7 +0,0 @@ -#ifndef RAWGPS_H -#define RAWGPS_H - -void rawgps_init(); -void rawgps_destroy(); - -#endif diff --git a/selfdrive/sensord/sensors.cc b/selfdrive/sensord/sensors.cc index 952a4863..5e8741db 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 90962cb5..48447527 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 @@ class LogMessageHandler(logging.Handler): # 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/process_replay.py b/selfdrive/test/process_replay/process_replay.py index b8288987..106d871e 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -92,6 +92,7 @@ class FakeSubMaster(messaging.SubMaster): wait_for_event(self.update_ready) self.update_ready.clear() + def update_msgs(self, cur_time, msgs): wait_for_event(self.update_called) self.update_called.clear() @@ -186,8 +187,14 @@ def radar_rcv_callback(msg, CP, cfg, fsm): def calibration_rcv_callback(msg, CP, cfg, fsm): # calibrationd publishes 1 calibrationData every 5 cameraOdometry packets. # should_recv always true to increment frame - recv_socks = ["liveCalibration"] if (fsm.frame + 1) % 5 == 0 else [] - return recv_socks, True + if msg.which() == 'carState': + if ((fsm.frame +1)% 25) == 0: + recv_socks = ["liveCalibration"] + else: + recv_socks = [] + return recv_socks, True + else: + return [], False CONFIGS = [ @@ -225,7 +232,8 @@ CONFIGS = [ ProcessConfig( proc_name="calibrationd", pub_sub={ - "cameraOdometry": ["liveCalibration"] + "carState": ["liveCalibration"], + "cameraOdometry": [] }, ignore=["logMonoTime", "valid"], init_callback=get_car_params, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 150649e6..64390dba 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -b98f4b6858a21fd6602fb41c6a21f69b1ca81921 \ No newline at end of file +79bba4ca90f4c7d6c4fddcd9a916f94966a40dca \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index f57fc83c..06f1b9bc 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -1,9 +1,7 @@ #!/usr/bin/env python3 import argparse import os -import requests import sys -import tempfile from selfdrive.car.car_helpers import interface_names from selfdrive.test.process_replay.process_replay import replay_process, CONFIGS @@ -19,23 +17,24 @@ segments = [ ("TOYOTA", "77611a1fac303767|2020-02-29--13-29-33--3"), # TOYOTA.COROLLA_TSS2 ("GM", "7cc2a8365b4dd8a9|2018-12-02--12-10-44--2"), # GM.ACADIA ("CHRYSLER", "b6849f5cf2c926b1|2020-02-28--07-29-48--13"), # CHRYSLER.PACIFICA - ("HYUNDAI", "38bfd238edecbcd7|2018-08-29--22-02-15--4"), # HYUNDAI.SANTA_FE + ("HYUNDAI", "5b7c365c50084530|2020-04-15--16-13-24--3"), # HYUNDAI.SONATA #("CHRYSLER", "b6e1317e1bfbefa6|2020-03-04--13-11-40"), # CHRYSLER.JEEP_CHEROKEE ("SUBARU", "7873afaf022d36e2|2019-07-03--18-46-44--0"), # SUBARU.IMPREZA ("VOLKSWAGEN", "76b83eb0245de90e|2020-03-05--19-16-05--3"), # VW.GOLF # Enable when port is tested and dascamOnly is no longer set - # ("NISSAN", "fbbfa6af821552b9|2020-03-03--08-09-43--0"), # NISSAN.XTRAIL + ("NISSAN", "fbbfa6af821552b9|2020-03-03--08-09-43--0"), # NISSAN.XTRAIL ] # ford doesn't need to be tested until a full port is done -excluded_interfaces = ["mock", "ford", "nissan"] +excluded_interfaces = ["mock", "ford"] BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/" # run the full test (including checks) when no args given FULL_TEST = len(sys.argv) <= 1 + def get_segment(segment_name, original=True): route_name, segment_num = segment_name.rsplit("--", 1) if original: @@ -44,25 +43,13 @@ def get_segment(segment_name, original=True): process_replay_dir = os.path.dirname(os.path.abspath(__file__)) model_ref_commit = open(os.path.join(process_replay_dir, "model_ref_commit")).read().strip() rlog_url = BASE_URL + "%s/%s/rlog_%s.bz2" % (route_name.replace("|", "/"), segment_num, model_ref_commit) - req = requests.get(rlog_url) - assert req.status_code == 200, ("Failed to download log for %s" % segment_name) - with tempfile.NamedTemporaryFile(delete=False, suffix=".bz2") as f: - f.write(req.content) - return f.name + return rlog_url + def test_process(cfg, lr, cmp_log_fn, ignore_fields=[], ignore_msgs=[]): - if not os.path.isfile(cmp_log_fn): - req = requests.get(BASE_URL + os.path.basename(cmp_log_fn)) - assert req.status_code == 200, ("Failed to download %s" % cmp_log_fn) - - with tempfile.NamedTemporaryFile(suffix=".bz2") as f: - f.write(req.content) - f.flush() - f.seek(0) - cmp_log_msgs = list(LogReader(f.name)) - else: - cmp_log_msgs = list(LogReader(cmp_log_fn)) + url = BASE_URL + os.path.basename(cmp_log_fn) + cmp_log_msgs = list(LogReader(url)) log_msgs = replay_process(cfg, lr) @@ -80,6 +67,7 @@ def test_process(cfg, lr, cmp_log_fn, ignore_fields=[], ignore_msgs=[]): return compare_logs(cmp_log_msgs, log_msgs, ignore_fields+cfg.ignore, ignore_msgs) + def format_diff(results, ref_commit): diff1, diff2 = "", "" diff2 += "***** tested against commit %s *****\n" % ref_commit @@ -166,7 +154,6 @@ if __name__ == "__main__": cmp_log_fn = os.path.join(process_replay_dir, "%s_%s_%s.bz2" % (segment, cfg.proc_name, ref_commit)) results[segment][cfg.proc_name] = test_process(cfg, lr, cmp_log_fn, args.ignore_fields, args.ignore_msgs) - os.remove(rlog_fn) diff1, diff2, failed = format_diff(results, ref_commit) with open(os.path.join(process_replay_dir, "diff.txt"), "w") as f: diff --git a/selfdrive/test/test_car_models.py b/selfdrive/test/test_car_models.py index fd7a8e60..63f42cf5 100755 --- a/selfdrive/test/test_car_models.py +++ b/selfdrive/test/test_car_models.py @@ -57,6 +57,7 @@ def get_route_log(route_name): print("failed to download test log %s" % route_name) sys.exit(-1) + routes = { "420a8e183f1aed48|2020-03-05--07-15-29": { 'carFingerprint': CHRYSLER.PACIFICA_2017_HYBRID, @@ -75,10 +76,6 @@ routes = { 'carFingerprint': FORD.FUSION, 'enableCamera': True, }, - "7ed9cdf8d0c5f43e|2018-05-17--09-31-36": { - 'carFingerprint': GM.CADILLAC_CT6, - 'enableCamera': True, - }, "c950e28c26b5b168|2018-05-30--22-03-41": { 'carFingerprint': GM.VOLT, 'enableCamera': True, @@ -109,6 +106,11 @@ routes = { 'carFingerprint': HONDA.FIT, 'enableCamera': True, }, + "03be5f2fd5c508d1|2020-04-19--18-44-15": { + 'carFingerprint': HONDA.HRV, + 'enableCamera': True, + 'fingerprintSource': 'fixed', + }, "2ac95059f70d76eb|2018-02-05--15-03-29": { 'carFingerprint': HONDA.ACURA_ILX, 'enableCamera': True, @@ -175,6 +177,14 @@ routes = { 'carFingerprint': HYUNDAI.KIA_OPTIMA, 'enableCamera': True, }, + "5b7c365c50084530|2020-04-15--16-13-24": { + 'carFingerprint': HYUNDAI.SONATA, + 'enableCamera': True, + }, + "9c917ba0d42ffe78|2020-04-17--12-43-19": { + 'carFingerprint': HYUNDAI.PALISADE, + 'enableCamera': True, + }, "f7b6be73e3dfd36c|2019-05-12--18-07-16": { 'carFingerprint': TOYOTA.AVALON, 'enableCamera': False, @@ -335,6 +345,10 @@ routes = { 'carFingerprint': NISSAN.XTRAIL, 'enableCamera': True, }, + "5b7c365c50084530|2020-03-25--22-10-13": { + 'carFingerprint': NISSAN.LEAF, + 'enableCamera': True, + }, } passive_routes = [ @@ -344,9 +358,6 @@ forced_dashcam_routes = [ # Ford fusion "f1b4c567731f4a1b|2018-04-18--11-29-37", "f1b4c567731f4a1b|2018-04-30--10-15-35", - - # Nissan - "fbbfa6af821552b9|2020-03-03--08-09-43", ] # TODO: add routes for these cars @@ -355,6 +366,7 @@ non_tested_cars = [ CHRYSLER.JEEP_CHEROKEE_2019, CHRYSLER.PACIFICA_2018, CHRYSLER.PACIFICA_2018_HYBRID, + CHRYSLER.PACIFICA_2020, GM.CADILLAC_ATS, GM.HOLDEN_ASTRA, GM.MALIBU, @@ -362,13 +374,24 @@ non_tested_cars = [ HONDA.CRV, HONDA.RIDGELINE, HYUNDAI.ELANTRA, - HYUNDAI.GENESIS, + HYUNDAI.ELANTRA_GT_I30, + HYUNDAI.GENESIS_G80, + HYUNDAI.GENESIS_G90, + HYUNDAI.HYUNDAI_GENESIS, + HYUNDAI.IONIQ, + HYUNDAI.IONIQ_EV_LTD, + HYUNDAI.KIA_FORTE, + HYUNDAI.KIA_OPTIMA, + HYUNDAI.KIA_OPTIMA_H, HYUNDAI.KIA_SORENTO, HYUNDAI.KIA_STINGER, + HYUNDAI.KONA, + HYUNDAI.KONA_EV, TOYOTA.CAMRYH, TOYOTA.CHR, TOYOTA.CHRH, TOYOTA.HIGHLANDERH, + TOYOTA.HIGHLANDERH_TSS2, ] if __name__ == "__main__": diff --git a/selfdrive/test/test_fingerprints.py b/selfdrive/test/test_fingerprints.py index b0c4d451..8877255c 100755 --- a/selfdrive/test/test_fingerprints.py +++ b/selfdrive/test/test_fingerprints.py @@ -8,7 +8,7 @@ from selfdrive.car.fingerprints import IGNORED_FINGERPRINTS # (addr, len) CAN_IGNITION_MSGS = { 'gm': [(0x1F1, 8), (0x160, 5)], - 'tesla' : [(0x348, 8)], + #'tesla' : [(0x348, 8)], } def _get_fingerprints(): diff --git a/selfdrive/test/test_openpilot.py b/selfdrive/test/test_openpilot.py index 3e9a3175..624524de 100644 --- a/selfdrive/test/test_openpilot.py +++ b/selfdrive/test/test_openpilot.py @@ -4,6 +4,7 @@ os.environ['FAKEUPLOAD'] = "1" from common.apk import update_apks, start_offroad, pm_apply_packages, android_packages from common.params import Params +from common.realtime import sec_since_boot from common.testing import phone_only from selfdrive.manager import manager_init, manager_prepare from selfdrive.manager import start_managed_process, kill_managed_process, get_running @@ -14,6 +15,7 @@ import requests import signal import subprocess import time +from datetime import datetime, timedelta DID_INIT = False @@ -108,6 +110,7 @@ def test_uploader(): @phone_only def test_athena(): print("ATHENA") + start = sec_since_boot() start_daemon_process("manage_athenad") params = Params() manage_athenad_pid = params.get("AthenadPid") @@ -155,7 +158,7 @@ def test_athena(): else: print(f'athena_post failed {e}. retrying...') - def expect_athena_registers(): + def expect_athena_registers(test_t0): resp = athena_post({ "method": "echo", "params": ["hello"], @@ -164,6 +167,10 @@ def test_athena(): }, max_retries=12, wait=5) assert resp.get('result') == "hello", f'Athena failed to register ({resp})' + last_pingtime = params.get("LastAthenaPingTime", encoding='utf8') + assert last_pingtime, last_pingtime + assert ((int(last_pingtime)/1e9) - test_t0) < (sec_since_boot() - test_t0) + try: athenad_pid = expect_athena_starts() # kill athenad and ensure it is restarted (check_output will throw if it is not) @@ -174,7 +181,7 @@ def test_athena(): print('WARNING: COMMA_JWT env not set, will not test requests to athena.comma.ai') return - expect_athena_registers() + expect_athena_registers(start) print("ATHENA: getSimInfo") resp = athena_post({ diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index 376ec070..378bee6f 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -58,12 +58,11 @@ def panda_current_to_actual_current(panda_current): class PowerMonitoring: - def __init__(self, is_uno): + def __init__(self): self.last_measurement_time = None # Used for integration delta self.power_used_uWh = 0 # Integrated power usage in uWh since going into offroad self.next_pulsed_measurement_time = None self.integration_lock = threading.Lock() - self.is_uno = is_uno # Calculation tick def calculate(self, health): @@ -76,7 +75,8 @@ class PowerMonitoring: # 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): + 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 @@ -89,6 +89,7 @@ class PowerMonitoring: 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': @@ -131,7 +132,7 @@ class PowerMonitoring: self.next_pulsed_measurement_time = None return - elif self.next_pulsed_measurement_time is None and not self.is_uno: + 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 @@ -151,7 +152,10 @@ class PowerMonitoring: try: if self.last_measurement_time: integration_time_h = (t - self.last_measurement_time) / 3600 - self.power_used_uWh += (current_power * 1000000) * integration_time_h + 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") diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index f273a40b..1d69547c 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python3.7 +#!/usr/bin/env python3 import os import json import copy @@ -8,7 +8,7 @@ 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 @@ -25,8 +25,10 @@ ThermalStatus = log.ThermalData.ThermalStatus NetworkType = log.ThermalData.NetworkType NetworkStrength = log.ThermalData.NetworkStrength CURRENT_TAU = 15. # 15s time constant +CPU_TEMP_TAU = 5. # 5s 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 +DISCONNECT_TIMEOUT = 5. # wait 5 seconds before going offroad after disconnect so you get an alert LEON = False last_eon_fan_val = None @@ -173,21 +175,18 @@ 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 params = Params() - pm = PowerMonitoring(is_uno) + pm = PowerMonitoring() + no_panda_cnt = 0 while 1: health = messaging.recv_sock(health_sock, wait=True) @@ -195,14 +194,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 +254,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 +290,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 +314,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 +322,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 +351,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 +381,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: @@ -379,7 +406,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 55a09233..022f9fdf --- 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,47 @@ from raven.transport.http import HTTPTransport 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() + contents = 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) + # Get summary for sentry title + message = " ".join(contents.split('\n')[5:7]) - 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] + # Cut off pid/tid, since that varies per run + name_idx = message.find('name') + if name_idx >= 0: + message = message[name_idx:] + # Cut off fault addr + fault_idx = message.find(', fault addr') + if fault_idx >= 0: + message = message[:fault_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 +62,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 e0c353b4..88bde059 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -1,7 +1,7 @@ Import('env', 'arch', 'common', 'messaging', 'gpucommon', 'visionipc', 'cereal') src = ['ui.cc', 'paint.cc', 'sidebar.cc', '#phonelibs/nanovg/nanovg.c'] -libs = [common, 'zmq', 'czmq', 'capnp', 'capnp_c', 'm', cereal, messaging, gpucommon, visionipc] +libs = [common, 'zmq', 'czmq', 'capnp', 'kj', 'm', cereal, messaging, gpucommon, visionipc] if arch == "aarch64": src += ['sound.cc', 'slplay.c'] diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index df3e9d8d..4cb7966a 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -1,6 +1,6 @@ -#include #include "ui.hpp" - +#include +#include #include "common/util.h" #define NANOVG_GLES3_IMPLEMENTATION @@ -27,13 +27,6 @@ const uint8_t alert_colors[][4] = { [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. vec3 car_space_to_full_frame(const UIState *s, vec4 car_space_projective) { @@ -57,98 +50,99 @@ static float lerp(float v0, float v1, float t) { return (1 - t) * v0 + t * v1; } +static void ui_draw_text(NVGcontext *vg, float x, float y, const char* string, float size, NVGcolor color, int font){ + nvgFontFaceId(vg, font); + nvgFontSize(vg, size); + nvgFillColor(vg, color); + nvgText(vg, x, y, string, NULL); +} + static void draw_chevron(UIState *s, float x_in, float y_in, float sz, NVGcolor fillColor, NVGcolor glowColor) { - const UIScene *scene = &s->scene; - - nvgSave(s->vg); - - nvgTranslate(s->vg, 240.0f, 0.0); - nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); - nvgScale(s->vg, 2.0, 2.0); - nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height); - const vec4 p_car_space = (vec4){{x_in, y_in, 0., 1.}}; const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); + float x = p_full_frame.v[0]; + float y = p_full_frame.v[1]; + if (x < 0 || y < 0.){ + return; + } + sz *= 30; sz /= (x_in / 3 + 30); if (sz > 30) sz = 30; if (sz < 15) sz = 15; - - float x = p_full_frame.v[0]; - float y = p_full_frame.v[1]; - + // glow - nvgBeginPath(s->vg); float g_xo = sz/5; float g_yo = sz/10; - if (x >= 0 && y >= 0.) { - 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); - } + nvgBeginPath(s->vg); + 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); + nvgClosePath(s->vg); nvgFillColor(s->vg, glowColor); nvgFill(s->vg); // chevron nvgBeginPath(s->vg); - if (x >= 0 && y >= 0.) { - 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); - } + nvgMoveTo(s->vg, x+(sz*1.25), y+sz); + nvgLineTo(s->vg, x, y); + nvgLineTo(s->vg, x-(sz*1.25), y+sz); + nvgClosePath(s->vg); nvgFillColor(s->vg, fillColor); nvgFill(s->vg); +} - nvgRestore(s->vg); +static void ui_draw_circle_image(NVGcontext *vg, float x, float y, int size, int image, NVGcolor color, float img_alpha, int img_y = 0) { + const int img_size = size * 1.5; + nvgBeginPath(vg); + nvgCircle(vg, x, y + (bdr_s * 1.5), size); + nvgFillColor(vg, color); + nvgFill(vg); + ui_draw_image(vg, x - (img_size / 2), img_y ? img_y : y - (size / 4), img_size, img_size, image, img_alpha); +} + +static void ui_draw_circle_image(NVGcontext *vg, float x, float y, int size, int image, bool active) { + float bg_alpha = active ? 0.3f : 0.1f; + float img_alpha = active ? 1.0f : 0.15f; + ui_draw_circle_image(vg, x, y, size, image, nvgRGBA(0, 0, 0, (255 * bg_alpha)), img_alpha); } static void draw_lead(UIState *s, float d_rel, float v_rel, float y_rel){ - // Draw lead car indicator - float fillAlpha = 0; - float speedBuff = 10.; - float leadBuff = 40.; - if (d_rel < leadBuff) { - fillAlpha = 255*(1.0-(d_rel/leadBuff)); - if (v_rel < 0) { - fillAlpha += 255*(-1*(v_rel/speedBuff)); - } - fillAlpha = (int)(fmin(fillAlpha, 255)); + // Draw lead car indicator + float fillAlpha = 0; + float speedBuff = 10.; + float leadBuff = 40.; + if (d_rel < leadBuff) { + fillAlpha = 255*(1.0-(d_rel/leadBuff)); + if (v_rel < 0) { + fillAlpha += 255*(-1*(v_rel/speedBuff)); } - draw_chevron(s, d_rel, y_rel, 25, - nvgRGBA(201, 34, 49, fillAlpha), nvgRGBA(218, 202, 37, 255)); + fillAlpha = (int)(fmin(fillAlpha, 255)); + } + draw_chevron(s, d_rel, y_rel, 25, nvgRGBA(201, 34, 49, fillAlpha), COLOR_YELLOW); } static void ui_draw_lane_line(UIState *s, const model_path_vertices_data *pvd, NVGcolor color) { - nvgSave(s->vg); - nvgTranslate(s->vg, 240.0f, 0.0); // rgb-box space - nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); // zoom 2x - nvgScale(s->vg, 2.0, 2.0); - nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height); nvgBeginPath(s->vg); - bool started = false; for (int i=0; icnt; i++) { - if (pvd->v[i].x < 0 || pvd->v[i].y < 0.) { + float x = pvd->v[i].x; + float y = pvd->v[i].y; + if (x < 0 || y < 0.) { continue; } if (!started) { - nvgMoveTo(s->vg, pvd->v[i].x, pvd->v[i].y); + nvgMoveTo(s->vg, x, y); started = true; } else { - nvgLineTo(s->vg, pvd->v[i].x, pvd->v[i].y); + nvgLineTo(s->vg, x, y); } } - nvgClosePath(s->vg); nvgFillColor(s->vg, color); nvgFill(s->vg); - nvgRestore(s->vg); } static void update_track_data(UIState *s, bool is_mpc, track_vertices_data *pvd) { @@ -218,37 +212,21 @@ static void update_all_track_data(UIState *s) { static void ui_draw_track(UIState *s, bool is_mpc, track_vertices_data *pvd) { - const UIScene *scene = &s->scene; - const PathData path = scene->model.path; - const float *mpc_x_coords = &scene->mpc_x[0]; - const float *mpc_y_coords = &scene->mpc_y[0]; - - nvgSave(s->vg); - nvgTranslate(s->vg, 240.0f, 0.0); // rgb-box space - nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); // zoom 2x - nvgScale(s->vg, 2.0, 2.0); - nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height); nvgBeginPath(s->vg); - bool started = false; - float off = is_mpc?0.3:0.5; - float lead_d = scene->lead_d_rel*2.; - float path_height = is_mpc?(lead_d>5.)?fmin(lead_d, 25.)-fmin(lead_d*0.35, 10.):20. - :(lead_d>0.)?fmin(lead_d, 50.)-fmin(lead_d*0.35, 10.):49.; - int vi = 0; for(int i = 0;i < pvd->cnt;i++) { - if (pvd->v[i].x < 0 || pvd->v[i].y < 0) { + float x = pvd->v[i].x; + float y = pvd->v[i].y; + if (x < 0 || y < 0) { continue; } - if (!started) { - nvgMoveTo(s->vg, pvd->v[i].x, pvd->v[i].y); + nvgMoveTo(s->vg, x, y); started = true; } else { - nvgLineTo(s->vg, pvd->v[i].x, pvd->v[i].y); + nvgLineTo(s->vg, x, y); } } - nvgClosePath(s->vg); NVGpaint track_bg; @@ -260,12 +238,10 @@ 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); nvgFill(s->vg); - nvgRestore(s->vg); } static void draw_steering(UIState *s, float curvature) { @@ -281,7 +257,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 { @@ -400,6 +375,21 @@ static void ui_draw_world(UIState *s) { return; } + const int inner_height = viz_w*9/16; + const int ui_viz_rx = scene->ui_viz_rx; + const int ui_viz_rw = scene->ui_viz_rw; + const int ui_viz_ro = scene->ui_viz_ro; + + nvgSave(s->vg); + nvgScissor(s->vg, ui_viz_rx, box_y, ui_viz_rw, box_h); + + nvgTranslate(s->vg, ui_viz_rx+ui_viz_ro, box_y + (box_h-inner_height)/2.0); + nvgScale(s->vg, (float)viz_w / s->fb_w, (float)inner_height / s->fb_h); + nvgTranslate(s->vg, 240.0f, 0.0); + nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); + nvgScale(s->vg, 2.0, 2.0); + nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height); + // Draw lane edges and vision/mpc tracks ui_draw_vision_lanes(s); @@ -409,17 +399,13 @@ static void ui_draw_world(UIState *s) { if ((scene->lead_status2) && (fabs(scene->lead_d_rel - scene->lead_d_rel2) > 3.0)) { draw_lead(s, scene->lead_d_rel2, scene->lead_v_rel2, scene->lead_y_rel2); } + nvgRestore(s->vg); } static void ui_draw_vision_maxspeed(UIState *s) { /*if (!s->longitudinal_control){ return; }*/ - - const UIScene *scene = &s->scene; - int ui_viz_rx = scene->ui_viz_rx; - int ui_viz_rw = scene->ui_viz_rw; - char maxspeed_str[32]; float maxspeed = s->scene.v_cruise; int maxspeed_calc = maxspeed * 0.6225 + 0.5; @@ -439,7 +425,7 @@ static void ui_draw_vision_maxspeed(UIState *s) { int viz_maxspeed_w = 184; int viz_maxspeed_h = 202; - int viz_maxspeed_x = (ui_viz_rx + (bdr_s*2)); + int viz_maxspeed_x = (s->scene.ui_viz_rx + (bdr_s*2)); int viz_maxspeed_y = (box_y + (bdr_s*1.5)); int viz_maxspeed_xo = 180; @@ -451,62 +437,32 @@ static void ui_draw_vision_maxspeed(UIState *s) { #endif // Draw Background - nvgBeginPath(s->vg); - nvgRoundedRect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, 30); - if (is_set_over_limit) { - nvgFillColor(s->vg, nvgRGBA(218, 111, 37, 180)); - } else { - nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 100)); - } - nvgFill(s->vg); + ui_draw_rect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, + is_set_over_limit ? nvgRGBA(218, 111, 37, 180) : COLOR_BLACK_ALPHA(100), 30); // Draw Border - nvgBeginPath(s->vg); - nvgRoundedRect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, 20); + NVGcolor color = COLOR_WHITE_ALPHA(100); if (is_set_over_limit) { - nvgStrokeColor(s->vg, nvgRGBA(218, 111, 37, 255)); - } else if (is_speedlim_valid && !s->is_ego_over_limit) { - nvgStrokeColor(s->vg, nvgRGBA(255, 255, 255, 255)); - } else if (is_speedlim_valid && s->is_ego_over_limit) { - nvgStrokeColor(s->vg, nvgRGBA(255, 255, 255, 20)); - } else { - nvgStrokeColor(s->vg, nvgRGBA(255, 255, 255, 100)); + color = COLOR_OCHRE; + } else if (is_speedlim_valid) { + color = s->is_ego_over_limit ? COLOR_WHITE_ALPHA(20) : COLOR_WHITE; } - nvgStrokeWidth(s->vg, 10); - nvgStroke(s->vg); - // Draw "MAX" Text + ui_draw_rect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, color, 20, 10); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); - nvgFontFace(s->vg, "sans-regular"); - nvgFontSize(s->vg, 26*2.5); - if (is_cruise_set) { - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200)); - } else { - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 100)); - } - nvgText(s->vg, viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 148, "MAX", NULL); + const int text_x = viz_maxspeed_x + (viz_maxspeed_xo / 2) + (viz_maxspeed_w / 2); + ui_draw_text(s->vg, text_x, 148, "MAX", 26 * 2.5, COLOR_WHITE_ALPHA(is_cruise_set ? 200 : 100), s->font_sans_regular); - // Draw Speed Text - nvgFontFace(s->vg, "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)); - nvgText(s->vg, viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 242, maxspeed_str, NULL); + ui_draw_text(s->vg, text_x, 242, maxspeed_str, 48 * 2.5, COLOR_WHITE, s->font_sans_bold); } else { - nvgFontFace(s->vg, "sans-semibold"); - nvgFontSize(s->vg, 42*2.5); - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 100)); - nvgText(s->vg, viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 242, "N/A", NULL); + ui_draw_text(s->vg, text_x, 242, "N/A", 42 * 2.5, COLOR_WHITE_ALPHA(100), s->font_sans_semibold); } - } static void ui_draw_vision_speedlimit(UIState *s) { - const UIScene *scene = &s->scene; - int ui_viz_rx = scene->ui_viz_rx; - int ui_viz_rw = scene->ui_viz_rw; - char speedlim_str[32]; float speedlimit = s->scene.speedlimit; int speedlim_calc = speedlimit * 2.2369363 + 0.5; @@ -523,7 +479,7 @@ static void ui_draw_vision_speedlimit(UIState *s) { int viz_speedlim_w = 180; int viz_speedlim_h = 202; - int viz_speedlim_x = (ui_viz_rx + (bdr_s*2)); + int viz_speedlim_x = (s->scene.ui_viz_rx + (bdr_s*2)); int viz_speedlim_y = (box_y + (bdr_s*1.5)); if (!is_speedlim_valid) { viz_speedlim_w -= 5; @@ -531,201 +487,149 @@ static void ui_draw_vision_speedlimit(UIState *s) { viz_speedlim_x += 9; viz_speedlim_y += 5; } - int viz_speedlim_bdr = is_speedlim_valid ? 30 : 15; - // Draw Background - nvgBeginPath(s->vg); - nvgRoundedRect(s->vg, viz_speedlim_x, viz_speedlim_y, viz_speedlim_w, viz_speedlim_h, viz_speedlim_bdr); + NVGcolor color = COLOR_WHITE_ALPHA(100); if (is_speedlim_valid && s->is_ego_over_limit) { - nvgFillColor(s->vg, nvgRGBA(218, 111, 37, 180)); + color = nvgRGBA(218, 111, 37, 180); } else if (is_speedlim_valid) { - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); - } else { - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 100)); + color = COLOR_WHITE; } - nvgFill(s->vg); + ui_draw_rect(s->vg, viz_speedlim_x, viz_speedlim_y, viz_speedlim_w, viz_speedlim_h, color, is_speedlim_valid ? 30 : 15); // Draw Border if (is_speedlim_valid) { - nvgStrokeWidth(s->vg, 10); - nvgStroke(s->vg); - 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)); - } else if (is_speedlim_valid) { - nvgStrokeColor(s->vg, nvgRGBA(255, 255, 255, 255)); - } + ui_draw_rect(s->vg, viz_speedlim_x, viz_speedlim_y, viz_speedlim_w, viz_speedlim_h, + s->is_ego_over_limit ? COLOR_OCHRE : COLOR_WHITE, 20, 10); } - + const float text_x = viz_speedlim_x + viz_speedlim_w / 2; + const float text_y = viz_speedlim_y + (is_speedlim_valid ? 50 : 45); // Draw "Speed Limit" Text nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); - nvgFontFace(s->vg, "sans-semibold"); - nvgFontSize(s->vg, 50); - nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 255)); - if (is_speedlim_valid && s->is_ego_over_limit) { - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); - } - 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); + color = is_speedlim_valid && s->is_ego_over_limit ? COLOR_WHITE : COLOR_BLACK; + ui_draw_text(s->vg, text_x + (is_speedlim_valid ? 6 : 0), text_y, "SMART", 50, color, s->font_sans_semibold); + ui_draw_text(s->vg, text_x + (is_speedlim_valid ? 6 : 0), text_y + 40, "SPEED", 50, color, s->font_sans_semibold); // Draw Speed Text - nvgFontFace(s->vg, "sans-bold"); - nvgFontSize(s->vg, 48*2.5); - if (s->is_ego_over_limit) { - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); - } else { - nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 255)); - } + color = s->is_ego_over_limit ? COLOR_WHITE : 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); + ui_draw_text(s->vg, text_x, viz_speedlim_y + (is_speedlim_valid ? 170 : 165), speedlim_str, 48*2.5, color, s->font_sans_bold); } else { - nvgFontFace(s->vg, "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); + ui_draw_text(s->vg, text_x, viz_speedlim_y + (is_speedlim_valid ? 170 : 165), "N/A", 42*2.5, color, s->font_sans_semibold); } } static void ui_draw_vision_speed(UIState *s) { const UIScene *scene = &s->scene; - int ui_viz_rx = scene->ui_viz_rx; - int ui_viz_rw = scene->ui_viz_rw; - float speed = s->scene.v_ego; - + float speed = s->scene.v_ego * 2.2369363 + 0.5; + if (s->is_metric){ + speed = s->scene.v_ego * 3.6 + 0.5; + } const int viz_speed_w = 280; - const int viz_speed_x = ui_viz_rx+((ui_viz_rw/2)-(viz_speed_w/2)); + const int viz_speed_x = scene->ui_viz_rx+((scene->ui_viz_rw/2)-(viz_speed_w/2)); char speed_str[32]; nvgBeginPath(s->vg); nvgRect(s->vg, viz_speed_x, box_y, viz_speed_w, header_h); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); - if (s->is_metric) { - snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 3.6 + 0.5)); - } else { - snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 2.2369363 + 0.5)); - } - nvgFontFace(s->vg, "sans-bold"); - nvgFontSize(s->vg, 96*2.5); - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); - nvgText(s->vg, viz_speed_x+viz_speed_w/2, 240, speed_str, NULL); - - nvgFontFace(s->vg, "sans-regular"); - nvgFontSize(s->vg, 36*2.5); - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200)); - - if (s->is_metric) { - nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "kph", NULL); - } else { - nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "mph", NULL); - } + snprintf(speed_str, sizeof(speed_str), "%d", (int)speed); + ui_draw_text(s->vg, viz_speed_x + viz_speed_w / 2, 240, speed_str, 96*2.5, COLOR_WHITE, s->font_sans_bold); + ui_draw_text(s->vg, viz_speed_x + viz_speed_w / 2, 320, s->is_metric?"kph":"mph", 36*2.5, COLOR_WHITE_ALPHA(200), s->font_sans_regular); } static void ui_draw_vision_event(UIState *s) { - const UIScene *scene = &s->scene; - const int ui_viz_rx = scene->ui_viz_rx; - const int ui_viz_rw = scene->ui_viz_rw; const int viz_event_w = 220; - const int viz_event_x = ((ui_viz_rx + ui_viz_rw) - (viz_event_w + (bdr_s*2))); + const int viz_event_x = ((s->scene.ui_viz_rx + s->scene.ui_viz_rw) - (viz_event_w + (bdr_s*2))); const int viz_event_y = (box_y + (bdr_s*1.5)); - const int viz_event_h = (header_h - (bdr_s*1.5)); 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; const int bg_wheel_x = viz_event_x + (viz_event_w-bg_wheel_size); const int bg_wheel_y = viz_event_y + (bg_wheel_size/2); - const int img_wheel_size = bg_wheel_size*1.5; - const int img_wheel_x = bg_wheel_x-(img_wheel_size/2); - const int img_wheel_y = bg_wheel_y-25; - float img_wheel_alpha = 0.1f; - bool is_engaged = (s->status == STATUS_ENGAGED); - bool is_warning = (s->status == STATUS_WARNING); - bool is_engageable = scene->engageable; - if (is_engaged || is_warning || is_engageable) { - nvgBeginPath(s->vg); - nvgCircle(s->vg, bg_wheel_x, (bg_wheel_y + (bdr_s*1.5)), bg_wheel_size); - if (is_engaged) { - nvgFillColor(s->vg, nvgRGBA(23, 134, 68, 255)); - } else if (is_warning) { - nvgFillColor(s->vg, nvgRGBA(218, 111, 37, 255)); - } else if (is_engageable) { - nvgFillColor(s->vg, nvgRGBA(23, 51, 73, 255)); - } - nvgFill(s->vg); - img_wheel_alpha = 1.0f; + NVGcolor color = COLOR_BLACK_ALPHA(0); + if (s->status == STATUS_ENGAGED) { + color = nvgRGBA(23, 134, 68, 255); + } else if (s->status == STATUS_WARNING) { + color = COLOR_OCHRE; + } else if (s->scene.engageable) { + color = nvgRGBA(23, 51, 73, 255); } - 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_circle_image(s->vg, bg_wheel_x, bg_wheel_y, bg_wheel_size, s->img_wheel, color, 1.0f, bg_wheel_y - 25); } } static void ui_draw_vision_map(UIState *s) { - const UIScene *scene = &s->scene; const int map_size = 96; - const int map_x = (scene->ui_viz_rx + (map_size * 3) + (bdr_s * 3)); + const int map_x = (s->scene.ui_viz_rx + (map_size * 3) + (bdr_s * 3)); const int map_y = (footer_y + ((footer_h - map_size) / 2)); - const int map_img_size = (map_size * 1.5); - const int map_img_x = (map_x - (map_img_size / 2)); - const int map_img_y = (map_y - (map_size / 4)); - - bool map_valid = s->scene.map_valid; - 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_circle_image(s->vg, map_x, map_y, map_size, s->img_map, s->scene.map_valid); } static void ui_draw_vision_face(UIState *s) { - const UIScene *scene = &s->scene; const int face_size = 96; - const int face_x = (scene->ui_viz_rx + face_size + (bdr_s * 2)); + const int face_x = (s->scene.ui_viz_rx + face_size + (bdr_s * 2)); const int face_y = (footer_y + ((footer_h - face_size) / 2)); - 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->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)); - 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); + ui_draw_circle_image(s->vg, face_x, face_y, face_size, s->img_face, s->scene.monitoring_active); +} - nvgBeginPath(s->vg); - nvgCircle(s->vg, face_x, (face_y + (bdr_s * 1.5)), face_size); - nvgFillColor(s->vg, face_bg); - nvgFill(s->vg); +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; - nvgBeginPath(s->vg); - nvgRect(s->vg, face_img_x, face_img_y, face_img_size, face_img_size); - nvgFillPaint(s->vg, face_img); - nvgFill(s->vg); + // blackout + if (!scene->is_rhd) { + 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)); + ui_draw_rect(s->vg, valid_frame_x + box_h / 2, box_y, valid_frame_w - box_h / 2, box_h, gradient); + } else { + 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)); + ui_draw_rect(s->vg, valid_frame_x, box_y, valid_frame_w - box_h / 2, box_h, gradient); + } + ui_draw_rect(s->vg, scene->is_rhd ? valid_frame_x : valid_frame_x + box_h / 2, box_y, valid_frame_w - box_h / 2, box_h, COLOR_BLACK_ALPHA(144)); + + // borders + ui_draw_rect(s->vg, frame_x, box_y, valid_frame_x - frame_x, box_h, nvgRGBA(23, 51, 73, 255)); + ui_draw_rect(s->vg, valid_frame_x + valid_frame_w, box_y, frame_w - valid_frame_w - (valid_frame_x - frame_x), box_h, nvgRGBA(23, 51, 73, 255)); + + // 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) { + ui_draw_rect(s->vg, fbox_x, fbox_y, 0.6 * box_h / 2, 0.6 * box_h / 2, + 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), + 35, 10); + } else { + ui_draw_rect(s->vg, fbox_x, fbox_y, 0.6 * box_h / 2, 0.6 * box_h / 2, nvgRGBAf(1.0, 1.0, 1.0, 0.2), 35, 10); + } + } 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)); + ui_draw_circle_image(s->vg, face_x, face_y, face_size, s->img_face, scene->face_prob > 0.4); } static void ui_draw_vision_header(UIState *s) { @@ -733,14 +637,11 @@ static void ui_draw_vision_header(UIState *s) { int ui_viz_rx = scene->ui_viz_rx; int ui_viz_rw = scene->ui_viz_rw; - nvgBeginPath(s->vg); NVGpaint gradient = nvgLinearGradient(s->vg, ui_viz_rx, (box_y+(header_h-(header_h/2.5))), ui_viz_rx, box_y+header_h, nvgRGBAf(0,0,0,0.45), nvgRGBAf(0,0,0,0)); - nvgFillPaint(s->vg, gradient); - nvgRect(s->vg, ui_viz_rx, box_y, ui_viz_rw, header_h); - nvgFill(s->vg); + ui_draw_rect(s->vg, ui_viz_rx, box_y, ui_viz_rw, header_h, gradient); ui_draw_vision_maxspeed(s); @@ -752,12 +653,8 @@ static void ui_draw_vision_header(UIState *s) { } static void ui_draw_vision_footer(UIState *s) { - const UIScene *scene = &s->scene; - int ui_viz_rx = scene->ui_viz_rx; - int ui_viz_rw = scene->ui_viz_rw; - nvgBeginPath(s->vg); - nvgRect(s->vg, ui_viz_rx, footer_y, ui_viz_rw, footer_h); + nvgRect(s->vg, s->scene.ui_viz_rx, footer_y, s->scene.ui_viz_rw, footer_h); ui_draw_vision_face(s); @@ -766,55 +663,48 @@ static void ui_draw_vision_footer(UIState *s) { #endif } -void ui_draw_vision_alert(UIState *s, int va_size, int va_color, +void ui_draw_vision_alert(UIState *s, cereal::ControlsState::AlertSize va_size, int va_color, const char* va_text1, const char* va_text2) { + static std::map alert_size_map = { + {cereal::ControlsState::AlertSize::NONE, 0}, + {cereal::ControlsState::AlertSize::SMALL, 241}, + {cereal::ControlsState::AlertSize::MID, 390}, + {cereal::ControlsState::AlertSize::FULL, vwp_h}}; + 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]; - const int alr_s = alert_sizes[va_size]; - const int alr_x = ui_viz_rx-(mapEnabled?(hasSidebar?nav_w:(nav_ww)):0)-bdr_s; - const int alr_w = ui_viz_rw+(mapEnabled?(hasSidebar?nav_w:(nav_ww)):0)+(bdr_s*2); - const int alr_h = alr_s+(va_size==ALERTSIZE_NONE?0:bdr_s); + int alr_s = alert_size_map[va_size]; + + const int alr_x = scene->ui_viz_rx-(mapEnabled?(hasSidebar?nav_w:(nav_ww)):0)-bdr_s; + const int alr_w = scene->ui_viz_rw+(mapEnabled?(hasSidebar?nav_w:(nav_ww)):0)+(bdr_s*2); + const int alr_h = alr_s+(va_size==cereal::ControlsState::AlertSize::NONE?0:bdr_s); const int alr_y = vwp_h-alr_h; - nvgBeginPath(s->vg); - nvgRect(s->vg, alr_x, alr_y, alr_w, alr_h); - nvgFillColor(s->vg, nvgRGBA(color[0],color[1],color[2],(color[3]*s->alert_blinking_alpha))); - nvgFill(s->vg); + ui_draw_rect(s->vg, alr_x, alr_y, alr_w, alr_h, nvgRGBA(color[0],color[1],color[2],(color[3]*s->alert_blinking_alpha))); - nvgBeginPath(s->vg); NVGpaint gradient = nvgLinearGradient(s->vg, alr_x, alr_y, alr_x, alr_y+alr_h, nvgRGBAf(0.0,0.0,0.0,0.05), nvgRGBAf(0.0,0.0,0.0,0.35)); - nvgFillPaint(s->vg, gradient); - nvgRect(s->vg, alr_x, alr_y, alr_w, alr_h); - nvgFill(s->vg); + ui_draw_rect(s->vg, alr_x, alr_y, alr_w, alr_h, gradient); - 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"); - 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"); - 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"); - 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) { + if (va_size == cereal::ControlsState::AlertSize::SMALL) { + ui_draw_text(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+15, va_text1, 40*2.5, COLOR_WHITE, s->font_sans_semibold); + } else if (va_size == cereal::ControlsState::AlertSize::MID) { + ui_draw_text(s->vg, alr_x+alr_w/2, alr_y+alr_h/2-45, va_text1, 48*2.5, COLOR_WHITE, s->font_sans_bold); + ui_draw_text(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+75, va_text2, 36*2.5, COLOR_WHITE, s->font_sans_regular); + } else if (va_size == cereal::ControlsState::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); } @@ -822,81 +712,86 @@ void ui_draw_vision_alert(UIState *s, int va_size, int va_color, static void ui_draw_vision(UIState *s) { const UIScene *scene = &s->scene; - int ui_viz_rx = scene->ui_viz_rx; - 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); - glScissor(ui_viz_rx, s->fb_h-(box_y+box_h), ui_viz_rw, box_h); - glEnable(GL_BLEND); - glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + glViewport(scene->ui_viz_rx+scene->ui_viz_ro, s->fb_h-(box_y+box_h), viz_w, box_h); + glScissor(scene->ui_viz_rx, s->fb_h-(box_y+box_h), scene->ui_viz_rw, box_h); draw_frame(s); - glViewport(0, 0, s->fb_w, s->fb_h); glDisable(GL_SCISSOR_TEST); - glClear(GL_STENCIL_BUFFER_BIT); - - nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); - nvgSave(s->vg); + glViewport(0, 0, s->fb_w, s->fb_h); // Draw augmented elements - const int inner_height = viz_w*9/16; - nvgScissor(s->vg, ui_viz_rx, box_y, ui_viz_rw, box_h); - nvgTranslate(s->vg, ui_viz_rx+ui_viz_ro, box_y + (box_h-inner_height)/2.0); - nvgScale(s->vg, (float)viz_w / s->fb_w, (float)inner_height / s->fb_h); if (!scene->frontview && !scene->fullview) { ui_draw_world(s); } - nvgRestore(s->vg); - // Set Speed, Current Speed, Status/Events - ui_draw_vision_header(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); + if (!scene->frontview) { + ui_draw_vision_header(s); } else { - ui_draw_vision_footer(s); + ui_draw_driver_view(s); } - - nvgEndFrame(s->vg); - glDisable(GL_BLEND); + if (scene->alert_size != cereal::ControlsState::AlertSize::NONE) { + // Controls Alerts + ui_draw_vision_alert(s, scene->alert_size, s->status, + scene->alert_text1.c_str(), scene->alert_text2.c_str()); + } else { + if (!scene->frontview){ui_draw_vision_footer(s);} + } } -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, 1.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_sidebar(s); - ui_draw_vision(s); + ui_draw_background(s); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + glViewport(0, 0, s->fb_w, s->fb_h); + nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); + ui_draw_sidebar(s); + if (s->started && s->active_app == cereal::UiLayoutState::App::NONE && s->status != STATUS_STOPPED && s->vision_seen) { + ui_draw_vision(s); + } + nvgEndFrame(s->vg); + glDisable(GL_BLEND); +} + +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); +} + +void ui_draw_rect(NVGcontext *vg, float x, float y, float w, float h, NVGcolor color, float r, int width) { + nvgBeginPath(vg); + r > 0? nvgRoundedRect(vg, x, y, w, h, r) : nvgRect(vg, x, y, w, h); + if (width) { + nvgStrokeColor(vg, color); + nvgStrokeWidth(vg, width); + nvgStroke(vg); } else { - ui_draw_blank(s); - if (!s->scene.uilayout_sidebarcollapsed) { - ui_draw_sidebar(s); - } + nvgFillColor(vg, color); + nvgFill(vg); } +} - { - glEnable(GL_BLEND); - glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - glClear(GL_STENCIL_BUFFER_BIT); - - nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); - - nvgEndFrame(s->vg); - glDisable(GL_BLEND); - } +void ui_draw_rect(NVGcontext *vg, float x, float y, float w, float h, NVGpaint paint, float r){ + nvgBeginPath(vg); + r > 0? nvgRoundedRect(vg, x, y, w, h, r) : nvgRect(vg, x, y, w, h); + nvgFillPaint(vg, paint); + nvgFill(vg); } #ifdef NANOVG_GL3_IMPLEMENTATION @@ -977,35 +872,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 717449de..3b30ac75 100644 --- a/selfdrive/ui/sidebar.cc +++ b/selfdrive/ui/sidebar.cc @@ -1,104 +1,83 @@ #include #include #include +#include #include "ui.hpp" -static void ui_draw_sidebar_background(UIState *s, bool hasSidebar) { - int sbr_x = hasSidebar ? 0 : -(sbr_w) + bdr_s * 2; - - nvgBeginPath(s->vg); - nvgRect(s->vg, sbr_x, 0, sbr_w, vwp_h); - nvgFillColor(s->vg, COLOR_BLACK_ALPHA); - nvgFill(s->vg); +static void ui_draw_sidebar_background(UIState *s) { + int sbr_x = !s->scene.uilayout_sidebarcollapsed ? 0 : -(sbr_w) + bdr_s * 2; + ui_draw_rect(s->vg, sbr_x, 0, sbr_w, vwp_h, COLOR_BLACK_ALPHA(85)); } -static void ui_draw_sidebar_settings_button(UIState *s, bool hasSidebar) { - bool settingsActive = s->active_app == cereal_UiLayoutState_App_settings; - const int settings_btn_xr = hasSidebar ? settings_btn_x : -(sbr_w); +static void ui_draw_sidebar_settings_button(UIState *s) { + bool settingsActive = s->active_app == cereal::UiLayoutState::App::SETTINGS; + 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) { - bool homeActive = s->active_app == cereal_UiLayoutState_App_home; - const int home_btn_xr = hasSidebar ? home_btn_x : -(sbr_w); +static void ui_draw_sidebar_home_button(UIState *s) { + bool homeActive = s->active_app == cereal::UiLayoutState::App::HOME; + 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) { + static std::map network_strength_map = { + {cereal::ThermalData::NetworkStrength::UNKNOWN, 1}, + {cereal::ThermalData::NetworkStrength::POOR, 2}, + {cereal::ThermalData::NetworkStrength::MODERATE, 3}, + {cereal::ThermalData::NetworkStrength::GOOD, 4}, + {cereal::ThermalData::NetworkStrength::GREAT, 5}}; 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); + const int img_idx = s->scene.networkType == cereal::ThermalData::NetworkType::NONE ? 0 : network_strength_map[s->scene.networkStrength]; + ui_draw_image(s->vg, network_img_x, network_img_y, network_img_w, network_img_h, s->img_network[img_idx], 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 ? - s->img_battery_charging : s->img_battery; + int battery_img = s->scene.batteryCharging ? s->img_battery_charging : s->img_battery; - nvgBeginPath(s->vg); - nvgRect(s->vg, battery_img_x + 6, battery_img_y + 5, - ((battery_img_w - 19) * (s->scene.batteryPercent * 0.01)), battery_img_h - 11); - nvgFillColor(s->vg, COLOR_WHITE); - nvgFill(s->vg); + ui_draw_rect(s->vg, battery_img_x + 6, battery_img_y + 5, + ((battery_img_w - 19) * (s->scene.batteryPercent * 0.01)), battery_img_h - 11, COLOR_WHITE); - 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) { + static std::map network_type_map = { + {cereal::ThermalData::NetworkType::NONE, "--"}, + {cereal::ThermalData::NetworkType::WIFI, "WiFi"}, + {cereal::ThermalData::NetworkType::CELL2_G, "2G"}, + {cereal::ThermalData::NetworkType::CELL3_G, "3G"}, + {cereal::ThermalData::NetworkType::CELL4_G, "4G"}, + {cereal::ThermalData::NetworkType::CELL5_G, "5G"}}; + 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; - const char *network_types[6] = {"--", "WiFi", "2G", "3G", "4G", "5G"}; - char network_type_str[32]; - - if (s->scene.networkType <= 5) { - snprintf(network_type_str, sizeof(network_type_str), "%s", network_types[s->scene.networkType]); - } - + const char *network_type = network_type_map[s->scene.networkType]; 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); + nvgTextBox(s->vg, network_x, network_y, network_w, network_type ? network_type : "--", 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) { @@ -109,11 +88,8 @@ static void ui_draw_sidebar_metric(UIState *s, const char* label_str, const char status_color = COLOR_RED; } - nvgBeginPath(s->vg); - nvgRoundedRect(s->vg, metric_x, metric_y, metric_w, metric_h, 20); - nvgStrokeColor(s->vg, severity > 0 ? COLOR_WHITE : COLOR_WHITE_ALPHA); - nvgStrokeWidth(s->vg, 2); - nvgStroke(s->vg); + ui_draw_rect(s->vg, metric_x, metric_y, metric_w, metric_h, + severity > 0 ? COLOR_WHITE : COLOR_WHITE_ALPHA(85), 20, 2); nvgBeginPath(s->vg); nvgRoundedRectVarying(s->vg, metric_x + 6, metric_y + 6, 18, metric_h - 12, 25, 0, 0, 25); @@ -123,112 +99,89 @@ 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) { - int temp_severity; +static void ui_draw_sidebar_temp_metric(UIState *s) { + static std::map temp_severity_map = { + {cereal::ThermalData::ThermalStatus::GREEN, 0}, + {cereal::ThermalData::ThermalStatus::YELLOW, 1}, + {cereal::ThermalData::ThermalStatus::RED, 2}, + {cereal::ThermalData::ThermalStatus::DANGER, 3}}; char temp_label_str[32]; char temp_value_str[32]; char temp_value_unit[32]; - const int temp_y_offset = 148 + 32; - - if (s->scene.thermalStatus == cereal_ThermalData_ThermalStatus_green) { - temp_severity = 0; - } else if (s->scene.thermalStatus == cereal_ThermalData_ThermalStatus_yellow) { - temp_severity = 1; - } else if (s->scene.thermalStatus == cereal_ThermalData_ThermalStatus_red) { - temp_severity = 2; - } else if (s->scene.thermalStatus == cereal_ThermalData_ThermalStatus_danger) { - temp_severity = 3; - } - + const int temp_y_offset = 0; snprintf(temp_value_str, sizeof(temp_value_str), "%d", s->scene.paTemp); snprintf(temp_value_unit, sizeof(temp_value_unit), "%s", "°C"); snprintf(temp_label_str, sizeof(temp_label_str), "%s", "TEMP"); strcat(temp_value_str, temp_value_unit); - ui_draw_sidebar_metric(s, temp_label_str, temp_value_str, temp_severity, temp_y_offset, NULL, hasSidebar); + ui_draw_sidebar_metric(s, temp_label_str, temp_value_str, temp_severity_map[s->scene.thermalStatus], 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) { + 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) { + snprintf(panda_message_str, sizeof(panda_message_str), "%s", "NO\nVEHICLE"); + } else { + if (s->started){ + if (s->scene.satelliteCount < 6) { panda_severity = 1; - snprintf(panda_message_str, sizeof(panda_message_str), "%s", "PANDA\nNO GPS"); + 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", "PANDA GOOD GPS"); + snprintf(panda_message_str, sizeof(panda_message_str), "%s", "VEHICLE\nGOOD GPS"); } + } else { + panda_severity = 0; + snprintf(panda_message_str, sizeof(panda_message_str), "%s", "VEHICLE\nONLINE"); + } } - 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); + if (s->scene.uilayout_sidebarcollapsed){ + return; + } + 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 701fa56b..f64cde07 100644 --- a/selfdrive/ui/sound.cc +++ b/selfdrive/ui/sound.cc @@ -13,28 +13,34 @@ extern "C"{ #include "slplay.h" } -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); +int last_volume = 0; - // 5 second timeout at 60fps - int volume_changed = system(volume_change_cmd); +void set_volume(int 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); + last_volume = volume; + } } sound_file sound_table[] = { - { cereal_CarControl_HUDControl_AudibleAlert_chimeDisengage, "../assets/sounds/disengaged.wav", false }, - { 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_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 }, - { cereal_CarControl_HUDControl_AudibleAlert_none, NULL, false }, + { cereal::CarControl::HUDControl::AudibleAlert::CHIME_DISENGAGE, "../assets/sounds/disengaged.wav", false }, + { cereal::CarControl::HUDControl::AudibleAlert::CHIME_ENGAGE, "../assets/sounds/engaged.wav", false }, + { cereal::CarControl::HUDControl::AudibleAlert::CHIME_WARNING1, "../assets/sounds/warning_1.wav", false }, + { cereal::CarControl::HUDControl::AudibleAlert::CHIME_WARNING2, "../assets/sounds/warning_2.wav", false }, + { cereal::CarControl::HUDControl::AudibleAlert::CHIME_WARNING2_REPEAT, "../assets/sounds/warning_2.wav", true }, + { cereal::CarControl::HUDControl::AudibleAlert::CHIME_WARNING_REPEAT, "../assets/sounds/warning_repeat.wav", true }, + { cereal::CarControl::HUDControl::AudibleAlert::CHIME_ERROR, "../assets/sounds/error.wav", false }, + { cereal::CarControl::HUDControl::AudibleAlert::CHIME_PROMPT, "../assets/sounds/error.wav", false }, + { cereal::CarControl::HUDControl::AudibleAlert::NONE, NULL, false }, }; sound_file* get_sound_file(AudibleAlert alert) { - for (sound_file *s = sound_table; s->alert != cereal_CarControl_HUDControl_AudibleAlert_none; s++) { + for (sound_file *s = sound_table; s->alert != cereal::CarControl::HUDControl::AudibleAlert::NONE; s++) { if (s->alert == alert) { return s; } @@ -68,7 +74,7 @@ void ui_sound_init() { slplay_setup(&error); if (error) goto fail; - for (sound_file *s = sound_table; s->alert != cereal_CarControl_HUDControl_AudibleAlert_none; s++) { + for (sound_file *s = sound_table; s->alert != cereal::CarControl::HUDControl::AudibleAlert::NONE; s++) { slplay_create_player_for_uri(s->uri, &error); if (error) goto fail; } diff --git a/selfdrive/ui/sound.hpp b/selfdrive/ui/sound.hpp index cc44c1b4..ec8934ef 100644 --- a/selfdrive/ui/sound.hpp +++ b/selfdrive/ui/sound.hpp @@ -1,9 +1,9 @@ #ifndef __SOUND_HPP #define __SOUND_HPP -#include "cereal/gen/c/log.capnp.h" +#include "cereal/gen/cpp/log.capnp.h" -typedef enum cereal_CarControl_HUDControl_AudibleAlert AudibleAlert; +typedef cereal::CarControl::HUDControl::AudibleAlert AudibleAlert; void ui_sound_init(); void ui_sound_destroy(); diff --git a/selfdrive/ui/text/Makefile b/selfdrive/ui/text/Makefile new file mode 100644 index 00000000..7a011edb --- /dev/null +++ b/selfdrive/ui/text/Makefile @@ -0,0 +1,73 @@ +CC = clang +CXX = clang++ + +PHONELIBS = ../../../phonelibs + +WARN_FLAGS = -Werror=implicit-function-declaration \ + -Werror=incompatible-pointer-types \ + -Werror=int-conversion \ + -Werror=return-type \ + -Werror=format-extra-args + +CFLAGS = -std=gnu11 -fPIC -O2 $(WARN_FLAGS) +CXXFLAGS = -std=c++11 -fPIC -O2 $(WARN_FLAGS) + +NANOVG_FLAGS = -I$(PHONELIBS)/nanovg + +OPENGL_LIBS = -lGLESv3 + +FRAMEBUFFER_LIBS = -lutils -lgui -lEGL + +OBJS = text.o \ + ../../common/framebuffer.o \ + ../../../selfdrive/common/touch.o \ + $(PHONELIBS)/nanovg/nanovg.o \ + opensans_regular.o \ + +DEPS := $(OBJS:.o=.d) + +.PHONY: all +all: text + +text: $(OBJS) + @echo "[ LINK ] $@" + $(CXX) -fPIC -o '$@' $^ \ + -s \ + $(FRAMEBUFFER_LIBS) \ + -L/system/vendor/lib64 \ + $(OPENGL_LIBS) \ + -lm -llog + +opensans_regular.o: ../../assets/fonts/opensans_regular.ttf + @echo "[ bin2o ] $@" + cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)' + +%.o: %.c + mkdir -p $(@D) + @echo "[ CC ] $@" + $(CC) $(CPPFLAGS) $(CFLAGS) \ + -I../.. \ + -I$(PHONELIBS)/android_frameworks_native/include \ + -I$(PHONELIBS)/android_system_core/include \ + -I$(PHONELIBS)/android_hardware_libhardware/include \ + $(NANOVG_FLAGS) \ + -c -o '$@' '$<' + +%.o: %.cc + mkdir -p $(@D) + @echo "[ CXX ] $@" + $(CXX) $(CPPFLAGS) $(CXXFLAGS) \ + -I../../selfdrive \ + -I../../ \ + -I$(PHONELIBS)/android_frameworks_native/include \ + -I$(PHONELIBS)/android_system_core/include \ + -I$(PHONELIBS)/android_hardware_libhardware/include \ + $(NANOVG_FLAGS) \ + -c -o '$@' '$<' + + +.PHONY: clean +clean: + rm -f text $(OBJS) $(DEPS) + +-include $(DEPS) diff --git a/selfdrive/ui/text/text b/selfdrive/ui/text/text new file mode 100755 index 00000000..fc0a176b Binary files /dev/null and b/selfdrive/ui/text/text differ diff --git a/selfdrive/ui/text/text.c b/selfdrive/ui/text/text.c new file mode 100644 index 00000000..cd910cc8 --- /dev/null +++ b/selfdrive/ui/text/text.c @@ -0,0 +1,141 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "nanovg.h" +#define NANOVG_GLES3_IMPLEMENTATION +#include "nanovg_gl.h" +#include "nanovg_gl_utils.h" + + +#include "common/framebuffer.h" +#include "common/touch.h" + + +#define COLOR_WHITE nvgRGBA(255, 255, 255, 255) +#define MAX_TEXT_SIZE 2048 + +extern const unsigned char _binary_opensans_regular_ttf_start[]; +extern const unsigned char _binary_opensans_regular_ttf_end[]; + +static void set_brightness(int brightness) { + FILE *f = fopen("/sys/class/leds/lcd-backlight/brightness", "wb"); + if (f != NULL) { + fprintf(f, "%d", brightness); + fclose(f); + } +} + +int main(int argc, char** argv) { + int err; + + // spinner + int fb_w, fb_h; + FramebufferState *fb = framebuffer_init("text", 0x00001000, false, + &fb_w, &fb_h); + assert(fb); + + NVGcontext *vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES); + assert(vg); + + int font = nvgCreateFontMem(vg, "regular", (unsigned char*)_binary_opensans_regular_ttf_start, _binary_opensans_regular_ttf_end-_binary_opensans_regular_ttf_start, 0); +assert(font >= 0); + + // Awake + framebuffer_set_power(fb, HWC_POWER_MODE_NORMAL); + set_brightness(255); + + glClearColor(0.1, 0.1, 0.1, 1.0); + glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + nvgBeginFrame(vg, fb_w, fb_h, 1.0f); + + // background + nvgBeginPath(vg); + NVGpaint bg = nvgLinearGradient(vg, fb_w, 0, fb_w, fb_h, + nvgRGBA(0, 0, 0, 175), nvgRGBA(0, 0, 0, 255)); + nvgFillPaint(vg, bg); + nvgRect(vg, 0, 0, fb_w, fb_h); + nvgFill(vg); + + + // Text + nvgFillColor(vg, COLOR_WHITE); + nvgFontSize(vg, 75.0f); + + if (argc >= 2) { + float x = 150; + float y = 150; + + // Copy text + char * text = malloc(MAX_TEXT_SIZE); + strncpy(text, argv[1], MAX_TEXT_SIZE); + + float lineh; + nvgTextMetrics(vg, NULL, NULL, &lineh); + + // nvgTextBox strips leading whitespace. We have to reimplement + char * next = strtok(text, "\n"); + while (next != NULL){ + nvgText(vg, x, y, next, NULL); + y += lineh; + next = strtok(NULL, "\n"); + } + } + + // Button + int b_x = 1500; + int b_y = 800; + int b_w = 300; + int b_h = 150; + + nvgBeginPath(vg); + nvgFillColor(vg, nvgRGBA(8, 8, 8, 255)); + nvgRoundedRect(vg, b_x, b_y, b_w, b_h, 20); + nvgFill(vg); + + nvgFillColor(vg, nvgRGBA(255, 255, 255, 255)); + nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); + nvgText(vg, b_x+b_w/2, b_y+b_h/2, "Exit", NULL); + + nvgBeginPath(vg); + nvgStrokeColor(vg, nvgRGBA(255, 255, 255, 50)); + nvgStrokeWidth(vg, 5); + nvgRoundedRect(vg, b_x, b_y, b_w, b_h, 20); + nvgStroke(vg); + + // Draw to screen + nvgEndFrame(vg); + framebuffer_swap(fb); + assert(glGetError() == GL_NO_ERROR); + + + // Wait for button + TouchState touch; + touch_init(&touch); + + while (true){ + int touch_x = -1, touch_y = -1; + int res = touch_poll(&touch, &touch_x, &touch_y, 0); + if (res){ + + if (touch_x > b_x && touch_x < b_x + b_w){ + if (touch_y > b_y && touch_y < b_y + b_h){ + return 1; + } + } + } + + usleep(1000000 / 60); + } + + return 0; +} diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 12a469a4..c700dad0 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -1,3 +1,4 @@ +#include "ui.hpp" #include #include #include @@ -5,9 +6,8 @@ #include #include #include - +#include #include - #include "common/util.h" #include "common/timing.h" #include "common/swaglog.h" @@ -15,9 +15,6 @@ #include "common/visionimg.h" #include "common/params.h" -#include "ui.hpp" -#include "sound.hpp" - static int last_brightness = -1; static void set_brightness(UIState *s, int brightness) { if (last_brightness != brightness && (s->awake || brightness == 0)) { @@ -30,6 +27,17 @@ 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) { @@ -42,13 +50,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 +65,23 @@ static void set_awake(UIState *s, bool awake) { #endif } +static void update_offroad_layout_state(UIState *s) { + capnp::MallocMessageBuilder msg; + auto event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto layout = event.initUiLayoutState(); + layout.setActiveApp(s->active_app); + layout.setSidebarCollapsed(s->scene.uilayout_sidebarcollapsed); + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + s->offroad_sock->send((char*)bytes.begin(), bytes.size()); + LOGD("setting active app to %d with sidebar %d", (int)s->active_app, s->scene.uilayout_sidebarcollapsed); +} + 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 +89,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 +109,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("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) - && (s->active_app != cereal_UiLayoutState_App_settings)) { - s->scene.uilayout_sidebarcollapsed = !s->scene.uilayout_sidebarcollapsed; + if (s->started && (touch_x >= s->scene.ui_viz_rx - bdr_s) + && (s->active_app != cereal::UiLayoutState::App::SETTINGS)) { + 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); } } @@ -101,47 +138,82 @@ static void set_do_exit(int sig) { do_exit = 1; } -static void read_param_bool(bool* param, const char* param_name) { +static void read_param_bool(bool* param, const char* param_name, bool persistent_param = false) { char *s; - const int result = read_db_value(NULL, param_name, &s, NULL); + const int result = read_db_value(param_name, &s, NULL, persistent_param); if (result == 0) { *param = s[0] == '1'; free(s); } } -static void read_param_float(float* param, const char* param_name) { +static int read_param_float(float* param, const char* param_name, bool persistent_param = false) { char *s; - const int result = read_db_value(NULL, param_name, &s, NULL); + const int result = read_db_value(param_name, &s, NULL, persistent_param); if (result == 0) { *param = strtod(s, NULL); free(s); } + return result; } -static void read_param_bool_timeout(bool* param, const char* param_name, int* timeout) { +static int read_param_uint64(uint64_t* dest, const char* param_name, bool persistent_param = false) { + char *s; + const int result = read_db_value(param_name, &s, NULL, persistent_param); + 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, bool persistent_param = false) { if (*timeout > 0){ (*timeout)--; } else { - read_param_bool(param, param_name); + read_param_bool(param, param_name, persistent_param); *timeout = 2 * UI_FREQ; // 0.5Hz } } -static void read_param_float_timeout(float* param, const char* param_name, int* timeout) { +static void read_param_float_timeout(float* param, const char* param_name, int* timeout, bool persistent_param = false) { if (*timeout > 0){ (*timeout)--; } else { - read_param_float(param, param_name); + read_param_float(param, param_name, persistent_param); *timeout = 2 * UI_FREQ; // 0.5Hz } } +static int read_param_uint64_timeout(uint64_t* dest, const char* param_name, int* timeout, bool persistent_param = false) { + if (*timeout > 0){ + (*timeout)--; + return 0; + } else { + return read_param_uint64(dest, param_name, persistent_param); + *timeout = 2 * UI_FREQ; // 0.5Hz + } +} + +static int write_param_float(float param, const char* param_name, bool persistent_param = false) { + char s[16]; + int size = snprintf(s, sizeof(s), "%f", param); + return write_db_value(param_name, s, MIN(size, sizeof(s)), persistent_param); +} + +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 +224,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 +236,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 +248,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,7 +262,7 @@ static void ui_init(UIState *s) { s->ipc_fd = -1; // init display - s->fb = framebuffer_init("ui", 0x00010000, true, &s->fb_w, &s->fb_h); + s->fb = framebuffer_init("ui", 0, true, &s->fb_w, &s->fb_h); assert(s->fb); set_awake(s, true); @@ -249,19 +329,15 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, s->limit_set_speed_timeout = UI_FREQ; } -static PathData read_path(cereal_ModelData_PathData_ptr pathp) { +static PathData read_path(cereal::ModelData::PathData::Reader pathp) { PathData ret = {0}; - struct cereal_ModelData_PathData pathd; - cereal_read_ModelData_PathData(&pathd, pathp); + ret.prob = pathp.getProb(); + ret.std = pathp.getStd(); - ret.prob = pathd.prob; - ret.std = pathd.std; - - capn_list32 polyp = pathd.poly; - capn_resolve(&polyp.p); + auto polyp = pathp.getPoly(); for (int i = 0; i < POLYFIT_DEGREE; i++) { - ret.poly[i] = capn_to_f32(capn_get32(polyp, i)); + ret.poly[i] = polyp[i]; } // Compute points locations @@ -272,20 +348,16 @@ static PathData read_path(cereal_ModelData_PathData_ptr pathp) { return ret; } -static ModelData read_model(cereal_ModelData_ptr modelp) { - struct cereal_ModelData modeld; - cereal_read_ModelData(&modeld, modelp); - +static ModelData read_model(cereal::ModelData::Reader model) { ModelData d = {0}; - d.path = read_path(modeld.path); - d.left_lane = read_path(modeld.leftLane); - d.right_lane = read_path(modeld.rightLane); + d.path = read_path(model.getPath()); + d.left_lane = read_path(model.getLeftLane()); + d.right_lane = read_path(model.getRightLane()); - struct cereal_ModelData_LeadData leadd; - cereal_read_ModelData_LeadData(&leadd, modeld.lead); + auto leadd = model.getLead(); d.lead = (LeadData){ - .dist = leadd.dist, .prob = leadd.prob, .std = leadd.std, + .dist = leadd.getDist(), .prob = leadd.getProb(), .std = leadd.getStd(), }; return d; @@ -294,195 +366,172 @@ static ModelData read_model(cereal_ModelData_ptr modelp) { static void update_status(UIState *s, int status) { if (s->status != status) { s->status = status; - // wake up bg thread to change - pthread_cond_signal(&s->bg_cond); } } +void handle_message(UIState *s, Message* msg) { + 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(); -void handle_message(UIState *s, Message * msg) { - struct capn ctx; - capn_init_mem(&ctx, (uint8_t*)msg->getData(), msg->getSize(), 0); - - cereal_Event_ptr eventp; - eventp.p = capn_getp(capn_root(&ctx), 0, 1); - struct cereal_Event eventd; - cereal_read_Event(&eventd, eventp); - - if (eventd.which == cereal_Event_controlsState) { - struct cereal_ControlsState datad; - cereal_read_ControlsState(&datad, eventd.controlsState); + auto which = event.which(); + UIScene &scene = s->scene; + if (which == cereal::Event::CONTROLS_STATE && s->started) { + auto data = event.getControlsState(); s->controls_timeout = 1 * UI_FREQ; - s->controls_seen = true; + scene.frontview = data.getRearViewCam(); + if (!scene.frontview){ s->controls_seen = true; } - if (datad.vCruise != s->scene.v_cruise) { - s->scene.v_cruise_update_ts = eventd.logMonoTime; + if (data.getVCruise() != scene.v_cruise) { + scene.v_cruise_update_ts = event.getLogMonoTime(); } - s->scene.v_cruise = datad.vCruise; - s->scene.v_ego = datad.vEgo; - s->scene.curvature = datad.curvature; - s->scene.engaged = datad.enabled; - s->scene.engageable = datad.engageable; - s->scene.gps_planner_active = datad.gpsPlannerActive; - s->scene.monitoring_active = datad.driverMonitoringOn; + scene.v_cruise = data.getVCruise(); + scene.v_ego = data.getVEgo(); + scene.curvature = data.getCurvature(); + scene.engaged = data.getEnabled(); + scene.engageable = data.getEngageable(); + scene.gps_planner_active = data.getGpsPlannerActive(); + scene.monitoring_active = data.getDriverMonitoringOn(); - 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) { + scene.decel_for_model = data.getDecelForModel(); + auto alert_sound = data.getAlertSound(); + const auto sound_none = cereal::CarControl::HUDControl::AudibleAlert::NONE; + if (alert_sound != s->alert_sound){ + if (s->alert_sound != sound_none){ stop_alert_sound(s->alert_sound); } - play_alert_sound(datad.alertSound); - - s->alert_sound = datad.alertSound; - snprintf(s->alert_type, sizeof(s->alert_type), "%s", datad.alertType.str); - } else if ((!datad.alertSound || datad.alertSound == cereal_CarControl_HUDControl_AudibleAlert_none) - && s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) { - stop_alert_sound(s->alert_sound); - s->alert_type[0] = '\0'; - s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none; - } - - if (datad.alertText1.str) { - snprintf(s->scene.alert_text1, sizeof(s->scene.alert_text1), "%s", datad.alertText1.str); - } else { - s->scene.alert_text1[0] = '\0'; - } - if (datad.alertText2.str) { - snprintf(s->scene.alert_text2, sizeof(s->scene.alert_text2), "%s", datad.alertText2.str); - } else { - s->scene.alert_text2[0] = '\0'; - } - s->scene.awareness_status = datad.awarenessStatus; - - s->scene.alert_ts = eventd.logMonoTime; - - s->scene.alert_size = datad.alertSize; - if (datad.alertSize == cereal_ControlsState_AlertSize_none) { - s->alert_size = ALERTSIZE_NONE; - } else if (datad.alertSize == cereal_ControlsState_AlertSize_small) { - s->alert_size = ALERTSIZE_SMALL; - } else if (datad.alertSize == cereal_ControlsState_AlertSize_mid) { - s->alert_size = ALERTSIZE_MID; - } else if (datad.alertSize == cereal_ControlsState_AlertSize_full) { - 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 (alert_sound != sound_none){ + play_alert_sound(alert_sound); + s->alert_type = data.getAlertType(); } + s->alert_sound = alert_sound; + } + scene.alert_text1 = data.getAlertText1(); + scene.alert_text2 = data.getAlertText2(); + scene.alert_ts = event.getLogMonoTime(); + scene.alert_size = data.getAlertSize(); + auto alertStatus = data.getAlertStatus(); + if (alertStatus == cereal::ControlsState::AlertStatus::USER_PROMPT) { + update_status(s, STATUS_WARNING); + } else if (alertStatus == cereal::ControlsState::AlertStatus::CRITICAL) { + update_status(s, STATUS_ALERT); + } else{ + update_status(s, scene.engaged ? STATUS_ENGAGED : STATUS_DISENGAGED); } - s->scene.alert_blinkingrate = datad.alertBlinkingRate; - if (datad.alertBlinkingRate > 0.) { + scene.alert_blinkingrate = data.getAlertBlinkingRate(); + if (scene.alert_blinkingrate > 0.) { if (s->alert_blinked) { if (s->alert_blinking_alpha > 0.0 && s->alert_blinking_alpha < 1.0) { - s->alert_blinking_alpha += (0.05*datad.alertBlinkingRate); + s->alert_blinking_alpha += (0.05*scene.alert_blinkingrate); } else { s->alert_blinked = false; } } else { if (s->alert_blinking_alpha > 0.25) { - s->alert_blinking_alpha -= (0.05*datad.alertBlinkingRate); + s->alert_blinking_alpha -= (0.05*scene.alert_blinkingrate); } else { s->alert_blinking_alpha += 0.25; s->alert_blinked = true; } } } - } else if (eventd.which == cereal_Event_radarState) { - struct cereal_RadarState datad; - cereal_read_RadarState(&datad, eventd.radarState); - struct cereal_RadarState_LeadData leaddatad; - cereal_read_RadarState_LeadData(&leaddatad, datad.leadOne); - s->scene.lead_status = leaddatad.status; - s->scene.lead_d_rel = leaddatad.dRel; - s->scene.lead_y_rel = leaddatad.yRel; - s->scene.lead_v_rel = leaddatad.vRel; - cereal_read_RadarState_LeadData(&leaddatad, datad.leadTwo); - s->scene.lead_status2 = leaddatad.status; - s->scene.lead_d_rel2 = leaddatad.dRel; - s->scene.lead_y_rel2 = leaddatad.yRel; - s->scene.lead_v_rel2 = leaddatad.vRel; - s->livempc_or_radarstate_changed = true; - } else if (eventd.which == cereal_Event_liveCalibration) { - s->scene.world_objects_visible = true; - struct cereal_LiveCalibrationData datad; - cereal_read_LiveCalibrationData(&datad, eventd.liveCalibration); + } else if (which == cereal::Event::RADAR_STATE) { + auto data = event.getRadarState(); - capn_list32 extrinsicl = datad.extrinsicMatrix; - capn_resolve(&extrinsicl.p); // is this a bug? + auto leaddatad = data.getLeadOne(); + scene.lead_status = leaddatad.getStatus(); + scene.lead_d_rel = leaddatad.getDRel(); + scene.lead_y_rel = leaddatad.getYRel(); + scene.lead_v_rel = leaddatad.getVRel(); + auto leaddatad2 = data.getLeadTwo(); + scene.lead_status2 = leaddatad2.getStatus(); + scene.lead_d_rel2 = leaddatad2.getDRel(); + scene.lead_y_rel2 = leaddatad2.getYRel(); + scene.lead_v_rel2 = leaddatad2.getVRel(); + + s->livempc_or_radarstate_changed = true; + } else if (which == cereal::Event::LIVE_CALIBRATION) { + scene.world_objects_visible = true; + auto extrinsicl = event.getLiveCalibration().getExtrinsicMatrix(); for (int i = 0; i < 3 * 4; i++) { - s->scene.extrinsic_matrix.v[i] = - capn_to_f32(capn_get32(extrinsicl, i)); + scene.extrinsic_matrix.v[i] = extrinsicl[i]; } - } else if (eventd.which == cereal_Event_model) { - s->scene.model = read_model(eventd.model); + } else if (which == cereal::Event::MODEL) { + scene.model = read_model(event.getModel()); s->model_changed = true; - } else if (eventd.which == cereal_Event_liveMpc) { - struct cereal_LiveMpcData datad; - cereal_read_LiveMpcData(&datad, eventd.liveMpc); - - capn_list32 x_list = datad.x; - capn_resolve(&x_list.p); + } else if (which == cereal::Event::LIVE_MPC) { + auto data = event.getLiveMpc(); + auto x_list = data.getX(); + auto y_list = data.getY(); for (int i = 0; i < 50; i++){ - s->scene.mpc_x[i] = capn_to_f32(capn_get32(x_list, i)); - } - - capn_list32 y_list = datad.y; - capn_resolve(&y_list.p); - - for (int i = 0; i < 50; i++){ - s->scene.mpc_y[i] = capn_to_f32(capn_get32(y_list, i)); + scene.mpc_x[i] = x_list[i]; + scene.mpc_y[i] = y_list[i]; } s->livempc_or_radarstate_changed = true; - } else if (eventd.which == cereal_Event_uiLayoutState) { - struct cereal_UiLayoutState datad; - cereal_read_UiLayoutState(&datad, eventd.uiLayoutState); - s->active_app = datad.activeApp; - s->scene.uilayout_sidebarcollapsed = datad.sidebarCollapsed; - } else if (eventd.which == cereal_Event_liveMapData) { - struct cereal_LiveMapData datad; - cereal_read_LiveMapData(&datad, eventd.liveMapData); - s->scene.map_valid = datad.mapValid; - } else if (eventd.which == cereal_Event_thermal) { - struct cereal_ThermalData datad; - cereal_read_ThermalData(&datad, eventd.thermal); + } else if (which == cereal::Event::UI_LAYOUT_STATE) { + auto data = event.getUiLayoutState(); - s->scene.networkType = datad.networkType; - s->scene.networkStrength = datad.networkStrength; - s->scene.batteryPercent = datad.batteryPercent; - snprintf(s->scene.batteryStatus, sizeof(s->scene.batteryStatus), "%s", datad.batteryStatus.str); - s->scene.freeSpace = datad.freeSpace; - s->scene.thermalStatus = datad.thermalStatus; - s->scene.paTemp = datad.pa0; - } else if (eventd.which == cereal_Event_ubloxGnss) { - struct cereal_UbloxGnss datad; - cereal_read_UbloxGnss(&datad, eventd.ubloxGnss); - if (datad.which == cereal_UbloxGnss_measurementReport) { - struct cereal_UbloxGnss_MeasurementReport reportdatad; - cereal_read_UbloxGnss_MeasurementReport(&reportdatad, datad.measurementReport); - s->scene.satelliteCount = reportdatad.numMeas; + s->active_app = data.getActiveApp(); + scene.uilayout_sidebarcollapsed = data.getSidebarCollapsed(); + if (data.getMockEngaged() != scene.uilayout_mockengaged) { + scene.uilayout_mockengaged = data.getMockEngaged(); } - } else if (eventd.which == cereal_Event_health) { - struct cereal_HealthData datad; - cereal_read_HealthData(&datad, eventd.health); + } else if (which == cereal::Event::LIVE_MAP_DATA) { + scene.map_valid = event.getLiveMapData().getMapValid(); + } else if (which == cereal::Event::THERMAL) { + auto data = event.getThermal(); - s->scene.hwType = datad.hwType; + scene.networkType = data.getNetworkType(); + scene.networkStrength = data.getNetworkStrength(); + scene.batteryPercent = data.getBatteryPercent(); + scene.batteryCharging = data.getBatteryStatus() == "Charging"; + scene.freeSpace = data.getFreeSpace(); + scene.thermalStatus = data.getThermalStatus(); + scene.paTemp = data.getPa0(); + + s->thermal_started = data.getStarted(); + } else if (which == cereal::Event::UBLOX_GNSS) { + auto data = event.getUbloxGnss(); + if (data.which() == cereal::UbloxGnss::MEASUREMENT_REPORT) { + scene.satelliteCount = data.getMeasurementReport().getNumMeas(); + } + } else if (which == cereal::Event::HEALTH) { + scene.hwType = event.getHealth().getHwType(); s->hardware_timeout = 5*30; // 5 seconds at 30 fps + } else if (which == cereal::Event::DRIVER_STATE) { + auto data = event.getDriverState(); + scene.face_prob = data.getFaceProb(); + auto fxy_list = data.getFacePosition(); + scene.face_x = fxy_list[0]; + scene.face_y = fxy_list[1]; + } else if (which == cereal::Event::D_MONITORING_STATE) { + auto data = event.getDMonitoringState(); + scene.is_rhd = data.getIsRHD(); + scene.awareness_status = data.getAwarenessStatus(); + s->preview_started = data.getIsPreview(); + } + + 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); } static void check_messages(UIState *s) { @@ -493,12 +542,11 @@ static void check_messages(UIState *s) { break; for (auto sock : polls){ - Message * msg = sock->receive(); - if (msg == NULL) continue; - - handle_message(s, msg); - - delete msg; + Message *msg = sock->receive(); + if (msg) { + handle_message(s, msg); + delete msg; + } } } } @@ -573,6 +621,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; @@ -652,8 +701,6 @@ static void ui_update(UIState *s) { } 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 +768,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 +833,7 @@ static void* light_sensor_thread(void *args) { s->light_sensor = buffer[0].light; } } - + sensors_close(device); return NULL; fail: @@ -794,35 +842,6 @@ fail: 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 int is_leon() { @@ -841,8 +860,6 @@ int is_leon() { return strstr(str, "letv") != NULL; } - - int main(int argc, char* argv[]) { int err; setpriority(PRIO_PROCESS, 0, -14); @@ -853,6 +870,7 @@ int main(int argc, char* argv[]) { UIState uistate; UIState *s = &uistate; ui_init(s); + enable_event_processing(true); pthread_t connect_thread_handle; err = pthread_create(&connect_thread_handle, NULL, @@ -864,11 +882,6 @@ 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 TouchState touch = {0}; @@ -879,10 +892,18 @@ int main(int argc, char* argv[]) { // light sensor scaling params const int LEON = is_leon(); - const float BRIGHTNESS_B = LEON ? 10.0 : 5.0; - const float BRIGHTNESS_M = LEON ? 2.6 : 1.3; + float brightness_b, brightness_m; + int result = read_param_float(&brightness_b, "BRIGHTNESS_B", true); + result += read_param_float(&brightness_m, "BRIGHTNESS_M", true); - float smooth_brightness = BRIGHTNESS_B; + if(result != 0){ + brightness_b = LEON ? 10.0 : 5.0; + brightness_m = LEON ? 2.6 : 1.3; + write_param_float(brightness_b, "BRIGHTNESS_B", true); + write_param_float(brightness_m, "BRIGHTNESS_M", true); + } + + float smooth_brightness = brightness_b; const int MIN_VOLUME = LEON ? 12 : 9; const int MAX_VOLUME = LEON ? 15 : 12; @@ -892,10 +913,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); @@ -904,7 +927,7 @@ int main(int argc, char* argv[]) { double u1 = millis_since_boot(); // light sensor is only exposed on EONs - float clipped_brightness = (s->light_sensor*BRIGHTNESS_M) + BRIGHTNESS_B; + float clipped_brightness = (s->light_sensor*brightness_m) + brightness_b; if (clipped_brightness > 512) clipped_brightness = 512; smooth_brightness = clipped_brightness * 0.01 + smooth_brightness * 0.99; if (smooth_brightness > 255) smooth_brightness = 255; @@ -925,22 +948,22 @@ int main(int argc, char* argv[]) { handle_vision_touch(s, touch_x, touch_y); } - if (!s->vision_connected) { - if (s->status != STATUS_STOPPED) { - update_status(s, STATUS_STOPPED); - } + if (!s->started) { + // always process events offroad check_messages(s); } else { set_awake(s, true); - if (s->status == STATUS_STOPPED) { - update_status(s, STATUS_DISENGAGED); + // Car started, fetch a new rgb image from ipc + 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. - s->scene.satelliteCount = -1; + + check_messages(s); + + // Visiond process is just stopped, force a redraw to make screen blank again. + if (!s->started) { s->scene.uilayout_sidebarcollapsed = false; + update_offroad_layout_state(s); ui_draw(s); glFinish(); should_swap = true; @@ -958,7 +981,7 @@ int main(int argc, char* argv[]) { if (s->hardware_timeout > 0) { s->hardware_timeout--; } else { - s->scene.hwType = cereal_HealthData_HwType_unknown; + s->scene.hwType = cereal::HealthData::HwType::UNKNOWN; } // Don't waste resources on drawing in case screen is off @@ -976,40 +999,49 @@ 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 (s->started && s->controls_seen && s->scene.alert_text2 != "Controls Unresponsive") { + LOGE("Controls unresponsive"); + s->scene.alert_size = cereal::ControlsState::AlertSize::FULL; + update_status(s, STATUS_ALERT); - // 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) { - s->scene.alert_size = ALERTSIZE_FULL; - if (s->status != STATUS_STOPPED) { - 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->scene.alert_text1 = "TAKE CONTROL IMMEDIATELY"; + s->scene.alert_text2 = "Controls Unresponsive"; + + ui_draw_vision_alert(s, s->scene.alert_size, s->status, s->scene.alert_text1.c_str(), s->scene.alert_text2.c_str()); s->alert_sound_timeout = 2 * UI_FREQ; - - s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_chimeWarningRepeat; + s->alert_sound = cereal::CarControl::HUDControl::AudibleAlert::CHIME_WARNING_REPEAT; 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 +1063,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 ee431a44..771b5501 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -1,6 +1,6 @@ #ifndef _UI_H #define _UI_H - +#include "cereal/gen/cpp/log.capnp.h" #ifdef __APPLE__ #include #define NANOVG_GL3_IMPLEMENTATION @@ -12,8 +12,8 @@ #define nvgCreate nvgCreateGLES3 #endif +#include #include - #include "nanovg.h" #include "common/mat.h" @@ -22,9 +22,6 @@ #include "common/framebuffer.h" #include "common/modeldata.h" #include "messaging.hpp" - -#include "cereal/gen/c/log.capnp.h" - #include "sound.hpp" #define STATUS_STOPPED 0 @@ -33,16 +30,17 @@ #define STATUS_WARNING 3 #define STATUS_ALERT 4 -#define ALERTSIZE_NONE 0 -#define ALERTSIZE_SMALL 1 -#define ALERTSIZE_MID 2 -#define ALERTSIZE_FULL 3 +#define NET_CONNECTED 0 +#define NET_DISCONNECTED 1 +#define NET_ERROR 2 -#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 @@ -63,6 +61,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 +121,7 @@ typedef struct UIScene { bool uilayout_sidebarcollapsed; bool uilayout_mapenabled; + bool uilayout_mockengaged; // responsive layout int ui_viz_rx; int ui_viz_rw; @@ -133,12 +133,16 @@ 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; - char alert_text1[1024]; - char alert_text2[1024]; - uint8_t alert_size; + std::string alert_text1; + std::string alert_text2; + cereal::ControlsState::AlertSize alert_size; float alert_blinkingrate; float awareness_status; @@ -146,15 +150,16 @@ typedef struct UIScene { // Used to show gps planner status bool gps_planner_active; - uint8_t networkType; - uint8_t networkStrength; + cereal::ThermalData::NetworkType networkType; + cereal::ThermalData::NetworkStrength networkStrength; int batteryPercent; - char batteryStatus[64]; + bool batteryCharging; float freeSpace; - uint8_t thermalStatus; + cereal::ThermalData::ThermalStatus thermalStatus; int paTemp; - int hwType; + cereal::HealthData::HwType hwType; int satelliteCount; + uint8_t athenaStatus; } UIScene; typedef struct { @@ -174,7 +179,6 @@ typedef struct { typedef struct UIState { pthread_mutex_t lock; - pthread_cond_t bg_cond; // framebuffer FramebufferState *fb; @@ -209,10 +213,13 @@ 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; - int active_app; + cereal::UiLayoutState::App active_app; // vision state bool vision_connected; @@ -255,20 +262,25 @@ 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; bool limit_set_speed; float speed_lim_off; bool is_ego_over_limit; - char alert_type[64]; + std::string alert_type; AudibleAlert alert_sound; - int alert_size; float alert_blinking_alpha; bool alert_blinked; + bool started; + bool thermal_started, preview_started; + bool vision_seen; float light_sensor; @@ -287,10 +299,13 @@ typedef struct UIState { } UIState; // API -void ui_draw_vision_alert(UIState *s, int va_size, int va_color, +void ui_draw_vision_alert(UIState *s, cereal::ControlsState::AlertSize 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_draw_rect(NVGcontext *vg, float x, float y, float w, float h, NVGcolor color, float r = 0, int width = 0); +void ui_draw_rect(NVGcontext *vg, float x, float y, float w, float h, NVGpaint paint, float r = 0); void ui_nvg_init(UIState *s); #endif diff --git a/selfdrive/updated.py b/selfdrive/updated.py index acc4603f..c31ab956 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -112,7 +112,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: @@ -132,6 +132,35 @@ def dismount_ovfs(): run(["umount", "-l", OVERLAY_MERGED]) +def setup_git_options(cwd): + # We sync FS object atimes (which EON doesn't use) and mtimes, but ctimes + # are outside user control. Make sure Git is set up to ignore system ctimes, + # because they change when we make hard links during finalize. Otherwise, + # there is a lot of unnecessary churn. This appears to be a common need on + # OSX as well: https://www.git-tower.com/blog/make-git-rebase-safe-on-osx/ + try: + trustctime = run(["git", "config", "--get", "core.trustctime"], cwd) + trustctime_set = (trustctime.strip() == "false") + except subprocess.CalledProcessError: + trustctime_set = False + + if not trustctime_set: + cloudlog.info("Setting core.trustctime false") + run(["git", "config", "core.trustctime", "false"], cwd) + + # We are temporarily using copytree to copy the directory, which also changes + # inode numbers. Ignore those changes too. + try: + checkstat = run(["git", "config", "--get", "core.checkStat"], cwd) + checkstat_set = (checkstat.strip() == "minimal") + except subprocess.CalledProcessError: + checkstat_set = False + + if not checkstat_set: + cloudlog.info("Setting core.checkState minimal") + run(["git", "config", "core.checkStat", "minimal"], cwd) + + def init_ovfs(): cloudlog.info("preparing new safe staging area") Params().put("UpdateAvailable", "0") @@ -151,17 +180,6 @@ def init_ovfs(): if os.path.isfile(os.path.join(BASEDIR, ".overlay_consistent")): os.remove(os.path.join(BASEDIR, ".overlay_consistent")) - # We sync FS object atimes (which EON doesn't use) and mtimes, but ctimes - # are outside user control. Make sure Git is set up to ignore system ctimes, - # because they change when we make hard links during finalize. Otherwise, - # there is a lot of unnecessary churn. This appears to be a common need on - # OSX as well: https://www.git-tower.com/blog/make-git-rebase-safe-on-osx/ - run(["git", "config", "core.trustctime", "false"], BASEDIR) - - # We are temporarily using copytree to copy the directory, which also changes - # inode numbers. Ignore those changes too. - run(["git", "config", "core.checkStat", "minimal"], BASEDIR) - # Leave a timestamped canary in BASEDIR to check at startup. The EON clock # should be correct by the time we get here. If the init file disappears, or # critical mtimes in BASEDIR are newer than .overlay_init, continue.sh can @@ -253,6 +271,8 @@ def finalize_from_ovfs_copy(): def attempt_update(): cloudlog.info("attempting git update inside staging overlay") + setup_git_options(OVERLAY_MERGED) + git_fetch_output = run(NICE_LOW_PRIORITY + ["git", "fetch"], OVERLAY_MERGED) cloudlog.info("git fetch success: %s", git_fetch_output) @@ -314,7 +334,7 @@ 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