diff --git a/.github/PULL_REQUEST_TEMPLATE/car_port.md b/.github/PULL_REQUEST_TEMPLATE/car_port.md
index d275468395..4264363ba2 100644
--- a/.github/PULL_REQUEST_TEMPLATE/car_port.md
+++ b/.github/PULL_REQUEST_TEMPLATE/car_port.md
@@ -9,7 +9,7 @@ assignees: ''
 **Checklist**
 
 - [ ] added to README
-- [ ] test route added to [test_routes.py](https://github.com/commaai/openpilot/blob/master/selfdrive/test/test_models.py)
+- [ ] test route added to [routes.py](https://github.com/commaai/openpilot/blob/master/selfdrive/car/tests/routes.py)
 - [ ] route with openpilot:
 - [ ] route with stock system:
 - [ ] car harness used (if comma doesn't sell it, put N/A):
diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml
index 9870637003..0083d39c3a 100644
--- a/.github/workflows/selfdrive_tests.yaml
+++ b/.github/workflows/selfdrive_tests.yaml
@@ -245,7 +245,6 @@ jobs:
     - name: Run unit tests
       run: |
         ${{ env.RUN }} "scons -j$(nproc) --test && \
-                        coverage run selfdrive/test/test_fingerprints.py && \
                         $UNIT_TEST common && \
                         $UNIT_TEST opendbc/can && \
                         $UNIT_TEST selfdrive/boardd && \
@@ -384,7 +383,7 @@ jobs:
       uses: actions/cache@v2
       with:
         path: /tmp/comma_download_cache
-        key: car_models-${{ hashFiles('selfdrive/test/test_models.py', 'selfdrive/test/test_routes.py') }}-${{ matrix.job }}
+        key: car_models-${{ hashFiles('selfdrive/car/tests/test_models.py', 'selfdrive/test/test_routes.py') }}-${{ matrix.job }}
     - name: Cache scons
       id: scons-cache
       # TODO: Change the version to the released version when https://github.com/actions/cache/pull/489 (or 571) is merged.
@@ -402,7 +401,7 @@ jobs:
     - name: Test car models
       run: |
         ${{ env.RUN }} "scons -j$(nproc) --test && \
-                        FILEREADER_CACHE=1 coverage run -m pytest selfdrive/test/test_models.py && \
+                        FILEREADER_CACHE=1 coverage run -m pytest selfdrive/car/tests/test_models.py && \
                         coverage xml && \
                         chmod -R 777 /tmp/comma_download_cache"
       env:
diff --git a/Pipfile.lock b/Pipfile.lock
index 25cc81cbb2..fdad4cb64d 100644
--- a/Pipfile.lock
+++ b/Pipfile.lock
@@ -16,20 +16,13 @@
         ]
     },
     "default": {
-        "appdirs": {
-            "hashes": [
-                "sha256:7d5d0167b2b1ba821647616af46a749d1c653740dd0d2415100fe26e27afdf41",
-                "sha256:a841dacd6b99318a741b166adb07e19ee71a274450e68237b4650ca1055ab128"
-            ],
-            "version": "==1.4.4"
-        },
         "astroid": {
             "hashes": [
-                "sha256:72ace9c3333e274e9248168fc4f3e300da8545af1c303bd69197027f49e2bfff",
-                "sha256:aa296702f1a5c3102c860de49473aaa90a7f6d221555d5cf2678940a9be32a4e"
+                "sha256:1efdf4e867d4d8ba4a9f6cf9ce07cd182c4c41de77f23814feb27ca93ca9d877",
+                "sha256:506daabe5edffb7e696ad82483ad0228245a9742ed7d2d8c9cdb31537decf9f6"
             ],
             "markers": "python_full_version >= '3.6.2'",
-            "version": "==2.9.2"
+            "version": "==2.9.3"
         },
         "atomicwrites": {
             "hashes": [
@@ -149,19 +142,19 @@
         },
         "charset-normalizer": {
             "hashes": [
-                "sha256:876d180e9d7432c5d1dfd4c5d26b72f099d503e8fcc0feb7532c9289be60fcbd",
-                "sha256:cb957888737fc0bbcd78e3df769addb41fd1ff8cf950dc9e7ad7793f1bf44455"
+                "sha256:2857e29ff0d34db842cd7ca3230549d1a697f96ee6d3fb071cfa6c7393832597",
+                "sha256:6881edbebdb17b39b4eaaa821b438bf6eddffb4468cf344f09f89def34a8b1df"
             ],
             "markers": "python_version >= '3'",
-            "version": "==2.0.10"
+            "version": "==2.0.12"
         },
         "click": {
             "hashes": [
-                "sha256:353f466495adaeb40b6b5f592f9f91cb22372351c84caeb068132442a4518ef3",
-                "sha256:410e932b050f5eed773c4cda94de75971c89cdb3155a72a0831139a79e5ecb5b"
+                "sha256:6a7a62563bbfabfda3a38f3023a1db4a35978c0abd76f6c9605ecd6554d6d9b1",
+                "sha256:8458d7b1287c5fb128c90e23381cf99dcde74beaf6c7ff6384ce84d6fe090adb"
             ],
             "markers": "python_version >= '3.6'",
-            "version": "==8.0.3"
+            "version": "==8.0.4"
         },
         "crcmod": {
             "hashes": [
@@ -201,55 +194,45 @@
         },
         "cython": {
             "hashes": [
-                "sha256:0205b685802eb4c039b14f67b7ac3f00c55ff04b9e3871df2249576d3e59ba42",
-                "sha256:0c3093bc99facfc97e5019f6c5bc39987663792265c1334d9fc9e37c3a3dcd6f",
-                "sha256:0ffce25bf50fa926ec6bf8d6f29650e7cb33fae445938c9880e1ce9b776353ef",
-                "sha256:10402f0f1564ffc6ecb9c45e07f995771d05bb0b0e1d151e40574638292ee3a5",
-                "sha256:1519eb639de308f5763eb0666b4cc7bd3958268f3f6228cc610b7b4d6c94b68b",
-                "sha256:233a87db76941626f1db08f4b25a4a5b425b13b64ed0e673c3641f7b650a48d8",
-                "sha256:2b834ff6e4d10ba6d7a0d676dd71c1b427a181ddbbbbf79e91d1861557aab59f",
-                "sha256:362fbb9cb4627c7786231429768b54aaba5459a2a0e46c25e59f202ca6155437",
-                "sha256:3aed8c642e8fb27024bca46830b7f62335a44a92354acf708d6b8d050f945a3a",
-                "sha256:41ee918480371ae5e5851ba9b1ead5a183e24aedb27bf807c7405d124e943f40",
-                "sha256:4b7d04b393d9a4b5fec0cbc4b0f29fe083a9d862d95231a6e7608978bd661d7e",
-                "sha256:4d868e1a41f5123f51a20c1b8e82f7cb6fa3370c104e98e707f7c910e8cadad1",
-                "sha256:5041adfef502d67ecd5e291a7cf645a37fed7a9dac557f40d491053f35204d00",
-                "sha256:51923120f57a42c59f5ee6bba9e89a31a394ae8cd419c753f64d8a3aea1ee8b7",
-                "sha256:531303085503959338e6cdac630626280ef111aecbb22d48321673a8c3897c0a",
-                "sha256:5ecf5cf5b57086cc6c1cfc76d6353bbd7023e95da32e0883f1302ca50e481c33",
-                "sha256:5fd5db458c9d3d2c2abd047f3190624d9cce8a80a8e0ca0baa69cfd133a523bc",
-                "sha256:6773cce9d4b3b6168d8feb2b6f06b658ef1e11cbfec075041745666d8e2a5e45",
-                "sha256:6b385f68789c3e8a75b4827e8a4970ff04605ad3cb1c0b41005cc69368dad65d",
-                "sha256:706ea55f58c2722206e51cd9a8754ed0995c4c4231d24b095875d2641d745222",
-                "sha256:75eaa22911d2ec37a3841f77b710b178c805cd378b5e1c4fb82dbc35620d2062",
-                "sha256:77913fe27c5e22c995bac090d01e200ff91e5f58aa944e2d2e94cbf67ea2ae34",
-                "sha256:7df94e56872df8f396ca669466fee60256f69f678654239f706b1e643c2ac4a5",
-                "sha256:82881565d04027728d7762edd8c085927a840873af7ee049d703e0ca226bc08d",
-                "sha256:868f309095e557f06dc58723ae865e8c65cfedb2646a562bd8080c92d69e4e4b",
-                "sha256:8e07121b34221458a2151d37e137b8f5b011a9c51dd38db2499a6198590aa319",
-                "sha256:93840f2071c1f15e613509eadee1fbcd335e8666772437fe5038e24059edd48c",
-                "sha256:a1cc55db32cd39474081d478263b96e036552cdbbab8831c90ea43fb385a9b66",
-                "sha256:af377d543a762867da11fcf6e558f7a4a535ff8693f30cce123fab10c00fa312",
-                "sha256:af91dd63ac5f1f7fc70dc91ea063f727db42b5eb934d1f3832611be18e25171e",
-                "sha256:b3041e45aefaa4449fd671902132c0ac1f72eedaedac745c0e1a70a13bf990bb",
-                "sha256:b5ca05c2bfba0c2480b5fd390ecffe46b8da574d887d600388d6e3caf3f99a88",
-                "sha256:bbf0149680c1fca07200a3ed372b22e6bad7851d191b717a61f9a68af370e180",
-                "sha256:be13be1e2b9b7395588f2a23bfa692f2f3e6f7936ccf7825c83431b8c8c3452b",
-                "sha256:be550b566345bf53b95616334793ce42a128cf1d9dcde1e28cbf7ce52ea61d6d",
-                "sha256:c4b003b6b7aa9e74552ef8d4e6009b3e3c3e8fa585710b3a6d062e088e460c1b",
-                "sha256:c813799d533194b7d85203d881d8b4f567a8c644a67f50d47f1ffbf316df412f",
-                "sha256:c91b1ba0d43f7f7ccde8121c672207c7891735ddcc83496af1e0ab617ddc4aba",
-                "sha256:ca10e9fde0eaba1407ab353ff07a26daaa3e4dbe356108a149e482d441f070dd",
-                "sha256:ce804a021c92fea66c8c100781a111706f21bade7a546895c5a9c57fe2df8b24",
-                "sha256:d83dad8dc6c63706cb3178dc79010b3865b1345090727189d2cd61758a825ee8",
-                "sha256:e118525defec3f67471d8ee5ce04340d43195410a87e5d7ec8a1a9e953c0066a",
-                "sha256:ebe32e002a9e6553de399033e259ece72aa17c77f740b265e66f122572a8a278",
-                "sha256:ed76fb98979f02b5e89079906071983a36f3634d3028b86f935cf0196f24fcaa",
-                "sha256:f5e15ff892c8afad64931ee3dd723c4755c2c516606f9aae7613bebfac62b0f6",
-                "sha256:fec66cd0a48697fab903854566235aecf1084f62e3163d6589ae7335a1b4d448"
-            ],
-            "index": "pypi",
-            "version": "==0.29.26"
+                "sha256:004387d8b94c64681ee05660d6a234e125396097726cf2f419c0fa2ac38034d6",
+                "sha256:0378a14d2580dcea234d7a2dc8d75f60c091105885096e6dd5b032be97542c16",
+                "sha256:03b749e4f0bbf631cee472add2806d338a7d496f8383f6fb28cc5fdc34b7fdb8",
+                "sha256:05edfa51c0ff31a8df3cb291b90ca93ab499686d023b9b81c216cd3509f73def",
+                "sha256:076aa8da83383e2bed0ca5f92c13a7e76e684bc41fe8e438bbed735f5b1c2731",
+                "sha256:09448aadb818387160ca4d1e1b82dbb7001526b6d0bed7529c4e8ac12e3b6f4c",
+                "sha256:1612d7439590ba3b8de5f907bf0e54bd8e024eafb8c59261531a7988030c182d",
+                "sha256:16f2e74fcac223c53e298ecead62c353d3cffa107bea5d8232e4b2ba40781634",
+                "sha256:26d8d0ededca42be50e0ac377c08408e18802b1391caa3aea045a72c1bff47ac",
+                "sha256:31465dce7fd3f058d02afb98b13af962848cc607052388814428dc801cc26f57",
+                "sha256:33b69ac9bbf2b93d8cae336cfe48889397a857e6ceeb5cef0b2f0b31b6c54f2b",
+                "sha256:341917bdb2c95bcf8322aacfe50bbe6b4794880b16fa8b2300330520e123a5e5",
+                "sha256:43eca77169f855dd04be11921a585c8854a174f30bc925257e92bc7b9197fbd2",
+                "sha256:49076747b731ed78acf203666c3b3c5d664754ea01ca4527f62f6d8675703688",
+                "sha256:4b3089255b6b1cc69e4b854626a41193e6acae5332263d24707976b3cb8ca644",
+                "sha256:5658fa477e80d96c49d5ff011938dd4b62da9aa428f771b91f1a7c49af45aad8",
+                "sha256:59f4e86b415620a097cf0ec602adf5a7ee3cc33e8220567ded96566f753483f8",
+                "sha256:5e82f6b3dc2133b2e0e2c5c63d352d40a695e40cc7ed99f4cbe83334bcf9ab39",
+                "sha256:6626f9691ce2093ccbcc9932f449efe3b6e1c893b556910881d177c61612e8ff",
+                "sha256:75686c586e37b1fed0fe4a2c053474f96fc07da0063bbfc98023454540515d31",
+                "sha256:7962a78ceb80cdec21345fb5088e675060fa65982030d446069f2d675d30e3cd",
+                "sha256:9d39ee7ddef6856413f950b8959e852d83376d9db1c509505e3f4873df32aa70",
+                "sha256:9f2b7c86a73db0d8dbbd885fe67f04c7b787df37a3848b9867270d3484101fbd",
+                "sha256:a0ed39c63ba52edd03a39ea9d6da6f5326aaee5d333c317feba543270a1b3af5",
+                "sha256:a3b27812ac9e9737026bfbb1dd47434f3e84013f430bafe1c6cbaf1cd51b5518",
+                "sha256:b6c77cc24861a33714e74212abfab4e54bf42e1ad602623f193b8e369389af2f",
+                "sha256:c9848a423a14e8f51bd4bbf8e2ff37031764ce66bdc7c6bc06c70d4084eb23c7",
+                "sha256:d6036f6a5a0c7fb1af88889872268b15bf20dd9cefe33a6602d79ba18b8db20f",
+                "sha256:d6fac2342802c30e51426828fe084ff4deb1b3387367cf98976bb2e64b6f8e45",
+                "sha256:d7c98727397c2547a56aa0c3c98140f1873c69a0642edc9446c6c870d0d8a5b5",
+                "sha256:d7d7beb600d5dd551e9322e1393b74286f4a3d4aa387f7bfbaccc1495a98603b",
+                "sha256:ded4fd3da4dee2f4414c35214244e29befa7f6fede3e9be317e765169df2cbc7",
+                "sha256:e24bd94946ffa37f30fcb865f2340fb6d429a3c7bf87b47b22f7d22e0e68a15c",
+                "sha256:e9cc6af0c9c477c5e175e807dce439509934efefc24ea2da9fced7fbc8170591",
+                "sha256:ed32c206e1d68056a34b21d2ec0cf0f23d338d6531476a68c73e21e20bd7bb63",
+                "sha256:fdcef7abb09fd827691e3abe6fd42c6c34beaccfa0bc2df6074f0a49949df6a8"
+            ],
+            "index": "pypi",
+            "version": "==0.29.28"
         },
         "flake8": {
             "hashes": [
@@ -261,11 +244,11 @@
         },
         "flask": {
             "hashes": [
-                "sha256:7b2fb8e934ddd50731893bdcdb00fc8c0315916f9fcd50d22c7cc1a95ab634e2",
-                "sha256:cb90f62f1d8e4dc4621f52106613488b5ba826b2e1e10a33eac92f723093ab6a"
+                "sha256:59da8a3170004800a2837844bfa84d49b022550616070f7cb1a659682b2e7c9f",
+                "sha256:e1120c228ca2f553b470df4a5fa927ab66258467526069981b3eb0a91902687d"
             ],
             "index": "pypi",
-            "version": "==2.0.2"
+            "version": "==2.0.3"
         },
         "flatbuffers": {
             "hashes": [
@@ -315,11 +298,11 @@
         },
         "itsdangerous": {
             "hashes": [
-                "sha256:5174094b9637652bdb841a3029700391451bd092ba3db90600dea710ba28e97c",
-                "sha256:9e724d68fc22902a1435351f84c3fb8623f303fffcc566a4cb952df8c572cff0"
+                "sha256:29285842166554469a56d427addc0843914172343784cb909695fdbe90a3e129",
+                "sha256:d848fcb8bc7d507c4546b448574e8a44fc4ea2ba84ebf8d783290d53e81992f5"
             ],
-            "markers": "python_version >= '3.6'",
-            "version": "==2.0.1"
+            "markers": "python_version >= '3.7'",
+            "version": "==2.1.0"
         },
         "jinja2": {
             "hashes": [
@@ -382,88 +365,59 @@
         },
         "libusb1": {
             "hashes": [
-                "sha256:81381ce1d8852a4d4345b2ee8218971d35865b5f025fef96b43ee082757099cd",
-                "sha256:9fda3055c98ab043cfb3beac93ef1de2900ad11d949c694f58bdf414ce2bd03c",
-                "sha256:a97bcb90f589d863c5e971b013c8cf7e1915680a951e66c4222a2c5bb64b7153",
-                "sha256:d3ba82ecf7ab6a48d21dac6697e26504670cc3522b8e5941bd28fb56cf3f6c46"
+                "sha256:083f75e5d15cb5e2159e64c308c5317284eae926a820d6dce53a9505d18be3b2",
+                "sha256:0e652b04cbe85ec8e74f9ee82b49f861fb14b5320ae51399387ad2601ccc0500",
+                "sha256:5792a9defee40f15d330a40d9b1800545c32e47ba7fc66b6f28f133c9fcc8538",
+                "sha256:6f6bb010632ada35c661d17a65e135077beef0fbb2434d5ffdb3a4a911fd9490"
             ],
             "index": "pypi",
-            "version": "==2.0.1"
+            "version": "==3.0.0"
         },
         "markupsafe": {
             "hashes": [
-                "sha256:01a9b8ea66f1658938f65b93a85ebe8bc016e6769611be228d797c9d998dd298",
-                "sha256:023cb26ec21ece8dc3907c0e8320058b2e0cb3c55cf9564da612bc325bed5e64",
-                "sha256:0446679737af14f45767963a1a9ef7620189912317d095f2d9ffa183a4d25d2b",
-                "sha256:04635854b943835a6ea959e948d19dcd311762c5c0c6e1f0e16ee57022669194",
-                "sha256:0717a7390a68be14b8c793ba258e075c6f4ca819f15edfc2a3a027c823718567",
-                "sha256:0955295dd5eec6cb6cc2fe1698f4c6d84af2e92de33fbcac4111913cd100a6ff",
-                "sha256:0d4b31cc67ab36e3392bbf3862cfbadac3db12bdd8b02a2731f509ed5b829724",
-                "sha256:10f82115e21dc0dfec9ab5c0223652f7197feb168c940f3ef61563fc2d6beb74",
-                "sha256:168cd0a3642de83558a5153c8bd34f175a9a6e7f6dc6384b9655d2697312a646",
-                "sha256:1d609f577dc6e1aa17d746f8bd3c31aa4d258f4070d61b2aa5c4166c1539de35",
-                "sha256:1f2ade76b9903f39aa442b4aadd2177decb66525062db244b35d71d0ee8599b6",
-                "sha256:20dca64a3ef2d6e4d5d615a3fd418ad3bde77a47ec8a23d984a12b5b4c74491a",
-                "sha256:2a7d351cbd8cfeb19ca00de495e224dea7e7d919659c2841bbb7f420ad03e2d6",
-                "sha256:2d7d807855b419fc2ed3e631034685db6079889a1f01d5d9dac950f764da3dad",
-                "sha256:2ef54abee730b502252bcdf31b10dacb0a416229b72c18b19e24a4509f273d26",
-                "sha256:36bc903cbb393720fad60fc28c10de6acf10dc6cc883f3e24ee4012371399a38",
-                "sha256:37205cac2a79194e3750b0af2a5720d95f786a55ce7df90c3af697bfa100eaac",
-                "sha256:3c112550557578c26af18a1ccc9e090bfe03832ae994343cfdacd287db6a6ae7",
-                "sha256:3dd007d54ee88b46be476e293f48c85048603f5f516008bee124ddd891398ed6",
-                "sha256:4296f2b1ce8c86a6aea78613c34bb1a672ea0e3de9c6ba08a960efe0b0a09047",
-                "sha256:47ab1e7b91c098ab893b828deafa1203de86d0bc6ab587b160f78fe6c4011f75",
-                "sha256:49e3ceeabbfb9d66c3aef5af3a60cc43b85c33df25ce03d0031a608b0a8b2e3f",
-                "sha256:4dc8f9fb58f7364b63fd9f85013b780ef83c11857ae79f2feda41e270468dd9b",
-                "sha256:4efca8f86c54b22348a5467704e3fec767b2db12fc39c6d963168ab1d3fc9135",
-                "sha256:53edb4da6925ad13c07b6d26c2a852bd81e364f95301c66e930ab2aef5b5ddd8",
-                "sha256:5855f8438a7d1d458206a2466bf82b0f104a3724bf96a1c781ab731e4201731a",
-                "sha256:594c67807fb16238b30c44bdf74f36c02cdf22d1c8cda91ef8a0ed8dabf5620a",
-                "sha256:5b6d930f030f8ed98e3e6c98ffa0652bdb82601e7a016ec2ab5d7ff23baa78d1",
-                "sha256:5bb28c636d87e840583ee3adeb78172efc47c8b26127267f54a9c0ec251d41a9",
-                "sha256:60bf42e36abfaf9aff1f50f52644b336d4f0a3fd6d8a60ca0d054ac9f713a864",
-                "sha256:611d1ad9a4288cf3e3c16014564df047fe08410e628f89805e475368bd304914",
-                "sha256:6300b8454aa6930a24b9618fbb54b5a68135092bc666f7b06901f897fa5c2fee",
-                "sha256:63f3268ba69ace99cab4e3e3b5840b03340efed0948ab8f78d2fd87ee5442a4f",
-                "sha256:6557b31b5e2c9ddf0de32a691f2312a32f77cd7681d8af66c2692efdbef84c18",
-                "sha256:693ce3f9e70a6cf7d2fb9e6c9d8b204b6b39897a2c4a1aa65728d5ac97dcc1d8",
-                "sha256:6a7fae0dd14cf60ad5ff42baa2e95727c3d81ded453457771d02b7d2b3f9c0c2",
-                "sha256:6c4ca60fa24e85fe25b912b01e62cb969d69a23a5d5867682dd3e80b5b02581d",
-                "sha256:6fcf051089389abe060c9cd7caa212c707e58153afa2c649f00346ce6d260f1b",
-                "sha256:7d91275b0245b1da4d4cfa07e0faedd5b0812efc15b702576d103293e252af1b",
-                "sha256:89c687013cb1cd489a0f0ac24febe8c7a666e6e221b783e53ac50ebf68e45d86",
-                "sha256:8d206346619592c6200148b01a2142798c989edcb9c896f9ac9722a99d4e77e6",
-                "sha256:905fec760bd2fa1388bb5b489ee8ee5f7291d692638ea5f67982d968366bef9f",
-                "sha256:97383d78eb34da7e1fa37dd273c20ad4320929af65d156e35a5e2d89566d9dfb",
-                "sha256:984d76483eb32f1bcb536dc27e4ad56bba4baa70be32fa87152832cdd9db0833",
-                "sha256:99df47edb6bda1249d3e80fdabb1dab8c08ef3975f69aed437cb69d0a5de1e28",
-                "sha256:9f02365d4e99430a12647f09b6cc8bab61a6564363f313126f775eb4f6ef798e",
-                "sha256:a30e67a65b53ea0a5e62fe23682cfe22712e01f453b95233b25502f7c61cb415",
-                "sha256:ab3ef638ace319fa26553db0624c4699e31a28bb2a835c5faca8f8acf6a5a902",
-                "sha256:aca6377c0cb8a8253e493c6b451565ac77e98c2951c45f913e0b52facdcff83f",
-                "sha256:add36cb2dbb8b736611303cd3bfcee00afd96471b09cda130da3581cbdc56a6d",
-                "sha256:b2f4bf27480f5e5e8ce285a8c8fd176c0b03e93dcc6646477d4630e83440c6a9",
-                "sha256:b7f2d075102dc8c794cbde1947378051c4e5180d52d276987b8d28a3bd58c17d",
-                "sha256:baa1a4e8f868845af802979fcdbf0bb11f94f1cb7ced4c4b8a351bb60d108145",
-                "sha256:be98f628055368795d818ebf93da628541e10b75b41c559fdf36d104c5787066",
-                "sha256:bf5d821ffabf0ef3533c39c518f3357b171a1651c1ff6827325e4489b0e46c3c",
-                "sha256:c47adbc92fc1bb2b3274c4b3a43ae0e4573d9fbff4f54cd484555edbf030baf1",
-                "sha256:cdfba22ea2f0029c9261a4bd07e830a8da012291fbe44dc794e488b6c9bb353a",
-                "sha256:d6c7ebd4e944c85e2c3421e612a7057a2f48d478d79e61800d81468a8d842207",
-                "sha256:d7f9850398e85aba693bb640262d3611788b1f29a79f0c93c565694658f4071f",
-                "sha256:d8446c54dc28c01e5a2dbac5a25f071f6653e6e40f3a8818e8b45d790fe6ef53",
-                "sha256:deb993cacb280823246a026e3b2d81c493c53de6acfd5e6bfe31ab3402bb37dd",
-                "sha256:e0f138900af21926a02425cf736db95be9f4af72ba1bb21453432a07f6082134",
-                "sha256:e9936f0b261d4df76ad22f8fee3ae83b60d7c3e871292cd42f40b81b70afae85",
-                "sha256:f0567c4dc99f264f49fe27da5f735f414c4e7e7dd850cfd8e69f0862d7c74ea9",
-                "sha256:f5653a225f31e113b152e56f154ccbe59eeb1c7487b39b9d9f9cdb58e6c79dc5",
-                "sha256:f826e31d18b516f653fe296d967d700fddad5901ae07c622bb3705955e1faa94",
-                "sha256:f8ba0e8349a38d3001fae7eadded3f6606f0da5d748ee53cc1dab1d6527b9509",
-                "sha256:f9081981fe268bd86831e5c75f7de206ef275defcb82bc70740ae6dc507aee51",
-                "sha256:fa130dd50c57d53368c9d59395cb5526eda596d3ffe36666cd81a44d56e48872"
+                "sha256:023af8c54fe63530545f70dd2a2a7eed18d07a9a77b94e8bf1e2ff7f252db9a3",
+                "sha256:09c86c9643cceb1d87ca08cdc30160d1b7ab49a8a21564868921959bd16441b8",
+                "sha256:142119fb14a1ef6d758912b25c4e803c3ff66920635c44078666fe7cc3f8f759",
+                "sha256:1d1fb9b2eec3c9714dd936860850300b51dbaa37404209c8d4cb66547884b7ed",
+                "sha256:204730fd5fe2fe3b1e9ccadb2bd18ba8712b111dcabce185af0b3b5285a7c989",
+                "sha256:24c3be29abb6b34052fd26fc7a8e0a49b1ee9d282e3665e8ad09a0a68faee5b3",
+                "sha256:290b02bab3c9e216da57c1d11d2ba73a9f73a614bbdcc027d299a60cdfabb11a",
+                "sha256:3028252424c72b2602a323f70fbf50aa80a5d3aa616ea6add4ba21ae9cc9da4c",
+                "sha256:30c653fde75a6e5eb814d2a0a89378f83d1d3f502ab710904ee585c38888816c",
+                "sha256:3cace1837bc84e63b3fd2dfce37f08f8c18aeb81ef5cf6bb9b51f625cb4e6cd8",
+                "sha256:4056f752015dfa9828dce3140dbadd543b555afb3252507348c493def166d454",
+                "sha256:454ffc1cbb75227d15667c09f164a0099159da0c1f3d2636aa648f12675491ad",
+                "sha256:598b65d74615c021423bd45c2bc5e9b59539c875a9bdb7e5f2a6b92dfcfc268d",
+                "sha256:599941da468f2cf22bf90a84f6e2a65524e87be2fce844f96f2dd9a6c9d1e635",
+                "sha256:5ddea4c352a488b5e1069069f2f501006b1a4362cb906bee9a193ef1245a7a61",
+                "sha256:62c0285e91414f5c8f621a17b69fc0088394ccdaa961ef469e833dbff64bd5ea",
+                "sha256:679cbb78914ab212c49c67ba2c7396dc599a8479de51b9a87b174700abd9ea49",
+                "sha256:6e104c0c2b4cd765b4e83909cde7ec61a1e313f8a75775897db321450e928cce",
+                "sha256:736895a020e31b428b3382a7887bfea96102c529530299f426bf2e636aacec9e",
+                "sha256:75bb36f134883fdbe13d8e63b8675f5f12b80bb6627f7714c7d6c5becf22719f",
+                "sha256:7d2f5d97fcbd004c03df8d8fe2b973fe2b14e7bfeb2cfa012eaa8759ce9a762f",
+                "sha256:80beaf63ddfbc64a0452b841d8036ca0611e049650e20afcb882f5d3c266d65f",
+                "sha256:84ad5e29bf8bab3ad70fd707d3c05524862bddc54dc040982b0dbcff36481de7",
+                "sha256:8da5924cb1f9064589767b0f3fc39d03e3d0fb5aa29e0cb21d43106519bd624a",
+                "sha256:961eb86e5be7d0973789f30ebcf6caab60b844203f4396ece27310295a6082c7",
+                "sha256:96de1932237abe0a13ba68b63e94113678c379dca45afa040a17b6e1ad7ed076",
+                "sha256:a0a0abef2ca47b33fb615b491ce31b055ef2430de52c5b3fb19a4042dbc5cadb",
+                "sha256:b2a5a856019d2833c56a3dcac1b80fe795c95f401818ea963594b345929dffa7",
+                "sha256:b8811d48078d1cf2a6863dafb896e68406c5f513048451cd2ded0473133473c7",
+                "sha256:c532d5ab79be0199fa2658e24a02fce8542df196e60665dd322409a03db6a52c",
+                "sha256:d3b64c65328cb4cd252c94f83e66e3d7acf8891e60ebf588d7b493a55a1dbf26",
+                "sha256:d4e702eea4a2903441f2735799d217f4ac1b55f7d8ad96ab7d4e25417cb0827c",
+                "sha256:d5653619b3eb5cbd35bfba3c12d575db2a74d15e0e1c08bf1db788069d410ce8",
+                "sha256:d66624f04de4af8bbf1c7f21cc06649c1c69a7f84109179add573ce35e46d448",
+                "sha256:e67ec74fada3841b8c5f4c4f197bea916025cb9aa3fe5abf7d52b655d042f956",
+                "sha256:e6f7f3f41faffaea6596da86ecc2389672fa949bd035251eab26dc6697451d05",
+                "sha256:f02cf7221d5cd915d7fa58ab64f7ee6dd0f6cddbb48683debf5d04ae9b1c2cc1",
+                "sha256:f0eddfcabd6936558ec020130f932d479930581171368fd728efcfb6ef0dd357",
+                "sha256:fabbe18087c3d33c5824cb145ffca52eccd053061df1d79d4b66dafa5ad2a5ea",
+                "sha256:fc3150f85e2dbcf99e65238c842d1cfe69d3e7649b19864c1cc043213d9cd730"
             ],
-            "markers": "python_version >= '3.6'",
-            "version": "==2.0.1"
+            "markers": "python_version >= '3.7'",
+            "version": "==2.1.0"
         },
         "mccabe": {
             "hashes": [
@@ -490,62 +444,59 @@
         },
         "numpy": {
             "hashes": [
-                "sha256:0cfe07133fd00b27edee5e6385e333e9eeb010607e8a46e1cd673f05f8596595",
-                "sha256:11a1f3816ea82eed4178102c56281782690ab5993251fdfd75039aad4d20385f",
-                "sha256:2762331de395739c91f1abb88041f94a080cb1143aeec791b3b223976228af3f",
-                "sha256:283d9de87c0133ef98f93dfc09fad3fb382f2a15580de75c02b5bb36a5a159a5",
-                "sha256:3d22662b4b10112c545c91a0741f2436f8ca979ab3d69d03d19322aa970f9695",
-                "sha256:41388e32e40b41dd56eb37fcaa7488b2b47b0adf77c66154d6b89622c110dfe9",
-                "sha256:42c16cec1c8cf2728f1d539bd55aaa9d6bb48a7de2f41eb944697293ef65a559",
-                "sha256:47ee7a839f5885bc0c63a74aabb91f6f40d7d7b639253768c4199b37aede7982",
-                "sha256:5a311ee4d983c487a0ab546708edbdd759393a3dc9cd30305170149fedd23c88",
-                "sha256:5dc65644f75a4c2970f21394ad8bea1a844104f0fe01f278631be1c7eae27226",
-                "sha256:6ed0d073a9c54ac40c41a9c2d53fcc3d4d4ed607670b9e7b0de1ba13b4cbfe6f",
-                "sha256:76ba7c40e80f9dc815c5e896330700fd6e20814e69da9c1267d65a4d051080f1",
-                "sha256:818b9be7900e8dc23e013a92779135623476f44a0de58b40c32a15368c01d471",
-                "sha256:a024181d7aef0004d76fb3bce2a4c9f2e67a609a9e2a6ff2571d30e9976aa383",
-                "sha256:a955e4128ac36797aaffd49ab44ec74a71c11d6938df83b1285492d277db5397",
-                "sha256:a97a954a8c2f046d3817c2bce16e3c7e9a9c2afffaf0400f5c16df5172a67c9c",
-                "sha256:a97e82c39d9856fe7d4f9b86d8a1e66eff99cf3a8b7ba48202f659703d27c46f",
-                "sha256:b55b953a1bdb465f4dc181758570d321db4ac23005f90ffd2b434cc6609a63dd",
-                "sha256:bb02929b0d6bfab4c48a79bd805bd7419114606947ec8284476167415171f55b",
-                "sha256:bece0a4a49e60e472a6d1f70ac6cdea00f9ab80ff01132f96bd970cdd8a9e5a9",
-                "sha256:e41e8951749c4b5c9a2dc5fdbc1a4eec6ab2a140fdae9b460b0f557eed870f4d",
-                "sha256:f71d57cc8645f14816ae249407d309be250ad8de93ef61d9709b45a0ddf4050c"
-            ],
-            "index": "pypi",
-            "version": "==1.22.0"
+                "sha256:03ae5850619abb34a879d5f2d4bb4dcd025d6d8fb72f5e461dae84edccfe129f",
+                "sha256:076aee5a3763d41da6bef9565fdf3cb987606f567cd8b104aded2b38b7b47abf",
+                "sha256:0b536b6840e84c1c6a410f3a5aa727821e6108f3454d81a5cd5900999ef04f89",
+                "sha256:15efb7b93806d438e3bc590ca8ef2f953b0ce4f86f337ef4559d31ec6cf9d7dd",
+                "sha256:168259b1b184aa83a514f307352c25c56af111c269ffc109d9704e81f72e764b",
+                "sha256:2638389562bda1635b564490d76713695ff497242a83d9b684d27bb4a6cc9d7a",
+                "sha256:3556c5550de40027d3121ebbb170f61bbe19eb639c7ad0c7b482cd9b560cd23b",
+                "sha256:4a176959b6e7e00b5a0d6f549a479f869829bfd8150282c590deee6d099bbb6e",
+                "sha256:515a8b6edbb904594685da6e176ac9fbea8f73a5ebae947281de6613e27f1956",
+                "sha256:55535c7c2f61e2b2fc817c5cbe1af7cb907c7f011e46ae0a52caa4be1f19afe2",
+                "sha256:59153979d60f5bfe9e4c00e401e24dfe0469ef8da6d68247439d3278f30a180f",
+                "sha256:60cb8e5933193a3cc2912ee29ca331e9c15b2da034f76159b7abc520b3d1233a",
+                "sha256:6767ad399e9327bfdbaa40871be4254d1995f4a3ca3806127f10cec778bd9896",
+                "sha256:76a4f9bce0278becc2da7da3b8ef854bed41a991f4226911a24a9711baad672c",
+                "sha256:8cf33634b60c9cef346663a222d9841d3bbbc0a2f00221d6bcfd0d993d5543f6",
+                "sha256:94dd11d9f13ea1be17bac39c1942f527cbf7065f94953cf62dfe805653da2f8f",
+                "sha256:aafa46b5a39a27aca566198d3312fb3bde95ce9677085efd02c86f7ef6be4ec7",
+                "sha256:badca914580eb46385e7f7e4e426fea6de0a37b9e06bec252e481ae7ec287082",
+                "sha256:d76a26c5118c4d96e264acc9e3242d72e1a2b92e739807b3b69d8d47684b6677"
+            ],
+            "index": "pypi",
+            "version": "==1.22.2"
         },
         "onnx": {
             "hashes": [
-                "sha256:0c176ef6e0c3b6bdfb69a43a66dcb8e6ba687437e302c79b4efb75027e1007dc",
-                "sha256:186abf5e9189b4b011da290c6d83d5499adefac8f6a07f5d596a192b4c911098",
-                "sha256:1e2f92a77d84ae84d25ac84ec84a77b53e427cc7b2eb72ed7d56f2204f885715",
-                "sha256:24d73ca7dfd7e6c7339944f89554b4010719899337924fca1447d8f1b5db50d6",
-                "sha256:24e654cca4c7285ea339fae15998dd33a5b9e57831d8ecb0bdb1f439c61c5736",
-                "sha256:253fd36cbcfcbbbe00e55dde7a09995b22fc2cc825f6de28e5ef9c47f581f264",
-                "sha256:358fc6f71841e30ca793a0c1bcd3d0b9c62e436e215773e77a301acb6106cbda",
-                "sha256:38e7d106fa98921faf909c2908bfd022eb2c594ecfbd275b60f80e0161cb8476",
-                "sha256:3b73128c269ef84694099dad2b06568f2672ce95761a51e0225401695dc2c136",
-                "sha256:4138093cbf11e4300b7a7679aedfe1972f81abeb284a731e90dffdf3ef6c5ca3",
-                "sha256:48a747b247bc626e049341b8e8c4aeac20aa2306d6b8dff9c9e53a6b14931f1e",
-                "sha256:4a53055b8f13747b607dbf835914c2bd60fa7214ee719893b003ceb5fc903220",
-                "sha256:526de93b57dd65b136bec85d5b4c6fa4455d6d817bb319b54797d29111b9c407",
-                "sha256:57f93db536766b1dcfeee583c02bd86c9f1c9a652253bd4f9bf189a39446de1c",
-                "sha256:6205849c935837a934a9ec1fd994f1e858ad7d253e02d0bacbe4add211e4255d",
-                "sha256:63aee84aed68c8e14583af48c79d99405844034043dee1efbd1937a78dfa7f6b",
-                "sha256:796fa0b80f108f2824cccf5c7298895a925aaea7831330a0bd720ceffc7be3c6",
-                "sha256:7e59a6da6e437488059080babc9d96cde7c929cc758ffe4b0171aceaea559ada",
-                "sha256:86baab35fc1a317369f2a0cd3816c0eeb9036c29f9a27ed5e8f6935e67cbf0a8",
-                "sha256:898915bcba9c1d54abef00f4ea7d60e59fdb2d21d49e7493acac40c121eca4df",
-                "sha256:a86e3f956e2a1d39772ae36d28c5b7f20fb6a883ae35971ada261b25548a8b32",
-                "sha256:bd31e61ba95c62548543d8de2007fcb18fd2f017a9a36f712bbc08ddad1f25f4",
-                "sha256:cc830b15fe11846911fdf068460fd5f20b0f711c8b4c575c68478a6bf2884304",
-                "sha256:ce14dbe32a250b7691751e809c232b9a206da138ac055e24b9e60a1500b4d5b8",
-                "sha256:d0a3951276ac83fde93632303ad0b3b69e10894b69b7fe5eab0361e4f4212627"
-            ],
-            "index": "pypi",
-            "version": "==1.10.2"
+                "sha256:0cf47c205b376b3763beef92a6de4152f3b1552d6f640d93044938500baf5958",
+                "sha256:3403884c482859f8cf2e0c276da84bd9ac2235d266726f4ddc9625d3fd263218",
+                "sha256:43b32a2f20c94aa98866deae9e4218faf0495144ad05402e918fa279674b6df9",
+                "sha256:4454906de80a351de6929b0896ad605d106c324c3112c92249240e531f68fbba",
+                "sha256:4aa899f74acd4c5543f0efed8bfe98a3b701df75c5ffa179212e3088c51971bb",
+                "sha256:58d4873ec587ac14c44227d8027787edc88cd61596e646e3417f2a826a920898",
+                "sha256:593ca9e11f15afa26b3aaf2d170bb803d4bd86dbd560aa7be4e5f535d03f83d5",
+                "sha256:67c6d2654c1c203e5c839a47900b51f588fd0de71bbd497fb193d30a0b3ec1e9",
+                "sha256:7924d9baa13dbbf335737229f6d068f380d153679f357e495da60007b61cf56d",
+                "sha256:7a2f5d6998fe79aed80fad9d4522140d02c4d29513047e335d5c5355c1ebda5e",
+                "sha256:82221a07707b1ccf71fb18c6abb77f2566517a55d5185809775b5ff008bfb35c",
+                "sha256:89420e5b824d7e182846fe2aa09190ddb41162b261465c6ca928174bc2ac10b7",
+                "sha256:997d91ffd7b7ae7aee09c6d652a896d906be430d425865c759b51a8de5df9fe0",
+                "sha256:9b9f58ea01c1b20b057f55f628df4fc0403bbc160b7282a56e3bb4df5c7fb96f",
+                "sha256:a6e9135f1d02539ca7573f699fb0d31d3c43d10fac1d2d2239a9a1c553506c29",
+                "sha256:ae74bf8fa343b64e2b7fe205091b7f3728887c018ae061d161dd86ec95eb66a8",
+                "sha256:b2de0b117ad77689d308824a0c9eb89539ec28a799b4e2e05b3bb977b0da0b45",
+                "sha256:c3d3503110f2cab2c818f4a7b2bc8abc3bc79649daa39e70d5fb504b208ddb1e",
+                "sha256:d6581dd2122525549d1d8b431b8bf375298993c77bddb8fd0bf0d92611df76a1",
+                "sha256:d6ddbe89e32f885db736d36fcb132784e368331a18c3b6168ac9f561eb462057",
+                "sha256:df85666ab2b88fd9cf9b2504bcb551da39422eab65a143926a8db58f81b09164",
+                "sha256:ea06dbf57a287657b6dc4e189918e4cb451450308589d482117216194d6f83d6",
+                "sha256:eb46f31f12bb0bfdcfb68497d10b20447cf8fa6c4f693120c013e052645357b8",
+                "sha256:eca224c7c2c8ee4072a0743e4898a84a9bdf8297b5e5910a2632e4c4182ffb2a",
+                "sha256:f335d982b8ed201cf767459b993630acfd20c32b100529f70af9f28a26e72167"
+            ],
+            "index": "pypi",
+            "version": "==1.11.0"
         },
         "onnxruntime-gpu": {
             "hashes": [
@@ -564,79 +515,84 @@
         },
         "pillow": {
             "hashes": [
-                "sha256:03b27b197deb4ee400ed57d8d4e572d2d8d80f825b6634daf6e2c18c3c6ccfa6",
-                "sha256:0b281fcadbb688607ea6ece7649c5d59d4bbd574e90db6cd030e9e85bde9fecc",
-                "sha256:0ebd8b9137630a7bbbff8c4b31e774ff05bbb90f7911d93ea2c9371e41039b52",
-                "sha256:113723312215b25c22df1fdf0e2da7a3b9c357a7d24a93ebbe80bfda4f37a8d4",
-                "sha256:2d16b6196fb7a54aff6b5e3ecd00f7c0bab1b56eee39214b2b223a9d938c50af",
-                "sha256:2fd8053e1f8ff1844419842fd474fc359676b2e2a2b66b11cc59f4fa0a301315",
-                "sha256:31b265496e603985fad54d52d11970383e317d11e18e856971bdbb86af7242a4",
-                "sha256:3586e12d874ce2f1bc875a3ffba98732ebb12e18fb6d97be482bd62b56803281",
-                "sha256:47f5cf60bcb9fbc46011f75c9b45a8b5ad077ca352a78185bd3e7f1d294b98bb",
-                "sha256:490e52e99224858f154975db61c060686df8a6b3f0212a678e5d2e2ce24675c9",
-                "sha256:500d397ddf4bbf2ca42e198399ac13e7841956c72645513e8ddf243b31ad2128",
-                "sha256:52abae4c96b5da630a8b4247de5428f593465291e5b239f3f843a911a3cf0105",
-                "sha256:6579f9ba84a3d4f1807c4aab4be06f373017fc65fff43498885ac50a9b47a553",
-                "sha256:68e06f8b2248f6dc8b899c3e7ecf02c9f413aab622f4d6190df53a78b93d97a5",
-                "sha256:6c5439bfb35a89cac50e81c751317faea647b9a3ec11c039900cd6915831064d",
-                "sha256:72c3110228944019e5f27232296c5923398496b28be42535e3b2dc7297b6e8b6",
-                "sha256:72f649d93d4cc4d8cf79c91ebc25137c358718ad75f99e99e043325ea7d56100",
-                "sha256:7aaf07085c756f6cb1c692ee0d5a86c531703b6e8c9cae581b31b562c16b98ce",
-                "sha256:80fe92813d208ce8aa7d76da878bdc84b90809f79ccbad2a288e9bcbeac1d9bd",
-                "sha256:95545137fc56ce8c10de646074d242001a112a92de169986abd8c88c27566a05",
-                "sha256:97b6d21771da41497b81652d44191489296555b761684f82b7b544c49989110f",
-                "sha256:98cb63ca63cb61f594511c06218ab4394bf80388b3d66cd61d0b1f63ee0ea69f",
-                "sha256:9f3b4522148586d35e78313db4db0df4b759ddd7649ef70002b6c3767d0fdeb7",
-                "sha256:a09a9d4ec2b7887f7a088bbaacfd5c07160e746e3d47ec5e8050ae3b2a229e9f",
-                "sha256:b5050d681bcf5c9f2570b93bee5d3ec8ae4cf23158812f91ed57f7126df91762",
-                "sha256:bb47a548cea95b86494a26c89d153fd31122ed65255db5dcbc421a2d28eb3379",
-                "sha256:bc462d24500ba707e9cbdef436c16e5c8cbf29908278af053008d9f689f56dee",
-                "sha256:c2067b3bb0781f14059b112c9da5a91c80a600a97915b4f48b37f197895dd925",
-                "sha256:d154ed971a4cc04b93a6d5b47f37948d1f621f25de3e8fa0c26b2d44f24e3e8f",
-                "sha256:d5dcea1387331c905405b09cdbfb34611050cc52c865d71f2362f354faee1e9f",
-                "sha256:ee6e2963e92762923956fe5d3479b1fdc3b76c83f290aad131a2f98c3df0593e",
-                "sha256:fd0e5062f11cb3e730450a7d9f323f4051b532781026395c4323b8ad055523c4"
-            ],
-            "index": "pypi",
-            "version": "==9.0.0"
+                "sha256:011233e0c42a4a7836498e98c1acf5e744c96a67dd5032a6f666cc1fb97eab97",
+                "sha256:0f29d831e2151e0b7b39981756d201f7108d3d215896212ffe2e992d06bfe049",
+                "sha256:12875d118f21cf35604176872447cdb57b07126750a33748bac15e77f90f1f9c",
+                "sha256:14d4b1341ac07ae07eb2cc682f459bec932a380c3b122f5540432d8977e64eae",
+                "sha256:1c3c33ac69cf059bbb9d1a71eeaba76781b450bc307e2291f8a4764d779a6b28",
+                "sha256:1d19397351f73a88904ad1aee421e800fe4bbcd1aeee6435fb62d0a05ccd1030",
+                "sha256:253e8a302a96df6927310a9d44e6103055e8fb96a6822f8b7f514bb7ef77de56",
+                "sha256:2632d0f846b7c7600edf53c48f8f9f1e13e62f66a6dbc15191029d950bfed976",
+                "sha256:335ace1a22325395c4ea88e00ba3dc89ca029bd66bd5a3c382d53e44f0ccd77e",
+                "sha256:413ce0bbf9fc6278b2d63309dfeefe452835e1c78398efb431bab0672fe9274e",
+                "sha256:5100b45a4638e3c00e4d2320d3193bdabb2d75e79793af7c3eb139e4f569f16f",
+                "sha256:514ceac913076feefbeaf89771fd6febde78b0c4c1b23aaeab082c41c694e81b",
+                "sha256:528a2a692c65dd5cafc130de286030af251d2ee0483a5bf50c9348aefe834e8a",
+                "sha256:6295f6763749b89c994fcb6d8a7f7ce03c3992e695f89f00b741b4580b199b7e",
+                "sha256:6c8bc8238a7dfdaf7a75f5ec5a663f4173f8c367e5a39f87e720495e1eed75fa",
+                "sha256:718856856ba31f14f13ba885ff13874be7fefc53984d2832458f12c38205f7f7",
+                "sha256:7f7609a718b177bf171ac93cea9fd2ddc0e03e84d8fa4e887bdfc39671d46b00",
+                "sha256:80ca33961ced9c63358056bd08403ff866512038883e74f3a4bf88ad3eb66838",
+                "sha256:80fe64a6deb6fcfdf7b8386f2cf216d329be6f2781f7d90304351811fb591360",
+                "sha256:81c4b81611e3a3cb30e59b0cf05b888c675f97e3adb2c8672c3154047980726b",
+                "sha256:855c583f268edde09474b081e3ddcd5cf3b20c12f26e0d434e1386cc5d318e7a",
+                "sha256:9bfdb82cdfeccec50aad441afc332faf8606dfa5e8efd18a6692b5d6e79f00fd",
+                "sha256:a5d24e1d674dd9d72c66ad3ea9131322819ff86250b30dc5821cbafcfa0b96b4",
+                "sha256:a9f44cd7e162ac6191491d7249cceb02b8116b0f7e847ee33f739d7cb1ea1f70",
+                "sha256:b5b3f092fe345c03bca1e0b687dfbb39364b21ebb8ba90e3fa707374b7915204",
+                "sha256:b9618823bd237c0d2575283f2939655f54d51b4527ec3972907a927acbcc5bfc",
+                "sha256:cef9c85ccbe9bee00909758936ea841ef12035296c748aaceee535969e27d31b",
+                "sha256:d21237d0cd37acded35154e29aec853e945950321dd2ffd1a7d86fe686814669",
+                "sha256:d3c5c79ab7dfce6d88f1ba639b77e77a17ea33a01b07b99840d6ed08031cb2a7",
+                "sha256:d9d7942b624b04b895cb95af03a23407f17646815495ce4547f0e60e0b06f58e",
+                "sha256:db6d9fac65bd08cea7f3540b899977c6dee9edad959fa4eaf305940d9cbd861c",
+                "sha256:ede5af4a2702444a832a800b8eb7f0a7a1c0eed55b644642e049c98d589e5092",
+                "sha256:effb7749713d5317478bb3acb3f81d9d7c7f86726d41c1facca068a04cf5bb4c",
+                "sha256:f154d173286a5d1863637a7dcd8c3437bb557520b01bddb0be0258dcb72696b5",
+                "sha256:f25ed6e28ddf50de7e7ea99d7a976d6a9c415f03adcaac9c41ff6ff41b6d86ac"
+            ],
+            "index": "pypi",
+            "version": "==9.0.1"
         },
         "platformdirs": {
             "hashes": [
-                "sha256:1d7385c7db91728b83efd0ca99a5afb296cab9d0ed8313a45ed8ba17967ecfca",
-                "sha256:440633ddfebcc36264232365d7840a970e75e1018d15b4327d11f91909045fda"
+                "sha256:7535e70dfa32e84d4b34996ea99c5e432fa29a708d0f4e394bbcb2a8faa4f16d",
+                "sha256:bcae7cab893c2d310a711b70b24efb93334febe65f8de776ee320b517471e227"
             ],
             "markers": "python_version >= '3.7'",
-            "version": "==2.4.1"
+            "version": "==2.5.1"
         },
         "protobuf": {
             "hashes": [
-                "sha256:038daf4fa38a7e818dd61f51f22588d61755160a98db087a046f80d66b855942",
-                "sha256:28ccea56d4dc38d35cd70c43c2da2f40ac0be0a355ef882242e8586c6d66666f",
-                "sha256:36d90676d6f426718463fe382ec6274909337ca6319d375eebd2044e6c6ac560",
-                "sha256:3cd0458870ea7d1c58e948ac8078f6ba8a7ecc44a57e03032ed066c5bb318089",
-                "sha256:5935c8ce02e3d89c7900140a8a42b35bc037ec07a6aeb61cc108be8d3c9438a6",
-                "sha256:615b426a177780ce381ecd212edc1e0f70db8557ed72560b82096bd36b01bc04",
-                "sha256:62a8e4baa9cb9e064eb62d1002eca820857ab2138440cb4b3ea4243830f94ca7",
-                "sha256:655264ed0d0efe47a523e2255fc1106a22f6faab7cc46cfe99b5bae085c2a13e",
-                "sha256:6e8ea9173403219239cdfd8d946ed101f2ab6ecc025b0fda0c6c713c35c9981d",
-                "sha256:71b0250b0cfb738442d60cab68abc166de43411f2a4f791d31378590bfb71bd7",
-                "sha256:74f33edeb4f3b7ed13d567881da8e5a92a72b36495d57d696c2ea1ae0cfee80c",
-                "sha256:77d2fadcf369b3f22859ab25bd12bb8e98fb11e05d9ff9b7cd45b711c719c002",
-                "sha256:8b30a7de128c46b5ecb343917d9fa737612a6e8280f440874e5cc2ba0d79b8f6",
-                "sha256:8e51561d72efd5bd5c91490af1f13e32bcba8dab4643761eb7de3ce18e64a853",
-                "sha256:a529e7df52204565bcd33738a7a5f288f3d2d37d86caa5d78c458fa5fabbd54d",
-                "sha256:b691d996c6d0984947c4cf8b7ae2fe372d99b32821d0584f0b90277aa36982d3",
-                "sha256:d80f80eb175bf5f1169139c2e0c5ada98b1c098e2b3c3736667f28cbbea39fc8",
-                "sha256:d83e1ef8cb74009bebee3e61cc84b1c9cd04935b72bca0cbc83217d140424995",
-                "sha256:d8919368410110633717c406ab5c97e8df5ce93020cfcf3012834f28b1fab1ea",
-                "sha256:db3532d9f7a6ebbe2392041350437953b6d7a792de10e629c1e4f5a6b1fe1ac6",
-                "sha256:e7b24c11df36ee8e0c085e5b0dc560289e4b58804746fb487287dda51410f1e2",
-                "sha256:e7e8d2c20921f8da0dea277dfefc6abac05903ceac8e72839b2da519db69206b",
-                "sha256:e813b1c9006b6399308e917ac5d298f345d95bb31f46f02b60cd92970a9afa17",
-                "sha256:fd390367fc211cc0ffcf3a9e149dfeca78fecc62adb911371db0cec5c8b7472d"
+                "sha256:072fbc78d705d3edc7ccac58a62c4c8e0cec856987da7df8aca86e647be4e35c",
+                "sha256:09297b7972da685ce269ec52af761743714996b4381c085205914c41fcab59fb",
+                "sha256:16f519de1313f1b7139ad70772e7db515b1420d208cb16c6d7858ea989fc64a9",
+                "sha256:1c91ef4110fdd2c590effb5dca8fdbdcb3bf563eece99287019c4204f53d81a4",
+                "sha256:3112b58aac3bac9c8be2b60a9daf6b558ca3f7681c130dcdd788ade7c9ffbdca",
+                "sha256:36cecbabbda242915529b8ff364f2263cd4de7c46bbe361418b5ed859677ba58",
+                "sha256:4276cdec4447bd5015453e41bdc0c0c1234eda08420b7c9a18b8d647add51e4b",
+                "sha256:435bb78b37fc386f9275a7035fe4fb1364484e38980d0dd91bc834a02c5ec909",
+                "sha256:48ed3877fa43e22bcacc852ca76d4775741f9709dd9575881a373bd3e85e54b2",
+                "sha256:54a1473077f3b616779ce31f477351a45b4fef8c9fd7892d6d87e287a38df368",
+                "sha256:69da7d39e39942bd52848438462674c463e23963a1fdaa84d88df7fbd7e749b2",
+                "sha256:6cbc312be5e71869d9d5ea25147cdf652a6781cf4d906497ca7690b7b9b5df13",
+                "sha256:7bb03bc2873a2842e5ebb4801f5c7ff1bfbdf426f85d0172f7644fcda0671ae0",
+                "sha256:7ca7da9c339ca8890d66958f5462beabd611eca6c958691a8fe6eccbd1eb0c6e",
+                "sha256:835a9c949dc193953c319603b2961c5c8f4327957fe23d914ca80d982665e8ee",
+                "sha256:84123274d982b9e248a143dadd1b9815049f4477dc783bf84efe6250eb4b836a",
+                "sha256:8961c3a78ebfcd000920c9060a262f082f29838682b1f7201889300c1fbe0616",
+                "sha256:96bd766831596d6014ca88d86dc8fe0fb2e428c0b02432fd9db3943202bf8c5e",
+                "sha256:9df0c10adf3e83015ced42a9a7bd64e13d06c4cf45c340d2c63020ea04499d0a",
+                "sha256:b38057450a0c566cbd04890a40edf916db890f2818e8682221611d78dc32ae26",
+                "sha256:bd95d1dfb9c4f4563e6093a9aa19d9c186bf98fa54da5252531cc0d3a07977e7",
+                "sha256:c1068287025f8ea025103e37d62ffd63fec8e9e636246b89c341aeda8a67c934",
+                "sha256:c438268eebb8cf039552897d78f402d734a404f1360592fef55297285f7f953f",
+                "sha256:cdc076c03381f5c1d9bb1abdcc5503d9ca8b53cf0a9d31a9f6754ec9e6c8af0f",
+                "sha256:f358aa33e03b7a84e0d91270a4d4d8f5df6921abe99a377828839e8ed0c04e07",
+                "sha256:f51d5a9f137f7a2cec2d326a74b6e3fc79d635d69ffe1b036d39fc7d75430d37"
             ],
             "markers": "python_version >= '3.5'",
-            "version": "==3.19.1"
+            "version": "==3.19.4"
         },
         "psutil": {
             "hashes": [
@@ -644,6 +600,7 @@
                 "sha256:1070a9b287846a21a5d572d6dddd369517510b68710fca56b0e9e02fd24bed9a",
                 "sha256:1d7b433519b9a38192dfda962dd8f44446668c009833e1429a52424624f408b4",
                 "sha256:3151a58f0fbd8942ba94f7c31c7e6b310d2989f4da74fcbf28b934374e9bf841",
+                "sha256:32acf55cb9a8cbfb29167cd005951df81b567099295291bcfd1027365b36591d",
                 "sha256:3611e87eea393f779a35b192b46a164b1d01167c9d323dda9b1e527ea69d697d",
                 "sha256:3d00a664e31921009a84367266b35ba0aac04a2a6cad09c550a89041034d19a0",
                 "sha256:4e2fb92e3aeae3ec3b7b66c528981fd327fb93fd906a77215200404444ec1845",
@@ -662,8 +619,12 @@
                 "sha256:9b51917c1af3fa35a3f2dabd7ba96a2a4f19df3dec911da73875e1edaf22a40b",
                 "sha256:b2237f35c4bbae932ee98902a08050a27821f8f6dfa880a47195e5993af4702d",
                 "sha256:c3400cae15bdb449d518545cbd5b649117de54e3596ded84aacabfbb3297ead2",
+                "sha256:c51f1af02334e4b516ec221ee26b8fdf105032418ca5a5ab9737e8c87dafe203",
                 "sha256:cb8d10461c1ceee0c25a64f2dd54872b70b89c26419e147a05a10b753ad36ec2",
+                "sha256:d62a2796e08dd024b8179bd441cb714e0f81226c352c802fca0fd3f89eeacd94",
                 "sha256:df2c8bd48fb83a8408c8390b143c6a6fa10cb1a674ca664954de193fdcab36a9",
+                "sha256:e5c783d0b1ad6ca8a5d3e7b680468c9c926b804be83a3a8e95141b05c39c9f64",
+                "sha256:e9805fed4f2a81de98ae5fe38b75a74c6e6ad2df8a5c479594c7629a1fe35f56",
                 "sha256:ea42d747c5f71b5ccaa6897b216a7dadb9f52c72a0fe2b872ef7d3e1eacf3ba3",
                 "sha256:ef216cc9feb60634bda2f341a9559ac594e2eeaadd0ba187a4c2eb5b5d40b91c",
                 "sha256:ff0d41f8b3e9ebb6b6110057e40019a432e96aae2008951121ba4e56040b84f3"
@@ -707,39 +668,39 @@
         },
         "pycryptodome": {
             "hashes": [
-                "sha256:1181c90d1a6aee68a84826825548d0db1b58d8541101f908d779d601d1690586",
-                "sha256:12c7343aec5a3b3df5c47265281b12b611f26ec9367b6129199d67da54b768c1",
-                "sha256:212c7f7fe11cad9275fbcff50ca977f1c6643f13560d081e7b0f70596df447b8",
-                "sha256:32d15da81959faea6cbed95df2bb44f7f796211c110cf90b5ad3b2aeeb97fc8e",
-                "sha256:341c6bbf932c406b4f3ee2372e8589b67ac0cf4e99e7dc081440f43a3cde9f0f",
-                "sha256:3558616f45d8584aee3eba27559bc6fd0ba9be6c076610ed3cc62bd5229ffdc3",
-                "sha256:39da5807aa1ff820799c928f745f89432908bf6624b9e981d2d7f9e55d91b860",
-                "sha256:3f2f3dd596c6128d91314e60a6bcf4344610ef0e97f4ae4dd1770f86dd0748d8",
-                "sha256:4cded12e13785bbdf4ba1ff5fb9d261cd98162145f869e4fbc4a4b9083392f0b",
-                "sha256:5a8c24d39d4a237dbfe181ea6593792bf9b5582c7fcfa7b8e0e12fda5eec07af",
-                "sha256:5d4264039a2087977f50072aaff2346d1c1c101cb359f9444cf92e3d1f42b4cd",
-                "sha256:6bb0d340c93bcb674ea8899e2f6408ec64c6c21731a59481332b4b2a8143cc60",
-                "sha256:6f8f5b7b53516da7511951910ab458e799173722c91fea54e2ba2f56d102e4aa",
-                "sha256:90ad3381ccdc6a24cc2841e295706a168f32abefe64c679695712acac71fd5da",
-                "sha256:93acad54a72d81253242eb0a15064be559ec9d989e5173286dc21cad19f01765",
-                "sha256:9ea2f6674c803602a7c0437fccdc2ea036707e60456974fe26ca263bd501ec45",
-                "sha256:a6e1bcd9d5855f1a3c0f8d585f44c81b08f39a02754007f374fb8db9605ba29c",
-                "sha256:a78e4324e566b5fbc2b51e9240950d82fa9e1c7eb77acdf27f58712f65622c1d",
-                "sha256:aceb1d217c3a025fb963849071446cf3aca1353282fe1c3cb7bd7339a4d47947",
-                "sha256:aed7eb4b64c600fbc5e6d4238991ad1b4179a558401f203d1fcbd24883748982",
-                "sha256:b07a4238465eb8c65dd5df2ab8ba6df127e412293c0ed7656c003336f557a100",
-                "sha256:b91404611767a7485837a6f1fd20cf9a5ae0ad362040a022cd65827ecb1b0d00",
-                "sha256:d8083de50f6dec56c3c6f270fb193590999583a1b27c9c75bc0b5cac22d438cc",
-                "sha256:d845c587ceb82ac7cbac7d0bf8c62a1a0fe7190b028b322da5ca65f6e5a18b9e",
-                "sha256:db66ccda65d5d20c17b00768e462a86f6f540f9aea8419a7f76cc7d9effd82cd",
-                "sha256:dc88355c4b261ed259268e65705b28b44d99570337694d593f06e3b1698eaaf3",
-                "sha256:de0b711d673904dd6c65307ead36cb76622365a393569bf880895cba21195b7a",
-                "sha256:e05f994f30f1cda3cbe57441f41220d16731cf99d868bb02a8f6484c454c206b",
-                "sha256:e80f7469b0b3ea0f694230477d8501dc5a30a717e94fddd4821e6721f3053eae",
-                "sha256:f699360ae285fcae9c8f53ca6acf33796025a82bb0ccd7c1c551b04c1726def3"
-            ],
-            "index": "pypi",
-            "version": "==3.12.0"
+                "sha256:028dcbf62d128b4335b61c9fbb7dd8c376594db607ef36d5721ee659719935d5",
+                "sha256:12ef157eb1e01a157ca43eda275fa68f8db0dd2792bc4fe00479ab8f0e6ae075",
+                "sha256:2562de213960693b6d657098505fd4493c45f3429304da67efcbeb61f0edfe89",
+                "sha256:27e92c1293afcb8d2639baf7eb43f4baada86e4de0f1fb22312bfc989b95dae2",
+                "sha256:36e3242c4792e54ed906c53f5d840712793dc68b726ec6baefd8d978c5282d30",
+                "sha256:50a5346af703330944bea503106cd50c9c2212174cfcb9939db4deb5305a8367",
+                "sha256:53dedbd2a6a0b02924718b520a723e88bcf22e37076191eb9b91b79934fb2192",
+                "sha256:69f05aaa90c99ac2f2af72d8d7f185f729721ad7c4be89e9e3d0ab101b0ee875",
+                "sha256:75a3a364fee153e77ed889c957f6f94ec6d234b82e7195b117180dcc9fc16f96",
+                "sha256:766a8e9832128c70012e0c2b263049506cbf334fb21ff7224e2704102b6ef59e",
+                "sha256:7fb90a5000cc9c9ff34b4d99f7f039e9c3477700e309ff234eafca7b7471afc0",
+                "sha256:893f32210de74b9f8ac869ed66c97d04e7d351182d6d39ebd3b36d3db8bda65d",
+                "sha256:8b5c28058102e2974b9868d72ae5144128485d466ba8739abd674b77971454cc",
+                "sha256:924b6aad5386fb54f2645f22658cb0398b1f25bc1e714a6d1522c75d527deaa5",
+                "sha256:9924248d6920b59c260adcae3ee231cd5af404ac706ad30aa4cd87051bf09c50",
+                "sha256:9ec761a35dbac4a99dcbc5cd557e6e57432ddf3e17af8c3c86b44af9da0189c0",
+                "sha256:a36ab51674b014ba03da7f98b675fcb8eabd709a2d8e18219f784aba2db73b72",
+                "sha256:aae395f79fa549fb1f6e3dc85cf277f0351e15a22e6547250056c7f0c990d6a5",
+                "sha256:c880a98376939165b7dc504559f60abe234b99e294523a273847f9e7756f4132",
+                "sha256:ce7a875694cd6ccd8682017a7c06c6483600f151d8916f2b25cf7a439e600263",
+                "sha256:d1b7739b68a032ad14c5e51f7e4e1a5f92f3628bba024a2bda1f30c481fc85d8",
+                "sha256:dcd65355acba9a1d0fc9b923875da35ed50506e339b35436277703d7ace3e222",
+                "sha256:e04e40a7f8c1669195536a37979dd87da2c32dbdc73d6fe35f0077b0c17c803b",
+                "sha256:e0c04c41e9ade19fbc0eff6aacea40b831bfcb2c91c266137bcdfd0d7b2f33ba",
+                "sha256:e24d4ec4b029611359566c52f31af45c5aecde7ef90bf8f31620fd44c438efe7",
+                "sha256:e64738207a02a83590df35f59d708bf1e7ea0d6adce712a777be2967e5f7043c",
+                "sha256:ea56a35fd0d13121417d39a83f291017551fa2c62d6daa6b04af6ece7ed30d84",
+                "sha256:f2772af1c3ef8025c85335f8b828d0193fa1e43256621f613280e2c81bfad423",
+                "sha256:f403a3e297a59d94121cb3ee4b1cf41f844332940a62d71f9e4a009cc3533493",
+                "sha256:f572a3ff7b6029dd9b904d6be4e0ce9e309dcb847b03e3ac8698d9d23bb36525"
+            ],
+            "index": "pypi",
+            "version": "==3.14.1"
         },
         "pyflakes": {
             "hashes": [
@@ -767,16 +728,16 @@
         },
         "pyopencl": {
             "hashes": [
-                "sha256:0a7cc1a461a4e57aa142b558b678fe23114aa6314d4a0c969bd2e2b5a02b65ad",
-                "sha256:2042811175bc8c915f5b56d8aa43561a5c62d6a145d67309e1e3f93d3b964744",
-                "sha256:334a6cdde7ef18e4370604b9a1d6551b055e8828a4da004893f26091669b561b",
-                "sha256:3678c8b02494e6d9627647c037aabed8244088d51cc6de8605f3854747985ac1",
-                "sha256:8bc2495cc4d78e8ca2358d3d14c5ba4b078cdbdb1a38e765a10c70e13df4871c",
-                "sha256:93f0c93e0fb6607ba0ee9bd11165edaf6406bf303b4ec795c3d2caa2e44f394e",
-                "sha256:e007e4e932170f0343cf1ab7733a2568aa8fda89f9a62e02aa359066084ee5c9"
+                "sha256:1ab792ff11a7ff271b1ba39686bd7aae271168e813a7e771bff6aa611ada383b",
+                "sha256:24c6d9a0a1ff7609f3d7365ebd8778616a5433c426566c2c8e35ffab00ef22c4",
+                "sha256:3b3f0d8b69923064f6d64b520c594aa2506e96ab398a0f1ed4895c4fceb20ff4",
+                "sha256:4bea0e965252f477204a87cc73eca492de5c30252198bc6679ef4968880a2443",
+                "sha256:7d9ec6c8daf283666dd577d6bcc49ab3d5b44c9532c2f642daf7e9c85b73606f",
+                "sha256:d8af9c4ea375f51237059124f7bab1fd32e32a8893997ffa05dff88efc4d9274",
+                "sha256:dee72b29c4e47a99f5ecc7fc7ba70bfc3d85e3cc9734765d6992380f5296787b"
             ],
             "index": "pypi",
-            "version": "==2021.2.11"
+            "version": "==2022.1"
         },
         "pyserial": {
             "hashes": [
@@ -796,10 +757,10 @@
         },
         "pytools": {
             "hashes": [
-                "sha256:db6cf83c9ba0a165d545029e2301621486d1e9ef295684072e5cd75316a13755"
+                "sha256:197aacf6e1f5d60c715b92438792acb7702aed59ebbfb147fa84eda780e54fee"
             ],
             "markers": "python_version ~= '3.6'",
-            "version": "==2021.2.9"
+            "version": "==2022.1"
         },
         "pyyaml": {
             "hashes": [
@@ -911,11 +872,11 @@
         },
         "sentry-sdk": {
             "hashes": [
-                "sha256:2a1757d6611e4bec7d672c2b7ef45afef79fed201d064f53994753303944f5a8",
-                "sha256:e4cb107e305b2c1b919414775fa73a9997f996447417d22b98e7610ded1e9eb5"
+                "sha256:1ab34e3851a34aeb3d1af1a0f77cec73978c4e9698e5210d050e4932953cb241",
+                "sha256:ac2a50128409d57655279817aedcb7800cace1f76b266f3dd62055d5afd6e098"
             ],
             "index": "pypi",
-            "version": "==1.5.1"
+            "version": "==1.5.6"
         },
         "setproctitle": {
             "hashes": [
@@ -946,11 +907,11 @@
         },
         "setuptools": {
             "hashes": [
-                "sha256:5c89b1a14a67ac5f0956f1cb0aeb7d1d3f4c8ba4e4e1ab7bf1af4933f9a2f0fe",
-                "sha256:675fcebecb43c32eb930481abf907619137547f4336206e4d673180242e1a278"
+                "sha256:2347b2b432c891a863acadca2da9ac101eae6169b1d3dfee2ec605ecd50dbfe5",
+                "sha256:e4f30b9f84e5ab3decf945113119649fec09c1fc3507c6ebffec75646c56e62b"
             ],
             "markers": "python_version >= '3.7'",
-            "version": "==60.2.0"
+            "version": "==60.9.3"
         },
         "six": {
             "hashes": [
@@ -1002,19 +963,19 @@
         },
         "typing-extensions": {
             "hashes": [
-                "sha256:4ca091dea149f945ec56afb48dae714f21e8692ef22a395223bcd328961b6a0e",
-                "sha256:7f001e5ac290a0c0401508864c7ec868be4e701886d5b573a9528ed3973d9d3b"
+                "sha256:1a9462dcc3347a79b1f1c0271fbe79e844580bb598bafa1ed208b94da3cdcd42",
+                "sha256:21c85e0fe4b9a155d0799430b0ad741cdce7e359660ccbd8b530613e8df88ce2"
             ],
-            "markers": "python_version < '3.10'",
-            "version": "==4.0.1"
+            "markers": "python_version >= '3.6'",
+            "version": "==4.1.1"
         },
         "urllib3": {
             "hashes": [
-                "sha256:4987c65554f7a2dbf30c18fd48778ef124af6fab771a377103da0585e2336ece",
-                "sha256:c4fdf4019605b6e5423637e01bc9fe4daef873709a7973e195ceba0a62bbc844"
+                "sha256:000ca7f471a233c2251c6c7023ee85305721bfdf18621ebff4fd17a8653427ed",
+                "sha256:0e7c33d9a63e7ddfcb86780aac87befc2fbddf46c58dbb487e0855f7ceec283c"
             ],
             "index": "pypi",
-            "version": "==1.26.7"
+            "version": "==1.26.8"
         },
         "utm": {
             "hashes": [
@@ -1025,19 +986,19 @@
         },
         "websocket-client": {
             "hashes": [
-                "sha256:1315816c0acc508997eb3ae03b9d3ff619c9d12d544c9a9b553704b1cc4f6af5",
-                "sha256:2eed4cc58e4d65613ed6114af2f380f7910ff416fc8c46947f6e76b6815f56c0"
+                "sha256:074e2ed575e7c822fc0940d31c3ac9bb2b1142c303eafcf3e304e6ce035522e8",
+                "sha256:6278a75065395418283f887de7c3beafb3aa68dada5cacbe4b214e8d26da499b"
             ],
             "index": "pypi",
-            "version": "==1.2.3"
+            "version": "==1.3.1"
         },
         "werkzeug": {
             "hashes": [
-                "sha256:63d3dc1cf60e7b7e35e97fa9861f7397283b75d765afcaefd993d6046899de8f",
-                "sha256:aa2bb6fc8dee8d6c504c0ac1e7f5f7dc5810a9903e793b6f715a9f015bdadb9a"
+                "sha256:1421ebfc7648a39a5c58c601b154165d05cf47a3cd0ccb70857cbdacf6c8f2b8",
+                "sha256:b863f8ff057c522164b6067c9e28b041161b4be5ba4d0daceeaa50a163822d3c"
             ],
             "markers": "python_version >= '3.6'",
-            "version": "==2.0.2"
+            "version": "==2.0.3"
         },
         "wrapt": {
             "hashes": [
@@ -1139,11 +1100,11 @@
         },
         "breathe": {
             "hashes": [
-                "sha256:19faef9d63c39acb3026eeaf6e6fdc5edb95334ed36fe0c627b358d6a2d5e0da",
-                "sha256:925eeff96c6640cd857e4ddeae6f75464a1d5e2e08ee56dccce4043583ae2050"
+                "sha256:553aeffb00efc2cf96c4c9ed388d6ee8036ecd6d1bd9bd0c656fc25ca271bd3c",
+                "sha256:c4b9ff4d5298fd91518d336ede28b6a2d8cacc685d0eae17eb20e760e06bb904"
             ],
             "index": "pypi",
-            "version": "==4.31.0"
+            "version": "==4.33.1"
         },
         "certifi": {
             "hashes": [
@@ -1218,11 +1179,11 @@
         },
         "charset-normalizer": {
             "hashes": [
-                "sha256:876d180e9d7432c5d1dfd4c5d26b72f099d503e8fcc0feb7532c9289be60fcbd",
-                "sha256:cb957888737fc0bbcd78e3df769addb41fd1ff8cf950dc9e7ad7793f1bf44455"
+                "sha256:2857e29ff0d34db842cd7ca3230549d1a697f96ee6d3fb071cfa6c7393832597",
+                "sha256:6881edbebdb17b39b4eaaa821b438bf6eddffb4468cf344f09f89def34a8b1df"
             ],
             "markers": "python_version >= '3'",
-            "version": "==2.0.10"
+            "version": "==2.0.12"
         },
         "control": {
             "hashes": [
@@ -1233,56 +1194,50 @@
         },
         "coverage": {
             "hashes": [
-                "sha256:01774a2c2c729619760320270e42cd9e797427ecfddd32c2a7b639cdc481f3c0",
-                "sha256:03b20e52b7d31be571c9c06b74746746d4eb82fc260e594dc662ed48145e9efd",
-                "sha256:0a7726f74ff63f41e95ed3a89fef002916c828bb5fcae83b505b49d81a066884",
-                "sha256:1219d760ccfafc03c0822ae2e06e3b1248a8e6d1a70928966bafc6838d3c9e48",
-                "sha256:13362889b2d46e8d9f97c421539c97c963e34031ab0cb89e8ca83a10cc71ac76",
-                "sha256:174cf9b4bef0db2e8244f82059a5a72bd47e1d40e71c68ab055425172b16b7d0",
-                "sha256:17e6c11038d4ed6e8af1407d9e89a2904d573be29d51515f14262d7f10ef0a64",
-                "sha256:215f8afcc02a24c2d9a10d3790b21054b58d71f4b3c6f055d4bb1b15cecce685",
-                "sha256:22e60a3ca5acba37d1d4a2ee66e051f5b0e1b9ac950b5b0cf4aa5366eda41d47",
-                "sha256:2641f803ee9f95b1f387f3e8f3bf28d83d9b69a39e9911e5bfee832bea75240d",
-                "sha256:276651978c94a8c5672ea60a2656e95a3cce2a3f31e9fb2d5ebd4c215d095840",
-                "sha256:3f7c17209eef285c86f819ff04a6d4cbee9b33ef05cbcaae4c0b4e8e06b3ec8f",
-                "sha256:3feac4084291642165c3a0d9eaebedf19ffa505016c4d3db15bfe235718d4971",
-                "sha256:49dbff64961bc9bdd2289a2bda6a3a5a331964ba5497f694e2cbd540d656dc1c",
-                "sha256:4e547122ca2d244f7c090fe3f4b5a5861255ff66b7ab6d98f44a0222aaf8671a",
-                "sha256:5829192582c0ec8ca4a2532407bc14c2f338d9878a10442f5d03804a95fac9de",
-                "sha256:5d6b09c972ce9200264c35a1d53d43ca55ef61836d9ec60f0d44273a31aa9f17",
-                "sha256:600617008aa82032ddeace2535626d1bc212dfff32b43989539deda63b3f36e4",
-                "sha256:619346d57c7126ae49ac95b11b0dc8e36c1dd49d148477461bb66c8cf13bb521",
-                "sha256:63c424e6f5b4ab1cf1e23a43b12f542b0ec2e54f99ec9f11b75382152981df57",
-                "sha256:6dbc1536e105adda7a6312c778f15aaabe583b0e9a0b0a324990334fd458c94b",
-                "sha256:6e1394d24d5938e561fbeaa0cd3d356207579c28bd1792f25a068743f2d5b282",
-                "sha256:86f2e78b1eff847609b1ca8050c9e1fa3bd44ce755b2ec30e70f2d3ba3844644",
-                "sha256:8bdfe9ff3a4ea37d17f172ac0dff1e1c383aec17a636b9b35906babc9f0f5475",
-                "sha256:8e2c35a4c1f269704e90888e56f794e2d9c0262fb0c1b1c8c4ee44d9b9e77b5d",
-                "sha256:92b8c845527eae547a2a6617d336adc56394050c3ed8a6918683646328fbb6da",
-                "sha256:9365ed5cce5d0cf2c10afc6add145c5037d3148585b8ae0e77cc1efdd6aa2953",
-                "sha256:9a29311bd6429be317c1f3fe4bc06c4c5ee45e2fa61b2a19d4d1d6111cb94af2",
-                "sha256:9a2b5b52be0a8626fcbffd7e689781bf8c2ac01613e77feda93d96184949a98e",
-                "sha256:a4bdeb0a52d1d04123b41d90a4390b096f3ef38eee35e11f0b22c2d031222c6c",
-                "sha256:a9c8c4283e17690ff1a7427123ffb428ad6a52ed720d550e299e8291e33184dc",
-                "sha256:b637c57fdb8be84e91fac60d9325a66a5981f8086c954ea2772efe28425eaf64",
-                "sha256:bf154ba7ee2fd613eb541c2bc03d3d9ac667080a737449d1a3fb342740eb1a74",
-                "sha256:c254b03032d5a06de049ce8bca8338a5185f07fb76600afff3c161e053d88617",
-                "sha256:c332d8f8d448ded473b97fefe4a0983265af21917d8b0cdcb8bb06b2afe632c3",
-                "sha256:c7912d1526299cb04c88288e148c6c87c0df600eca76efd99d84396cfe00ef1d",
-                "sha256:cfd9386c1d6f13b37e05a91a8583e802f8059bebfccde61a418c5808dea6bbfa",
-                "sha256:d5d2033d5db1d58ae2d62f095e1aefb6988af65b4b12cb8987af409587cc0739",
-                "sha256:dca38a21e4423f3edb821292e97cec7ad38086f84313462098568baedf4331f8",
-                "sha256:e2cad8093172b7d1595b4ad66f24270808658e11acf43a8f95b41276162eb5b8",
-                "sha256:e3db840a4dee542e37e09f30859f1612da90e1c5239a6a2498c473183a50e781",
-                "sha256:edcada2e24ed68f019175c2b2af2a8b481d3d084798b8c20d15d34f5c733fa58",
-                "sha256:f467bbb837691ab5a8ca359199d3429a11a01e6dfb3d9dcc676dc035ca93c0a9",
-                "sha256:f506af4f27def639ba45789fa6fde45f9a217da0be05f8910458e4557eed020c",
-                "sha256:f614fc9956d76d8a88a88bb41ddc12709caa755666f580af3a688899721efecd",
-                "sha256:f9afb5b746781fc2abce26193d1c817b7eb0e11459510fba65d2bd77fe161d9e",
-                "sha256:fb8b8ee99b3fffe4fd86f4c81b35a6bf7e4462cba019997af2fe679365db0c49"
-            ],
-            "index": "pypi",
-            "version": "==6.2"
+                "sha256:03e2a7826086b91ef345ff18742ee9fc47a6839ccd517061ef8fa1976e652ce9",
+                "sha256:07e6db90cd9686c767dcc593dff16c8c09f9814f5e9c51034066cad3373b914d",
+                "sha256:18d520c6860515a771708937d2f78f63cc47ab3b80cb78e86573b0a760161faf",
+                "sha256:1ebf730d2381158ecf3dfd4453fbca0613e16eaa547b4170e2450c9707665ce7",
+                "sha256:21b7745788866028adeb1e0eca3bf1101109e2dc58456cb49d2d9b99a8c516e6",
+                "sha256:26e2deacd414fc2f97dd9f7676ee3eaecd299ca751412d89f40bc01557a6b1b4",
+                "sha256:2c6dbb42f3ad25760010c45191e9757e7dce981cbfb90e42feef301d71540059",
+                "sha256:2fea046bfb455510e05be95e879f0e768d45c10c11509e20e06d8fcaa31d9e39",
+                "sha256:34626a7eee2a3da12af0507780bb51eb52dca0e1751fd1471d0810539cefb536",
+                "sha256:37d1141ad6b2466a7b53a22e08fe76994c2d35a5b6b469590424a9953155afac",
+                "sha256:46191097ebc381fbf89bdce207a6c107ac4ec0890d8d20f3360345ff5976155c",
+                "sha256:4dd8bafa458b5c7d061540f1ee9f18025a68e2d8471b3e858a9dad47c8d41903",
+                "sha256:4e21876082ed887baed0146fe222f861b5815455ada3b33b890f4105d806128d",
+                "sha256:58303469e9a272b4abdb9e302a780072c0633cdcc0165db7eec0f9e32f901e05",
+                "sha256:5ca5aeb4344b30d0bec47481536b8ba1181d50dbe783b0e4ad03c95dc1296684",
+                "sha256:68353fe7cdf91f109fc7d474461b46e7f1f14e533e911a2a2cbb8b0fc8613cf1",
+                "sha256:6f89d05e028d274ce4fa1a86887b071ae1755082ef94a6740238cd7a8178804f",
+                "sha256:7a15dc0a14008f1da3d1ebd44bdda3e357dbabdf5a0b5034d38fcde0b5c234b7",
+                "sha256:8bdde1177f2311ee552f47ae6e5aa7750c0e3291ca6b75f71f7ffe1f1dab3dca",
+                "sha256:8ce257cac556cb03be4a248d92ed36904a59a4a5ff55a994e92214cde15c5bad",
+                "sha256:8cf5cfcb1521dc3255d845d9dca3ff204b3229401994ef8d1984b32746bb45ca",
+                "sha256:8fbbdc8d55990eac1b0919ca69eb5a988a802b854488c34b8f37f3e2025fa90d",
+                "sha256:9548f10d8be799551eb3a9c74bbf2b4934ddb330e08a73320123c07f95cc2d92",
+                "sha256:96f8a1cb43ca1422f36492bebe63312d396491a9165ed3b9231e778d43a7fca4",
+                "sha256:9b27d894748475fa858f9597c0ee1d4829f44683f3813633aaf94b19cb5453cf",
+                "sha256:9baff2a45ae1f17c8078452e9e5962e518eab705e50a0aa8083733ea7d45f3a6",
+                "sha256:a2a8b8bcc399edb4347a5ca8b9b87e7524c0967b335fbb08a83c8421489ddee1",
+                "sha256:acf53bc2cf7282ab9b8ba346746afe703474004d9e566ad164c91a7a59f188a4",
+                "sha256:b0be84e5a6209858a1d3e8d1806c46214e867ce1b0fd32e4ea03f4bd8b2e3359",
+                "sha256:b31651d018b23ec463e95cf10070d0b2c548aa950a03d0b559eaa11c7e5a6fa3",
+                "sha256:b78e5afb39941572209f71866aa0b206c12f0109835aa0d601e41552f9b3e620",
+                "sha256:c76aeef1b95aff3905fb2ae2d96e319caca5b76fa41d3470b19d4e4a3a313512",
+                "sha256:dd035edafefee4d573140a76fdc785dc38829fe5a455c4bb12bac8c20cfc3d69",
+                "sha256:dd6fe30bd519694b356cbfcaca9bd5c1737cddd20778c6a581ae20dc8c04def2",
+                "sha256:e5f4e1edcf57ce94e5475fe09e5afa3e3145081318e5fd1a43a6b4539a97e518",
+                "sha256:ec6bc7fe73a938933d4178c9b23c4e0568e43e220aef9472c4f6044bfc6dd0f0",
+                "sha256:f1555ea6d6da108e1999b2463ea1003fe03f29213e459145e70edbaf3e004aaa",
+                "sha256:f5fa5803f47e095d7ad8443d28b01d48c0359484fec1b9d8606d0e3282084bc4",
+                "sha256:f7331dbf301b7289013175087636bbaf5b2405e57259dd2c42fdcc9fcc47325e",
+                "sha256:f9987b0354b06d4df0f4d3e0ec1ae76d7ce7cbca9a2f98c25041eb79eec766f1",
+                "sha256:fd9e830e9d8d89b20ab1e5af09b32d33e1a08ef4c4e14411e559556fd788e6b2"
+            ],
+            "index": "pypi",
+            "version": "==6.3.2"
         },
         "cryptography": {
             "hashes": [
@@ -1374,19 +1329,19 @@
         },
         "filelock": {
             "hashes": [
-                "sha256:38b4f4c989f9d06d44524df1b24bd19e167d851f19b50bf3e3559952dddc5b80",
-                "sha256:cf0fc6a2f8d26bd900f19bf33915ca70ba4dd8c56903eeb14e1e7a2fd7590146"
+                "sha256:9cd540a9352e432c7246a48fe4e8712b10acb1df2ad1f30e8c070b82ae1fed85",
+                "sha256:f8314284bfffbdcfa0ff3d7992b023d4c628ced6feb957351d4c48d059f56bc0"
             ],
             "markers": "python_version >= '3.7'",
-            "version": "==3.4.2"
+            "version": "==3.6.0"
         },
         "fonttools": {
             "hashes": [
-                "sha256:545c05d0f7903a863c2020e07b8f0a57517f2c40d940bded77076397872d14ca",
-                "sha256:edf251d5d2cc0580d5f72de4621c338d8c66c5f61abb50cf486640f73c8194d5"
+                "sha256:1933415e0fbdf068815cb1baaa1f159e17830215f7e8624e5731122761627557",
+                "sha256:2b18a172120e32128a80efee04cff487d5d140fe7d817deb648b2eee023a40e4"
             ],
             "markers": "python_version >= '3.7'",
-            "version": "==4.28.5"
+            "version": "==4.29.1"
         },
         "hexdump": {
             "hashes": [
@@ -1397,19 +1352,19 @@
         },
         "hypothesis": {
             "hashes": [
-                "sha256:317f8d2f670fa69e258ab43e21c2befd413c559e386581f7e9641a80460b1063",
-                "sha256:803792d416ff71307d775fe760e2b2f07ca302a2c941b576629668092b9f3e3d"
+                "sha256:1c1777ccd3074fcca768d7594a284a41501c99843089d37e3235d38319e3f217",
+                "sha256:79321035b9174ffa506d724ca5e8af375d7bf532c80e4f602bd433792c527e6c"
             ],
             "index": "pypi",
-            "version": "==6.34.2"
+            "version": "==6.37.2"
         },
         "identify": {
             "hashes": [
-                "sha256:0192893ff68b03d37fed553e261d4a22f94ea974093aefb33b29df2ff35fed3c",
-                "sha256:64d4885e539f505dd8ffb5e93c142a1db45480452b1594cacd3e91dca9a984e9"
+                "sha256:2986942d3974c8f2e5019a190523b0b0e2a07cb8e89bf236727fb4b26f27f8fd",
+                "sha256:fd906823ed1db23c7a48f9b176a1d71cb8abede1e21ebe614bac7bdd688d9213"
             ],
-            "markers": "python_full_version >= '3.6.1'",
-            "version": "==2.4.1"
+            "markers": "python_version >= '3.7'",
+            "version": "==2.4.11"
         },
         "idna": {
             "hashes": [
@@ -1427,6 +1382,14 @@
             "markers": "python_version >= '2.7' and python_version not in '3.0, 3.1, 3.2, 3.3'",
             "version": "==1.3.0"
         },
+        "importlib-metadata": {
+            "hashes": [
+                "sha256:175f4ee440a0317f6e8d81b7f8d4869f93316170a65ad2b007d2929186c8052c",
+                "sha256:e0bc84ff355328a4adfc5240c4f211e0ab386f80aa640d1b11f0618a1d282094"
+            ],
+            "markers": "python_version < '3.10'",
+            "version": "==4.11.1"
+        },
         "iniconfig": {
             "hashes": [
                 "sha256:011e24c64b7f47f6ebd835bb12a743f2fbe9a26d4cecaa7f53bc4f35ee9da8b3",
@@ -1509,86 +1472,57 @@
         },
         "markdown-it-py": {
             "hashes": [
-                "sha256:15cc69c5b7c493ba8603722b710e39ce3fab2961994179fb4fa1c99b070d2059",
-                "sha256:c138a596f6c9988e0b5fa3299bc38ffa76c75076bc178e8dfac40a84343c7022"
+                "sha256:31974138ca8cafbcb62213f4974b29571b940e78364584729233f59b8dfdb8bd",
+                "sha256:7b5c153ae1ab2cde00a33938bce68f3ad5d68fbe363f946de7d28555bed4e08a"
             ],
             "index": "pypi",
-            "version": "==2.0.0"
+            "version": "==2.0.1"
         },
         "markupsafe": {
             "hashes": [
-                "sha256:01a9b8ea66f1658938f65b93a85ebe8bc016e6769611be228d797c9d998dd298",
-                "sha256:023cb26ec21ece8dc3907c0e8320058b2e0cb3c55cf9564da612bc325bed5e64",
-                "sha256:0446679737af14f45767963a1a9ef7620189912317d095f2d9ffa183a4d25d2b",
-                "sha256:04635854b943835a6ea959e948d19dcd311762c5c0c6e1f0e16ee57022669194",
-                "sha256:0717a7390a68be14b8c793ba258e075c6f4ca819f15edfc2a3a027c823718567",
-                "sha256:0955295dd5eec6cb6cc2fe1698f4c6d84af2e92de33fbcac4111913cd100a6ff",
-                "sha256:0d4b31cc67ab36e3392bbf3862cfbadac3db12bdd8b02a2731f509ed5b829724",
-                "sha256:10f82115e21dc0dfec9ab5c0223652f7197feb168c940f3ef61563fc2d6beb74",
-                "sha256:168cd0a3642de83558a5153c8bd34f175a9a6e7f6dc6384b9655d2697312a646",
-                "sha256:1d609f577dc6e1aa17d746f8bd3c31aa4d258f4070d61b2aa5c4166c1539de35",
-                "sha256:1f2ade76b9903f39aa442b4aadd2177decb66525062db244b35d71d0ee8599b6",
-                "sha256:20dca64a3ef2d6e4d5d615a3fd418ad3bde77a47ec8a23d984a12b5b4c74491a",
-                "sha256:2a7d351cbd8cfeb19ca00de495e224dea7e7d919659c2841bbb7f420ad03e2d6",
-                "sha256:2d7d807855b419fc2ed3e631034685db6079889a1f01d5d9dac950f764da3dad",
-                "sha256:2ef54abee730b502252bcdf31b10dacb0a416229b72c18b19e24a4509f273d26",
-                "sha256:36bc903cbb393720fad60fc28c10de6acf10dc6cc883f3e24ee4012371399a38",
-                "sha256:37205cac2a79194e3750b0af2a5720d95f786a55ce7df90c3af697bfa100eaac",
-                "sha256:3c112550557578c26af18a1ccc9e090bfe03832ae994343cfdacd287db6a6ae7",
-                "sha256:3dd007d54ee88b46be476e293f48c85048603f5f516008bee124ddd891398ed6",
-                "sha256:4296f2b1ce8c86a6aea78613c34bb1a672ea0e3de9c6ba08a960efe0b0a09047",
-                "sha256:47ab1e7b91c098ab893b828deafa1203de86d0bc6ab587b160f78fe6c4011f75",
-                "sha256:49e3ceeabbfb9d66c3aef5af3a60cc43b85c33df25ce03d0031a608b0a8b2e3f",
-                "sha256:4dc8f9fb58f7364b63fd9f85013b780ef83c11857ae79f2feda41e270468dd9b",
-                "sha256:4efca8f86c54b22348a5467704e3fec767b2db12fc39c6d963168ab1d3fc9135",
-                "sha256:53edb4da6925ad13c07b6d26c2a852bd81e364f95301c66e930ab2aef5b5ddd8",
-                "sha256:5855f8438a7d1d458206a2466bf82b0f104a3724bf96a1c781ab731e4201731a",
-                "sha256:594c67807fb16238b30c44bdf74f36c02cdf22d1c8cda91ef8a0ed8dabf5620a",
-                "sha256:5b6d930f030f8ed98e3e6c98ffa0652bdb82601e7a016ec2ab5d7ff23baa78d1",
-                "sha256:5bb28c636d87e840583ee3adeb78172efc47c8b26127267f54a9c0ec251d41a9",
-                "sha256:60bf42e36abfaf9aff1f50f52644b336d4f0a3fd6d8a60ca0d054ac9f713a864",
-                "sha256:611d1ad9a4288cf3e3c16014564df047fe08410e628f89805e475368bd304914",
-                "sha256:6300b8454aa6930a24b9618fbb54b5a68135092bc666f7b06901f897fa5c2fee",
-                "sha256:63f3268ba69ace99cab4e3e3b5840b03340efed0948ab8f78d2fd87ee5442a4f",
-                "sha256:6557b31b5e2c9ddf0de32a691f2312a32f77cd7681d8af66c2692efdbef84c18",
-                "sha256:693ce3f9e70a6cf7d2fb9e6c9d8b204b6b39897a2c4a1aa65728d5ac97dcc1d8",
-                "sha256:6a7fae0dd14cf60ad5ff42baa2e95727c3d81ded453457771d02b7d2b3f9c0c2",
-                "sha256:6c4ca60fa24e85fe25b912b01e62cb969d69a23a5d5867682dd3e80b5b02581d",
-                "sha256:6fcf051089389abe060c9cd7caa212c707e58153afa2c649f00346ce6d260f1b",
-                "sha256:7d91275b0245b1da4d4cfa07e0faedd5b0812efc15b702576d103293e252af1b",
-                "sha256:89c687013cb1cd489a0f0ac24febe8c7a666e6e221b783e53ac50ebf68e45d86",
-                "sha256:8d206346619592c6200148b01a2142798c989edcb9c896f9ac9722a99d4e77e6",
-                "sha256:905fec760bd2fa1388bb5b489ee8ee5f7291d692638ea5f67982d968366bef9f",
-                "sha256:97383d78eb34da7e1fa37dd273c20ad4320929af65d156e35a5e2d89566d9dfb",
-                "sha256:984d76483eb32f1bcb536dc27e4ad56bba4baa70be32fa87152832cdd9db0833",
-                "sha256:99df47edb6bda1249d3e80fdabb1dab8c08ef3975f69aed437cb69d0a5de1e28",
-                "sha256:9f02365d4e99430a12647f09b6cc8bab61a6564363f313126f775eb4f6ef798e",
-                "sha256:a30e67a65b53ea0a5e62fe23682cfe22712e01f453b95233b25502f7c61cb415",
-                "sha256:ab3ef638ace319fa26553db0624c4699e31a28bb2a835c5faca8f8acf6a5a902",
-                "sha256:aca6377c0cb8a8253e493c6b451565ac77e98c2951c45f913e0b52facdcff83f",
-                "sha256:add36cb2dbb8b736611303cd3bfcee00afd96471b09cda130da3581cbdc56a6d",
-                "sha256:b2f4bf27480f5e5e8ce285a8c8fd176c0b03e93dcc6646477d4630e83440c6a9",
-                "sha256:b7f2d075102dc8c794cbde1947378051c4e5180d52d276987b8d28a3bd58c17d",
-                "sha256:baa1a4e8f868845af802979fcdbf0bb11f94f1cb7ced4c4b8a351bb60d108145",
-                "sha256:be98f628055368795d818ebf93da628541e10b75b41c559fdf36d104c5787066",
-                "sha256:bf5d821ffabf0ef3533c39c518f3357b171a1651c1ff6827325e4489b0e46c3c",
-                "sha256:c47adbc92fc1bb2b3274c4b3a43ae0e4573d9fbff4f54cd484555edbf030baf1",
-                "sha256:cdfba22ea2f0029c9261a4bd07e830a8da012291fbe44dc794e488b6c9bb353a",
-                "sha256:d6c7ebd4e944c85e2c3421e612a7057a2f48d478d79e61800d81468a8d842207",
-                "sha256:d7f9850398e85aba693bb640262d3611788b1f29a79f0c93c565694658f4071f",
-                "sha256:d8446c54dc28c01e5a2dbac5a25f071f6653e6e40f3a8818e8b45d790fe6ef53",
-                "sha256:deb993cacb280823246a026e3b2d81c493c53de6acfd5e6bfe31ab3402bb37dd",
-                "sha256:e0f138900af21926a02425cf736db95be9f4af72ba1bb21453432a07f6082134",
-                "sha256:e9936f0b261d4df76ad22f8fee3ae83b60d7c3e871292cd42f40b81b70afae85",
-                "sha256:f0567c4dc99f264f49fe27da5f735f414c4e7e7dd850cfd8e69f0862d7c74ea9",
-                "sha256:f5653a225f31e113b152e56f154ccbe59eeb1c7487b39b9d9f9cdb58e6c79dc5",
-                "sha256:f826e31d18b516f653fe296d967d700fddad5901ae07c622bb3705955e1faa94",
-                "sha256:f8ba0e8349a38d3001fae7eadded3f6606f0da5d748ee53cc1dab1d6527b9509",
-                "sha256:f9081981fe268bd86831e5c75f7de206ef275defcb82bc70740ae6dc507aee51",
-                "sha256:fa130dd50c57d53368c9d59395cb5526eda596d3ffe36666cd81a44d56e48872"
+                "sha256:023af8c54fe63530545f70dd2a2a7eed18d07a9a77b94e8bf1e2ff7f252db9a3",
+                "sha256:09c86c9643cceb1d87ca08cdc30160d1b7ab49a8a21564868921959bd16441b8",
+                "sha256:142119fb14a1ef6d758912b25c4e803c3ff66920635c44078666fe7cc3f8f759",
+                "sha256:1d1fb9b2eec3c9714dd936860850300b51dbaa37404209c8d4cb66547884b7ed",
+                "sha256:204730fd5fe2fe3b1e9ccadb2bd18ba8712b111dcabce185af0b3b5285a7c989",
+                "sha256:24c3be29abb6b34052fd26fc7a8e0a49b1ee9d282e3665e8ad09a0a68faee5b3",
+                "sha256:290b02bab3c9e216da57c1d11d2ba73a9f73a614bbdcc027d299a60cdfabb11a",
+                "sha256:3028252424c72b2602a323f70fbf50aa80a5d3aa616ea6add4ba21ae9cc9da4c",
+                "sha256:30c653fde75a6e5eb814d2a0a89378f83d1d3f502ab710904ee585c38888816c",
+                "sha256:3cace1837bc84e63b3fd2dfce37f08f8c18aeb81ef5cf6bb9b51f625cb4e6cd8",
+                "sha256:4056f752015dfa9828dce3140dbadd543b555afb3252507348c493def166d454",
+                "sha256:454ffc1cbb75227d15667c09f164a0099159da0c1f3d2636aa648f12675491ad",
+                "sha256:598b65d74615c021423bd45c2bc5e9b59539c875a9bdb7e5f2a6b92dfcfc268d",
+                "sha256:599941da468f2cf22bf90a84f6e2a65524e87be2fce844f96f2dd9a6c9d1e635",
+                "sha256:5ddea4c352a488b5e1069069f2f501006b1a4362cb906bee9a193ef1245a7a61",
+                "sha256:62c0285e91414f5c8f621a17b69fc0088394ccdaa961ef469e833dbff64bd5ea",
+                "sha256:679cbb78914ab212c49c67ba2c7396dc599a8479de51b9a87b174700abd9ea49",
+                "sha256:6e104c0c2b4cd765b4e83909cde7ec61a1e313f8a75775897db321450e928cce",
+                "sha256:736895a020e31b428b3382a7887bfea96102c529530299f426bf2e636aacec9e",
+                "sha256:75bb36f134883fdbe13d8e63b8675f5f12b80bb6627f7714c7d6c5becf22719f",
+                "sha256:7d2f5d97fcbd004c03df8d8fe2b973fe2b14e7bfeb2cfa012eaa8759ce9a762f",
+                "sha256:80beaf63ddfbc64a0452b841d8036ca0611e049650e20afcb882f5d3c266d65f",
+                "sha256:84ad5e29bf8bab3ad70fd707d3c05524862bddc54dc040982b0dbcff36481de7",
+                "sha256:8da5924cb1f9064589767b0f3fc39d03e3d0fb5aa29e0cb21d43106519bd624a",
+                "sha256:961eb86e5be7d0973789f30ebcf6caab60b844203f4396ece27310295a6082c7",
+                "sha256:96de1932237abe0a13ba68b63e94113678c379dca45afa040a17b6e1ad7ed076",
+                "sha256:a0a0abef2ca47b33fb615b491ce31b055ef2430de52c5b3fb19a4042dbc5cadb",
+                "sha256:b2a5a856019d2833c56a3dcac1b80fe795c95f401818ea963594b345929dffa7",
+                "sha256:b8811d48078d1cf2a6863dafb896e68406c5f513048451cd2ded0473133473c7",
+                "sha256:c532d5ab79be0199fa2658e24a02fce8542df196e60665dd322409a03db6a52c",
+                "sha256:d3b64c65328cb4cd252c94f83e66e3d7acf8891e60ebf588d7b493a55a1dbf26",
+                "sha256:d4e702eea4a2903441f2735799d217f4ac1b55f7d8ad96ab7d4e25417cb0827c",
+                "sha256:d5653619b3eb5cbd35bfba3c12d575db2a74d15e0e1c08bf1db788069d410ce8",
+                "sha256:d66624f04de4af8bbf1c7f21cc06649c1c69a7f84109179add573ce35e46d448",
+                "sha256:e67ec74fada3841b8c5f4c4f197bea916025cb9aa3fe5abf7d52b655d042f956",
+                "sha256:e6f7f3f41faffaea6596da86ecc2389672fa949bd035251eab26dc6697451d05",
+                "sha256:f02cf7221d5cd915d7fa58ab64f7ee6dd0f6cddbb48683debf5d04ae9b1c2cc1",
+                "sha256:f0eddfcabd6936558ec020130f932d479930581171368fd728efcfb6ef0dd357",
+                "sha256:fabbe18087c3d33c5824cb145ffca52eccd053061df1d79d4b66dafa5ad2a5ea",
+                "sha256:fc3150f85e2dbcf99e65238c842d1cfe69d3e7649b19864c1cc043213d9cd730"
             ],
-            "markers": "python_version >= '3.6'",
-            "version": "==2.0.1"
+            "markers": "python_version >= '3.7'",
+            "version": "==2.1.0"
         },
         "matplotlib": {
             "hashes": [
@@ -1649,29 +1583,29 @@
         },
         "mypy": {
             "hashes": [
-                "sha256:0feb82e9fa849affca7edd24713dbe809dce780ced9f3feca5ed3d80e40b777f",
-                "sha256:1d2296f35aae9802eeb1327058b550371ee382d71374b3e7d2804035ef0b830b",
-                "sha256:1e689e92cdebd87607a041585f1dc7339aa2e8a9f9bad9ba7e6ece619431b20c",
-                "sha256:1ea7199780c1d7940b82dbc0a4e37722b4e3851264dbba81e01abecc9052d8a7",
-                "sha256:221cc94dc6a801ccc2be7c0c9fd791c5e08d1fa2c5e1c12dec4eab15b2469871",
-                "sha256:2e9c5409e9cb81049bb03fa1009b573dea87976713e3898561567a86c4eaee01",
-                "sha256:45a4dc21c789cfd09b8ccafe114d6de66f0b341ad761338de717192f19397a8c",
-                "sha256:51426262ae4714cc7dd5439814676e0992b55bcc0f6514eccb4cf8e0678962c2",
-                "sha256:554873e45c1ca20f31ddf873deb67fa5d2e87b76b97db50669f0468ccded8fae",
-                "sha256:5feb56f8bb280468fe5fc8e6f56f48f99aa0df9eed3c507a11505ee4657b5380",
-                "sha256:601f46593f627f8a9b944f74fd387c9b5f4266b39abad77471947069c2fc7651",
-                "sha256:70b197dd8c78fc5d2daf84bd093e8466a2b2e007eedaa85e792e513a820adbf7",
-                "sha256:959319b9a3cafc33a8185f440a433ba520239c72e733bf91f9efd67b0a8e9b30",
-                "sha256:a9d8dffefba634b27d650e0de2564379a1a367e2e08d6617d8f89261a3bf63b2",
-                "sha256:b419e9721260161e70d054a15abbd50603c16f159860cfd0daeab647d828fc29",
-                "sha256:bc1a0607ea03c30225347334af66b0af12eefba018a89a88c209e02b7065ea95",
-                "sha256:bf4a44e03040206f7c058d1f5ba02ef2d1820720c88bc4285c7d9a4269f54173",
-                "sha256:db3a87376a1380f396d465bed462e76ea89f838f4c5e967d68ff6ee34b785c31",
-                "sha256:ed4e0ea066bb12f56b2812a15ff223c57c0a44eca817ceb96b214bb055c7051f",
-                "sha256:f9f665d69034b1fcfdbcd4197480d26298bbfb5d2dfe206245b6498addb34999"
-            ],
-            "index": "pypi",
-            "version": "==0.930"
+                "sha256:0038b21890867793581e4cb0d810829f5fd4441aa75796b53033af3aa30430ce",
+                "sha256:1171f2e0859cfff2d366da2c7092b06130f232c636a3f7301e3feb8b41f6377d",
+                "sha256:1b06268df7eb53a8feea99cbfff77a6e2b205e70bf31743e786678ef87ee8069",
+                "sha256:1b65714dc296a7991000b6ee59a35b3f550e0073411ac9d3202f6516621ba66c",
+                "sha256:1bf752559797c897cdd2c65f7b60c2b6969ffe458417b8d947b8340cc9cec08d",
+                "sha256:300717a07ad09525401a508ef5d105e6b56646f7942eb92715a1c8d610149714",
+                "sha256:3c5b42d0815e15518b1f0990cff7a705805961613e701db60387e6fb663fe78a",
+                "sha256:4365c60266b95a3f216a3047f1d8e3f895da6c7402e9e1ddfab96393122cc58d",
+                "sha256:50c7346a46dc76a4ed88f3277d4959de8a2bd0a0fa47fa87a4cde36fe247ac05",
+                "sha256:5b56154f8c09427bae082b32275a21f500b24d93c88d69a5e82f3978018a0266",
+                "sha256:74f7eccbfd436abe9c352ad9fb65872cc0f1f0a868e9d9c44db0893440f0c697",
+                "sha256:7b3f6f557ba4afc7f2ce6d3215d5db279bcf120b3cfd0add20a5d4f4abdae5bc",
+                "sha256:8c11003aaeaf7cc2d0f1bc101c1cc9454ec4cc9cb825aef3cafff8a5fdf4c799",
+                "sha256:8ca7f8c4b1584d63c9a0f827c37ba7a47226c19a23a753d52e5b5eddb201afcd",
+                "sha256:c89702cac5b302f0c5d33b172d2b55b5df2bede3344a2fbed99ff96bddb2cf00",
+                "sha256:d8f1ff62f7a879c9fe5917b3f9eb93a79b78aad47b533911b853a757223f72e7",
+                "sha256:d9d2b84b2007cea426e327d2483238f040c49405a6bf4074f605f0156c91a47a",
+                "sha256:e839191b8da5b4e5d805f940537efcaa13ea5dd98418f06dc585d2891d228cf0",
+                "sha256:f9fe20d0872b26c4bba1c1be02c5340de1019530302cf2dcc85c7f9fc3252ae0",
+                "sha256:ff3bf387c14c805ab1388185dd22d6b210824e164d4bb324b195ff34e322d166"
+            ],
+            "index": "pypi",
+            "version": "==0.931"
         },
         "mypy-extensions": {
             "hashes": [
@@ -1682,11 +1616,11 @@
         },
         "myst-parser": {
             "hashes": [
-                "sha256:617a90ceda2162ebf81cd13ad17d879bd4f49e7fb5c4f177bb905272555a2268",
-                "sha256:a6473b9735c8c74959b49b36550725464f4aecc4481340c9a5f9153829191f83"
+                "sha256:555ec2950aba5ae5dac5c162c7e9a43ad4a7291cfac644d8f5f84da8efa6f356",
+                "sha256:d412347a5cacb77ebc03d7f7ffef050cd61957d46f234313d350e84e24972260"
             ],
             "index": "pypi",
-            "version": "==0.16.1"
+            "version": "==0.17.0"
         },
         "nodeenv": {
             "hashes": [
@@ -1697,31 +1631,28 @@
         },
         "numpy": {
             "hashes": [
-                "sha256:0cfe07133fd00b27edee5e6385e333e9eeb010607e8a46e1cd673f05f8596595",
-                "sha256:11a1f3816ea82eed4178102c56281782690ab5993251fdfd75039aad4d20385f",
-                "sha256:2762331de395739c91f1abb88041f94a080cb1143aeec791b3b223976228af3f",
-                "sha256:283d9de87c0133ef98f93dfc09fad3fb382f2a15580de75c02b5bb36a5a159a5",
-                "sha256:3d22662b4b10112c545c91a0741f2436f8ca979ab3d69d03d19322aa970f9695",
-                "sha256:41388e32e40b41dd56eb37fcaa7488b2b47b0adf77c66154d6b89622c110dfe9",
-                "sha256:42c16cec1c8cf2728f1d539bd55aaa9d6bb48a7de2f41eb944697293ef65a559",
-                "sha256:47ee7a839f5885bc0c63a74aabb91f6f40d7d7b639253768c4199b37aede7982",
-                "sha256:5a311ee4d983c487a0ab546708edbdd759393a3dc9cd30305170149fedd23c88",
-                "sha256:5dc65644f75a4c2970f21394ad8bea1a844104f0fe01f278631be1c7eae27226",
-                "sha256:6ed0d073a9c54ac40c41a9c2d53fcc3d4d4ed607670b9e7b0de1ba13b4cbfe6f",
-                "sha256:76ba7c40e80f9dc815c5e896330700fd6e20814e69da9c1267d65a4d051080f1",
-                "sha256:818b9be7900e8dc23e013a92779135623476f44a0de58b40c32a15368c01d471",
-                "sha256:a024181d7aef0004d76fb3bce2a4c9f2e67a609a9e2a6ff2571d30e9976aa383",
-                "sha256:a955e4128ac36797aaffd49ab44ec74a71c11d6938df83b1285492d277db5397",
-                "sha256:a97a954a8c2f046d3817c2bce16e3c7e9a9c2afffaf0400f5c16df5172a67c9c",
-                "sha256:a97e82c39d9856fe7d4f9b86d8a1e66eff99cf3a8b7ba48202f659703d27c46f",
-                "sha256:b55b953a1bdb465f4dc181758570d321db4ac23005f90ffd2b434cc6609a63dd",
-                "sha256:bb02929b0d6bfab4c48a79bd805bd7419114606947ec8284476167415171f55b",
-                "sha256:bece0a4a49e60e472a6d1f70ac6cdea00f9ab80ff01132f96bd970cdd8a9e5a9",
-                "sha256:e41e8951749c4b5c9a2dc5fdbc1a4eec6ab2a140fdae9b460b0f557eed870f4d",
-                "sha256:f71d57cc8645f14816ae249407d309be250ad8de93ef61d9709b45a0ddf4050c"
-            ],
-            "index": "pypi",
-            "version": "==1.22.0"
+                "sha256:03ae5850619abb34a879d5f2d4bb4dcd025d6d8fb72f5e461dae84edccfe129f",
+                "sha256:076aee5a3763d41da6bef9565fdf3cb987606f567cd8b104aded2b38b7b47abf",
+                "sha256:0b536b6840e84c1c6a410f3a5aa727821e6108f3454d81a5cd5900999ef04f89",
+                "sha256:15efb7b93806d438e3bc590ca8ef2f953b0ce4f86f337ef4559d31ec6cf9d7dd",
+                "sha256:168259b1b184aa83a514f307352c25c56af111c269ffc109d9704e81f72e764b",
+                "sha256:2638389562bda1635b564490d76713695ff497242a83d9b684d27bb4a6cc9d7a",
+                "sha256:3556c5550de40027d3121ebbb170f61bbe19eb639c7ad0c7b482cd9b560cd23b",
+                "sha256:4a176959b6e7e00b5a0d6f549a479f869829bfd8150282c590deee6d099bbb6e",
+                "sha256:515a8b6edbb904594685da6e176ac9fbea8f73a5ebae947281de6613e27f1956",
+                "sha256:55535c7c2f61e2b2fc817c5cbe1af7cb907c7f011e46ae0a52caa4be1f19afe2",
+                "sha256:59153979d60f5bfe9e4c00e401e24dfe0469ef8da6d68247439d3278f30a180f",
+                "sha256:60cb8e5933193a3cc2912ee29ca331e9c15b2da034f76159b7abc520b3d1233a",
+                "sha256:6767ad399e9327bfdbaa40871be4254d1995f4a3ca3806127f10cec778bd9896",
+                "sha256:76a4f9bce0278becc2da7da3b8ef854bed41a991f4226911a24a9711baad672c",
+                "sha256:8cf33634b60c9cef346663a222d9841d3bbbc0a2f00221d6bcfd0d993d5543f6",
+                "sha256:94dd11d9f13ea1be17bac39c1942f527cbf7065f94953cf62dfe805653da2f8f",
+                "sha256:aafa46b5a39a27aca566198d3312fb3bde95ce9677085efd02c86f7ef6be4ec7",
+                "sha256:badca914580eb46385e7f7e4e426fea6de0a37b9e06bec252e481ae7ec287082",
+                "sha256:d76a26c5118c4d96e264acc9e3242d72e1a2b92e739807b3b69d8d47684b6677"
+            ],
+            "index": "pypi",
+            "version": "==1.22.2"
         },
         "opencv-python-headless": {
             "hashes": [
@@ -1754,57 +1685,60 @@
         },
         "paramiko": {
             "hashes": [
-                "sha256:a1fdded3b55f61d23389e4fe52d9ae428960ac958d2edf50373faa5d8926edd0",
-                "sha256:db5d3f19607941b1c90233588d60213c874392c4961c6297037da989c24f8070"
+                "sha256:04097dbd96871691cdb34c13db1883066b8a13a0df2afd4cb0a92221f51c2603",
+                "sha256:944a9e5dbdd413ab6c7951ea46b0ab40713235a9c4c5ca81cfe45c6f14fa677b"
             ],
             "index": "pypi",
-            "version": "==2.9.1"
+            "version": "==2.9.2"
         },
         "pillow": {
             "hashes": [
-                "sha256:03b27b197deb4ee400ed57d8d4e572d2d8d80f825b6634daf6e2c18c3c6ccfa6",
-                "sha256:0b281fcadbb688607ea6ece7649c5d59d4bbd574e90db6cd030e9e85bde9fecc",
-                "sha256:0ebd8b9137630a7bbbff8c4b31e774ff05bbb90f7911d93ea2c9371e41039b52",
-                "sha256:113723312215b25c22df1fdf0e2da7a3b9c357a7d24a93ebbe80bfda4f37a8d4",
-                "sha256:2d16b6196fb7a54aff6b5e3ecd00f7c0bab1b56eee39214b2b223a9d938c50af",
-                "sha256:2fd8053e1f8ff1844419842fd474fc359676b2e2a2b66b11cc59f4fa0a301315",
-                "sha256:31b265496e603985fad54d52d11970383e317d11e18e856971bdbb86af7242a4",
-                "sha256:3586e12d874ce2f1bc875a3ffba98732ebb12e18fb6d97be482bd62b56803281",
-                "sha256:47f5cf60bcb9fbc46011f75c9b45a8b5ad077ca352a78185bd3e7f1d294b98bb",
-                "sha256:490e52e99224858f154975db61c060686df8a6b3f0212a678e5d2e2ce24675c9",
-                "sha256:500d397ddf4bbf2ca42e198399ac13e7841956c72645513e8ddf243b31ad2128",
-                "sha256:52abae4c96b5da630a8b4247de5428f593465291e5b239f3f843a911a3cf0105",
-                "sha256:6579f9ba84a3d4f1807c4aab4be06f373017fc65fff43498885ac50a9b47a553",
-                "sha256:68e06f8b2248f6dc8b899c3e7ecf02c9f413aab622f4d6190df53a78b93d97a5",
-                "sha256:6c5439bfb35a89cac50e81c751317faea647b9a3ec11c039900cd6915831064d",
-                "sha256:72c3110228944019e5f27232296c5923398496b28be42535e3b2dc7297b6e8b6",
-                "sha256:72f649d93d4cc4d8cf79c91ebc25137c358718ad75f99e99e043325ea7d56100",
-                "sha256:7aaf07085c756f6cb1c692ee0d5a86c531703b6e8c9cae581b31b562c16b98ce",
-                "sha256:80fe92813d208ce8aa7d76da878bdc84b90809f79ccbad2a288e9bcbeac1d9bd",
-                "sha256:95545137fc56ce8c10de646074d242001a112a92de169986abd8c88c27566a05",
-                "sha256:97b6d21771da41497b81652d44191489296555b761684f82b7b544c49989110f",
-                "sha256:98cb63ca63cb61f594511c06218ab4394bf80388b3d66cd61d0b1f63ee0ea69f",
-                "sha256:9f3b4522148586d35e78313db4db0df4b759ddd7649ef70002b6c3767d0fdeb7",
-                "sha256:a09a9d4ec2b7887f7a088bbaacfd5c07160e746e3d47ec5e8050ae3b2a229e9f",
-                "sha256:b5050d681bcf5c9f2570b93bee5d3ec8ae4cf23158812f91ed57f7126df91762",
-                "sha256:bb47a548cea95b86494a26c89d153fd31122ed65255db5dcbc421a2d28eb3379",
-                "sha256:bc462d24500ba707e9cbdef436c16e5c8cbf29908278af053008d9f689f56dee",
-                "sha256:c2067b3bb0781f14059b112c9da5a91c80a600a97915b4f48b37f197895dd925",
-                "sha256:d154ed971a4cc04b93a6d5b47f37948d1f621f25de3e8fa0c26b2d44f24e3e8f",
-                "sha256:d5dcea1387331c905405b09cdbfb34611050cc52c865d71f2362f354faee1e9f",
-                "sha256:ee6e2963e92762923956fe5d3479b1fdc3b76c83f290aad131a2f98c3df0593e",
-                "sha256:fd0e5062f11cb3e730450a7d9f323f4051b532781026395c4323b8ad055523c4"
-            ],
-            "index": "pypi",
-            "version": "==9.0.0"
+                "sha256:011233e0c42a4a7836498e98c1acf5e744c96a67dd5032a6f666cc1fb97eab97",
+                "sha256:0f29d831e2151e0b7b39981756d201f7108d3d215896212ffe2e992d06bfe049",
+                "sha256:12875d118f21cf35604176872447cdb57b07126750a33748bac15e77f90f1f9c",
+                "sha256:14d4b1341ac07ae07eb2cc682f459bec932a380c3b122f5540432d8977e64eae",
+                "sha256:1c3c33ac69cf059bbb9d1a71eeaba76781b450bc307e2291f8a4764d779a6b28",
+                "sha256:1d19397351f73a88904ad1aee421e800fe4bbcd1aeee6435fb62d0a05ccd1030",
+                "sha256:253e8a302a96df6927310a9d44e6103055e8fb96a6822f8b7f514bb7ef77de56",
+                "sha256:2632d0f846b7c7600edf53c48f8f9f1e13e62f66a6dbc15191029d950bfed976",
+                "sha256:335ace1a22325395c4ea88e00ba3dc89ca029bd66bd5a3c382d53e44f0ccd77e",
+                "sha256:413ce0bbf9fc6278b2d63309dfeefe452835e1c78398efb431bab0672fe9274e",
+                "sha256:5100b45a4638e3c00e4d2320d3193bdabb2d75e79793af7c3eb139e4f569f16f",
+                "sha256:514ceac913076feefbeaf89771fd6febde78b0c4c1b23aaeab082c41c694e81b",
+                "sha256:528a2a692c65dd5cafc130de286030af251d2ee0483a5bf50c9348aefe834e8a",
+                "sha256:6295f6763749b89c994fcb6d8a7f7ce03c3992e695f89f00b741b4580b199b7e",
+                "sha256:6c8bc8238a7dfdaf7a75f5ec5a663f4173f8c367e5a39f87e720495e1eed75fa",
+                "sha256:718856856ba31f14f13ba885ff13874be7fefc53984d2832458f12c38205f7f7",
+                "sha256:7f7609a718b177bf171ac93cea9fd2ddc0e03e84d8fa4e887bdfc39671d46b00",
+                "sha256:80ca33961ced9c63358056bd08403ff866512038883e74f3a4bf88ad3eb66838",
+                "sha256:80fe64a6deb6fcfdf7b8386f2cf216d329be6f2781f7d90304351811fb591360",
+                "sha256:81c4b81611e3a3cb30e59b0cf05b888c675f97e3adb2c8672c3154047980726b",
+                "sha256:855c583f268edde09474b081e3ddcd5cf3b20c12f26e0d434e1386cc5d318e7a",
+                "sha256:9bfdb82cdfeccec50aad441afc332faf8606dfa5e8efd18a6692b5d6e79f00fd",
+                "sha256:a5d24e1d674dd9d72c66ad3ea9131322819ff86250b30dc5821cbafcfa0b96b4",
+                "sha256:a9f44cd7e162ac6191491d7249cceb02b8116b0f7e847ee33f739d7cb1ea1f70",
+                "sha256:b5b3f092fe345c03bca1e0b687dfbb39364b21ebb8ba90e3fa707374b7915204",
+                "sha256:b9618823bd237c0d2575283f2939655f54d51b4527ec3972907a927acbcc5bfc",
+                "sha256:cef9c85ccbe9bee00909758936ea841ef12035296c748aaceee535969e27d31b",
+                "sha256:d21237d0cd37acded35154e29aec853e945950321dd2ffd1a7d86fe686814669",
+                "sha256:d3c5c79ab7dfce6d88f1ba639b77e77a17ea33a01b07b99840d6ed08031cb2a7",
+                "sha256:d9d7942b624b04b895cb95af03a23407f17646815495ce4547f0e60e0b06f58e",
+                "sha256:db6d9fac65bd08cea7f3540b899977c6dee9edad959fa4eaf305940d9cbd861c",
+                "sha256:ede5af4a2702444a832a800b8eb7f0a7a1c0eed55b644642e049c98d589e5092",
+                "sha256:effb7749713d5317478bb3acb3f81d9d7c7f86726d41c1facca068a04cf5bb4c",
+                "sha256:f154d173286a5d1863637a7dcd8c3437bb557520b01bddb0be0258dcb72696b5",
+                "sha256:f25ed6e28ddf50de7e7ea99d7a976d6a9c415f03adcaac9c41ff6ff41b6d86ac"
+            ],
+            "index": "pypi",
+            "version": "==9.0.1"
         },
         "platformdirs": {
             "hashes": [
-                "sha256:1d7385c7db91728b83efd0ca99a5afb296cab9d0ed8313a45ed8ba17967ecfca",
-                "sha256:440633ddfebcc36264232365d7840a970e75e1018d15b4327d11f91909045fda"
+                "sha256:7535e70dfa32e84d4b34996ea99c5e432fa29a708d0f4e394bbcb2a8faa4f16d",
+                "sha256:bcae7cab893c2d310a711b70b24efb93334febe65f8de776ee320b517471e227"
             ],
             "markers": "python_version >= '3.7'",
-            "version": "==2.4.1"
+            "version": "==2.5.1"
         },
         "pluggy": {
             "hashes": [
@@ -1823,11 +1757,11 @@
         },
         "pre-commit": {
             "hashes": [
-                "sha256:758d1dc9b62c2ed8881585c254976d66eae0889919ab9b859064fc2fe3c7743e",
-                "sha256:fe9897cac830aa7164dbd02a4e7b90cae49630451ce88464bca73db486ba9f65"
+                "sha256:725fa7459782d7bec5ead072810e47351de01709be838c2ce1726b9591dad616",
+                "sha256:c1a8040ff15ad3d648c70cc3e55b93e4d2d5b687320955505587fd79bbaed06a"
             ],
             "index": "pypi",
-            "version": "==2.16.0"
+            "version": "==2.17.0"
         },
         "py": {
             "hashes": [
@@ -1917,43 +1851,35 @@
         },
         "pygments": {
             "hashes": [
-                "sha256:59b895e326f0fb0d733fd28c6839bd18ad0687ba20efc26d4277fd1d30b971f4",
-                "sha256:9135c1af61eec0f650cd1ea1ed8ce298e54d56bcd8cc2ef46edd7702c171337c"
+                "sha256:44238f1b60a76d78fc8ca0528ee429702aae011c265fe6a8dd8b63049ae41c65",
+                "sha256:4e426f72023d88d03b2fa258de560726ce890ff3b630f88c21cbb8b2503b8c6a"
             ],
             "markers": "python_version >= '3.5'",
-            "version": "==2.11.1"
+            "version": "==2.11.2"
         },
         "pynacl": {
             "hashes": [
-                "sha256:06cbb4d9b2c4bd3c8dc0d267416aaed79906e7b33f114ddbf0911969794b1cc4",
-                "sha256:11335f09060af52c97137d4ac54285bcb7df0cef29014a1a4efe64ac065434c4",
-                "sha256:2fe0fc5a2480361dcaf4e6e7cea00e078fcda07ba45f811b167e3f99e8cff574",
-                "sha256:30f9b96db44e09b3304f9ea95079b1b7316b2b4f3744fe3aaecccd95d547063d",
-                "sha256:4e10569f8cbed81cb7526ae137049759d2a8d57726d52c1a000a3ce366779634",
-                "sha256:511d269ee845037b95c9781aa702f90ccc36036f95d0f31373a6a79bd8242e25",
-                "sha256:537a7ccbea22905a0ab36ea58577b39d1fa9b1884869d173b5cf111f006f689f",
-                "sha256:54e9a2c849c742006516ad56a88f5c74bf2ce92c9f67435187c3c5953b346505",
-                "sha256:757250ddb3bff1eecd7e41e65f7f833a8405fede0194319f87899690624f2122",
-                "sha256:7757ae33dae81c300487591c68790dfb5145c7d03324000433d9a2c141f82af7",
-                "sha256:7c6092102219f59ff29788860ccb021e80fffd953920c4a8653889c029b2d420",
-                "sha256:8122ba5f2a2169ca5da936b2e5a511740ffb73979381b4229d9188f6dcb22f1f",
-                "sha256:9c4a7ea4fb81536c1b1f5cc44d54a296f96ae78c1ebd2311bd0b60be45a48d96",
-                "sha256:c914f78da4953b33d4685e3cdc7ce63401247a21425c16a39760e282075ac4a6",
-                "sha256:cd401ccbc2a249a47a3a1724c2918fcd04be1f7b54eb2a5a71ff915db0ac51c6",
-                "sha256:d452a6746f0a7e11121e64625109bc4468fc3100452817001dbe018bb8b08514",
-                "sha256:ea6841bc3a76fa4942ce00f3bda7d436fda21e2d91602b9e21b7ca9ecab8f3ff",
-                "sha256:f8851ab9041756003119368c1e6cd0b9c631f46d686b3904b18c0139f4419f80"
+                "sha256:06b8f6fa7f5de8d5d2f7573fe8c863c051225a27b61e6860fd047b1775807858",
+                "sha256:0c84947a22519e013607c9be43706dd42513f9e6ae5d39d3613ca1e142fba44d",
+                "sha256:20f42270d27e1b6a29f54032090b972d97f0a1b0948cc52392041ef7831fee93",
+                "sha256:401002a4aaa07c9414132aaed7f6836ff98f59277a234704ff66878c2ee4a0d1",
+                "sha256:52cb72a79269189d4e0dc537556f4740f7f0a9ec41c1322598799b0bdad4ef92",
+                "sha256:61f642bf2378713e2c2e1de73444a3778e5f0a38be6fee0fe532fe30060282ff",
+                "sha256:8ac7448f09ab85811607bdd21ec2464495ac8b7c66d146bf545b0f08fb9220ba",
+                "sha256:a36d4a9dda1f19ce6e03c9a784a2921a4b726b02e1c736600ca9c22029474394",
+                "sha256:a422368fc821589c228f4c49438a368831cb5bbc0eab5ebe1d7fac9dded6567b",
+                "sha256:e46dae94e34b085175f8abb3b0aaa7da40767865ac82c928eeb9e57e1ea8a543"
             ],
-            "markers": "python_version >= '2.7' and python_version not in '3.0, 3.1, 3.2, 3.3'",
-            "version": "==1.4.0"
+            "markers": "python_version >= '3.6'",
+            "version": "==1.5.0"
         },
         "pyparsing": {
             "hashes": [
-                "sha256:04ff808a5b90911829c55c4e26f75fa5ca8a2f5f36aa3a51f68e27033341d3e4",
-                "sha256:d9bdec0013ef1eb5a84ab39a3b3868911598afa494f5faa038647101504e2b81"
+                "sha256:18ee9022775d270c55187733956460083db60b37d0d0fb357445f3094eed3eea",
+                "sha256:a6c06a88f252e6c322f65faf8f418b16213b51bdfaece0524c1c1bc30c63c484"
             ],
             "markers": "python_version >= '3.6'",
-            "version": "==3.0.6"
+            "version": "==3.0.7"
         },
         "pyprof2calltree": {
             "hashes": [
@@ -1964,11 +1890,11 @@
         },
         "pytest": {
             "hashes": [
-                "sha256:131b36680866a76e6781d13f101efb86cf674ebb9762eb70d3082b6f29889e89",
-                "sha256:7310f8d27bc79ced999e760ca304d69f6ba6c6649c0b60fb0e04a4a77cacc134"
+                "sha256:9ce3ff477af913ecf6321fe337b93a2c0dcf2a0a1439c43f5452112c1e4280db",
+                "sha256:e30905a0c131d3d94b89624a1cc5afec3e0ba2fbdb151867d8e0ebd49850f171"
             ],
             "index": "pypi",
-            "version": "==6.2.5"
+            "version": "==7.0.1"
         },
         "pytest-forked": {
             "hashes": [
@@ -2057,46 +1983,32 @@
         },
         "scipy": {
             "hashes": [
-                "sha256:033ce76ed4e9f62923e1f8124f7e2b0800db533828c853b402c7eec6e9465d80",
-                "sha256:173308efba2270dcd61cd45a30dfded6ec0085b4b6eb33b5eb11ab443005e088",
-                "sha256:21b66200cf44b1c3e86495e3a436fc7a26608f92b8d43d344457c54f1c024cbc",
-                "sha256:2c56b820d304dffcadbbb6cbfbc2e2c79ee46ea291db17e288e73cd3c64fefa9",
-                "sha256:304dfaa7146cffdb75fbf6bb7c190fd7688795389ad060b970269c8576d038e9",
-                "sha256:3f78181a153fa21c018d346f595edd648344751d7f03ab94b398be2ad083ed3e",
-                "sha256:4d242d13206ca4302d83d8a6388c9dfce49fc48fdd3c20efad89ba12f785bf9e",
-                "sha256:5d1cc2c19afe3b5a546ede7e6a44ce1ff52e443d12b231823268019f608b9b12",
-                "sha256:5f2cfc359379c56b3a41b17ebd024109b2049f878badc1e454f31418c3a18436",
-                "sha256:65bd52bf55f9a1071398557394203d881384d27b9c2cad7df9a027170aeaef93",
-                "sha256:7edd9a311299a61e9919ea4192dd477395b50c014cdc1a1ac572d7c27e2207fa",
-                "sha256:8499d9dd1459dc0d0fe68db0832c3d5fc1361ae8e13d05e6849b358dc3f2c279",
-                "sha256:866ada14a95b083dd727a845a764cf95dd13ba3dc69a16b99038001b05439709",
-                "sha256:87069cf875f0262a6e3187ab0f419f5b4280d3dcf4811ef9613c605f6e4dca95",
-                "sha256:93378f3d14fff07572392ce6a6a2ceb3a1f237733bd6dcb9eb6a2b29b0d19085",
-                "sha256:95c2d250074cfa76715d58830579c64dff7354484b284c2b8b87e5a38321672c",
-                "sha256:ab5875facfdef77e0a47d5fd39ea178b58e60e454a4c85aa1e52fcb80db7babf",
-                "sha256:b0e0aeb061a1d7dcd2ed59ea57ee56c9b23dd60100825f98238c06ee5cc4467e",
-                "sha256:b78a35c5c74d336f42f44106174b9851c783184a85a3fe3e68857259b37b9ffb",
-                "sha256:c9e04d7e9b03a8a6ac2045f7c5ef741be86727d8f49c45db45f244bdd2bcff17",
-                "sha256:ca36e7d9430f7481fc7d11e015ae16fbd5575615a8e9060538104778be84addf",
-                "sha256:ceebc3c4f6a109777c0053dfa0282fddb8893eddfb0d598574acfb734a926168",
-                "sha256:e2c036492e673aad1b7b0d0ccdc0cb30a968353d2c4bf92ac8e73509e1bf212c",
-                "sha256:eb326658f9b73c07081300daba90a8746543b5ea177184daed26528273157294",
-                "sha256:eb7ae2c4dbdb3c9247e07acc532f91077ae6dbc40ad5bd5dca0bb5a176ee9bda",
-                "sha256:edad1cf5b2ce1912c4d8ddad20e11d333165552aba262c882e28c78bbc09dbf6",
-                "sha256:eef93a446114ac0193a7b714ce67659db80caf940f3232bad63f4c7a81bc18df",
-                "sha256:f7eaea089345a35130bc9a39b89ec1ff69c208efa97b3f8b25ea5d4c41d88094",
-                "sha256:f99d206db1f1ae735a8192ab93bd6028f3a42f6fa08467d37a14eb96c9dd34a3"
-            ],
-            "index": "pypi",
-            "version": "==1.7.3"
-        },
-        "setuptools": {
-            "hashes": [
-                "sha256:5c89b1a14a67ac5f0956f1cb0aeb7d1d3f4c8ba4e4e1ab7bf1af4933f9a2f0fe",
-                "sha256:675fcebecb43c32eb930481abf907619137547f4336206e4d673180242e1a278"
-            ],
-            "markers": "python_version >= '3.7'",
-            "version": "==60.2.0"
+                "sha256:011d4386b53b933142f58a652aa0f149c9b9242abd4f900b9f4ea5fbafc86b89",
+                "sha256:16e09ef68b352d73befa8bcaf3ebe25d3941fe1a58c82909d5589856e6bc8174",
+                "sha256:31d4f2d6b724bc9a98e527b5849b8a7e589bf1ea630c33aa563eda912c9ff0bd",
+                "sha256:38aa39b6724cb65271e469013aeb6f2ce66fd44f093e241c28a9c6bc64fd79ed",
+                "sha256:3d573228c10a3a8c32b9037be982e6440e411b443a6267b067cac72f690b8d56",
+                "sha256:3d9dd6c8b93a22bf9a3a52d1327aca7e092b1299fb3afc4f89e8eba381be7b59",
+                "sha256:559a8a4c03a5ba9fe3232f39ed24f86457e4f3f6c0abbeae1fb945029f092720",
+                "sha256:5e73343c5e0d413c1f937302b2e04fb07872f5843041bcfd50699aef6e95e399",
+                "sha256:723b9f878095ed994756fa4ee3060c450e2db0139c5ba248ee3f9628bd64e735",
+                "sha256:87b01c7d5761e8a266a0fbdb9d88dcba0910d63c1c671bdb4d99d29f469e9e03",
+                "sha256:8f4d059a97b29c91afad46b1737274cb282357a305a80bdd9e8adf3b0ca6a3f0",
+                "sha256:92b2c2af4183ed09afb595709a8ef5783b2baf7f41e26ece24e1329c109691a7",
+                "sha256:937d28722f13302febde29847bbe554b89073fbb924a30475e5ed7b028898b5f",
+                "sha256:a279e27c7f4566ef18bab1b1e2c37d168e365080974758d107e7d237d3f0f484",
+                "sha256:ad5be4039147c808e64f99c0e8a9641eb5d2fa079ff5894dcd8240e94e347af4",
+                "sha256:ae3e327da323d82e918e593460e23babdce40d7ab21490ddf9fc06dec6b91a18",
+                "sha256:bb7088e89cd751acf66195d2f00cf009a1ea113f3019664032d9075b1e727b6c",
+                "sha256:c17a1878d00a5dd2797ccd73623ceca9d02375328f6218ee6d921e1325e61aff",
+                "sha256:c2bae431d127bf0b1da81fc24e4bba0a84d058e3a96b9dd6475dfcb3c5e8761e",
+                "sha256:de2e80ee1d925984c2504812a310841c241791c5279352be4707cdcd7c255039",
+                "sha256:e6f0cd9c0bd374ef834ee1e0f0999678d49dcc400ea6209113d81528958f97c7",
+                "sha256:f3720d0124aced49f6f2198a6900304411dbbeed12f56951d7c66ebef05e3df6",
+                "sha256:f4a6d3b9f9797eb2d43938ac2c5d96d02aed17ef170c8b38f11798717523ddba"
+            ],
+            "index": "pypi",
+            "version": "==1.8.0"
         },
         "six": {
             "hashes": [
@@ -2122,11 +2034,11 @@
         },
         "sphinx": {
             "hashes": [
-                "sha256:0a8836751a68306b3fe97ecbe44db786f8479c3bf4b80e3a7f5c838657b4698c",
-                "sha256:6a11ea5dd0bdb197f9c2abc2e0ce73e01340464feaece525e64036546d24c851"
+                "sha256:5da895959511473857b6d0200f56865ed62c31e8f82dd338063b84ec022701fe",
+                "sha256:6caad9786055cb1fa22b4a365c1775816b876f91966481765d7d50e9f0dd35cc"
             ],
             "index": "pypi",
-            "version": "==4.3.2"
+            "version": "==4.4.0"
         },
         "sphinx-rtd-theme": {
             "hashes": [
@@ -2218,35 +2130,43 @@
         },
         "tomli": {
             "hashes": [
-                "sha256:b5bde28da1fed24b9bd1d4d2b8cba62300bfb4ec9a6187a957e8ddb9434c5224",
-                "sha256:c292c34f58502a1eb2bbb9f5bbc9a5ebc37bee10ffb8c2d6bbdfa8eb13cc14e1"
+                "sha256:939de3e7a6161af0c887ef91b7d41a53e7c5a1ca976325f429cb46ea9bc30ecc",
+                "sha256:de526c12914f0c550d15924c62d72abc48d6fe7364aa87328337a31007fe8a4f"
             ],
             "markers": "python_version >= '3.7'",
-            "version": "==2.0.0"
+            "version": "==2.0.1"
         },
         "typing-extensions": {
             "hashes": [
-                "sha256:4ca091dea149f945ec56afb48dae714f21e8692ef22a395223bcd328961b6a0e",
-                "sha256:7f001e5ac290a0c0401508864c7ec868be4e701886d5b573a9528ed3973d9d3b"
+                "sha256:1a9462dcc3347a79b1f1c0271fbe79e844580bb598bafa1ed208b94da3cdcd42",
+                "sha256:21c85e0fe4b9a155d0799430b0ad741cdce7e359660ccbd8b530613e8df88ce2"
             ],
-            "markers": "python_version < '3.10'",
-            "version": "==4.0.1"
+            "markers": "python_version >= '3.6'",
+            "version": "==4.1.1"
         },
         "urllib3": {
             "hashes": [
-                "sha256:4987c65554f7a2dbf30c18fd48778ef124af6fab771a377103da0585e2336ece",
-                "sha256:c4fdf4019605b6e5423637e01bc9fe4daef873709a7973e195ceba0a62bbc844"
+                "sha256:000ca7f471a233c2251c6c7023ee85305721bfdf18621ebff4fd17a8653427ed",
+                "sha256:0e7c33d9a63e7ddfcb86780aac87befc2fbddf46c58dbb487e0855f7ceec283c"
             ],
             "index": "pypi",
-            "version": "==1.26.7"
+            "version": "==1.26.8"
         },
         "virtualenv": {
             "hashes": [
-                "sha256:339f16c4a86b44240ba7223d0f93a7887c3ca04b5f9c8129da7958447d079b09",
-                "sha256:d8458cf8d59d0ea495ad9b34c2599487f8a7772d796f9910858376d1600dd2dd"
+                "sha256:01f5f80744d24a3743ce61858123488e91cb2dd1d3bdf92adaf1bba39ffdedf0",
+                "sha256:e7b34c9474e6476ee208c43a4d9ac1510b041c68347eabfe9a9ea0c86aa0a46b"
             ],
             "markers": "python_version >= '2.7' and python_version not in '3.0, 3.1, 3.2, 3.3, 3.4'",
-            "version": "==20.13.0"
+            "version": "==20.13.2"
+        },
+        "zipp": {
+            "hashes": [
+                "sha256:9f50f446828eb9d45b267433fd3e9da8d801f614129124863f9c51ebceafb87d",
+                "sha256:b47250dd24f92b7dd6a0a8fc5244da14608f3ca90a5efcd37a3b1642fac9a375"
+            ],
+            "markers": "python_version >= '3.7'",
+            "version": "==3.7.0"
         }
     }
 }
diff --git a/SConstruct b/SConstruct
index ea9afc68e2..6b520bee41 100644
--- a/SConstruct
+++ b/SConstruct
@@ -89,7 +89,6 @@ if arch == "aarch64" or arch == "larch64":
     "/usr/local/lib",
     "/usr/lib",
     "/system/vendor/lib64",
-    "/system/comma/usr/lib",
     f"#third_party/acados/{arch}/lib",
   ]
 
@@ -172,8 +171,8 @@ if arch != "Darwin":
   ldflags += ["-Wl,--as-needed", "-Wl,--no-undefined"]
 
 # Enable swaglog include in submodules
-cflags += ["-DSWAGLOG"]
-cxxflags += ["-DSWAGLOG"]
+cflags += ['-DSWAGLOG="\\"selfdrive/common/swaglog.h\\""']
+cxxflags += ['-DSWAGLOG="\\"selfdrive/common/swaglog.h\\""']
 
 env = Environment(
   ENV=lenv,
@@ -306,11 +305,11 @@ if arch == "Darwin":
   qt_env["FRAMEWORKS"] += [f"Qt{m}" for m in qt_modules] + ["OpenGL"]
   qt_env.AppendENVPath('PATH', os.path.join(qt_env['QTDIR'], "bin"))
 elif arch == "aarch64":
-  qt_env['QTDIR'] = "/system/comma/usr"
+  qt_env['QTDIR'] = "/usr"
   qt_dirs = [
-    f"/system/comma/usr/include/qt",
+    f"/usr/include/qt",
   ]
-  qt_dirs += [f"/system/comma/usr/include/qt/Qt{m}" for m in qt_modules]
+  qt_dirs += [f"/usr/include/qt/Qt{m}" for m in qt_modules]
 
   qt_libs = [f"Qt5{m}" for m in qt_modules]
   qt_libs += ['EGL', 'GLESv3', 'c++_shared']
diff --git a/cereal b/cereal
index 28d458a9af..ad2fe885da 160000
--- a/cereal
+++ b/cereal
@@ -1 +1 @@
-Subproject commit 28d458a9af49b38bd0a9052f09fbe927324320fb
+Subproject commit ad2fe885dab99896908b88e765a5f720bfd79b3b
diff --git a/common/conversions.py b/common/conversions.py
new file mode 100644
index 0000000000..b02b33c625
--- /dev/null
+++ b/common/conversions.py
@@ -0,0 +1,19 @@
+import numpy as np
+
+class Conversions:
+  # Speed
+  MPH_TO_KPH = 1.609344
+  KPH_TO_MPH = 1. / MPH_TO_KPH
+  MS_TO_KPH = 3.6
+  KPH_TO_MS = 1. / MS_TO_KPH
+  MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH
+  MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS
+  MS_TO_KNOTS = 1.9438
+  KNOTS_TO_MS = 1. / MS_TO_KNOTS
+
+  # Angle
+  DEG_TO_RAD = np.pi / 180.
+  RAD_TO_DEG = 1. / DEG_TO_RAD
+
+  # Mass
+  LB_TO_KG = 0.453592
diff --git a/docs/CARS.md b/docs/CARS.md
index c32b31dbf2..c7ab9bbfde 100644
--- a/docs/CARS.md
+++ b/docs/CARS.md
@@ -44,9 +44,10 @@
 | Lexus     | RX Hybrid 2020-21             | All               | openpilot        | 0mph               | 0mph              |
 | Lexus     | UX Hybrid 2019-21             | All               | openpilot        | 0mph               | 0mph              |
 | Toyota    | Alphard 2019-20               | All               | openpilot        | 0mph               | 0mph              |
-| Toyota    | Avalon 2016-21                | TSS-P             | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
+| Toyota    | Avalon 2016-18                | TSS-P             | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
+| Toyota    | Avalon 2019-21                | TSS-P             | Stock<sup>3</sup>| 0mph               | 0mph              |
 | Toyota    | Avalon 2022                   | All               | openpilot        | 0mph               | 0mph              |
-| Toyota    | Avalon Hybrid 2019-21         | TSS-P             | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
+| Toyota    | Avalon Hybrid 2019-21         | TSS-P             | Stock<sup>3</sup>| 0mph               | 0mph              |
 | Toyota    | Camry 2018-20                 | All               | Stock            | 0mph<sup>4</sup>   | 0mph              |
 | Toyota    | Camry 2021-22                 | All               | openpilot        | 0mph<sup>4</sup>   | 0mph              |
 | Toyota    | Camry Hybrid 2018-20          | All               | Stock            | 0mph<sup>4</sup>   | 0mph              |
@@ -86,7 +87,7 @@
 | Audi      | A3 Sportback e-tron 2017-18     | ACC + Lane Assist | Stock            | 0mph               | 0mph         |
 | Audi      | Q2 2018                         | ACC + Lane Assist | Stock            | 0mph               | 0mph         |
 | Audi      | Q3 2020-21                      | ACC + Lane Assist | Stock            | 0mph               | 0mph         |
-| Audi      | S3 2015                         | ACC + Lane Assist | Stock            | 0mph               | 0mph         |
+| Audi      | S3 2015-17                      | ACC + Lane Assist | Stock            | 0mph               | 0mph         |
 | Cadillac  | Escalade ESV 2016<sup>1</sup>   | ACC + LKAS        | openpilot        | 0mph               | 7mph         |
 | Chevrolet | Volt 2017-18<sup>1</sup>        | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
 | Chrysler  | Pacifica 2017-18                | Adaptive Cruise   | Stock            | 0mph               | 9mph         |
@@ -123,7 +124,7 @@
 | Kia       | Forte 2018-21                   | SCC + LKAS        | Stock            | 0mph               | 0mph         |
 | Kia       | K5 2021-22                      | SCC + LFA         | Stock            | 0mph               | 0mph         |
 | Kia       | Niro EV 2019-22                 | All               | Stock            | 0mph               | 0mph         |
-| Kia       | Niro Hybrid 2021                | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Kia       | Niro Hybrid 2021-22             | SCC + LKAS        | Stock            | 0mph               | 0mph         |
 | Kia       | Niro PHEV 2019                  | SCC + LKAS        | Stock            | 10mph              | 32mph        |
 | Kia       | Optima 2017                     | SCC + LKAS        | Stock            | 0mph               | 32mph        |
 | Kia       | Optima 2019                     | SCC + LKAS        | Stock            | 0mph               | 0mph         |
@@ -169,7 +170,7 @@
 | Volkswagen| T-Cross 2021<sup>4</sup>        | Driver Assistance | Stock            | 0mph               | 0mph         |
 | Volkswagen| T-Roc 2021<sup>4</sup>          | Driver Assistance | Stock            | 0mph               | 0mph         |
 | Volkswagen| Taos 2022<sup>4</sup>           | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Volkswagen| Tiguan 2020                     | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| Tiguan 2020-22<sup>4</sup>      | Driver Assistance | Stock            | 0mph               | 0mph         |
 | Volkswagen| Touran 2017                     | Driver Assistance | Stock            | 0mph               | 0mph         |
 
 <sup>1</sup>Requires an [OBD-II car harness](https://comma.ai/shop/products/comma-car-harness) and [community built ASCM harness](https://github.com/commaai/openpilot/wiki/GM#hardware). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).*** <br />
diff --git a/laika_repo b/laika_repo
index dec99a0f77..94066cb2b4 160000
--- a/laika_repo
+++ b/laika_repo
@@ -1 +1 @@
-Subproject commit dec99a0f77328f7a9f104020d98d7227345d1288
+Subproject commit 94066cb2b4ad5f2bcb8e33ce02fe15a73a00aace
diff --git a/models/big_supercombo.dlc b/models/big_supercombo.dlc
deleted file mode 100644
index a27e3d1180..0000000000
--- a/models/big_supercombo.dlc
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:ba3fe3e61853cc1434e3e220f40c8e9d1f1b9bab8458196ba3bea6a10b82c6ed
-size 72718099
diff --git a/models/big_supercombo.onnx b/models/big_supercombo.onnx
deleted file mode 100644
index 3039035fbc..0000000000
--- a/models/big_supercombo.onnx
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:bda57c1a66944f5a633ecd739a24d62702c717a234f2fdcc499dfa1d61c3c19e
-size 73147489
diff --git a/models/supercombo.dlc b/models/supercombo.dlc
index 2ebf4fa828..a27e3d1180 100644
--- a/models/supercombo.dlc
+++ b/models/supercombo.dlc
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:209e9544e456dbc2a7d60490da65154e129bc84830909d8d931f97b3df93949b
-size 56684955
+oid sha256:ba3fe3e61853cc1434e3e220f40c8e9d1f1b9bab8458196ba3bea6a10b82c6ed
+size 72718099
diff --git a/models/supercombo.onnx b/models/supercombo.onnx
index 17d233dad7..3039035fbc 100644
--- a/models/supercombo.onnx
+++ b/models/supercombo.onnx
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:2365bae967cce21ce68707c30bf2981bb7081ee5c3e6a3dff793e660f23ff622
-size 57554657
+oid sha256:bda57c1a66944f5a633ecd739a24d62702c717a234f2fdcc499dfa1d61c3c19e
+size 73147489
diff --git a/opendbc b/opendbc
index 46a942d679..eab58f6f8f 160000
--- a/opendbc
+++ b/opendbc
@@ -1 +1 @@
-Subproject commit 46a942d6790531cf5b94b14266140e43afcfda3e
+Subproject commit eab58f6f8f41f255920365ab1fd9c75e312a6869
diff --git a/panda b/panda
index 51ccb9fbd2..5a7af82f06 160000
--- a/panda
+++ b/panda
@@ -1 +1 @@
-Subproject commit 51ccb9fbd266796e1bf6ffda8b93c4119ab09ff4
+Subproject commit 5a7af82f06bf5f8039df8f58f9b1114f7d3436ee
diff --git a/pyextra/acados_template/acados_layout.json b/pyextra/acados_template/acados_layout.json
index 16fd452337..59aa13aa87 100644
--- a/pyextra/acados_template/acados_layout.json
+++ b/pyextra/acados_template/acados_layout.json
@@ -5,6 +5,9 @@
     "acados_include_path": [
         "str"
     ],
+    "cython_include_dirs": [
+        "str"
+    ],
     "model": {
          "name" : [
             "str"
@@ -23,7 +26,15 @@
         ],
         "dyn_disc_fun" : [
             "str"
-        ]
+        ],
+        "gnsf" : {
+            "nontrivial_f_LO": [
+                "int"
+            ],
+            "purely_linear": [
+                "int"
+            ]
+        }
     },
     "parameter_values": [
         "ndarray",
@@ -693,6 +704,18 @@
         "alpha_reduction": [
             "float"
         ],
+        "line_search_use_sufficient_descent": [
+            "int"
+        ],
+        "globalization_use_SOC": [
+            "int"
+        ],
+        "full_step_dual": [
+            "int"
+        ],
+        "eps_sufficient_descent": [
+            "float"
+        ],
         "sim_method_num_stages": [
             "ndarray",
             [
diff --git a/pyextra/acados_template/acados_model.py b/pyextra/acados_template/acados_model.py
index 4871a2c0d5..e292cc7477 100644
--- a/pyextra/acados_template/acados_model.py
+++ b/pyextra/acados_template/acados_model.py
@@ -78,6 +78,13 @@ class AcadosModel():
         self.dyn_disc_fun_jac = None  #: name of function discrete dyanamics + jacobian; Default: :code:`None`
         self.dyn_disc_fun = None  #: name of function discrete dyanamics; Default: :code:`None`
 
+        # for GNSF models
+        self.gnsf = {'nontrivial_f_LO': 1, 'purely_linear': 0}
+        """
+        dictionary containing information on GNSF structure needed when rendering templates.
+        Contains integers `nontrivial_f_LO`, `purely_linear`.
+        """
+
         ## for OCP
         # constraints
         self.con_h_expr = None  #: CasADi expression for the constraint :math:`h`; Default: :code:`None`
diff --git a/pyextra/acados_template/acados_ocp.py b/pyextra/acados_template/acados_ocp.py
index 198ab033dc..8ea129936f 100644
--- a/pyextra/acados_template/acados_ocp.py
+++ b/pyextra/acados_template/acados_ocp.py
@@ -270,28 +270,28 @@ class AcadosOcpDims:
 
     @nx.setter
     def nx(self, nx):
-        if type(nx) == int and nx > 0:
+        if isinstance(nx, int) and nx > 0:
             self.__nx = nx
         else:
             raise Exception('Invalid nx value, expected positive integer. Exiting.')
 
     @nz.setter
     def nz(self, nz):
-        if type(nz) == int and nz > -1:
+        if isinstance(nz, int) and nz > -1:
             self.__nz = nz
         else:
             raise Exception('Invalid nz value, expected nonnegative integer. Exiting.')
 
     @nu.setter
     def nu(self, nu):
-        if type(nu) == int and nu > -1:
+        if isinstance(nu, int) and nu > -1:
             self.__nu = nu
         else:
             raise Exception('Invalid nu value, expected nonnegative integer. Exiting.')
 
     @np.setter
     def np(self, np):
-        if type(np) == int and np > -1:
+        if isinstance(np, int) and np > -1:
             self.__np = np
         else:
             raise Exception('Invalid np value, expected nonnegative integer. Exiting.')
@@ -312,49 +312,49 @@ class AcadosOcpDims:
 
     @ny_e.setter
     def ny_e(self, ny_e):
-        if type(ny_e) == int and ny_e > -1:
+        if isinstance(ny_e, int) and ny_e > -1:
             self.__ny_e = ny_e
         else:
             raise Exception('Invalid ny_e value, expected nonnegative integer. Exiting.')
 
     @nr.setter
     def nr(self, nr):
-        if type(nr) == int and nr > -1:
+        if isinstance(nr, int) and nr > -1:
             self.__nr = nr
         else:
             raise Exception('Invalid nr value, expected nonnegative integer. Exiting.')
 
     @nr_e.setter
     def nr_e(self, nr_e):
-        if type(nr_e) == int and nr_e > -1:
+        if isinstance(nr_e, int) and nr_e > -1:
             self.__nr_e = nr_e
         else:
             raise Exception('Invalid nr_e value, expected nonnegative integer. Exiting.')
 
     @nh.setter
     def nh(self, nh):
-        if type(nh) == int and nh > -1:
+        if isinstance(nh, int) and nh > -1:
             self.__nh = nh
         else:
             raise Exception('Invalid nh value, expected nonnegative integer. Exiting.')
 
     @nh_e.setter
     def nh_e(self, nh_e):
-        if type(nh_e) == int and nh_e > -1:
+        if isinstance(nh_e, int) and nh_e > -1:
             self.__nh_e = nh_e
         else:
             raise Exception('Invalid nh_e value, expected nonnegative integer. Exiting.')
 
     @nphi.setter
     def nphi(self, nphi):
-        if type(nphi) == int and nphi > -1:
+        if isinstance(nphi, int) and nphi > -1:
             self.__nphi = nphi
         else:
             raise Exception('Invalid nphi value, expected nonnegative integer. Exiting.')
 
     @nphi_e.setter
     def nphi_e(self, nphi_e):
-        if type(nphi_e) == int and nphi_e > -1:
+        if isinstance(nphi_e, int) and nphi_e > -1:
             self.__nphi_e = nphi_e
         else:
             raise Exception('Invalid nphi_e value, expected nonnegative integer. Exiting.')
@@ -375,42 +375,42 @@ class AcadosOcpDims:
 
     @nbx_0.setter
     def nbx_0(self, nbx_0):
-        if type(nbx_0) == int and nbx_0 > -1:
+        if isinstance(nbx_0, int) and nbx_0 > -1:
             self.__nbx_0 = nbx_0
         else:
             raise Exception('Invalid nbx_0 value, expected nonnegative integer. Exiting.')
 
     @nbx_e.setter
     def nbx_e(self, nbx_e):
-        if type(nbx_e) == int and nbx_e > -1:
+        if isinstance(nbx_e, int) and nbx_e > -1:
             self.__nbx_e = nbx_e
         else:
             raise Exception('Invalid nbx_e value, expected nonnegative integer. Exiting.')
 
     @nbu.setter
     def nbu(self, nbu):
-        if type(nbu) == int and nbu > -1:
+        if isinstance(nbu, int) and nbu > -1:
             self.__nbu = nbu
         else:
             raise Exception('Invalid nbu value, expected nonnegative integer. Exiting.')
 
     @nsbx.setter
     def nsbx(self, nsbx):
-        if type(nsbx) == int and nsbx > -1:
+        if isinstance(nsbx, int) and nsbx > -1:
             self.__nsbx = nsbx
         else:
             raise Exception('Invalid nsbx value, expected nonnegative integer. Exiting.')
 
     @nsbx_e.setter
     def nsbx_e(self, nsbx_e):
-        if type(nsbx_e) == int and nsbx_e > -1:
+        if isinstance(nsbx_e, int) and nsbx_e > -1:
             self.__nsbx_e = nsbx_e
         else:
             raise Exception('Invalid nsbx_e value, expected nonnegative integer. Exiting.')
 
     @nsbu.setter
     def nsbu(self, nsbu):
-        if type(nsbu) == int and nsbu > -1:
+        if isinstance(nsbu, int) and nsbu > -1:
             self.__nsbu = nsbu
         else:
             raise Exception('Invalid nsbu value, expected nonnegative integer. Exiting.')
@@ -1592,14 +1592,14 @@ class AcadosOcpConstraints:
     # initial x
     @lbx_0.setter
     def lbx_0(self, lbx_0):
-        if type(lbx_0) == np.ndarray:
+        if isinstance(lbx_0, np.ndarray):
             self.__lbx_0 = lbx_0
         else:
             raise Exception('Invalid lbx_0 value. Exiting.')
 
     @ubx_0.setter
     def ubx_0(self, ubx_0):
-        if type(ubx_0) == np.ndarray:
+        if isinstance(ubx_0, np.ndarray):
             self.__ubx_0 = ubx_0
         else:
             raise Exception('Invalid ubx_0 value. Exiting.')
@@ -1613,7 +1613,7 @@ class AcadosOcpConstraints:
 
     @Jbx_0.setter
     def Jbx_0(self, Jbx_0):
-        if type(Jbx_0) == np.ndarray:
+        if isinstance(Jbx_0, np.ndarray):
             self.__idxbx_0 = J_to_idx(Jbx_0)
         else:
             raise Exception('Invalid Jbx_0 value. Exiting.')
@@ -1639,28 +1639,28 @@ class AcadosOcpConstraints:
     # bounds on x
     @lbx.setter
     def lbx(self, lbx):
-        if type(lbx) == np.ndarray:
+        if isinstance(lbx, np.ndarray):
             self.__lbx = lbx
         else:
             raise Exception('Invalid lbx value. Exiting.')
 
     @ubx.setter
     def ubx(self, ubx):
-        if type(ubx) == np.ndarray:
+        if isinstance(ubx, np.ndarray):
             self.__ubx = ubx
         else:
             raise Exception('Invalid ubx value. Exiting.')
 
     @idxbx.setter
     def idxbx(self, idxbx):
-        if type(idxbx) == np.ndarray:
+        if isinstance(idxbx, np.ndarray):
             self.__idxbx = idxbx
         else:
             raise Exception('Invalid idxbx value. Exiting.')
 
     @Jbx.setter
     def Jbx(self, Jbx):
-        if type(Jbx) == np.ndarray:
+        if isinstance(Jbx, np.ndarray):
             self.__idxbx = J_to_idx(Jbx)
         else:
             raise Exception('Invalid Jbx value. Exiting.')
@@ -1668,28 +1668,28 @@ class AcadosOcpConstraints:
     # bounds on u
     @lbu.setter
     def lbu(self, lbu):
-        if type(lbu) == np.ndarray:
+        if isinstance(lbu, np.ndarray):
             self.__lbu = lbu
         else:
             raise Exception('Invalid lbu value. Exiting.')
 
     @ubu.setter
     def ubu(self, ubu):
-        if type(ubu) == np.ndarray:
+        if isinstance(ubu, np.ndarray):
             self.__ubu = ubu
         else:
             raise Exception('Invalid ubu value. Exiting.')
 
     @idxbu.setter
     def idxbu(self, idxbu):
-        if type(idxbu) == np.ndarray:
+        if isinstance(idxbu, np.ndarray):
             self.__idxbu = idxbu
         else:
             raise Exception('Invalid idxbu value. Exiting.')
 
     @Jbu.setter
     def Jbu(self, Jbu):
-        if type(Jbu) == np.ndarray:
+        if isinstance(Jbu, np.ndarray):
             self.__idxbu = J_to_idx(Jbu)
         else:
             raise Exception('Invalid Jbu value. Exiting.')
@@ -1697,28 +1697,28 @@ class AcadosOcpConstraints:
     # bounds on x at shooting node N
     @lbx_e.setter
     def lbx_e(self, lbx_e):
-        if type(lbx_e) == np.ndarray:
+        if isinstance(lbx_e, np.ndarray):
             self.__lbx_e = lbx_e
         else:
             raise Exception('Invalid lbx_e value. Exiting.')
 
     @ubx_e.setter
     def ubx_e(self, ubx_e):
-        if type(ubx_e) == np.ndarray:
+        if isinstance(ubx_e, np.ndarray):
             self.__ubx_e = ubx_e
         else:
             raise Exception('Invalid ubx_e value. Exiting.')
 
     @idxbx_e.setter
     def idxbx_e(self, idxbx_e):
-        if type(idxbx_e) == np.ndarray:
+        if isinstance(idxbx_e, np.ndarray):
             self.__idxbx_e = idxbx_e
         else:
             raise Exception('Invalid idxbx_e value. Exiting.')
 
     @Jbx_e.setter
     def Jbx_e(self, Jbx_e):
-        if type(Jbx_e) == np.ndarray:
+        if isinstance(Jbx_e, np.ndarray):
             self.__idxbx_e = J_to_idx(Jbx_e)
         else:
             raise Exception('Invalid Jbx_e value. Exiting.')
@@ -1742,14 +1742,14 @@ class AcadosOcpConstraints:
 
     @lg.setter
     def lg(self, lg):
-        if type(lg) == np.ndarray:
+        if isinstance(lg, np.ndarray):
             self.__lg = lg
         else:
             raise Exception('Invalid lg value. Exiting.')
 
     @ug.setter
     def ug(self, ug):
-        if type(ug) == np.ndarray:
+        if isinstance(ug, np.ndarray):
             self.__ug = ug
         else:
             raise Exception('Invalid ug value. Exiting.')
@@ -1765,14 +1765,14 @@ class AcadosOcpConstraints:
 
     @lg_e.setter
     def lg_e(self, lg_e):
-        if type(lg_e) == np.ndarray:
+        if isinstance(lg_e, np.ndarray):
             self.__lg_e = lg_e
         else:
             raise Exception('Invalid lg_e value. Exiting.')
 
     @ug_e.setter
     def ug_e(self, ug_e):
-        if type(ug_e) == np.ndarray:
+        if isinstance(ug_e, np.ndarray):
             self.__ug_e = ug_e
         else:
             raise Exception('Invalid ug_e value. Exiting.')
@@ -1780,14 +1780,14 @@ class AcadosOcpConstraints:
     # nonlinear constraints
     @lh.setter
     def lh(self, lh):
-        if type(lh) == np.ndarray:
+        if isinstance(lh, np.ndarray):
             self.__lh = lh
         else:
             raise Exception('Invalid lh value. Exiting.')
 
     @uh.setter
     def uh(self, uh):
-        if type(uh) == np.ndarray:
+        if isinstance(uh, np.ndarray):
             self.__uh = uh
         else:
             raise Exception('Invalid uh value. Exiting.')
@@ -1795,14 +1795,14 @@ class AcadosOcpConstraints:
     # convex-over-nonlinear constraints
     @lphi.setter
     def lphi(self, lphi):
-        if type(lphi) == np.ndarray:
+        if isinstance(lphi, np.ndarray):
             self.__lphi = lphi
         else:
             raise Exception('Invalid lphi value. Exiting.')
 
     @uphi.setter
     def uphi(self, uphi):
-        if type(uphi) == np.ndarray:
+        if isinstance(uphi, np.ndarray):
             self.__uphi = uphi
         else:
             raise Exception('Invalid uphi value. Exiting.')
@@ -1810,14 +1810,14 @@ class AcadosOcpConstraints:
     # nonlinear constraints at shooting node N
     @lh_e.setter
     def lh_e(self, lh_e):
-        if type(lh_e) == np.ndarray:
+        if isinstance(lh_e, np.ndarray):
             self.__lh_e = lh_e
         else:
             raise Exception('Invalid lh_e value. Exiting.')
 
     @uh_e.setter
     def uh_e(self, uh_e):
-        if type(uh_e) == np.ndarray:
+        if isinstance(uh_e, np.ndarray):
             self.__uh_e = uh_e
         else:
             raise Exception('Invalid uh_e value. Exiting.')
@@ -1825,14 +1825,14 @@ class AcadosOcpConstraints:
     # convex-over-nonlinear constraints at shooting node N
     @lphi_e.setter
     def lphi_e(self, lphi_e):
-        if type(lphi_e) == np.ndarray:
+        if isinstance(lphi_e, np.ndarray):
             self.__lphi_e = lphi_e
         else:
             raise Exception('Invalid lphi_e value. Exiting.')
 
     @uphi_e.setter
     def uphi_e(self, uphi_e):
-        if type(uphi_e) == np.ndarray:
+        if isinstance(uphi_e, np.ndarray):
             self.__uphi_e = uphi_e
         else:
             raise Exception('Invalid uphi_e value. Exiting.')
@@ -1841,21 +1841,21 @@ class AcadosOcpConstraints:
     # soft bounds on x
     @lsbx.setter
     def lsbx(self, lsbx):
-        if type(lsbx) == np.ndarray:
+        if isinstance(lsbx, np.ndarray):
             self.__lsbx = lsbx
         else:
             raise Exception('Invalid lsbx value. Exiting.')
 
     @usbx.setter
     def usbx(self, usbx):
-        if type(usbx) == np.ndarray:
+        if isinstance(usbx, np.ndarray):
             self.__usbx = usbx
         else:
             raise Exception('Invalid usbx value. Exiting.')
 
     @idxsbx.setter
     def idxsbx(self, idxsbx):
-        if type(idxsbx) == np.ndarray:
+        if isinstance(idxsbx, np.ndarray):
             self.__idxsbx = idxsbx
         else:
             raise Exception('Invalid idxsbx value. Exiting.')
@@ -1870,28 +1870,28 @@ class AcadosOcpConstraints:
     # soft bounds on u
     @lsbu.setter
     def lsbu(self, lsbu):
-        if type(lsbu) == np.ndarray:
+        if isinstance(lsbu, np.ndarray):
             self.__lsbu = lsbu
         else:
             raise Exception('Invalid lsbu value. Exiting.')
 
     @usbu.setter
     def usbu(self, usbu):
-        if type(usbu) == np.ndarray:
+        if isinstance(usbu, np.ndarray):
             self.__usbu = usbu
         else:
             raise Exception('Invalid usbu value. Exiting.')
 
     @idxsbu.setter
     def idxsbu(self, idxsbu):
-        if type(idxsbu) == np.ndarray:
+        if isinstance(idxsbu, np.ndarray):
             self.__idxsbu = idxsbu
         else:
             raise Exception('Invalid idxsbu value. Exiting.')
 
     @Jsbu.setter
     def Jsbu(self, Jsbu):
-        if type(Jsbu) == np.ndarray:
+        if isinstance(Jsbu, np.ndarray):
             self.__idxsbu = J_to_idx_slack(Jsbu)
         else:
             raise Exception('Invalid Jsbu value. Exiting.')
@@ -1899,28 +1899,28 @@ class AcadosOcpConstraints:
     # soft bounds on x at shooting node N
     @lsbx_e.setter
     def lsbx_e(self, lsbx_e):
-        if type(lsbx_e) == np.ndarray:
+        if isinstance(lsbx_e, np.ndarray):
             self.__lsbx_e = lsbx_e
         else:
             raise Exception('Invalid lsbx_e value. Exiting.')
 
     @usbx_e.setter
     def usbx_e(self, usbx_e):
-        if type(usbx_e) == np.ndarray:
+        if isinstance(usbx_e, np.ndarray):
             self.__usbx_e = usbx_e
         else:
             raise Exception('Invalid usbx_e value. Exiting.')
 
     @idxsbx_e.setter
     def idxsbx_e(self, idxsbx_e):
-        if type(idxsbx_e) == np.ndarray:
+        if isinstance(idxsbx_e, np.ndarray):
             self.__idxsbx_e = idxsbx_e
         else:
             raise Exception('Invalid idxsbx_e value. Exiting.')
 
     @Jsbx_e.setter
     def Jsbx_e(self, Jsbx_e):
-        if type(Jsbx_e) == np.ndarray:
+        if isinstance(Jsbx_e, np.ndarray):
             self.__idxsbx_e = J_to_idx_slack(Jsbx_e)
         else:
             raise Exception('Invalid Jsbx_e value. Exiting.')
@@ -1959,21 +1959,21 @@ class AcadosOcpConstraints:
     # soft bounds on nonlinear constraints
     @lsh.setter
     def lsh(self, lsh):
-        if type(lsh) == np.ndarray:
+        if isinstance(lsh, np.ndarray):
             self.__lsh = lsh
         else:
             raise Exception('Invalid lsh value. Exiting.')
 
     @ush.setter
     def ush(self, ush):
-        if type(ush) == np.ndarray:
+        if isinstance(ush, np.ndarray):
             self.__ush = ush
         else:
             raise Exception('Invalid ush value. Exiting.')
 
     @idxsh.setter
     def idxsh(self, idxsh):
-        if type(idxsh) == np.ndarray:
+        if isinstance(idxsh, np.ndarray):
             self.__idxsh = idxsh
         else:
             raise Exception('Invalid idxsh value. Exiting.')
@@ -1989,21 +1989,21 @@ class AcadosOcpConstraints:
     # soft bounds on convex-over-nonlinear constraints
     @lsphi.setter
     def lsphi(self, lsphi):
-        if type(lsphi) == np.ndarray:
+        if isinstance(lsphi, np.ndarray):
             self.__lsphi = lsphi
         else:
             raise Exception('Invalid lsphi value. Exiting.')
 
     @usphi.setter
     def usphi(self, usphi):
-        if type(usphi) == np.ndarray:
+        if isinstance(usphi, np.ndarray):
             self.__usphi = usphi
         else:
             raise Exception('Invalid usphi value. Exiting.')
 
     @idxsphi.setter
     def idxsphi(self, idxsphi):
-        if type(idxsphi) == np.ndarray:
+        if isinstance(idxsphi, np.ndarray):
             self.__idxsphi = idxsphi
         else:
             raise Exception('Invalid idxsphi value. Exiting.')
@@ -2151,6 +2151,10 @@ class AcadosOcpOptions:
         self.__ext_cost_num_hess = 0
         self.__alpha_min = 0.05
         self.__alpha_reduction = 0.7
+        self.__line_search_use_sufficient_descent = 0
+        self.__globalization_use_SOC = 0
+        self.__full_step_dual = 0
+        self.__eps_sufficient_descent = 1e-4
 
 
     @property
@@ -2367,6 +2371,43 @@ class AcadosOcpOptions:
         """Step size reduction factor for globalization MERIT_BACKTRACKING, default: 0.7."""
         return self.__alpha_reduction
 
+    @property
+    def line_search_use_sufficient_descent(self):
+        """
+        Determines if sufficient descent (Armijo) condition is used in line search.
+        Type: int; 0 or 1;
+        default: 0.
+        """
+        return self.__line_search_use_sufficient_descent
+
+    @property
+    def eps_sufficient_descent(self):
+        """
+        Factor for sufficient descent (Armijo) conditon, see line_search_use_sufficient_descent.
+        Type: float,
+        default: 1e-4.
+        """
+        return self.__eps_sufficient_descent
+
+    @property
+    def globalization_use_SOC(self):
+        """
+        Determines if second order correction (SOC) is done when using MERIT_BACKTRACKING.
+        SOC is done if preliminary line search does not return full step.
+        Type: int; 0 or 1;
+        default: 0.
+        """
+        return self.__globalization_use_SOC
+
+    @property
+    def full_step_dual(self):
+        """
+        Determines if dual variables are updated with full steps (alpha=1.0) when primal variables are updated with smaller step.
+        Type: int; 0 or 1;
+        default: 0.
+        """
+        return self.__full_step_dual
+
     @property
     def nlp_solver_tol_ineq(self):
         """NLP solver inequality tolerance"""
@@ -2524,12 +2565,23 @@ class AcadosOcpOptions:
 
     @time_steps.setter
     def time_steps(self, time_steps):
-        self.__time_steps = time_steps
+        if isinstance(time_steps, np.ndarray):
+            if len(time_steps.shape) == 1:
+                    self.__time_steps = time_steps
+            else:
+                raise Exception('Invalid time_steps, expected np.ndarray of shape (N,).')
+        else:
+            raise Exception('Invalid time_steps, expected np.ndarray.')
 
     @shooting_nodes.setter
     def shooting_nodes(self, shooting_nodes):
-        self.__shooting_nodes = shooting_nodes
-
+        if isinstance(shooting_nodes, np.ndarray):
+            if len(shooting_nodes.shape) == 1:
+                self.__shooting_nodes = shooting_nodes
+            else:
+                raise Exception('Invalid shooting_nodes, expected np.ndarray of shape (N+1,).')
+        else:
+            raise Exception('Invalid shooting_nodes, expected np.ndarray.')
 
     @Tsim.setter
     def Tsim(self, Tsim):
@@ -2537,7 +2589,12 @@ class AcadosOcpOptions:
 
     @globalization.setter
     def globalization(self, globalization):
-        self.__globalization = globalization
+        globalization_types = ('MERIT_BACKTRACKING', 'FIXED_STEP')
+        if globalization in globalization_types:
+            self.__globalization = globalization
+        else:
+            raise Exception('Invalid globalization value. Possible values are:\n\n' \
+                    + ',\n'.join(globalization_types) + '.\n\nYou have: ' + globalization + '.\n\nExiting.')
 
     @alpha_min.setter
     def alpha_min(self, alpha_min):
@@ -2547,10 +2604,38 @@ class AcadosOcpOptions:
     def alpha_reduction(self, alpha_reduction):
         self.__alpha_reduction = alpha_reduction
 
+    @line_search_use_sufficient_descent.setter
+    def line_search_use_sufficient_descent(self, line_search_use_sufficient_descent):
+        if line_search_use_sufficient_descent in [0, 1]:
+            self.__line_search_use_sufficient_descent = line_search_use_sufficient_descent
+        else:
+            raise Exception(f'Invalid value for line_search_use_sufficient_descent. Possible values are 0, 1, got {line_search_use_sufficient_descent}')
+
+    @globalization_use_SOC.setter
+    def globalization_use_SOC(self, globalization_use_SOC):
+        if globalization_use_SOC in [0, 1]:
+            self.__globalization_use_SOC = globalization_use_SOC
+        else:
+            raise Exception(f'Invalid value for globalization_use_SOC. Possible values are 0, 1, got {globalization_use_SOC}')
+
+    @full_step_dual.setter
+    def full_step_dual(self, full_step_dual):
+        if full_step_dual in [0, 1]:
+            self.__full_step_dual = full_step_dual
+        else:
+            raise Exception(f'Invalid value for full_step_dual. Possible values are 0, 1, got {full_step_dual}')
+
+    @eps_sufficient_descent.setter
+    def eps_sufficient_descent(self, eps_sufficient_descent):
+        if isinstance(eps_sufficient_descent, float) and eps_sufficient_descent > 0:
+            self.__eps_sufficient_descent = eps_sufficient_descent
+        else:
+            raise Exception('Invalid eps_sufficient_descent value. eps_sufficient_descent must be a positive float. Exiting')
+
     @sim_method_num_stages.setter
     def sim_method_num_stages(self, sim_method_num_stages):
 
-        # if type(sim_method_num_stages) == int:
+        # if isinstance(sim_method_num_stages, int):
         #     self.__sim_method_num_stages = sim_method_num_stages
         # else:
         #     raise Exception('Invalid sim_method_num_stages value. sim_method_num_stages must be an integer. Exiting.')
@@ -2560,7 +2645,7 @@ class AcadosOcpOptions:
     @sim_method_num_steps.setter
     def sim_method_num_steps(self, sim_method_num_steps):
 
-        # if type(sim_method_num_steps) == int:
+        # if isinstance(sim_method_num_steps, int):
         #     self.__sim_method_num_steps = sim_method_num_steps
         # else:
         #     raise Exception('Invalid sim_method_num_steps value. sim_method_num_steps must be an integer. Exiting.')
@@ -2570,7 +2655,7 @@ class AcadosOcpOptions:
     @sim_method_newton_iter.setter
     def sim_method_newton_iter(self, sim_method_newton_iter):
 
-        if type(sim_method_newton_iter) == int:
+        if isinstance(sim_method_newton_iter, int):
             self.__sim_method_newton_iter = sim_method_newton_iter
         else:
             raise Exception('Invalid sim_method_newton_iter value. sim_method_newton_iter must be an integer. Exiting.')
@@ -2593,7 +2678,7 @@ class AcadosOcpOptions:
 
     @nlp_solver_step_length.setter
     def nlp_solver_step_length(self, nlp_solver_step_length):
-        if type(nlp_solver_step_length) == float and nlp_solver_step_length > 0:
+        if isinstance(nlp_solver_step_length, float) and nlp_solver_step_length > 0:
             self.__nlp_solver_step_length = nlp_solver_step_length
         else:
             raise Exception('Invalid nlp_solver_step_length value. nlp_solver_step_length must be a positive float. Exiting')
@@ -2614,7 +2699,7 @@ class AcadosOcpOptions:
 
     @qp_solver_cond_N.setter
     def qp_solver_cond_N(self, qp_solver_cond_N):
-        if isinstance(qp_solver_cond_N, int) and qp_solver_cond_N > 0:
+        if isinstance(qp_solver_cond_N, int) and qp_solver_cond_N >= 0:
             self.__qp_solver_cond_N = qp_solver_cond_N
         else:
             raise Exception('Invalid qp_solver_cond_N value. qp_solver_cond_N must be a positive int. Exiting')
@@ -2705,21 +2790,21 @@ class AcadosOcpOptions:
     @nlp_solver_max_iter.setter
     def nlp_solver_max_iter(self, nlp_solver_max_iter):
 
-        if type(nlp_solver_max_iter) == int and nlp_solver_max_iter > 0:
+        if isinstance(nlp_solver_max_iter, int) and nlp_solver_max_iter > 0:
             self.__nlp_solver_max_iter = nlp_solver_max_iter
         else:
             raise Exception('Invalid nlp_solver_max_iter value. nlp_solver_max_iter must be a positive int. Exiting')
 
     @print_level.setter
     def print_level(self, print_level):
-        if type(print_level) == int and print_level >= 0:
+        if isinstance(print_level, int) and print_level >= 0:
             self.__print_level = print_level
         else:
             raise Exception('Invalid print_level value. print_level takes one of the values >=0. Exiting')
 
     @model_external_shared_lib_dir.setter
     def model_external_shared_lib_dir(self, model_external_shared_lib_dir):
-        if type(model_external_shared_lib_dir) == str :
+        if isinstance(model_external_shared_lib_dir, str) :
             self.__model_external_shared_lib_dir = model_external_shared_lib_dir
         else:
             raise Exception('Invalid model_external_shared_lib_dir value. Str expected.' \
@@ -2727,7 +2812,7 @@ class AcadosOcpOptions:
 
     @model_external_shared_lib_name.setter
     def model_external_shared_lib_name(self, model_external_shared_lib_name):
-        if type(model_external_shared_lib_name) == str :
+        if isinstance(model_external_shared_lib_name, str) :
             if model_external_shared_lib_name[-3:] == '.so' : 
                 raise Exception('Invalid model_external_shared_lib_name value. Remove the .so extension.' \
             + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\nExiting.')
@@ -2810,6 +2895,9 @@ class AcadosOcp:
         self.acados_lib_path = f'{acados_path}/lib'
         """Path to where acados library is located, type: string"""
 
+        import numpy
+        self.cython_include_dirs = numpy.get_include()
+
         self.__parameter_values = np.array([])
         self.__problem_class = 'OCP'
 
diff --git a/pyextra/acados_template/acados_ocp_solver.py b/pyextra/acados_template/acados_ocp_solver.py
index b34b8fa84a..6d1ff9869c 100644
--- a/pyextra/acados_template/acados_ocp_solver.py
+++ b/pyextra/acados_template/acados_ocp_solver.py
@@ -37,7 +37,7 @@ import os
 import json
 import numpy as np
 from datetime import datetime
-import ctypes
+import importlib
 from ctypes import POINTER, cast, CDLL, c_void_p, c_char_p, c_double, c_int, c_int64, byref
 
 from copy import deepcopy
@@ -51,9 +51,9 @@ from .generate_c_code_nls_cost import generate_c_code_nls_cost
 from .generate_c_code_external_cost import generate_c_code_external_cost
 from .acados_ocp import AcadosOcp
 from .acados_model import acados_model_strip_casadi_symbolics
-from .utils import is_column, is_empty, casadi_length, render_template, acados_class2dict,\
+from .utils import is_column, is_empty, casadi_length, render_template,\
      format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\
-     set_up_imported_gnsf_model, get_acados_path, get_ocp_nlp_layout, get_python_interface_path
+     set_up_imported_gnsf_model, get_ocp_nlp_layout, get_python_interface_path
 
 
 def make_ocp_dims_consistent(acados_ocp):
@@ -90,7 +90,7 @@ def make_ocp_dims_consistent(acados_ocp):
         raise Exception('inconsistent dimension np, regarding model.p and parameter_values.' + \
             f'\nGot np = {dims.np}, acados_ocp.parameter_values.shape = {acados_ocp.parameter_values.shape[0]}\n')
 
-    # cost
+    ## cost
     # initial stage - if not set, copy fields from path constraints
     if cost.cost_type_0 is None:
         cost.cost_type_0 = cost.cost_type
@@ -434,18 +434,14 @@ def make_ocp_dims_consistent(acados_ocp):
         if np.shape(opts.shooting_nodes)[0] != dims.N+1:
             raise Exception('inconsistent dimension N, regarding shooting_nodes.')
 
-        # time_steps = opts.shooting_nodes[1:] - opts.shooting_nodes[0:-1]
-        # # identify constant time-steps: due to numerical reasons the content of time_steps might vary a bit
-        # delta_time_steps = time_steps[1:] - time_steps[0:-1]
-        # avg_time_steps = np.average(time_steps)
-        # # criterion for constant time-step detection: the min/max difference in values normalized by the average
-        # check_const_time_step = np.max(delta_time_steps)-np.min(delta_time_steps) / avg_time_steps
-        # # if the criterion is small, we have a constant time-step
-        # if check_const_time_step < 1e-9:
-        #     time_steps[:] = avg_time_steps  # if we have a constant time-step: apply the average time-step
-        time_steps = np.zeros((dims.N,))
-        for i in range(dims.N):
-            time_steps[i] = opts.shooting_nodes[i+1] - opts.shooting_nodes[i] # TODO use commented code above
+        time_steps = opts.shooting_nodes[1:] - opts.shooting_nodes[0:-1]
+        # identify constant time_steps: due to numerical reasons the content of time_steps might vary a bit
+        avg_time_steps = np.average(time_steps)
+        # criterion for constant time step detection: the min/max difference in values normalized by the average
+        check_const_time_step = (np.max(time_steps)-np.min(time_steps)) / avg_time_steps
+        # if the criterion is small, we have a constant time_step
+        if check_const_time_step < 1e-9:
+            time_steps[:] = avg_time_steps  # if we have a constant time_step: apply the average time_step
 
         opts.time_steps = time_steps
 
@@ -525,8 +521,7 @@ def ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file='acados_ocp_n
 
     # strip shooting_nodes
     ocp_nlp_dict['solver_options'].pop('shooting_nodes', None)
-
-    dims_dict = acados_class2dict(acados_ocp.dims)
+    dims_dict = format_class_dict(acados_ocp.dims.__dict__)
 
     ocp_check_against_layout(ocp_nlp_dict, dims_dict)
 
@@ -782,8 +777,15 @@ class AcadosOcpSolver:
         dlclose.argtypes = [c_void_p]
 
     @classmethod
-    def generate(cls, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True):
+    def generate(cls, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None):
+        """
+        Generates the code for an acados OCP solver, given the description in acados_ocp.
+            :param acados_ocp: type AcadosOcp - description of the OCP for acados
+            :param json_file: name for the json file used to render the templated code - default: acados_ocp_nlp.json
+            :param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible Inputs and Outputs
+        """
         model = acados_ocp.model
+        acados_ocp.code_export_directory = os.path.abspath(acados_ocp.code_export_directory)
 
         if simulink_opts is None:
             simulink_opts = get_simulink_default_opts()
@@ -807,24 +809,91 @@ class AcadosOcpSolver:
         # dump to json
         ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file)
 
-        code_export_dir = acados_ocp.code_export_directory
         # render templates
         ocp_render_templates(acados_ocp, json_file)
+        acados_ocp.json_file = json_file
 
-        if build:
-          ## Compile solver
-          cwd=os.getcwd()
-          os.chdir(code_export_dir)
-          os.system('make clean_ocp_shared_lib')
-          os.system('make ocp_shared_lib')
-          os.chdir(cwd)
-
-    def __init__(self, model_name, N, code_export_dir):
-        self.model_name = model_name
-        self.N = N
+
+    @classmethod
+    def build(cls, code_export_dir, with_cython=False):
+        """
+        Builds the code for an acados OCP solver, that has been generated in code_export_dir
+            :param code_export_dir: directory in which acados OCP solver has been generated, see generate()
+            :param with_cython: option indicating if the cython interface is build, default: False.
+        """
+        cwd=os.getcwd()
+        os.chdir(code_export_dir)
+        if with_cython:
+            os.system('make clean_ocp_cython')
+            os.system('make ocp_cython')
+        else:
+            os.system('make clean_ocp_shared_lib')
+            os.system('make ocp_shared_lib')
+        os.chdir(cwd)
+
+
+    @classmethod
+    def create_cython_solver(cls, json_file):
+        """
+        Returns an `AcadosOcpSolverCython` object.
+
+        This is an alternative Cython based Python wrapper to the acados OCP solver in C.
+        This offers faster interaction with the solver, because getter and setter calls, which include shape checking are done in compiled C code.
+
+        The default wrapper `AcadosOcpSolver` is based on ctypes.
+        """
+        with open(json_file, 'r') as f:
+            acados_ocp_json = json.load(f)
+        code_export_directory = acados_ocp_json['code_export_directory']
+
+        importlib.invalidate_caches()
+        rel_code_export_directory = os.path.relpath(code_export_directory)
+        acados_ocp_solver_pyx = importlib.import_module(f'{rel_code_export_directory}.acados_ocp_solver_pyx')
+
+        AcadosOcpSolverCython = getattr(acados_ocp_solver_pyx, 'AcadosOcpSolverCython')
+        return AcadosOcpSolverCython(acados_ocp_json['model']['name'],
+                    acados_ocp_json['solver_options']['nlp_solver_type'],
+                    acados_ocp_json['dims']['N'])
+
+
+    def __init__(self, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True, generate=True):
 
         self.solver_created = False
-        self.shared_lib_name = f'{code_export_dir}/libacados_ocp_solver_{self.model_name}.so'
+        if generate:
+            self.generate(acados_ocp, json_file=json_file, simulink_opts=simulink_opts)
+
+        # load json, store options in object
+        with open(json_file, 'r') as f:
+            acados_ocp_json = json.load(f)
+        self.N = acados_ocp_json['dims']['N']
+        self.model_name = acados_ocp_json['model']['name']
+        self.solver_options = acados_ocp_json['solver_options']
+
+        acados_lib_path = acados_ocp_json['acados_lib_path']
+        code_export_directory = acados_ocp_json['code_export_directory']
+
+        if build:
+            self.build(code_export_directory, with_cython=False)
+
+        # Load acados library to avoid unloading the library.
+        # This is necessary if acados was compiled with OpenMP, since the OpenMP threads can't be destroyed.
+        # Unloading a library which uses OpenMP results in a segfault (on any platform?).
+        # see [https://stackoverflow.com/questions/34439956/vc-crash-when-freeing-a-dll-built-with-openmp]
+        # or [https://python.hotexamples.com/examples/_ctypes/-/dlclose/python-dlclose-function-examples.html]
+        libacados_name = 'libacados.so'
+        libacados_filepath = os.path.join(acados_lib_path, libacados_name)
+        self.__acados_lib = CDLL(libacados_filepath)
+        # find out if acados was compiled with OpenMP
+        try:
+            self.__acados_lib_uses_omp = getattr(self.__acados_lib, 'omp_get_thread_num') is not None
+        except AttributeError as e:
+            self.__acados_lib_uses_omp = False
+        if self.__acados_lib_uses_omp:
+            print('acados was compiled with OpenMP.')
+        else:
+            print('acados was compiled without OpenMP.')
+
+        self.shared_lib_name = f'{code_export_directory}/libacados_ocp_solver_{self.model_name}.so'
 
         # get shared_lib
         self.shared_lib = CDLL(self.shared_lib_name)
@@ -842,6 +911,8 @@ class AcadosOcpSolver:
         # get pointers solver
         self.__get_pointers_solver()
 
+        self.status = 0
+
 
     def __get_pointers_solver(self):
         """
@@ -864,6 +935,10 @@ class AcadosOcpSolver:
         getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out").restype = c_void_p
         self.nlp_out = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out")(self.capsule)
 
+        getattr(self.shared_lib, f"{self.model_name}_acados_get_sens_out").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{self.model_name}_acados_get_sens_out").restype = c_void_p
+        self.sens_out = getattr(self.shared_lib, f"{self.model_name}_acados_get_sens_out")(self.capsule)
+
         getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in").argtypes = [c_void_p]
         getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in").restype = c_void_p
         self.nlp_in = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in")(self.capsule)
@@ -872,46 +947,26 @@ class AcadosOcpSolver:
         getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver").restype = c_void_p
         self.nlp_solver = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver")(self.capsule)
 
-        # treat parameters separately
-        getattr(self.shared_lib, f"{self.model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)]
-        getattr(self.shared_lib, f"{self.model_name}_acados_update_params").restype = c_int
-        self._set_param = getattr(self.shared_lib, f"{self.model_name}_acados_update_params")
-
-        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
-        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int
-
-        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
-        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int
-
-        self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-        self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-        self.shared_lib.ocp_nlp_out_set.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-        self.shared_lib.ocp_nlp_set.argtypes = \
-            [c_void_p, c_void_p, c_int, c_char_p, c_void_p]
 
     def solve(self):
         """
         Solve the ocp with current input.
         """
-
         getattr(self.shared_lib, f"{self.model_name}_acados_solve").argtypes = [c_void_p]
         getattr(self.shared_lib, f"{self.model_name}_acados_solve").restype = c_int
-        status = getattr(self.shared_lib, f"{self.model_name}_acados_solve")(self.capsule)
-        return status
+        self.status = getattr(self.shared_lib, f"{self.model_name}_acados_solve")(self.capsule)
+
+        return self.status
 
 
     def set_new_time_steps(self, new_time_steps):
         """
-        Set new time steps before solving. Only reload library without code generation but with new time steps.
+        Set new time steps.
+        Recreates the solver if N changes.
 
-            :param new_time_steps: vector of new time steps for the solver
+            :param new_time_steps: 1 dimensional np array of new time steps for the solver
 
-            .. note:: This allows for different use-cases: either set a new size of time-steps or a new distribution of
+            .. note:: This allows for different use-cases: either set a new size of time_steps or a new distribution of
                       the shooting nodes without changing the number, e.g., to reach a different final time. Both cases
                       do not require a new code export and compilation.
         """
@@ -921,15 +976,14 @@ class AcadosOcpSolver:
             raise Exception('Solver was not yet created!')
 
         # check if time steps really changed in value
-        if np.array_equal(self.acados_ocp.solver_options.time_steps, new_time_steps):
+        if np.array_equal(self.solver_options['time_steps'], new_time_steps):
             return
 
         N = new_time_steps.size
-        model = self.acados_ocp.model
         new_time_steps_data = cast(new_time_steps.ctypes.data, POINTER(c_double))
 
         # check if recreation of acados is necessary (no need to recreate acados if sizes are identical)
-        if self.acados_ocp.solver_options.time_steps.size == N:
+        if len(self.solver_options['time_steps']) == N:
             getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps").argtypes = [c_void_p, c_int, c_void_p]
             getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps").restype = c_int
             assert getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps")(self.capsule, N, new_time_steps_data) == 0
@@ -941,11 +995,6 @@ class AcadosOcpSolver:
             getattr(self.shared_lib, f"{self.model_name}_acados_free").restype = c_int
             getattr(self.shared_lib, f"{self.model_name}_acados_free")(self.capsule)
 
-            # store N and new time steps
-            self.N = self.acados_ocp.dims.N = N
-            self.acados_ocp.solver_options.time_steps = new_time_steps
-            self.acados_ocp.solver_options.Tsim = self.acados_ocp.solver_options.time_steps[0]
-
             # create solver with new time steps
             getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization").argtypes = [c_void_p, c_int, c_void_p]
             getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization").restype = c_int
@@ -956,6 +1005,75 @@ class AcadosOcpSolver:
             # get pointers solver
             self.__get_pointers_solver()
 
+        # store time_steps, N
+        self.solver_options['time_steps'] = new_time_steps
+        self.N = N
+        self.solver_options['Tsim'] = self.solver_options['time_steps'][0]
+
+
+    def update_qp_solver_cond_N(self, qp_solver_cond_N: int):
+        """
+        Recreate solver with new value `qp_solver_cond_N` with a partial condensing QP solver.
+        This function is relevant for code reuse, i.e., if either `set_new_time_steps(...)` is used or
+        the influence of a different `qp_solver_cond_N` is studied without code export and compilation.
+            :param qp_solver_cond_N: new number of condensing stages for the solver
+
+            .. note:: This function can only be used in combination with a partial condensing QP solver.
+
+            .. note:: After `set_new_time_steps(...)` is used and depending on the new number of time steps it might be
+                      necessary to change `qp_solver_cond_N` as well (using this function), i.e., typically
+                      `qp_solver_cond_N < N`.
+        """
+        # unlikely but still possible
+        if not self.solver_created:
+            raise Exception('Solver was not yet created!')
+        if self.N < qp_solver_cond_N:
+            raise Exception('Setting qp_solver_cond_N to be larger than N does not work!')
+        if self.solver_options['qp_solver_cond_N'] != qp_solver_cond_N:
+            self.solver_created = False
+
+            # recreate the solver
+            fun_name = f'{self.model_name}_acados_update_qp_solver_cond_N'
+            getattr(self.shared_lib, fun_name).argtypes = [c_void_p, c_int]
+            getattr(self.shared_lib, fun_name).restype = c_int
+            assert getattr(self.shared_lib, fun_name)(self.capsule, qp_solver_cond_N) == 0
+
+            # store the new value
+            self.solver_options['qp_solver_cond_N'] = qp_solver_cond_N
+            self.solver_created = True
+
+            # get pointers solver
+            self.__get_pointers_solver()
+
+
+    def eval_param_sens(self, index, stage=0, field="ex"):
+        """
+        Calculate the sensitivity of the curent solution with respect to the initial state component of index
+
+            :param index: integer corresponding to initial state index in range(nx)
+        """
+
+        field_ = field
+        field = field_.encode('utf-8')
+
+        # checks
+        if not isinstance(index, int):
+            raise Exception('AcadosOcpSolver.eval_param_sens(): index must be Integer.')
+
+        self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
+        self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
+        nx = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8'))
+
+        if index < 0 or index > nx:
+            raise Exception(f'AcadosOcpSolver.eval_param_sens(): index must be in [0, nx-1], got: {index}.')
+
+        # actual eval_param
+        self.shared_lib.ocp_nlp_eval_param_sens.argtypes = [c_void_p, c_char_p, c_int, c_int, c_void_p]
+        self.shared_lib.ocp_nlp_eval_param_sens.restype = None
+        self.shared_lib.ocp_nlp_eval_param_sens(self.nlp_solver, field, stage, index, self.sens_out)
+
+        return
+
 
     def get(self, stage_, field_):
         """
@@ -978,23 +1096,30 @@ class AcadosOcpSolver:
 
         out_fields = ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su']
         # mem_fields = ['sl', 'su']
+        sens_fields = ['sens_u', "sens_x"]
+        all_fields = out_fields + sens_fields
+
         field = field_
-        field = field.encode('utf-8')
 
-        if (field_ not in out_fields):
+        if (field_ not in all_fields):
             raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\
-                    \n Possible values are {}. Exiting.'.format(field_, out_fields))
+                    \n Possible values are {}. Exiting.'.format(field_, all_fields))
 
         if not isinstance(stage_, int):
             raise Exception('AcadosOcpSolver.get(): stage index must be Integer.')
 
         if stage_ < 0 or stage_ > self.N:
-            raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(self.N))
+            raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(stage_))
 
         if stage_ == self.N and field_ == 'pi':
             raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\
                 .format(field_, stage_))
 
+        if field_ in sens_fields:
+            field = field_.replace('sens_', '')
+
+        field = field.encode('utf-8')
+
         self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
             [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
         self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
@@ -1015,6 +1140,11 @@ class AcadosOcpSolver:
         #         [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
         #     self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \
         #         self.nlp_dims, self.nlp_solver, stage_, field, out_data)
+        elif field_ in sens_fields:
+            self.shared_lib.ocp_nlp_out_get.argtypes = \
+                [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+            self.shared_lib.ocp_nlp_out_get(self.nlp_config, \
+                self.nlp_dims, self.sens_out, stage_, field, out_data)
 
         return out
 
@@ -1029,6 +1159,7 @@ class AcadosOcpSolver:
             - res_comp: residual wrt complementarity conditions
             - qp_stat: status of QP solver
             - qp_iter: number of QP iterations
+            - alpha: SQP step size
             - qp_res_stat: stationarity residual of the last QP solution
             - qp_res_eq: residual wrt equality constraints (dynamics) of the last QP solution
             - qp_res_ineq: residual wrt inequality constraints (constraints)  of the last QP solution
@@ -1036,19 +1167,18 @@ class AcadosOcpSolver:
         """
         stat = self.get_stats("statistics")
 
-        if self.acados_ocp.solver_options.nlp_solver_type == 'SQP':
-            print('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter')
-            if stat.shape[0]>7:
+        if self.solver_options['nlp_solver_type'] == 'SQP':
+            print('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha')
+            if stat.shape[0]>8:
                 print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp')
             for jj in range(stat.shape[1]):
-                print('{:d}\t{:e}\t{:e}\t{:e}\t{:e}\t{:d}\t{:d}'.format( \
-                     int(stat[0][jj]), stat[1][jj], stat[2][jj], \
-                     stat[3][jj], stat[4][jj], int(stat[5][jj]), int(stat[6][jj])))
-                if stat.shape[0]>7:
+                print(f'{int(stat[0][jj]):d}\t{stat[1][jj]:e}\t{stat[2][jj]:e}\t{stat[3][jj]:e}\t' +
+                      f'{stat[4][jj]:e}\t{int(stat[5][jj]):d}\t{int(stat[6][jj]):d}\t{stat[7][jj]:e}\t')
+                if stat.shape[0]>8:
                     print('\t{:e}\t{:e}\t{:e}\t{:e}'.format( \
-                        stat[7][jj], stat[8][jj], stat[9][jj], stat[10][jj]))
+                        stat[8][jj], stat[9][jj], stat[10][jj], stat[11][jj]))
             print('\n')
-        elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
+        elif self.solver_options['nlp_solver_type'] == 'SQP_RTI':
             print('\niter\tqp_stat\tqp_iter')
             if stat.shape[0]>3:
                 print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp')
@@ -1108,6 +1238,7 @@ class AcadosOcpSolver:
         with open(filename, 'r') as f:
             solution = json.load(f)
 
+        print(f"loading iterate {filename}")
         for key in solution.keys():
             (field, stage) = key.split('_')
             self.set(int(stage), field, np.array(solution[key]))
@@ -1117,62 +1248,99 @@ class AcadosOcpSolver:
         """
         Get the information of the last solver call.
 
-            :param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter']
+            :param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter', 'residuals', 'qp_iter', 'alpha']
+
+        Available fileds:
+            - time_tot: total CPU time previous call
+            - time_lin: CPU time for linearization
+            - time_sim: CPU time for integrator
+            - time_sim_ad: CPU time for integrator contribution of external function calls
+            - time_sim_la: CPU time for integrator contribution of linear algebra
+            - time_qp: CPU time qp solution
+            - time_qp_solver_call: CPU time inside qp solver (without converting the QP)
+            - time_qp_xcond: time_glob: CPU time globalization
+            - time_solution_sensitivities: CPU time for previous call to eval_param_sens
+            - time_reg: CPU time regularization
+            - sqp_iter: number of SQP iterations
+            - qp_iter: vector of QP iterations for last SQP call
+            - statistics: table with info about last iteration
+            - stat_m: number of rows in statistics matrix
+            - stat_n: number of columns in statistics matrix
+            - residuals: residuals of last iterate
+            - alpha: step sizes of SQP iterations
         """
 
-        fields = ['time_tot',  # total cpu time previous call
-                  'time_lin',  # cpu time for linearization
-                  'time_sim',  # cpu time for integrator
-                  'time_sim_ad',  # cpu time for integrator contribution of external function calls
-                  'time_sim_la',  # cpu time for integrator contribution of linear algebra
-                  'time_qp',   # cpu time qp solution
-                  'time_qp_solver_call',  # cpu time inside qp solver (without converting the QP)
+        double_fields = ['time_tot',
+                  'time_lin',
+                  'time_sim',
+                  'time_sim_ad',
+                  'time_sim_la',
+                  'time_qp',
+                  'time_qp_solver_call',
                   'time_qp_xcond',
-                  'time_glob',  # cpu time globalization
-                  'time_reg',  # cpu time regularization
-                  'sqp_iter',  # number of SQP iterations
-                  'qp_iter',  # vector of QP iterations for last SQP call
-                  'statistics',  # table with info about last iteration
+                  'time_glob',
+                  'time_solution_sensitivities',
+                  'time_reg'
+        ]
+        fields = double_fields + [
+                  'sqp_iter',
+                  'qp_iter',
+                  'statistics',
                   'stat_m',
-                  'stat_n',]
+                  'stat_n',
+                  'residuals',
+                  'alpha',
+                ]
+        field = field_.encode('utf-8')
 
-        field = field_
-        field = field.encode('utf-8')
-        if (field_ not in fields):
-            raise Exception('AcadosOcpSolver.get_stats(): {} is not a valid argument.\
-                    \n Possible values are {}. Exiting.'.format(fields, fields))
 
         if field_ in ['sqp_iter', 'stat_m', 'stat_n']:
             out = np.ascontiguousarray(np.zeros((1,)), dtype=np.int64)
             out_data = cast(out.ctypes.data, POINTER(c_int64))
+            self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
+            self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
+            return out
+
+        # TODO: just return double instead of np.
+        elif field_ in double_fields:
+            out = np.zeros((1,))
+            out_data = cast(out.ctypes.data, POINTER(c_double))
+            self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
+            self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
+            return out
 
         elif field_ == 'statistics':
             sqp_iter = self.get_stats("sqp_iter")
             stat_m = self.get_stats("stat_m")
             stat_n = self.get_stats("stat_n")
-
             min_size = min([stat_m, sqp_iter+1])
-
             out = np.ascontiguousarray(
                         np.zeros((stat_n[0]+1, min_size[0])), dtype=np.float64)
             out_data = cast(out.ctypes.data, POINTER(c_double))
+            self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
+            self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
+            return out
 
         elif field_ == 'qp_iter':
             full_stats = self.get_stats('statistics')
-            if self.acados_ocp.solver_options.nlp_solver_type == 'SQP':
-                out = full_stats[6, :]
-            elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
-                out = full_stats[2, :]
+            if self.solver_options['nlp_solver_type'] == 'SQP':
+                return full_stats[6, :]
+            elif self.solver_options['nlp_solver_type'] == 'SQP_RTI':
+                return full_stats[2, :]
 
-        else:
-            out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64)
-            out_data = cast(out.ctypes.data, POINTER(c_double))
+        elif field_ == 'alpha':
+            full_stats = self.get_stats('statistics')
+            if self.solver_options['nlp_solver_type'] == 'SQP':
+                return full_stats[7, :]
+            else: # self.solver_options['nlp_solver_type'] == 'SQP_RTI':
+                raise Exception("alpha values are not available for SQP_RTI")
 
-        if not field_ == 'qp_iter':
-            self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
-            self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
+        elif field_ == 'residuals':
+            return self.get_residuals()
 
-        return out
+        else:
+            raise Exception(f'AcadosOcpSolver.get_stats(): {field} is not a valid argument.'
+                    + f'\n Possible values are {fields}.')
 
 
     def get_cost(self):
@@ -1201,7 +1369,7 @@ class AcadosOcpSolver:
         Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
         """
         # compute residuals if RTI
-        if self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
+        if self.solver_options['nlp_solver_type'] == 'SQP_RTI':
             self.shared_lib.ocp_nlp_eval_residuals.argtypes = [c_void_p, c_void_p, c_void_p]
             self.shared_lib.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
 
@@ -1230,9 +1398,7 @@ class AcadosOcpSolver:
 
 
     # Note: this function should not be used anymore, better use cost_set, constraints_set
-
     def set(self, stage_, field_, value_):
-
         """
         Set numerical data inside the solver.
 
@@ -1253,6 +1419,7 @@ class AcadosOcpSolver:
         cost_fields = ['y_ref', 'yref']
         constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
         out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su']
+        mem_fields = ['xdot_guess']
 
         # cast value_ to avoid conversion issues
         if isinstance(value_, (float, int)):
@@ -1294,18 +1461,25 @@ class AcadosOcpSolver:
             value_data_p = cast((value_data), c_void_p)
 
             if field_ in constraints_fields:
+                self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
+                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
                 self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
                     self.nlp_dims, self.nlp_in, stage, field, value_data_p)
             elif field_ in cost_fields:
+                self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
+                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
                 self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
                     self.nlp_dims, self.nlp_in, stage, field, value_data_p)
             elif field_ in out_fields:
+                self.shared_lib.ocp_nlp_out_set.argtypes = \
+                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
                 self.shared_lib.ocp_nlp_out_set(self.nlp_config, \
                     self.nlp_dims, self.nlp_out, stage, field, value_data_p)
-            # elif field_ in mem_fields:
-            #     self.shared_lib.ocp_nlp_set(self.nlp_config, \
-            #         self.nlp_solver, stage, field, value_data_p)
-
+            elif field_ in mem_fields:
+                self.shared_lib.ocp_nlp_set.argtypes = \
+                    [c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+                self.shared_lib.ocp_nlp_set(self.nlp_config, \
+                    self.nlp_solver, stage, field, value_data_p)
         return
 
 
@@ -1364,9 +1538,8 @@ class AcadosOcpSolver:
                 raise Exception("Unknown api: '{}'".format(api))
 
         if value_shape != tuple(dims):
-            raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \
-                ' for field "{}" with dimension {} (you have {})'.format( \
-                field_, tuple(dims), value_shape))
+            raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension' +
+                f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
 
         value_data = cast(value_.ctypes.data, POINTER(c_double))
         value_data_p = cast((value_data), c_void_p)
@@ -1433,8 +1606,8 @@ class AcadosOcpSolver:
                 raise Exception("Unknown api: '{}'".format(api))
 
         if value_shape != tuple(dims):
-            raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
-                ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))
+            raise Exception(f'AcadosOcpSolver.constraints_set(): mismatching dimension' +
+                f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
 
         value_data = cast(value_.ctypes.data, POINTER(c_double))
         value_data_p = cast((value_data), c_void_p)
@@ -1490,11 +1663,11 @@ class AcadosOcpSolver:
         """
         Set options of the solver.
 
-            :param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction'
+            :param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC'
             :param value: of type int, float
         """
-        int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks']
-        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction']
+        int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC']
+        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent']
         string_fields = ['globalization']
 
         # check field availability and type
@@ -1522,10 +1695,10 @@ class AcadosOcpSolver:
 
         if field_ == 'rti_phase':
             if value_ < 0 or value_ > 2:
-                raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
+                raise Exception('AcadosOcpSolver.options_set(): argument \'rti_phase\' can '
                     'take only values 0, 1, 2 for SQP-RTI-type solvers')
-            if self.acados_ocp.solver_options.nlp_solver_type != 'SQP_RTI' and value_ > 0:
-                raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
+            if self.solver_options['nlp_solver_type'] != 'SQP_RTI' and value_ > 0:
+                raise Exception('AcadosOcpSolver.options_set(): argument \'rti_phase\' can '
                     'take only value 0 for SQP-type solvers')
 
         # encode
diff --git a/pyextra/acados_template/acados_ocp_solver_fast.py b/pyextra/acados_template/acados_ocp_solver_fast.py
deleted file mode 100644
index 656d288f1c..0000000000
--- a/pyextra/acados_template/acados_ocp_solver_fast.py
+++ /dev/null
@@ -1,402 +0,0 @@
-import sys
-import os
-import json
-import numpy as np
-from datetime import datetime
-
-from ctypes import POINTER, CDLL, c_void_p, c_int, cast, c_double, c_char_p
-
-from copy import deepcopy
-
-from .generate_c_code_explicit_ode import generate_c_code_explicit_ode
-from .generate_c_code_implicit_ode import generate_c_code_implicit_ode
-from .generate_c_code_gnsf import generate_c_code_gnsf
-from .generate_c_code_discrete_dynamics import generate_c_code_discrete_dynamics
-from .generate_c_code_constraint import generate_c_code_constraint
-from .generate_c_code_nls_cost import generate_c_code_nls_cost
-from .generate_c_code_external_cost import generate_c_code_external_cost
-from .acados_ocp import AcadosOcp
-from .acados_model import acados_model_strip_casadi_symbolics
-from .utils import is_column, is_empty, casadi_length, render_template, acados_class2dict,\
-     format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\
-     set_up_imported_gnsf_model, get_acados_path
-
-
-class AcadosOcpSolverFast:
-    dlclose = CDLL(None).dlclose
-    dlclose.argtypes = [c_void_p]
-
-    def __init__(self, model_name, N, code_export_dir):
-
-        self.solver_created = False
-        self.N = N
-        self.model_name = model_name
-
-        self.shared_lib_name = f'{code_export_dir}/libacados_ocp_solver_{model_name}.so'
-
-        # get shared_lib
-        self.shared_lib = CDLL(self.shared_lib_name)
-
-        # create capsule
-        getattr(self.shared_lib, f"{model_name}_acados_create_capsule").restype = c_void_p
-        self.capsule = getattr(self.shared_lib, f"{model_name}_acados_create_capsule")()
-
-        # create solver
-        getattr(self.shared_lib, f"{model_name}_acados_create").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_create").restype = c_int
-        assert getattr(self.shared_lib, f"{model_name}_acados_create")(self.capsule)==0
-        self.solver_created = True
-
-        # get pointers solver
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts").restype = c_void_p
-        self.nlp_opts = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts")(self.capsule)
-
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims").restype = c_void_p
-        self.nlp_dims = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims")(self.capsule)
-
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config").restype = c_void_p
-        self.nlp_config = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config")(self.capsule)
-
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out").restype = c_void_p
-        self.nlp_out = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out")(self.capsule)
-
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in").restype = c_void_p
-        self.nlp_in = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in")(self.capsule)
-
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver").restype = c_void_p
-        self.nlp_solver = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver")(self.capsule)
-
-
-    def solve(self):
-        """
-        Solve the ocp with current input.
-        """
-        model_name = self.model_name
-
-        getattr(self.shared_lib, f"{model_name}_acados_solve").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_solve").restype = c_int
-        status = getattr(self.shared_lib, f"{model_name}_acados_solve")(self.capsule)
-        return status
-
-    def cost_set(self, start_stage_, field_, value_, api='warn'):
-      self.cost_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
-      return
-
-    def cost_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
-        """
-        Set numerical data in the cost module of the solver.
-
-            :param stage: integer corresponding to shooting node
-            :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess'
-            :param value: of appropriate size
-        """
-        # cast value_ to avoid conversion issues
-        if isinstance(value_, (float, int)):
-            value_ = np.array([value_])
-        value_ = np.ascontiguousarray(np.copy(value_), dtype=np.float64)
-        field = field_
-        field = field.encode('utf-8')
-        dim = np.product(value_.shape[1:])
-
-        start_stage = c_int(start_stage_)
-        end_stage = c_int(end_stage_)
-        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
-        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int
-
-        dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
-        dims_data = cast(dims.ctypes.data, POINTER(c_int))
-
-        self.shared_lib.ocp_nlp_cost_dims_get_from_attr(self.nlp_config,
-            self.nlp_dims, self.nlp_out, start_stage_, field, dims_data)
-
-        value_shape = value_.shape
-        expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims]))
-        if len(value_shape) == 2:
-            value_shape = (value_shape[0], value_shape[1], 0)
-
-        elif len(value_shape) == 3:
-            if api=='old':
-                pass
-            elif api=='warn':
-                if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')):
-                    raise Exception("Ambiguity in API detected.\n"
-                                    "Are you making an acados model from scrach? Add api='new' to cost_set and carry on.\n"
-                                    "Are you seeing this error suddenly in previously running code? Read on.\n"
-                                    "  You are relying on a now-fixed bug in cost_set for field '{}'.\n".format(field_) +
-                                    "  acados_template now correctly passes on any matrices to acados in column major format.\n" +
-                                    "  Two options to fix this error: \n" +
-                                    "   * Add api='old' to cost_set to restore old incorrect behaviour\n" +
-                                    "   * Add api='new' to cost_set and remove any unnatural manipulation of the value argument " +
-                                    "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " +
-                                    "If there is no such manipulation, then you have probably been getting an incorrect solution before.")
-                # Get elements in column major order
-                value_ = np.ravel(value_, order='F')
-            elif api=='new':
-                # Get elements in column major order
-                value_ = np.ravel(value_, order='F')
-            else:
-                raise Exception("Unknown api: '{}'".format(api))
-
-        if value_shape != expected_shape:
-            raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension',
-                            ' for field "{}" with dimension {} (you have {})'.format(
-                               field_, expected_shape, value_shape))
-
-
-        value_data = cast(value_.ctypes.data, POINTER(c_double))
-        value_data_p = cast((value_data), c_void_p)
-
-        self.shared_lib.ocp_nlp_cost_model_set_slice.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
-        self.shared_lib.ocp_nlp_cost_model_set_slice(self.nlp_config,
-            self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim)
-        return
-
-    def constraints_set(self, start_stage_, field_, value_, api='warn'):
-      self.constraints_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
-      return
-
-    def constraints_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
-        """
-        Set numerical data in the constraint module of the solver.
-
-            :param stage: integer corresponding to shooting node
-            :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi']
-            :param value: of appropriate size
-        """
-        # cast value_ to avoid conversion issues
-        if isinstance(value_, (float, int)):
-            value_ = np.array([value_])
-        value_ = value_.astype(float)
-
-        field = field_
-        field = field.encode('utf-8')
-        dim = np.product(value_.shape[1:])
-
-        start_stage = c_int(start_stage_)
-        end_stage = c_int(end_stage_)
-        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
-        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int
-
-        dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
-        dims_data = cast(dims.ctypes.data, POINTER(c_int))
-
-        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \
-            self.nlp_dims, self.nlp_out, start_stage_, field, dims_data)
-
-        value_shape = value_.shape
-        expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims]))
-        if len(value_shape) == 2:
-            value_shape = (value_shape[0], value_shape[1], 0)
-        elif len(value_shape) == 3:
-            if api=='old':
-                pass
-            elif api=='warn':
-                if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')):
-                    raise Exception("Ambiguity in API detected.\n"
-                                    "Are you making an acados model from scrach? Add api='new' to constraints_set and carry on.\n"
-                                    "Are you seeing this error suddenly in previously running code? Read on.\n"
-                                    "  You are relying on a now-fixed bug in constraints_set for field '{}'.\n".format(field_) +
-                                    "  acados_template now correctly passes on any matrices to acados in column major format.\n" +
-                                    "  Two options to fix this error: \n" +
-                                    "   * Add api='old' to constraints_set to restore old incorrect behaviour\n" +
-                                    "   * Add api='new' to constraints_set and remove any unnatural manipulation of the value argument " +
-                                    "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " +
-                                    "If there is no such manipulation, then you have probably been getting an incorrect solution before.")
-                # Get elements in column major order
-                value_ = np.ravel(value_, order='F')
-            elif api=='new':
-                # Get elements in column major order
-                value_ = np.ravel(value_, order='F')
-            else:
-                raise Exception("Unknown api: '{}'".format(api))
-        if value_shape != expected_shape:
-            raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
-                ' for field "{}" with dimension {} (you have {})'.format(field_, expected_shape, value_shape))
-
-        value_data = cast(value_.ctypes.data, POINTER(c_double))
-        value_data_p = cast((value_data), c_void_p)
-
-        self.shared_lib.ocp_nlp_constraints_model_set_slice.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
-        self.shared_lib.ocp_nlp_constraints_model_set_slice(self.nlp_config, \
-            self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim)
-        return
-
-    # Note: this function should not be used anymore, better use cost_set, constraints_set
-    def set(self, stage_, field_, value_):
-        """
-        Set numerical data inside the solver.
-
-            :param stage: integer corresponding to shooting node
-            :param field: string in ['x', 'u', 'pi', 'lam', 't', 'p']
-
-            .. note:: regarding lam, t: \n
-                    the inequalities are internally organized in the following order: \n
-                    [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
-                      lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
-
-            .. note:: pi: multipliers for dynamics equality constraints \n
-                      lam: multipliers for inequalities \n
-                      t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
-                      sl: slack variables of soft lower inequality constraints \n
-                      su: slack variables of soft upper inequality constraints \n
-        """
-        cost_fields = ['y_ref', 'yref']
-        constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
-        out_fields = ['x', 'u', 'pi', 'lam', 't', 'z']
-        mem_fields = ['sl', 'su']
-
-        # cast value_ to avoid conversion issues
-        if isinstance(value_, (float, int)):
-            value_ = np.array([value_])
-        value_ = value_.astype(float)
-
-        model_name = self.model_name
-
-        field = field_
-        field = field.encode('utf-8')
-
-        stage = c_int(stage_)
-
-        # treat parameters separately
-        if field_ == 'p':
-            getattr(self.shared_lib, f"{model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)]
-            getattr(self.shared_lib, f"{model_name}_acados_update_params").restype = c_int
-
-            value_data = cast(value_.ctypes.data, POINTER(c_double))
-
-            assert getattr(self.shared_lib, f"{model_name}_acados_update_params")(self.capsule, stage, value_data, value_.shape[0])==0
-        else:
-            if field_ not in constraints_fields + cost_fields + out_fields + mem_fields:
-                raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
-                    \nPossible values are {}. Exiting.".format(field, \
-                    constraints_fields + cost_fields + out_fields + ['p']))
-
-            self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
-                [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
-            self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
-
-            dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
-                self.nlp_dims, self.nlp_out, stage_, field)
-
-            if value_.shape[0] != dims:
-                msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
-                msg += 'with dimension {} (you have {})'.format(dims, value_.shape)
-                raise Exception(msg)
-
-            value_data = cast(value_.ctypes.data, POINTER(c_double))
-            value_data_p = cast((value_data), c_void_p)
-
-            if field_ in constraints_fields:
-                self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
-                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-                self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
-                    self.nlp_dims, self.nlp_in, stage, field, value_data_p)
-            elif field_ in cost_fields:
-                self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
-                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-                self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
-                    self.nlp_dims, self.nlp_in, stage, field, value_data_p)
-            elif field_ in out_fields:
-                self.shared_lib.ocp_nlp_out_set.argtypes = \
-                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-                self.shared_lib.ocp_nlp_out_set(self.nlp_config, \
-                    self.nlp_dims, self.nlp_out, stage, field, value_data_p)
-            elif field_ in mem_fields:
-                self.shared_lib.ocp_nlp_set.argtypes = \
-                    [c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-                self.shared_lib.ocp_nlp_set(self.nlp_config, \
-                    self.nlp_solver, stage, field, value_data_p)
-        return
-
-
-    def get_slice(self, start_stage_, end_stage_, field_):
-        """
-        Get the last solution of the solver:
-
-            :param start_stage: integer corresponding to shooting node that indicates start of slice
-            :param end_stage: integer corresponding to shooting node that indicates end of slice
-            :param field: string in ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su',]
-
-            .. note:: regarding lam, t: \n
-                    the inequalities are internally organized in the following order: \n
-                    [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
-                      lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
-
-            .. note:: pi: multipliers for dynamics equality constraints \n
-                      lam: multipliers for inequalities \n
-                      t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
-                      sl: slack variables of soft lower inequality constraints \n
-                      su: slack variables of soft upper inequality constraints \n
-        """
-        out_fields = ['x', 'u', 'z', 'pi', 'lam', 't']
-        mem_fields = ['sl', 'su']
-        field = field_
-        field = field.encode('utf-8')
-
-        if (field_ not in out_fields + mem_fields):
-            raise Exception('AcadosOcpSolver.get_slice(): {} is an invalid argument.\
-                    \n Possible values are {}. Exiting.'.format(field_, out_fields))
-
-        if not isinstance(start_stage_, int):
-            raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.')
-
-        if not isinstance(end_stage_, int):
-            raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.')
-
-        if start_stage_ >= end_stage_:
-            raise Exception('AcadosOcpSolver.get_slice(): end stage index must be larger than start stage index')
-
-        if start_stage_ < 0 or end_stage_ > self.N + 1:
-            raise Exception('AcadosOcpSolver.get_slice(): stage index must be in [0, N], got: {}.'.format(self.N))
-        self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
-        self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
-
-        dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
-            self.nlp_dims, self.nlp_out, start_stage_, field)
-
-        out = np.ascontiguousarray(np.zeros((end_stage_ - start_stage_, dims)), dtype=np.float64)
-        out_data = cast(out.ctypes.data, POINTER(c_double))
-
-        if (field_ in out_fields):
-            self.shared_lib.ocp_nlp_out_get_slice.argtypes = \
-                [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p]
-            self.shared_lib.ocp_nlp_out_get_slice(self.nlp_config, \
-                self.nlp_dims, self.nlp_out, start_stage_, end_stage_, field, out_data)
-        elif field_ in mem_fields:
-            self.shared_lib.ocp_nlp_get_at_stage.argtypes = \
-                [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-            self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \
-                self.nlp_dims, self.nlp_solver, start_stage_, end_stage_, field, out_data)
-
-        return out
-
-    def get_cost(self):
-        """
-        Returns the cost value of the current solution.
-        """
-        # compute cost internally
-        self.shared_lib.ocp_nlp_eval_cost.argtypes = [c_void_p, c_void_p, c_void_p]
-        self.shared_lib.ocp_nlp_eval_cost(self.nlp_solver, self.nlp_in, self.nlp_out)
-
-        # create output array
-        out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64)
-        out_data = cast(out.ctypes.data, POINTER(c_double))
-
-        # call getter
-        self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
-
-        field = "cost_value".encode('utf-8')
-        self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
-
-        return out[0]
diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx
index 9bcf5bb46a..97d5b2855f 100644
--- a/pyextra/acados_template/acados_ocp_solver_pyx.pyx
+++ b/pyextra/acados_template/acados_ocp_solver_pyx.pyx
@@ -39,21 +39,19 @@ cimport cython
 from libc cimport string
 
 cimport acados_solver_common
+# TODO: make this import more clear? it is not a general solver, but problem specific.
 cimport acados_solver
 
 cimport numpy as cnp
 
 import os
+from datetime import datetime
 import numpy as np
 
 
-cdef class AcadosOcpSolverFast:
+cdef class AcadosOcpSolverCython:
     """
     Class to interact with the acados ocp solver C object.
-
-        :param acados_ocp: type AcadosOcp - description of the OCP for acados
-        :param json_file: name for the json file used to render the templated code - default: acados_ocp_nlp.json
-        :param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible Inputs and Outputs
     """
 
     cdef acados_solver.nlp_solver_capsule *capsule
@@ -61,19 +59,26 @@ cdef class AcadosOcpSolverFast:
     cdef acados_solver_common.ocp_nlp_dims *nlp_dims
     cdef acados_solver_common.ocp_nlp_config *nlp_config
     cdef acados_solver_common.ocp_nlp_out *nlp_out
+    cdef acados_solver_common.ocp_nlp_out *sens_out
     cdef acados_solver_common.ocp_nlp_in *nlp_in
     cdef acados_solver_common.ocp_nlp_solver *nlp_solver
 
+    cdef int status
+    cdef bint solver_created
+
     cdef str model_name
     cdef int N
-    cdef bint solver_created
 
-    def __cinit__(self, str model_name, int N, str code_export_dir):
-        self.model_name = model_name
-        self.N = N
+    cdef str nlp_solver_type
+
+    def __cinit__(self, model_name, nlp_solver_type, N):
 
         self.solver_created = False
 
+        self.N = N
+        self.model_name = model_name
+        self.nlp_solver_type = nlp_solver_type
+
         # create capsule
         self.capsule = acados_solver.acados_create_capsule()
 
@@ -81,11 +86,21 @@ cdef class AcadosOcpSolverFast:
         assert acados_solver.acados_create(self.capsule) == 0
         self.solver_created = True
 
+        # get pointers solver
+        self.__get_pointers_solver()
+        self.status = 0
+
+
+    def __get_pointers_solver(self):
+        """
+        Private function to get the pointers for solver
+        """
         # get pointers solver
         self.nlp_opts = acados_solver.acados_get_nlp_opts(self.capsule)
         self.nlp_dims = acados_solver.acados_get_nlp_dims(self.capsule)
         self.nlp_config = acados_solver.acados_get_nlp_config(self.capsule)
         self.nlp_out = acados_solver.acados_get_nlp_out(self.capsule)
+        self.sens_out = acados_solver.acados_get_sens_out(self.capsule)
         self.nlp_in = acados_solver.acados_get_nlp_in(self.capsule)
         self.nlp_solver = acados_solver.acados_get_nlp_solver(self.capsule)
 
@@ -99,15 +114,112 @@ cdef class AcadosOcpSolverFast:
 
     def set_new_time_steps(self, new_time_steps):
         """
-        Set new time steps before solving. Only reload library without code generation but with new time steps.
+        Set new time steps.
+        Recreates the solver if N changes.
 
-            :param new_time_steps: vector of new time steps for the solver
+            :param new_time_steps: 1 dimensional np array of new time steps for the solver
 
             .. note:: This allows for different use-cases: either set a new size of time-steps or a new distribution of
                       the shooting nodes without changing the number, e.g., to reach a different final time. Both cases
                       do not require a new code export and compilation.
         """
-        raise NotImplementedError()
+
+        raise NotImplementedError("AcadosOcpSolverCython: does not support set_new_time_steps() since it is only a prototyping feature")
+        # # unlikely but still possible
+        # if not self.solver_created:
+        #     raise Exception('Solver was not yet created!')
+
+        # ## check if time steps really changed in value
+        # # get time steps
+        # cdef cnp.ndarray[cnp.float64_t, ndim=1] old_time_steps = np.ascontiguousarray(np.zeros((self.N,)), dtype=np.float64)
+        # assert acados_solver.acados_get_time_steps(self.capsule, self.N, <double *> old_time_steps.data)
+
+        # if np.array_equal(old_time_steps, new_time_steps):
+        #     return
+
+        # N = new_time_steps.size
+        # cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(new_time_steps, dtype=np.float64)
+
+        # # check if recreation of acados is necessary (no need to recreate acados if sizes are identical)
+        # if len(old_time_steps) == N:
+        #     assert acados_solver.acados_update_time_steps(self.capsule, N, <double *> value.data) == 0
+
+        # else:  # recreate the solver with the new time steps
+        #     self.solver_created = False
+
+        #     # delete old memory (analog to __del__)
+        #     acados_solver.acados_free(self.capsule)
+
+        #     # create solver with new time steps
+        #     assert acados_solver.acados_create_with_discretization(self.capsule, N, <double *> value.data) == 0
+
+        #     self.solver_created = True
+
+        #     # get pointers solver
+        #     self.__get_pointers_solver()
+
+        # # store time_steps, N
+        # self.time_steps = new_time_steps
+        # self.N = N
+
+
+    def update_qp_solver_cond_N(self, qp_solver_cond_N: int):
+        """
+        Recreate solver with new value `qp_solver_cond_N` with a partial condensing QP solver.
+        This function is relevant for code reuse, i.e., if either `set_new_time_steps(...)` is used or
+        the influence of a different `qp_solver_cond_N` is studied without code export and compilation.
+            :param qp_solver_cond_N: new number of condensing stages for the solver
+
+            .. note:: This function can only be used in combination with a partial condensing QP solver.
+
+            .. note:: After `set_new_time_steps(...)` is used and depending on the new number of time steps it might be
+                      necessary to change `qp_solver_cond_N` as well (using this function), i.e., typically
+                      `qp_solver_cond_N < N`.
+        """
+        raise NotImplementedError("AcadosOcpSolverCython: does not support update_qp_solver_cond_N() since it is only a prototyping feature")
+
+        # # unlikely but still possible
+        # if not self.solver_created:
+        #     raise Exception('Solver was not yet created!')
+        # if self.N < qp_solver_cond_N:
+        #     raise Exception('Setting qp_solver_cond_N to be larger than N does not work!')
+        # if self.qp_solver_cond_N != qp_solver_cond_N:
+        #     self.solver_created = False
+
+        #     # recreate the solver
+        #     acados_solver.acados_update_qp_solver_cond_N(self.capsule, qp_solver_cond_N)
+
+        #     # store the new value
+        #     self.qp_solver_cond_N = qp_solver_cond_N
+        #     self.solver_created = True
+
+        #     # get pointers solver
+        #     self.__get_pointers_solver()
+
+
+    def eval_param_sens(self, index, stage=0, field="ex"):
+        """
+        Calculate the sensitivity of the curent solution with respect to the initial state component of index
+
+            :param index: integer corresponding to initial state index in range(nx)
+        """
+
+        field_ = field
+        field = field_.encode('utf-8')
+
+        # checks
+        if not isinstance(index, int):
+            raise Exception('AcadosOcpSolverCython.eval_param_sens(): index must be Integer.')
+
+        cdef int nx = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8'))
+
+        if index < 0 or index > nx:
+            raise Exception(f'AcadosOcpSolverCython.eval_param_sens(): index must be in [0, nx-1], got: {index}.')
+
+        # actual eval_param
+        acados_solver_common.ocp_nlp_eval_param_sens(self.nlp_solver, field, stage, index, self.sens_out)
+
+        return
 
 
     def get(self, int stage, str field_):
@@ -133,14 +245,14 @@ cdef class AcadosOcpSolverFast:
         field = field_.encode('utf-8')
 
         if field_ not in out_fields:
-            raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\
+            raise Exception('AcadosOcpSolverCython.get(): {} is an invalid argument.\
                     \n Possible values are {}. Exiting.'.format(field_, out_fields))
 
         if stage < 0 or stage > self.N:
-            raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(self.N))
+            raise Exception('AcadosOcpSolverCython.get(): stage index must be in [0, N], got: {}.'.format(self.N))
 
         if stage == self.N and field_ == 'pi':
-            raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\
+            raise Exception('AcadosOcpSolverCython.get(): field {} does not exist at final stage {}.'\
                 .format(field_, stage))
 
         cdef int dims = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config,
@@ -168,7 +280,7 @@ cdef class AcadosOcpSolverFast:
             - qp_res_ineq: residual wrt inequality constraints (constraints)  of the last QP solution
             - qp_res_comp: residual wrt complementarity conditions of the last QP solution
         """
-        raise NotImplementedError()
+        acados_solver.acados_print_stats(self.capsule)
 
 
     def store_iterate(self, filename='', overwrite=False):
@@ -178,14 +290,50 @@ cdef class AcadosOcpSolverFast:
             :param filename: if not set, use model_name + timestamp + '.json'
             :param overwrite: if false and filename exists add timestamp to filename
         """
-        raise NotImplementedError()
+        import json
+        if filename == '':
+            filename += self.model_name + '_' + 'iterate' + '.json'
+
+        if not overwrite:
+            # append timestamp
+            if os.path.isfile(filename):
+                filename = filename[:-5]
+                filename += datetime.utcnow().strftime('%Y-%m-%d-%H:%M:%S.%f') + '.json'
+
+        # get iterate:
+        solution = dict()
+
+        for i in range(self.N+1):
+            solution['x_'+str(i)] = self.get(i,'x')
+            solution['u_'+str(i)] = self.get(i,'u')
+            solution['z_'+str(i)] = self.get(i,'z')
+            solution['lam_'+str(i)] = self.get(i,'lam')
+            solution['t_'+str(i)] = self.get(i, 't')
+            solution['sl_'+str(i)] = self.get(i, 'sl')
+            solution['su_'+str(i)] = self.get(i, 'su')
+        for i in range(self.N):
+            solution['pi_'+str(i)] = self.get(i,'pi')
+
+        # save
+        with open(filename, 'w') as f:
+            json.dump(solution, f, default=lambda x: x.tolist(), indent=4, sort_keys=True)
+        print("stored current iterate in ", os.path.join(os.getcwd(), filename))
 
 
     def load_iterate(self, filename):
         """
         Loads the iterate stored in json file with filename into the ocp solver.
         """
-        raise NotImplementedError()
+        import json
+        if not os.path.isfile(filename):
+            raise Exception('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename))
+
+        with open(filename, 'r') as f:
+            solution = json.load(f)
+
+        for key in solution.keys():
+            (field, stage) = key.split('_')
+            self.set(int(stage), field, np.array(solution[key]))
 
 
     def get_stats(self, field_):
@@ -193,8 +341,97 @@ cdef class AcadosOcpSolverFast:
         Get the information of the last solver call.
 
             :param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter']
+        Available fileds:
+            - time_tot: total CPU time previous call
+            - time_lin: CPU time for linearization
+            - time_sim: CPU time for integrator
+            - time_sim_ad: CPU time for integrator contribution of external function calls
+            - time_sim_la: CPU time for integrator contribution of linear algebra
+            - time_qp: CPU time qp solution
+            - time_qp_solver_call: CPU time inside qp solver (without converting the QP)
+            - time_qp_xcond: time_glob: CPU time globalization
+            - time_solution_sensitivities: CPU time for previous call to eval_param_sens
+            - time_reg: CPU time regularization
+            - sqp_iter: number of SQP iterations
+            - qp_iter: vector of QP iterations for last SQP call
+            - statistics: table with info about last iteration
+            - stat_m: number of rows in statistics matrix
+            - stat_n: number of columns in statistics matrix
+            - residuals: residuals of last iterate
+            - alpha: step sizes of SQP iterations
         """
-        raise NotImplementedError()
+
+        double_fields = ['time_tot',
+                  'time_lin',
+                  'time_sim',
+                  'time_sim_ad',
+                  'time_sim_la',
+                  'time_qp',
+                  'time_qp_solver_call',
+                  'time_qp_xcond',
+                  'time_glob',
+                  'time_solution_sensitivities',
+                  'time_reg'
+        ]
+        fields = double_fields + [
+                  'sqp_iter',
+                  'qp_iter',
+                  'statistics',
+                  'stat_m',
+                  'stat_n',
+                  'residuals',
+                  'alpha',
+                ]
+        field = field_.encode('utf-8')
+
+        if field_ in ['sqp_iter', 'stat_m', 'stat_n']:
+            return self.__get_stat_int(field)
+
+        elif field_ in double_fields:
+            return self.__get_stat_double(field)
+
+        elif field_ == 'statistics':
+            sqp_iter = self.get_stats("sqp_iter")
+            stat_m = self.get_stats("stat_m")
+            stat_n = self.get_stats("stat_n")
+            min_size = min([stat_m, sqp_iter+1])
+            return self.__get_stat_matrix(field, stat_n+1, min_size)
+
+        elif field_ == 'qp_iter':
+            full_stats = self.get_stats('statistics')
+            if self.nlp_solver_type == 'SQP':
+                return full_stats[6, :]
+            elif self.nlp_solver_type == 'SQP_RTI':
+                return full_stats[2, :]
+
+        elif field_ == 'alpha':
+            full_stats = self.get_stats('statistics')
+            if self.nlp_solver_type == 'SQP':
+                return full_stats[7, :]
+            else: # self.nlp_solver_type == 'SQP_RTI':
+                raise Exception("alpha values are not available for SQP_RTI")
+
+        elif field_ == 'residuals':
+            return self.get_residuals()
+
+        else:
+            raise NotImplementedError("TODO!")
+
+
+    def __get_stat_int(self, field):
+        cdef int out
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &out)
+        return out
+
+    def __get_stat_double(self, field):
+        cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.zeros((1,))
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> out.data)
+        return out
+
+    def __get_stat_matrix(self, field, n, m):
+        cdef cnp.ndarray[cnp.float64_t, ndim=2] out_mat = np.ascontiguousarray(np.zeros((n, m)), dtype=np.float64)
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> out_mat.data)
+        return out_mat
 
 
     def get_cost(self):
@@ -217,7 +454,31 @@ cdef class AcadosOcpSolverFast:
         """
         Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
         """
-        raise NotImplementedError()
+        # compute residuals if RTI
+        if self.nlp_solver_type == 'SQP_RTI':
+            acados_solver_common.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
+
+        # create output array
+        cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.ascontiguousarray(np.zeros((4,), dtype=np.float64))
+        cdef double double_value
+
+        field = "res_stat".encode('utf-8')
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
+        out[0] = double_value
+
+        field = "res_eq".encode('utf-8')
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
+        out[1] = double_value
+
+        field = "res_ineq".encode('utf-8')
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
+        out[2] = double_value
+
+        field = "res_comp".encode('utf-8')
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
+        out[3] = double_value
+
+        return out
 
 
     # Note: this function should not be used anymore, better use cost_set, constraints_set
@@ -243,18 +504,18 @@ cdef class AcadosOcpSolverFast:
         cost_fields = ['y_ref', 'yref']
         constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
         out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su']
+        mem_fields = ['xdot_guess']
 
         field = field_.encode('utf-8')
 
-        cdef double[::1] value
+        cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(value_, dtype=np.float64)
 
         # treat parameters separately
         if field_ == 'p':
-            value = np.ascontiguousarray(value_, dtype=np.double)
-            assert acados_solver.acados_update_params(self.capsule, stage, <double *> &value[0], value.shape[0]) == 0
+            assert acados_solver.acados_update_params(self.capsule, stage, <double *> value.data, value.shape[0]) == 0
         else:
             if field_ not in constraints_fields + cost_fields + out_fields:
-                raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
+                raise Exception("AcadosOcpSolverCython.set(): {} is not a valid argument.\
                     \nPossible values are {}. Exiting.".format(field, \
                     constraints_fields + cost_fields + out_fields + ['p']))
 
@@ -262,20 +523,22 @@ cdef class AcadosOcpSolverFast:
                 self.nlp_dims, self.nlp_out, stage, field)
 
             if value_.shape[0] != dims:
-                msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
+                msg = 'AcadosOcpSolverCython.set(): mismatching dimension for field "{}" '.format(field_)
                 msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0])
                 raise Exception(msg)
 
-            value = np.ascontiguousarray(value_, dtype=np.double)
             if field_ in constraints_fields:
                 acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config,
-                    self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0])
+                    self.nlp_dims, self.nlp_in, stage, field, <void *> value.data)
             elif field_ in cost_fields:
                 acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config,
-                    self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0])
+                    self.nlp_dims, self.nlp_in, stage, field, <void *> value.data)
             elif field_ in out_fields:
                 acados_solver_common.ocp_nlp_out_set(self.nlp_config,
-                    self.nlp_dims, self.nlp_out, stage, field, <void *> &value[0])
+                    self.nlp_dims, self.nlp_out, stage, field, <void *> value.data)
+            elif field_ in mem_fields:
+                acados_solver_common.ocp_nlp_set(self.nlp_config, \
+                    self.nlp_solver, stage, field, <void *> value.data)
 
 
     def cost_set(self, int stage, str field_, value_):
@@ -304,9 +567,8 @@ cdef class AcadosOcpSolverFast:
             value = np.asfortranarray(value_)
 
         if value_shape[0] != dims[0] or value_shape[1] != dims[1]:
-            raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \
-                ' for field "{}" with dimension {} (you have {})'.format( \
-                field_, tuple(dims), value_shape))
+            raise Exception('AcadosOcpSolverCython.cost_set(): mismatching dimension' +
+                f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
 
         acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config, \
             self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0][0])
@@ -338,8 +600,8 @@ cdef class AcadosOcpSolverFast:
             value = np.asfortranarray(value_)
 
         if value_shape[0] != dims[0] or value_shape[1] != dims[1]:
-            raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
-                ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))
+            raise Exception(f'AcadosOcpSolverCython.constraints_set(): mismatching dimension' +
+                f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
 
         acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, \
             self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0][0])
@@ -361,7 +623,7 @@ cdef class AcadosOcpSolverFast:
         acados_solver_common.ocp_nlp_dynamics_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field, &dims[0])
 
         # create output data
-        out = np.zeros((dims[0], dims[1]), order='F', dtype=np.float64)
+        cdef cnp.ndarray[cnp.float64_t, ndim=2] out = np.zeros((dims[0], dims[1]), order='F')
 
         # call getter
         acados_solver_common.ocp_nlp_get_at_stage(self.nlp_config, self.nlp_dims, self.nlp_solver, stage, field, <void *> out.data)
@@ -376,8 +638,8 @@ cdef class AcadosOcpSolverFast:
             :param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction'
             :param value: of type int, float
         """
-        int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks']
-        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction']
+        int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC']
+        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent']
         string_fields = ['globalization']
 
         # encode
@@ -394,10 +656,10 @@ cdef class AcadosOcpSolverFast:
 
             if field_ == 'rti_phase':
                 if value_ < 0 or value_ > 2:
-                    raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
+                    raise Exception('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can '
                         'take only values 0, 1, 2 for SQP-RTI-type solvers')
-                if self.acados_ocp.solver_options.nlp_solver_type != 'SQP_RTI' and value_ > 0:
-                    raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
+                if self.nlp_solver_type != 'SQP_RTI' and value_ > 0:
+                    raise Exception('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can '
                         'take only value 0 for SQP-type solvers')
 
             int_value = value_
@@ -418,7 +680,7 @@ cdef class AcadosOcpSolverFast:
             acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &string_value[0])
 
         else:
-            raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\
+            raise Exception('AcadosOcpSolverCython.options_set() does not support field {}.'\
                 '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
 
 
diff --git a/pyextra/acados_template/acados_sim.py b/pyextra/acados_template/acados_sim.py
index d7ad1487dc..c63cffc316 100644
--- a/pyextra/acados_template/acados_sim.py
+++ b/pyextra/acados_template/acados_sim.py
@@ -70,28 +70,28 @@ class AcadosSimDims:
 
     @nx.setter
     def nx(self, nx):
-        if type(nx) == int and nx > 0:
+        if isinstance(nx, int) and nx > 0:
             self.__nx = nx
         else:
             raise Exception('Invalid nx value, expected positive integer. Exiting.')
 
     @nz.setter
     def nz(self, nz):
-        if type(nz) == int and nz > -1:
+        if isinstance(nz, int) and nz > -1:
             self.__nz = nz
         else:
             raise Exception('Invalid nz value, expected nonnegative integer. Exiting.')
 
     @nu.setter
     def nu(self, nu):
-        if type(nu) == int and nu > -1:
+        if isinstance(nu, int) and nu > -1:
             self.__nu = nu
         else:
             raise Exception('Invalid nu value, expected nonnegative integer. Exiting.')
 
     @np.setter
     def np(self, np):
-        if type(np) == int and np > -1:
+        if isinstance(np, int) and np > -1:
             self.__np = np
         else:
             raise Exception('Invalid np value, expected nonnegative integer. Exiting.')
@@ -302,6 +302,7 @@ class AcadosSim:
         self.code_export_directory = 'c_generated_code'
         """Path to where code will be exported. Default: `c_generated_code`."""
 
+        self.cython_include_dirs = ''
         self.__parameter_values = np.array([])
 
     @property
diff --git a/pyextra/acados_template/acados_sim_solver.py b/pyextra/acados_template/acados_sim_solver.py
index 145f90293e..a12ecb1765 100644
--- a/pyextra/acados_template/acados_sim_solver.py
+++ b/pyextra/acados_template/acados_sim_solver.py
@@ -215,6 +215,24 @@ class AcadosSimSolver:
         model_name = self.sim_struct.model.name
         self.model_name = model_name
 
+        # Load acados library to avoid unloading the library.
+        # This is necessary if acados was compiled with OpenMP, since the OpenMP threads can't be destroyed.
+        # Unloading a library which uses OpenMP results in a segfault (on any platform?).
+        # see [https://stackoverflow.com/questions/34439956/vc-crash-when-freeing-a-dll-built-with-openmp]
+        # or [https://python.hotexamples.com/examples/_ctypes/-/dlclose/python-dlclose-function-examples.html]
+        libacados_name = 'libacados.so'
+        libacados_filepath = os.path.join(acados_sim.acados_lib_path, libacados_name)
+        self.__acados_lib = CDLL(libacados_filepath)
+        # find out if acados was compiled with OpenMP
+        try:
+            self.__acados_lib_uses_omp = getattr(self.__acados_lib, 'omp_get_thread_num') is not None
+        except AttributeError as e:
+            self.__acados_lib_uses_omp = False
+        if self.__acados_lib_uses_omp:
+            print('acados was compiled with OpenMP.')
+        else:
+            print('acados was compiled without OpenMP.')
+
         # Ctypes
         shared_lib = f'{code_export_dir}/libacados_sim_solver_{model_name}.so'
         self.shared_lib = CDLL(shared_lib)
diff --git a/pyextra/acados_template/acados_solver_common.pxd b/pyextra/acados_template/acados_solver_common.pxd
index 9314802e61..fedd7190d9 100644
--- a/pyextra/acados_template/acados_solver_common.pxd
+++ b/pyextra/acados_template/acados_solver_common.pxd
@@ -95,6 +95,7 @@ cdef extern from "acados_c/ocp_nlp_interface.h":
 
     # solver
     void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out)
+    void ocp_nlp_eval_param_sens(ocp_nlp_solver *solver, char *field, int stage, int index, ocp_nlp_out *sens_nlp_out)
     void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in_, ocp_nlp_out *nlp_out)
 
     # get/set
diff --git a/pyextra/acados_template/c_templates_tera/Makefile.in b/pyextra/acados_template/c_templates_tera/Makefile.in
index 487e66ab07..d45be0a9c7 100644
--- a/pyextra/acados_template/c_templates_tera/Makefile.in
+++ b/pyextra/acados_template/c_templates_tera/Makefile.in
@@ -125,134 +125,134 @@
 	{%- endif %}
 {%- endif %}
 
-{# acados flags #}
-ACADOS_FLAGS = -fPIC -std=c99 {{ openmp_flag }} #-fno-diagnostics-show-line-numbers -g
-{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
-ACADOS_FLAGS += -DACADOS_WITH_QPOASES
-{%- endif %}
-{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %}
-ACADOS_FLAGS += -DACADOS_WITH_OSQP
-{%- endif %}
-{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" %}
-ACADOS_FLAGS += -DACADOS_WITH_QPDUNES
-{%- endif %}
-# # Debugging
-# ACADOS_FLAGS += -g3
+# define sources and use make's implicit rules to generate object files (*.o)
 
-MODEL_OBJ=
+# model
+MODEL_SRC=
 {%- if  solver_options.integrator_type == "ERK" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.o
-{%- if hessian_approx == "EXACT" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.o
-{%- endif %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.c
+	{%- if hessian_approx == "EXACT" %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.c
+	{%- endif %}
 {%- elif solver_options.integrator_type == "IRK" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.o
-{%- if hessian_approx == "EXACT" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.o
-{%- endif %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c
+	{%- if hessian_approx == "EXACT" %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
+	{%- endif %}
 {%- elif solver_options.integrator_type == "LIFTED_IRK" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.o
-{%- if hessian_approx == "EXACT" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.o
-{%- endif %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.c
+	{%- if hessian_approx == "EXACT" %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
+	{%- endif %}
 {%- elif solver_options.integrator_type == "GNSF" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.o
+	{% if model.gnsf.purely_linear != 1 %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c
+		{% if model.gnsf.nontrivial_f_LO == 1 %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c
+		{%- endif %}
+	{%- endif %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c
 {%- elif solver_options.integrator_type == "DISCRETE" %}
-{%- if model.dyn_ext_fun_type == "casadi" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.o
-{%- if hessian_approx == "EXACT" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.o
-{%- endif %}
-{%- else %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.dyn_source_discrete }}
-{%- endif %}
+	{%- if model.dyn_ext_fun_type == "casadi" %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c
+		{%- if hessian_approx == "EXACT" %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c
+		{%- endif %}
+	{%- else %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.dyn_source_discrete }}
+	{%- endif %}
 {%- endif %}
+MODEL_OBJ := $(MODEL_SRC:.c=.o)
 
-
-OCP_OBJ=
+# optimal control problem - mostly CasADi exports
+OCP_SRC=
 {%- if constr_type == "BGP" and dims_nphi > 0 %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_phi_constraint.o
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_phi_constraint.c
 {%- endif %}
 {%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.o
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.c
 {%- endif %}
 
 {%- if constr_type == "BGH" and dims_nh > 0 %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.o
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun.o
-{%- if hessian_approx == "EXACT" %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.o
-{%- endif %}
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c
+	{%- if hessian_approx == "EXACT" %}
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c
+	{%- endif %}
 {%- endif %}
 
 {%- if constr_type_e == "BGH" and dims_nh_e > 0 %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.o
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.o
-{%- if hessian_approx == "EXACT" %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.o
-{%- endif %}
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c
+	{%- if hessian_approx == "EXACT" %}
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c
+	{%- endif %}
 {%- endif %}
 
 {%- if cost_type_0 == "NONLINEAR_LS" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c
 {%- elif cost_type_0 == "EXTERNAL" %}
-{% if cost.cost_ext_fun_type_0 == "casadi" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c
-{% else %}
-OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }}
-{% endif %}
+	{%- if cost.cost_ext_fun_type_0 == "casadi" %}
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c
+	{%- else %}
+OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }}
+	{%- endif %}
 {%- endif %}
 {%- if cost_type == "NONLINEAR_LS" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c
 {%- elif cost_type == "EXTERNAL" %}
-{% if cost.cost_ext_fun_type == "casadi" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c
-{% elif cost.cost_source_ext_cost != cost.cost_source_ext_cost_0 %}
-OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }}
-{% endif %}
+	{%- if cost.cost_ext_fun_type == "casadi" %}
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c
+	{%- elif cost.cost_source_ext_cost != cost.cost_source_ext_cost_0 %}
+OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }}
+	{%- endif %}
 {%- endif %}
 {%- if cost_type_e == "NONLINEAR_LS" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c
 {%- elif cost_type_e == "EXTERNAL" %}
-{% if cost.cost_ext_fun_type_e == "casadi" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c
-{% elif cost.cost_source_ext_cost_e != cost.cost_source_ext_cost_0 %}
-OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }}
-{% endif %}
+	{%- if cost.cost_ext_fun_type_e == "casadi" %}
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c
+	{%- elif cost.cost_source_ext_cost_e != cost.cost_source_ext_cost_0 %}
+OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }}
+	{%- endif %}
 {%- endif %}
-OCP_OBJ+= acados_solver_{{ model.name }}.o
+OCP_SRC+= acados_solver_{{ model.name }}.c
+OCP_OBJ := $(OCP_SRC:.c=.o)
 
+# for sim solver
+SIM_SRC= acados_sim_solver_{{ model.name }}.c
+SIM_OBJ := $(SIM_SRC:.c=.o)
 
-SIM_OBJ=
-SIM_OBJ+= acados_sim_solver_{{ model.name }}.o
+# for target example
+EX_SRC= main_{{ model.name }}.c
+EX_OBJ := $(EX_SRC:.c=.o)
+EX_EXE := $(EX_SRC:.c=)
 
-EX_OBJ=
-EX_OBJ+= main_{{ model.name }}.o
-
-EX_SIM_OBJ=
-EX_SIM_OBJ+= main_sim_{{ model.name }}.o
+# for target example_sim
+EX_SIM_SRC= main_sim_{{ model.name }}.c
+EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o)
+EX_SIM_EXE := $(EX_SIM_SRC:.c=)
 
+# combine model, sim and ocp object files
 OBJ=
 OBJ+= $(MODEL_OBJ)
 {%- if solver_options.integrator_type != "DISCRETE" %}
@@ -271,233 +271,103 @@ EXTERNAL_LIB+= {{ model_external_shared_lib_name }}
 INCLUDE_PATH = {{ acados_include_path }}
 LIB_PATH = {{ acados_lib_path }}
 
-{%- if solver_options.integrator_type == "DISCRETE" %}
-all: clean casadi_fun example
-shared_lib: ocp_shared_lib
-{%- else %}
-all: clean casadi_fun example_sim example
-shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
-{%- endif %}
-
-CASADI_MODEL_SOURCE=
-{%- if solver_options.integrator_type == "ERK" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_fun.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_expl_vde_forw.c
-{%- if hessian_approx == "EXACT" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_hess.c
-{%- endif %}
-{%- elif solver_options.integrator_type == "IRK" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_jac_x_xdot_u_z.c
-{%- if hessian_approx == "EXACT" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_hess.c
-{%- endif %}
-{%- elif solver_options.integrator_type == "LIFTED_IRK" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun.c
-# CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
-# CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_jac_x_xdot_u_z.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_u.c
-{%- if hessian_approx == "EXACT" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_hess.c
-{%- endif %}
-{%- elif solver_options.integrator_type == "GNSF" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_fun.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_fun_jac_y.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_jac_y_uhat.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_get_matrices_fun.c
-{%- elif solver_options.integrator_type == "DISCRETE" and model.dyn_ext_fun_type == "casadi" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun_jac.c
-{%- if hessian_approx == "EXACT" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun_jac_hess.c
-{%- endif %}
-{%- endif %}
-{%- if constr_type == "BGP" and dims_nphi > 0 %}
-CASADI_CON_PHI_SOURCE=
-CASADI_CON_PHI_SOURCE+= {{ model.name }}_phi_constraint.c
-{%- endif %}
-{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
-CASADI_CON_PHI_E_SOURCE=
-CASADI_CON_PHI_E_SOURCE+= {{ model.name }}_phi_e_constraint.c
-{%- endif %}
-{%- if constr_type == "BGH" and dims_nh > 0 %}
-CASADI_CON_H_SOURCE=
-CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun_jac_uxt_zt.c
-CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun.c
-{%- if hessian_approx == "EXACT" %}
-CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c
-{%- endif %}
-{%- endif %}
-
-{%- if dims_nh_e > 0 %}
-CASADI_CON_H_E_SOURCE=
-CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun_jac_uxt_zt.c
-CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun.c
-{%- if hessian_approx == "EXACT" %}
-CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c
-{%- endif %}
-{%- endif %}
-
-{%- if cost_type == "NONLINEAR_LS" %}
-CASADI_COST_Y_SOURCE=
-CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_fun.c
-CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_fun_jac_ut_xt.c
-CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_hess.c
-{%- endif %}
-{%- if cost_type_e == "NONLINEAR_LS" %}
-CASADI_COST_Y_E_SOURCE=
-CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_fun.c
-CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_fun_jac_ut_xt.c
-CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_hess.c
+# preprocessor flags for make's implicit rules
+{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
+CPPFLAGS += -DACADOS_WITH_QPOASES
 {%- endif %}
-{%- if cost_type_0 == "NONLINEAR_LS" %}
-CASADI_COST_Y_0_SOURCE=
-CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_fun.c
-CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_fun_jac_ut_xt.c
-CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_hess.c
+{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %}
+CPPFLAGS += -DACADOS_WITH_OSQP
 {%- endif %}
+{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" %}
+CPPFLAGS += -DACADOS_WITH_QPDUNES
+{%- endif %}
+CPPFLAGS+= -I$(INCLUDE_PATH)
+CPPFLAGS+= -I$(INCLUDE_PATH)/acados
+CPPFLAGS+= -I$(INCLUDE_PATH)/blasfeo/include
+CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include
+ {%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
+CPPFLAGS+= -I $(INCLUDE_PATH)/qpOASES_e/
+ {%- endif %}
+
+{# c-compiler flags #}
+# define the c-compiler flags for make's implicit rules
+CFLAGS = -fPIC -std=c99 {{ openmp_flag }} #-fno-diagnostics-show-line-numbers -g
+# # Debugging
+# CFLAGS += -g3
 
-casadi_fun:
-	{%- if model.dyn_ext_fun_type == "casadi" %}
-	( cd {{ model.name }}_model {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_MODEL_SOURCE))
-	{%- endif %}
-	{%- if constr_type == "BGP" and dims_nphi > 0 %}
-	( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_PHI_SOURCE))
-	{%- endif %}
-	{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
-	( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_PHI_E_SOURCE))
-	{%- endif %}
-	{%- if constr_type == "BGH" and dims_nh > 0 %}
-	( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_H_SOURCE))
-	{%- endif %}
-	{%- if constr_type_e == "BGH" and dims_nh_e > 0 %}
-	( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_H_E_SOURCE))
-	{%- endif %}
-	{%- if cost_type == "NONLINEAR_LS" %}
-	( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_SOURCE))
-	{%- endif %}
-	{%- if cost_type_e == "NONLINEAR_LS" %}
-	( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_E_SOURCE))
-	{%- endif %}
-	{%- if cost_type_0 == "NONLINEAR_LS" %}
-	( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_0_SOURCE))
-	{%- endif %}
-
-main:
-	gcc $(ACADOS_FLAGS) -c main_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
-	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
-	-I $(INCLUDE_PATH)/qpOASES_e/
-	{%- endif %}
+# linker flags
+LDFLAGS+= -L$(LIB_PATH)
 
-main_sim:
-	gcc $(ACADOS_FLAGS) -c main_sim_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/
+# link to libraries
+LDLIBS+= -lacados
+LDLIBS+= -lhpipm
+LDLIBS+= -lblasfeo
+LDLIBS+= -lm
+LDLIBS+= {{ link_libs }}
 
-ocp_solver:
-	gcc $(ACADOS_FLAGS) -c acados_solver_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
-	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
-	-I $(INCLUDE_PATH)/qpOASES_e/
-	{%- endif %}
+# libraries
+LIBACADOS_SOLVER=libacados_solver_{{ model.name }}.so
+LIBACADOS_OCP_SOLVER=libacados_ocp_solver_{{ model.name }}.so
+LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so)
 
-sim_solver:
-	gcc $(ACADOS_FLAGS) -c acados_sim_solver_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
-	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
-	-I $(INCLUDE_PATH)/qpOASES_e/
-	{%- endif %}
+# virtual targets
+.PHONY : all clean
 
-example: ocp_solver main
-	gcc $(ACADOS_FLAGS) -o main_{{ model.name }} $(EX_OBJ) $(OBJ) -L $(LIB_PATH) \
-	-lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
-	-I $(INCLUDE_PATH)/blasfeo/include/ \
-	-I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) \
-	-I $(INCLUDE_PATH)/acados/ \
-	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
-	-I $(INCLUDE_PATH)/qpOASES_e/
-	{%- endif %}
-
-
-example_sim: sim_solver main_sim
-	gcc $(ACADOS_FLAGS) -o main_sim_{{ model.name }} $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ) -L $(LIB_PATH) \
-	-lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
-	-I $(INCLUDE_PATH)/blasfeo/include/ \
-	-I $(INCLUDE_PATH)/acados/ \
+#all: clean example_sim example shared_lib
+{% if solver_options.integrator_type == "DISCRETE" -%}
+all: clean example
+shared_lib: ocp_shared_lib
+{%- else %}
+all: clean example_sim example
+shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
+{%- endif %}
 
-{%- if solver_options.integrator_type != "DISCRETE" %}
+# some linker targets
+example: $(EX_OBJ) $(OBJ)
+	$(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS)
 
-bundled_shared_lib: casadi_fun ocp_solver sim_solver
-	gcc $(ACADOS_FLAGS) -shared -o libacados_solver_{{ model.name }}.so $(OBJ) \
-	-I $(INCLUDE_PATH)/blasfeo/include/ \
-	-I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) \
-	-L $(LIB_PATH) \
-	-lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
+example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ)
+	$(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS)
 
-ocp_shared_lib: casadi_fun ocp_solver
-	gcc $(ACADOS_FLAGS) -shared -o libacados_ocp_solver_{{ model.name }}.so $(OCP_OBJ) $(MODEL_OBJ) \
-	-I $(INCLUDE_PATH)/blasfeo/include/ \
-	-I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) \
-	-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
-	-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
+{% if solver_options.integrator_type != "DISCRETE" -%}
+bundled_shared_lib: $(OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS)
+{%- endif %}
 
-{%- else %}
+ocp_shared_lib: $(OCP_OBJ) $(MODEL_OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_OCP_SOLVER) $(LDFLAGS) $(LDLIBS) \
+	-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB)
 
-ocp_shared_lib: casadi_fun ocp_solver
-	gcc $(ACADOS_FLAGS) -shared -o libacados_ocp_solver_{{ model.name }}.so $(OCP_OBJ) $(MODEL_OBJ) \
-	-I $(INCLUDE_PATH)/blasfeo/include/ \
-	-I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) \
-	-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
-	-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
+sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS)
 
-{%- endif %}
 
+# Cython targets
 ocp_cython_c: ocp_shared_lib
 	cython \
 	-o acados_ocp_solver_pyx.c \
 	-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
 	$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \
+	-I {{ code_export_directory }} \
 
 ocp_cython_o: ocp_cython_c
-	clang $(ACADOS_FLAGS) -c -O2 \
+	$(CC) $(ACADOS_FLAGS) -c -O2 \
+	-fPIC \
 	-o acados_ocp_solver_pyx.o \
 	-I /usr/include/python3.8 \
 	-I $(INCLUDE_PATH)/blasfeo/include/ \
 	-I $(INCLUDE_PATH)/hpipm/include/ \
 	-I $(INCLUDE_PATH) \
+	-I {{ cython_include_dirs }} \
 	acados_ocp_solver_pyx.c \
 
 ocp_cython: ocp_cython_o
-	clang $(ACADOS_FLAGS) -shared \
+	$(CC) $(ACADOS_FLAGS) -shared \
 	-o acados_ocp_solver_pyx.so \
 	-Wl,-rpath=$(LIB_PATH) \
 	acados_ocp_solver_pyx.o \
 	$(abspath .)/libacados_ocp_solver_{{ model.name }}.so \
-	-L $(LIB_PATH) -lacados -lhpipm -lblasfeo -lqpOASES_e \
-	{{ link_libs }} \
-	-lm \
-
-sim_shared_lib: casadi_fun sim_solver
-	gcc $(ACADOS_FLAGS) -shared -o libacados_sim_solver_{{ model.name }}.so $(SIM_OBJ) $(MODEL_OBJ) -L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
-	-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
+	$(LDFLAGS) $(LDLIBS)
 
 {%- if os and os == "pc" %}
 
@@ -510,15 +380,27 @@ clean_ocp_shared_lib:
 	del \Q libacados_ocp_solver_{{ model.name }}.so 2>nul
 	del \Q acados_solver_{{ model.name }}.o 2>nul
 
+clean_ocp_cython:
+	del \Q libacados_ocp_solver_{{ model.name }}.so 2>nul
+	del \Q acados_solver_{{ model.name }}.o 2>nul
+	del \Q acados_ocp_solver_pyx.so 2>nul
+	del \Q acados_ocp_solver_pyx.o 2>nul
+
 {%- else %}
 
 clean:
-	rm -f *.o
-	rm -f *.so
-	rm -f main_{{ model.name }}
+	$(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ)
+	$(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER)
+	$(RM) $(EX_EXE) $(EX_SIM_EXE)
 
 clean_ocp_shared_lib:
-	rm -f libacados_ocp_solver_{{ model.name }}.so
-	rm -f acados_solver_{{ model.name }}.o
+	$(RM) $(LIBACADOS_OCP_SOLVER)
+	$(RM) $(OCP_OBJ)
+
+clean_ocp_cython:
+	$(RM) libacados_ocp_solver_{{ model.name }}.so
+	$(RM) acados_solver_{{ model.name }}.o
+	$(RM) acados_ocp_solver_pyx.so
+	$(RM) acados_ocp_solver_pyx.o
 
 {%- endif %}
diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c
index 251b249b2f..e67a51567a 100644
--- a/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c
@@ -63,7 +63,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     mexPrintf("{{ model.name }}_acados_create() -> success!\n");
 
     // get pointers to nlp solver related objects
-    ocp_nlp_plan *nlp_plan = {{ model.name }}_acados_get_nlp_plan(acados_ocp_capsule);
+    ocp_nlp_plan_t *nlp_plan = {{ model.name }}_acados_get_nlp_plan(acados_ocp_capsule);
     ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(acados_ocp_capsule);
     ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(acados_ocp_capsule);
     ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(acados_ocp_capsule);
@@ -238,14 +238,18 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_hess;
 {%- endif %}
 {% elif solver_options.integrator_type == "GNSF" %}
+    {% if model.gnsf.purely_linear != 1 %}
     l_ptr = mxGetData(gnsf_phi_fun_mat);
     l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun;
     l_ptr = mxGetData(gnsf_phi_fun_jac_y_mat);
     l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun_jac_y;
     l_ptr = mxGetData(gnsf_phi_jac_y_uhat_mat);
     l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_jac_y_uhat;
+    {% if model.gnsf.nontrivial_f_LO == 1 %}
     l_ptr = mxGetData(gnsf_f_lo_jac_x1_x1dot_u_z_mat);
     l_ptr[0] = (long long) acados_ocp_capsule->gnsf_f_lo_jac_x1_x1dot_u_z;
+    {%- endif %}
+    {%- endif %}
     l_ptr = mxGetData(gnsf_get_matrices_fun_mat);
     l_ptr[0] = (long long) acados_ocp_capsule->gnsf_get_matrices_fun;
 {% elif solver_options.integrator_type == "DISCRETE" %}
diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c
index ae8c311557..f8e1e5e445 100644
--- a/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c
@@ -69,7 +69,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     {{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
     // plan
     ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "plan" ) );
-    ocp_nlp_plan *plan = (ocp_nlp_plan *) ptr[0];
+    ocp_nlp_plan_t *plan = (ocp_nlp_plan_t *) ptr[0];
     // config
     ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "config" ) );
     ocp_nlp_config *config = (ocp_nlp_config *) ptr[0];
@@ -404,7 +404,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     }
     else if (!strcmp(field, "init_z"))
     {
-        sim_solver_plan sim_plan = plan->sim_solver_plan[0];
+        sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
         sim_solver_t type = sim_plan.sim_solver;
         if (type == IRK)
         {
@@ -426,7 +426,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     }
     else if (!strcmp(field, "init_xdot"))
     {
-        sim_solver_plan sim_plan = plan->sim_solver_plan[0];
+        sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
         sim_solver_t type = sim_plan.sim_solver;
         if (type == IRK)
         {
@@ -448,7 +448,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     }
     else if (!strcmp(field, "init_gnsf_phi"))
     {
-        sim_solver_plan sim_plan = plan->sim_solver_plan[0];
+        sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
         sim_solver_t type = sim_plan.sim_solver;
         if (type == GNSF)
         {
diff --git a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c
index 971fb8a78f..f2e75058c1 100644
--- a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c
@@ -164,12 +164,17 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
 {%- endif %}
 
     {% elif solver_options.integrator_type == "GNSF" -%}
+  {% if model.gnsf.purely_linear != 1 %}
     capsule->sim_gnsf_phi_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
     capsule->sim_gnsf_phi_fun_jac_y = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
     capsule->sim_gnsf_phi_jac_y_uhat = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+  {% if model.gnsf.nontrivial_f_LO == 1 %}
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+  {%- endif %}
+  {%- endif %}
     capsule->sim_gnsf_get_matrices_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
 
+  {% if model.gnsf.purely_linear != 1 %}
     capsule->sim_gnsf_phi_fun->casadi_fun = &{{ model.name }}_gnsf_phi_fun;
     capsule->sim_gnsf_phi_fun->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_n_in;
     capsule->sim_gnsf_phi_fun->casadi_n_out = &{{ model.name }}_gnsf_phi_fun_n_out;
@@ -194,6 +199,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     capsule->sim_gnsf_phi_jac_y_uhat->casadi_work = &{{ model.name }}_gnsf_phi_jac_y_uhat_work;
     external_function_param_casadi_create(capsule->sim_gnsf_phi_jac_y_uhat, np);
 
+  {% if model.gnsf.nontrivial_f_LO == 1 %}
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_fun = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz;
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in;
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out;
@@ -201,6 +207,8 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out;
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_work = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work;
     external_function_param_casadi_create(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, np);
+  {%- endif %}
+  {%- endif %}
 
     capsule->sim_gnsf_get_matrices_fun->casadi_fun = &{{ model.name }}_gnsf_get_matrices_fun;
     capsule->sim_gnsf_get_matrices_fun->casadi_n_in = &{{ model.name }}_gnsf_get_matrices_fun_n_in;
@@ -212,7 +220,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     {% endif %}
 
     // sim plan & config
-    sim_solver_plan plan;
+    sim_solver_plan_t plan;
     plan.sim_solver = {{ solver_options.integrator_type }};
 
     // create correct config based on plan
@@ -307,14 +315,18 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
                 "expl_ode_hess", capsule->sim_expl_ode_hess);
 {%- endif %}
 {%- elif solver_options.integrator_type == "GNSF" %}
+  {% if model.gnsf.purely_linear != 1 %}
     {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
                  "phi_fun", capsule->sim_gnsf_phi_fun);
     {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
                  "phi_fun_jac_y", capsule->sim_gnsf_phi_fun_jac_y);
     {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
                  "phi_jac_y_uhat", capsule->sim_gnsf_phi_jac_y_uhat);
+  {% if model.gnsf.nontrivial_f_LO == 1 %}
     {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
                  "f_lo_jac_x1_x1dot_u_z", capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
+  {%- endif %}
+  {%- endif %}
     {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
                  "gnsf_get_matrices_fun", capsule->sim_gnsf_get_matrices_fun);
 {%- endif %}
@@ -409,10 +421,14 @@ int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule)
     external_function_param_casadi_free(capsule->sim_expl_ode_hess);
 {%- endif %}
 {%- elif solver_options.integrator_type == "GNSF" %}
+  {% if model.gnsf.purely_linear != 1 %}
     external_function_param_casadi_free(capsule->sim_gnsf_phi_fun);
     external_function_param_casadi_free(capsule->sim_gnsf_phi_fun_jac_y);
     external_function_param_casadi_free(capsule->sim_gnsf_phi_jac_y_uhat);
+  {% if model.gnsf.nontrivial_f_LO == 1 %}
     external_function_param_casadi_free(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
+  {%- endif %}
+  {%- endif %}
     external_function_param_casadi_free(capsule->sim_gnsf_get_matrices_fun);
 {% endif %}
 
@@ -445,10 +461,14 @@ int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, doubl
     capsule->sim_impl_dae_hess[0].set_param(capsule->sim_impl_dae_hess, p);
 {%- endif %}
 {%- elif solver_options.integrator_type == "GNSF" %}
+  {% if model.gnsf.purely_linear != 1 %}
     capsule->sim_gnsf_phi_fun[0].set_param(capsule->sim_gnsf_phi_fun, p);
     capsule->sim_gnsf_phi_fun_jac_y[0].set_param(capsule->sim_gnsf_phi_fun_jac_y, p);
     capsule->sim_gnsf_phi_jac_y_uhat[0].set_param(capsule->sim_gnsf_phi_jac_y_uhat, p);
+  {% if model.gnsf.nontrivial_f_LO == 1 %}
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z[0].set_param(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, p);
+  {%- endif %}
+  {%- endif %}
     capsule->sim_gnsf_get_matrices_fun[0].set_param(capsule->sim_gnsf_get_matrices_fun, p);
 {% endif %}
 
diff --git a/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c b/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c
index 234295fa29..68a6a3f80f 100644
--- a/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c
@@ -37,7 +37,7 @@
 #define MDL_START
 
 // acados
-#include "acados/utils/print.h"
+// #include "acados/utils/print.h"
 #include "acados_c/ocp_nlp_interface.h"
 #include "acados_c/external_function_interface.h"
 
diff --git a/pyextra/acados_template/c_templates_tera/acados_solver.in.c b/pyextra/acados_template/c_templates_tera/acados_solver.in.c
index 6e13242a41..a42937ed28 100644
--- a/pyextra/acados_template/c_templates_tera/acados_solver.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_solver.in.c
@@ -34,8 +34,9 @@
 // standard
 #include <stdio.h>
 #include <stdlib.h>
+#include <assert.h>
 // acados
-#include "acados/utils/print.h"
+// #include "acados/utils/print.h"
 #include "acados_c/ocp_nlp_interface.h"
 #include "acados_c/external_function_interface.h"
 
@@ -121,14 +122,15 @@ int {{ model.name }}_acados_free_capsule({{ model.name }}_solver_capsule *capsul
 }
 
 
-int {{ model.name }}_acados_create({{ model.name }}_solver_capsule * capsule)
+int {{ model.name }}_acados_create({{ model.name }}_solver_capsule* capsule)
 {
     int N_shooting_intervals = {{ model.name | upper }}_N;
     double* new_time_steps = NULL; // NULL -> don't alter the code generated time-steps
     return {{ model.name }}_acados_create_with_discretization(capsule, N_shooting_intervals, new_time_steps);
 }
 
-int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps)
+
+int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule* capsule, int N, double* new_time_steps)
 {
     if (N != capsule->nlp_solver_plan->N) {
         fprintf(stderr, "{{ model.name }}_acados_update_time_steps: given number of time steps (= %d) " \
@@ -151,30 +153,20 @@ int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule *
     return 0;
 }
 
-int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps)
+/**
+ * Internal function for {{ model.name }}_acados_create: step 1
+ */
+void {{ model.name }}_acados_create_1_set_plan(ocp_nlp_plan_t* nlp_solver_plan, const int N)
 {
-    int status = 0;
-    // If N does not match the number of shooting intervals used for code generation, new_time_steps must be given.
-    if (N != {{ model.name | upper }}_N && !new_time_steps) {
-        fprintf(stderr, "{{ model.name }}_acados_create_with_discretization: new_time_steps is NULL " \
-            "but the number of shooting intervals (= %d) differs from the number of " \
-            "shooting intervals (= %d) during code generation! Please provide a new vector of time_stamps!\n", \
-             N, {{ model.name | upper }}_N);
-        return 1;
-    }
-
-    // number of expected runtime parameters
-    capsule->nlp_np = NP;
+    assert(N == nlp_solver_plan->N);
 
     /************************************************
-    *  plan & config
+    *  plan
     ************************************************/
-    ocp_nlp_plan * nlp_solver_plan = ocp_nlp_plan_create(N);
-    capsule->nlp_solver_plan = nlp_solver_plan;
 
     {%- if solver_options.nlp_solver_type == "SQP" %}
     nlp_solver_plan->nlp_solver = SQP;
-    {% else %}
+    {%- else %}
     nlp_solver_plan->nlp_solver = SQP_RTI;
     {%- endif %}
 
@@ -188,11 +180,11 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
 
     for (int i = 0; i < N; i++)
     {
-        {% if solver_options.integrator_type == "DISCRETE" %}
+        {%- if solver_options.integrator_type == "DISCRETE" %}
         nlp_solver_plan->nlp_dynamics[i] = DISCRETE_MODEL;
         // discrete dynamics does not need sim solver option, this field is ignored
         nlp_solver_plan->sim_solver_plan[i].sim_solver = INVALID_SIM_SOLVER;
-        {% else %}
+        {%- else %}
         nlp_solver_plan->nlp_dynamics[i] = CONTINUOUS_MODEL;
         nlp_solver_plan->sim_solver_plan[i].sim_solver = {{ solver_options.integrator_type }};
         {%- endif %}
@@ -200,7 +192,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
 
     for (int i = 0; i < N; i++)
     {
-        {% if constraints.constr_type == "BGP" %}
+        {%- if constraints.constr_type == "BGP" %}
         nlp_solver_plan->nlp_constraints[i] = BGP;
         {%- else -%}
         nlp_solver_plan->nlp_constraints[i] = BGH;
@@ -209,7 +201,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
 
     {%- if constraints.constr_type_e == "BGP" %}
     nlp_solver_plan->nlp_constraints[N] = BGP;
-    {% else %}
+    {%- else %}
     nlp_solver_plan->nlp_constraints[N] = BGH;
     {%- endif %}
 
@@ -226,10 +218,18 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     nlp_solver_plan->regularization = CONVEXIFY;
     {%- endif %}
 {%- endif %}
-    ocp_nlp_config * nlp_config = ocp_nlp_config_create(*nlp_solver_plan);
-    capsule->nlp_config = nlp_config;
+}
 
 
+/**
+ * Internal function for {{ model.name }}_acados_create: step 2
+ */
+ocp_nlp_dims* {{ model.name }}_acados_create_2_create_and_set_dimensions({{ model.name }}_solver_capsule* capsule)
+{
+    ocp_nlp_plan_t* nlp_solver_plan = capsule->nlp_solver_plan;
+    const int N = nlp_solver_plan->N;
+    ocp_nlp_config* nlp_config = capsule->nlp_config;
+
     /************************************************
     *  dimensions
     ************************************************/
@@ -307,7 +307,6 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
 
     /* create and set ocp_nlp_dims */
     ocp_nlp_dims * nlp_dims = ocp_nlp_dims_create(nlp_config);
-    capsule->nlp_dims = nlp_dims;
 
     ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nx", nx);
     ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nu", nu);
@@ -325,14 +324,14 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
         ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbxe", &nbxe[i]);
     }
 
-    {%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %}
+{%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %}
     ocp_nlp_dims_set_cost(nlp_config, nlp_dims, 0, "ny", &ny[0]);
-    {%- endif %}
+{%- endif %}
 
-    {%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %}
+{%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %}
     for (int i = 1; i < N; i++)
         ocp_nlp_dims_set_cost(nlp_config, nlp_dims, i, "ny", &ny[i]);
-    {%- endif %}
+{%- endif %}
 
     for (int i = 0; i < N; i++)
     {
@@ -346,21 +345,20 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
         {%- endif %}
     }
 
-    {%- if constraints.constr_type_e == "BGH" %}
+{%- if constraints.constr_type_e == "BGH" %}
     ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nh", &nh[N]);
     ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsh", &nsh[N]);
-    {%- elif constraints.constr_type_e == "BGP" %}
+{%- elif constraints.constr_type_e == "BGP" %}
     ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nr", &nr[N]);
     ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nphi", &nphi[N]);
     ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsphi", &nsphi[N]);
-    {%- endif %}
-    {%- if cost.cost_type_e == "NONLINEAR_LS" or cost.cost_type_e == "LINEAR_LS" %}
+{%- endif %}
+{%- if cost.cost_type_e == "NONLINEAR_LS" or cost.cost_type_e == "LINEAR_LS" %}
     ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]);
-    {%- endif %}
-
+{%- endif %}
     free(intNp1mem);
 
-{% if solver_options.integrator_type == "GNSF" -%}
+{%- if solver_options.integrator_type == "GNSF" -%}
     // GNSF specific dimensions
     int gnsf_nx1 = {{ dims.gnsf_nx1 }};
     int gnsf_nz1 = {{ dims.gnsf_nz1 }};
@@ -380,136 +378,91 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
         }
     }
 {%- endif %}
+return nlp_dims;
+}
+
+
+/**
+ * Internal function for {{ model.name }}_acados_create: step 3
+ */
+void {{ model.name }}_acados_create_3_create_and_set_functions({{ model.name }}_solver_capsule* capsule)
+{
+    const int N = capsule->nlp_solver_plan->N;
+    ocp_nlp_config* nlp_config = capsule->nlp_config;
 
     /************************************************
     *  external functions
     ************************************************/
-    {%- if constraints.constr_type == "BGP" %}
+
+#define MAP_CASADI_FNC(__CAPSULE_FNC__, __MODEL_BASE_FNC__) do{ \
+        capsule->__CAPSULE_FNC__.casadi_fun = & __MODEL_BASE_FNC__ ;\
+        capsule->__CAPSULE_FNC__.casadi_n_in = & __MODEL_BASE_FNC__ ## _n_in; \
+        capsule->__CAPSULE_FNC__.casadi_n_out = & __MODEL_BASE_FNC__ ## _n_out; \
+        capsule->__CAPSULE_FNC__.casadi_sparsity_in = & __MODEL_BASE_FNC__ ## _sparsity_in; \
+        capsule->__CAPSULE_FNC__.casadi_sparsity_out = & __MODEL_BASE_FNC__ ## _sparsity_out; \
+        capsule->__CAPSULE_FNC__.casadi_work = & __MODEL_BASE_FNC__ ## _work; \
+        external_function_param_casadi_create(&capsule->__CAPSULE_FNC__ , {{ dims.np }}); \
+    }while(false)
+
+{% if constraints.constr_type == "BGP" %}
+    // constraints.constr_type == "BGP"
     capsule->phi_constraint = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++)
     {
         // nonlinear part of convex-composite constraint
-        capsule->phi_constraint[i].casadi_fun = &{{ model.name }}_phi_constraint;
-        capsule->phi_constraint[i].casadi_n_in = &{{ model.name }}_phi_constraint_n_in;
-        capsule->phi_constraint[i].casadi_n_out = &{{ model.name }}_phi_constraint_n_out;
-        capsule->phi_constraint[i].casadi_sparsity_in = &{{ model.name }}_phi_constraint_sparsity_in;
-        capsule->phi_constraint[i].casadi_sparsity_out = &{{ model.name }}_phi_constraint_sparsity_out;
-        capsule->phi_constraint[i].casadi_work = &{{ model.name }}_phi_constraint_work;
-
-        external_function_param_casadi_create(&capsule->phi_constraint[i], {{ dims.np }});
+        MAP_CASADI_FNC(phi_constraint[i], {{ model.name }}_phi_constraint);
     }
-    {%- endif %}
+{%- endif %}
 
-    {%- if constraints.constr_type_e == "BGP" %}
+{%- if constraints.constr_type_e == "BGP" %}
+    // constraints.constr_type_e == "BGP"
     // nonlinear part of convex-composite constraint
-    capsule->phi_e_constraint.casadi_fun = &{{ model.name }}_phi_e_constraint;
-    capsule->phi_e_constraint.casadi_n_in = &{{ model.name }}_phi_e_constraint_n_in;
-    capsule->phi_e_constraint.casadi_n_out = &{{ model.name }}_phi_e_constraint_n_out;
-    capsule->phi_e_constraint.casadi_sparsity_in = &{{ model.name }}_phi_e_constraint_sparsity_in;
-    capsule->phi_e_constraint.casadi_sparsity_out = &{{ model.name }}_phi_e_constraint_sparsity_out;
-    capsule->phi_e_constraint.casadi_work = &{{ model.name }}_phi_e_constraint_work;
-
-    external_function_param_casadi_create(&capsule->phi_e_constraint, {{ dims.np }});
-    {% endif %}
+    MAP_CASADI_FNC(phi_e_constraint, {{ model.name }}_phi_e_constraint);
+{%- endif %}
 
-    {%- if constraints.constr_type == "BGH" and dims.nh > 0  %}
+{%- if constraints.constr_type == "BGH" and dims.nh > 0  %}
+    // constraints.constr_type == "BGH" and dims.nh > 0
     capsule->nl_constr_h_fun_jac = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->nl_constr_h_fun_jac[i].casadi_fun = &{{ model.name }}_constr_h_fun_jac_uxt_zt;
-        capsule->nl_constr_h_fun_jac[i].casadi_n_in = &{{ model.name }}_constr_h_fun_jac_uxt_zt_n_in;
-        capsule->nl_constr_h_fun_jac[i].casadi_n_out = &{{ model.name }}_constr_h_fun_jac_uxt_zt_n_out;
-        capsule->nl_constr_h_fun_jac[i].casadi_sparsity_in = &{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_in;
-        capsule->nl_constr_h_fun_jac[i].casadi_sparsity_out = &{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_out;
-        capsule->nl_constr_h_fun_jac[i].casadi_work = &{{ model.name }}_constr_h_fun_jac_uxt_zt_work;
-        external_function_param_casadi_create(&capsule->nl_constr_h_fun_jac[i], {{ dims.np }});
+        MAP_CASADI_FNC(nl_constr_h_fun_jac[i], {{ model.name }}_constr_h_fun_jac_uxt_zt);
     }
     capsule->nl_constr_h_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->nl_constr_h_fun[i].casadi_fun = &{{ model.name }}_constr_h_fun;
-        capsule->nl_constr_h_fun[i].casadi_n_in = &{{ model.name }}_constr_h_fun_n_in;
-        capsule->nl_constr_h_fun[i].casadi_n_out = &{{ model.name }}_constr_h_fun_n_out;
-        capsule->nl_constr_h_fun[i].casadi_sparsity_in = &{{ model.name }}_constr_h_fun_sparsity_in;
-        capsule->nl_constr_h_fun[i].casadi_sparsity_out = &{{ model.name }}_constr_h_fun_sparsity_out;
-        capsule->nl_constr_h_fun[i].casadi_work = &{{ model.name }}_constr_h_fun_work;
-        external_function_param_casadi_create(&capsule->nl_constr_h_fun[i], {{ dims.np }});
+        MAP_CASADI_FNC(nl_constr_h_fun[i], {{ model.name }}_constr_h_fun);
     }
     {% if solver_options.hessian_approx == "EXACT" %}
     capsule->nl_constr_h_fun_jac_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->nl_constr_h_fun_jac_hess[i].casadi_fun = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess;
-        capsule->nl_constr_h_fun_jac_hess[i].casadi_n_in = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_in;
-        capsule->nl_constr_h_fun_jac_hess[i].casadi_n_out = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_out;
-        capsule->nl_constr_h_fun_jac_hess[i].casadi_sparsity_in = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_in;
-        capsule->nl_constr_h_fun_jac_hess[i].casadi_sparsity_out = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_out;
-        capsule->nl_constr_h_fun_jac_hess[i].casadi_work = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_work;
-
-        external_function_param_casadi_create(&capsule->nl_constr_h_fun_jac_hess[i], {{ dims.np }});
+        MAP_CASADI_FNC(nl_constr_h_fun_jac_hess[i], {{ model.name }}_constr_h_fun_jac_uxt_zt_hess);
     }
     {% endif %}
-    {% endif %}
+{% endif %}
 
-    {%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
-    capsule->nl_constr_h_e_fun_jac.casadi_fun = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt;
-    capsule->nl_constr_h_e_fun_jac.casadi_n_in = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_in;
-    capsule->nl_constr_h_e_fun_jac.casadi_n_out = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_out;
-    capsule->nl_constr_h_e_fun_jac.casadi_sparsity_in = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_in;
-    capsule->nl_constr_h_e_fun_jac.casadi_sparsity_out = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_out;
-    capsule->nl_constr_h_e_fun_jac.casadi_work = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_work;
-    external_function_param_casadi_create(&capsule->nl_constr_h_e_fun_jac, {{ dims.np }});
-
-    capsule->nl_constr_h_e_fun.casadi_fun = &{{ model.name }}_constr_h_e_fun;
-    capsule->nl_constr_h_e_fun.casadi_n_in = &{{ model.name }}_constr_h_e_fun_n_in;
-    capsule->nl_constr_h_e_fun.casadi_n_out = &{{ model.name }}_constr_h_e_fun_n_out;
-    capsule->nl_constr_h_e_fun.casadi_sparsity_in = &{{ model.name }}_constr_h_e_fun_sparsity_in;
-    capsule->nl_constr_h_e_fun.casadi_sparsity_out = &{{ model.name }}_constr_h_e_fun_sparsity_out;
-    capsule->nl_constr_h_e_fun.casadi_work = &{{ model.name }}_constr_h_e_fun_work;
-    external_function_param_casadi_create(&capsule->nl_constr_h_e_fun, {{ dims.np }});
+{%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
+    MAP_CASADI_FNC(nl_constr_h_e_fun_jac, {{ model.name }}_constr_h_e_fun_jac_uxt_zt);
+    MAP_CASADI_FNC(nl_constr_h_e_fun, {{ model.name }}_constr_h_e_fun);
 
-    {% if solver_options.hessian_approx == "EXACT" %}
-    capsule->nl_constr_h_e_fun_jac_hess.casadi_fun = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess;
-    capsule->nl_constr_h_e_fun_jac_hess.casadi_n_in = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_in;
-    capsule->nl_constr_h_e_fun_jac_hess.casadi_n_out = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_out;
-    capsule->nl_constr_h_e_fun_jac_hess.casadi_sparsity_in = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_in;
-    capsule->nl_constr_h_e_fun_jac_hess.casadi_sparsity_out = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_out;
-    capsule->nl_constr_h_e_fun_jac_hess.casadi_work = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_work;
-    external_function_param_casadi_create(&capsule->nl_constr_h_e_fun_jac_hess, {{ dims.np }});
+    {%- if solver_options.hessian_approx == "EXACT" %}
+    MAP_CASADI_FNC(nl_constr_h_e_fun_jac_hess, {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess);
     {% endif %}
-    {%- endif %}
+{%- endif %}
 
 {% if solver_options.integrator_type == "ERK" %}
     // explicit ode
     capsule->forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->forw_vde_casadi[i].casadi_fun = &{{ model.name }}_expl_vde_forw;
-        capsule->forw_vde_casadi[i].casadi_n_in = &{{ model.name }}_expl_vde_forw_n_in;
-        capsule->forw_vde_casadi[i].casadi_n_out = &{{ model.name }}_expl_vde_forw_n_out;
-        capsule->forw_vde_casadi[i].casadi_sparsity_in = &{{ model.name }}_expl_vde_forw_sparsity_in;
-        capsule->forw_vde_casadi[i].casadi_sparsity_out = &{{ model.name }}_expl_vde_forw_sparsity_out;
-        capsule->forw_vde_casadi[i].casadi_work = &{{ model.name }}_expl_vde_forw_work;
-        external_function_param_casadi_create(&capsule->forw_vde_casadi[i], {{ dims.np }});
+        MAP_CASADI_FNC(forw_vde_casadi[i], {{ model.name }}_expl_vde_forw);
     }
 
     capsule->expl_ode_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->expl_ode_fun[i].casadi_fun = &{{ model.name }}_expl_ode_fun;
-        capsule->expl_ode_fun[i].casadi_n_in = &{{ model.name }}_expl_ode_fun_n_in;
-        capsule->expl_ode_fun[i].casadi_n_out = &{{ model.name }}_expl_ode_fun_n_out;
-        capsule->expl_ode_fun[i].casadi_sparsity_in = &{{ model.name }}_expl_ode_fun_sparsity_in;
-        capsule->expl_ode_fun[i].casadi_sparsity_out = &{{ model.name }}_expl_ode_fun_sparsity_out;
-        capsule->expl_ode_fun[i].casadi_work = &{{ model.name }}_expl_ode_fun_work;
-        external_function_param_casadi_create(&capsule->expl_ode_fun[i], {{ dims.np }});
+        MAP_CASADI_FNC(expl_ode_fun[i], {{ model.name }}_expl_ode_fun);
     }
 
     {%- if solver_options.hessian_approx == "EXACT" %}
     capsule->hess_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->hess_vde_casadi[i].casadi_fun = &{{ model.name }}_expl_ode_hess;
-        capsule->hess_vde_casadi[i].casadi_n_in = &{{ model.name }}_expl_ode_hess_n_in;
-        capsule->hess_vde_casadi[i].casadi_n_out = &{{ model.name }}_expl_ode_hess_n_out;
-        capsule->hess_vde_casadi[i].casadi_sparsity_in = &{{ model.name }}_expl_ode_hess_sparsity_in;
-        capsule->hess_vde_casadi[i].casadi_sparsity_out = &{{ model.name }}_expl_ode_hess_sparsity_out;
-        capsule->hess_vde_casadi[i].casadi_work = &{{ model.name }}_expl_ode_hess_work;
-        external_function_param_casadi_create(&capsule->hess_vde_casadi[i], {{ dims.np }});
+        MAP_CASADI_FNC(hess_vde_casadi[i], {{ model.name }}_expl_ode_hess);
     }
     {%- endif %}
 
@@ -517,126 +470,64 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     // implicit dae
     capsule->impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->impl_dae_fun[i].casadi_fun = &{{ model.name }}_impl_dae_fun;
-        capsule->impl_dae_fun[i].casadi_work = &{{ model.name }}_impl_dae_fun_work;
-        capsule->impl_dae_fun[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_sparsity_in;
-        capsule->impl_dae_fun[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_sparsity_out;
-        capsule->impl_dae_fun[i].casadi_n_in = &{{ model.name }}_impl_dae_fun_n_in;
-        capsule->impl_dae_fun[i].casadi_n_out = &{{ model.name }}_impl_dae_fun_n_out;
-        external_function_param_casadi_create(&capsule->impl_dae_fun[i], {{ dims.np }});
+        MAP_CASADI_FNC(impl_dae_fun[i], {{ model.name }}_impl_dae_fun);
     }
 
     capsule->impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_fun = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z;
-        capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_work = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_work;
-        capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_in;
-        capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out;
-        capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_n_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in;
-        capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_n_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out;
-        external_function_param_casadi_create(&capsule->impl_dae_fun_jac_x_xdot_z[i], {{ dims.np }});
+        MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_z[i], {{ model.name }}_impl_dae_fun_jac_x_xdot_z);
     }
 
     capsule->impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->impl_dae_jac_x_xdot_u_z[i].casadi_fun = &{{ model.name }}_impl_dae_jac_x_xdot_u_z;
-        capsule->impl_dae_jac_x_xdot_u_z[i].casadi_work = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_work;
-        capsule->impl_dae_jac_x_xdot_u_z[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in;
-        capsule->impl_dae_jac_x_xdot_u_z[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out;
-        capsule->impl_dae_jac_x_xdot_u_z[i].casadi_n_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in;
-        capsule->impl_dae_jac_x_xdot_u_z[i].casadi_n_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out;
-        external_function_param_casadi_create(&capsule->impl_dae_jac_x_xdot_u_z[i], {{ dims.np }});
+        MAP_CASADI_FNC(impl_dae_jac_x_xdot_u_z[i], {{ model.name }}_impl_dae_jac_x_xdot_u_z);
     }
 
     {%- if solver_options.hessian_approx == "EXACT" %}
     capsule->impl_dae_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->impl_dae_hess[i].casadi_fun = &{{ model.name }}_impl_dae_hess;
-        capsule->impl_dae_hess[i].casadi_work = &{{ model.name }}_impl_dae_hess_work;
-        capsule->impl_dae_hess[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_hess_sparsity_in;
-        capsule->impl_dae_hess[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_hess_sparsity_out;
-        capsule->impl_dae_hess[i].casadi_n_in = &{{ model.name }}_impl_dae_hess_n_in;
-        capsule->impl_dae_hess[i].casadi_n_out = &{{ model.name }}_impl_dae_hess_n_out;
-        external_function_param_casadi_create(&capsule->impl_dae_hess[i], {{ dims.np }});
+        MAP_CASADI_FNC(impl_dae_hess[i], {{ model.name }}_impl_dae_hess);
     }
     {%- endif %}
 {% elif solver_options.integrator_type == "LIFTED_IRK" %}
     // external functions (implicit model)
     capsule->impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->impl_dae_fun[i].casadi_fun = &{{ model.name }}_impl_dae_fun;
-        capsule->impl_dae_fun[i].casadi_work = &{{ model.name }}_impl_dae_fun_work;
-        capsule->impl_dae_fun[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_sparsity_in;
-        capsule->impl_dae_fun[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_sparsity_out;
-        capsule->impl_dae_fun[i].casadi_n_in = &{{ model.name }}_impl_dae_fun_n_in;
-        capsule->impl_dae_fun[i].casadi_n_out = &{{ model.name }}_impl_dae_fun_n_out;
-        external_function_param_casadi_create(&capsule->impl_dae_fun[i], {{ dims.np }});
+        MAP_CASADI_FNC(impl_dae_fun[i], {{ model.name }}_impl_dae_fun);
     }
 
-    capsule->impl_dae_fun_jac_x_xdot_u = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);    for (int i = 0; i < N; i++) {
-        capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_fun = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u;
-        capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_work = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_work;
-        capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_in;
-        capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_out;
-        capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_n_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_in;
-        capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_n_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_out;
-        external_function_param_casadi_create(&capsule->impl_dae_fun_jac_x_xdot_u[i], {{ dims.np }});
+    capsule->impl_dae_fun_jac_x_xdot_u = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N; i++) {
+        MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_u[i], {{ model.name }}_impl_dae_fun_jac_x_xdot_u);
     }
 
 {% elif solver_options.integrator_type == "GNSF" %}
+    {% if model.gnsf.purely_linear != 1 %}
     capsule->gnsf_phi_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->gnsf_phi_fun[i].casadi_fun = &{{ model.name }}_gnsf_phi_fun;
-        capsule->gnsf_phi_fun[i].casadi_work = &{{ model.name }}_gnsf_phi_fun_work;
-        capsule->gnsf_phi_fun[i].casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_sparsity_in;
-        capsule->gnsf_phi_fun[i].casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_sparsity_out;
-        capsule->gnsf_phi_fun[i].casadi_n_in = &{{ model.name }}_gnsf_phi_fun_n_in;
-        capsule->gnsf_phi_fun[i].casadi_n_out = &{{ model.name }}_gnsf_phi_fun_n_out;
-        external_function_param_casadi_create(&capsule->gnsf_phi_fun[i], {{ dims.np }});
+        MAP_CASADI_FNC(gnsf_phi_fun[i], {{ model.name }}_gnsf_phi_fun);
     }
 
     capsule->gnsf_phi_fun_jac_y = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->gnsf_phi_fun_jac_y[i].casadi_fun = &{{ model.name }}_gnsf_phi_fun_jac_y;
-        capsule->gnsf_phi_fun_jac_y[i].casadi_work = &{{ model.name }}_gnsf_phi_fun_jac_y_work;
-        capsule->gnsf_phi_fun_jac_y[i].casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in;
-        capsule->gnsf_phi_fun_jac_y[i].casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out;
-        capsule->gnsf_phi_fun_jac_y[i].casadi_n_in = &{{ model.name }}_gnsf_phi_fun_jac_y_n_in;
-        capsule->gnsf_phi_fun_jac_y[i].casadi_n_out = &{{ model.name }}_gnsf_phi_fun_jac_y_n_out;
-        external_function_param_casadi_create(&capsule->gnsf_phi_fun_jac_y[i], {{ dims.np }});
+        MAP_CASADI_FNC(gnsf_phi_fun_jac_y[i], {{ model.name }}_gnsf_phi_fun_jac_y);
     }
 
     capsule->gnsf_phi_jac_y_uhat = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->gnsf_phi_jac_y_uhat[i].casadi_fun = &{{ model.name }}_gnsf_phi_jac_y_uhat;
-        capsule->gnsf_phi_jac_y_uhat[i].casadi_work = &{{ model.name }}_gnsf_phi_jac_y_uhat_work;
-        capsule->gnsf_phi_jac_y_uhat[i].casadi_sparsity_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in;
-        capsule->gnsf_phi_jac_y_uhat[i].casadi_sparsity_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out;
-        capsule->gnsf_phi_jac_y_uhat[i].casadi_n_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_in;
-        capsule->gnsf_phi_jac_y_uhat[i].casadi_n_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_out;
-        external_function_param_casadi_create(&capsule->gnsf_phi_jac_y_uhat[i], {{ dims.np }});
+        MAP_CASADI_FNC(gnsf_phi_jac_y_uhat[i], {{ model.name }}_gnsf_phi_jac_y_uhat);
     }
 
+    {% if model.gnsf.nontrivial_f_LO == 1 %}
     capsule->gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_fun = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz;
-        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_work = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work;
-        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_sparsity_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in;
-        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_sparsity_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out;
-        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_n_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in;
-        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_n_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out;
-        external_function_param_casadi_create(&capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i], {{ dims.np }});
+        MAP_CASADI_FNC(gnsf_f_lo_jac_x1_x1dot_u_z[i], {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz);
     }
-
+    {%- endif %}
+    {%- endif %}
     capsule->gnsf_get_matrices_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->gnsf_get_matrices_fun[i].casadi_fun = &{{ model.name }}_gnsf_get_matrices_fun;
-        capsule->gnsf_get_matrices_fun[i].casadi_work = &{{ model.name }}_gnsf_get_matrices_fun_work;
-        capsule->gnsf_get_matrices_fun[i].casadi_sparsity_in = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_in;
-        capsule->gnsf_get_matrices_fun[i].casadi_sparsity_out = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_out;
-        capsule->gnsf_get_matrices_fun[i].casadi_n_in = &{{ model.name }}_gnsf_get_matrices_fun_n_in;
-        capsule->gnsf_get_matrices_fun[i].casadi_n_out = &{{ model.name }}_gnsf_get_matrices_fun_n_out;
-        external_function_param_casadi_create(&capsule->gnsf_get_matrices_fun[i], {{ dims.np }});
+        MAP_CASADI_FNC(gnsf_get_matrices_fun[i], {{ model.name }}_gnsf_get_matrices_fun);
     }
 {% elif solver_options.integrator_type == "DISCRETE" %}
     // discrete dynamics
@@ -644,32 +535,22 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     for (int i = 0; i < N; i++)
     {
         {%- if model.dyn_ext_fun_type == "casadi" %}
-        capsule->discr_dyn_phi_fun[i].casadi_fun = &{{ model.name }}_dyn_disc_phi_fun;
-        capsule->discr_dyn_phi_fun[i].casadi_n_in = &{{ model.name }}_dyn_disc_phi_fun_n_in;
-        capsule->discr_dyn_phi_fun[i].casadi_n_out = &{{ model.name }}_dyn_disc_phi_fun_n_out;
-        capsule->discr_dyn_phi_fun[i].casadi_sparsity_in = &{{ model.name }}_dyn_disc_phi_fun_sparsity_in;
-        capsule->discr_dyn_phi_fun[i].casadi_sparsity_out = &{{ model.name }}_dyn_disc_phi_fun_sparsity_out;
-        capsule->discr_dyn_phi_fun[i].casadi_work = &{{ model.name }}_dyn_disc_phi_fun_work;
+        MAP_CASADI_FNC(discr_dyn_phi_fun[i], {{ model.name }}_dyn_disc_phi_fun);
         {%- else %}
         capsule->discr_dyn_phi_fun[i].fun = &{{ model.dyn_disc_fun }};
-        {%- endif %}
         external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun[i], {{ dims.np }});
+        {%- endif %}
     }
 
     capsule->discr_dyn_phi_fun_jac_ut_xt = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N);
     for (int i = 0; i < N; i++)
     {
         {%- if model.dyn_ext_fun_type == "casadi" %}
-        capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_fun = &{{ model.name }}_dyn_disc_phi_fun_jac;
-        capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_n_in = &{{ model.name }}_dyn_disc_phi_fun_jac_n_in;
-        capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_n_out = &{{ model.name }}_dyn_disc_phi_fun_jac_n_out;
-        capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_sparsity_in = &{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_in;
-        capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_sparsity_out = &{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_out;
-        capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_work = &{{ model.name }}_dyn_disc_phi_fun_jac_work;
+        MAP_CASADI_FNC(discr_dyn_phi_fun_jac_ut_xt[i], {{ model.name }}_dyn_disc_phi_fun_jac);
         {%- else %}
         capsule->discr_dyn_phi_fun_jac_ut_xt[i].fun = &{{ model.dyn_disc_fun_jac }};
-        {%- endif %}
         external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun_jac_ut_xt[i], {{ dims.np }});
+        {%- endif %}
     }
 
   {%- if solver_options.hessian_approx == "EXACT" %}
@@ -677,86 +558,46 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     for (int i = 0; i < N; i++)
     {
         {%- if model.dyn_ext_fun_type == "casadi" %}
-        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_fun = &{{ model.name }}_dyn_disc_phi_fun_jac_hess;
-        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_n_in = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_n_in;
-        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_n_out = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_n_out;
-        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_sparsity_in = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_in;
-        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_sparsity_out = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_out;
-        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_work = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_work;
+        MAP_CASADI_FNC(discr_dyn_phi_fun_jac_ut_xt_hess[i], {{ model.name }}_dyn_disc_phi_fun_jac_hess);
         {%- else %}
         capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].fun = &{{ model.dyn_disc_fun_jac_hess }};
-        {%- endif %}
         external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i], {{ dims.np }});
+        {%- endif %}
     }
   {%- endif %}
 {%- endif %}
 
 
 {%- if cost.cost_type_0 == "NONLINEAR_LS" %}
-    // nonlinear least square function
-    capsule->cost_y_0_fun.casadi_fun = &{{ model.name }}_cost_y_0_fun;
-    capsule->cost_y_0_fun.casadi_n_in = &{{ model.name }}_cost_y_0_fun_n_in;
-    capsule->cost_y_0_fun.casadi_n_out = &{{ model.name }}_cost_y_0_fun_n_out;
-    capsule->cost_y_0_fun.casadi_sparsity_in = &{{ model.name }}_cost_y_0_fun_sparsity_in;
-    capsule->cost_y_0_fun.casadi_sparsity_out = &{{ model.name }}_cost_y_0_fun_sparsity_out;
-    capsule->cost_y_0_fun.casadi_work = &{{ model.name }}_cost_y_0_fun_work;
-    external_function_param_casadi_create(&capsule->cost_y_0_fun, {{ dims.np }});
-
-    capsule->cost_y_0_fun_jac_ut_xt.casadi_fun = &{{ model.name }}_cost_y_0_fun_jac_ut_xt;
-    capsule->cost_y_0_fun_jac_ut_xt.casadi_n_in = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in;
-    capsule->cost_y_0_fun_jac_ut_xt.casadi_n_out = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out;
-    capsule->cost_y_0_fun_jac_ut_xt.casadi_sparsity_in = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_in;
-    capsule->cost_y_0_fun_jac_ut_xt.casadi_sparsity_out = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_out;
-    capsule->cost_y_0_fun_jac_ut_xt.casadi_work = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_work;
-    external_function_param_casadi_create(&capsule->cost_y_0_fun_jac_ut_xt, {{ dims.np }});
-
-    capsule->cost_y_0_hess.casadi_fun = &{{ model.name }}_cost_y_0_hess;
-    capsule->cost_y_0_hess.casadi_n_in = &{{ model.name }}_cost_y_0_hess_n_in;
-    capsule->cost_y_0_hess.casadi_n_out = &{{ model.name }}_cost_y_0_hess_n_out;
-    capsule->cost_y_0_hess.casadi_sparsity_in = &{{ model.name }}_cost_y_0_hess_sparsity_in;
-    capsule->cost_y_0_hess.casadi_sparsity_out = &{{ model.name }}_cost_y_0_hess_sparsity_out;
-    capsule->cost_y_0_hess.casadi_work = &{{ model.name }}_cost_y_0_hess_work;
-    external_function_param_casadi_create(&capsule->cost_y_0_hess, {{ dims.np }});
+    // nonlinear least squares function
+    MAP_CASADI_FNC(cost_y_0_fun, {{ model.name }}_cost_y_0_fun);
+    MAP_CASADI_FNC(cost_y_0_fun_jac_ut_xt, {{ model.name }}_cost_y_0_fun_jac_ut_xt);
+    MAP_CASADI_FNC(cost_y_0_hess, {{ model.name }}_cost_y_0_hess);
 
 {%- elif cost.cost_type_0 == "EXTERNAL" %}
     // external cost
-    {% if cost.cost_ext_fun_type_0 == "casadi" %}
-    capsule->ext_cost_0_fun.casadi_fun = &{{ model.name }}_cost_ext_cost_0_fun;
-    capsule->ext_cost_0_fun.casadi_n_in = &{{ model.name }}_cost_ext_cost_0_fun_n_in;
-    capsule->ext_cost_0_fun.casadi_n_out = &{{ model.name }}_cost_ext_cost_0_fun_n_out;
-    capsule->ext_cost_0_fun.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_0_fun_sparsity_in;
-    capsule->ext_cost_0_fun.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_0_fun_sparsity_out;
-    capsule->ext_cost_0_fun.casadi_work = &{{ model.name }}_cost_ext_cost_0_fun_work;
-    {% else %}
+    {%- if cost.cost_ext_fun_type_0 == "casadi" %}
+    MAP_CASADI_FNC(ext_cost_0_fun, {{ model.name }}_cost_ext_cost_0_fun);
+    {%- else %}
     capsule->ext_cost_0_fun.fun = &{{ cost.cost_function_ext_cost_0 }};
-    {% endif %}
     external_function_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun, {{ dims.np }});
+    {%- endif %}
 
     // external cost
-    {% if cost.cost_ext_fun_type_0 == "casadi" %}
-    capsule->ext_cost_0_fun_jac.casadi_fun = &{{ model.name }}_cost_ext_cost_0_fun_jac;
-    capsule->ext_cost_0_fun_jac.casadi_n_in = &{{ model.name }}_cost_ext_cost_0_fun_jac_n_in;
-    capsule->ext_cost_0_fun_jac.casadi_n_out = &{{ model.name }}_cost_ext_cost_0_fun_jac_n_out;
-    capsule->ext_cost_0_fun_jac.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_in;
-    capsule->ext_cost_0_fun_jac.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_out;
-    capsule->ext_cost_0_fun_jac.casadi_work = &{{ model.name }}_cost_ext_cost_0_fun_jac_work;
-    {% else %}
+    {%- if cost.cost_ext_fun_type_0 == "casadi" %}
+    MAP_CASADI_FNC(ext_cost_0_fun_jac, {{ model.name }}_cost_ext_cost_0_fun_jac);
+    {%- else %}
     capsule->ext_cost_0_fun_jac.fun = &{{ cost.cost_function_ext_cost_0 }};
-    {% endif %}
     external_function_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun_jac, {{ dims.np }});
+    {%- endif %}
 
     // external cost
-    {% if cost.cost_ext_fun_type_0 == "casadi" %}
-    capsule->ext_cost_0_fun_jac_hess.casadi_fun = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess;
-    capsule->ext_cost_0_fun_jac_hess.casadi_n_in = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in;
-    capsule->ext_cost_0_fun_jac_hess.casadi_n_out = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out;
-    capsule->ext_cost_0_fun_jac_hess.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_in;
-    capsule->ext_cost_0_fun_jac_hess.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_out;
-    capsule->ext_cost_0_fun_jac_hess.casadi_work = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_work;
-    {% else %}
+    {%- if cost.cost_ext_fun_type_0 == "casadi" %}
+    MAP_CASADI_FNC(ext_cost_0_fun_jac_hess, {{ model.name }}_cost_ext_cost_0_fun_jac_hess);
+    {%- else %}
     capsule->ext_cost_0_fun_jac_hess.fun = &{{ cost.cost_function_ext_cost_0 }};
-    {% endif %}
     external_function_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun_jac_hess, {{ dims.np }});
+    {%- endif %}
 {%- endif %}
 
 {%- if cost.cost_type == "NONLINEAR_LS" %}
@@ -764,164 +605,130 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     capsule->cost_y_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N-1; i++)
     {
-        capsule->cost_y_fun[i].casadi_fun = &{{ model.name }}_cost_y_fun;
-        capsule->cost_y_fun[i].casadi_n_in = &{{ model.name }}_cost_y_fun_n_in;
-        capsule->cost_y_fun[i].casadi_n_out = &{{ model.name }}_cost_y_fun_n_out;
-        capsule->cost_y_fun[i].casadi_sparsity_in = &{{ model.name }}_cost_y_fun_sparsity_in;
-        capsule->cost_y_fun[i].casadi_sparsity_out = &{{ model.name }}_cost_y_fun_sparsity_out;
-        capsule->cost_y_fun[i].casadi_work = &{{ model.name }}_cost_y_fun_work;
-
-        external_function_param_casadi_create(&capsule->cost_y_fun[i], {{ dims.np }});
+        MAP_CASADI_FNC(cost_y_fun[i], {{ model.name }}_cost_y_fun);
     }
 
     capsule->cost_y_fun_jac_ut_xt = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N-1; i++)
     {
-        capsule->cost_y_fun_jac_ut_xt[i].casadi_fun = &{{ model.name }}_cost_y_fun_jac_ut_xt;
-        capsule->cost_y_fun_jac_ut_xt[i].casadi_n_in = &{{ model.name }}_cost_y_fun_jac_ut_xt_n_in;
-        capsule->cost_y_fun_jac_ut_xt[i].casadi_n_out = &{{ model.name }}_cost_y_fun_jac_ut_xt_n_out;
-        capsule->cost_y_fun_jac_ut_xt[i].casadi_sparsity_in = &{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_in;
-        capsule->cost_y_fun_jac_ut_xt[i].casadi_sparsity_out = &{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_out;
-        capsule->cost_y_fun_jac_ut_xt[i].casadi_work = &{{ model.name }}_cost_y_fun_jac_ut_xt_work;
-
-        external_function_param_casadi_create(&capsule->cost_y_fun_jac_ut_xt[i], {{ dims.np }});
+        MAP_CASADI_FNC(cost_y_fun_jac_ut_xt[i], {{ model.name }}_cost_y_fun_jac_ut_xt);
     }
 
     capsule->cost_y_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N-1; i++)
     {
-        capsule->cost_y_hess[i].casadi_fun = &{{ model.name }}_cost_y_hess;
-        capsule->cost_y_hess[i].casadi_n_in = &{{ model.name }}_cost_y_hess_n_in;
-        capsule->cost_y_hess[i].casadi_n_out = &{{ model.name }}_cost_y_hess_n_out;
-        capsule->cost_y_hess[i].casadi_sparsity_in = &{{ model.name }}_cost_y_hess_sparsity_in;
-        capsule->cost_y_hess[i].casadi_sparsity_out = &{{ model.name }}_cost_y_hess_sparsity_out;
-        capsule->cost_y_hess[i].casadi_work = &{{ model.name }}_cost_y_hess_work;
-
-        external_function_param_casadi_create(&capsule->cost_y_hess[i], {{ dims.np }});
+        MAP_CASADI_FNC(cost_y_hess[i], {{ model.name }}_cost_y_hess);
     }
 {%- elif cost.cost_type == "EXTERNAL" %}
     // external cost
     capsule->ext_cost_fun = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N);
     for (int i = 0; i < N-1; i++)
     {
-        {% if cost.cost_ext_fun_type == "casadi" %}
-        capsule->ext_cost_fun[i].casadi_fun = &{{ model.name }}_cost_ext_cost_fun;
-        capsule->ext_cost_fun[i].casadi_n_in = &{{ model.name }}_cost_ext_cost_fun_n_in;
-        capsule->ext_cost_fun[i].casadi_n_out = &{{ model.name }}_cost_ext_cost_fun_n_out;
-        capsule->ext_cost_fun[i].casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_fun_sparsity_in;
-        capsule->ext_cost_fun[i].casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_fun_sparsity_out;
-        capsule->ext_cost_fun[i].casadi_work = &{{ model.name }}_cost_ext_cost_fun_work;
-        {% else %}
+        {%- if cost.cost_ext_fun_type == "casadi" %}
+        MAP_CASADI_FNC(ext_cost_fun[i], {{ model.name }}_cost_ext_cost_fun);
+        {%- else %}
         capsule->ext_cost_fun[i].fun = &{{ cost.cost_function_ext_cost }};
-        {% endif %}
         external_function_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun[i], {{ dims.np }});
+        {%- endif %}
     }
 
     capsule->ext_cost_fun_jac = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N);
     for (int i = 0; i < N-1; i++)
     {
-        {% if cost.cost_ext_fun_type == "casadi" %}
-        capsule->ext_cost_fun_jac[i].casadi_fun = &{{ model.name }}_cost_ext_cost_fun_jac;
-        capsule->ext_cost_fun_jac[i].casadi_n_in = &{{ model.name }}_cost_ext_cost_fun_jac_n_in;
-        capsule->ext_cost_fun_jac[i].casadi_n_out = &{{ model.name }}_cost_ext_cost_fun_jac_n_out;
-        capsule->ext_cost_fun_jac[i].casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_fun_jac_sparsity_in;
-        capsule->ext_cost_fun_jac[i].casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_fun_jac_sparsity_out;
-        capsule->ext_cost_fun_jac[i].casadi_work = &{{ model.name }}_cost_ext_cost_fun_jac_work;
-        {% else %}
+        {%- if cost.cost_ext_fun_type == "casadi" %}
+        MAP_CASADI_FNC(ext_cost_fun_jac[i], {{ model.name }}_cost_ext_cost_fun_jac);
+        {%- else %}
         capsule->ext_cost_fun_jac[i].fun = &{{ cost.cost_function_ext_cost }};
-        {% endif %}
         external_function_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun_jac[i], {{ dims.np }});
+        {%- endif %}
     }
 
     capsule->ext_cost_fun_jac_hess = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N);
     for (int i = 0; i < N-1; i++)
     {
-        {% if cost.cost_ext_fun_type == "casadi" %}
-        capsule->ext_cost_fun_jac_hess[i].casadi_fun = &{{ model.name }}_cost_ext_cost_fun_jac_hess;
-        capsule->ext_cost_fun_jac_hess[i].casadi_n_in = &{{ model.name }}_cost_ext_cost_fun_jac_hess_n_in;
-        capsule->ext_cost_fun_jac_hess[i].casadi_n_out = &{{ model.name }}_cost_ext_cost_fun_jac_hess_n_out;
-        capsule->ext_cost_fun_jac_hess[i].casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_in;
-        capsule->ext_cost_fun_jac_hess[i].casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_out;
-        capsule->ext_cost_fun_jac_hess[i].casadi_work = &{{ model.name }}_cost_ext_cost_fun_jac_hess_work;
-        {% else %}
+        {%- if cost.cost_ext_fun_type == "casadi" %}
+        MAP_CASADI_FNC(ext_cost_fun_jac_hess[i], {{ model.name }}_cost_ext_cost_fun_jac_hess);
+        {%- else %}
         capsule->ext_cost_fun_jac_hess[i].fun = &{{ cost.cost_function_ext_cost }};
-        {% endif %}
         external_function_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun_jac_hess[i], {{ dims.np }});
+        {%- endif %}
     }
 {%- endif %}
 
 {%- if cost.cost_type_e == "NONLINEAR_LS" %}
     // nonlinear least square function
-    capsule->cost_y_e_fun.casadi_fun = &{{ model.name }}_cost_y_e_fun;
-    capsule->cost_y_e_fun.casadi_n_in = &{{ model.name }}_cost_y_e_fun_n_in;
-    capsule->cost_y_e_fun.casadi_n_out = &{{ model.name }}_cost_y_e_fun_n_out;
-    capsule->cost_y_e_fun.casadi_sparsity_in = &{{ model.name }}_cost_y_e_fun_sparsity_in;
-    capsule->cost_y_e_fun.casadi_sparsity_out = &{{ model.name }}_cost_y_e_fun_sparsity_out;
-    capsule->cost_y_e_fun.casadi_work = &{{ model.name }}_cost_y_e_fun_work;
-    external_function_param_casadi_create(&capsule->cost_y_e_fun, {{ dims.np }});
-
-    capsule->cost_y_e_fun_jac_ut_xt.casadi_fun = &{{ model.name }}_cost_y_e_fun_jac_ut_xt;
-    capsule->cost_y_e_fun_jac_ut_xt.casadi_n_in = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in;
-    capsule->cost_y_e_fun_jac_ut_xt.casadi_n_out = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out;
-    capsule->cost_y_e_fun_jac_ut_xt.casadi_sparsity_in = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_in;
-    capsule->cost_y_e_fun_jac_ut_xt.casadi_sparsity_out = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_out;
-    capsule->cost_y_e_fun_jac_ut_xt.casadi_work = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_work;
-    external_function_param_casadi_create(&capsule->cost_y_e_fun_jac_ut_xt, {{ dims.np }});
-
-    capsule->cost_y_e_hess.casadi_fun = &{{ model.name }}_cost_y_e_hess;
-    capsule->cost_y_e_hess.casadi_n_in = &{{ model.name }}_cost_y_e_hess_n_in;
-    capsule->cost_y_e_hess.casadi_n_out = &{{ model.name }}_cost_y_e_hess_n_out;
-    capsule->cost_y_e_hess.casadi_sparsity_in = &{{ model.name }}_cost_y_e_hess_sparsity_in;
-    capsule->cost_y_e_hess.casadi_sparsity_out = &{{ model.name }}_cost_y_e_hess_sparsity_out;
-    capsule->cost_y_e_hess.casadi_work = &{{ model.name }}_cost_y_e_hess_work;
-    external_function_param_casadi_create(&capsule->cost_y_e_hess, {{ dims.np }});
-
+    MAP_CASADI_FNC(cost_y_e_fun, {{ model.name }}_cost_y_e_fun);
+    MAP_CASADI_FNC(cost_y_e_fun_jac_ut_xt, {{ model.name }}_cost_y_e_fun_jac_ut_xt);
+    MAP_CASADI_FNC(cost_y_e_hess, {{ model.name }}_cost_y_e_hess);
 {%- elif cost.cost_type_e == "EXTERNAL" %}
-    // external cost
-    {% if cost.cost_ext_fun_type_e == "casadi" %}
-    capsule->ext_cost_e_fun.casadi_fun = &{{ model.name }}_cost_ext_cost_e_fun;
-    capsule->ext_cost_e_fun.casadi_n_in = &{{ model.name }}_cost_ext_cost_e_fun_n_in;
-    capsule->ext_cost_e_fun.casadi_n_out = &{{ model.name }}_cost_ext_cost_e_fun_n_out;
-    capsule->ext_cost_e_fun.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_e_fun_sparsity_in;
-    capsule->ext_cost_e_fun.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_e_fun_sparsity_out;
-    capsule->ext_cost_e_fun.casadi_work = &{{ model.name }}_cost_ext_cost_e_fun_work;
+    // external cost - function
+    {%- if cost.cost_ext_fun_type_e == "casadi" %}
+    MAP_CASADI_FNC(ext_cost_e_fun, {{ model.name }}_cost_ext_cost_e_fun);
     {% else %}
     capsule->ext_cost_e_fun.fun = &{{ cost.cost_function_ext_cost_e }};
-    {% endif %}
     external_function_param_{{ cost.cost_ext_fun_type_e }}_create(&capsule->ext_cost_e_fun, {{ dims.np }});
+    {%- endif %}
 
-    // external cost
-    {% if cost.cost_ext_fun_type_e == "casadi" %}
-    capsule->ext_cost_e_fun_jac.casadi_fun = &{{ model.name }}_cost_ext_cost_e_fun_jac;
-    capsule->ext_cost_e_fun_jac.casadi_n_in = &{{ model.name }}_cost_ext_cost_e_fun_jac_n_in;
-    capsule->ext_cost_e_fun_jac.casadi_n_out = &{{ model.name }}_cost_ext_cost_e_fun_jac_n_out;
-    capsule->ext_cost_e_fun_jac.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_in;
-    capsule->ext_cost_e_fun_jac.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_out;
-    capsule->ext_cost_e_fun_jac.casadi_work = &{{ model.name }}_cost_ext_cost_e_fun_jac_work;
-    {% else %}
+    // external cost - jacobian
+    {%- if cost.cost_ext_fun_type_e == "casadi" %}
+    MAP_CASADI_FNC(ext_cost_e_fun_jac, {{ model.name }}_cost_ext_cost_e_fun_jac);
+    {%- else %}
     capsule->ext_cost_e_fun_jac.fun = &{{ cost.cost_function_ext_cost_e }};
-    {% endif %}
     external_function_param_{{ cost.cost_ext_fun_type_e }}_create(&capsule->ext_cost_e_fun_jac, {{ dims.np }});
+    {%- endif %}
 
-    // external cost
-    {% if cost.cost_ext_fun_type_e == "casadi" %}
-    capsule->ext_cost_e_fun_jac_hess.casadi_fun = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess;
-    capsule->ext_cost_e_fun_jac_hess.casadi_n_in = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in;
-    capsule->ext_cost_e_fun_jac_hess.casadi_n_out = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out;
-    capsule->ext_cost_e_fun_jac_hess.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_in;
-    capsule->ext_cost_e_fun_jac_hess.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_out;
-    capsule->ext_cost_e_fun_jac_hess.casadi_work = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_work;
-    {% else %}
+    // external cost - hessian
+    {%- if cost.cost_ext_fun_type_e == "casadi" %}
+    MAP_CASADI_FNC(ext_cost_e_fun_jac_hess, {{ model.name }}_cost_ext_cost_e_fun_jac_hess);
+    {%- else %}
     capsule->ext_cost_e_fun_jac_hess.fun = &{{ cost.cost_function_ext_cost_e }};
-    {% endif %}
     external_function_param_{{ cost.cost_ext_fun_type_e }}_create(&capsule->ext_cost_e_fun_jac_hess, {{ dims.np }});
+    {%- endif %}
 {%- endif %}
 
+#undef MAP_CASADI_FNC
+}
+
+
+/**
+ * Internal function for {{ model.name }}_acados_create: step 4
+ */
+void {{ model.name }}_acados_create_4_set_default_parameters({{ model.name }}_solver_capsule* capsule) {
+{%- if dims.np > 0 %}
+    const int N = capsule->nlp_solver_plan->N;
+    // initialize parameters to nominal value
+    double* p = calloc(NP, sizeof(double));
+    {%- for item in parameter_values %}
+        {%- if item != 0 %}
+    p[{{ loop.index0 }}] = {{ item }};
+        {%- endif %}
+    {%- endfor %}
+
+    for (int i = 0; i <= N; i++) {
+        {{ model.name }}_acados_update_params(capsule, i, p, NP);
+    }
+    free(p);
+{%- else %}
+    // no parameters defined
+{%- endif %}{# if dims.np #}
+}
+
+
+/**
+ * Internal function for {{ model.name }}_acados_create: step 5
+ */
+void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule* capsule, const int N, double* new_time_steps)
+{
+    assert(N == capsule->nlp_solver_plan->N);
+    ocp_nlp_config* nlp_config = capsule->nlp_config;
+    ocp_nlp_dims* nlp_dims = capsule->nlp_dims;
+
     /************************************************
     *  nlp_in
     ************************************************/
-    ocp_nlp_in * nlp_in = ocp_nlp_in_create(nlp_config, nlp_dims);
-    capsule->nlp_in = nlp_in;
+//    ocp_nlp_in * nlp_in = ocp_nlp_in_create(nlp_config, nlp_dims);
+//    capsule->nlp_in = nlp_in;
+    ocp_nlp_in * nlp_in = capsule->nlp_in;
 
     // set up time_steps
     {% set all_equal = true -%}
@@ -978,11 +785,15 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
         ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i,
                                    "impl_dae_fun_jac_x_xdot_u", &capsule->impl_dae_fun_jac_x_xdot_u[i]);
     {% elif solver_options.integrator_type == "GNSF" %}
+        {% if model.gnsf.purely_linear != 1 %}
         ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_fun", &capsule->gnsf_phi_fun[i]);
         ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_fun_jac_y", &capsule->gnsf_phi_fun_jac_y[i]);
         ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_jac_y_uhat", &capsule->gnsf_phi_jac_y_uhat[i]);
+            {% if model.gnsf.nontrivial_f_LO == 1 %}
         ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "f_lo_jac_x1_x1dot_u_z",
                                    &capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i]);
+            {%- endif %}
+        {%- endif %}
         ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "gnsf_get_matrices_fun",
                                    &capsule->gnsf_get_matrices_fun[i]);
     {% elif solver_options.integrator_type == "DISCRETE" %}
@@ -996,10 +807,9 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     {%- endif %}
     }
 
-
     /**** Cost ****/
 {%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %}
-{% if dims.ny_0 > 0 %}
+    {%- if dims.ny_0 > 0 %}
     double* W_0 = calloc(NY0*NY0, sizeof(double));
     // change only the non-zero elements:
     {%- for j in range(end=dims.ny_0) %}
@@ -1021,14 +831,14 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     {%- endfor %}
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0);
     free(yref_0);
-{% endif %}
-{% endif %}
+    {%- endif %}
+{%- endif %}
 
 {%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %}
-{% if dims.ny > 0 %}
+    {%- if dims.ny > 0 %}
     double* W = calloc(NY*NY, sizeof(double));
     // change only the non-zero elements:
-    {% for j in range(end=dims.ny) %}
+    {%- for j in range(end=dims.ny) %}
         {%- for k in range(end=dims.ny) %}
             {%- if cost.W[j][k] != 0 %}
     W[{{ j }}+(NY) * {{ k }}] = {{ cost.W[j][k] }};
@@ -1051,13 +861,13 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
     free(W);
     free(yref);
-{% endif %}
-{% endif %}
+    {%- endif %}
+{%- endif %}
 
 {%- if cost.cost_type_0 == "LINEAR_LS" %}
     double* Vx_0 = calloc(NY0*NX, sizeof(double));
     // change only the non-zero elements:
-    {% for j in range(end=dims.ny_0) %}
+    {%- for j in range(end=dims.ny_0) %}
         {%- for k in range(end=dims.nx) %}
             {%- if cost.Vx_0[j][k] != 0 %}
     Vx_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vx_0[j][k] }};
@@ -1067,10 +877,10 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vx", Vx_0);
     free(Vx_0);
 
-{% if dims.ny_0 > 0 and dims.nu > 0 %}
+    {%- if dims.ny_0 > 0 and dims.nu > 0 %}
     double* Vu_0 = calloc(NY0*NU, sizeof(double));
     // change only the non-zero elements:
-    {% for j in range(end=dims.ny_0) %}
+    {%- for j in range(end=dims.ny_0) %}
         {%- for k in range(end=dims.nu) %}
             {%- if cost.Vu_0[j][k] != 0 %}
     Vu_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vu_0[j][k] }};
@@ -1079,8 +889,9 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     {%- endfor %}
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vu", Vu_0);
     free(Vu_0);
-{% endif %}
-{% if dims.ny_0 > 0 and dims.nz > 0 %}
+    {%- endif %}
+
+    {%- if dims.ny_0 > 0 and dims.nz > 0 %}
     double* Vz_0 = calloc(NY0*NZ, sizeof(double));
     // change only the non-zero elements:
     {% for j in range(end=dims.ny_0) %}
@@ -1092,14 +903,14 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     {%- endfor %}
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vz", Vz_0);
     free(Vz_0);
-{%- endif %}
+    {%- endif %}
 {%- endif %}{# LINEAR LS #}
 
 
 {%- if cost.cost_type == "LINEAR_LS" %}
     double* Vx = calloc(NY*NX, sizeof(double));
     // change only the non-zero elements:
-    {% for j in range(end=dims.ny) %}
+    {%- for j in range(end=dims.ny) %}
         {%- for k in range(end=dims.nx) %}
             {%- if cost.Vx[j][k] != 0 %}
     Vx[{{ j }}+(NY) * {{ k }}] = {{ cost.Vx[j][k] }};
@@ -1112,7 +923,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
     free(Vx);
 
-{% if dims.ny > 0 and dims.nu > 0 %}
+    {% if dims.ny > 0 and dims.nu > 0 %}
     double* Vu = calloc(NY*NU, sizeof(double));
     // change only the non-zero elements:
     {% for j in range(end=dims.ny) %}
@@ -1128,9 +939,9 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
         ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vu", Vu);
     }
     free(Vu);
-{% endif %}
+    {%- endif %}
 
-{% if dims.ny > 0 and dims.nz > 0 %}
+    {%- if dims.ny > 0 and dims.nz > 0 %}
     double* Vz = calloc(NY*NZ, sizeof(double));
     // change only the non-zero elements:
     {% for j in range(end=dims.ny) %}
@@ -1146,7 +957,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
         ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vz", Vz);
     }
     free(Vz);
-{%- endif %}
+    {%- endif %}
 {%- endif %}{# LINEAR LS #}
 
 
@@ -1176,8 +987,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
 {%- endif %}
 
-
-{% if dims.ns > 0 %}
+{%- if dims.ns > 0 %}
     double* zlumem = calloc(4*NS, sizeof(double));
     double* Zl = zlumem+NS*0;
     double* Zu = zlumem+NS*1;
@@ -1216,14 +1026,14 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
         ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "zu", zu);
     }
     free(zlumem);
-{% endif %}
+{%- endif %}
 
     // terminal cost
-{% if cost.cost_type_e == "LINEAR_LS" or cost.cost_type_e == "NONLINEAR_LS" %}
-{% if dims.ny_e > 0 %}
+{%- if cost.cost_type_e == "LINEAR_LS" or cost.cost_type_e == "NONLINEAR_LS" %}
+    {%- if dims.ny_e > 0 %}
     double* yref_e = calloc(NYN, sizeof(double));
     // change only the non-zero elements:
-    {% for j in range(end=dims.ny_e) %}
+    {%- for j in range(end=dims.ny_e) %}
         {%- if cost.yref_e[j] != 0 %}
     yref_e[{{ j }}] = {{ cost.yref_e[j] }};
         {%- endif %}
@@ -1233,7 +1043,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
 
     double* W_e = calloc(NYN*NYN, sizeof(double));
     // change only the non-zero elements:
-    {% for j in range(end=dims.ny_e) %}
+    {%- for j in range(end=dims.ny_e) %}
         {%- for k in range(end=dims.ny_e) %}
             {%- if cost.W_e[j][k] != 0 %}
     W_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.W_e[j][k] }};
@@ -1243,7 +1053,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e);
     free(W_e);
 
-    {%- if cost.cost_type_e == "LINEAR_LS" %}
+        {%- if cost.cost_type_e == "LINEAR_LS" %}
     double* Vx_e = calloc(NYN*NX, sizeof(double));
     // change only the non-zero elements:
     {% for j in range(end=dims.ny_e) %}
@@ -1255,14 +1065,14 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     {%- endfor %}
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e);
     free(Vx_e);
-    {%- endif %}
+        {%- endif %}
 
-    {%- if cost.cost_type_e == "NONLINEAR_LS" %}
+        {%- if cost.cost_type_e == "NONLINEAR_LS" %}
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun", &capsule->cost_y_e_fun);
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun_jac", &capsule->cost_y_e_fun_jac_ut_xt);
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_hess", &capsule->cost_y_e_hess);
-    {%- endif %}
-{%- endif %}{# ny_e > 0 #}
+        {%- endif %}
+    {%- endif %}{# ny_e > 0 #}
 
 {%- elif cost.cost_type_e == "EXTERNAL" %}
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun", &capsule->ext_cost_e_fun);
@@ -1312,7 +1122,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     /**** Constraints ****/
 
     // bounds for initial stage
-{% if dims.nbx_0 > 0 %}
+{%- if dims.nbx_0 > 0 %}
     // x0
     int* idxbx0 = malloc(NBX0 * sizeof(int));
     {%- for i in range(end=dims.nbx_0) %}
@@ -1337,8 +1147,9 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
     free(idxbx0);
     free(lubx0);
-{% endif %}
-{% if dims.nbxe_0 > 0 %}
+{%- endif %}
+
+{%- if dims.nbxe_0 > 0 %}
     // idxbxe_0
     int* idxbxe_0 = malloc({{ dims.nbxe_0 }} * sizeof(int));
     {% for i in range(end=dims.nbxe_0) %}
@@ -1346,7 +1157,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     {%- endfor %}
     ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbxe", idxbxe_0);
     free(idxbxe_0);
-{% endif %}
+{%- endif %}
 
     /* constraints that are the same for initial and intermediate */
 {%- if dims.nsbx > 0 %}
@@ -1357,14 +1168,14 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
 
     // soft bounds on x
     int* idxsbx = malloc(NSBX * sizeof(int));
-    {% for i in range(end=dims.nsbx) %}
+    {%- for i in range(end=dims.nsbx) %}
     idxsbx[{{ i }}] = {{ constraints.idxsbx[i] }};
     {%- endfor %}
 
     double* lusbx = calloc(2*NSBX, sizeof(double));
     double* lsbx = lusbx;
     double* usbx = lusbx + NSBX;
-    {% for i in range(end=dims.nsbx) %}
+    {%- for i in range(end=dims.nsbx) %}
         {%- if constraints.lsbx[i] != 0 %}
     lsbx[{{ i }}] = {{ constraints.lsbx[i] }};
         {%- endif %}
@@ -1384,7 +1195,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
 {%- endif %}
 
 
-{% if dims.nbu > 0 %}
+{%- if dims.nbu > 0 %}
     // u
     int* idxbu = malloc(NBU * sizeof(int));
     {% for i in range(end=dims.nbu) %}
@@ -1410,9 +1221,9 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
     free(idxbu);
     free(lubu);
-{% endif %}
+{%- endif %}
 
-{% if dims.nsbu > 0 %}
+{%- if dims.nsbu > 0 %}
     // set up soft bounds for u
     int* idxsbu = malloc(NSBU * sizeof(int));
     {% for i in range(end=dims.nsbu) %}
@@ -1437,7 +1248,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
     free(idxsbu);
     free(lusbu);
-{% endif %}
+{%- endif %}
 
 {% if dims.nsg > 0 %}
     // set up soft bounds for general linear constraints
@@ -1465,7 +1276,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
     free(idxsg);
     free(lusg);
-{% endif %}
+{%- endif %}
 
 {% if dims.nsh > 0 %}
     // set up soft bounds for nonlinear constraints
@@ -1493,7 +1304,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
     free(idxsh);
     free(lush);
-{% endif %}
+{%- endif %}
 
 {% if dims.nsphi > 0 %}
     // set up soft bounds for convex-over-nonlinear constraints
@@ -1521,7 +1332,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
     free(idxsphi);
     free(lusphi);
-{% endif %}
+{%- endif %}
 
 {% if dims.nbx > 0 %}
     // x
@@ -1549,7 +1360,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
     free(idxbx);
     free(lubx);
-{% endif %}
+{%- endif %}
 
 {% if dims.ng > 0 %}
     // set up general constraints for stage 0 to N-1 
@@ -1597,7 +1408,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     free(D);
     free(C);
     free(lug);
-{% endif %}
+{%- endif %}
 
 {% if dims.nh > 0 %}
     // set up nonlinear constraints for stage 0 to N-1
@@ -1632,7 +1443,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
         ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uh", uh);
     }
     free(luh);
-{% endif %}
+{%- endif %}
 
 {% if dims.nphi > 0 and constraints.constr_type == "BGP" %}
     // set up convex-over-nonlinear constraints for stage 0 to N-1
@@ -1659,7 +1470,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
         ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uphi", uphi);
     }
     free(luphi);
-{% endif %}
+{%- endif %}
 
     /* terminal constraints */
 {% if dims.nbx_e > 0 %}
@@ -1866,42 +1677,62 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
                                   "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_e_constraint);
     free(luphi_e);
 {% endif %}
+}
+
 
+/**
+ * Internal function for {{ model.name }}_acados_create: step 6
+ */
+void {{ model.name }}_acados_create_6_set_opts({{ model.name }}_solver_capsule* capsule)
+{
+    const int N = capsule->nlp_solver_plan->N;
+    ocp_nlp_config* nlp_config = capsule->nlp_config;
+    ocp_nlp_dims* nlp_dims = capsule->nlp_dims;
+    void *nlp_opts = capsule->nlp_opts;
 
     /************************************************
     *  opts
     ************************************************/
 
-    capsule->nlp_opts = ocp_nlp_solver_opts_create(nlp_config, nlp_dims);
-
 {% if solver_options.hessian_approx == "EXACT" %}
     bool nlp_solver_exact_hessian = true;
     // TODO: this if should not be needed! however, calling the setter with false leads to weird behavior. Investigate!
     if (nlp_solver_exact_hessian)
     {
-        ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "exact_hess", &nlp_solver_exact_hessian);
+        ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess", &nlp_solver_exact_hessian);
     }
     int exact_hess_dyn = {{ solver_options.exact_hess_dyn }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "exact_hess_dyn", &exact_hess_dyn);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess_dyn", &exact_hess_dyn);
 
     int exact_hess_cost = {{ solver_options.exact_hess_cost }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "exact_hess_cost", &exact_hess_cost);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess_cost", &exact_hess_cost);
 
     int exact_hess_constr = {{ solver_options.exact_hess_constr }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "exact_hess_constr", &exact_hess_constr);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess_constr", &exact_hess_constr);
 {%- endif -%}
 
 {%- if solver_options.globalization == "FIXED_STEP" %}
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization", "fixed_step");
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "globalization", "fixed_step");
 {%- elif solver_options.globalization == "MERIT_BACKTRACKING" %}
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization", "merit_backtracking");
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "globalization", "merit_backtracking");
 
     double alpha_min = {{ solver_options.alpha_min }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "alpha_min", &alpha_min);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "alpha_min", &alpha_min);
 
     double alpha_reduction = {{ solver_options.alpha_reduction }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "alpha_reduction", &alpha_reduction);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "alpha_reduction", &alpha_reduction);
+
+    int line_search_use_sufficient_descent = {{ solver_options.line_search_use_sufficient_descent }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "line_search_use_sufficient_descent", &line_search_use_sufficient_descent);
+
+    int globalization_use_SOC = {{ solver_options.globalization_use_SOC }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization_use_SOC", &globalization_use_SOC);
+
+    double eps_sufficient_descent = {{ solver_options.eps_sufficient_descent }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "eps_sufficient_descent", &eps_sufficient_descent);
 {%- endif -%}
+    int full_step_dual = {{ solver_options.full_step_dual }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "full_step_dual", &full_step_dual);
 
 {%- if dims.nz > 0 %}
     // TODO: these options are lower level -> should be encapsulated! maybe through hessian approx option.
@@ -1909,9 +1740,9 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     bool sens_algebraic_val = true;
 
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_output_z", &output_z_val);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_output_z", &output_z_val);
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_sens_algebraic", &sens_algebraic_val);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_sens_algebraic", &sens_algebraic_val);
 {%- endif %}
 
 {%- if solver_options.integrator_type != "DISCRETE" %}
@@ -1919,7 +1750,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     // set collocation type (relevant for implicit integrators)
     sim_collocation_type collocation_type = {{ solver_options.collocation_type }};
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_collocation_type", &collocation_type);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_collocation_type", &collocation_type);
 
     // set up sim_method_num_steps
     {%- set all_equal = true %}
@@ -1935,7 +1766,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     // all sim_method_num_steps are identical
     int sim_method_num_steps = {{ solver_options.sim_method_num_steps[0] }};
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps);
     {%- else %}
     // sim_method_num_steps are different
     int* sim_method_num_steps = malloc(N*sizeof(int));
@@ -1944,7 +1775,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     {%- endfor %}
 
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps[i]);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps[i]);
     free(sim_method_num_steps);
     {%- endif %}
 
@@ -1962,7 +1793,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     // all sim_method_num_stages are identical
     int sim_method_num_stages = {{ solver_options.sim_method_num_stages[0] }};
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages);
   {%- else %}
     int* sim_method_num_stages = malloc(N*sizeof(int));
     {%- for j in range(end=dims.N) %}
@@ -1970,13 +1801,13 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     {%- endfor %}
 
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages[i]);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages[i]);
     free(sim_method_num_stages);
   {%- endif %}
 
     int newton_iter_val = {{ solver_options.sim_method_newton_iter }};
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_newton_iter", &newton_iter_val);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_newton_iter", &newton_iter_val);
 
 
     // set up sim_method_jac_reuse
@@ -1991,7 +1822,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
   {%- if all_equal == true %}
     bool tmp_bool = (bool) {{ solver_options.sim_method_jac_reuse[0] }};
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_jac_reuse", &tmp_bool);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_jac_reuse", &tmp_bool);
   {%- else %}
     bool* sim_method_jac_reuse = malloc(N*sizeof(bool));
     {%- for j in range(end=dims.N) %}
@@ -1999,104 +1830,114 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     {%- endfor %}
 
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_jac_reuse", &sim_method_jac_reuse[i]);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_jac_reuse", &sim_method_jac_reuse[i]);
     free(sim_method_jac_reuse);
   {%- endif %}
 
 {%- endif %}
 
     double nlp_solver_step_length = {{ solver_options.nlp_solver_step_length }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "step_length", &nlp_solver_step_length);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "step_length", &nlp_solver_step_length);
 
     {%- if solver_options.nlp_solver_warm_start_first_qp %}
     int nlp_solver_warm_start_first_qp = {{ solver_options.nlp_solver_warm_start_first_qp }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "warm_start_first_qp", &nlp_solver_warm_start_first_qp);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "warm_start_first_qp", &nlp_solver_warm_start_first_qp);
     {%- endif %}
 
     double levenberg_marquardt = {{ solver_options.levenberg_marquardt }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "levenberg_marquardt", &levenberg_marquardt);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "levenberg_marquardt", &levenberg_marquardt);
 
     /* options QP solver */
 {%- if solver_options.qp_solver is starting_with("PARTIAL_CONDENSING") %}
     int qp_solver_cond_N;
 
-    {%- if solver_options.qp_solver_cond_N %}
-    qp_solver_cond_N = {{ solver_options.qp_solver_cond_N }};
-    {% else %}
+    {% if solver_options.qp_solver_cond_N -%}
+    const int qp_solver_cond_N_ori = {{ solver_options.qp_solver_cond_N }};
+    qp_solver_cond_N = N < qp_solver_cond_N_ori ? N : qp_solver_cond_N_ori; // use the minimum value here
+    {%- else %}
     // NOTE: there is no condensing happening here!
     qp_solver_cond_N = N;
     {%- endif %}
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_cond_N", &qp_solver_cond_N);
-{% endif %}
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_N", &qp_solver_cond_N);
+{%- endif %}
+
+
+{% if solver_options.nlp_solver_type == "SQP" %}
+    // set SQP specific options
+    double nlp_solver_tol_stat = {{ solver_options.nlp_solver_tol_stat }};
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_stat", &nlp_solver_tol_stat);
+
+    double nlp_solver_tol_eq = {{ solver_options.nlp_solver_tol_eq }};
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_eq", &nlp_solver_tol_eq);
+
+    double nlp_solver_tol_ineq = {{ solver_options.nlp_solver_tol_ineq }};
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_ineq", &nlp_solver_tol_ineq);
+
+    double nlp_solver_tol_comp = {{ solver_options.nlp_solver_tol_comp }};
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_comp", &nlp_solver_tol_comp);
+
+    int nlp_solver_max_iter = {{ solver_options.nlp_solver_max_iter }};
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "max_iter", &nlp_solver_max_iter);
+
+    int initialize_t_slacks = {{ solver_options.initialize_t_slacks }};
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "initialize_t_slacks", &initialize_t_slacks);
+{%- endif %}
 
     int qp_solver_iter_max = {{ solver_options.qp_solver_iter_max }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_iter_max", &qp_solver_iter_max);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_iter_max", &qp_solver_iter_max);
 
+{# NOTE: qp_solver tolerances must be set after NLP ones, since the setter for NLP tolerances sets the QP tolerances to the sam values. #}
     {%- if solver_options.qp_solver_tol_stat %}
     double qp_solver_tol_stat = {{ solver_options.qp_solver_tol_stat }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_tol_stat", &qp_solver_tol_stat);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_stat", &qp_solver_tol_stat);
     {%- endif -%}
 
     {%- if solver_options.qp_solver_tol_eq %}
     double qp_solver_tol_eq = {{ solver_options.qp_solver_tol_eq }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_tol_eq", &qp_solver_tol_eq);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_eq", &qp_solver_tol_eq);
     {%- endif -%}
 
     {%- if solver_options.qp_solver_tol_ineq %}
     double qp_solver_tol_ineq = {{ solver_options.qp_solver_tol_ineq }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_tol_ineq", &qp_solver_tol_ineq);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_ineq", &qp_solver_tol_ineq);
     {%- endif -%}
 
     {%- if solver_options.qp_solver_tol_comp %}
     double qp_solver_tol_comp = {{ solver_options.qp_solver_tol_comp }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_tol_comp", &qp_solver_tol_comp);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_comp", &qp_solver_tol_comp);
     {%- endif -%}
 
     {%- if solver_options.qp_solver_warm_start %}
     int qp_solver_warm_start = {{ solver_options.qp_solver_warm_start }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_warm_start", &qp_solver_warm_start);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_warm_start", &qp_solver_warm_start);
     {%- endif -%}
-    
-{% if solver_options.nlp_solver_type == "SQP" %}
-    // set SQP specific options
-    double nlp_solver_tol_stat = {{ solver_options.nlp_solver_tol_stat }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "tol_stat", &nlp_solver_tol_stat);
-
-    double nlp_solver_tol_eq = {{ solver_options.nlp_solver_tol_eq }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "tol_eq", &nlp_solver_tol_eq);
-
-    double nlp_solver_tol_ineq = {{ solver_options.nlp_solver_tol_ineq }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "tol_ineq", &nlp_solver_tol_ineq);
-
-    double nlp_solver_tol_comp = {{ solver_options.nlp_solver_tol_comp }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "tol_comp", &nlp_solver_tol_comp);
-
-    int nlp_solver_max_iter = {{ solver_options.nlp_solver_max_iter }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "max_iter", &nlp_solver_max_iter);
-
-    int initialize_t_slacks = {{ solver_options.initialize_t_slacks }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "initialize_t_slacks", &initialize_t_slacks);
-{%- endif %}
 
     int print_level = {{ solver_options.print_level }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "print_level", &print_level);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "print_level", &print_level);
 
 
     int ext_cost_num_hess = {{ solver_options.ext_cost_num_hess }};
 {%- if cost.cost_type == "EXTERNAL" %}
     for (int i = 0; i < N; i++)
     {
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "cost_numerical_hessian", &ext_cost_num_hess);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "cost_numerical_hessian", &ext_cost_num_hess);
     }
 {%- endif %}
 {%- if cost.cost_type_e == "EXTERNAL" %}
-    ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, N, "cost_numerical_hessian", &ext_cost_num_hess);
+    ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, N, "cost_numerical_hessian", &ext_cost_num_hess);
 {%- endif %}
+}
 
 
-    /* out */
-    ocp_nlp_out * nlp_out = ocp_nlp_out_create(nlp_config, nlp_dims);
-    capsule->nlp_out = nlp_out;
+/**
+ * Internal function for {{ model.name }}_acados_create: step 7
+ */
+void {{ model.name }}_acados_create_7_set_nlp_out({{ model.name }}_solver_capsule* capsule)
+{
+    const int N = capsule->nlp_solver_plan->N;
+    ocp_nlp_config* nlp_config = capsule->nlp_config;
+    ocp_nlp_dims* nlp_dims = capsule->nlp_dims;
+    ocp_nlp_out* nlp_out = capsule->nlp_out;
 
     // initialize primal solution
     double* xu0 = calloc(NX+NU, sizeof(double));
@@ -2123,39 +1964,114 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
     ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x0);
     free(xu0);
-    
-    capsule->nlp_solver = ocp_nlp_solver_create(nlp_config, nlp_dims, capsule->nlp_opts);
+}
 
 
-{% if dims.np > 0 %}
-    // initialize parameters to nominal value
-    double* p = calloc(NP, sizeof(double));
-    {% for item in parameter_values %}
-        {%- if item != 0 %}
-    p[{{ loop.index0 }}] = {{ item }};
-        {%- endif %}
-    {%- endfor %}
+/**
+ * Internal function for {{ model.name }}_acados_create: step 8
+ */
+//void {{ model.name }}_acados_create_8_create_solver({{ model.name }}_solver_capsule* capsule)
+//{
+//    capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts);
+//}
 
-    for (int i = 0; i <= N; i++)
-    {
-        {{ model.name }}_acados_update_params(capsule, i, p, NP);
+/**
+ * Internal function for {{ model.name }}_acados_create: step 9
+ */
+int {{ model.name }}_acados_create_9_precompute({{ model.name }}_solver_capsule* capsule) {
+    int status = ocp_nlp_precompute(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out);
+
+    if (status != ACADOS_SUCCESS) {
+        printf("\nocp_nlp_precompute failed!\n\n");
+        exit(1);
     }
-    free(p);
-{%- endif %}{# if dims.np #}
 
-    status = ocp_nlp_precompute(capsule->nlp_solver, nlp_in, nlp_out);
+    return status;
+}
 
-    if (status != ACADOS_SUCCESS)
-    {
-        printf("\nocp_precompute failed!\n\n");
-        exit(1);
+
+int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_capsule* capsule, int N, double* new_time_steps)
+{
+    // If N does not match the number of shooting intervals used for code generation, new_time_steps must be given.
+    if (N != {{ model.name | upper }}_N && !new_time_steps) {
+        fprintf(stderr, "{{ model.name }}_acados_create_with_discretization: new_time_steps is NULL " \
+            "but the number of shooting intervals (= %d) differs from the number of " \
+            "shooting intervals (= %d) during code generation! Please provide a new vector of time_stamps!\n", \
+             N, {{ model.name | upper }}_N);
+        return 1;
     }
 
+    // number of expected runtime parameters
+    capsule->nlp_np = NP;
+
+    // 1) create and set nlp_solver_plan; create nlp_config
+    capsule->nlp_solver_plan = ocp_nlp_plan_create(N);
+    {{ model.name }}_acados_create_1_set_plan(capsule->nlp_solver_plan, N);
+    capsule->nlp_config = ocp_nlp_config_create(*capsule->nlp_solver_plan);
+
+    // 3) create and set dimensions
+    capsule->nlp_dims = {{ model.name }}_acados_create_2_create_and_set_dimensions(capsule);
+    {{ model.name }}_acados_create_3_create_and_set_functions(capsule);
+
+    // 4) set default parameters in functions
+    {{ model.name }}_acados_create_4_set_default_parameters(capsule);
+
+    // 5) create and set nlp_in
+    capsule->nlp_in = ocp_nlp_in_create(capsule->nlp_config, capsule->nlp_dims);
+    {{ model.name }}_acados_create_5_set_nlp_in(capsule, N, new_time_steps);
+
+    // 6) create and set nlp_opts
+    capsule->nlp_opts = ocp_nlp_solver_opts_create(capsule->nlp_config, capsule->nlp_dims);
+    {{ model.name }}_acados_create_6_set_opts(capsule);
+
+    // 7) create and set nlp_out
+    // 7.1) nlp_out
+    capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims);
+    // 7.2) sens_out
+    capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims);
+    {{ model.name }}_acados_create_7_set_nlp_out(capsule);
+
+    // 8) create solver
+    capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts);
+    //{{ model.name }}_acados_create_8_create_solver(capsule);
+
+    // 9) do precomputations
+    int status = {{ model.name }}_acados_create_9_precompute(capsule);
+    return status;
+}
+
+/**
+ * This function is for updating an already initialized solver with a different number of qp_cond_N. It is useful for code reuse after code export.
+ */
+int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_capsule* capsule, int qp_solver_cond_N)
+{
+{%- if solver_options.qp_solver is starting_with("PARTIAL_CONDENSING") %}
+    // 1) destroy solver
+    ocp_nlp_solver_destroy(capsule->nlp_solver);
+
+    // 2) set new value for "qp_cond_N"
+    const int N = capsule->nlp_solver_plan->N;
+    if(qp_solver_cond_N > N)
+        printf("Warning: qp_solver_cond_N = %d > N = %d\n", qp_solver_cond_N, N);
+    ocp_nlp_solver_opts_set(capsule->nlp_config, capsule->nlp_opts, "qp_cond_N", &qp_solver_cond_N);
+
+    // 3) continue with the remaining steps from {{ model.name }}_acados_create_with_discretization(...):
+    // -> 8) create solver
+    capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts);
+
+    // -> 9) do precomputations
+    int status = {{ model.name }}_acados_create_9_precompute(capsule);
     return status;
+{%- else %}
+    printf("\nacados_update_qp_solver_cond_N() failed, since no partial condensing solver is used!\n\n");
+    // Todo: what is an adequate behavior here?
+    exit(1);
+    return -1;
+{%- endif %}
 }
 
 
-int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * capsule, int stage, double *p, int np)
+int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsule, int stage, double *p, int np)
 {
     int solver_status = 0;
 
@@ -2189,11 +2105,14 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * caps
         capsule->hess_vde_casadi[stage].set_param(capsule->hess_vde_casadi+stage, p);
         {%- endif %}
     {% elif solver_options.integrator_type == "GNSF" %}
+    {% if model.gnsf.purely_linear != 1 %}
         capsule->gnsf_phi_fun[stage].set_param(capsule->gnsf_phi_fun+stage, p);
         capsule->gnsf_phi_fun_jac_y[stage].set_param(capsule->gnsf_phi_fun_jac_y+stage, p);
         capsule->gnsf_phi_jac_y_uhat[stage].set_param(capsule->gnsf_phi_jac_y_uhat+stage, p);
-
-        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[stage].set_param(capsule->gnsf_f_lo_jac_x1_x1dot_u_z+stage, p);
+        {% if model.gnsf.nontrivial_f_LO == 1 %}
+            capsule->gnsf_f_lo_jac_x1_x1dot_u_z[stage].set_param(capsule->gnsf_f_lo_jac_x1_x1dot_u_z+stage, p);
+        {%- endif %}
+    {%- endif %}
     {% elif solver_options.integrator_type == "DISCRETE" %}
         capsule->discr_dyn_phi_fun[stage].set_param(capsule->discr_dyn_phi_fun+stage, p);
         capsule->discr_dyn_phi_fun_jac_ut_xt[stage].set_param(capsule->discr_dyn_phi_fun_jac_ut_xt+stage, p);
@@ -2271,7 +2190,7 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * caps
 
 
 
-int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule)
+int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule* capsule)
 {
     // solve NLP 
     int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out);
@@ -2280,7 +2199,7 @@ int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule)
 }
 
 
-int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule)
+int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule)
 {
     // before destroying, keep some info
     const int N = capsule->nlp_solver_plan->N;
@@ -2288,6 +2207,7 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule)
     ocp_nlp_solver_opts_destroy(capsule->nlp_opts);
     ocp_nlp_in_destroy(capsule->nlp_in);
     ocp_nlp_out_destroy(capsule->nlp_out);
+    ocp_nlp_out_destroy(capsule->sens_out);
     ocp_nlp_solver_destroy(capsule->nlp_solver);
     ocp_nlp_dims_destroy(capsule->nlp_dims);
     ocp_nlp_config_destroy(capsule->nlp_config);
@@ -2339,16 +2259,24 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule)
 {%- elif solver_options.integrator_type == "GNSF" %}
     for (int i = 0; i < N; i++)
     {
+        {% if model.gnsf.purely_linear != 1 %}
         external_function_param_casadi_free(&capsule->gnsf_phi_fun[i]);
         external_function_param_casadi_free(&capsule->gnsf_phi_fun_jac_y[i]);
         external_function_param_casadi_free(&capsule->gnsf_phi_jac_y_uhat[i]);
+        {% if model.gnsf.nontrivial_f_LO == 1 %}
         external_function_param_casadi_free(&capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i]);
+        {%- endif %}
+        {%- endif %}
         external_function_param_casadi_free(&capsule->gnsf_get_matrices_fun[i]);
     }
+  {% if model.gnsf.purely_linear != 1 %}
     free(capsule->gnsf_phi_fun);
     free(capsule->gnsf_phi_fun_jac_y);
     free(capsule->gnsf_phi_jac_y_uhat);
+  {% if model.gnsf.nontrivial_f_LO == 1 %}
     free(capsule->gnsf_f_lo_jac_x1_x1dot_u_z);
+  {%- endif %}
+  {%- endif %}
     free(capsule->gnsf_get_matrices_fun);
 {%- elif solver_options.integrator_type == "DISCRETE" %}
     for (int i = 0; i < N; i++)
@@ -2448,34 +2376,36 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule)
     return 0;
 }
 
-ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_in; }
-ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_out; }
-ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_solver; }
-ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_config; }
-void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_opts; }
-ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_dims; }
-ocp_nlp_plan *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_solver_plan; }
+ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_in; }
+ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_out; }
+ocp_nlp_out *{{ model.name }}_acados_get_sens_out({{ model.name }}_solver_capsule* capsule) { return capsule->sens_out; }
+ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_solver; }
+ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_config; }
+void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_opts; }
+ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_dims; }
+ocp_nlp_plan_t *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_solver_plan; }
 
 
-void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsule)
+void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule* capsule)
 {
     int sqp_iter, stat_m, stat_n, tmp_int;
     ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "sqp_iter", &sqp_iter);
     ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "stat_n", &stat_n);
     ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "stat_m", &stat_m);
 
-    {% set stat_n_max = 10 %}
+    {% set stat_n_max = 12 %}
     double stat[{{ solver_options.nlp_solver_max_iter * stat_n_max }}];
     ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "statistics", stat);
 
     int nrow = sqp_iter+1 < stat_m ? sqp_iter+1 : stat_m;
 
-    printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\n");
+{%- if solver_options.nlp_solver_type == "SQP" %}
+    printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha\n");
     for (int i = 0; i < nrow; i++)
     {
         for (int j = 0; j < stat_n + 1; j++)
         {
-            if (j == 0 || j > 4)
+            if (j == 0 || j == 5 || j == 6)
             {
                 tmp_int = (int) stat[i + j * nrow];
                 printf("%d\t", tmp_int);
@@ -2487,5 +2417,17 @@ void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsu
         }
         printf("\n");
     }
+{% else %}
+    printf("iter\tqp_stat\tqp_iter\n");
+    for (int i = 0; i < nrow; i++)
+    {
+        for (int j = 0; j < stat_n + 1; j++)
+        {
+            tmp_int = (int) stat[i + j * nrow];
+            printf("%d\t", tmp_int);
+        }
+        printf("\n");
+    }
+{%- endif %}
 }
 
diff --git a/pyextra/acados_template/c_templates_tera/acados_solver.in.h b/pyextra/acados_template/c_templates_tera/acados_solver.in.h
index e8c0a38ca3..2efc9e5157 100644
--- a/pyextra/acados_template/c_templates_tera/acados_solver.in.h
+++ b/pyextra/acados_template/c_templates_tera/acados_solver.in.h
@@ -78,9 +78,10 @@ typedef struct {{ model.name }}_solver_capsule
     // acados objects
     ocp_nlp_in *nlp_in;
     ocp_nlp_out *nlp_out;
+    ocp_nlp_out *sens_out;
     ocp_nlp_solver *nlp_solver;
     void *nlp_opts;
-    ocp_nlp_plan *nlp_solver_plan;
+    ocp_nlp_plan_t *nlp_solver_plan;
     ocp_nlp_config *nlp_config;
     ocp_nlp_dims *nlp_dims;
 
@@ -186,6 +187,10 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
  * nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0.
  */
 int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps);
+/**
+ * This function is used for updating an already initialized solver with a different number of qp_cond_N.
+ */
+int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_capsule * capsule, int qp_solver_cond_N);
 int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * capsule, int stage, double *value, int np);
 int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule);
 int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule);
@@ -193,11 +198,12 @@ void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsu
 
 ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule * capsule);
 ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule * capsule);
+ocp_nlp_out *{{ model.name }}_acados_get_sens_out({{ model.name }}_solver_capsule * capsule);
 ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule * capsule);
 ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule * capsule);
 void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule * capsule);
 ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule * capsule);
-ocp_nlp_plan *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule);
+ocp_nlp_plan_t *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule);
 
 #ifdef __cplusplus
 } /* extern "C" */
diff --git a/pyextra/acados_template/c_templates_tera/acados_solver.in.pxd b/pyextra/acados_template/c_templates_tera/acados_solver.in.pxd
index 8387980c24..2cc5ee3910 100644
--- a/pyextra/acados_template/c_templates_tera/acados_solver.in.pxd
+++ b/pyextra/acados_template/c_templates_tera/acados_solver.in.pxd
@@ -1,3 +1,36 @@
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
 cimport acados_solver_common
 
 cdef extern from "acados_solver_{{ model.name }}.h":
@@ -8,6 +41,11 @@ cdef extern from "acados_solver_{{ model.name }}.h":
     int acados_free_capsule "{{ model.name }}_acados_free_capsule"(nlp_solver_capsule *capsule)
 
     int acados_create "{{ model.name }}_acados_create"(nlp_solver_capsule * capsule)
+
+    int acados_create_with_discretization "{{ model.name }}_acados_create_with_discretization"(nlp_solver_capsule * capsule, int n_time_steps, double* new_time_steps)
+    int acados_update_time_steps "{{ model.name }}_acados_update_time_steps"(nlp_solver_capsule * capsule, int N, double* new_time_steps)
+    int acados_update_qp_solver_cond_N "{{ model.name }}_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N)
+
     int acados_update_params "{{ model.name }}_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_)
     int acados_solve "{{ model.name }}_acados_solve"(nlp_solver_capsule * capsule)
     int acados_free "{{ model.name }}_acados_free"(nlp_solver_capsule * capsule)
@@ -15,6 +53,7 @@ cdef extern from "acados_solver_{{ model.name }}.h":
 
     acados_solver_common.ocp_nlp_in *acados_get_nlp_in "{{ model.name }}_acados_get_nlp_in"(nlp_solver_capsule * capsule)
     acados_solver_common.ocp_nlp_out *acados_get_nlp_out "{{ model.name }}_acados_get_nlp_out"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_out *acados_get_sens_out "{{ model.name }}_acados_get_sens_out"(nlp_solver_capsule * capsule)
     acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "{{ model.name }}_acados_get_nlp_solver"(nlp_solver_capsule * capsule)
     acados_solver_common.ocp_nlp_config *acados_get_nlp_config "{{ model.name }}_acados_get_nlp_config"(nlp_solver_capsule * capsule)
     void *acados_get_nlp_opts "{{ model.name }}_acados_get_nlp_opts"(nlp_solver_capsule * capsule)
diff --git a/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c b/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c
index a6cd1faa8c..a27ba8837f 100644
--- a/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c
@@ -37,7 +37,7 @@
 #define MDL_START
 
 // acados
-#include "acados/utils/print.h"
+// #include "acados/utils/print.h"
 #include "acados_c/sim_interface.h"
 #include "acados_c/external_function_interface.h"
 
diff --git a/pyextra/acados_template/c_templates_tera/main.in.c b/pyextra/acados_template/c_templates_tera/main.in.c
index 3348eea5cb..99c4f13be1 100644
--- a/pyextra/acados_template/c_templates_tera/main.in.c
+++ b/pyextra/acados_template/c_templates_tera/main.in.c
@@ -156,11 +156,12 @@ int main()
     for (int ii = 0; ii < NTIMINGS; ii++)
     {
         // initialize solution
-        for (int i = 0; i <= nlp_dims->N; i++)
+        for (int i = 0; i < N; i++)
         {
             ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init);
             ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
         }
+        ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x_init);
         ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &rti_phase);
         status = {{ model.name }}_acados_solve(acados_ocp_capsule);
         ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);
diff --git a/pyextra/acados_template/c_templates_tera/make_sfun.in.m b/pyextra/acados_template/c_templates_tera/make_sfun.in.m
index 172da654ee..30339bf5dc 100644
--- a/pyextra/acados_template/c_templates_tera/make_sfun.in.m
+++ b/pyextra/acados_template/c_templates_tera/make_sfun.in.m
@@ -46,10 +46,14 @@ SOURCES = { ...
             '{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c',...
             {%- endif %}
         {%- elif solver_options.integrator_type == "GNSF" %}
+            {% if model.gnsf.purely_linear != 1 %}
             '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c',...
             '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c',...
             '{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c',...
+            {% if model.gnsf.nontrivial_f_LO == 1 %}
             '{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c',...
+            {%- endif %}
+            {%- endif %}
             '{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c',...
         {%- elif solver_options.integrator_type == "DISCRETE" %}
             '{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c',...
diff --git a/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m b/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m
index 1c5cf0b123..a0c503e41a 100644
--- a/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m
+++ b/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m
@@ -47,10 +47,14 @@ SOURCES = [ 'acados_sim_solver_sfunction_{{ model.name }}.c ', ...
             '{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c ',...
                 {%- endif %}
             {%- elif solver_options.integrator_type == "GNSF" %}
+                {% if model.gnsf.purely_linear != 1 %}
             '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c '
             '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c '
             '{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c '
+                {% if model.gnsf.nontrivial_f_LO == 1 %}
             '{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c '
+                {%- endif %}
+                {%- endif %}
             '{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c '
             {%- endif %}
           ];
diff --git a/pyextra/acados_template/c_templates_tera/mex_solver.in.m b/pyextra/acados_template/c_templates_tera/mex_solver.in.m
index 728741a46e..278e0a605c 100644
--- a/pyextra/acados_template/c_templates_tera/mex_solver.in.m
+++ b/pyextra/acados_template/c_templates_tera/mex_solver.in.m
@@ -125,15 +125,15 @@ classdef {{ model.name }}_mex_solver < handle
             if strcmp(field, 'stat')
                 stat = obj.get('stat');
                 {%- if solver_options.nlp_solver_type == "SQP" %}
-                fprintf('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter');
-                if size(stat,2)>7
+                fprintf('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha');
+                if size(stat,2)>8
                     fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp');
                 end
                 fprintf('\n');
                 for jj=1:size(stat,1)
-                    fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d', stat(jj,1), stat(jj,2), stat(jj,3), stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7));
-                    if size(stat,2)>7
-                        fprintf('\t%e\t%e\t%e\t%e', stat(jj,8), stat(jj,9), stat(jj,10), stat(jj,11));
+                    fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d\t%e', stat(jj,1), stat(jj,2), stat(jj,3), stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7), stat(jj, 8));
+                    if size(stat,2)>8
+                        fprintf('\t%e\t%e\t%e\t%e', stat(jj,9), stat(jj,10), stat(jj,11), stat(jj,12));
                     end
                     fprintf('\n');
                 end
diff --git a/pyextra/acados_template/c_templates_tera/model.in.h b/pyextra/acados_template/c_templates_tera/model.in.h
index 661811232c..918e2bc29e 100644
--- a/pyextra/acados_template/c_templates_tera/model.in.h
+++ b/pyextra/acados_template/c_templates_tera/model.in.h
@@ -90,14 +90,7 @@ int {{ model.name }}_impl_dae_hess_n_out(void);
 
 {% elif solver_options.integrator_type == "GNSF" %}
 /* GNSF Functions */
-// used to import model matrices
-int        {{ model.name }}_gnsf_get_matrices_fun(const double** arg, double** res, int* iw, double* w, void *mem);
-int        {{ model.name }}_gnsf_get_matrices_fun_work(int *, int *, int *, int *);
-const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_in(int);
-const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_out(int);
-int        {{ model.name }}_gnsf_get_matrices_fun_n_in(void);
-int        {{ model.name }}_gnsf_get_matrices_fun_n_out(void);
-
+	{% if model.gnsf.purely_linear != 1 %}
 // phi_fun
 int        {{ model.name }}_gnsf_phi_fun(const double** arg, double** res, int* iw, double* w, void *mem);
 int        {{ model.name }}_gnsf_phi_fun_work(int *, int *, int *, int *);
@@ -121,7 +114,7 @@ const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in(int);
 const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out(int);
 int        {{ model.name }}_gnsf_phi_jac_y_uhat_n_in(void);
 int        {{ model.name }}_gnsf_phi_jac_y_uhat_n_out(void);
-
+	{% if model.gnsf.nontrivial_f_LO == 1 %}
 // f_lo_fun_jac_x1k1uz
 int        {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz(const double** arg, double** res, int* iw, double* w, void *mem);
 int        {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work(int *, int *, int *, int *);
@@ -129,6 +122,15 @@ const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in(int);
 const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out(int);
 int        {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in(void);
 int        {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out(void);
+	{%- endif %}
+	{%- endif %}
+// used to import model matrices
+int        {{ model.name }}_gnsf_get_matrices_fun(const double** arg, double** res, int* iw, double* w, void *mem);
+int        {{ model.name }}_gnsf_get_matrices_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_in(int);
+const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_out(int);
+int        {{ model.name }}_gnsf_get_matrices_fun_n_in(void);
+int        {{ model.name }}_gnsf_get_matrices_fun_n_out(void);
 
 {% elif solver_options.integrator_type == "ERK" %}
 /* explicit ODE */
diff --git a/pyextra/acados_template/utils.py b/pyextra/acados_template/utils.py
index bf8ae4d5db..a60e99fc75 100644
--- a/pyextra/acados_template/utils.py
+++ b/pyextra/acados_template/utils.py
@@ -254,22 +254,6 @@ def format_class_dict(d):
     return out
 
 
-def acados_class2dict(class_instance):
-    """
-    removes the __ artifact from class to dict conversion
-    """
-
-    d = dict(class_instance.__dict__)
-    out = {}
-    for k, v in d.items():
-        if isinstance(v, dict):
-            v = format_class_dict(v)
-
-        out_key = k.split('__', 1)[-1]
-        out[k.replace(k, out_key)] = v
-    return out
-
-
 def get_ocp_nlp_layout():
     python_interface_path = get_python_interface_path()
     abs_path = os.path.join(python_interface_path, 'acados_layout.json')
@@ -433,6 +417,13 @@ def set_up_imported_gnsf_model(acados_formulation):
     acados_formulation.model.phi_jac_y_uhat = phi_jac_y_uhat
     acados_formulation.model.get_matrices_fun = get_matrices_fun
 
+    # get_matrices_fun = Function([model_name,'_gnsf_get_matrices_fun'], {dummy},...
+    #  {A, B, C, E, L_x, L_xdot, L_z, L_u, A_LO, c, E_LO, B_LO,...
+    #   nontrivial_f_LO, purely_linear, ipiv_x, ipiv_z, c_LO});
+    get_matrices_out = get_matrices_fun(0)
+    acados_formulation.model.gnsf['nontrivial_f_LO'] = int(get_matrices_out[12])
+    acados_formulation.model.gnsf['purely_linear'] = int(get_matrices_out[13])
+
     if "f_lo_fun_jac_x1k1uz" in gnsf:
         f_lo_fun_jac_x1k1uz = Function.deserialize(gnsf['f_lo_fun_jac_x1k1uz'])
         acados_formulation.model.f_lo_fun_jac_x1k1uz = f_lo_fun_jac_x1k1uz
diff --git a/rednose_repo b/rednose_repo
index 5b526a8e00..7663289f1e 160000
--- a/rednose_repo
+++ b/rednose_repo
@@ -1 +1 @@
-Subproject commit 5b526a8e00bdc1c3922be470af1602cf9dc72dde
+Subproject commit 7663289f1e68860f53dc34337ef080dde69a2586
diff --git a/release/files_common b/release/files_common
index cfc8150e3b..d2d5f14ae4 100644
--- a/release/files_common
+++ b/release/files_common
@@ -17,6 +17,7 @@ site_scons/site_tools/cython.py
 
 common/.gitignore
 common/__init__.py
+common/conversions.py
 common/gpio.py
 common/realtime.py
 common/clock.pyx
@@ -58,7 +59,6 @@ common/transformations/transformations.pyx
 common/api/__init__.py
 
 models/supercombo.dlc
-models/big_supercombo.dlc
 models/dmonitoring_model_q.dlc
 
 release/*
@@ -70,12 +70,10 @@ installer/updater/updater
 selfdrive/version.py
 
 selfdrive/__init__.py
-selfdrive/config.py
 selfdrive/sentry.py
 selfdrive/swaglog.py
 selfdrive/logmessaged.py
 selfdrive/tombstoned.py
-selfdrive/pandad.py
 selfdrive/updated.py
 selfdrive/rtshield.py
 selfdrive/statsd.py
@@ -99,6 +97,7 @@ selfdrive/boardd/panda.h
 selfdrive/boardd/pigeon.cc
 selfdrive/boardd/pigeon.h
 selfdrive/boardd/set_time.py
+selfdrive/boardd/pandad.py
 
 selfdrive/car/__init__.py
 selfdrive/car/car_helpers.py
@@ -337,11 +336,11 @@ selfdrive/sensord/sensord
 
 selfdrive/thermald/thermald.py
 selfdrive/thermald/power_monitoring.py
+selfdrive/thermald/fan_controller.py
 
 selfdrive/test/__init__.py
 selfdrive/test/helpers.py
 selfdrive/test/setup_device_ci.sh
-selfdrive/test/test_fingerprints.py
 selfdrive/test/test_onroad.py
 
 selfdrive/ui/.gitignore
@@ -424,6 +423,7 @@ selfdrive/modeld/transforms/transform.cc
 selfdrive/modeld/transforms/transform.h
 selfdrive/modeld/transforms/transform.cl
 
+selfdrive/modeld/thneed/*.py
 selfdrive/modeld/thneed/thneed.*
 selfdrive/modeld/thneed/serialize.cc
 selfdrive/modeld/thneed/compile.cc
diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py
index 7250754d54..1bb99bf2d1 100755
--- a/selfdrive/athena/athenad.py
+++ b/selfdrive/athena/athenad.py
@@ -163,8 +163,6 @@ def upload_handler(end_event: threading.Event) -> None:
   sm = messaging.SubMaster(['deviceState'])
   tid = threading.get_ident()
 
-  cellular_unmetered = Params().get_bool("CellularUnmetered")
-
   while not end_event.is_set():
     cur_upload_items[tid] = None
 
@@ -181,46 +179,45 @@ def upload_handler(end_event: threading.Event) -> None:
         cloudlog.event("athena.upload_handler.expired", item=cur_upload_items[tid], error=True)
         continue
 
-      # Check if uploading over cell is allowed
+      # Check if uploading over metered connection is allowed
       sm.update(0)
-      cell = sm['deviceState'].networkType not in [NetworkType.wifi, NetworkType.ethernet]
-      if cell and (not cur_upload_items[tid].allow_cellular) and (not cellular_unmetered):
+      metered = sm['deviceState'].networkMetered
+      network_type = sm['deviceState'].networkType.raw
+      if metered and (not cur_upload_items[tid].allow_cellular):
         retry_upload(tid, end_event, False)
         continue
 
       try:
         def cb(sz, cur):
-          # Abort transfer if connection changed to cell after starting upload
+          # Abort transfer if connection changed to metered after starting upload
           sm.update(0)
-          cell = sm['deviceState'].networkType not in [NetworkType.wifi, NetworkType.ethernet]
-          if cell and (not cur_upload_items[tid].allow_cellular) and (not cellular_unmetered):
+          metered = sm['deviceState'].networkMetered
+          if metered and (not cur_upload_items[tid].allow_cellular):
             raise AbortTransferException
 
           cur_upload_items[tid] = cur_upload_items[tid]._replace(progress=cur / sz if sz else 1)
 
-
-        network_type = sm['deviceState'].networkType.raw
         fn = cur_upload_items[tid].path
         try:
           sz = os.path.getsize(fn)
         except OSError:
           sz = -1
 
-        cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type)
+        cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type, metered=metered)
         response = _do_upload(cur_upload_items[tid], cb)
 
         if response.status_code not in (200, 201, 403, 412):
-          cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type)
+          cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type, metered=metered)
           retry_upload(tid, end_event)
         else:
-          cloudlog.event("athena.upload_handler.success", fn=fn, sz=sz, network_type=network_type)
+          cloudlog.event("athena.upload_handler.success", fn=fn, sz=sz, network_type=network_type, metered=metered)
 
         UploadQueueCache.cache(upload_queue)
       except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError):
-        cloudlog.event("athena.upload_handler.timeout", fn=fn, sz=sz, network_type=network_type)
+        cloudlog.event("athena.upload_handler.timeout", fn=fn, sz=sz, network_type=network_type, metered=metered)
         retry_upload(tid, end_event)
       except AbortTransferException:
-        cloudlog.event("athena.upload_handler.abort", fn=fn, sz=sz, network_type=network_type)
+        cloudlog.event("athena.upload_handler.abort", fn=fn, sz=sz, network_type=network_type, metered=metered)
         retry_upload(tid, end_event, False)
 
     except queue.Empty:
@@ -459,6 +456,12 @@ def getNetworkType():
   return HARDWARE.get_network_type()
 
 
+@dispatcher.add_method
+def getNetworkMetered():
+  network_type = HARDWARE.get_network_type()
+  return HARDWARE.get_network_metered(network_type)
+
+
 @dispatcher.add_method
 def getNetworks():
   return HARDWARE.get_networks()
diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index d7e1adf1eb..9ae00d4e90 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -371,7 +371,7 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
 
     size_t j = 0;
     for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION);
-        f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_TICK); f++) {
+        f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_EXTI); f++) {
       if (fault_bits.test(f)) {
         faults.set(j, cereal::PandaState::FaultType(f));
         j++;
diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc
index 5e621b12cd..4a7af2b718 100644
--- a/selfdrive/boardd/panda.cc
+++ b/selfdrive/boardd/panda.cc
@@ -338,6 +338,10 @@ void Panda::set_power_saving(bool power_saving) {
   usb_write(0xe7, power_saving, 0);
 }
 
+void Panda::enable_deepsleep() {
+  usb_write(0xfb, 0, 0);
+}
+
 void Panda::set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode) {
   usb_write(0xe6, (uint16_t)power_mode, 0);
 }
diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h
index dbd866adf4..279c96a6a6 100644
--- a/selfdrive/boardd/panda.h
+++ b/selfdrive/boardd/panda.h
@@ -85,6 +85,7 @@ class Panda {
   std::optional<std::vector<uint8_t>> get_firmware_version();
   std::optional<std::string> get_serial();
   void set_power_saving(bool power_saving);
+  void enable_deepsleep();
   void set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode);
   void send_heartbeat(bool engaged);
   void set_can_speed_kbps(uint16_t bus, uint16_t speed);
diff --git a/selfdrive/pandad.py b/selfdrive/boardd/pandad.py
similarity index 100%
rename from selfdrive/pandad.py
rename to selfdrive/boardd/pandad.py
diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc
index f534cad99c..20edab14ea 100644
--- a/selfdrive/camerad/cameras/camera_common.cc
+++ b/selfdrive/camerad/cameras/camera_common.cc
@@ -161,6 +161,8 @@ bool CameraBuf::acquire() {
   cl_mem camrabuf_cl = camera_bufs[cur_buf_idx].buf_cl;
   cl_event event;
 
+  float start_time = millis_since_boot();
+
   if (debayer) {
     float gain = 0.0;
 
@@ -181,6 +183,8 @@ bool CameraBuf::acquire() {
   cur_yuv_buf = vipc_server->get_buffer(yuv_type);
   rgb2yuv->queue(q, cur_rgb_buf->buf_cl, cur_yuv_buf->buf_cl);
 
+  cur_frame_data.processing_time = (millis_since_boot() - start_time) / 1000.0;
+
   VisionIpcBufExtra extra = {
                         cur_frame_data.frame_id,
                         cur_frame_data.timestamp_sof,
@@ -219,6 +223,7 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr
   framed.setLensPos(frame_data.lens_pos);
   framed.setLensErr(frame_data.lens_err);
   framed.setLensTruePos(frame_data.lens_true_pos);
+  framed.setProcessingTime(frame_data.processing_time);
 }
 
 kj::Array<uint8_t> get_frame_image(const CameraBuf *b) {
diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h
index c2d2904f54..d3cdc67b91 100644
--- a/selfdrive/camerad/cameras/camera_common.h
+++ b/selfdrive/camerad/cameras/camera_common.h
@@ -75,6 +75,8 @@ typedef struct FrameMetadata {
   unsigned int lens_pos;
   float lens_err;
   float lens_true_pos;
+
+  float processing_time;
 } FrameMetadata;
 
 typedef struct CameraExpInfo {
diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc
index da01b94177..411ff0aec4 100644
--- a/selfdrive/camerad/cameras/camera_qcom.cc
+++ b/selfdrive/camerad/cameras/camera_qcom.cc
@@ -211,12 +211,12 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i
               /*fps*/ 20,
 #endif
               device_id, ctx,
-              VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD);
+              VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD);
 
   camera_init(v, &s->driver_cam, CAMERA_ID_OV8865, 1,
               /*pixel_clock=*/72000000, /*line_length_pclk=*/1602,
               /*max_gain=*/510, 10, device_id, ctx,
-              VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
+              VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER);
 
   s->sm = new SubMaster({"driverState"});
   s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc
index f75e982be2..2e810e4202 100644
--- a/selfdrive/camerad/cameras/camera_qcom2.cc
+++ b/selfdrive/camerad/cameras/camera_qcom2.cc
@@ -26,7 +26,7 @@ extern ExitHandler do_exit;
 
 const size_t FRAME_WIDTH = 1928;
 const size_t FRAME_HEIGHT = 1208;
-const size_t FRAME_STRIDE = 2416;  // for 10 bit output
+const size_t FRAME_STRIDE = 2896;  // for 12 bit output. 1928 * 12 / 8 + 4 (alignment)
 
 const int MIPI_SETTLE_CNT = 33;  // Calculated by camera_freqs.py
 
@@ -56,7 +56,7 @@ const int EXPOSURE_TIME_MIN = 2; // with HDR, fastest ss
 const int EXPOSURE_TIME_MAX = 1904; // with HDR, slowest ss
 
 // ************** low level camera helpers ****************
-int cam_control(int fd, int op_code, void *handle, int size) {
+int do_cam_control(int fd, int op_code, void *handle, int size) {
   struct cam_control camcontrol = {0};
   camcontrol.op_code = op_code;
   camcontrol.handle = (uint64_t)handle;
@@ -83,7 +83,7 @@ std::optional<int32_t> device_acquire(int fd, int32_t session_handle, void *data
       .num_resources = (uint32_t)(data ? 1 : 0),
       .resource_hdl = (uint64_t)data,
   };
-  int err = cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd));
+  int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd));
   return err == 0 ? std::make_optional(cmd.dev_handle) : std::nullopt;
 };
 
@@ -93,13 +93,13 @@ int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t p
       .dev_handle = dev_handle,
       .packet_handle = packet_handle,
   };
-  return cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd));
+  return do_cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd));
 }
 
 int device_control(int fd, int op_code, int session_handle, int dev_handle) {
   // start stop and release are all the same
   struct cam_start_stop_dev_cmd cmd { .session_handle = session_handle, .dev_handle = dev_handle };
-  return cam_control(fd, op_code, &cmd, sizeof(cmd));
+  return do_cam_control(fd, op_code, &cmd, sizeof(cmd));
 }
 
 void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE,
@@ -118,7 +118,7 @@ void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, i
     mem_mgr_alloc_cmd.num_hdl++;
   }
 
-  cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd));
+  do_cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd));
   *handle = mem_mgr_alloc_cmd.out.buf_handle;
 
   void *ptr = NULL;
@@ -137,7 +137,7 @@ void release(int video0_fd, uint32_t handle) {
   struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0};
   mem_mgr_release_cmd.buf_handle = handle;
 
-  ret = cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd));
+  ret = do_cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd));
   assert(ret == 0);
 }
 
@@ -153,34 +153,39 @@ void clear_req_queue(int fd, int32_t session_hdl, int32_t link_hdl) {
   req_mgr_flush_request.link_hdl = link_hdl;
   req_mgr_flush_request.flush_type = CAM_REQ_MGR_FLUSH_TYPE_ALL;
   int ret;
-  ret = cam_control(fd, CAM_REQ_MGR_FLUSH_REQ, &req_mgr_flush_request, sizeof(req_mgr_flush_request));
+  ret = do_cam_control(fd, CAM_REQ_MGR_FLUSH_REQ, &req_mgr_flush_request, sizeof(req_mgr_flush_request));
   // LOGD("flushed all req: %d", ret);
 }
 
 // ************** high level camera helpers ****************
 
-void sensors_poke(struct CameraState *s, int request_id) {
+void CameraState::sensors_start() {
+  int start_reg_len = sizeof(start_reg_array) / sizeof(struct i2c_random_wr_payload);
+  sensors_i2c(start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
+}
+
+void CameraState::sensors_poke(int request_id) {
   uint32_t cam_packet_handle = 0;
   int size = sizeof(struct cam_packet);
-  struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
+  struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
   pkt->num_cmd_buf = 0;
   pkt->kmd_cmd_buf_index = -1;
   pkt->header.size = size;
   pkt->header.op_code = 0x7f;
   pkt->header.request_id = request_id;
 
-  int ret = device_config(s->sensor_fd, s->session_handle, s->sensor_dev_handle, cam_packet_handle);
+  int ret = device_config(sensor_fd, session_handle, sensor_dev_handle, cam_packet_handle);
   assert(ret == 0);
 
   munmap(pkt, size);
-  release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
+  release_fd(multi_cam_state->video0_fd, cam_packet_handle);
 }
 
-void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int len, int op_code) {
+void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code) {
   // LOGD("sensors_i2c: %d", len);
   uint32_t cam_packet_handle = 0;
   int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1;
-  struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
+  struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
   pkt->num_cmd_buf = 1;
   pkt->kmd_cmd_buf_index = -1;
   pkt->header.size = size;
@@ -190,7 +195,7 @@ void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int l
   buf_desc[0].size = buf_desc[0].length = sizeof(struct i2c_rdwr_header) + len*sizeof(struct i2c_random_wr_payload);
   buf_desc[0].type = CAM_CMD_BUF_I2C;
 
-  struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
+  struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
   i2c_random_wr->header.count = len;
   i2c_random_wr->header.op_code = 1;
   i2c_random_wr->header.cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR;
@@ -198,14 +203,15 @@ void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int l
   i2c_random_wr->header.addr_type = CAMERA_SENSOR_I2C_TYPE_WORD;
   memcpy(i2c_random_wr->random_wr_payload, dat, len*sizeof(struct i2c_random_wr_payload));
 
-  int ret = device_config(s->sensor_fd, s->session_handle, s->sensor_dev_handle, cam_packet_handle);
+  int ret = device_config(sensor_fd, session_handle, sensor_dev_handle, cam_packet_handle);
   assert(ret == 0);
 
   munmap(i2c_random_wr, buf_desc[0].size);
-  release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
+  release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle);
   munmap(pkt, size);
-  release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
+  release_fd(multi_cam_state->video0_fd, cam_packet_handle);
 }
+
 static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) {
   cam_cmd_unconditional_wait *unconditional_wait = (cam_cmd_unconditional_wait *)((char *)power + (sizeof(struct cam_cmd_power) + (power->count - 1) * sizeof(struct cam_power_settings)));
   unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT;
@@ -214,7 +220,8 @@ static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) {
   return (struct cam_cmd_power *)(unconditional_wait + 1);
 };
 
-void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
+void CameraState::sensors_init() {
+  int video0_fd = multi_cam_state->video0_fd;
   uint32_t cam_packet_handle = 0;
   int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2;
   struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(video0_fd, size, &cam_packet_handle);
@@ -333,7 +340,7 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
   power->power_settings[2].power_seq_type = 3;
 
   LOGD("probing the sensor");
-  int ret = cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0);
+  int ret = do_cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0);
   assert(ret == 0);
 
   munmap(i2c_info, buf_desc[0].size);
@@ -344,13 +351,13 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
   release_fd(video0_fd, cam_packet_handle);
 }
 
-void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset) {
+void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset) {
   uint32_t cam_packet_handle = 0;
   int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2;
   if (io_mem_handle != 0) {
     size += sizeof(struct cam_buf_io_cfg);
   }
-  struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
+  struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
   pkt->num_cmd_buf = 2;
   pkt->kmd_cmd_buf_index = 0;
   // YUV has kmd_cmd_buf_offset = 1780
@@ -445,7 +452,7 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
   buf_desc[1].length = buf_desc[1].size - buf_desc[1].offset;
   buf_desc[1].type = CAM_CMD_BUF_GENERIC;
   buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON;
-  uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20);
+  uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20);
   memcpy(buf2, &tmp, sizeof(tmp));
 
   if (io_mem_handle != 0) {
@@ -464,10 +471,10 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
 		 .h_init = 0x0,
 		 .v_init = 0x0,
 		};
-    io_cfg[0].format = CAM_FORMAT_MIPI_RAW_10;             // CAM_FORMAT_UBWC_TP10 for YUV
+    io_cfg[0].format = CAM_FORMAT_MIPI_RAW_12;             // CAM_FORMAT_UBWC_TP10 for YUV
     io_cfg[0].color_space = CAM_COLOR_SPACE_BASE;          // CAM_COLOR_SPACE_BT601_FULL for YUV
     io_cfg[0].color_pattern = 0x5;                         // 0x0 for YUV
-    io_cfg[0].bpp = 0xa;
+    io_cfg[0].bpp = 0xc;
     io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0;   // CAM_ISP_IFE_OUT_RES_FULL for YUV
     io_cfg[0].fence = fence;
     io_cfg[0].direction = CAM_BUF_OUTPUT;
@@ -475,142 +482,141 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
     io_cfg[0].framedrop_pattern = 0x1;
   }
 
-  int ret = device_config(s->multi_cam_state->isp_fd, s->session_handle, s->isp_dev_handle, cam_packet_handle);
+  int ret = device_config(multi_cam_state->isp_fd, session_handle, isp_dev_handle, cam_packet_handle);
   assert(ret == 0);
   if (ret != 0) {
     printf("ISP CONFIG FAILED\n");
   }
 
   munmap(buf2, buf_desc[1].size);
-  release_fd(s->multi_cam_state->video0_fd, buf_desc[1].mem_handle);
-  // release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
+  release_fd(multi_cam_state->video0_fd, buf_desc[1].mem_handle);
+  // release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle);
   munmap(pkt, size);
-  release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
+  release_fd(multi_cam_state->video0_fd, cam_packet_handle);
 }
 
-void enqueue_buffer(struct CameraState *s, int i, bool dp) {
+void CameraState::enqueue_buffer(int i, bool dp) {
   int ret;
-  int request_id = s->request_ids[i];
+  int request_id = request_ids[i];
 
-  if (s->buf_handle[i]) {
-    release(s->multi_cam_state->video0_fd, s->buf_handle[i]);
+  if (buf_handle[i]) {
+    release(multi_cam_state->video0_fd, buf_handle[i]);
     // wait
     struct cam_sync_wait sync_wait = {0};
-    sync_wait.sync_obj = s->sync_objs[i];
+    sync_wait.sync_obj = sync_objs[i];
     sync_wait.timeout_ms = 50; // max dt tolerance, typical should be 23
-    ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
+    ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
     // LOGD("fence wait: %d %d", ret, sync_wait.sync_obj);
 
-    s->buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof
-    if (dp) s->buf.queue(i);
+    buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof
+    if (dp) buf.queue(i);
 
     // destroy old output fence
     struct cam_sync_info sync_destroy = {0};
     strcpy(sync_destroy.name, "NodeOutputPortFence");
-    sync_destroy.sync_obj = s->sync_objs[i];
-    ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy));
+    sync_destroy.sync_obj = sync_objs[i];
+    ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy));
     // LOGD("fence destroy: %d %d", ret, sync_destroy.sync_obj);
   }
 
   // do stuff
   struct cam_req_mgr_sched_request req_mgr_sched_request = {0};
-  req_mgr_sched_request.session_hdl = s->session_handle;
-  req_mgr_sched_request.link_hdl = s->link_handle;
+  req_mgr_sched_request.session_hdl = session_handle;
+  req_mgr_sched_request.link_hdl = link_handle;
   req_mgr_sched_request.req_id = request_id;
-  ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request));
+  ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request));
   // LOGD("sched req: %d %d", ret, request_id);
 
   // create output fence
   struct cam_sync_info sync_create = {0};
   strcpy(sync_create.name, "NodeOutputPortFence");
-  ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
+  ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
   // LOGD("fence req: %d %d", ret, sync_create.sync_obj);
-  s->sync_objs[i] = sync_create.sync_obj;
+  sync_objs[i] = sync_create.sync_obj;
 
   // configure ISP to put the image in place
   struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0};
-  mem_mgr_map_cmd.mmu_hdls[0] = s->multi_cam_state->device_iommu;
+  mem_mgr_map_cmd.mmu_hdls[0] = multi_cam_state->device_iommu;
   mem_mgr_map_cmd.num_hdl = 1;
   mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE;
-  mem_mgr_map_cmd.fd = s->buf.camera_bufs[i].fd;
-  ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd));
-  // LOGD("map buf req: (fd: %d) 0x%x %d", s->bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret);
-  s->buf_handle[i] = mem_mgr_map_cmd.out.buf_handle;
+  mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd;
+  ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd));
+  // LOGD("map buf req: (fd: %d) 0x%x %d", bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret);
+  buf_handle[i] = mem_mgr_map_cmd.out.buf_handle;
 
   // poke sensor
-  sensors_poke(s, request_id);
+  sensors_poke(request_id);
   // LOGD("Poked sensor");
 
   // push the buffer
-  config_isp(s, s->buf_handle[i], s->sync_objs[i], request_id, s->buf0_handle, 65632*(i+1));
+  config_isp(buf_handle[i], sync_objs[i], request_id, buf0_handle, 65632*(i+1));
 }
 
-void enqueue_req_multi(struct CameraState *s, int start, int n, bool dp) {
+void CameraState::enqueue_req_multi(int start, int n, bool dp) {
    for (int i=start;i<start+n;++i) {
-     s->request_ids[(i - 1) % FRAME_BUF_COUNT] = i;
-     enqueue_buffer(s, (i - 1) % FRAME_BUF_COUNT, dp);
+     request_ids[(i - 1) % FRAME_BUF_COUNT] = i;
+     enqueue_buffer((i - 1) % FRAME_BUF_COUNT, dp);
    }
 }
 
 // ******************* camera *******************
 
-static void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v, CameraState *s, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) {
+void CameraState::camera_init(MultiCameraState *multi_cam_state_, VisionIpcServer * v, int camera_id, int camera_num_, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) {
   LOGD("camera init %d", camera_num);
-  s->multi_cam_state = multi_cam_state;
+  multi_cam_state = multi_cam_state_;
   assert(camera_id < std::size(cameras_supported));
-  s->ci = cameras_supported[camera_id];
-  assert(s->ci.frame_width != 0);
+  ci = cameras_supported[camera_id];
+  assert(ci.frame_width != 0);
 
-  s->camera_num = camera_num;
+  camera_num = camera_num_;
 
-  s->request_id_last = 0;
-  s->skipped = true;
+  request_id_last = 0;
+  skipped = true;
 
-  s->min_ev = EXPOSURE_TIME_MIN * sensor_analog_gains[ANALOG_GAIN_MIN_IDX];
-  s->max_ev = EXPOSURE_TIME_MAX * sensor_analog_gains[ANALOG_GAIN_MAX_IDX] * DC_GAIN;
-  s->target_grey_fraction = 0.3;
+  min_ev = EXPOSURE_TIME_MIN * sensor_analog_gains[ANALOG_GAIN_MIN_IDX];
+  max_ev = EXPOSURE_TIME_MAX * sensor_analog_gains[ANALOG_GAIN_MAX_IDX] * DC_GAIN;
+  target_grey_fraction = 0.3;
 
-  s->dc_gain_enabled = false;
-  s->gain_idx = ANALOG_GAIN_REC_IDX;
-  s->exposure_time = 5;
-  s->cur_ev[0] = s->cur_ev[1] = s->cur_ev[2] = (s->dc_gain_enabled ? DC_GAIN : 1) * sensor_analog_gains[s->gain_idx] * s->exposure_time;
+  dc_gain_enabled = false;
+  gain_idx = ANALOG_GAIN_REC_IDX;
+  exposure_time = 5;
+  cur_ev[0] = cur_ev[1] = cur_ev[2] = (dc_gain_enabled ? DC_GAIN : 1) * sensor_analog_gains[gain_idx] * exposure_time;
 
-  s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type);
+  buf.init(device_id, ctx, this, v, FRAME_BUF_COUNT, rgb_type, yuv_type);
 }
 
-static void camera_open(CameraState *s) {
-  s->sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", s->camera_num);
-  assert(s->sensor_fd >= 0);
-  LOGD("opened sensor for %d", s->camera_num);
+void CameraState::camera_open() {
+  sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", camera_num);
+  assert(sensor_fd >= 0);
+  LOGD("opened sensor for %d", camera_num);
 
   // probe the sensor
-  LOGD("-- Probing sensor %d", s->camera_num);
-  sensors_init(s->multi_cam_state->video0_fd, s->sensor_fd, s->camera_num);
+  LOGD("-- Probing sensor %d", camera_num);
+  sensors_init();
 
   // create session
   struct cam_req_mgr_session_info session_info = {};
-  int ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info));
+  int ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info));
   LOGD("get session: %d 0x%X", ret, session_info.session_hdl);
-  s->session_handle = session_info.session_hdl;
+  session_handle = session_info.session_hdl;
 
   // access the sensor
   LOGD("-- Accessing sensor");
-  auto sensor_dev_handle = device_acquire(s->sensor_fd, s->session_handle, nullptr);
-  assert(sensor_dev_handle);
-  s->sensor_dev_handle = *sensor_dev_handle;
+  auto sensor_dev_handle_ = device_acquire(sensor_fd, session_handle, nullptr);
+  assert(sensor_dev_handle_);
+  sensor_dev_handle = *sensor_dev_handle_;
   LOGD("acquire sensor dev");
 
   struct cam_isp_in_port_info in_port_info = {
-      .res_type = (uint32_t[]){CAM_ISP_IFE_IN_RES_PHY_0, CAM_ISP_IFE_IN_RES_PHY_1, CAM_ISP_IFE_IN_RES_PHY_2}[s->camera_num],
+      .res_type = (uint32_t[]){CAM_ISP_IFE_IN_RES_PHY_0, CAM_ISP_IFE_IN_RES_PHY_1, CAM_ISP_IFE_IN_RES_PHY_2}[camera_num],
 
       .lane_type = CAM_ISP_LANE_TYPE_DPHY,
       .lane_num = 4,
       .lane_cfg = 0x3210,
 
       .vc = 0x0,
-      // .dt = 0x2C; //CSI_RAW12
-      .dt = 0x2B,  //CSI_RAW10
-      .format = CAM_FORMAT_MIPI_RAW_10,
+      .dt = 0x2C,  // CSI_RAW12
+      .format = CAM_FORMAT_MIPI_RAW_12,
 
       .test_pattern = 0x2,  // 0x3?
       .usage_type = 0x0,
@@ -636,7 +642,7 @@ static void camera_open(CameraState *s) {
       .num_out_res = 0x1,
       .data[0] = (struct cam_isp_out_port_info){
           .res_type = CAM_ISP_IFE_OUT_RES_RDI_0,
-          .format = CAM_FORMAT_MIPI_RAW_10,
+          .format = CAM_FORMAT_MIPI_RAW_12,
           .width = FRAME_WIDTH,
           .height = FRAME_HEIGHT,
           .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0,
@@ -649,29 +655,29 @@ static void camera_open(CameraState *s) {
       .length = sizeof(in_port_info),
   };
 
-  auto isp_dev_handle = device_acquire(s->multi_cam_state->isp_fd, s->session_handle, &isp_resource);
-  assert(isp_dev_handle);
-  s->isp_dev_handle = *isp_dev_handle;
+  auto isp_dev_handle_ = device_acquire(multi_cam_state->isp_fd, session_handle, &isp_resource);
+  assert(isp_dev_handle_);
+  isp_dev_handle = *isp_dev_handle_;
   LOGD("acquire isp dev");
 
-  s->csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", s->camera_num);
-  assert(s->csiphy_fd >= 0);
-  LOGD("opened csiphy for %d", s->camera_num);
+  csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", camera_num);
+  assert(csiphy_fd >= 0);
+  LOGD("opened csiphy for %d", camera_num);
 
   struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = {.combo_mode = 0};
-  auto csiphy_dev_handle = device_acquire(s->csiphy_fd, s->session_handle, &csiphy_acquire_dev_info);
-  assert(csiphy_dev_handle);
-  s->csiphy_dev_handle = *csiphy_dev_handle;
+  auto csiphy_dev_handle_ = device_acquire(csiphy_fd, session_handle, &csiphy_acquire_dev_info);
+  assert(csiphy_dev_handle_);
+  csiphy_dev_handle = *csiphy_dev_handle_;
   LOGD("acquire csiphy dev");
 
   // config ISP
-  alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, 984480, (uint32_t*)&s->buf0_handle, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, s->multi_cam_state->device_iommu, s->multi_cam_state->cdm_iommu);
-  config_isp(s, 0, 0, 1, s->buf0_handle, 0);
+  alloc_w_mmu_hdl(multi_cam_state->video0_fd, 984480, (uint32_t*)&buf0_handle, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, multi_cam_state->device_iommu, multi_cam_state->cdm_iommu);
+  config_isp(0, 0, 1, buf0_handle, 0);
 
   LOG("-- Configuring sensor");
-  sensors_i2c(s, init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
-  //sensors_i2c(s, start_reg_array, std::size(start_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON);
-  //sensors_i2c(s, stop_reg_array, std::size(stop_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
+  sensors_i2c(init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
+  //sensors_i2c(start_reg_array, std::size(start_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON);
+  //sensors_i2c(stop_reg_array, std::size(stop_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
 
 
   // config csiphy
@@ -679,7 +685,7 @@ static void camera_open(CameraState *s) {
   {
     uint32_t cam_packet_handle = 0;
     int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1;
-    struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
+    struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
     pkt->num_cmd_buf = 1;
     pkt->kmd_cmd_buf_index = -1;
     pkt->header.size = size;
@@ -688,7 +694,7 @@ static void camera_open(CameraState *s) {
     buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_csiphy_info);
     buf_desc[0].type = CAM_CMD_BUF_GENERIC;
 
-    struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
+    struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
     csiphy_info->lane_mask = 0x1f;
     csiphy_info->lane_assign = 0x3210;// skip clk. How is this 16 bit for 5 channels??
     csiphy_info->csiphy_3phase = 0x0; // no 3 phase, only 2 conductors per lane
@@ -698,54 +704,51 @@ static void camera_open(CameraState *s) {
     csiphy_info->settle_time = MIPI_SETTLE_CNT * 200000000ULL;
     csiphy_info->data_rate = 48000000;  // Calculated by camera_freqs.py
 
-    int ret_ = device_config(s->csiphy_fd, s->session_handle, s->csiphy_dev_handle, cam_packet_handle);
+    int ret_ = device_config(csiphy_fd, session_handle, csiphy_dev_handle, cam_packet_handle);
     assert(ret_ == 0);
 
     munmap(csiphy_info, buf_desc[0].size);
-    release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
+    release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle);
     munmap(pkt, size);
-    release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
+    release_fd(multi_cam_state->video0_fd, cam_packet_handle);
   }
 
   // link devices
   LOG("-- Link devices");
   struct cam_req_mgr_link_info req_mgr_link_info = {0};
-  req_mgr_link_info.session_hdl = s->session_handle;
+  req_mgr_link_info.session_hdl = session_handle;
   req_mgr_link_info.num_devices = 2;
-  req_mgr_link_info.dev_hdls[0] = s->isp_dev_handle;
-  req_mgr_link_info.dev_hdls[1] = s->sensor_dev_handle;
-  ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info));
-  s->link_handle = req_mgr_link_info.link_hdl;
-  LOGD("link: %d hdl: 0x%X", ret, s->link_handle);
+  req_mgr_link_info.dev_hdls[0] = isp_dev_handle;
+  req_mgr_link_info.dev_hdls[1] = sensor_dev_handle;
+  ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info));
+  link_handle = req_mgr_link_info.link_hdl;
+  LOGD("link: %d hdl: 0x%X", ret, link_handle);
 
   struct cam_req_mgr_link_control req_mgr_link_control = {0};
   req_mgr_link_control.ops = CAM_REQ_MGR_LINK_ACTIVATE;
-  req_mgr_link_control.session_hdl = s->session_handle;
+  req_mgr_link_control.session_hdl = session_handle;
   req_mgr_link_control.num_links = 1;
-  req_mgr_link_control.link_hdls[0] = s->link_handle;
-  ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
+  req_mgr_link_control.link_hdls[0] = link_handle;
+  ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
   LOGD("link control: %d", ret);
 
-  ret = device_control(s->csiphy_fd, CAM_START_DEV, s->session_handle, s->csiphy_dev_handle);
+  ret = device_control(csiphy_fd, CAM_START_DEV, session_handle, csiphy_dev_handle);
   LOGD("start csiphy: %d", ret);
-  ret = device_control(s->multi_cam_state->isp_fd, CAM_START_DEV, s->session_handle, s->isp_dev_handle);
+  ret = device_control(multi_cam_state->isp_fd, CAM_START_DEV, session_handle, isp_dev_handle);
   LOGD("start isp: %d", ret);
-  ret = device_control(s->sensor_fd, CAM_START_DEV, s->session_handle, s->sensor_dev_handle);
+  ret = device_control(sensor_fd, CAM_START_DEV, session_handle, sensor_dev_handle);
   LOGD("start sensor: %d", ret);
 
-  enqueue_req_multi(s, 1, FRAME_BUF_COUNT, 0);
+  enqueue_req_multi(1, FRAME_BUF_COUNT, 0);
 }
 
 void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
-  camera_init(s, v, &s->driver_cam, CAMERA_ID_AR0231, 2, 20, device_id, ctx,
-              VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
+  s->driver_cam.camera_init(s, v, CAMERA_ID_AR0231, 2, 20, device_id, ctx, VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER);
   printf("driver camera initted \n");
   if (!env_only_driver) {
-    camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx,
-                VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right
+    s->road_cam.camera_init(s, v, CAMERA_ID_AR0231, 1, 20, device_id, ctx, VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD); // swap left/right
     printf("road camera initted \n");
-    camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx,
-                VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD);
+    s->wide_road_cam.camera_init(s, v, CAMERA_ID_AR0231, 0, 20, device_id, ctx, VISION_STREAM_RGB_WIDE_ROAD, VISION_STREAM_WIDE_ROAD);
     printf("wide road camera initted \n");
   }
 
@@ -768,7 +771,7 @@ void cameras_open(MultiCameraState *s) {
   LOGD("opened video1");
 
   // looks like there's only one of these
-  s->isp_fd = HANDLE_EINTR(open("/dev/v4l-subdev1", O_RDWR | O_NONBLOCK));
+  s->isp_fd = open_v4l_by_name_and_index("cam-isp");
   assert(s->isp_fd >= 0);
   LOGD("opened isp");
 
@@ -779,7 +782,7 @@ void cameras_open(MultiCameraState *s) {
   query_cap_cmd.handle_type = 1;
   query_cap_cmd.caps_handle = (uint64_t)&isp_query_cap_cmd;
   query_cap_cmd.size = sizeof(isp_query_cap_cmd);
-  ret = cam_control(s->isp_fd, CAM_QUERY_CAP, &query_cap_cmd, sizeof(query_cap_cmd));
+  ret = do_cam_control(s->isp_fd, CAM_QUERY_CAP, &query_cap_cmd, sizeof(query_cap_cmd));
   assert(ret == 0);
   LOGD("using MMU handle: %x", isp_query_cap_cmd.device_iommu.non_secure);
   LOGD("using MMU handle: %x", isp_query_cap_cmd.cdm_iommu.non_secure);
@@ -794,74 +797,72 @@ void cameras_open(MultiCameraState *s) {
   ret = HANDLE_EINTR(ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub));
   printf("req mgr subscribe: %d\n", ret);
 
-  camera_open(&s->driver_cam);
+  s->driver_cam.camera_open();
   printf("driver camera opened \n");
   if (!env_only_driver) {
-    camera_open(&s->road_cam);
+    s->road_cam.camera_open();
     printf("road camera opened \n");
-    camera_open(&s->wide_road_cam);
+    s->wide_road_cam.camera_open();
     printf("wide road camera opened \n");
   }
 }
 
-static void camera_close(CameraState *s) {
+void CameraState::camera_close() {
   int ret;
 
   // stop devices
   LOG("-- Stop devices");
-  // ret = device_control(s->sensor_fd, CAM_STOP_DEV, s->session_handle, s->sensor_dev_handle);
+  // ret = device_control(sensor_fd, CAM_STOP_DEV, session_handle, sensor_dev_handle);
   // LOGD("stop sensor: %d", ret);
-  ret = device_control(s->multi_cam_state->isp_fd, CAM_STOP_DEV, s->session_handle, s->isp_dev_handle);
+  ret = device_control(multi_cam_state->isp_fd, CAM_STOP_DEV, session_handle, isp_dev_handle);
   LOGD("stop isp: %d", ret);
-  ret = device_control(s->csiphy_fd, CAM_STOP_DEV, s->session_handle, s->csiphy_dev_handle);
+  ret = device_control(csiphy_fd, CAM_STOP_DEV, session_handle, csiphy_dev_handle);
   LOGD("stop csiphy: %d", ret);
   // link control stop
   LOG("-- Stop link control");
   static struct cam_req_mgr_link_control req_mgr_link_control = {0};
   req_mgr_link_control.ops = CAM_REQ_MGR_LINK_DEACTIVATE;
-  req_mgr_link_control.session_hdl = s->session_handle;
+  req_mgr_link_control.session_hdl = session_handle;
   req_mgr_link_control.num_links = 1;
-  req_mgr_link_control.link_hdls[0] = s->link_handle;
-  ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
+  req_mgr_link_control.link_hdls[0] = link_handle;
+  ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
   LOGD("link control stop: %d", ret);
 
   // unlink
   LOG("-- Unlink");
   static struct cam_req_mgr_unlink_info req_mgr_unlink_info = {0};
-  req_mgr_unlink_info.session_hdl = s->session_handle;
-  req_mgr_unlink_info.link_hdl = s->link_handle;
-  ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info));
+  req_mgr_unlink_info.session_hdl = session_handle;
+  req_mgr_unlink_info.link_hdl = link_handle;
+  ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info));
   LOGD("unlink: %d", ret);
 
   // release devices
   LOGD("-- Release devices");
-  ret = device_control(s->sensor_fd, CAM_RELEASE_DEV, s->session_handle, s->sensor_dev_handle);
+  ret = device_control(sensor_fd, CAM_RELEASE_DEV, session_handle, sensor_dev_handle);
   LOGD("release sensor: %d", ret);
-  ret = device_control(s->multi_cam_state->isp_fd, CAM_RELEASE_DEV, s->session_handle, s->isp_dev_handle);
+  ret = device_control(multi_cam_state->isp_fd, CAM_RELEASE_DEV, session_handle, isp_dev_handle);
   LOGD("release isp: %d", ret);
-  ret = device_control(s->csiphy_fd, CAM_RELEASE_DEV, s->session_handle, s->csiphy_dev_handle);
+  ret = device_control(csiphy_fd, CAM_RELEASE_DEV, session_handle, csiphy_dev_handle);
   LOGD("release csiphy: %d", ret);
 
   // destroyed session
-  struct cam_req_mgr_session_info session_info = {.session_hdl = s->session_handle};
-  ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &session_info, sizeof(session_info));
+  struct cam_req_mgr_session_info session_info = {.session_hdl = session_handle};
+  ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &session_info, sizeof(session_info));
   LOGD("destroyed session: %d", ret);
 }
 
 void cameras_close(MultiCameraState *s) {
-  camera_close(&s->driver_cam);
+  s->driver_cam.camera_close();
   if (!env_only_driver) {
-    camera_close(&s->road_cam);
-    camera_close(&s->wide_road_cam);
+    s->road_cam.camera_close();
+    s->wide_road_cam.camera_close();
   }
 
   delete s->sm;
   delete s->pm;
 }
 
-// ******************* just a helper *******************
-
-void handle_camera_event(CameraState *s, void *evdat) {
+void CameraState::handle_camera_event(void *evdat) {
   struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)evdat;
 
   uint64_t timestamp = event_data->u.frame_msg.timestamp;
@@ -869,53 +870,53 @@ void handle_camera_event(CameraState *s, void *evdat) {
   int real_id = event_data->u.frame_msg.request_id;
 
   if (real_id != 0) { // next ready
-    if (real_id == 1) {s->idx_offset = main_id;}
+    if (real_id == 1) {idx_offset = main_id;}
     int buf_idx = (real_id - 1) % FRAME_BUF_COUNT;
 
     // check for skipped frames
-    if (main_id > s->frame_id_last + 1 && !s->skipped) {
+    if (main_id > frame_id_last + 1 && !skipped) {
       // realign
-      clear_req_queue(s->multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
-      enqueue_req_multi(s, real_id + 1, FRAME_BUF_COUNT - 1, 0);
-      s->skipped = true;
-    } else if (main_id == s->frame_id_last + 1) {
-      s->skipped = false;
+      clear_req_queue(multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
+      enqueue_req_multi(real_id + 1, FRAME_BUF_COUNT - 1, 0);
+      skipped = true;
+    } else if (main_id == frame_id_last + 1) {
+      skipped = false;
     }
 
     // check for dropped requests
-    if (real_id > s->request_id_last + 1) {
-      enqueue_req_multi(s, s->request_id_last + 1 + FRAME_BUF_COUNT, real_id - (s->request_id_last + 1), 0);
+    if (real_id > request_id_last + 1) {
+      enqueue_req_multi(request_id_last + 1 + FRAME_BUF_COUNT, real_id - (request_id_last + 1), 0);
     }
 
     // metas
-    s->frame_id_last = main_id;
-    s->request_id_last = real_id;
+    frame_id_last = main_id;
+    request_id_last = real_id;
 
-    auto &meta_data = s->buf.camera_bufs_metadata[buf_idx];
-    meta_data.frame_id = main_id - s->idx_offset;
+    auto &meta_data = buf.camera_bufs_metadata[buf_idx];
+    meta_data.frame_id = main_id - idx_offset;
     meta_data.timestamp_sof = timestamp;
-    s->exp_lock.lock();
-    meta_data.gain = s->dc_gain_enabled ? s->analog_gain_frac * DC_GAIN : s->analog_gain_frac;
-    meta_data.high_conversion_gain = s->dc_gain_enabled;
-    meta_data.integ_lines = s->exposure_time;
-    meta_data.measured_grey_fraction = s->measured_grey_fraction;
-    meta_data.target_grey_fraction = s->target_grey_fraction;
-    s->exp_lock.unlock();
+    exp_lock.lock();
+    meta_data.gain = dc_gain_enabled ? analog_gain_frac * DC_GAIN : analog_gain_frac;
+    meta_data.high_conversion_gain = dc_gain_enabled;
+    meta_data.integ_lines = exposure_time;
+    meta_data.measured_grey_fraction = measured_grey_fraction;
+    meta_data.target_grey_fraction = target_grey_fraction;
+    exp_lock.unlock();
 
     // dispatch
-    enqueue_req_multi(s, real_id + FRAME_BUF_COUNT, 1, 1);
+    enqueue_req_multi(real_id + FRAME_BUF_COUNT, 1, 1);
   } else { // not ready
     // reset after half second of no response
-    if (main_id > s->frame_id_last + 10) {
-      clear_req_queue(s->multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
-      enqueue_req_multi(s, s->request_id_last + 1, FRAME_BUF_COUNT, 0);
-      s->frame_id_last = main_id;
-      s->skipped = true;
+    if (main_id > frame_id_last + 10) {
+      clear_req_queue(multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
+      enqueue_req_multi(request_id_last + 1, FRAME_BUF_COUNT, 0);
+      frame_id_last = main_id;
+      skipped = true;
     }
   }
 }
 
-static void set_camera_exposure(CameraState *s, float grey_frac) {
+void CameraState::set_camera_exposure(float grey_frac) {
   const float dt = 0.05;
 
   const float ts_grey = 10.0;
@@ -929,15 +930,15 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
   // Therefore we use the target EV from 3 frames ago, the grey fraction that was just measured was the result of that control action.
   // TODO: Lower latency to 2 frames, by using the histogram outputed by the sensor we can do AE before the debayering is complete
 
-  const float cur_ev = s->cur_ev[s->buf.cur_frame_data.frame_id % 3];
+  const float cur_ev_ = cur_ev[buf.cur_frame_data.frame_id % 3];
 
   // Scale target grey between 0.1 and 0.4 depending on lighting conditions
-  float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + cur_ev) / log2(6000.0), 0.1, 0.4);
-  float target_grey = (1.0 - k_grey) * s->target_grey_fraction + k_grey * new_target_grey;
+  float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + cur_ev_) / log2(6000.0), 0.1, 0.4);
+  float target_grey = (1.0 - k_grey) * target_grey_fraction + k_grey * new_target_grey;
 
-  float desired_ev = std::clamp(cur_ev * target_grey / grey_frac, s->min_ev, s->max_ev);
+  float desired_ev = std::clamp(cur_ev_ * target_grey / grey_frac, min_ev, max_ev);
   float k = (1.0 - k_ev) / 3.0;
-  desired_ev = (k * s->cur_ev[0]) + (k * s->cur_ev[1]) + (k * s->cur_ev[2]) + (k_ev * desired_ev);
+  desired_ev = (k * cur_ev[0]) + (k * cur_ev[1]) + (k * cur_ev[2]) + (k_ev * desired_ev);
 
   float best_ev_score = 1e6;
   int new_g = 0;
@@ -945,7 +946,7 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
 
   // Hysteresis around high conversion gain
   // We usually want this on since it results in lower noise, but turn off in very bright day scenes
-  bool enable_dc_gain = s->dc_gain_enabled;
+  bool enable_dc_gain = dc_gain_enabled;
   if (!enable_dc_gain && target_grey < 0.2) {
     enable_dc_gain = true;
   } else if (enable_dc_gain && target_grey > 0.3) {
@@ -954,14 +955,14 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
 
   // Simple brute force optimizer to choose sensor parameters
   // to reach desired EV
-  for (int g = std::max((int)ANALOG_GAIN_MIN_IDX, s->gain_idx - 1); g <= std::min((int)ANALOG_GAIN_MAX_IDX, s->gain_idx + 1); g++) {
+  for (int g = std::max((int)ANALOG_GAIN_MIN_IDX, gain_idx - 1); g <= std::min((int)ANALOG_GAIN_MAX_IDX, gain_idx + 1); g++) {
     float gain = sensor_analog_gains[g] * (enable_dc_gain ? DC_GAIN : 1);
 
     // Compute optimal time for given gain
     int t = std::clamp(int(std::round(desired_ev / gain)), EXPOSURE_TIME_MIN, EXPOSURE_TIME_MAX);
 
     // Only go below recomended gain when absolutely necessary to not overexpose
-    if (g < ANALOG_GAIN_REC_IDX && t > 20 && g < s->gain_idx) {
+    if (g < ANALOG_GAIN_REC_IDX && t > 20 && g < gain_idx) {
       continue;
     }
 
@@ -972,10 +973,10 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
     float m = g > ANALOG_GAIN_REC_IDX ? 5.0 : 0.1;
     score += std::abs(g - (int)ANALOG_GAIN_REC_IDX) * m;
 
-    // LOGE("cam: %d - gain: %d, t: %d (%.2f), score %.2f, score + gain %.2f, %.3f, %.3f", s->camera_num, g, t, desired_ev / gain, score, score + std::abs(g - s->gain_idx) * (score + 1.0) / 10.0, desired_ev, s->min_ev);
+    // LOGE("cam: %d - gain: %d, t: %d (%.2f), score %.2f, score + gain %.2f, %.3f, %.3f", camera_num, g, t, desired_ev / gain, score, score + std::abs(g - gain_idx) * (score + 1.0) / 10.0, desired_ev, min_ev);
 
     // Small penalty on changing gain
-    score += std::abs(g - s->gain_idx) * (score + 1.0) / 10.0;
+    score += std::abs(g - gain_idx) * (score + 1.0) / 10.0;
 
     if (score < best_ev_score) {
       new_t = t;
@@ -984,42 +985,41 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
     }
   }
 
-  s->exp_lock.lock();
+  exp_lock.lock();
 
-  s->measured_grey_fraction = grey_frac;
-  s->target_grey_fraction = target_grey;
+  measured_grey_fraction = grey_frac;
+  target_grey_fraction = target_grey;
 
-  s->analog_gain_frac = sensor_analog_gains[new_g];
-  s->gain_idx = new_g;
-  s->exposure_time = new_t;
-  s->dc_gain_enabled = enable_dc_gain;
+  analog_gain_frac = sensor_analog_gains[new_g];
+  gain_idx = new_g;
+  exposure_time = new_t;
+  dc_gain_enabled = enable_dc_gain;
 
-  float gain = s->analog_gain_frac * (s->dc_gain_enabled ? DC_GAIN : 1.0);
-  s->cur_ev[s->buf.cur_frame_data.frame_id % 3] = s->exposure_time * gain;
+  float gain = analog_gain_frac * (dc_gain_enabled ? DC_GAIN : 1.0);
+  cur_ev[buf.cur_frame_data.frame_id % 3] = exposure_time * gain;
 
-  s->exp_lock.unlock();
+  exp_lock.unlock();
 
   // Processing a frame takes right about 50ms, so we need to wait a few ms
   // so we don't send i2c commands around the frame start.
-  int ms = (nanos_since_boot() - s->buf.cur_frame_data.timestamp_sof) / 1000000;
+  int ms = (nanos_since_boot() - buf.cur_frame_data.timestamp_sof) / 1000000;
   if (ms < 60) {
     util::sleep_for(60 - ms);
   }
-  // LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", s->camera_num, 1e-9 * nanos_since_boot(), 1e-9 * s->buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - s->buf.cur_frame_data.timestamp_sof));
+  // LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera_num, 1e-9 * nanos_since_boot(), 1e-9 * buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - buf.cur_frame_data.timestamp_sof));
 
   uint16_t analog_gain_reg = 0xFF00 | (new_g << 4) | new_g;
   struct i2c_random_wr_payload exp_reg_array[] = {
                                                   {0x3366, analog_gain_reg},
-                                                  {0x3362, (uint16_t)(s->dc_gain_enabled ? 0x1 : 0x0)},
-                                                  {0x3012, (uint16_t)s->exposure_time},
+                                                  {0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)},
+                                                  {0x3012, (uint16_t)exposure_time},
                                                 };
-  sensors_i2c(s, exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload),
+  sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload),
               CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
-
 }
 
 void camera_autoexposure(CameraState *s, float grey_frac) {
-  set_camera_exposure(s, grey_frac);
+  s->set_camera_exposure(grey_frac);
 }
 
 // called by processing_thread
@@ -1053,11 +1053,10 @@ void cameras_run(MultiCameraState *s) {
 
   // start devices
   LOG("-- Starting devices");
-  int start_reg_len = sizeof(start_reg_array) / sizeof(struct i2c_random_wr_payload);
-  sensors_i2c(&s->driver_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
+  s->driver_cam.sensors_start();
   if (!env_only_driver) {
-    sensors_i2c(&s->road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
-    sensors_i2c(&s->wide_road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
+    s->road_cam.sensors_start();
+    s->wide_road_cam.sensors_start();
   }
 
   // poll events
@@ -1088,11 +1087,11 @@ void cameras_run(MultiCameraState *s) {
         }
 
         if (event_data->session_hdl == s->road_cam.session_handle) {
-          handle_camera_event(&s->road_cam, event_data);
+          s->road_cam.handle_camera_event(event_data);
         } else if (event_data->session_hdl == s->wide_road_cam.session_handle) {
-          handle_camera_event(&s->wide_road_cam, event_data);
+          s->wide_road_cam.handle_camera_event(event_data);
         } else if (event_data->session_hdl == s->driver_cam.session_handle) {
-          handle_camera_event(&s->driver_cam, event_data);
+          s->driver_cam.handle_camera_event(event_data);
         } else {
           printf("Unknown vidioc event source\n");
           assert(false);
diff --git a/selfdrive/camerad/cameras/camera_qcom2.h b/selfdrive/camerad/cameras/camera_qcom2.h
index f8ab2da8ae..d021ba256e 100644
--- a/selfdrive/camerad/cameras/camera_qcom2.h
+++ b/selfdrive/camerad/cameras/camera_qcom2.h
@@ -9,7 +9,8 @@
 
 #define FRAME_BUF_COUNT 4
 
-typedef struct CameraState {
+class CameraState {
+public:
   MultiCameraState *multi_cam_state;
   CameraInfo ci;
 
@@ -31,6 +32,21 @@ typedef struct CameraState {
 
   int camera_num;
 
+  void config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset);
+  void enqueue_req_multi(int start, int n, bool dp);
+  void enqueue_buffer(int i, bool dp);
+  void handle_camera_event(void *evdat);
+  void set_camera_exposure(float grey_frac);
+
+  void sensors_start();
+  void sensors_poke(int request_id);
+  void sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code);
+  void sensors_init();
+
+  void camera_open();
+  void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type);
+  void camera_close();
+
   int32_t session_handle;
   int32_t sensor_dev_handle;
   int32_t isp_dev_handle;
@@ -48,7 +64,7 @@ typedef struct CameraState {
   bool skipped;
 
   CameraBuf buf;
-} CameraState;
+};
 
 typedef struct MultiCameraState {
   unique_fd video0_fd;
diff --git a/selfdrive/camerad/cameras/camera_replay.cc b/selfdrive/camerad/cameras/camera_replay.cc
index b5b2e6ad29..a9983fe23e 100644
--- a/selfdrive/camerad/cameras/camera_replay.cc
+++ b/selfdrive/camerad/cameras/camera_replay.cc
@@ -98,9 +98,9 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
 
 void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
   camera_init(v, &s->road_cam, CAMERA_ID_LGC920, 20, device_id, ctx,
-              VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD, get_url(road_camera_route, "fcamera", 0));
+              VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD, get_url(road_camera_route, "fcamera", 0));
   // camera_init(v, &s->driver_cam, CAMERA_ID_LGC615, 10, device_id, ctx,
-  //             VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER, get_url(driver_camera_route, "dcamera", 0));
+  //             VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER, get_url(driver_camera_route, "dcamera", 0));
   s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
 }
 
diff --git a/selfdrive/camerad/cameras/camera_webcam.cc b/selfdrive/camerad/cameras/camera_webcam.cc
index 956f2dc88f..6001f9fd3b 100644
--- a/selfdrive/camerad/cameras/camera_webcam.cc
+++ b/selfdrive/camerad/cameras/camera_webcam.cc
@@ -141,9 +141,9 @@ void driver_camera_thread(CameraState *s) {
 
 void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
   camera_init(v, &s->road_cam, CAMERA_ID_LGC920, 20, device_id, ctx,
-              VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD);
+              VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD);
   camera_init(v, &s->driver_cam, CAMERA_ID_LGC615, 10, device_id, ctx,
-              VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
+              VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER);
   s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
 }
 
diff --git a/selfdrive/camerad/cameras/real_debayer.cl b/selfdrive/camerad/cameras/real_debayer.cl
index fe6a99f373..4553036ee3 100644
--- a/selfdrive/camerad/cameras/real_debayer.cl
+++ b/selfdrive/camerad/cameras/real_debayer.cl
@@ -40,12 +40,12 @@ half3 color_correct(half3 rgb) {
 }
 
 half val_from_10(const uchar * source, int gx, int gy) {
-  // parse 10bit
-  int start = gy * FRAME_STRIDE + (5 * (gx / 4));
-  int offset = gx % 4;
-  uint major = (uint)source[start + offset] << 2;
-  uint minor = (source[start + 4] >> (2 * offset)) & 3;
-  half pv = (half)(major + minor);
+  // parse 12bit
+  int start = gy * FRAME_STRIDE + (3 * (gx / 2));
+  int offset = gx % 2;
+  uint major = (uint)source[start + offset] << 4;
+  uint minor = (source[start + 2] >> (4 * offset)) & 0xf;
+  half pv = (half)((major + minor)/4);
 
   // normalize
   pv = max(0.0h, pv - black_level);
diff --git a/selfdrive/camerad/cameras/sensor2_i2c.h b/selfdrive/camerad/cameras/sensor2_i2c.h
index c3d8861a97..0f91503be0 100644
--- a/selfdrive/camerad/cameras/sensor2_i2c.h
+++ b/selfdrive/camerad/cameras/sensor2_i2c.h
@@ -9,7 +9,7 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
   {0x302C, 0x0001}, // VT_SYS_CLK_DIV
   {0x302E, 0x0002}, // PRE_PLL_CLK_DIV
   {0x3030, 0x0032}, // PLL_MULTIPLIER
-  {0x3036, 0x000A}, // OP_WORD_CLK_DIV
+  {0x3036, 0x000C}, // OP_WORD_CLK_DIV
   {0x3038, 0x0001}, // OP_SYS_CLK_DIV
 
   // FORMAT
@@ -46,11 +46,11 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
 
   // Readout Settings
   {0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI
-  {0x31AC, 0x0C0A}, // DATA_FORMAT_BITS, 12 -> 10
-  {0x3342, 0x122B}, // MIPI_F1_PDT_EDT
-  {0x3346, 0x122B}, // MIPI_F2_PDT_EDT
-  {0x334A, 0x122B}, // MIPI_F3_PDT_EDT
-  {0x334E, 0x122B}, // MIPI_F4_PDT_EDT
+  {0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12
+  {0x3342, 0x122C}, // MIPI_F1_PDT_EDT
+  {0x3346, 0x122C}, // MIPI_F2_PDT_EDT
+  {0x334A, 0x122C}, // MIPI_F3_PDT_EDT
+  {0x334E, 0x122C}, // MIPI_F4_PDT_EDT
   {0x3344, 0x0011}, // MIPI_F1_VDT_VC
   {0x3348, 0x0111}, // MIPI_F2_VDT_VC
   {0x334C, 0x0211}, // MIPI_F3_VDT_VC
diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py
index 506064de3d..1ec7677d31 100755
--- a/selfdrive/camerad/snapshot/snapshot.py
+++ b/selfdrive/camerad/snapshot/snapshot.py
@@ -17,9 +17,9 @@ from selfdrive.manager.process_config import managed_processes
 LM_THRESH = 120  # defined in selfdrive/camerad/imgproc/utils.h
 
 VISION_STREAMS = {
-  "roadCameraState": VisionStreamType.VISION_STREAM_RGB_BACK,
-  "driverCameraState": VisionStreamType.VISION_STREAM_RGB_FRONT,
-  "wideRoadCameraState": VisionStreamType.VISION_STREAM_RGB_WIDE,
+  "roadCameraState": VisionStreamType.VISION_STREAM_RGB_ROAD,
+  "driverCameraState": VisionStreamType.VISION_STREAM_RGB_DRIVER,
+  "wideRoadCameraState": VisionStreamType.VISION_STREAM_RGB_WIDE_ROAD,
 }
 
 
diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py
index 30a0bf6a3a..5f83bbde8b 100644
--- a/selfdrive/car/chrysler/carstate.py
+++ b/selfdrive/car/chrysler/carstate.py
@@ -1,7 +1,7 @@
 from cereal import car
+from common.conversions import Conversions as CV
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
-from selfdrive.config import Conversions as CV
 from selfdrive.car.interfaces import CarStateBase
 from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD
 
@@ -58,7 +58,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"]
-    ret.steerError = steer_state == 4 or (steer_state == 0 and ret.vEgo > self.CP.minSteerSpeed)
+    ret.steerFaultPermanent = 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/ford/carstate.py b/selfdrive/car/ford/carstate.py
index eba623f5ce..ff82e585b4 100644
--- a/selfdrive/car/ford/carstate.py
+++ b/selfdrive/car/ford/carstate.py
@@ -1,7 +1,7 @@
 from cereal import car
-from opendbc.can.parser import CANParser
+from common.conversions import Conversions as CV
 from common.numpy_fast import mean
-from selfdrive.config import Conversions as CV
+from opendbc.can.parser import CANParser
 from selfdrive.car.interfaces import CarStateBase
 from selfdrive.car.ford.values import DBC
 
@@ -23,7 +23,7 @@ class CarState(CarStateBase):
     ret.standstill = not ret.vEgoRaw > 0.001
     ret.steeringAngleDeg = 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.steerFaultPermanent = 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
diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py
index 0faaa3f669..5d89950816 100755
--- a/selfdrive/car/ford/interface.py
+++ b/selfdrive/car/ford/interface.py
@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 from cereal import car
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.car.ford.values import MAX_ANGLE
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
@@ -25,6 +25,8 @@ class CarInterface(CarInterfaceBase):
     ret.steerRateCost = 1.0
     ret.centerToFront = ret.wheelbase * 0.44
     tire_stiffness_factor = 0.5328
+    # TODO: add minSteerSpeed
+    ret.minEnableSpeed = 12. * CV.MPH_TO_MS
 
     # TODO: get actual value, for now starting with reasonable value for
     # civic and scaling by mass and wheelbase
@@ -64,7 +66,7 @@ class CarInterface(CarInterfaceBase):
   def apply(self, c):
 
     ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
-                               c.hudControl.visualAlert, c.cruiseControl.cancel)
+                         c.hudControl.visualAlert, c.cruiseControl.cancel)
 
     self.frame += 1
     return ret
diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py
index 94eb8bb0cc..f8477951cd 100755
--- a/selfdrive/car/ford/radar_interface.py
+++ b/selfdrive/car/ford/radar_interface.py
@@ -1,8 +1,8 @@
 #!/usr/bin/env python3
 from cereal import car
+from common.conversions import Conversions as CV
 from opendbc.can.parser import CANParser
 from selfdrive.car.ford.values import DBC
-from selfdrive.config import Conversions as CV
 from selfdrive.car.interfaces import RadarInterfaceBase
 
 RADAR_MSGS = list(range(0x500, 0x540))
diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py
index 7ad1e7cf88..11c2d367cb 100644
--- a/selfdrive/car/gm/carcontroller.py
+++ b/selfdrive/car/gm/carcontroller.py
@@ -1,11 +1,11 @@
 from cereal import car
+from common.conversions import Conversions as CV
 from common.realtime import DT_CTRL
 from common.numpy_fast import interp
-from selfdrive.config import Conversions as CV
+from opendbc.can.packer import CANPacker
 from selfdrive.car import apply_std_steer_torque_limits
 from selfdrive.car.gm import gmcan
 from selfdrive.car.gm.values import DBC, CanBus, CarControllerParams
-from opendbc.can.packer import CANPacker
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
@@ -27,8 +27,7 @@ class CarController():
     self.packer_obj = CANPacker(DBC[CP.carFingerprint]['radar'])
     self.packer_ch = CANPacker(DBC[CP.carFingerprint]['chassis'])
 
-  def update(self, c, enabled, CS, frame, actuators,
-             hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
+  def update(self, c, CS, frame, actuators, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
 
     P = self.params
 
@@ -41,7 +40,7 @@ class CarController():
     if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last:
       self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter
     elif (frame % P.STEER_STEP) == 0:
-      lkas_enabled = c.active and not (CS.out.steerWarning or CS.out.steerError) and CS.out.vEgo > P.MIN_STEER_SPEED
+      lkas_enabled = c.latActive and CS.out.vEgo > P.MIN_STEER_SPEED
       if lkas_enabled:
         new_steer = int(round(actuators.steer * P.STEER_MAX))
         apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
@@ -58,7 +57,7 @@ class CarController():
 
     # Gas/regen and brakes - all at 25Hz
     if (frame % 4) == 0:
-      if not c.active:
+      if not c.longActive:
         # Stock ECU sends max regen when not enabled.
         self.apply_gas = P.MAX_ACC_REGEN
         self.apply_brake = 0
@@ -68,15 +67,15 @@ class CarController():
 
       idx = (frame // 4) % 4
 
-      at_full_stop = enabled and CS.out.standstill
-      near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
+      at_full_stop = c.longActive and CS.out.standstill
+      near_stop = c.longActive and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
       can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop))
-      can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, enabled, at_full_stop))
+      can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, c.longActive, at_full_stop))
 
     # Send dashboard UI commands (ACC status), 25hz
     if (frame % 4) == 0:
       send_fcw = hud_alert == VisualAlert.fcw
-      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_fcw))
+      can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, c.longActive, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw))
 
     # Radar needs to know current speed and yaw rate (50hz),
     # and that ADAS is alive (10hz)
diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py
index 4a6b75fa3f..b94fd0f2ab 100644
--- a/selfdrive/car/gm/carstate.py
+++ b/selfdrive/car/gm/carstate.py
@@ -45,8 +45,8 @@ class CarState(CarStateBase):
 
     # 0 inactive, 1 active, 2 temporarily limited, 3 failed
     self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"]
-    ret.steerWarning = self.lkas_status == 2
-    ret.steerError = self.lkas_status == 3
+    ret.steerFaultTemporary = self.lkas_status == 2
+    ret.steerFaultPermanent = self.lkas_status == 3
 
     # 1 - open, 0 - closed
     ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or
@@ -59,7 +59,7 @@ class CarState(CarStateBase):
     ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1
     ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2
 
-    self.park_brake = pt_cp.vl["EPBStatus"]["EPBClosed"]
+    ret.parkingBrake = pt_cp.vl["EPBStatus"]["EPBClosed"] == 1
     ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0
     ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
     self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]["CruiseState"]
diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py
index d6a2d3cfee..0473abfc74 100755
--- a/selfdrive/car/gm/interface.py
+++ b/selfdrive/car/gm/interface.py
@@ -1,10 +1,11 @@
 #!/usr/bin/env python3
 from cereal import car
 from math import fabs
-from selfdrive.config import Conversions as CV
+
+from common.conversions import Conversions as CV
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.gm.values import CAR, CruiseButtons, \
                                     AccState, CarControllerParams
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 
 ButtonType = car.CarState.ButtonEvent.Type
@@ -46,7 +47,7 @@ class CarInterface(CarInterfaceBase):
 
     # These cars have been put into dashcam only due to both a lack of users and test coverage.
     # These cars likely still work fine. Once a user confirms each car works and a test route is
-    # added to selfdrive/test/test_routes, we can remove it from this list.
+    # added to selfdrive/car/tests/routes.py, we can remove it from this list.
     ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL}
 
     # Presence of a camera on the object bus is ok.
@@ -191,8 +192,6 @@ class CarInterface(CarInterfaceBase):
 
     if ret.vEgo < self.CP.minEnableSpeed:
       events.add(EventName.belowEngageSpeed)
-    if self.CS.park_brake:
-      events.add(EventName.parkBrake)
     if ret.cruiseState.standstill:
       events.add(EventName.resumeRequired)
     if self.CS.pcm_acc_status == AccState.FAULTED:
@@ -222,12 +221,7 @@ class CarInterface(CarInterfaceBase):
     if hud_v_cruise > 70:
       hud_v_cruise = 0
 
-    # For Openpilot, "enabled" includes pre-enable.
-    # In GM, PCM faults out if ACC command overlaps user gas.
-    enabled = c.enabled and not self.CS.out.gasPressed
-
-    ret = self.CC.update(c, enabled, self.CS, self.frame,
-                         c.actuators,
+    ret = self.CC.update(c, self.CS, self.frame, c.actuators,
                          hud_v_cruise, hud_control.lanesVisible,
                          hud_control.leadVisible, hud_control.visualAlert)
 
diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py
index 66fac54748..6904e6f899 100755
--- a/selfdrive/car/gm/radar_interface.py
+++ b/selfdrive/car/gm/radar_interface.py
@@ -1,9 +1,9 @@
 #!/usr/bin/env python3
 import math
 from cereal import car
+from common.conversions import Conversions as CV
 from opendbc.can.parser import CANParser
 from selfdrive.car.gm.values import DBC, CAR, CanBus
-from selfdrive.config import Conversions as CV
 from selfdrive.car.interfaces import RadarInterfaceBase
 
 RADAR_HEADER_MSG = 1120
diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py
index 49581799a7..bf5456bc4b 100644
--- a/selfdrive/car/honda/carcontroller.py
+++ b/selfdrive/car/honda/carcontroller.py
@@ -112,12 +112,12 @@ class CarController():
 
     self.params = CarControllerParams(CP)
 
-  def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd,
+  def update(self, c, CS, frame, actuators, pcm_cancel_cmd,
              hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
 
     P = self.params
 
-    if active:
+    if c.longActive:
       accel = actuators.accel
       gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, CS.CP.carFingerprint)
     else:
@@ -136,7 +136,7 @@ class CarController():
     else:
       hud_lanes = 0
 
-    if enabled:
+    if c.enabled:
       if hud_show_car:
         hud_car = 2
       else:
@@ -152,8 +152,6 @@ class CarController():
     # steer torque is converted back to CAN reference (positive when steering right)
     apply_steer = int(interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V))
 
-    lkas_active = active and not CS.steer_not_allowed
-
     # Send CAN commands.
     can_sends = []
 
@@ -165,7 +163,7 @@ class CarController():
     # Send steering command.
     idx = frame % 4
     can_sends.append(hondacan.create_steering_control(self.packer, apply_steer,
-      lkas_active, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl))
+      c.latActive, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl))
 
     stopping = actuators.longControlState == LongCtrlState.stopping
 
@@ -217,7 +215,7 @@ class CarController():
         if CS.CP.carFingerprint in HONDA_BOSCH:
           self.accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX)
           self.gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V)
-          can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, active, accel, self.gas, idx, stopping, CS.CP.carFingerprint))
+          can_sends.extend(hondacan.create_acc_commands(self.packer, c.enabled, c.longActive, accel, self.gas, idx, stopping, CS.CP.carFingerprint))
         else:
           apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
           apply_brake = int(clip(apply_brake * P.NIDEC_BRAKE_MAX, 0, P.NIDEC_BRAKE_MAX - 1))
@@ -236,7 +234,7 @@ class CarController():
             # This prevents unexpected pedal range rescaling
             # Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected
             # when you do enable.
-            if active:
+            if c.longActive:
               self.gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.)
             else:
               self.gas = 0.0
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index 0a56a02b94..fc262ba42c 100644
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@@ -1,9 +1,10 @@
-from cereal import car
 from collections import defaultdict
+
+from cereal import car
+from common.conversions import Conversions as CV
 from common.numpy_fast import interp
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
-from selfdrive.config import Conversions as CV
 from selfdrive.car.interfaces import CarStateBase
 from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL
 
@@ -197,11 +198,10 @@ 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"]]
-    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")
+    ret.steerFaultPermanent = steer_status not in ("NORMAL", "NO_TORQUE_ALERT_1", "NO_TORQUE_ALERT_2", "LOW_SPEED_LOCKOUT", "TMP_FAULT")
     # LOW_SPEED_LOCKOUT is not worth a warning
-    ret.steerWarning = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2")
+    # NO_TORQUE_ALERT_2 can be caused by bump or steering nudge from driver
+    ret.steerFaultTemporary = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2")
 
     if self.CP.openpilotLongitudinalControl:
       self.brake_error = cp.vl["STANDSTILL"]["BRAKE_ERROR_1"] or cp.vl["STANDSTILL"]["BRAKE_ERROR_2"]
@@ -230,11 +230,10 @@ class CarState(CarStateBase):
       250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
     ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1
 
+    # TODO: set for all cars
     if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH,
                                   CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E):
-      self.park_brake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0
-    else:
-      self.park_brake = 0  # TODO
+      ret.parkingBrake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0
 
     gear = int(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"])
     ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None))
diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py
index db7104cd4f..513babf3d1 100644
--- a/selfdrive/car/honda/hondacan.py
+++ b/selfdrive/car/honda/hondacan.py
@@ -1,5 +1,5 @@
+from common.conversions import Conversions as CV
 from selfdrive.car.honda.values import HondaFlags, HONDA_BOSCH, CAR, CarControllerParams
-from selfdrive.config import Conversions as CV
 
 # CAN bus layout with relay
 # 0 = ACC-CAN - radar side
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
index 94e4305909..040587be6f 100755
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@@ -1,13 +1,13 @@
 #!/usr/bin/env python3
 from cereal import car
 from panda import Panda
+from common.conversions import Conversions as CV
 from common.numpy_fast import interp
 from common.params import Params
 from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, HondaFlags, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL
 from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 from selfdrive.car.disable_ecu import disable_ecu
-from selfdrive.config import Conversions as CV
 
 
 ButtonType = car.CarState.ButtonEvent.Type
@@ -380,8 +380,6 @@ class CarInterface(CarInterfaceBase):
     events = self.create_common_events(ret, pcm_enable=False)
     if self.CS.brake_error:
       events.add(EventName.brakeUnavailable)
-    if self.CS.park_brake:
-      events.add(EventName.parkBrake)
 
     if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed:
       events.add(EventName.belowEngageSpeed)
@@ -427,11 +425,9 @@ class CarInterface(CarInterfaceBase):
     else:
       hud_v_cruise = 255
 
-    ret = self.CC.update(c.enabled, c.active, self.CS, self.frame,
-                         c.actuators,
-                         c.cruiseControl.cancel,
-                         hud_v_cruise,
-                         hud_control.lanesVisible,
+    ret = self.CC.update(c, self.CS, self.frame,
+                         c.actuators, c.cruiseControl.cancel,
+                         hud_v_cruise, hud_control.lanesVisible,
                          hud_show_car=hud_control.leadVisible,
                          hud_alert=hud_control.visualAlert)
 
diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index f7c43bd6e3..6b2cbd422d 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -1,7 +1,7 @@
 from cereal import car
 from common.realtime import DT_CTRL
 from common.numpy_fast import clip, interp
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.car import apply_std_steer_torque_limits
 from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_lfahda_mfc, create_acc_commands, create_acc_opt, create_frt_radar_opt
 from selfdrive.car.hyundai.values import Buttons, CarControllerParams, CAR
@@ -46,23 +46,20 @@ class CarController():
     self.last_resume_frame = 0
     self.accel = 0
 
-  def update(self, c, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
+  def update(self, c, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
              left_lane, right_lane, left_lane_depart, right_lane_depart):
     # Steering Torque
     new_steer = int(round(actuators.steer * self.p.STEER_MAX))
     apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
     self.steer_rate_limited = new_steer != apply_steer
 
-    # disable when temp fault is active, or below LKA minimum speed
-    lkas_active = c.active and not CS.out.steerWarning and CS.out.vEgo >= CS.CP.minSteerSpeed
-
-    if not lkas_active:
+    if not c.latActive:
       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,
+      process_hud_alert(c.enabled, self.car_fingerprint, visual_alert,
                         left_lane, right_lane, left_lane_depart, right_lane_depart)
 
     can_sends = []
@@ -72,8 +69,8 @@ class CarController():
       if (frame % 100) == 0:
         can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0])
 
-    can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active,
-                                   CS.lkas11, sys_warning, sys_state, enabled,
+    can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, c.latActive,
+                                   CS.lkas11, sys_warning, sys_state, c.enabled,
                                    left_lane, right_lane,
                                    left_lane_warning, right_lane_warning))
 
@@ -89,7 +86,7 @@ class CarController():
 
     if frame % 2 == 0 and CS.CP.openpilotLongitudinalControl:
       lead_visible = False
-      accel = actuators.accel if c.active else 0
+      accel = actuators.accel if c.longActive else 0
 
       jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)
 
@@ -100,7 +97,7 @@ class CarController():
 
       stopping = (actuators.longControlState == LongCtrlState.stopping)
       set_speed_in_units = hud_speed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH)
-      can_sends.extend(create_acc_commands(self.packer, enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping))
+      can_sends.extend(create_acc_commands(self.packer, c.enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping))
       self.accel = accel
 
     # 20 Hz LFA MFA message
@@ -108,7 +105,7 @@ class CarController():
                                                    CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV,
                                                    CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
                                                    CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022):
-      can_sends.append(create_lfahda_mfc(self.packer, enabled))
+      can_sends.append(create_lfahda_mfc(self.packer, c.enabled))
 
     # 5 Hz ACC options
     if frame % 20 == 0 and CS.CP.openpilotLongitudinalControl:
diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
index bdd49e2067..b47fed75df 100644
--- a/selfdrive/car/hyundai/carstate.py
+++ b/selfdrive/car/hyundai/carstate.py
@@ -1,10 +1,10 @@
 import copy
 from cereal import car
-from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES, EV_CAR, HYBRID_CAR
-from selfdrive.car.interfaces import CarStateBase
+from common.conversions import Conversions as CV
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
-from selfdrive.config import Conversions as CV
+from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES, EV_CAR, HYBRID_CAR
+from selfdrive.car.interfaces import CarStateBase
 
 
 class CarState(CarStateBase):
@@ -47,7 +47,7 @@ class CarState(CarStateBase):
     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 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0
+    ret.steerFaultTemporary = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0
 
     # cruise state
     if self.CP.openpilotLongitudinalControl:
@@ -70,6 +70,7 @@ class CarState(CarStateBase):
     ret.brake = 0
     ret.brakePressed = cp.vl["TCS13"]["DriverBraking"] != 0
     ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
+    ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
 
     if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR):
       if self.CP.carFingerprint in HYBRID_CAR:
@@ -109,7 +110,6 @@ class CarState(CarStateBase):
     # save the entire LKAS11 and CLU11
     self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
     self.clu11 = copy.copy(cp.vl["CLU11"])
-    self.park_brake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
     self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"]  # 0 NOT ACTIVE, 1 ACTIVE
     self.brake_error = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
     self.prev_cruise_buttons = self.cruise_buttons
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 379e6937ae..5d75be0aa4 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -2,7 +2,7 @@
 from cereal import car
 from panda import Panda
 from common.params import Params
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.car.hyundai.values import CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons, CarControllerParams
 from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
@@ -32,7 +32,7 @@ class CarInterface(CarInterfaceBase):
 
     # These cars have been put into dashcam only due to both a lack of users and test coverage.
     # These cars likely still work fine. Once a user confirms each car works and a test route is
-    # added to selfdrive/test/test_routes, we can remove it from this list.
+    # added to selfdrive/car/tests/routes.py, we can remove it from this list.
     ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30}
 
     ret.steerActuatorDelay = 0.1  # Default delay
@@ -302,8 +302,6 @@ class CarInterface(CarInterfaceBase):
 
     if self.CS.brake_error:
       events.add(EventName.brakeUnavailable)
-    if self.CS.park_brake:
-      events.add(EventName.parkBrake)
 
     if self.CS.CP.openpilotLongitudinalControl:
       buttonEvents = []
@@ -352,8 +350,8 @@ class CarInterface(CarInterfaceBase):
 
   def apply(self, c):
     hud_control = c.hudControl
-    ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators,
-                         c.cruiseControl.cancel, hud_control.visualAlert, hud_control.setSpeed, hud_control.leftLaneVisible,
-                         hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)
+    ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert,
+                         hud_control.setSpeed, hud_control.leftLaneVisible, hud_control.rightLaneVisible,
+                         hud_control.leftLaneDepart, hud_control.rightLaneDepart)
     self.frame += 1
     return ret
diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py
index e689a57f48..78a0ead909 100644
--- a/selfdrive/car/hyundai/values.py
+++ b/selfdrive/car/hyundai/values.py
@@ -11,8 +11,8 @@ class CarControllerParams:
     # To determine the limit for your car, find the maximum value that the stock LKAS will request.
     # If the max stock LKAS request is <384, add your car to this list.
     if CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.HYUNDAI_GENESIS, CAR.ELANTRA_GT_I30, CAR.IONIQ,
-                             CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_HEV,
-                             CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA, CAR.KIA_SORENTO, CAR.KIA_STINGER):
+                             CAR.IONIQ_EV_LTD, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_HEV,
+                             CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA, CAR.KIA_SORENTO, CAR.KIA_STINGER):
       self.STEER_MAX = 255
     else:
       self.STEER_MAX = 384
@@ -184,21 +184,27 @@ FW_VERSIONS = {
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\000AEhe SCC FHCUP      1.00 1.02 99110-G2100         ',
       b'\xf1\x00AEhe SCC F-CUP      1.00 1.00 99110-G2200         ',
+      b'\xf1\x00AEhe SCC F-CUP      1.00 1.00 99110-G2600         ',
     ],
     (Ecu.eps, 0x7d4, None): [
       b'\xf1\000AE  MDPS C 1.00 1.01 56310/G2510 4APHC101',
       b'\xf1\x00AE  MDPS C 1.00 1.01 56310/G2560 4APHC101',
+      b'\xf1\x00AE  MDPS C 1.00 1.01 56310G2510\x00 4APHC101',
     ],
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\000AEP MFC  AT USA LHD 1.00 1.01 95740-G2600 190819',
       b'\xf1\x00AEP MFC  AT EUR RHD 1.00 1.01 95740-G2600 190819',
+      b'\xf1\x00AEP MFC  AT USA LHD 1.00 1.00 95740-G2700 201027',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x816H6F6051\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\xf1\x816H6G6051\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x816U3J9051\000\000\xf1\0006U3H1_C2\000\0006U3J9051\000\000PAE0G16NL0\x82zT\xd2',
       b'\xf1\x816U3J8051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J8051\x00\x00PAETG16UL0\x00\x00\x00\x00',
+      b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL2\xad\xeb\xabt',
+      b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL2\x00\x00\x00\x00',
     ],
   },
   CAR.IONIQ_EV_2020: {
@@ -460,6 +466,7 @@ FW_VERSIONS = {
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\x00TM__ SCC F-CUP      1.00 1.00 99110-S1500         ',
       b'\xf1\x8799110S1500\xf1\x00TM__ SCC F-CUP      1.00 1.00 99110-S1500         ',
+      b'\xf1\x8799110S1500\xf1\x00TM__ SCC FHCUP      1.00 1.00 99110-S1500         ',
     ],
     (Ecu.esp, 0x7d1, None): [
       b'\xf1\x00TM ESC \x02 101 \x08\x04 58910-S2GA0',
@@ -467,11 +474,14 @@ FW_VERSIONS = {
       b'\xf1\x8758910-S2DA0\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0',
       b'\xf1\x8758910-S2GA0\xf1\x00TM ESC \x02 101 \x08\x04 58910-S2GA0',
       b'\xf1\x8758910-S1DA0\xf1\x00TM ESC \x1e 102 \x08\x08 58910-S1DA0',
+      b'\xf1\x8758910-S2GA0\xf1\x00TM ESC \x04 102!\x04\x05 58910-S2GA0',
     ],
     (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x82TACVN5GMI3XXXH0A',
       b'\xf1\x82TMBZN5TMD3XXXG2E',
       b'\xf1\x82TACVN5GSI3XXXH0A',
       b'\xf1\x82TMCFD5MMCXXXXG0A',
+      b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x82TMDWN5TMD3TXXJ1A',
     ],
     (Ecu.eps, 0x7d4, None): [
       b'\xf1\x00TM  MDPS C 1.00 1.02 56370-S2AA0 0B19',
@@ -481,12 +491,15 @@ FW_VERSIONS = {
       b'\xf1\x00TMA MFC  AT MEX LHD 1.00 1.01 99211-S2500 210205',
       b'\xf1\x00TMA MFC  AT USA LHD 1.00 1.00 99211-S2500 200720',
       b'\xf1\x00TM  MFC  AT EUR LHD 1.00 1.03 99211-S1500 210224',
+      b'\xf1\x00TMA MFC  AT USA LHD 1.00 1.01 99211-S2500 210205',
     ],
     (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\x87SDMXCA9087684GN1VfvgUUeVwwgwwwwwffffU?\xfb\xff\x97\x88\x7f\xff+\xa4\xf1\x89HT6WAD00A1\xf1\x82STM4G25NH1\x00\x00\x00\x00\x00\x00',
       b'\xf1\x00T02601BL  T02730A1  VTMPT25XXX730NS2\xa6\x06\x88\xf7',
       b'\xf1\x87SDMXCA8653204GN1EVugEUuWwwwwww\x87wwwwwv/\xfb\xff\xa8\x88\x9f\xff\xa5\x9c\xf1\x89HT6WAD00A1\xf1\x82STM4G25NH1\x00\x00\x00\x00\x00\x00',
       b'\xf1\x87954A02N250\x00\x00\x00\x00\x00\xf1\x81T02730A1  \xf1\x00T02601BL  T02730A1  VTMPT25XXX730NS2\xa6\x06\x88\xf7',
       b'\xf1\x87KMMYBU034207SB72x\x89\x88\x98h\x88\x98\x89\x87fhvvfWf33_\xff\x87\xff\x8f\xfa\x81\xe5\xf1\x89HT6TAF00A1\xf1\x82STM0M25GS1\x00\x00\x00\x00\x00\x00',
+      b'\xf1\x87954A02N250\x00\x00\x00\x00\x00\xf1\x81T02730A1  \xf1\x00T02601BL  T02730A1  VTMPT25XXX730NS2\xa6',
     ],
   },
   CAR.SANTA_FE_HEV_2022: {
@@ -855,6 +868,7 @@ FW_VERSIONS = {
     ],
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\x00DEH MFC  AT USA LHD 1.00 1.07 99211-G5000 201221',
+      b'\xf1\x00DEH MFC  AT USA LHD 1.00 1.00 99211-G5500 210428',
     ],
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\x00DEhe SCC FHCUP      1.00 1.00 99110-G5600         ',
@@ -905,6 +919,7 @@ FW_VERSIONS = {
       b'\xf1\x00CN7_ SCC F-CUP      1.00 1.01 99110-AA000         ',
       b'\xf1\x00CN7_ SCC FHCUP      1.00 1.01 99110-AA000         ',
       b'\xf1\x8799110AA000\xf1\x00CN7_ SCC FHCUP      1.00 1.01 99110-AA000         ',
+      b'\xf1\x8799110AA000\xf1\x00CN7_ SCC F-CUP      1.00 1.01 99110-AA000         ',
     ],
     (Ecu.eps, 0x7d4, None): [
       b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x00CN7 MDPS C 1.00 1.06 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4CNDC106',
@@ -927,7 +942,8 @@ FW_VERSIONS = {
       b'\xf1\x87CXMQFM2135005JB2E\xb9\x89\x98W\xa9y\x97h\xa9\x98\x99wxvwh\x87\177\xffx\xff\xff\xff,,\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00',
       b'\xf1\x87CXMQFM1916035JB2\x88vvgg\x87Wuwgev\xa9\x98\x88\x98h\x99\x9f\xffh\xff\xff\xff\xa5\xee\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00',
       b'\xf1\x87CXLQF40189012JL2f\x88\x86\x88\x88vUex\xb8\x88\x88\x88\x87\x88\x89fh?\xffz\xff\xff\xff\x08z\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00',
-      b'\xf1\x87CXMQFM2728305JB2E\x97\x87xw\x87vwgw\x84x\x88\x88w\x89EI\xbf\xff{\xff\xff\xff\xe6\x0e\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00'
+      b'\xf1\x87CXMQFM2728305JB2E\x97\x87xw\x87vwgw\x84x\x88\x88w\x89EI\xbf\xff{\xff\xff\xff\xe6\x0e\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00',
+      b'\xf1\x87CXMQFM3806705JB2\x89\x87wwx\x88g\x86\x99\x87\x86xwwv\x88yv\x7f\xffz\xff\xff\xffV\x15\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x82CNCWD0AMFCXCSFFA',
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index b19eae8730..0116c1668d 100644
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@@ -7,7 +7,7 @@ from cereal import car
 from common.kalman.simple_kalman import KF1D
 from common.realtime import DT_CTRL
 from selfdrive.car import gen_empty_fingerprint
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
 from selfdrive.controls.lib.events import Events
 from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -90,7 +90,7 @@ class CarInterfaceBase(ABC):
     ret.stopAccel = -2.0
     ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop
     ret.vEgoStopping = 0.5
-    ret.vEgoStarting = 0.5  # needs to be >= vEgoStopping to avoid state transition oscillation
+    ret.vEgoStarting = 0.5
     ret.stoppingControl = True
     ret.longitudinalTuning.deadzoneBP = [0.]
     ret.longitudinalTuning.deadzoneV = [0.]
@@ -141,11 +141,13 @@ class CarInterfaceBase(ABC):
       events.add(EventName.wrongCruiseMode)
     if cs_out.brakeHoldActive and self.CP.openpilotLongitudinalControl:
       events.add(EventName.brakeHold)
+    if cs_out.parkingBrake:
+      events.add(EventName.parkBrake)
 
 
     # Handle permanent and temporary steering faults
     self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1
-    if cs_out.steerWarning:
+    if cs_out.steerFaultTemporary:
       # if the user overrode recently, show a less harsh alert
       if self.silent_steer_warning or cs_out.standstill or self.steering_unpressed < int(1.5 / DT_CTRL):
         self.silent_steer_warning = True
@@ -154,7 +156,7 @@ class CarInterfaceBase(ABC):
         events.add(EventName.steerTempUnavailable)
     else:
       self.silent_steer_warning = False
-    if cs_out.steerError:
+    if cs_out.steerFaultPermanent:
       events.add(EventName.steerUnavailable)
 
     # Disable on rising edge of gas or brake. Also disable on brake when speed > 0.
diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py
index 60bb620377..5f3d41ae34 100644
--- a/selfdrive/car/mazda/carcontroller.py
+++ b/selfdrive/car/mazda/carcontroller.py
@@ -19,7 +19,7 @@ class CarController():
     apply_steer = 0
     self.steer_rate_limited = False
 
-    if c.active:
+    if c.latActive:
       # calculate steer and also set limits due to driver torque
       new_steer = int(round(c.actuators.steer * CarControllerParams.STEER_MAX))
       apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last,
@@ -32,7 +32,7 @@ class CarController():
         # TODO: improve the resume trigger logic by looking at actual radar data
         can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
 
-    if c.cruiseControl.cancel or (CS.out.cruiseState.enabled and not c.enabled):
+    if c.cruiseControl.cancel:
       # If brake is pressed, let us wait >70ms before trying to disable crz to avoid
       # a race condition with the stock system, where the second cancel from openpilot
       # will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to
diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py
index feb1147549..fa15f2453c 100644
--- a/selfdrive/car/mazda/carstate.py
+++ b/selfdrive/car/mazda/carstate.py
@@ -1,5 +1,5 @@
 from cereal import car
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
 from selfdrive.car.interfaces import CarStateBase
@@ -88,7 +88,7 @@ class CarState(CarStateBase):
     # Check if LKAS is disabled due to lack of driver torque when all other states indicate
     # it should be enabled (steer lockout). Don't warn until we actually get lkas active
     # and lose it again, i.e, after initial lkas activation
-    ret.steerWarning = self.lkas_allowed_speed and lkas_blocked
+    ret.steerFaultTemporary = self.lkas_allowed_speed and lkas_blocked
 
     self.acc_active_last = ret.cruiseState.enabled
 
@@ -98,7 +98,7 @@ class CarState(CarStateBase):
     self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0
     self.cam_lkas = cp_cam.vl["CAM_LKAS"]
     self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
-    ret.steerError = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
+    ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
 
     return ret
 
diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py
index fb8edd6f42..c910d7e148 100755
--- a/selfdrive/car/mazda/interface.py
+++ b/selfdrive/car/mazda/interface.py
@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 from cereal import car
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.car.mazda.values import CAR, LKAS_LIMITS
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py
index b2e315a5f9..d7fcad0748 100755
--- a/selfdrive/car/mock/interface.py
+++ b/selfdrive/car/mock/interface.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 import math
 from cereal import car
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.swaglog import cloudlog
 import cereal.messaging as messaging
 from selfdrive.car import gen_empty_fingerprint, get_safety_config
diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py
index 8b30c11249..15cd44be92 100644
--- a/selfdrive/car/nissan/carcontroller.py
+++ b/selfdrive/car/nissan/carcontroller.py
@@ -18,20 +18,19 @@ class CarController():
 
     self.packer = CANPacker(dbc_name)
 
-  def update(self, c, enabled, CS, frame, actuators, cruise_cancel, hud_alert,
+  def update(self, c, CS, frame, actuators, cruise_cancel, hud_alert,
              left_line, right_line, left_lane_depart, right_lane_depart):
 
     can_sends = []
 
     ### STEER ###
-    acc_active = CS.out.cruiseState.enabled
     lkas_hud_msg = CS.lkas_hud_msg
     lkas_hud_info_msg = CS.lkas_hud_info_msg
     apply_angle = actuators.steeringAngleDeg
 
     steer_hud_alert = 1 if hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0
 
-    if c.active:
+    if c.latActive:
       # # windup slower
       if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
         angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V)
@@ -58,10 +57,6 @@ class CarController():
 
     self.last_angle = apply_angle
 
-    if not enabled and acc_active:
-      # send acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
-      cruise_cancel = 1
-
     if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and cruise_cancel:
         can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg, frame))
 
@@ -73,12 +68,12 @@ class CarController():
         can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, cruise_cancel))
 
     can_sends.append(nissancan.create_steering_control(
-        self.packer, apply_angle, frame, enabled, self.lkas_max_torque))
+        self.packer, apply_angle, frame, c.enabled, self.lkas_max_torque))
 
     if lkas_hud_msg and lkas_hud_info_msg:
       if frame % 2 == 0:
         can_sends.append(nissancan.create_lkas_hud_msg(
-          self.packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart))
+          self.packer, lkas_hud_msg, c.enabled, left_line, right_line, left_lane_depart, right_lane_depart))
 
       if frame % 50 == 0:
         can_sends.append(nissancan.create_lkas_hud_info_msg(
diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py
index 6b030e9b45..3c5d7dc24b 100644
--- a/selfdrive/car/nissan/carstate.py
+++ b/selfdrive/car/nissan/carstate.py
@@ -3,7 +3,7 @@ from collections import deque
 from cereal import car
 from opendbc.can.can_define import CANDefine
 from selfdrive.car.interfaces import CarStateBase
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from opendbc.can.parser import CANParser
 from selfdrive.car.nissan.values import CAR, DBC, CarControllerParams
 
diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py
index c32fb13780..63b4dad7fd 100644
--- a/selfdrive/car/nissan/interface.py
+++ b/selfdrive/car/nissan/interface.py
@@ -79,7 +79,7 @@ class CarInterface(CarInterfaceBase):
 
   def apply(self, c):
     hud_control = c.hudControl
-    ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators,
+    ret = self.CC.update(c, self.CS, self.frame, c.actuators,
                          c.cruiseControl.cancel, hud_control.visualAlert,
                          hud_control.leftLaneVisible, hud_control.rightLaneVisible,
                          hud_control.leftLaneDepart, hud_control.rightLaneDepart)
diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py
index afc91f4755..fd439356e9 100644
--- a/selfdrive/car/subaru/carcontroller.py
+++ b/selfdrive/car/subaru/carcontroller.py
@@ -15,7 +15,7 @@ class CarController():
     self.p = CarControllerParams(CP)
     self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
 
-  def update(self, c, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
+  def update(self, c, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
 
     can_sends = []
 
@@ -30,7 +30,7 @@ class CarController():
       apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
       self.steer_rate_limited = new_steer != apply_steer
 
-      if not c.active:
+      if not c.latActive:
         apply_steer = 0
 
       if CS.CP.carFingerprint in PREGLOBAL_CARS:
@@ -69,7 +69,7 @@ class CarController():
         self.es_distance_cnt = CS.es_distance_msg["Counter"]
 
       if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
-        can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart))
+        can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, c.enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart))
         self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
 
     new_actuators = actuators.copy()
diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py
index 1eefb18584..45ea66fb27 100644
--- a/selfdrive/car/subaru/carstate.py
+++ b/selfdrive/car/subaru/carstate.py
@@ -1,7 +1,7 @@
 import copy
 from cereal import car
 from opendbc.can.can_define import CANDefine
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
 from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD, CAR, PREGLOBAL_CARS
@@ -62,13 +62,13 @@ class CarState(CarStateBase):
                         cp.vl["BodyInfo"]["DOOR_OPEN_RL"],
                         cp.vl["BodyInfo"]["DOOR_OPEN_FR"],
                         cp.vl["BodyInfo"]["DOOR_OPEN_FL"]])
-    ret.steerError = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1
+    ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1
 
     if self.car_fingerprint in PREGLOBAL_CARS:
       self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"]
       self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"]
     else:
-      ret.steerWarning = cp.vl["Steering_Torque"]["Steer_Warning"] == 1
+      ret.steerFaultTemporary = cp.vl["Steering_Torque"]["Steer_Warning"] == 1
       ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1
       self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
     self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py
index 8c6d188643..fdd077ac49 100644
--- a/selfdrive/car/subaru/interface.py
+++ b/selfdrive/car/subaru/interface.py
@@ -123,7 +123,7 @@ class CarInterface(CarInterfaceBase):
 
   def apply(self, c):
     hud_control = c.hudControl
-    ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators,
+    ret = self.CC.update(c, self.CS, self.frame, c.actuators,
                          c.cruiseControl.cancel, hud_control.visualAlert,
                          hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)
     self.frame += 1
diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py
index 9bd3d009f8..ee11485291 100644
--- a/selfdrive/car/subaru/values.py
+++ b/selfdrive/car/subaru/values.py
@@ -74,12 +74,14 @@ FW_VERSIONS = {
       b'\x7a\x94\x3f\x90\x00',
       b'\xa2 \x185\x00',
       b'\xa2 \x193\x00',
+      b'\xa2 \x194\x00',
       b'z\x94.\x90\x00',
       b'z\x94\b\x90\x01',
       b'\xa2 \x19`\x00',
       b'z\x94\f\x90\001',
       b'z\x9c\x19\x80\x01',
       b'z\x94\x08\x90\x00',
+      b'z\x84\x19\x90\x00',
     ],
     (Ecu.eps, 0x746, None): [
       b'\x7a\xc0\x0c\x00',
diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py
index 03f09f2407..d5a5bb8629 100644
--- a/selfdrive/car/tesla/carcontroller.py
+++ b/selfdrive/car/tesla/carcontroller.py
@@ -12,12 +12,12 @@ class CarController():
     self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt'])
     self.tesla_can = TeslaCAN(self.packer, self.pt_packer)
 
-  def update(self, c, enabled, CS, frame, actuators, cruise_cancel):
+  def update(self, c, CS, frame, actuators, cruise_cancel):
     can_sends = []
 
     # Temp disable steering on a hands_on_fault, and allow for user override
     hands_on_fault = (CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3)
-    lkas_enabled = c.active and (not hands_on_fault)
+    lkas_enabled = c.latActive and (not hands_on_fault)
 
     if lkas_enabled:
       apply_angle = actuators.steeringAngleDeg
@@ -50,10 +50,6 @@ class CarController():
     if hands_on_fault:
       cruise_cancel = True
 
-    # Cancel when openpilot is not enabled anymore
-    if not enabled and bool(CS.out.cruiseState.enabled):
-      cruise_cancel = True
-
     if ((frame % 10) == 0 and cruise_cancel):
       # Spam every possible counter value, otherwise it might not be accepted
       for counter in range(16):
diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py
index 51ae43ad1b..42a7983505 100644
--- a/selfdrive/car/tesla/carstate.py
+++ b/selfdrive/car/tesla/carstate.py
@@ -1,10 +1,10 @@
 import copy
 from cereal import car
+from common.conversions import Conversions as CV
 from selfdrive.car.tesla.values import DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS
 from selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
-from selfdrive.config import Conversions as CV
 
 class CarState(CarStateBase):
   def __init__(self, CP):
@@ -43,8 +43,8 @@ class CarState(CarStateBase):
     ret.steeringRateDeg = -cp.vl["STW_ANGLHP_STAT"]["StW_AnglHP_Spd"] # This is from a different angle sensor, and at different rate
     ret.steeringTorque = -cp.vl["EPAS_sysStatus"]["EPAS_torsionBarTorque"]
     ret.steeringPressed = (self.hands_on_level > 0)
-    ret.steerError = steer_status == "EAC_FAULT"
-    ret.steerWarning = self.steer_warning != "EAC_ERROR_IDLE"
+    ret.steerFaultPermanent = steer_status == "EAC_FAULT"
+    ret.steerFaultTemporary = self.steer_warning != "EAC_ERROR_IDLE"
 
     # Cruise state
     cruise_state = self.can_define.dv["DI_state"]["DI_cruiseState"].get(int(cp.vl["DI_state"]["DI_cruiseState"]), None)
diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py
index 746cd356ea..3bd1a0efca 100755
--- a/selfdrive/car/tesla/interface.py
+++ b/selfdrive/car/tesla/interface.py
@@ -71,6 +71,6 @@ class CarInterface(CarInterfaceBase):
     return self.CS.out
 
   def apply(self, c):
-    ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel)
+    ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel)
     self.frame += 1
     return ret
diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py
index 1301802860..d48cd284d9 100644
--- a/selfdrive/car/tesla/teslacan.py
+++ b/selfdrive/car/tesla/teslacan.py
@@ -1,6 +1,6 @@
 import copy
 import crcmod
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.car.tesla.values import CANBUS, CarControllerParams
 
 
diff --git a/selfdrive/test/test_routes.py b/selfdrive/car/tests/routes.py
similarity index 99%
rename from selfdrive/test/test_routes.py
rename to selfdrive/car/tests/routes.py
index 02bcd36b19..1980a7f0ed 100644
--- a/selfdrive/test/test_routes.py
+++ b/selfdrive/car/tests/routes.py
@@ -69,7 +69,7 @@ routes = [
   TestRoute("6fe86b4e410e4c37|2020-07-22--16-27-13", HYUNDAI.HYUNDAI_GENESIS),
   TestRoute("70c5bec28ec8e345|2020-08-08--12-22-23", HYUNDAI.GENESIS_G70),
   TestRoute("6b301bf83f10aa90|2020-11-22--16-45-07", HYUNDAI.GENESIS_G80),
-  TestRoute("38bfd238edecbcd7|2018-08-29--22-02-15", HYUNDAI.SANTA_FE),
+  TestRoute("4dbd55df87507948|2022-03-01--09-45-38", HYUNDAI.SANTA_FE),
   TestRoute("bf43d9df2b660eb0|2021-09-23--14-16-37", HYUNDAI.SANTA_FE_2022),
   TestRoute("37398f32561a23ad|2021-11-18--00-11-35", HYUNDAI.SANTA_FE_HEV_2022),
   TestRoute("656ac0d830792fcc|2021-12-28--14-45-56", HYUNDAI.SANTA_FE_PHEV_2022),
diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py
index 930619ee67..2f4984c3d9 100755
--- a/selfdrive/car/tests/test_car_interfaces.py
+++ b/selfdrive/car/tests/test_car_interfaces.py
@@ -12,7 +12,6 @@ class TestCarInterfaces(unittest.TestCase):
 
   @parameterized.expand([(car,) for car in all_known_cars()])
   def test_car_interfaces(self, car_name):
-    print(car_name)
     if car_name in FINGERPRINTS:
       fingerprint = FINGERPRINTS[car_name][0]
     else:
diff --git a/selfdrive/test/test_fingerprints.py b/selfdrive/car/tests/test_fingerprints.py
similarity index 100%
rename from selfdrive/test/test_fingerprints.py
rename to selfdrive/car/tests/test_fingerprints.py
diff --git a/selfdrive/test/test_models.py b/selfdrive/car/tests/test_models.py
similarity index 97%
rename from selfdrive/test/test_models.py
rename to selfdrive/car/tests/test_models.py
index 3be0f96b37..09d17462e6 100755
--- a/selfdrive/test/test_models.py
+++ b/selfdrive/car/tests/test_models.py
@@ -17,7 +17,7 @@ from selfdrive.car.gm.values import CAR as GM
 from selfdrive.car.honda.values import CAR as HONDA, HONDA_BOSCH
 from selfdrive.car.hyundai.values import CAR as HYUNDAI
 from selfdrive.car.toyota.values import CAR as TOYOTA
-from selfdrive.test.test_routes import routes, non_tested_cars
+from selfdrive.car.tests.routes import routes, non_tested_cars
 from selfdrive.test.openpilotci import get_url
 from tools.lib.logreader import LogReader
 
@@ -29,11 +29,6 @@ PandaType = log.PandaState.PandaType
 NUM_JOBS = int(os.environ.get("NUM_JOBS", "1"))
 JOB_ID = int(os.environ.get("JOB_ID", "0"))
 
-# TODO: get updated routes for these cars
-ignore_can_valid = [
-  HYUNDAI.SANTA_FE,
-]
-
 ignore_addr_checks_valid = [
   GM.BUICK_REGAL,
   HYUNDAI.GENESIS_G70_2020,
@@ -135,8 +130,7 @@ class TestCarModel(unittest.TestCase):
       if i > 200:
         can_invalid_cnt += not CS.canValid
 
-    if self.car_model not in ignore_can_valid:
-      self.assertLess(can_invalid_cnt, 50)
+    self.assertLess(can_invalid_cnt, 50)
 
   def test_radar_interface(self):
     os.environ['NO_RADAR_SLEEP'] = "1"
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 87ba0055f0..5ec69488d5 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -22,11 +22,11 @@ class CarController():
     self.gas = 0
     self.accel = 0
 
-  def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
+  def update(self, c, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
              left_line, right_line, lead, left_lane_depart, right_lane_depart):
 
     # gas and brake
-    if CS.CP.enableGasInterceptor and active:
+    if CS.CP.enableGasInterceptor and c.longActive:
       MAX_INTERCEPTOR_GAS = 0.5
       # RAV4 has very sensitive gas pedal
       if CS.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH):
@@ -49,7 +49,7 @@ class CarController():
     self.steer_rate_limited = new_steer != apply_steer
 
     # Cut steering while we're in a known fault state (2s)
-    if not active or CS.steer_state in (9, 25):
+    if not c.latActive or CS.steer_state in (9, 25):
       apply_steer = 0
       apply_steer_req = 0
     else:
@@ -57,7 +57,7 @@ class CarController():
 
     # TODO: probably can delete this. CS.pcm_acc_status uses a different signal
     # than CS.cruiseState.enabled. confirm they're not meaningfully different
-    if not enabled and CS.pcm_acc_status:
+    if not c.enabled and CS.pcm_acc_status:
       pcm_cancel_cmd = 1
 
     # on entering standstill, send standstill request
@@ -122,7 +122,7 @@ class CarController():
       send_ui = True
 
     if (frame % 100 == 0 or send_ui):
-      can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, enabled))
+      can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, c.enabled))
 
     if frame % 100 == 0 and CS.CP.enableDsu:
       can_sends.append(create_fcw_command(self.packer, fcw_alert))
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index d73460ef32..a951955505 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -1,11 +1,11 @@
 from cereal import car
+from common.conversions import Conversions as CV
 from common.numpy_fast import mean
 from common.filter_simple import FirstOrderFilter
 from common.realtime import DT_CTRL
 from opendbc.can.can_define import CANDefine
-from selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
-from selfdrive.config import Conversions as CV
+from selfdrive.car.interfaces import CarStateBase
 from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, EPS_SCALE
 
 
@@ -19,7 +19,6 @@ class CarState(CarStateBase):
     # On cars with cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
     # the signal is zeroed to where the steering angle is at start.
     # Need to apply an offset as soon as the steering angle measurements are both received
-    self.needs_angle_offset = True
     self.accurate_steer_angle_seen = False
     self.angle_offset = FirstOrderFilter(None, 60.0, DT_CTRL, initialized=False)
 
@@ -32,6 +31,7 @@ class CarState(CarStateBase):
     ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"],
                         cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]])
     ret.seatbeltUnlatched = cp.vl["BODY_CONTROL_STATE"]["SEATBELT_DRIVER_UNLATCHED"] != 0
+    ret.parkingBrake = cp.vl["BODY_CONTROL_STATE"]["PARKING_BRAKE"] == 1
 
     ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
     ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1
@@ -58,8 +58,8 @@ class CarState(CarStateBase):
     ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
     torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
 
-    # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
-    if abs(torque_sensor_angle_deg) > 1e-3:
+    # On some cars, the angle measurement is non-zero while initializing
+    if abs(torque_sensor_angle_deg) > 1e-3 and not bool(cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE_INITIALIZING"]):
       self.accurate_steer_angle_seen = True
 
     if self.accurate_steer_angle_seen:
@@ -82,7 +82,7 @@ class CarState(CarStateBase):
     ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] * self.eps_torque_scale
     # 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)
+    ret.steerFaultTemporary = cp.vl["EPS_STATUS"]["LKA_STATE"] not in (1, 5)
 
     if self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
       ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
@@ -141,6 +141,7 @@ class CarState(CarStateBase):
       ("DOOR_OPEN_RL", "BODY_CONTROL_STATE"),
       ("DOOR_OPEN_RR", "BODY_CONTROL_STATE"),
       ("SEATBELT_DRIVER_UNLATCHED", "BODY_CONTROL_STATE"),
+      ("PARKING_BRAKE", "BODY_CONTROL_STATE"),
       ("TC_DISABLED", "ESP_CONTROL"),
       ("BRAKE_HOLD_ACTIVE", "ESP_CONTROL"),
       ("STEER_FRACTION", "STEER_ANGLE_SENSOR"),
@@ -151,6 +152,7 @@ class CarState(CarStateBase):
       ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR"),
       ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR"),
       ("STEER_ANGLE", "STEER_TORQUE_SENSOR"),
+      ("STEER_ANGLE_INITIALIZING", "STEER_TORQUE_SENSOR"),
       ("TURN_SIGNALS", "BLINKERS_STATE"),
       ("LKA_STATE", "EPS_STATUS"),
       ("AUTO_HIGH_BEAM", "LIGHT_STALK"),
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index be9d7fd587..4ac13d3d97 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 from cereal import car
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.car.toyota.tunes import LatTunes, LongTunes, set_long_tune, set_lat_tune
 from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, CarControllerParams
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
@@ -103,6 +103,9 @@ class CarInterface(CarInterfaceBase):
       set_lat_tune(ret.lateralTuning, LatTunes.PID_G)
 
     elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019, CAR.AVALON_TSS2):
+      # starting from 2019, all Avalon variants have stop and go
+      # https://engage.toyota.com/static/images/toyota_safety_sense/TSS_Applicability_Chart.pdf
+      stop_and_go = candidate != CAR.AVALON
       ret.wheelbase = 2.82
       ret.steerRatio = 14.8  # Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download
       tire_stiffness_factor = 0.7983
@@ -270,7 +273,7 @@ class CarInterface(CarInterfaceBase):
   # to be called @ 100hz
   def apply(self, c):
     hud_control = c.hudControl
-    ret = self.CC.update(c.enabled, c.active, self.CS, self.frame,
+    ret = self.CC.update(c, self.CS, self.frame,
                          c.actuators, c.cruiseControl.cancel,
                          hud_control.visualAlert, hud_control.leftLaneVisible,
                          hud_control.rightLaneVisible, hud_control.leadVisible,
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index b8a6ae7e0d..acf3f42c38 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -2,8 +2,8 @@ from collections import defaultdict
 from enum import IntFlag
 
 from cereal import car
+from common.conversions import Conversions as CV
 from selfdrive.car import dbc_dict
-from selfdrive.config import Conversions as CV
 
 Ecu = car.CarParams.Ecu
 MIN_ACC_SPEED = 19. * CV.MPH_TO_MS
@@ -30,7 +30,7 @@ class CAR:
   AVALON = "TOYOTA AVALON 2016"
   AVALON_2019 = "TOYOTA AVALON 2019"
   AVALONH_2019 = "TOYOTA AVALON HYBRID 2019"
-  AVALON_TSS2 = "TOYOTA AVALON 2022"
+  AVALON_TSS2 = "TOYOTA AVALON 2022"  # TSS 2.5
   CAMRY = "TOYOTA CAMRY 2018"
   CAMRYH = "TOYOTA CAMRY HYBRID 2018"
   CAMRY_TSS2 = "TOYOTA CAMRY 2021"  # TSS 2.5
@@ -1388,6 +1388,7 @@ FW_VERSIONS = {
     (Ecu.engine, 0x700, None): [
       b'\x018966378B2100\x00\x00\x00\x00',
       b'\x018966378G3000\x00\x00\x00\x00',
+      b'\x018966378B3000\x00\x00\x00\x00'
     ],
     (Ecu.esp, 0x7b0, None): [
       b'\x01F152678221\x00\x00\x00\x00\x00\x00',
@@ -1397,6 +1398,7 @@ FW_VERSIONS = {
     ],
     (Ecu.fwdRadar, 0x750, 0xf): [
       b"\x018821F3301400\x00\x00\x00\x00",
+      b'\x018821F3301200\x00\x00\x00\x00',
     ],
     (Ecu.fwdCamera, 0x750, 0x6d): [
       b'\x028646F78030A0\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py
index f85a81a538..5726c98285 100644
--- a/selfdrive/car/volkswagen/carcontroller.py
+++ b/selfdrive/car/volkswagen/carcontroller.py
@@ -21,7 +21,7 @@ class CarController():
 
     self.steer_rate_limited = False
 
-  def update(self, c, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart):
+  def update(self, c, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart):
     """ Controls thread """
 
     can_sends = []
@@ -39,7 +39,7 @@ class CarController():
       # torque value. Do that anytime we happen to have 0 torque, or failing that,
       # when exceeding ~1/3 the 360 second timer.
 
-      if c.active and CS.out.vEgo > CS.CP.minSteerSpeed and not (CS.out.standstill or CS.out.steerError or CS.out.steerWarning):
+      if c.latActive:
         new_steer = int(round(actuators.steer * P.STEER_MAX))
         apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
         self.steer_rate_limited = new_steer != apply_steer
@@ -77,7 +77,7 @@ class CarController():
       else:
         hud_alert = MQB_LDW_MESSAGES["none"]
 
-      can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, enabled,
+      can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, c.enabled,
                                                             CS.out.steeringPressed, hud_alert, left_lane_visible,
                                                             right_lane_visible, CS.ldw_stock_values,
                                                             left_lane_depart, right_lane_depart))
@@ -88,11 +88,11 @@ class CarController():
 
     if CS.CP.pcmCruise:
       if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP:
-        if not enabled and CS.out.cruiseState.enabled:
+        if c.cruiseControl.cancel:
           # Cancel ACC if it's engaged with OP disengaged.
           self.graButtonStatesToSend = BUTTON_STATES.copy()
           self.graButtonStatesToSend["cancel"] = True
-        elif enabled and CS.esp_hold_confirmation:
+        elif c.enabled and CS.esp_hold_confirmation:
           # Blip the Resume button if we're engaged at standstill.
           # FIXME: This is a naive implementation, improve with visiond or radar input.
           self.graButtonStatesToSend = BUTTON_STATES.copy()
diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py
index 1fe8b56b90..858b5e1440 100644
--- a/selfdrive/car/volkswagen/carstate.py
+++ b/selfdrive/car/volkswagen/carstate.py
@@ -1,6 +1,6 @@
 import numpy as np
 from cereal import car
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
@@ -41,14 +41,15 @@ class CarState(CarStateBase):
 
     # Verify EPS readiness to accept steering commands
     hca_status = self.hca_status_values.get(pt_cp.vl["LH_EPS_03"]["EPS_HCA_Status"])
-    ret.steerError = hca_status in ("DISABLED", "FAULT")
-    ret.steerWarning = hca_status in ("INITIALIZING", "REJECTED")
+    ret.steerFaultPermanent = hca_status in ("DISABLED", "FAULT")
+    ret.steerFaultTemporary = hca_status in ("INITIALIZING", "REJECTED")
 
     # Update gas, brakes, and gearshift.
     ret.gas = pt_cp.vl["Motor_20"]["MO_Fahrpedalrohwert_01"] / 100.0
     ret.gasPressed = ret.gas > 0
     ret.brake = pt_cp.vl["ESP_05"]["ESP_Bremsdruck"] / 250.0  # FIXME: this is pressure in Bar, not sure what OP expects
     ret.brakePressed = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"])
+    ret.parkingBrake = bool(pt_cp.vl["Kombi_01"]["KBI_Handbremse"])  # FIXME: need to include an EPB check as well
     self.esp_hold_confirmation = pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"]
 
     # Update gear and/or clutch position data.
@@ -140,7 +141,6 @@ class CarState(CarStateBase):
     self.graMsgBusCounter = pt_cp.vl["GRA_ACC_01"]["COUNTER"]
 
     # Additional safety checks performed in CarInterface.
-    self.parkingBrakeSet = bool(pt_cp.vl["Kombi_01"]["KBI_Handbremse"])  # FIXME: need to include an EPB check as well
     ret.espDisabled = pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"] != 0
 
     return ret
diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py
index 961cfed7fe..f96f1bae5a 100644
--- a/selfdrive/car/volkswagen/interface.py
+++ b/selfdrive/car/volkswagen/interface.py
@@ -186,8 +186,6 @@ class CarInterface(CarInterfaceBase):
     events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic])
 
     # Vehicle health and operation safety checks
-    if self.CS.parkingBrakeSet:
-      events.add(EventName.parkBrake)
     if self.CS.tsk_status in (6, 7):
       events.add(EventName.accFaulted)
 
@@ -211,7 +209,7 @@ class CarInterface(CarInterfaceBase):
 
   def apply(self, c):
     hud_control = c.hudControl
-    ret = self.CC.update(c, c.enabled, self.CS, self.frame, self.ext_bus, c.actuators,
+    ret = self.CC.update(c, self.CS, self.frame, self.ext_bus, c.actuators,
                          hud_control.visualAlert,
                          hud_control.leftLaneVisible,
                          hud_control.rightLaneVisible,
diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py
index 241338c2c5..935407fca8 100755
--- a/selfdrive/car/volkswagen/values.py
+++ b/selfdrive/car/volkswagen/values.py
@@ -301,6 +301,7 @@ FW_VERSIONS = {
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x8704E906024AK\xf1\x899937',
       b'\xf1\x8704E906024AS\xf1\x899912',
+      b'\xf1\x8704E906024BC\xf1\x899971',
       b'\xf1\x8704E906024B \xf1\x895594',
       b'\xf1\x8704E906024C \xf1\x899970',
       b'\xf1\x8704E906024L \xf1\x895595',
@@ -311,6 +312,7 @@ FW_VERSIONS = {
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x8709G927158BQ\xf1\x893545',
       b'\xf1\x8709S927158BS\xf1\x893642',
+      b'\xf1\x8709S927158BS\xf1\x893694',
       b'\xf1\x8709S927158R \xf1\x893552',
       b'\xf1\x8709S927158R \xf1\x893587',
       b'\xf1\x870GC300020N \xf1\x892803',
@@ -426,9 +428,11 @@ FW_VERSIONS = {
       b'\xf1\x8704L906027G \xf1\x899893',
       b'\xf1\x875N0906259  \xf1\x890002',
       b'\xf1\x8783A907115B \xf1\x890005',
+      b'\xf1\x8783A907115G \xf1\x890001',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x8709G927158DT\xf1\x893698',
+      b'\xf1\x8709G927158GD\xf1\x893820',
       b'\xf1\x870DL300011N \xf1\x892001',
       b'\xf1\x870DL300011N \xf1\x892012',
       b'\xf1\x870DL300013A \xf1\x893005',
@@ -439,6 +443,7 @@ FW_VERSIONS = {
       b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02316143231313500314641011750179333423100',
       b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02312110031333300314240583752379333423100',
       b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02331310031333336313140013950399333423100',
+      b'\xf1\x875Q0959655CB\xf1\x890421\xf1\x82\x1316143231313500314647021750179333613100',
     ],
     (Ecu.eps, 0x712, None): [
       b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820529A6060603',
@@ -450,6 +455,7 @@ FW_VERSIONS = {
       b'\xf1\x872Q0907572J \xf1\x890156',
       b'\xf1\x872Q0907572Q \xf1\x890342',
       b'\xf1\x872Q0907572R \xf1\x890372',
+      b'\xf1\x872Q0907572T \xf1\x890383',
     ],
   },
   CAR.TOURAN_MK2: {
@@ -516,6 +522,7 @@ FW_VERSIONS = {
       b'\xf1\x875G0906259L \xf1\x890002',
       b'\xf1\x875G0906259Q \xf1\x890002',
       b'\xf1\x878V0906259F \xf1\x890002',
+      b'\xf1\x878V0906259K \xf1\x890001',
       b'\xf1\x878V0906264B \xf1\x890003',
       b'\xf1\x878V0907115B \xf1\x890007',
     ],
@@ -525,6 +532,7 @@ FW_VERSIONS = {
       b'\xf1\x870D9300012  \xf1\x894912',
       b'\xf1\x870D9300013B \xf1\x894931',
       b'\xf1\x870D9300041N \xf1\x894512',
+      b'\xf1\x870D9300043T \xf1\x899699',
       b'\xf1\x870DD300046A \xf1\x891602',
       b'\xf1\x870DD300046F \xf1\x891602',
       b'\xf1\x870DD300046G \xf1\x891601',
@@ -542,6 +550,7 @@ FW_VERSIONS = {
     ],
     (Ecu.eps, 0x712, None): [
       b'\xf1\x873Q0909144H \xf1\x895061\xf1\x82\00566G0HA14A1',
+      b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571G0HA16A1',
       b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521G0G809A1',
       b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00503G00303A0',
       b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00503G00803A0',
@@ -705,11 +714,13 @@ FW_VERSIONS = {
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x8704E906016ER\xf1\x895823',
       b'\xf1\x8704E906027HD\xf1\x893742',
+      b'\xf1\x8704E906027MH\xf1\x894786',
       b'\xf1\x8704L906021DT\xf1\x898127',
       b'\xf1\x8704L906026BS\xf1\x891541',
       b'\xf1\x875G0906259C \xf1\x890002',
     ],
     (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\x870CW300041L \xf1\x891601',
       b'\xf1\x870CW300041N \xf1\x891605',
       b'\xf1\x870CW300043B \xf1\x891601',
       b'\xf1\x870D9300041C \xf1\x894936',
@@ -722,11 +733,13 @@ FW_VERSIONS = {
       b'\xf1\x873Q0959655AS\xf1\x890200\xf1\x82\r11120011100010022212110200',
       b'\xf1\x873Q0959655BH\xf1\x890703\xf1\x82\0163221003221002105755331052100',
       b'\xf1\x873Q0959655CN\xf1\x890720\xf1\x82\x0e3221003221002105755331052100',
+      b'\xf1\x875QD959655  \xf1\x890388\xf1\x82\x111101000011110006110411111111119111',
     ],
     (Ecu.eps, 0x712, None): [
       b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\00566A01513A1',
       b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\00521T00403A1',
       b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521T00403A1',
+      b'\xf1\x875QD909144E \xf1\x891081\xf1\x82\x0521T00503A1',
       b'\xf1\x875Q0909144R \xf1\x891061\xf1\x82\x0516A00604A1',
     ],
     (Ecu.fwdRadar, 0x757, None): [
@@ -734,6 +747,7 @@ FW_VERSIONS = {
       b'\xf1\x875Q0907572F \xf1\x890400\xf1\x82\00101',
       b'\xf1\x875Q0907572J \xf1\x890654',
       b'\xf1\x875Q0907572P \xf1\x890682',
+      b'\xf1\x875Q0907572R \xf1\x890771',
     ],
   },
   CAR.SKODA_SCALA_MK1: {
diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc
index 675a45fd7f..25062b4975 100644
--- a/selfdrive/common/params.cc
+++ b/selfdrive/common/params.cc
@@ -91,7 +91,6 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
     {"CarParamsCache", CLEAR_ON_MANAGER_START},
     {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
-    {"CellularUnmetered", PERSISTENT},
     {"CompletedTrainingVersion", PERSISTENT},
     {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
     {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
diff --git a/selfdrive/config.py b/selfdrive/config.py
deleted file mode 100644
index 511f6126c4..0000000000
--- a/selfdrive/config.py
+++ /dev/null
@@ -1,29 +0,0 @@
-import numpy as np
-
-class Conversions:
-  #Speed
-  MPH_TO_KPH = 1.609344
-  KPH_TO_MPH = 1. / MPH_TO_KPH
-  MS_TO_KPH = 3.6
-  KPH_TO_MS = 1. / MS_TO_KPH
-  MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH
-  MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS
-  MS_TO_KNOTS = 1.9438
-  KNOTS_TO_MS = 1. / MS_TO_KNOTS
-  #Angle
-  DEG_TO_RAD = np.pi / 180.
-  RAD_TO_DEG = 1. / DEG_TO_RAD
-  #Mass
-  LB_TO_KG = 0.453592
-
-
-RADAR_TO_CENTER = 2.7   # (deprecated) RADAR is ~ 2.7m ahead from center of car
-RADAR_TO_CAMERA = 1.52   # RADAR is ~ 1.5m ahead from center of mesh frame
-
-class UIParams:
-  lidar_x, lidar_y, lidar_zoom = 384, 960, 6
-  lidar_car_x, lidar_car_y = lidar_x / 2., lidar_y / 1.1
-  car_hwidth = 1.7272 / 2 * lidar_zoom
-  car_front = 2.6924 * lidar_zoom
-  car_back = 1.8796 * lidar_zoom
-  car_color = 110
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 1a34d31311..fcb1a4974a 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -9,7 +9,7 @@ from common.realtime import sec_since_boot, config_realtime_process, Priority, R
 from common.profiler import Profiler
 from common.params import Params, put_nonblocking
 import cereal.messaging as messaging
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.swaglog import cloudlog
 from selfdrive.boardd.boardd import can_list_to_can_capnp
 from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can
@@ -298,7 +298,7 @@ class Controls:
         self.events.add(EventName.cruiseMismatch)
 
     # Check for FCW
-    stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.5
+    stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25
     model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking
     planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled
     if planner_fcw or model_fcw:
@@ -455,10 +455,11 @@ class Controls:
           else:
             self.state = State.enabled
           self.current_alert_types.append(ET.ENABLE)
-          self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)
+          if not self.CP.pcmCruise:
+            self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)
 
     # Check if actuators are enabled
-    self.active = self.state == State.enabled or self.state == State.softDisabling
+    self.active = self.state in (State.enabled, State.softDisabling)
     if self.active:
       self.current_alert_types.append(ET.WARNING)
 
@@ -466,7 +467,7 @@ class Controls:
     self.enabled = self.active or self.state == State.preEnabled
 
   def state_control(self, CS):
-    """Given the state, this function returns an actuators packet"""
+    """Given the state, this function returns a CarControl packet"""
 
     # Update VehicleModel
     params = self.sm['liveParameters']
@@ -477,7 +478,14 @@ class Controls:
     lat_plan = self.sm['lateralPlan']
     long_plan = self.sm['longitudinalPlan']
 
-    actuators = car.CarControl.Actuators.new_message()
+    CC = car.CarControl.new_message()
+    CC.enabled = self.enabled
+    # Check which actuators can be enabled
+    CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
+                     CS.vEgo > self.CP.minSteerSpeed and not CS.standstill
+    CC.longActive = self.active
+
+    actuators = CC.actuators
     actuators.longControlState = self.LoC.long_control_state
 
     if CS.leftBlinker or CS.rightBlinker:
@@ -485,37 +493,40 @@ class Controls:
 
     # State specific actions
 
-    if not self.active:
+    if not CC.latActive:
       self.LaC.reset()
+    if not CC.longActive:
       self.LoC.reset(v_pid=CS.vEgo)
 
     if not self.joystick_mode:
       # accel PID loop
       pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS)
       t_since_plan = (self.sm.frame - self.sm.rcv_frame['longitudinalPlan']) * DT_CTRL
-      actuators.accel = self.LoC.update(self.active, CS, self.CP, long_plan, pid_accel_limits, t_since_plan)
+      actuators.accel = self.LoC.update(CC.longActive, CS, self.CP, long_plan, pid_accel_limits, t_since_plan)
 
       # Steering PID loop and lateral MPC
-      lat_active = self.active and not CS.steerWarning and not CS.steerError and CS.vEgo > self.CP.minSteerSpeed
       desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo,
                                                                              lat_plan.psis,
                                                                              lat_plan.curvatures,
                                                                              lat_plan.curvatureRates)
-      actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(lat_active, CS, self.CP, self.VM, params, self.last_actuators,
-                                                                             desired_curvature, desired_curvature_rate)
+      actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM,
+                                                                             params, self.last_actuators, desired_curvature,
+                                                                             desired_curvature_rate)
     else:
       lac_log = log.ControlsState.LateralDebugState.new_message()
-      if self.sm.rcv_frame['testJoystick'] > 0 and self.active:
-        actuators.accel = 4.0*clip(self.sm['testJoystick'].axes[0], -1, 1)
+      if self.sm.rcv_frame['testJoystick'] > 0:
+        if CC.longActive:
+          actuators.accel = 4.0*clip(self.sm['testJoystick'].axes[0], -1, 1)
 
-        steer = clip(self.sm['testJoystick'].axes[1], -1, 1)
-        # max angle is 45 for angle-based cars
-        actuators.steer, actuators.steeringAngleDeg = steer, steer * 45.
+        if CC.latActive:
+          steer = clip(self.sm['testJoystick'].axes[1], -1, 1)
+          # max angle is 45 for angle-based cars
+          actuators.steer, actuators.steeringAngleDeg = steer, steer * 45.
 
-        lac_log.active = True
+        lac_log.active = self.active
         lac_log.steeringAngleDeg = CS.steeringAngleDeg
-        lac_log.output = steer
-        lac_log.saturated = abs(steer) >= 0.9
+        lac_log.output = actuators.steer
+        lac_log.saturated = abs(actuators.steer) >= 0.9
 
     # Send a "steering required alert" if saturation count has reached the limit
     if lac_log.active and lac_log.saturated and not CS.steeringPressed:
@@ -539,7 +550,7 @@ class Controls:
         cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
         setattr(actuators, p, 0.0)
 
-    return actuators, lac_log
+    return CC, lac_log
 
   def update_button_timers(self, buttonEvents):
     # increment timer for buttons still pressed
@@ -551,14 +562,9 @@ class Controls:
       if b.type.raw in self.button_timers:
         self.button_timers[b.type.raw] = 1 if b.pressed else 0
 
-  def publish_logs(self, CS, start_time, actuators, lac_log):
+  def publish_logs(self, CS, start_time, CC, lac_log):
     """Send actuators and hud commands to the car, send controlsstate and MPC logging"""
 
-    CC = car.CarControl.new_message()
-    CC.enabled = self.enabled
-    CC.active = self.active
-    CC.actuators = actuators
-
     orientation_value = self.sm['liveLocationKalman'].orientationNED.value
     if len(orientation_value) > 2:
       CC.roll = orientation_value[0]
@@ -579,7 +585,7 @@ class Controls:
 
     recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0  # 5s blinker cooldown
     ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
-                    and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
+                    and not CC.latActive and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
 
     model_v2 = self.sm['modelV2']
     desire_prediction = model_v2.meta.desirePrediction
@@ -718,12 +724,12 @@ class Controls:
       self.prof.checkpoint("State transition")
 
     # Compute actuators (runs PID loops and lateral MPC)
-    actuators, lac_log = self.state_control(CS)
+    CC, lac_log = self.state_control(CS)
 
     self.prof.checkpoint("State Control")
 
     # Publish data
-    self.publish_logs(CS, start_time, actuators, lac_log)
+    self.publish_logs(CS, start_time, CC, lac_log)
     self.prof.checkpoint("Sent")
 
     self.update_button_timers(CS.buttonEvents)
diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py
index c34d143a5a..978c386530 100644
--- a/selfdrive/controls/lib/desire_helper.py
+++ b/selfdrive/controls/lib/desire_helper.py
@@ -1,6 +1,6 @@
 from cereal import log
+from common.conversions import Conversions as CV
 from common.realtime import DT_MDL
-from selfdrive.config import Conversions as CV
 
 LaneChangeState = log.LateralPlan.LaneChangeState
 LaneChangeDirection = log.LateralPlan.LaneChangeDirection
diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py
index 01d7600345..f34467081b 100644
--- a/selfdrive/controls/lib/drive_helpers.py
+++ b/selfdrive/controls/lib/drive_helpers.py
@@ -2,7 +2,7 @@ import math
 from cereal import car
 from common.numpy_fast import clip, interp
 from common.realtime import DT_MDL
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.modeld.constants import T_IDXS
 
 # WARNING: this value was determined based on the model's training distribution,
@@ -54,7 +54,7 @@ def update_v_cruise(v_cruise_kph, buttonEvents, button_timers, enabled, metric):
   long_press = False
   button_type = None
 
-  v_cruise_delta = 1 if metric else 1.6
+  v_cruise_delta = 1. if metric else CV.MPH_TO_KPH
 
   for b in buttonEvents:
     if b.type.raw in button_timers and not b.pressed:
@@ -91,9 +91,9 @@ def initialize_v_cruise(v_ego, buttonEvents, v_cruise_last):
 
 def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
   if len(psis) != CONTROL_N:
-    psis = [0.0 for i in range(CONTROL_N)]
-    curvatures = [0.0 for i in range(CONTROL_N)]
-    curvature_rates = [0.0 for i in range(CONTROL_N)]
+    psis = [0.0]*CONTROL_N
+    curvatures = [0.0]*CONTROL_N
+    curvature_rates = [0.0]*CONTROL_N
 
   # TODO this needs more thought, use .2s extra for now to estimate other delays
   delay = CP.steerActuatorDelay + .2
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index 931932cc0d..e04ec262de 100644
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -4,8 +4,8 @@ from typing import Dict, Union, Callable, List, Optional
 
 from cereal import log, car
 import cereal.messaging as messaging
+from common.conversions import Conversions as CV
 from common.realtime import DT_CTRL
-from selfdrive.config import Conversions as CV
 from selfdrive.locationd.calibrationd import MIN_SPEED_FILTER
 from selfdrive.version import get_short_branch
 
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/SConscript b/selfdrive/controls/lib/lateral_mpc_lib/SConscript
index f402e6e15e..64eada95c1 100644
--- a/selfdrive/controls/lib/lateral_mpc_lib/SConscript
+++ b/selfdrive/controls/lib/lateral_mpc_lib/SConscript
@@ -43,11 +43,19 @@ generated_files = [
   f'{gen}/lat_cost/lat_cost_y_0_fun.h',
 ] + build_files
 
+acados_dir = '#third_party/acados'
+source_list = ['lat_mpc.py',
+  f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
+  f'{acados_dir}/aarch64/lib/libacados.so',
+  f'{acados_dir}/x86_64/lib/libacados.so',
+  f'{acados_dir}/larch64/lib/libacados.so',
+]
+
 lenv = env.Clone()
 lenv.Clean(generated_files, Dir(gen))
 
 lenv.Command(generated_files,
-             ["lat_mpc.py"],
+             source_list,
              f"cd {Dir('.').abspath} && python3 lat_mpc.py")
 
 lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
index e4a73bf97d..a1037f040b 100755
--- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
+++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
@@ -11,18 +11,19 @@ from selfdrive.modeld.constants import T_IDXS
 if __name__ == '__main__':  # generating code
   from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
-  # from pyextra.acados_template import AcadosOcpSolverFast
-  from selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverFast  # pylint: disable=no-name-in-module, import-error
+  from selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython  # pylint: disable=no-name-in-module, import-error
 
 LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
 EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
-JSON_FILE = "acados_ocp_lat.json"
+JSON_FILE = os.path.join(LAT_MPC_DIR, "acados_ocp_lat.json")
 X_DIM = 4
 P_DIM = 2
+MODEL_NAME = 'lat'
+ACADOS_SOLVER_TYPE = 'SQP_RTI'
 
 def gen_lat_model():
   model = AcadosModel()
-  model.name = 'lat'
+  model.name = MODEL_NAME
 
   # set up states & controls
   x_ego = SX.sym('x_ego')
@@ -58,7 +59,7 @@ def gen_lat_model():
   return model
 
 
-def gen_lat_mpc_solver():
+def gen_lat_ocp():
   ocp = AcadosOcp()
   ocp.model = gen_lat_model()
 
@@ -103,7 +104,7 @@ def gen_lat_mpc_solver():
   ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
   ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
   ocp.solver_options.integrator_type = 'ERK'
-  ocp.solver_options.nlp_solver_type = 'SQP_RTI'
+  ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE
   ocp.solver_options.qp_solver_iter_max = 1
   ocp.solver_options.qp_solver_cond_N = 1
 
@@ -117,7 +118,7 @@ def gen_lat_mpc_solver():
 
 class LateralMpc():
   def __init__(self, x0=np.zeros(X_DIM)):
-    self.solver = AcadosOcpSolverFast('lat', N, EXPORT_DIR)
+    self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.reset(x0)
 
   def reset(self, x0=np.zeros(X_DIM)):
@@ -173,5 +174,6 @@ class LateralMpc():
 
 
 if __name__ == "__main__":
-  ocp = gen_lat_mpc_solver()
-  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False)
+  ocp = gen_lat_ocp()
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
+  # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 3ba50fd0cf..21682d8263 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -12,14 +12,15 @@ ACCEL_MIN_ISO = -3.5  # m/s^2
 ACCEL_MAX_ISO = 2.0  # m/s^2
 
 
-def long_control_state_trans(CP, active, long_control_state, v_ego, v_target_future,
-                             brake_pressed, cruise_standstill):
+def long_control_state_trans(CP, active, long_control_state, v_ego, v_target,
+                             v_target_future, brake_pressed, cruise_standstill):
   """Update longitudinal control state machine"""
+  accelerating = v_target_future > v_target
   stopping_condition = (v_ego < 2.0 and cruise_standstill) or \
                        (v_ego < CP.vEgoStopping and
-                        (v_target_future < CP.vEgoStopping or brake_pressed))
+                        ((v_target_future < CP.vEgoStopping and not accelerating) or brake_pressed))
 
-  starting_condition = v_target_future > CP.vEgoStarting and not cruise_standstill
+  starting_condition = v_target_future > CP.vEgoStarting and accelerating and not cruise_standstill
 
   if not active:
     long_control_state = LongCtrlState.off
@@ -83,7 +84,7 @@ class LongControl():
     # Update state machine
     output_accel = self.last_output_accel
     self.long_control_state = long_control_state_trans(CP, active, self.long_control_state, CS.vEgo,
-                                                       v_target_future, CS.brakePressed,
+                                                       v_target, v_target_future, CS.brakePressed,
                                                        CS.cruiseState.standstill)
 
     if self.long_control_state == LongCtrlState.off or CS.gasPressed:
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
index 4c43985d1f..cd764114c4 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
@@ -28,8 +28,6 @@ casadi_cost_0 = [
 casadi_constraints = [
   f'{gen}/long_constraints/long_constr_h_fun.c',
   f'{gen}/long_constraints/long_constr_h_fun_jac_uxt_zt.c',
-  f'{gen}/long_constraints/long_constr_h_e_fun.c',
-  f'{gen}/long_constraints/long_constr_h_e_fun_jac_uxt_zt.c',
 ]
 
 build_files = [f'{gen}/acados_solver_long.c'] + casadi_model + casadi_cost_y + casadi_cost_e +  \
@@ -47,17 +45,24 @@ generated_files = [
 
   f'{gen}/long_model/long_model.h',
   f'{gen}/long_constraints/long_h_constraint.h',
-  f'{gen}/long_constraints/long_h_e_constraint.h',
   f'{gen}/long_cost/long_cost_y_fun.h',
   f'{gen}/long_cost/long_cost_y_e_fun.h',
   f'{gen}/long_cost/long_cost_y_0_fun.h',
 ] + build_files
 
+acados_dir = '#third_party/acados'
+source_list = ['long_mpc.py',
+  f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
+  f'{acados_dir}/aarch64/lib/libacados.so',
+  f'{acados_dir}/x86_64/lib/libacados.so',
+  f'{acados_dir}/larch64/lib/libacados.so',
+]
+
 lenv = env.Clone()
 lenv.Clean(generated_files, Dir(gen))
 
 lenv.Command(generated_files,
-             ["long_mpc.py"],
+             source_list,
              f"cd {Dir('.').abspath} && python3 long_mpc.py")
 
 lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
index f9f9c31bb4..69bf081ee3 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -11,20 +11,20 @@ from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
 if __name__ == '__main__':  # generating code
   from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
-  # from pyextra.acados_template import AcadosOcpSolver as AcadosOcpSolverFast
-  from selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverFast  # pylint: disable=no-name-in-module, import-error
+  from selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython  # pylint: disable=no-name-in-module, import-error
 
 from casadi import SX, vertcat
 
+MODEL_NAME = 'long'
 LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
 EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code")
-JSON_FILE = "acados_ocp_long.json"
+JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json")
 
 SOURCES = ['lead0', 'lead1', 'cruise']
 
 X_DIM = 3
 U_DIM = 1
-PARAM_DIM= 4
+PARAM_DIM = 4
 COST_E_DIM = 5
 COST_DIM = COST_E_DIM + 1
 CONSTR_DIM = 4
@@ -34,17 +34,18 @@ X_EGO_COST = 0.
 V_EGO_COST = 0.
 A_EGO_COST = 0.
 J_EGO_COST = 5.0
-A_CHANGE_COST = .5
+A_CHANGE_COST = 200.
 DANGER_ZONE_COST = 100.
 CRASH_DISTANCE = .5
 LIMIT_COST = 1e6
+ACADOS_SOLVER_TYPE = 'SQP_RTI'
 
 
 # Fewer timestamps don't hurt performance and lead to
 # much better convergence of the MPC with low iterations
 N = 12
 MAX_T = 10.0
-T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N+1) for idx in range(N+1)]
+T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N) for idx in range(N+1)]
 
 T_IDXS = np.array(T_IDXS_LST)
 T_DIFFS = np.diff(T_IDXS, prepend=[0.])
@@ -65,7 +66,7 @@ def desired_follow_distance(v_ego, v_lead):
 
 def gen_long_model():
   model = AcadosModel()
-  model.name = 'long'
+  model.name = MODEL_NAME
 
   # set up states & controls
   x_ego = SX.sym('x_ego')
@@ -97,7 +98,7 @@ def gen_long_model():
   return model
 
 
-def gen_long_mpc_solver():
+def gen_long_ocp():
   ocp = AcadosOcp()
   ocp.model = gen_long_model()
 
@@ -136,7 +137,7 @@ def gen_long_mpc_solver():
            x_ego,
            v_ego,
            a_ego,
-           20*(a_ego - prev_a),
+           a_ego - prev_a,
            j_ego]
   ocp.model.cost_y_expr = vertcat(*costs)
   ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
@@ -149,7 +150,6 @@ def gen_long_mpc_solver():
                         (a_max - a_ego),
                         ((x_obstacle - x_ego) - (3/4) * (desired_dist_comfort)) / (v_ego + 10.))
   ocp.model.con_h_expr = constraints
-  ocp.model.con_h_expr_e = vertcat(np.zeros(CONSTR_DIM))
 
   x0 = np.zeros(X_DIM)
   ocp.constraints.x0 = x0
@@ -163,9 +163,7 @@ def gen_long_mpc_solver():
   ocp.cost.zu = cost_weights
 
   ocp.constraints.lh = np.zeros(CONSTR_DIM)
-  ocp.constraints.lh_e = np.zeros(CONSTR_DIM)
   ocp.constraints.uh = 1e4*np.ones(CONSTR_DIM)
-  ocp.constraints.uh_e = 1e4*np.ones(CONSTR_DIM)
   ocp.constraints.idxsh = np.arange(CONSTR_DIM)
 
   # The HPIPM solver can give decent solutions even when it is stopped early
@@ -175,12 +173,13 @@ def gen_long_mpc_solver():
   ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
   ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
   ocp.solver_options.integrator_type = 'ERK'
-  ocp.solver_options.nlp_solver_type = 'SQP_RTI'
-  ocp.solver_options.qp_solver_cond_N = N//4
+  ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE
+  ocp.solver_options.qp_solver_cond_N = 1
 
   # More iterations take too much time and less lead to inaccurate convergence in
   # some situations. Ideally we would run just 1 iteration to ensure fixed runtime.
   ocp.solver_options.qp_solver_iter_max = 10
+  ocp.solver_options.qp_tol = 1e-3
 
   # set prediction horizon
   ocp.solver_options.tf = Tf
@@ -197,7 +196,7 @@ class LongitudinalMpc:
     self.source = SOURCES[2]
 
   def reset(self):
-    self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR)
+    self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.v_solution = np.zeros(N+1)
     self.a_solution = np.zeros(N+1)
     self.prev_a = np.array(self.a_solution)
@@ -215,7 +214,11 @@ class LongitudinalMpc:
     self.status = False
     self.crash_cnt = 0.0
     self.solution_status = 0
+    # timers
     self.solve_time = 0.0
+    self.time_qp_solution = 0.0
+    self.time_linearization = 0.0
+    self.time_integrator = 0.0
     self.x0 = np.zeros(X_DIM)
     self.set_weights()
 
@@ -232,6 +235,7 @@ class LongitudinalMpc:
     a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
     W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST]))
     for i in range(N):
+      # reduce the cost on (a-a_prev) later in the horizon.
       W[4,4] = a_change_cost * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0])
       self.solver.cost_set(i, 'W', W)
     # Setting the slice without the copy make the array not contiguous,
@@ -257,14 +261,12 @@ class LongitudinalMpc:
       self.solver.cost_set(i, 'Zl', Zl)
 
   def set_cur_state(self, v, a):
-    if abs(self.x0[1] - v) > 2.:
-      self.x0[1] = v
-      self.x0[2] = a
+    v_prev = self.x0[1]
+    self.x0[1] = v
+    self.x0[2] = a
+    if abs(v_prev - v) > 2.: # probably only helps if v < v_prev
       for i in range(0, N+1):
         self.solver.set(i, 'x', self.x0)
-    else:
-      self.x0[1] = v
-      self.x0[2] = a
 
   @staticmethod
   def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau):
@@ -355,9 +357,17 @@ class LongitudinalMpc:
     self.solver.constraints_set(0, "lbx", self.x0)
     self.solver.constraints_set(0, "ubx", self.x0)
 
-    t = sec_since_boot()
     self.solution_status = self.solver.solve()
-    self.solve_time = sec_since_boot() - t
+    self.solve_time = float(self.solver.get_stats('time_tot')[0])
+    self.time_qp_solution = float(self.solver.get_stats('time_qp')[0])
+    self.time_linearization = float(self.solver.get_stats('time_lin')[0])
+    self.time_integrator = float(self.solver.get_stats('time_sim')[0])
+
+    # qp_iter = self.solver.get_stats('statistics')[-1][-1] # SQP_RTI specific
+    # print(f"long_mpc timings: tot {self.solve_time:.2e}, qp {self.time_qp_solution:.2e}, lin {self.time_linearization:.2e}, integrator {self.time_integrator:.2e}, qp_iter {qp_iter}")
+    # res = self.solver.get_residuals()
+    # print(f"long_mpc residuals: {res[0]:.2e}, {res[1]:.2e}, {res[2]:.2e}, {res[3]:.2e}")
+    # self.solver.print_statistics()
 
     for i in range(N+1):
       self.x_sol[i] = self.solver.get(i, 'x')
@@ -370,6 +380,7 @@ class LongitudinalMpc:
 
     self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution)
 
+    t = sec_since_boot()
     if self.solution_status != 0:
       if t > self.last_cloudlog_t + 5.0:
         self.last_cloudlog_t = t
@@ -378,5 +389,6 @@ class LongitudinalMpc:
 
 
 if __name__ == "__main__":
-  ocp = gen_long_mpc_solver()
-  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False)
+  ocp = gen_long_ocp()
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
+  # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)
diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 865442f735..fd76142366 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -4,10 +4,10 @@ import numpy as np
 from common.numpy_fast import interp
 
 import cereal.messaging as messaging
+from common.conversions import Conversions as CV
 from common.filter_simple import FirstOrderFilter
 from common.realtime import DT_MDL
 from selfdrive.modeld.constants import T_IDXS
-from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.longcontrol import LongCtrlState
 from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
 from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
@@ -55,6 +55,7 @@ class Planner:
     self.v_desired_trajectory = np.zeros(CONTROL_N)
     self.a_desired_trajectory = np.zeros(CONTROL_N)
     self.j_desired_trajectory = np.zeros(CONTROL_N)
+    self.solverExecutionTime = 0.0
 
   def update(self, sm):
     v_ego = sm['carState'].vEgo
diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py
index 4f87fdf09b..85699866b0 100644
--- a/selfdrive/controls/lib/radar_helpers.py
+++ b/selfdrive/controls/lib/radar_helpers.py
@@ -1,6 +1,5 @@
 from common.numpy_fast import mean
 from common.kalman.simple_kalman import KF1D
-from selfdrive.config import RADAR_TO_CAMERA
 
 
 # the longer lead decels, the more likely it will keep decelerating
@@ -13,6 +12,8 @@ SPEED, ACCEL = 0, 1   # Kalman filter states enum
 # stationary qualification parameters
 v_ego_stationary = 4.   # no stationary object flag below this speed
 
+RADAR_TO_CENTER = 2.7   # (deprecated) RADAR is ~ 2.7m ahead from center of car
+RADAR_TO_CAMERA = 1.52   # RADAR is ~ 1.5m ahead from center of mesh frame
 
 class Track():
   def __init__(self, v_lead, kalman_params):
diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py
index 65f8480c7c..ddba0920b8 100755
--- a/selfdrive/controls/radard.py
+++ b/selfdrive/controls/radard.py
@@ -8,9 +8,8 @@ from cereal import car
 from common.numpy_fast import interp
 from common.params import Params
 from common.realtime import Ratekeeper, Priority, config_realtime_process
-from selfdrive.config import RADAR_TO_CAMERA
 from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid
-from selfdrive.controls.lib.radar_helpers import Cluster, Track
+from selfdrive.controls.lib.radar_helpers import Cluster, Track, RADAR_TO_CAMERA
 from selfdrive.swaglog import cloudlog
 from selfdrive.hardware import TICI
 
diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py
index a0110a4dab..ebe4776739 100644
--- a/selfdrive/controls/tests/test_following_distance.py
+++ b/selfdrive/controls/tests/test_following_distance.py
@@ -21,7 +21,7 @@ def run_following_distance_simulation(v_lead, t_end=100.0):
 
 
 class TestFollowingDistance(unittest.TestCase):
-  def test_following_distanc(self):
+  def test_following_distance(self):
     for speed in np.arange(0, 40, 5):
       print(f'Testing {speed} m/s')
       v_lead = float(speed)
diff --git a/selfdrive/debug/can_print_changes.py b/selfdrive/debug/can_print_changes.py
index 4fa775ac17..b883fbc23f 100755
--- a/selfdrive/debug/can_print_changes.py
+++ b/selfdrive/debug/can_print_changes.py
@@ -1,46 +1,103 @@
 #!/usr/bin/env python3
+import argparse
 import binascii
-import sys
+import time
 from collections import defaultdict
 
 import cereal.messaging as messaging
-from common.realtime import sec_since_boot
+from selfdrive.debug.can_table import can_table
+from tools.lib.logreader import logreader_from_route_or_segment
 
+RED = '\033[91m'
+CLEAR = '\033[0m'
+
+def update(msgs, bus, dat, low_to_high, high_to_low, quiet=False):
+  for x in msgs:
+    if x.which() != 'can':
+      continue
+
+    for y in x.can:
+      if y.src == bus:
+        dat[y.address] = y.dat
+
+        i = int.from_bytes(y.dat, byteorder='big')
+        l_h = low_to_high[y.address]
+        h_l = high_to_low[y.address]
+
+        change = None
+        if (i | l_h) != l_h:
+          low_to_high[y.address] = i | l_h
+          change = "+"
+
+        if (~i | h_l) != h_l:
+          high_to_low[y.address] = ~i | h_l
+          change = "-"
+
+        if change and not quiet:
+          print(f"{time.monotonic():.2f}\t{hex(y.address)} ({y.address})\t{change}{binascii.hexlify(y.dat)}")
 
-def can_printer(bus=0):
-  """Collects messages and prints when a new bit transition is observed.
-  This is very useful to find signals based on user triggered actions, such as blinkers and seatbelt.
-  Leave the script running until no new transitions are seen, then perform the action."""
-  logcan = messaging.sub_sock('can')
 
+def can_printer(bus=0, init_msgs=None, new_msgs=None, table=False):
+  logcan = messaging.sub_sock('can', timeout=10)
+
+  dat = defaultdict(int)
   low_to_high = defaultdict(int)
   high_to_low = defaultdict(int)
 
-  while 1:
-    can_recv = messaging.drain_sock(logcan, wait_for_one=True)
-    for x in can_recv:
-      for y in x.can:
-        if y.src == bus:
-          i = int.from_bytes(y.dat, byteorder='big')
+  if init_msgs is not None:
+    update(init_msgs, bus, dat, low_to_high, high_to_low, quiet=True)
 
-          l_h = low_to_high[y.address]
-          h_l = high_to_low[y.address]
+  low_to_high_init = low_to_high.copy()
+  high_to_low_init = high_to_low.copy()
 
-          change = None
-          if (i | l_h) != l_h:
-            low_to_high[y.address] = i | l_h
-            change = "+"
+  if new_msgs is not None:
+    update(new_msgs, bus, dat, low_to_high, high_to_low)
+  else:
+    # Live mode
+    try:
+      while 1:
+        can_recv = messaging.drain_sock(logcan)
+        update(can_recv, bus, dat, low_to_high, high_to_low)
+        time.sleep(0.02)
+    except KeyboardInterrupt:
+      pass
 
-          if (~i | h_l) != h_l:
-            high_to_low[y.address] = ~i | h_l
-            change = "-"
+  print("\n\n")
+  tables = ""
+  for addr in sorted(dat.keys()):
+    init = low_to_high_init[addr] & high_to_low_init[addr]
+    now = low_to_high[addr] & high_to_low[addr]
+    d = now & ~init
+    if d == 0:
+      continue
+    b = d.to_bytes(len(dat[addr]), byteorder='big')
 
-          if change:
-            print(f"{sec_since_boot():.2f}\t{hex(y.address)} ({y.address})\t{change}{binascii.hexlify(y.dat)}")
+    byts = ''.join([(c if c == '0' else f'{RED}{c}{CLEAR}') for c in str(binascii.hexlify(b))[2:-1]])
+    header = f"{hex(addr).ljust(6)}({str(addr).ljust(4)})"
+    print(header, byts)
+    tables += f"{header}\n"
+    tables += can_table(b) + "\n\n"
 
+  if table:
+    print(tables)
 
 if __name__ == "__main__":
-  if len(sys.argv) > 1:
-    can_printer(int(sys.argv[1]))
-  else:
-    can_printer()
+  desc = """Collects messages and prints when a new bit transition is observed.
+  This is very useful to find signals based on user triggered actions, such as blinkers and seatbelt.
+  Leave the script running until no new transitions are seen, then perform the action."""
+  parser = argparse.ArgumentParser(description=desc,
+                                   formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+  parser.add_argument("--bus", type=int, help="CAN bus to print out", default=0)
+  parser.add_argument("--table", action="store_true", help="Print a cabana-like table")
+  parser.add_argument("init", type=str, nargs='?', help="Route or segment to initialize with")
+  parser.add_argument("comp", type=str, nargs='?', help="Route or segment to compare against init")
+
+  args = parser.parse_args()
+
+  init_lr, new_lr = None, None
+  if args.init:
+    init_lr = logreader_from_route_or_segment(args.init)
+  if args.comp:
+    new_lr = logreader_from_route_or_segment(args.comp)
+
+  can_printer(args.bus, init_msgs=init_lr, new_msgs=new_lr, table=args.table)
diff --git a/selfdrive/debug/can_table.py b/selfdrive/debug/can_table.py
index 1569849053..e8cd084a32 100755
--- a/selfdrive/debug/can_table.py
+++ b/selfdrive/debug/can_table.py
@@ -5,6 +5,19 @@ import pandas as pd  # pylint: disable=import-error
 import cereal.messaging as messaging
 
 
+def can_table(dat):
+  rows = []
+  for b in dat:
+    r = list(bin(b).lstrip('0b').zfill(8))
+    r += [hex(b)]
+    rows.append(r)
+
+  df = pd.DataFrame(data=rows)
+  df.columns = [str(n) for n in range(7, -1, -1)] + [' ']
+  table = df.to_markdown(tablefmt='grid')
+  return table
+
+
 if __name__ == "__main__":
   parser = argparse.ArgumentParser(description="Cabana-like table of bits for your terminal",
                                    formatter_class=argparse.ArgumentDefaultsHelpFormatter)
@@ -28,12 +41,5 @@ if __name__ == "__main__":
     if latest is None:
       continue
 
-    rows = []
-    for b in latest.dat:
-      r = list(bin(b).lstrip('0b').zfill(8))
-      r += [hex(b)]
-      rows.append(r)
-
-    df = pd.DataFrame(data=rows)
-    table = df.to_markdown(tablefmt='grid')
+    table = can_table(latest.dat)
     print(f"\n\n{hex(addr)} ({addr}) on bus {args.bus}\n{table}")
diff --git a/selfdrive/hardware/base.py b/selfdrive/hardware/base.py
index b551138ce3..20258b85d0 100644
--- a/selfdrive/hardware/base.py
+++ b/selfdrive/hardware/base.py
@@ -2,7 +2,11 @@ from abc import abstractmethod, ABC
 from collections import namedtuple
 from typing import Dict
 
+from cereal import log
+
 ThermalConfig = namedtuple('ThermalConfig', ['cpu', 'gpu', 'mem', 'bat', 'ambient', 'pmic'])
+NetworkType = log.DeviceState.NetworkType
+
 
 class HardwareBase(ABC):
   @staticmethod
@@ -67,6 +71,9 @@ class HardwareBase(ABC):
   def get_network_strength(self, network_type):
     pass
 
+  def get_network_metered(self, network_type) -> bool:
+    return network_type not in (NetworkType.none, NetworkType.wifi, NetworkType.ethernet)
+
   @staticmethod
   def set_bandwidth_limit(upload_speed_kbps: int, download_speed_kbps: int) -> None:
     pass
diff --git a/selfdrive/hardware/eon/hardware.py b/selfdrive/hardware/eon/hardware.py
index 4ab9f81fcf..dcc29d5919 100644
--- a/selfdrive/hardware/eon/hardware.py
+++ b/selfdrive/hardware/eon/hardware.py
@@ -380,7 +380,9 @@ class Android(HardwareBase):
     os.system('LD_LIBRARY_PATH="" svc power shutdown')
 
   def get_thermal_config(self):
-    return ThermalConfig(cpu=((5, 7, 10, 12), 10), gpu=((16,), 10), mem=(2, 10), bat=(29, 1000), ambient=(25, 1), pmic=((22,), 1000))
+    # the thermal sensors on the 820 don't have meaningful names
+    return ThermalConfig(cpu=((5, 7, 10, 12), 10), gpu=((16,), 10), mem=(2, 10),
+                         bat=("battery", 1000), ambient=("pa_therm0", 1), pmic=(("pm8994_tz",), 1000))
 
   def set_screen_brightness(self, percentage):
     with open("/sys/class/leds/lcd-backlight/brightness", "w") as f:
diff --git a/selfdrive/hardware/tici/hardware.py b/selfdrive/hardware/tici/hardware.py
index 41ae316d3c..7a0d5204c1 100644
--- a/selfdrive/hardware/tici/hardware.py
+++ b/selfdrive/hardware/tici/hardware.py
@@ -13,6 +13,7 @@ from selfdrive.hardware.tici.amplifier import Amplifier
 
 NM = 'org.freedesktop.NetworkManager'
 NM_CON_ACT = NM + '.Connection.Active'
+NM_DEV = NM + '.Device'
 NM_DEV_WL = NM + '.Device.Wireless'
 NM_AP = NM + '.AccessPoint'
 DBUS_PROPS = 'org.freedesktop.DBus.Properties'
@@ -37,6 +38,13 @@ class MM_MODEM_STATE(IntEnum):
        CONNECTING    = 10
        CONNECTED     = 11
 
+class NMMetered(IntEnum):
+  NM_METERED_UNKNOWN = 0
+  NM_METERED_YES = 1
+  NM_METERED_NO = 2
+  NM_METERED_GUESS_YES = 3
+  NM_METERED_GUESS_NO = 4
+
 TIMEOUT = 0.1
 
 NetworkType = log.DeviceState.NetworkType
@@ -91,11 +99,10 @@ class Tici(HardwareBase):
       primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
       primary_connection = self.bus.get_object(NM, primary_connection)
       primary_type = primary_connection.Get(NM_CON_ACT, 'Type', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
-      primary_id = primary_connection.Get(NM_CON_ACT, 'Id', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
 
       if primary_type == '802-3-ethernet':
         return NetworkType.ethernet
-      elif primary_type == '802-11-wireless' and primary_id != 'Hotspot':
+      elif primary_type == '802-11-wireless':
         return NetworkType.wifi
       else:
         active_connections = self.nm.Get(NM, 'ActiveConnections', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
@@ -218,6 +225,27 @@ class Tici(HardwareBase):
 
     return network_strength
 
+  def get_network_metered(self, network_type) -> bool:
+    try:
+      primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+      primary_connection = self.bus.get_object(NM, primary_connection)
+      primary_devices = primary_connection.Get(NM_CON_ACT, 'Devices', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+
+      for dev in primary_devices:
+        dev_obj = self.bus.get_object(NM, str(dev))
+        metered_prop = dev_obj.Get(NM_DEV, 'Metered', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+
+        if network_type == NetworkType.wifi:
+          if metered_prop in [NMMetered.NM_METERED_YES, NMMetered.NM_METERED_GUESS_YES]:
+            return True
+        elif network_type in [NetworkType.cell2G, NetworkType.cell3G, NetworkType.cell4G, NetworkType.cell5G]:
+          if metered_prop == NMMetered.NM_METERED_NO:
+            return False
+    except Exception:
+      pass
+
+    return super().get_network_metered(network_type)
+
   @staticmethod
   def set_bandwidth_limit(upload_speed_kbps: int, download_speed_kbps: int) -> None:
     upload_speed_kbps = int(upload_speed_kbps)  # Ensure integer value
@@ -334,7 +362,13 @@ class Tici(HardwareBase):
     os.system("sudo poweroff")
 
   def get_thermal_config(self):
-    return ThermalConfig(cpu=((1, 2, 3, 4, 5, 6, 7, 8), 1000), gpu=((48,49), 1000), mem=(15, 1000), bat=(None, 1), ambient=(65, 1000), pmic=((35, 36), 1000))
+    return ThermalConfig(cpu=(["cpu%d-silver-usr" % i for i in range(4)] +
+                              ["cpu%d-gold-usr" % i for i in range(4)], 1000),
+                         gpu=(("gpu0-usr", "gpu1-usr"), 1000),
+                         mem=("ddr-usr", 1000),
+                         bat=(None, 1),
+                         ambient=("xo-therm-adc", 1000),
+                         pmic=(("pm8998_tz", "pm8005_tz"), 1000))
 
   def set_screen_brightness(self, percentage):
     try:
diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py
index ae314e38c4..c8c3912528 100755
--- a/selfdrive/locationd/calibrationd.py
+++ b/selfdrive/locationd/calibrationd.py
@@ -13,12 +13,12 @@ from typing import NoReturn
 
 from cereal import log
 import cereal.messaging as messaging
+from common.conversions import Conversions as CV
 from common.params import Params, put_nonblocking
 from common.realtime import set_realtime_priority
 from common.transformations.model import model_height
 from common.transformations.camera import get_view_frame_from_road_frame
 from common.transformations.orientation import rot_from_euler, euler_from_rot
-from selfdrive.config import Conversions as CV
 from selfdrive.hardware import TICI
 from selfdrive.swaglog import cloudlog
 
diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc
index b723878e9d..5cec492d67 100755
--- a/selfdrive/locationd/locationd.cc
+++ b/selfdrive/locationd/locationd.cc
@@ -262,7 +262,6 @@ void Localizer::input_fake_gps_observations(double current_time) {
 
 void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log) {
   // ignore the message if the fix is invalid
-
   bool gps_invalid_flag = (log.getFlags() % 2 == 0);
   bool gps_unreasonable = (Vector2d(log.getAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
   bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
@@ -462,9 +461,9 @@ kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_build
 {
   cereal::Event::Builder evt = msg_builder.initEvent();
   evt.setLogMonoTime(logMonoTime);
+  evt.setValid(inputsOK);
   cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
   this->build_live_location(liveLoc);
-  liveLoc.setInputsOK(inputsOK);
   liveLoc.setSensorsOK(sensorsOK);
   liveLoc.setGpsOK(gpsOK);
   return msg_builder.toBytes();
@@ -497,14 +496,19 @@ int Localizer::locationd_thread() {
   PubMaster pm({ "liveLocationKalman" });
   SubMaster sm(service_list, nullptr, { "gpsLocationExternal" });
 
+  uint64_t cnt = 0;
+
   while (!do_exit) {
     sm.update();
-    for (const char* service : service_list) {
-      if (sm.updated(service) && sm.valid(service)) {
-        const cereal::Event::Reader log = sm[service];
-        this->handle_msg(log);
+    if (sm.allAliveAndValid()){
+      for (const char* service : service_list) {
+        if (sm.updated(service)){
+          const cereal::Event::Reader log = sm[service];
+          this->handle_msg(log);
+        }
       }
     }
+    
 
     if (sm.updated("cameraOdometry")) {
       uint64_t logMonoTime = sm["cameraOdometry"].getLogMonoTime();
@@ -516,7 +520,7 @@ int Localizer::locationd_thread() {
       kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK);
       pm.send("liveLocationKalman", bytes.begin(), bytes.size());
 
-      if (sm.frame % 1200 == 0 && gpsOK) {  // once a minute
+      if (cnt % 1200 == 0 && gpsOK) {  // once a minute
         VectorXd posGeo = this->get_position_geodetic();
         std::string lastGPSPosJSON = util::string_format(
           "{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
@@ -525,6 +529,7 @@ int Localizer::locationd_thread() {
           Params().put("LastGPSPosition", gpsjson);
         }, lastGPSPosJSON).detach();
       }
+      cnt++;
     }
   }
   return 0;
diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py
index 0efb4d04be..44ffccdf1a 100755
--- a/selfdrive/locationd/paramsd.py
+++ b/selfdrive/locationd/paramsd.py
@@ -62,7 +62,7 @@ class ParamsLearner:
       yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1  # rad/s
 
       if self.active:
-        if msg.inputsOK and msg.posenetOK:
+        if msg.posenetOK:
 
           if yaw_rate_valid:
             self.kf.predict_and_observe(t,
@@ -160,10 +160,11 @@ def main(sm=None, pm=None):
 
   while True:
     sm.update()
-    for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]):
-      if sm.updated[which]:
-        t = sm.logMonoTime[which] * 1e-9
-        learner.handle_log(t, which, sm[which])
+    if sm.all_alive_and_valid():
+      for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]):
+        if sm.updated[which]:
+          t = sm.logMonoTime[which] * 1e-9
+          learner.handle_log(t, which, sm[which])
 
     if sm.updated['liveLocationKalman']:
       x = learner.kf.x
@@ -198,6 +199,8 @@ def main(sm=None, pm=None):
       liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET])
       liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST])
 
+      msg.valid = sm.all_alive_and_valid()
+
       if sm.frame % 1200 == 0:  # once a minute
         params = {
           'carFingerprint': CP.carFingerprint,
diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py
index 76f82efc10..515bd59431 100755
--- a/selfdrive/locationd/test/test_locationd.py
+++ b/selfdrive/locationd/test/test_locationd.py
@@ -4,10 +4,12 @@ import json
 import random
 import unittest
 import time
+import capnp
 from cffi import FFI
 
 from cereal import log
 import cereal.messaging as messaging
+from cereal.services import service_list
 from common.params import Params
 
 from selfdrive.manager.process_config import managed_processes
@@ -103,13 +105,15 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t
     ret = self.localizer_get_msg()
     self.assertFalse(ret.liveLocationKalman.posenetOK)
 
+
 class TestLocationdProc(unittest.TestCase):
   MAX_WAITS = 1000
+  LLD_MSGS = {'gpsLocationExternal', 'cameraOdometry', 'carState', 'sensorEvents', 'liveCalibration'}
 
   def setUp(self):
     random.seed(123489234)
 
-    self.pm = messaging.PubMaster({'gpsLocationExternal', 'cameraOdometry'})
+    self.pm = messaging.PubMaster(self.LLD_MSGS)
 
     managed_processes['locationd'].prepare()
     managed_processes['locationd'].start()
@@ -127,43 +131,53 @@ class TestLocationdProc(unittest.TestCase):
       waits_left -= 1
     time.sleep(0.0001)
 
-  def test_params_gps(self):
-    # first reset params
-    Params().put('LastGPSPosition', json.dumps({"latitude": 0.0, "longitude": 0.0, "altitude": 0.0}))
-
-    lat = 30 + (random.random() * 10.0)
-    lon = -70 + (random.random() * 10.0)
-    alt = 5 + (random.random() * 10.0)
+  def get_fake_msg(self, name, t):
+    try:
+      msg = messaging.new_message(name)
+    except capnp.lib.capnp.KjException:
+      msg = messaging.new_message(name, 0)
 
-    for _ in range(1000):  # because of kalman filter, send often
-      msg = messaging.new_message('gpsLocationExternal')
-      msg.logMonoTime = 0
+    if name == "gpsLocationExternal":
       msg.gpsLocationExternal.flags = 1
       msg.gpsLocationExternal.verticalAccuracy = 1.0
       msg.gpsLocationExternal.speedAccuracy = 1.0
       msg.gpsLocationExternal.bearingAccuracyDeg = 1.0
       msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
-      msg.gpsLocationExternal.latitude = lat
-      msg.gpsLocationExternal.longitude = lon
-      msg.gpsLocationExternal.altitude = alt
-      self.send_msg(msg)
-
-    for _ in range(250):  # params is only written so often
-      msg = messaging.new_message('cameraOdometry')
-      msg.logMonoTime = 0
+      msg.gpsLocationExternal.latitude = self.lat
+      msg.gpsLocationExternal.longitude = self.lon
+      msg.gpsLocationExternal.altitude = self.alt
+    elif name == 'cameraOdometry':
       msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
       msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
       msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
       msg.cameraOdometry.transStd = [0.0, 0.0, 0.0]
-      self.send_msg(msg)
+    msg.logMonoTime = t
+    return msg
 
+  def test_params_gps(self):
+    # first reset params
+    Params().delete('LastGPSPosition')
+
+    self.lat = 30 + (random.random() * 10.0)
+    self.lon = -70 + (random.random() * 10.0)
+    self.alt = 5 + (random.random() * 10.0)
+    self.fake_duration = 90  # secs
+    # get fake messages at the correct frequency, listed in services.py
+    fake_msgs = []
+    for sec in range(self.fake_duration):
+      for name in self.LLD_MSGS:
+        for j in range(int(service_list[name].frequency)):
+          fake_msgs.append(self.get_fake_msg(name, int((sec + j / service_list[name].frequency) * 1e9)))
+
+    for fake_msg in sorted(fake_msgs, key=lambda x: x.logMonoTime):
+      self.send_msg(fake_msg)
     time.sleep(1)  # wait for async params write
 
     lastGPS = json.loads(Params().get('LastGPSPosition'))
 
-    self.assertAlmostEqual(lastGPS['latitude'], lat, places=3)
-    self.assertAlmostEqual(lastGPS['longitude'], lon, places=3)
-    self.assertAlmostEqual(lastGPS['altitude'], alt, places=3)
+    self.assertAlmostEqual(lastGPS['latitude'], self.lat, places=3)
+    self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=3)
+    self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=3)
 
 
 if __name__ == "__main__":
diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py
index afdc5a6ed1..4dd982a02b 100644
--- a/selfdrive/loggerd/uploader.py
+++ b/selfdrive/loggerd/uploader.py
@@ -165,14 +165,14 @@ class Uploader():
 
     return self.last_resp
 
-  def upload(self, key, fn, network_type):
+  def upload(self, key, fn, network_type, metered):
     try:
       sz = os.path.getsize(fn)
     except OSError:
       cloudlog.exception("upload: getsize failed")
       return False
 
-    cloudlog.event("upload_start", key=key, fn=fn, sz=sz, network_type=network_type)
+    cloudlog.event("upload_start", key=key, fn=fn, sz=sz, network_type=network_type, metered=metered)
 
     if sz == 0:
       try:
@@ -195,10 +195,10 @@ class Uploader():
         self.last_time = time.monotonic() - start_time
         self.last_speed = (sz / 1e6) / self.last_time
         success = True
-        cloudlog.event("upload_success" if stat.status_code != 412 else "upload_ignored", key=key, fn=fn, sz=sz, network_type=network_type)
+        cloudlog.event("upload_success" if stat.status_code != 412 else "upload_ignored", key=key, fn=fn, sz=sz, network_type=network_type, metered=metered)
       else:
         success = False
-        cloudlog.event("upload_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz, network_type=network_type)
+        cloudlog.event("upload_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz, network_type=network_type, metered=metered)
 
     return success
 
@@ -247,7 +247,7 @@ def uploader_fn(exit_event):
 
     key, fn = d
 
-    success = uploader.upload(key, fn, sm['deviceState'].networkType.raw)
+    success = uploader.upload(key, fn, sm['deviceState'].networkType.raw, sm['deviceState'].networkMetered)
     if success:
       backoff = 0.1
     elif allow_sleep:
@@ -257,6 +257,7 @@ def uploader_fn(exit_event):
 
     pm.send("uploaderState", uploader.get_msg())
 
+
 def main():
   uploader_fn(threading.Event())
 
diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py
index c017e48fab..bc88154c2c 100644
--- a/selfdrive/manager/process_config.py
+++ b/selfdrive/manager/process_config.py
@@ -27,7 +27,7 @@ procs = [
   PythonProcess("deleter", "selfdrive.loggerd.deleter", persistent=True),
   PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), driverview=True),
   PythonProcess("logmessaged", "selfdrive.logmessaged", persistent=True),
-  PythonProcess("pandad", "selfdrive.pandad", persistent=True),
+  PythonProcess("pandad", "selfdrive.boardd.pandad", persistent=True),
   PythonProcess("paramsd", "selfdrive.locationd.paramsd"),
   PythonProcess("plannerd", "selfdrive.controls.plannerd"),
   PythonProcess("radard", "selfdrive.controls.radard"),
diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript
index 9066a653fa..20d3fb8acc 100644
--- a/selfdrive/modeld/SConscript
+++ b/selfdrive/modeld/SConscript
@@ -31,9 +31,6 @@ thneed_src = [
 
 use_thneed = not GetOption('no_thneed')
 
-use_extra = True  # arch == "larch64"
-lenv['CXXFLAGS'].append('-DUSE_EXTRA=true' if use_extra else '-DUSE_EXTRA=false')
-
 if arch == "aarch64" or arch == "larch64":
   libs += ['gsl', 'CB']
   libs += ['gnustl_shared'] if arch == "aarch64" else ['pthread', 'dl']
@@ -68,16 +65,24 @@ common_model = lenv.Object(common_src)
 
 # build thneed model
 if use_thneed and arch in ("aarch64", "larch64"):
-  fn = "../../models/big_supercombo" if use_extra else "../../models/supercombo"
+  fn = File("#models/supercombo").abspath
   compiler = lenv.Program('thneed/compile', ["thneed/compile.cc"]+common_model, LIBS=libs)
-  cmd = f"cd {Dir('.').abspath} && {compiler[0].abspath} {fn}.dlc {fn}.thneed --binary"
+  cmd = f"cd {Dir('.').abspath} && {compiler[0].abspath} {fn}.dlc {fn}_badweights.thneed --binary"
 
   lib_paths = ':'.join(Dir(p).abspath for p in lenv["LIBPATH"])
   kernel_path = os.path.join(Dir('.').abspath, "thneed", "kernels")
   cenv = Environment(ENV={'LD_LIBRARY_PATH': f"{lib_paths}:{lenv['ENV']['LD_LIBRARY_PATH']}", 'KERNEL_PATH': kernel_path})
 
   kernels = [os.path.join(kernel_path, x) for x in os.listdir(kernel_path) if x.endswith(".cl")]
-  cenv.Command(fn + ".thneed", [fn + ".dlc", kernels, compiler], cmd)
+  cenv.Command(fn + "_badweights.thneed", [fn + ".dlc", kernels, compiler], cmd)
+
+  from selfdrive.modeld.thneed.weights_fixup import weights_fixup
+  def weights_fixup_action(target, source, env):
+    weights_fixup(target[0].abspath, source[0].abspath, source[1].abspath)
+
+  env = Environment(BUILDERS = {'WeightFixup' : Builder(action = weights_fixup_action)})
+  env.WeightFixup(target=fn + ".thneed", source=[fn+"_badweights.thneed", fn+".dlc"])
+
 
 lenv.Program('_dmonitoringmodeld', [
     "dmonitoringmodeld.cc",
diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc
index 697e31a7b7..2714370106 100644
--- a/selfdrive/modeld/modeld.cc
+++ b/selfdrive/modeld/modeld.cc
@@ -51,7 +51,12 @@ mat3 update_calibration(Eigen::Matrix<float, 3, 4> &extrinsics, bool wide_camera
   return matmul3(yuv_transform, transform);
 }
 
-void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcClient &vipc_client_extra, bool main_wide_camera, bool use_extra, bool use_extra_client) {
+static uint64_t get_ts(const VisionIpcBufExtra &extra) {
+  return Hardware::TICI() ? extra.timestamp_sof : extra.timestamp_eof;
+}
+
+
+void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcClient &vipc_client_extra, bool main_wide_camera, bool use_extra_client) {
   // messaging
   PubMaster pm({"modelV2", "cameraOdometry"});
   SubMaster sm({"lateralPlan", "roadCameraState", "liveCalibration"});
@@ -74,18 +79,9 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
   VisionIpcBufExtra meta_extra = {0};
 
   while (!do_exit) {
-    // TODO: change sync logic to use timestamp start of frame in case camerad skips a frame
-    // log frame id in model packet
-
     // Keep receiving frames until we are at least 1 frame ahead of previous extra frame
-    while (meta_main.frame_id <= meta_extra.frame_id) {
+    while (get_ts(meta_main) < get_ts(meta_extra) + 25000000ULL) {
       buf_main = vipc_client_main.recv(&meta_main);
-      if (meta_main.frame_id <= meta_extra.frame_id) {
-        LOGE("main camera behind! main: %d (%.5f), extra: %d (%.5f)",
-          meta_main.frame_id, double(meta_main.timestamp_sof) / 1e9,
-          meta_extra.frame_id, double(meta_extra.timestamp_sof) / 1e9);
-      }
-
       if (buf_main == nullptr)  break;
     }
 
@@ -98,20 +94,14 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
       // Keep receiving extra frames until frame id matches main camera
       do {
         buf_extra = vipc_client_extra.recv(&meta_extra);
-
-        if (meta_main.frame_id > meta_extra.frame_id) {
-          LOGE("extra camera behind! main: %d (%.5f), extra: %d (%.5f)",
-            meta_main.frame_id, double(meta_main.timestamp_sof) / 1e9,
-            meta_extra.frame_id, double(meta_extra.timestamp_sof) / 1e9);
-        }
-      } while (buf_extra != nullptr && meta_main.frame_id > meta_extra.frame_id);
+      } while (buf_extra != nullptr && get_ts(meta_main) > get_ts(meta_extra) + 25000000ULL);
 
       if (buf_extra == nullptr) {
         LOGE("vipc_client_extra no frame");
         continue;
       }
 
-      if (meta_main.frame_id != meta_extra.frame_id || std::abs((int64_t)meta_main.timestamp_sof - (int64_t)meta_extra.timestamp_sof) > 10000000ULL) {
+      if (std::abs((int64_t)meta_main.timestamp_sof - (int64_t)meta_extra.timestamp_sof) > 10000000ULL) {
         LOGE("frames out of sync! main: %d (%.5f), extra: %d (%.5f)",
           meta_main.frame_id, double(meta_main.timestamp_sof) / 1e9,
           meta_extra.frame_id, double(meta_extra.timestamp_sof) / 1e9);
@@ -134,9 +124,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
       }
 
       model_transform_main = update_calibration(extrinsic_matrix_eigen, main_wide_camera, false);
-      if (use_extra) {
-        model_transform_extra = update_calibration(extrinsic_matrix_eigen, Hardware::TICI(), true);
-      }
+      model_transform_extra = update_calibration(extrinsic_matrix_eigen, Hardware::TICI(), true);
       live_calib_seen = true;
     }
 
@@ -161,7 +149,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
 
     float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
 
-    model_publish(pm, meta_main.frame_id, frame_id, frame_drop_ratio, *model_output, meta_main.timestamp_eof, model_execution_time,
+    model_publish(pm, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, *model_output, meta_main.timestamp_eof, model_execution_time,
                   kj::ArrayPtr<const float>(model.output.data(), model.output.size()), live_calib_seen);
     posenet_publish(pm, meta_main.frame_id, vipc_dropped_frames, *model_output, meta_main.timestamp_eof, live_calib_seen);
 
@@ -181,8 +169,7 @@ int main(int argc, char **argv) {
   }
 
   bool main_wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false;
-  bool use_extra = USE_EXTRA;
-  bool use_extra_client = Hardware::TICI() && use_extra && !main_wide_camera;
+  bool use_extra_client = Hardware::TICI() && !main_wide_camera;
 
   // cl init
   cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
@@ -190,7 +177,7 @@ int main(int argc, char **argv) {
 
   // init the models
   ModelState model;
-  model_init(&model, device_id, context, use_extra);
+  model_init(&model, device_id, context);
   LOGW("models loaded, modeld starting");
 
   VisionIpcClient vipc_client_main = VisionIpcClient("camerad", main_wide_camera ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD, true, device_id, context);
@@ -215,7 +202,7 @@ int main(int argc, char **argv) {
       LOGW("connected extra cam with buffer size: %d (%d x %d)", wb->len, wb->width, wb->height);
     }
 
-    run_model(model, vipc_client_main, vipc_client_extra, main_wide_camera, use_extra, use_extra_client);
+    run_model(model, vipc_client_main, vipc_client_extra, main_wide_camera, use_extra_client);
   }
 
   model_free(&model);
diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc
index e42f2c0f46..18ab82223b 100644
--- a/selfdrive/modeld/models/driving.cc
+++ b/selfdrive/modeld/models/driving.cc
@@ -26,20 +26,18 @@ constexpr const kj::ArrayPtr<const T> to_kj_array_ptr(const std::array<T, size>
   return kj::ArrayPtr(arr.data(), arr.size());
 }
 
-void model_init(ModelState* s, cl_device_id device_id, cl_context context, bool use_extra) {
+void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
   s->frame = new ModelFrame(device_id, context);
   s->wide_frame = new ModelFrame(device_id, context);
 
 #ifdef USE_THNEED
-  s->m = std::make_unique<ThneedModel>(use_extra ? "../../models/big_supercombo.thneed" : "../../models/supercombo.thneed",
-   &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME, use_extra);
+  s->m = std::make_unique<ThneedModel>("../../models/supercombo.thneed",
 #elif USE_ONNX_MODEL
-  s->m = std::make_unique<ONNXModel>(use_extra ? "../../models/big_supercombo.onnx" : "../../models/supercombo.onnx",
-   &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME, use_extra);
+  s->m = std::make_unique<ONNXModel>("../../models/supercombo.onnx",
 #else
-  s->m = std::make_unique<SNPEModel>(use_extra ? "../../models/big_supercombo.dlc" : "../../models/supercombo.dlc",
-   &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME, use_extra);
+  s->m = std::make_unique<SNPEModel>("../../models/supercombo.dlc",
 #endif
+   &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME, true);
 
 #ifdef TEMPORAL
   s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE);
@@ -316,13 +314,14 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelOutput &net_out
   }
 }
 
-void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop,
+void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
                    const ModelOutput &net_outputs, uint64_t timestamp_eof,
                    float model_execution_time, kj::ArrayPtr<const float> raw_pred, const bool valid) {
   const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
   MessageBuilder msg;
   auto framed = msg.initEvent(valid).initModelV2();
   framed.setFrameId(vipc_frame_id);
+  framed.setFrameIdExtra(vipc_frame_id_extra);
   framed.setFrameAge(frame_age);
   framed.setFrameDropPerc(frame_drop * 100);
   framed.setTimestampEof(timestamp_eof);
diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h
index eead06575f..f83cb86939 100644
--- a/selfdrive/modeld/models/driving.h
+++ b/selfdrive/modeld/models/driving.h
@@ -266,11 +266,11 @@ struct ModelState {
 #endif
 };
 
-void model_init(ModelState* s, cl_device_id device_id, cl_context context, bool use_extra);
+void model_init(ModelState* s, cl_device_id device_id, cl_context context);
 ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide,
                               const mat3 &transform, const mat3 &transform_wide, float *desire_in);
 void model_free(ModelState* s);
-void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop,
+void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
                    const ModelOutput &net_outputs, uint64_t timestamp_eof,
                    float model_execution_time, kj::ArrayPtr<const float> raw_pred, const bool valid);
 void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
diff --git a/selfdrive/modeld/runners/thneedmodel.cc b/selfdrive/modeld/runners/thneedmodel.cc
index a85b2ac022..edc091bda9 100644
--- a/selfdrive/modeld/runners/thneedmodel.cc
+++ b/selfdrive/modeld/runners/thneedmodel.cc
@@ -47,7 +47,7 @@ void* ThneedModel::getExtraBuf() {
 
 void ThneedModel::execute() {
   if (!recorded) {
-    thneed->record = THNEED_RECORD;
+    thneed->record = true;
     if (use_extra) {
       float *inputs[5] = {recurrent, trafficConvention, desire, extra, input};
       thneed->copy_inputs(inputs);
diff --git a/selfdrive/modeld/thneed/__init__.py b/selfdrive/modeld/thneed/__init__.py
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/selfdrive/modeld/thneed/compile.cc b/selfdrive/modeld/thneed/compile.cc
index 8e031b9ab6..c22156d2c9 100644
--- a/selfdrive/modeld/thneed/compile.cc
+++ b/selfdrive/modeld/thneed/compile.cc
@@ -13,7 +13,7 @@ int main(int argc, char* argv[]) {
   #define OUTPUT_SIZE 0x10000
 
   float *output = (float*)calloc(OUTPUT_SIZE, sizeof(float));
-  SNPEModel mdl(argv[1], output, 0, USE_GPU_RUNTIME, USE_EXTRA);
+  SNPEModel mdl(argv[1], output, 0, USE_GPU_RUNTIME, true);
 
   float state[TEMPORAL_SIZE] = {0};
   float desire[DESIRE_LEN] = {0};
@@ -25,9 +25,7 @@ int main(int argc, char* argv[]) {
   mdl.addDesire(desire, DESIRE_LEN);
   mdl.addTrafficConvention(traffic_convention, TRAFFIC_CONVENTION_LEN);
   mdl.addImage(input, 0);
-  if (USE_EXTRA) {
-    mdl.addExtra(extra, 0);
-  }
+  mdl.addExtra(extra, 0);
 
   // first run
   printf("************** execute 1 **************\n");
@@ -37,6 +35,14 @@ int main(int argc, char* argv[]) {
   // save model
   bool save_binaries = (argc > 3) && (strcmp(argv[3], "--binary") == 0);
   mdl.thneed->save(argv[2], save_binaries);
+
+  // test model
+  auto thneed = new Thneed(true);
+  thneed->record = false;
+  thneed->load(argv[2]);
+  thneed->clexec();
+  thneed->find_inputs_outputs();
+
   return 0;
 }
 
diff --git a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads.cl b/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads.cl
index bc8add79aa..fcea88ce90 100644
--- a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads.cl
+++ b/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads.cl
@@ -1,4 +1,3 @@
 #define SUPPORT_DILATION
 
 __kernel void convolution_horizontal_reduced_reads(
-#include "convolution_.cl"
diff --git a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_1x1.cl b/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_1x1.cl
index 75a090ca22..0d15d80581 100644
--- a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_1x1.cl
+++ b/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_1x1.cl
@@ -2,4 +2,3 @@
 #define SUPPORT_ACCUMULATION
 
 __kernel void convolution_horizontal_reduced_reads_1x1(
-#include "convolution_.cl"
diff --git a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_5_outputs.cl b/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_5_outputs.cl
index 980e7d1f67..69421fc2a9 100644
--- a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_5_outputs.cl
+++ b/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_5_outputs.cl
@@ -1,4 +1,3 @@
 #define NUM_OUTPUTS 5
 
 __kernel void convolution_horizontal_reduced_reads_5_outputs(
-#include "convolution_.cl"
diff --git a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_depthwise.cl b/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_depthwise.cl
index 80be0da924..50e39941d4 100644
--- a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_depthwise.cl
+++ b/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_depthwise.cl
@@ -2,4 +2,3 @@
 #define SUPPORT_DILATION
 
 __kernel void convolution_horizontal_reduced_reads_depthwise(
-#include "convolution_.cl"
diff --git a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_depthwise_stride_1.cl b/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_depthwise_stride_1.cl
index 3d651c229b..b347cb6c71 100644
--- a/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_depthwise_stride_1.cl
+++ b/selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_depthwise_stride_1.cl
@@ -1,4 +1,3 @@
 #define DEPTHWISE
 
 __kernel void convolution_horizontal_reduced_reads_depthwise_stride_1(
-#include "convolution_.cl"
diff --git a/selfdrive/modeld/thneed/lib.py b/selfdrive/modeld/thneed/lib.py
new file mode 100644
index 0000000000..dccdfb10ac
--- /dev/null
+++ b/selfdrive/modeld/thneed/lib.py
@@ -0,0 +1,31 @@
+import struct, json
+
+def load_thneed(fn):
+  with open(fn, "rb") as f:
+    json_len = struct.unpack("I", f.read(4))[0]
+    jdat = json.loads(f.read(json_len).decode('latin_1'))
+    weights = f.read()
+  ptr = 0
+  for o in jdat['objects']:
+    if o['needs_load']:
+      nptr = ptr + o['size']
+      o['data'] = weights[ptr:nptr]
+      ptr = nptr
+  for o in jdat['binaries']:
+    nptr = ptr + o['length']
+    o['data'] = weights[ptr:nptr]
+    ptr = nptr
+  return jdat
+
+def save_thneed(jdat, fn):
+  new_weights = []
+  for o in jdat['objects'] + jdat['binaries']:
+    if 'data' in o:
+      new_weights.append(o['data'])
+      del o['data']
+  new_weights = b''.join(new_weights)
+  with open(fn, "wb") as f:
+    j = json.dumps(jdat, ensure_ascii=False).encode('latin_1')
+    f.write(struct.pack("I", len(j)))
+    f.write(j)
+    f.write(new_weights)
diff --git a/selfdrive/modeld/thneed/optimizer.cc b/selfdrive/modeld/thneed/optimizer.cc
index 3c7c41873d..b516b5fa50 100644
--- a/selfdrive/modeld/thneed/optimizer.cc
+++ b/selfdrive/modeld/thneed/optimizer.cc
@@ -4,6 +4,9 @@
 #include <assert.h>
 #include "thneed.h"
 
+#include "selfdrive/common/util.h"
+#include "selfdrive/common/clutil.h"
+
 extern map<cl_program, string> g_program_source;
 
 static int is_same_size_image(cl_mem a, cl_mem b) {
@@ -63,6 +66,14 @@ static cl_mem make_image_like(cl_context context, cl_mem val) {
 int Thneed::optimize() {
   const char *kernel_path = getenv("KERNEL_PATH");
   if (!kernel_path) { kernel_path = "/data/openpilot/selfdrive/modeld/thneed/kernels"; printf("no KERNEL_PATH set, defaulting to %s\n", kernel_path); }
+
+  string convolution_;
+  {
+    char fn[0x100];
+    snprintf(fn, sizeof(fn), "%s/%s.cl", kernel_path, "convolution_");
+    convolution_ = util::read_file(fn);
+  }
+
   // load custom kernels
   map<string, cl_program> g_programs;
   for (auto &k : kq) {
@@ -70,33 +81,17 @@ int Thneed::optimize() {
     if (g_programs.find(k->name) == g_programs.end()) {
       char fn[0x100];
       snprintf(fn, sizeof(fn), "%s/%s.cl", kernel_path, k->name.c_str());
-      FILE *g = fopen(fn, "rb");
-      if (g != NULL) {
-        char *src[0x10000];
-        const char *srcs[1]; srcs[0] = (const char *)src;
-        memset(src, 0, sizeof(src));
-        size_t length = fread(src, 1, sizeof(src), g);
-        fclose(g);
-
-        printf("building kernel %s\n", k->name.c_str());
-        k->program = clCreateProgramWithSource(context, 1, srcs, &length, NULL);
-        char options[0x100];
-        snprintf(options, sizeof(options)-1, "-I %s", kernel_path);
-        int err = clBuildProgram(k->program, 1, &device_id, options, NULL, NULL);
-
-        if (err != 0) {
-          printf("got err %d\n", err);
-          size_t err_length;
-          char buffer[2048];
-          clGetProgramBuildInfo(k->program, device_id, CL_PROGRAM_BUILD_LOG, sizeof(buffer), buffer, &err_length);
-          buffer[err_length] = '\0';
-          printf("%s\n", buffer);
+      if (util::file_exists(fn)) {
+        string kernel_src = util::read_file(fn);
+        if (k->name.rfind("convolution_", 0) == 0) {
+          kernel_src += convolution_;
         }
-        assert(err == 0);
+        printf("building kernel %s with len %lu\n", k->name.c_str(), kernel_src.length());
+        k->program = cl_program_from_source(context, device_id, kernel_src);
 
         // save in cache
         g_programs[k->name] = k->program;
-        g_program_source[k->program] = string((char *)src, length);
+        g_program_source[k->program] = kernel_src;
       } else {
         g_programs[k->name] = NULL;
       }
diff --git a/selfdrive/modeld/thneed/serialize.cc b/selfdrive/modeld/thneed/serialize.cc
index cd5584553b..89b761b9d2 100644
--- a/selfdrive/modeld/thneed/serialize.cc
+++ b/selfdrive/modeld/thneed/serialize.cc
@@ -63,14 +63,14 @@ void Thneed::load(const char *filename) {
 
   map<string, cl_program> g_programs;
   for (const auto &[name, source] : jdat["programs"].object_items()) {
-    if (record & THNEED_DEBUG) printf("building %s with size %zu\n", name.c_str(), source.string_value().size());
+    if (debug >= 1) printf("building %s with size %zu\n", name.c_str(), source.string_value().size());
     g_programs[name] = cl_program_from_source(context, device_id, source.string_value());
   }
 
   for (auto &obj : jdat["binaries"].array_items()) {
     string name = obj["name"].string_value();
     size_t length = obj["length"].int_value();
-    if (record & THNEED_DEBUG) printf("binary %s with size %zu\n", name.c_str(), length);
+    if (debug >= 1) printf("binary %s with size %zu\n", name.c_str(), length);
     g_programs[name] = cl_program_from_binary(context, device_id, (const uint8_t*)&buf[ptr], length);
     ptr += length;
   }
diff --git a/selfdrive/modeld/thneed/thneed.cc b/selfdrive/modeld/thneed/thneed.cc
index 31bfa3e2f3..e2dc9b72f2 100644
--- a/selfdrive/modeld/thneed/thneed.cc
+++ b/selfdrive/modeld/thneed/thneed.cc
@@ -55,12 +55,12 @@ int ioctl(int filedes, unsigned long request, void *argp) {
   if (thneed != NULL) {
     if (request == IOCTL_KGSL_GPU_COMMAND) {
       struct kgsl_gpu_command *cmd = (struct kgsl_gpu_command *)argp;
-      if (thneed->record & THNEED_RECORD) {
+      if (thneed->record) {
         thneed->timestamp = cmd->timestamp;
         thneed->context_id = cmd->context_id;
         thneed->cmds.push_back(unique_ptr<CachedCommand>(new CachedCommand(thneed, cmd)));
       }
-      if (thneed->record & THNEED_DEBUG) {
+      if (thneed->debug >= 1) {
         printf("IOCTL_KGSL_GPU_COMMAND(%2zu): flags: 0x%lx    context_id: %u  timestamp: %u  numcmds: %d  numobjs: %d\n",
             thneed->cmds.size(),
             cmd->flags,
@@ -70,7 +70,7 @@ int ioctl(int filedes, unsigned long request, void *argp) {
       struct kgsl_gpuobj_sync *cmd = (struct kgsl_gpuobj_sync *)argp;
       struct kgsl_gpuobj_sync_obj *objs = (struct kgsl_gpuobj_sync_obj *)(cmd->objs);
 
-      if (thneed->record & THNEED_DEBUG) {
+      if (thneed->debug >= 2) {
         printf("IOCTL_KGSL_GPUOBJ_SYNC count:%d ", cmd->count);
         for (int i = 0; i < cmd->count; i++) {
           printf(" -- offset:0x%lx len:0x%lx id:%d op:%d  ", objs[i].offset, objs[i].length, objs[i].id, objs[i].op);
@@ -78,21 +78,21 @@ int ioctl(int filedes, unsigned long request, void *argp) {
         printf("\n");
       }
 
-      if (thneed->record & THNEED_RECORD) {
+      if (thneed->record) {
         thneed->cmds.push_back(unique_ptr<CachedSync>(new
               CachedSync(thneed, string((char *)objs, sizeof(struct kgsl_gpuobj_sync_obj)*cmd->count))));
       }
     } else if (request == IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID) {
       struct kgsl_device_waittimestamp_ctxtid *cmd = (struct kgsl_device_waittimestamp_ctxtid *)argp;
-      if (thneed->record & THNEED_DEBUG) {
+      if (thneed->debug >= 1) {
         printf("IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID: context_id: %d  timestamp: %d  timeout: %d\n",
             cmd->context_id, cmd->timestamp, cmd->timeout);
       }
     } else if (request == IOCTL_KGSL_SETPROPERTY) {
-      if (thneed->record & THNEED_DEBUG) {
+      if (thneed->debug >= 1) {
         struct kgsl_device_getproperty *prop = (struct kgsl_device_getproperty *)argp;
         printf("IOCTL_KGSL_SETPROPERTY: 0x%x sizebytes:%zu\n", prop->type, prop->sizebytes);
-        if (thneed->record & THNEED_VERBOSE_DEBUG) {
+        if (thneed->debug >= 2) {
           hexdump((uint8_t *)prop->value, prop->sizebytes);
           if (prop->type == KGSL_PROP_PWR_CONSTRAINT) {
             struct kgsl_device_constraint *constraint = (struct kgsl_device_constraint *)prop->value;
@@ -105,7 +105,7 @@ int ioctl(int filedes, unsigned long request, void *argp) {
     } else if (request == IOCTL_KGSL_GPUOBJ_ALLOC || request == IOCTL_KGSL_GPUOBJ_FREE) {
       // this happens
     } else {
-      if (thneed->record & THNEED_DEBUG) {
+      if (thneed->debug >= 1) {
         printf("other ioctl %lx\n", request);
       }
     }
@@ -197,9 +197,9 @@ void CachedCommand::exec() {
   cache.timestamp = ++thneed->timestamp;
   int ret = ioctl(thneed->fd, IOCTL_KGSL_GPU_COMMAND, &cache);
 
-  if (thneed->record & THNEED_DEBUG) printf("CachedCommand::exec got %d\n", ret);
+  if (thneed->debug >= 1) printf("CachedCommand::exec got %d\n", ret);
 
-  if (thneed->record & THNEED_VERBOSE_DEBUG) {
+  if (thneed->debug >= 2) {
     for (auto &it : kq) {
       it->debug_print(false);
     }
@@ -220,9 +220,11 @@ Thneed::Thneed(bool do_clinit) {
   assert(g_fd != -1);
   fd = g_fd;
   ram = make_unique<GPUMalloc>(0x80000, fd);
-  record = THNEED_RECORD;
+  record = true;
   timestamp = -1;
   g_thneed = this;
+  char *thneed_debug_env = getenv("THNEED_DEBUG");
+  debug = (thneed_debug_env != NULL) ? atoi(thneed_debug_env) : 0;
 }
 
 void Thneed::stop() {
@@ -261,7 +263,7 @@ void Thneed::find_inputs_outputs() {
 void Thneed::copy_inputs(float **finputs) {
   //cl_int ret;
   for (int idx = 0; idx < inputs.size(); ++idx) {
-    if (record & THNEED_DEBUG) printf("copying %lu -- %p -> %p\n", input_sizes[idx], finputs[idx], inputs[idx]);
+    if (debug >= 1) printf("copying %lu -- %p -> %p\n", input_sizes[idx], finputs[idx], inputs[idx]);
     if (finputs[idx] != NULL) memcpy(inputs[idx], finputs[idx], input_sizes[idx]);
   }
 }
@@ -270,7 +272,7 @@ void Thneed::copy_output(float *foutput) {
   if (output != NULL) {
     size_t sz;
     clGetMemObjectInfo(output, CL_MEM_SIZE, sizeof(sz), &sz, NULL);
-    if (record & THNEED_DEBUG) printf("copying %lu for output %p -> %p\n", sz, output, foutput);
+    if (debug >= 1) printf("copying %lu for output %p -> %p\n", sz, output, foutput);
     clEnqueueReadBuffer(command_queue, output, CL_TRUE, 0, sz, foutput, 0, NULL, NULL);
   } else {
     printf("CAUTION: model output is NULL, does it have no outputs?\n");
@@ -287,12 +289,12 @@ void Thneed::wait() {
   int wret = ioctl(fd, IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID, &wait);
   uint64_t te = nanos_since_boot();
 
-  if (record & THNEED_DEBUG) printf("wait %d after %lu us\n", wret, (te-tb)/1000);
+  if (debug >= 1) printf("wait %d after %lu us\n", wret, (te-tb)/1000);
 }
 
 void Thneed::execute(float **finputs, float *foutput, bool slow) {
   uint64_t tb, te;
-  if (record & THNEED_DEBUG) tb = nanos_since_boot();
+  if (debug >= 1) tb = nanos_since_boot();
 
   // ****** copy inputs
   copy_inputs(finputs);
@@ -319,7 +321,7 @@ void Thneed::execute(float **finputs, float *foutput, bool slow) {
   int i = 0;
   for (auto &it : cmds) {
     ++i;
-    if (record & THNEED_DEBUG) printf("run %2d @ %7lu us: ", i, (nanos_since_boot()-tb)/1000);
+    if (debug >= 1) printf("run %2d @ %7lu us: ", i, (nanos_since_boot()-tb)/1000);
     it->exec();
     if ((i == cmds.size()) || slow) wait();
   }
@@ -335,7 +337,7 @@ void Thneed::execute(float **finputs, float *foutput, bool slow) {
   ret = ioctl(fd, IOCTL_KGSL_SETPROPERTY, &prop);
   assert(ret == 0);
 
-  if (record & THNEED_DEBUG) {
+  if (debug >= 1) {
     te = nanos_since_boot();
     printf("model exec in %lu us\n", (te-tb)/1000);
   }
@@ -353,7 +355,7 @@ void Thneed::clinit() {
 cl_int Thneed::clexec() {
   printf("Thneed::clexec: running %lu queued kernels\n", kq.size());
   for (auto &k : kq) {
-    if (record & THNEED_RECORD) ckq.push_back(k);
+    if (record) ckq.push_back(k);
     cl_int ret = k->exec();
     assert(ret == CL_SUCCESS);
   }
@@ -391,7 +393,7 @@ cl_int thneed_clEnqueueNDRangeKernel(cl_command_queue command_queue,
   assert(event_wait_list == NULL);
 
   cl_int ret = 0;
-  if (thneed != NULL && thneed->record & THNEED_RECORD) {
+  if (thneed != NULL && thneed->record) {
     if (thneed->context == NULL) {
       thneed->command_queue = command_queue;
       clGetKernelInfo(kernel, CL_KERNEL_CONTEXT, sizeof(thneed->context), &thneed->context, NULL);
@@ -413,7 +415,7 @@ cl_int thneed_clEnqueueNDRangeKernel(cl_command_queue command_queue,
 cl_int thneed_clFinish(cl_command_queue command_queue) {
   Thneed *thneed = g_thneed;
 
-  if (thneed != NULL && thneed->record & THNEED_RECORD) {
+  if (thneed != NULL && thneed->record) {
     #ifdef RUN_OPTIMIZER
       thneed->optimize();
     #endif
@@ -520,8 +522,8 @@ cl_int CLQueuedKernel::exec() {
     }
   }
 
-  if (thneed->record & THNEED_DEBUG) {
-    debug_print(thneed->record & THNEED_VERBOSE_DEBUG);
+  if (thneed->debug >= 1) {
+    debug_print(thneed->debug >= 2);
   }
 
   return clEnqueueNDRangeKernel(thneed->command_queue,
diff --git a/selfdrive/modeld/thneed/thneed.h b/selfdrive/modeld/thneed/thneed.h
index 1197e4c5ed..b09d32b0ef 100644
--- a/selfdrive/modeld/thneed/thneed.h
+++ b/selfdrive/modeld/thneed/thneed.h
@@ -14,10 +14,6 @@
 
 #include "selfdrive/modeld/thneed/include/msm_kgsl.h"
 
-#define THNEED_RECORD 1
-#define THNEED_DEBUG 2
-#define THNEED_VERBOSE_DEBUG 4
-
 using namespace std;
 
 namespace json11 {
@@ -110,7 +106,8 @@ class Thneed {
     int context_id;
 
     // protected?
-    int record;
+    bool record;
+    int debug;
     int timestamp;
     unique_ptr<GPUMalloc> ram;
     vector<unique_ptr<CachedIoctl> > cmds;
diff --git a/selfdrive/modeld/thneed/weights_fixup.py b/selfdrive/modeld/thneed/weights_fixup.py
new file mode 100755
index 0000000000..47875a9ee0
--- /dev/null
+++ b/selfdrive/modeld/thneed/weights_fixup.py
@@ -0,0 +1,145 @@
+#!/usr/bin/env python3
+import os
+import struct
+import zipfile
+import numpy as np
+from tqdm import tqdm
+
+from common.basedir import BASEDIR
+from selfdrive.modeld.thneed.lib import load_thneed, save_thneed
+
+# this is junk code, but it doesn't have deps
+def load_dlc_weights(fn):
+  archive = zipfile.ZipFile(fn, 'r')
+  dlc_params = archive.read("model.params")
+
+  def extract(rdat):
+    idx = rdat.find(b"\x00\x00\x00\x09\x04\x00\x00\x00")
+    rdat = rdat[idx+8:]
+    ll = struct.unpack("I", rdat[0:4])[0]
+    buf = np.frombuffer(rdat[4:4+ll*4], dtype=np.float32)
+    rdat = rdat[4+ll*4:]
+    dims = struct.unpack("I", rdat[0:4])[0]
+    buf = buf.reshape(struct.unpack("I"*dims, rdat[4:4+dims*4]))
+    if len(buf.shape) == 4:
+      buf = np.transpose(buf, (3,2,0,1))
+    return buf
+
+  def parse(tdat):
+    ll = struct.unpack("I", tdat[0:4])[0] + 4
+    return (None, [extract(tdat[0:]), extract(tdat[ll:])])
+
+  ptr = 0x20
+  def r4():
+    nonlocal ptr
+    ret = struct.unpack("I", dlc_params[ptr:ptr+4])[0]
+    ptr += 4
+    return ret
+  ranges = []
+  cnt = r4()
+  for _ in range(cnt):
+    o = r4() + ptr
+    # the header is 0xC
+    plen, is_4, is_2 = struct.unpack("III", dlc_params[o:o+0xC])
+    assert is_4 == 4 and is_2 == 2
+    ranges.append((o+0xC, o+plen+0xC))
+  ranges = sorted(ranges, reverse=True)
+
+  return [parse(dlc_params[s:e]) for s,e in ranges]
+
+# this won't run on device without onnx
+def load_onnx_weights(fn):
+  import onnx
+  from onnx import numpy_helper
+
+  model = onnx.load(fn)
+  graph = model.graph  # pylint: disable=maybe-no-member
+  init = {x.name:x for x in graph.initializer}
+
+  onnx_layers = []
+  for node in graph.node:
+    #print(node.name, node.op_type, node.input, node.output)
+    vals = []
+    for inp in node.input:
+      if inp in init:
+        vals.append(numpy_helper.to_array(init[inp]))
+    if len(vals) > 0:
+      onnx_layers.append((node.name, vals))
+  return onnx_layers
+
+def weights_fixup(target, source_thneed, dlc):
+  #onnx_layers = load_onnx_weights(os.path.join(BASEDIR, "models/supercombo.onnx"))
+  onnx_layers = load_dlc_weights(dlc)
+  jdat = load_thneed(source_thneed)
+
+  bufs = {}
+  for o in jdat['objects']:
+    bufs[o['id']] = o
+
+  thneed_layers = []
+  for k in jdat['kernels']:
+    #print(k['name'])
+    vals = []
+    for a in k['args']:
+      if a in bufs:
+        o = bufs[a]
+        if o['needs_load'] or ('buffer_id' in o and bufs[o['buffer_id']]['needs_load']):
+          #print("  ", o['arg_type'])
+          vals.append(o)
+    if len(vals) > 0:
+      thneed_layers.append((k['name'], vals))
+
+  assert len(thneed_layers) == len(onnx_layers)
+
+  # fix up weights
+  for tl, ol in tqdm(zip(thneed_layers, onnx_layers), total=len(thneed_layers)):
+    #print(tl[0], ol[0])
+    assert len(tl[1]) == len(ol[1])
+    for o, onnx_weight in zip(tl[1], ol[1]):
+      if o['arg_type'] == "image2d_t":
+        obuf = bufs[o['buffer_id']]
+        saved_weights = np.frombuffer(obuf['data'], dtype=np.float16).reshape(o['height'], o['row_pitch']//2)
+
+        if len(onnx_weight.shape) == 4:
+          # convolution
+          oc,ic,ch,cw = onnx_weight.shape
+
+          if 'depthwise' in tl[0]:
+            assert ic == 1
+            weights = np.transpose(onnx_weight.reshape(oc//4,4,ch,cw), (0,2,3,1)).reshape(o['height'], o['width']*4)
+          else:
+            weights = np.transpose(onnx_weight.reshape(oc//4,4,ic//4,4,ch,cw), (0,4,2,5,1,3)).reshape(o['height'], o['width']*4)
+        else:
+          # fc_Wtx
+          weights = onnx_weight
+
+        new_weights = np.zeros((o['height'], o['row_pitch']//2), dtype=np.float32)
+        new_weights[:, :weights.shape[1]] = weights
+
+        # weights shouldn't be too far off
+        err = np.mean((saved_weights.astype(np.float32) - new_weights)**2)
+        assert err < 1e-3
+        rerr = np.mean(np.abs((saved_weights.astype(np.float32) - new_weights)/(new_weights+1e-12)))
+        assert rerr < 0.5
+
+        # fix should improve things
+        fixed_err = np.mean((new_weights.astype(np.float16).astype(np.float32) - new_weights)**2)
+        assert (err/fixed_err) >= 1
+
+        #print("   ", o['size'], onnx_weight.shape, o['row_pitch'], o['width'], o['height'], "err %.2fx better" % (err/fixed_err))
+
+        obuf['data'] = new_weights.astype(np.float16).tobytes()
+
+      elif o['arg_type'] == "float*":
+        # unconverted floats are correct
+        new_weights = np.zeros(o['size']//4, dtype=np.float32)
+        new_weights[:onnx_weight.shape[0]] = onnx_weight
+        assert new_weights.tobytes() == o['data']
+        #print("   ", o['size'], onnx_weight.shape)
+
+  save_thneed(jdat, target)
+
+if __name__ == "__main__":
+  weights_fixup(os.path.join(BASEDIR, "models/supercombo_fixed.thneed"),
+                os.path.join(BASEDIR, "models/supercombo.thneed"),
+                os.path.join(BASEDIR, "models/supercombo.dlc"))
diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py
index efb00ecb77..2e317aefbd 100644
--- a/selfdrive/monitoring/driver_monitor.py
+++ b/selfdrive/monitoring/driver_monitor.py
@@ -18,8 +18,9 @@ EventName = car.CarEvent.EventName
 class DRIVER_MONITOR_SETTINGS():
   def __init__(self, TICI=TICI, DT_DMON=DT_DMON):
     self._DT_DMON = DT_DMON
-    self._AWARENESS_TIME = 35. # passive wheeltouch total timeout
-    self._AWARENESS_PRE_TIME_TILL_TERMINAL = 12.
+    # ref (page15-16): https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2
+    self._AWARENESS_TIME = 30. # passive wheeltouch total timeout
+    self._AWARENESS_PRE_TIME_TILL_TERMINAL = 15.
     self._AWARENESS_PROMPT_TIME_TILL_TERMINAL = 6.
     self._DISTRACTED_TIME = 11. # active monitoring total timeout
     self._DISTRACTED_PRE_TIME_TILL_TERMINAL = 8.
diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py
index 60aaeb724d..698877dd3a 100755
--- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py
+++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py
@@ -9,7 +9,7 @@ from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
 # TODO: make new FCW tests
 maneuvers = [
   Maneuver(
-    'approach stopped car at 20m/s',
+    'approach stopped car at 20m/s, initial distance: 120m',
     duration=20.,
     initial_speed=25.,
     lead_relevancy=True,
@@ -18,7 +18,7 @@ maneuvers = [
     breakpoints=[0., 1.],
   ),
   Maneuver(
-    'approach stopped car at 20m/s',
+    'approach stopped car at 20m/s, initial distance 90m',
     duration=20.,
     initial_speed=20.,
     lead_relevancy=True,
@@ -65,7 +65,7 @@ maneuvers = [
     breakpoints=[2., 2.01, 8.8],
   ),
   Maneuver(
-    "approach stopped car at 20m/s",
+    "approach stopped car at 20m/s, with prob_lead_values",
     duration=30.,
     initial_speed=20.,
     lead_relevancy=True,
diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit
index d210251c09..a875d709c2 100644
--- a/selfdrive/test/process_replay/model_replay_ref_commit
+++ b/selfdrive/test/process_replay/model_replay_ref_commit
@@ -1 +1 @@
-19720e79b1c5136a882efd689651d9044e2e2007
+710313545a2b5e0899c6ee0b99ecd9d322cfecb7
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index 9982949b20..3226b8614b 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-67c8f283858998b75ac28879e1350a589a968e5d
\ No newline at end of file
+37aa2e3afedc8364d2bbecc5f1b7ed4efec52e30
\ No newline at end of file
diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py
index 16ee5e9a28..e2493d048f 100755
--- a/selfdrive/test/process_replay/regen.py
+++ b/selfdrive/test/process_replay/regen.py
@@ -159,7 +159,7 @@ def replay_cameras(lr, frs):
                                      args=(s, stream, dt, vs, frames, size)))
 
   # hack to make UI work
-  vs.create_buffers(VisionStreamType.VISION_STREAM_RGB_BACK, 4, True, eon_f_frame_size[0], eon_f_frame_size[1])
+  vs.create_buffers(VisionStreamType.VISION_STREAM_RGB_ROAD, 4, True, eon_f_frame_size[0], eon_f_frame_size[1])
   vs.start_listener()
   return vs, p
 
diff --git a/selfdrive/test/setup_device_ci.sh b/selfdrive/test/setup_device_ci.sh
index 260a1b4487..f2f76299f2 100755
--- a/selfdrive/test/setup_device_ci.sh
+++ b/selfdrive/test/setup_device_ci.sh
@@ -20,28 +20,47 @@ fi
 umount /data/safe_staging/merged/ || true
 sudo umount /data/safe_staging/merged/ || true
 
+export KEYS_PARAM_PATH="/data/params/d/GithubSshKeys"
 if [ -f "/EON" ]; then
+  export KEYS_PATH="/data/data/com.termux/files/home/setup_keys"
+  export CONTINUE_PATH="/data/data/com.termux/files/continue.sh"
+
+  if ! grep -F "$KEYS_PATH" /usr/etc/ssh/sshd_config; then
+    echo "setting up keys"
+    mount -o rw,remount /system
+    sed -i "s,$KEYS_PARAM_PATH,$KEYS_PATH," /usr/etc/ssh/sshd_config
+    mount -o ro,remount /system
+  fi
+
+  # these can get pretty big
   rm -rf /data/core
   rm -rf /data/neoupdate
   rm -rf /data/safe_staging
-fi
+else
+  export KEYS_PATH="/usr/comma/setup_keys"
+  export CONTINUE_PATH="/data/continue.sh"
 
-export KEYS_PATH="/usr/comma/setup_keys"
-export CONTINUE_PATH="/data/continue.sh"
-if [ -f "/EON" ]; then
-  export KEYS_PATH="/data/data/com.termux/files/home/setup_keys"
-  export CONTINUE_PATH="/data/data/com.termux/files/continue.sh"
+  if ! grep -F "$KEYS_PATH" /etc/ssh/sshd_config; then
+    echo "setting up keys"
+    sudo mount -o rw,remount /
+    sudo systemctl enable ssh
+    sudo sed -i "s,$KEYS_PARAM_PATH,$KEYS_PATH," /etc/ssh/sshd_config
+    sudo mount -o ro,remount /
+  fi
 fi
+
 tee $CONTINUE_PATH << EOF
 #!/usr/bin/bash
 
-PARAMS_ROOT="/data/params/d"
-
 while true; do
-  mkdir -p \$PARAMS_ROOT
-  cp $KEYS_PATH \$PARAMS_ROOT/GithubSshKeys
-  echo -n 1 > \$PARAMS_ROOT/SshEnabled
-  sleep 1m
+  if [ -f /EON ]; then
+    setprop persist.neos.ssh 1
+  else
+    if ! sudo systemctl is-active -q ssh; then
+      sudo systemctl start ssh
+    fi
+  fi
+  sleep 10s
 done
 
 sleep infinity
diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py
index a49f93e37d..80a381d3fc 100755
--- a/selfdrive/test/test_onroad.py
+++ b/selfdrive/test/test_onroad.py
@@ -216,8 +216,9 @@ class TestOnroad(unittest.TestCase):
       ts = [getattr(getattr(m, s), "solverExecutionTime") for m in self.lr if m.which() == s]
       self.assertLess(min(ts), instant_max, f"high '{s}' execution time: {min(ts)}")
       self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}")
-      result += f"'{s}' execution time: {min(ts)}\n"
-      result += f"'{s}' avg execution time: {np.mean(ts)}\n"
+      result += f"'{s}' execution time: min  {min(ts):.5f}s\n"
+      result += f"'{s}' execution time: max  {max(ts):.5f}s\n"
+      result += f"'{s}' execution time: mean {np.mean(ts):.5f}s\n"
     result += "------------------------------------------------\n"
     print(result)
 
@@ -237,8 +238,8 @@ class TestOnroad(unittest.TestCase):
       ts = [getattr(getattr(m, s), "modelExecutionTime") for m in self.lr if m.which() == s]
       self.assertLess(min(ts), instant_max, f"high '{s}' execution time: {min(ts)}")
       self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}")
-      result += f"'{s}' execution time: {min(ts)}\n"
-      result += f"'{s}' avg execution time: {np.mean(ts)}\n"
+      result += f"'{s}' execution time: min  {min(ts):.5f}s\n"
+      result += f"'{s}' execution time: mean {np.mean(ts):.5f}s\n"
     result += "------------------------------------------------\n"
     print(result)
 
diff --git a/selfdrive/test/update_ci_routes.py b/selfdrive/test/update_ci_routes.py
index 3a392b4b36..fe6d815ebf 100755
--- a/selfdrive/test/update_ci_routes.py
+++ b/selfdrive/test/update_ci_routes.py
@@ -3,7 +3,7 @@ import sys
 import subprocess
 from azure.storage.blob import BlockBlobService  # pylint: disable=import-error
 
-from selfdrive.test.test_routes import routes as test_car_models_routes
+from selfdrive.car.tests.routes import routes as test_car_models_routes
 from selfdrive.test.process_replay.test_processes import original_segments as replay_segments
 from xx.chffr.lib import azureutil  # pylint: disable=import-error
 from xx.chffr.lib.storage import _DATA_ACCOUNT_PRODUCTION, _DATA_ACCOUNT_CI, _DATA_BUCKET_PRODUCTION  # pylint: disable=import-error
diff --git a/selfdrive/thermald/fan_controller.py b/selfdrive/thermald/fan_controller.py
new file mode 100644
index 0000000000..2fe52d0cb8
--- /dev/null
+++ b/selfdrive/thermald/fan_controller.py
@@ -0,0 +1,103 @@
+#!/usr/bin/env python3
+
+import os
+from smbus2 import SMBus
+from abc import ABC, abstractmethod
+from common.realtime import DT_TRML
+from common.numpy_fast import interp
+from selfdrive.swaglog import cloudlog
+from selfdrive.controls.lib.pid import PIController
+
+class BaseFanController(ABC):
+  @abstractmethod
+  def update(self, max_cpu_temp: float, ignition: bool) -> int:
+    pass
+
+
+class EonFanController(BaseFanController):
+  # Temp thresholds to control fan speed - high hysteresis
+  TEMP_THRS_H = [50., 65., 80., 10000]
+  # Temp thresholds to control fan speed - low hysteresis
+  TEMP_THRS_L = [42.5, 57.5, 72.5, 10000]
+  # Fan speed options
+  FAN_SPEEDS = [0, 16384, 32768, 65535]
+
+  def __init__(self) -> None:
+    super().__init__()
+    cloudlog.info("Setting up EON fan handler")
+
+    self.fan_speed = -1
+    self.setup_eon_fan()
+
+  def setup_eon_fan(self) -> None:
+    os.system("echo 2 > /sys/module/dwc3_msm/parameters/otg_switch")
+
+  def set_eon_fan(self, speed: int) -> None:
+    if self.fan_speed != speed:
+      # FIXME: this is such an ugly hack to get the right index
+      val = speed // 16384
+
+      bus = SMBus(7, force=True)
+      try:
+        i = [0x1, 0x3 | 0, 0x3 | 0x08, 0x3 | 0x10][val]
+        bus.write_i2c_block_data(0x3d, 0, [i])
+      except OSError:
+        # tusb320
+        if val == 0:
+          bus.write_i2c_block_data(0x67, 0xa, [0])
+        else:
+          bus.write_i2c_block_data(0x67, 0xa, [0x20])
+          bus.write_i2c_block_data(0x67, 0x8, [(val - 1) << 6])
+      bus.close()
+      self.fan_speed = speed
+
+  def update(self, max_cpu_temp: float, ignition: bool) -> int:
+    new_speed_h = next(speed for speed, temp_h in zip(self.FAN_SPEEDS, self.TEMP_THRS_H) if temp_h > max_cpu_temp)
+    new_speed_l = next(speed for speed, temp_l in zip(self.FAN_SPEEDS, self.TEMP_THRS_L) if temp_l > max_cpu_temp)
+
+    if new_speed_h > self.fan_speed:
+      self.set_eon_fan(new_speed_h)
+    elif new_speed_l < self.fan_speed:
+      self.set_eon_fan(new_speed_l)
+
+    return self.fan_speed
+
+
+class UnoFanController(BaseFanController):
+  def __init__(self) -> None:
+    super().__init__()
+    cloudlog.info("Setting up UNO fan handler")
+
+  def update(self, max_cpu_temp: float, ignition: bool) -> int:
+    new_speed = int(interp(max_cpu_temp, [40.0, 80.0], [0, 80]))
+
+    if not ignition:
+      new_speed = min(30, new_speed)
+
+    return new_speed
+
+
+class TiciFanController(BaseFanController):
+  def __init__(self) -> None:
+    super().__init__()
+    cloudlog.info("Setting up TICI fan handler")
+
+    self.last_ignition = False
+    self.controller = PIController(k_p=0, k_i=2e-3, k_f=1, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML))
+
+  def update(self, max_cpu_temp: float, ignition: bool) -> int:
+    self.controller.neg_limit = -(80 if ignition else 30)
+    self.controller.pos_limit = -(30 if ignition else 0)
+
+    if ignition != self.last_ignition:
+      self.controller.reset()
+
+    fan_pwr_out = -int(self.controller.update(
+                      setpoint=75,
+                      measurement=max_cpu_temp,
+                      feedforward=interp(max_cpu_temp, [60.0, 100.0], [0, -80])
+                    ))
+
+    self.last_ignition = ignition
+    return fan_pwr_out
+
diff --git a/selfdrive/thermald/tests/test_fan_controller.py b/selfdrive/thermald/tests/test_fan_controller.py
new file mode 100755
index 0000000000..8865b1f98b
--- /dev/null
+++ b/selfdrive/thermald/tests/test_fan_controller.py
@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+import unittest
+from unittest.mock import Mock, MagicMock, patch
+from parameterized import parameterized
+
+with patch("smbus2.SMBus", new=MagicMock()):
+  from selfdrive.thermald.fan_controller import EonFanController, UnoFanController, TiciFanController
+
+ALL_CONTROLLERS = [(EonFanController, ), (UnoFanController,), (TiciFanController,)]
+GEN2_CONTROLLERS = [(UnoFanController,), (TiciFanController,)]
+
+def patched_controller(controller_class):
+  with patch("os.system", new=Mock()):
+    return controller_class()
+
+class TestFanController(unittest.TestCase):
+  def wind_up(self, controller, ignition=True):
+    for _ in range(1000):
+      controller.update(max_cpu_temp=100, ignition=ignition)
+
+  def wind_down(self, controller, ignition=False):
+    for _ in range(1000):
+      controller.update(max_cpu_temp=10, ignition=ignition)
+
+  @parameterized.expand(ALL_CONTROLLERS)
+  def test_hot_onroad(self, controller_class):
+    controller = patched_controller(controller_class)
+    self.wind_up(controller)
+    self.assertGreaterEqual(controller.update(max_cpu_temp=100, ignition=True), 70)
+
+  @parameterized.expand(GEN2_CONTROLLERS)
+  def test_offroad_limits(self, controller_class):
+    controller = patched_controller(controller_class)
+    self.wind_up(controller)
+    self.assertLessEqual(controller.update(max_cpu_temp=100, ignition=False), 30)
+
+  @parameterized.expand(ALL_CONTROLLERS)
+  def test_no_fan_wear(self, controller_class):
+    controller = patched_controller(controller_class)
+    self.wind_down(controller)
+    self.assertEqual(controller.update(max_cpu_temp=10, ignition=False), 0)
+
+  @parameterized.expand(GEN2_CONTROLLERS)
+  def test_limited(self, controller_class):
+    controller = patched_controller(controller_class)
+    self.wind_up(controller, ignition=True)
+    self.assertGreaterEqual(controller.update(max_cpu_temp=100, ignition=True), 80)
+
+  @parameterized.expand(ALL_CONTROLLERS)
+  def test_windup_speed(self, controller_class):
+    controller = patched_controller(controller_class)
+    self.wind_down(controller, ignition=True)
+    for _ in range(10):
+      controller.update(max_cpu_temp=90, ignition=True)
+    self.assertGreaterEqual(controller.update(max_cpu_temp=90, ignition=True), 60)
+
+if __name__ == "__main__":
+  unittest.main()
diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py
index 113ffaa04c..1f62738ed7 100755
--- a/selfdrive/thermald/thermald.py
+++ b/selfdrive/thermald/thermald.py
@@ -9,22 +9,20 @@ from pathlib import Path
 from typing import Dict, Optional, Tuple
 
 import psutil
-from smbus2 import SMBus
 
 import cereal.messaging as messaging
 from cereal import log
 from common.dict_helpers import strip_deprecated_keys
 from common.filter_simple import FirstOrderFilter
-from common.numpy_fast import interp
 from common.params import Params
 from common.realtime import DT_TRML, sec_since_boot
 from selfdrive.controls.lib.alertmanager import set_offroad_alert
-from selfdrive.controls.lib.pid import PIController
 from selfdrive.hardware import EON, HARDWARE, PC, TICI
 from selfdrive.loggerd.config import get_available_percent
 from selfdrive.statsd import statlog
 from selfdrive.swaglog import cloudlog
 from selfdrive.thermald.power_monitoring import PowerMonitoring
+from selfdrive.thermald.fan_controller import EonFanController, UnoFanController, TiciFanController
 from selfdrive.version import terms_version, training_version
 
 ThermalStatus = log.DeviceState.ThermalStatus
@@ -36,7 +34,7 @@ DISCONNECT_TIMEOUT = 5.  # wait 5 seconds before going offroad after disconnect
 PANDA_STATES_TIMEOUT = int(1000 * 2.5 * DT_TRML)  # 2.5x the expected pandaState frequency
 
 ThermalBand = namedtuple("ThermalBand", ['min_temp', 'max_temp'])
-HardwareState = namedtuple("HardwareState", ['network_type', 'network_strength', 'network_info', 'nvme_temps', 'modem_temps'])
+HardwareState = namedtuple("HardwareState", ['network_type', 'network_metered', 'network_strength', 'network_info', 'nvme_temps', 'modem_temps'])
 
 # List of thermal bands. We will stay within this region as long as we are within the bounds.
 # When exiting the bounds, we'll jump to the lower or higher band. Bands are ordered in the dict.
@@ -52,10 +50,25 @@ OFFROAD_DANGER_TEMP = 79.5 if TICI else 70.0
 
 prev_offroad_states: Dict[str, Tuple[bool, Optional[str]]] = {}
 
+tz_by_type: Optional[Dict[str, int]] = None
+def populate_tz_by_type():
+  global tz_by_type
+  tz_by_type = {}
+  for n in os.listdir("/sys/devices/virtual/thermal"):
+    if not n.startswith("thermal_zone"):
+      continue
+    with open(os.path.join("/sys/devices/virtual/thermal", n, "type")) as f:
+      tz_by_type[f.read().strip()] = int(n.lstrip("thermal_zone"))
+
 def read_tz(x):
   if x is None:
     return 0
 
+  if isinstance(x, str):
+    if tz_by_type is None:
+      populate_tz_by_type()
+    x = tz_by_type[x]
+
   try:
     with open(f"/sys/devices/virtual/thermal/thermal_zone{x}/temp") as f:
       return int(f.read())
@@ -73,83 +86,6 @@ def read_thermal(thermal_config):
   return dat
 
 
-def setup_eon_fan():
-  os.system("echo 2 > /sys/module/dwc3_msm/parameters/otg_switch")
-
-
-last_eon_fan_val = None
-def set_eon_fan(val):
-  global last_eon_fan_val
-
-  if last_eon_fan_val is None or last_eon_fan_val != val:
-    bus = SMBus(7, force=True)
-    try:
-      i = [0x1, 0x3 | 0, 0x3 | 0x08, 0x3 | 0x10][val]
-      bus.write_i2c_block_data(0x3d, 0, [i])
-    except OSError:
-      # tusb320
-      if val == 0:
-        bus.write_i2c_block_data(0x67, 0xa, [0])
-      else:
-        bus.write_i2c_block_data(0x67, 0xa, [0x20])
-        bus.write_i2c_block_data(0x67, 0x8, [(val - 1) << 6])
-    bus.close()
-    last_eon_fan_val = val
-
-
-# temp thresholds to control fan speed - high hysteresis
-_TEMP_THRS_H = [50., 65., 80., 10000]
-# temp thresholds to control fan speed - low hysteresis
-_TEMP_THRS_L = [42.5, 57.5, 72.5, 10000]
-# fan speed options
-_FAN_SPEEDS = [0, 16384, 32768, 65535]
-
-
-def handle_fan_eon(controller, max_cpu_temp, fan_speed, ignition):
-  new_speed_h = next(speed for speed, temp_h in zip(_FAN_SPEEDS, _TEMP_THRS_H) if temp_h > max_cpu_temp)
-  new_speed_l = next(speed for speed, temp_l in zip(_FAN_SPEEDS, _TEMP_THRS_L) if temp_l > max_cpu_temp)
-
-  if new_speed_h > fan_speed:
-    # update speed if using the high thresholds results in fan speed increment
-    fan_speed = new_speed_h
-  elif new_speed_l < fan_speed:
-    # update speed if using the low thresholds results in fan speed decrement
-    fan_speed = new_speed_l
-
-  set_eon_fan(fan_speed // 16384)
-
-  return fan_speed
-
-
-def handle_fan_uno(controller, max_cpu_temp, fan_speed, ignition):
-  new_speed = int(interp(max_cpu_temp, [40.0, 80.0], [0, 80]))
-
-  if not ignition:
-    new_speed = min(30, new_speed)
-
-  return new_speed
-
-
-last_ignition = False
-def handle_fan_tici(controller, max_cpu_temp, fan_speed, ignition):
-  global last_ignition
-
-  controller.neg_limit = -(80 if ignition else 30)
-  controller.pos_limit = -(30 if ignition else 0)
-
-  if ignition != last_ignition:
-    controller.reset()
-
-  fan_pwr_out = -int(controller.update(
-                     setpoint=75,
-                     measurement=max_cpu_temp,
-                     feedforward=interp(max_cpu_temp, [60.0, 100.0], [0, -80])
-                  ))
-
-  last_ignition = ignition
-  return fan_pwr_out
-
-
 def set_offroad_alert_if_changed(offroad_alert: str, show_alert: bool, extra_text: Optional[str]=None):
   if prev_offroad_states.get(offroad_alert, None) == (show_alert, extra_text):
     return
@@ -161,19 +97,24 @@ def hw_state_thread(end_event, hw_queue):
   """Handles non critical hardware state, and sends over queue"""
   count = 0
   registered_count = 0
+  prev_hw_state = None
 
   while not end_event.is_set():
     # these are expensive calls. update every 10s
     if (count % int(10. / DT_TRML)) == 0:
       try:
         network_type = HARDWARE.get_network_type()
+        modem_temps = HARDWARE.get_modem_temperatures()
+        if len(modem_temps) == 0 and prev_hw_state is not None:
+          modem_temps = prev_hw_state.modem_temps
 
         hw_state = HardwareState(
           network_type=network_type,
+          network_metered=HARDWARE.get_network_metered(network_type),
           network_strength=HARDWARE.get_network_strength(network_type),
           network_info=HARDWARE.get_network_info(),
           nvme_temps=HARDWARE.get_nvme_temperatures(),
-          modem_temps=HARDWARE.get_modem_temperatures(),
+          modem_temps=modem_temps,
         )
 
         try:
@@ -191,8 +132,9 @@ def hw_state_thread(end_event, hw_queue):
           os.system("nmcli conn up lte")
           registered_count = 0
 
+        prev_hw_state = hw_state
       except Exception:
-        cloudlog.exception("Error getting network status")
+        cloudlog.exception("Error getting hardware state")
 
     count += 1
     time.sleep(DT_TRML)
@@ -202,7 +144,6 @@ def thermald_thread(end_event, hw_queue):
   pm = messaging.PubMaster(['deviceState'])
   sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "controlsState", "pandaStates"], poll=["pandaStates"])
 
-  fan_speed = 0
   count = 0
 
   onroad_conditions: Dict[str, bool] = {
@@ -219,6 +160,7 @@ def thermald_thread(end_event, hw_queue):
 
   last_hw_state = HardwareState(
     network_type=NetworkType.none,
+    network_metered=False,
     network_strength=NetworkStrength.unknown,
     network_info=None,
     nvme_temps=[],
@@ -229,7 +171,6 @@ def thermald_thread(end_event, hw_queue):
   temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML)
   should_start_prev = False
   in_car = False
-  handle_fan = None
   is_uno = False
   engaged_prev = False
 
@@ -239,8 +180,7 @@ def thermald_thread(end_event, hw_queue):
   HARDWARE.initialize_hardware()
   thermal_config = HARDWARE.get_thermal_config()
 
-  # TODO: use PI controller for UNO
-  controller = PIController(k_p=0, k_i=2e-3, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML))
+  fan_controller = None
 
   while not end_event.is_set():
     sm.update(PANDA_STATES_TIMEOUT)
@@ -261,19 +201,15 @@ def thermald_thread(end_event, hw_queue):
       usb_power = peripheralState.usbPowerMode != log.PeripheralState.UsbPowerMode.client
 
       # Setup fan handler on first connect to panda
-      if handle_fan is None and peripheralState.pandaType != log.PandaState.PandaType.unknown:
+      if fan_controller is None and peripheralState.pandaType != log.PandaState.PandaType.unknown:
         is_uno = peripheralState.pandaType == log.PandaState.PandaType.uno
 
         if TICI:
-          cloudlog.info("Setting up TICI fan handler")
-          handle_fan = handle_fan_tici
+          fan_controller = TiciFanController()
         elif is_uno or PC:
-          cloudlog.info("Setting up UNO fan handler")
-          handle_fan = handle_fan_uno
+          fan_controller = UnoFanController()
         else:
-          cloudlog.info("Setting up EON fan handler")
-          setup_eon_fan()
-          handle_fan = handle_fan_eon
+          fan_controller = EonFanController()
 
     try:
       last_hw_state = hw_queue.get_nowait()
@@ -286,6 +222,7 @@ def thermald_thread(end_event, hw_queue):
     msg.deviceState.gpuUsagePercent = int(round(HARDWARE.get_gpu_usage_percent()))
 
     msg.deviceState.networkType = last_hw_state.network_type
+    msg.deviceState.networkMetered = last_hw_state.network_metered
     msg.deviceState.networkStrength = last_hw_state.network_strength
     if last_hw_state.network_info is not None:
       msg.deviceState.networkInfo = last_hw_state.network_info
@@ -303,9 +240,8 @@ def thermald_thread(end_event, hw_queue):
       max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC))
     )
 
-    if handle_fan is not None:
-      fan_speed = handle_fan(controller, max_comp_temp, fan_speed, onroad_conditions["ignition"])
-      msg.deviceState.fanSpeedPercentDesired = fan_speed
+    if fan_controller is not None:
+      msg.deviceState.fanSpeedPercentDesired = fan_controller.update(max_comp_temp, onroad_conditions["ignition"])
 
     is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (sec_since_boot() - off_ts > 60 * 5))
     if is_offroad_for_5_min and max_comp_temp > OFFROAD_DANGER_TEMP:
diff --git a/selfdrive/ui/installer/installer.cc b/selfdrive/ui/installer/installer.cc
index 6ee6e99e09..5366a0eac6 100644
--- a/selfdrive/ui/installer/installer.cc
+++ b/selfdrive/ui/installer/installer.cc
@@ -221,7 +221,7 @@ void Installer::cloneFinished(int exitCode, QProcess::ExitStatus exitStatus) {
 }
 
 int main(int argc, char *argv[]) {
-  initApp();
+  initApp(argc, argv);
   QApplication a(argc, argv);
   Installer installer;
   setMainWindow(&installer);
diff --git a/selfdrive/ui/main.cc b/selfdrive/ui/main.cc
index 3d2411b9c6..8477bc8966 100644
--- a/selfdrive/ui/main.cc
+++ b/selfdrive/ui/main.cc
@@ -12,7 +12,7 @@ int main(int argc, char *argv[]) {
   setpriority(PRIO_PROCESS, 0, -20);
 
   qInstallMessageHandler(swagLogMessageHandler);
-  initApp();
+  initApp(argc, argv);
 
   if (Hardware::EON()) {
     QSslConfiguration ssl = QSslConfiguration::defaultConfiguration();
diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc
index 3ab5b999b2..c17d2e2573 100644
--- a/selfdrive/ui/qt/offroad/driverview.cc
+++ b/selfdrive/ui/qt/offroad/driverview.cc
@@ -12,7 +12,7 @@ DriverViewWindow::DriverViewWindow(QWidget* parent) : QWidget(parent) {
   layout = new QStackedLayout(this);
   layout->setStackingMode(QStackedLayout::StackAll);
 
-  cameraView = new CameraViewWidget("camerad", VISION_STREAM_RGB_FRONT, true, this);
+  cameraView = new CameraViewWidget("camerad", VISION_STREAM_RGB_DRIVER, true, this);
   layout->addWidget(cameraView);
 
   scene = new DriverViewScene(this);
diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc
index 44dde85817..e8d871aae1 100644
--- a/selfdrive/ui/qt/onroad.cc
+++ b/selfdrive/ui/qt/onroad.cc
@@ -20,7 +20,7 @@ OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) {
 
   QStackedLayout *road_view_layout = new QStackedLayout;
   road_view_layout->setStackingMode(QStackedLayout::StackAll);
-  nvg = new NvgWindow(VISION_STREAM_RGB_BACK, this);
+  nvg = new NvgWindow(VISION_STREAM_RGB_ROAD, this);
   road_view_layout->addWidget(nvg);
   hud = new OnroadHud(this);
   road_view_layout->addWidget(hud);
@@ -97,7 +97,7 @@ void OnroadWindow::offroadTransition(bool offroad) {
 
   // update stream type
   bool wide_cam = Hardware::TICI() && Params().getBool("EnableWideCamera");
-  nvg->setStreamType(wide_cam ? VISION_STREAM_RGB_WIDE : VISION_STREAM_RGB_BACK);
+  nvg->setStreamType(wide_cam ? VISION_STREAM_RGB_WIDE_ROAD : VISION_STREAM_RGB_ROAD);
 }
 
 void OnroadWindow::paintEvent(QPaintEvent *event) {
@@ -222,7 +222,7 @@ void OnroadHud::paintEvent(QPaintEvent *event) {
   configFont(p, "Open Sans", 48, "Regular");
   drawText(p, rc.center().x(), 118, "MAX", is_cruise_set ? 200 : 100);
   if (is_cruise_set) {
-    configFont(p, "Open Sans", 88, is_cruise_set ? "Bold" : "SemiBold");
+    configFont(p, "Open Sans", 88, "Bold");
     drawText(p, rc.center().x(), 212, maxSpeed, 255);
   } else {
     configFont(p, "Open Sans", 80, "SemiBold");
@@ -314,8 +314,8 @@ void NvgWindow::drawLaneLines(QPainter &painter, const UIScene &scene) {
   }
   // paint path
   QLinearGradient bg(0, height(), 0, height() / 4);
-  bg.setColorAt(0, scene.end_to_end ? redColor() : QColor(255, 255, 255));
-  bg.setColorAt(1, scene.end_to_end ? redColor(0) : QColor(255, 255, 255, 0));
+  bg.setColorAt(0, scene.end_to_end ? redColor() : whiteColor());
+  bg.setColorAt(1, scene.end_to_end ? redColor(0) : whiteColor(0));
   painter.setBrush(bg);
   painter.drawPolygon(scene.track_vertices.v, scene.track_vertices.cnt);
 }
diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h
index 05811c5060..61da7698c1 100644
--- a/selfdrive/ui/qt/onroad.h
+++ b/selfdrive/ui/qt/onroad.h
@@ -76,6 +76,7 @@ protected:
   void drawLaneLines(QPainter &painter, const UIScene &scene);
   void drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd);
   inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); }
+  inline QColor whiteColor(int alpha = 255) { return QColor(255, 255, 255, alpha); }
   double prev_draw_t = 0;
 };
 
diff --git a/selfdrive/ui/qt/setup/updater.cc b/selfdrive/ui/qt/setup/updater.cc
index dfa63fca11..6345d80db8 100644
--- a/selfdrive/ui/qt/setup/updater.cc
+++ b/selfdrive/ui/qt/setup/updater.cc
@@ -190,7 +190,7 @@ bool Updater::eventFilter(QObject *obj, QEvent *event) {
 }
 
 int main(int argc, char *argv[]) {
-  initApp();
+  initApp(argc, argv);
   QApplication a(argc, argv);
   Updater updater(argv[1], argv[2]);
   setMainWindow(&updater);
diff --git a/selfdrive/ui/qt/spinner.cc b/selfdrive/ui/qt/spinner.cc
index b9868f6035..e0b263227b 100644
--- a/selfdrive/ui/qt/spinner.cc
+++ b/selfdrive/ui/qt/spinner.cc
@@ -111,7 +111,7 @@ void Spinner::update(int n) {
 }
 
 int main(int argc, char *argv[]) {
-  initApp();
+  initApp(argc, argv);
   QApplication a(argc, argv);
   Spinner spinner;
   setMainWindow(&spinner);
diff --git a/selfdrive/ui/qt/text.cc b/selfdrive/ui/qt/text.cc
index cb69cc082b..04fda35483 100644
--- a/selfdrive/ui/qt/text.cc
+++ b/selfdrive/ui/qt/text.cc
@@ -11,7 +11,7 @@
 #include "selfdrive/ui/qt/widgets/scrollview.h"
 
 int main(int argc, char *argv[]) {
-  initApp();
+  initApp(argc, argv);
   QApplication a(argc, argv);
   QWidget window;
   setMainWindow(&window);
diff --git a/selfdrive/ui/qt/util.cc b/selfdrive/ui/qt/util.cc
index 16d5c174b5..967b7d15d1 100644
--- a/selfdrive/ui/qt/util.cc
+++ b/selfdrive/ui/qt/util.cc
@@ -89,9 +89,18 @@ void setQtSurfaceFormat() {
   QSurfaceFormat::setDefaultFormat(fmt);
 }
 
-void initApp() {
+void initApp(int argc, char *argv[]) {
   Hardware::set_display_power(true);
   Hardware::set_brightness(65);
+
+#ifdef __APPLE__
+  {
+    // Get the devicePixelRatio, and scale accordingly to maintain 1:1 rendering
+    QApplication tmp(argc, argv);
+    qputenv("QT_SCALE_FACTOR", QString::number(1.0 / tmp.devicePixelRatio() ).toLocal8Bit());
+  }
+#endif
+
   setQtSurfaceFormat();
   if (Hardware::EON()) {
     QApplication::setAttribute(Qt::AA_ShareOpenGLContexts);
diff --git a/selfdrive/ui/qt/util.h b/selfdrive/ui/qt/util.h
index 1b9461fabf..813777710a 100644
--- a/selfdrive/ui/qt/util.h
+++ b/selfdrive/ui/qt/util.h
@@ -19,6 +19,6 @@ void clearLayout(QLayout* layout);
 void setQtSurfaceFormat();
 QString timeAgo(const QDateTime &date);
 void swagLogMessageHandler(QtMsgType type, const QMessageLogContext &context, const QString &msg);
-void initApp();
+void initApp(int argc, char *argv[]);
 QWidget* topWidget (QWidget* widget);
 QPixmap loadPixmap(const QString &fileName, const QSize &size = {}, Qt::AspectRatioMode aspectRatioMode = Qt::KeepAspectRatio);
diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc
index fbd425b025..ed40094f73 100644
--- a/selfdrive/ui/qt/widgets/cameraview.cc
+++ b/selfdrive/ui/qt/widgets/cameraview.cc
@@ -123,7 +123,7 @@ void CameraViewWidget::initializeGL() {
   GLint frame_pos_loc = program->attributeLocation("aPosition");
   GLint frame_texcoord_loc = program->attributeLocation("aTexCoord");
 
-  auto [x1, x2, y1, y2] = stream_type == VISION_STREAM_RGB_FRONT ? std::tuple(0.f, 1.f, 1.f, 0.f) : std::tuple(1.f, 0.f, 1.f, 0.f);
+  auto [x1, x2, y1, y2] = stream_type == VISION_STREAM_RGB_DRIVER ? std::tuple(0.f, 1.f, 1.f, 0.f) : std::tuple(1.f, 0.f, 1.f, 0.f);
   const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3};
   const float frame_coords[4][4] = {
     {-1.0, -1.0, x2, y1}, // bl
@@ -171,12 +171,12 @@ void CameraViewWidget::hideEvent(QHideEvent *event) {
 
 void CameraViewWidget::updateFrameMat(int w, int h) {
   if (zoomed_view) {
-    if (stream_type == VISION_STREAM_RGB_FRONT) {
+    if (stream_type == VISION_STREAM_RGB_DRIVER) {
       frame_mat = matmul(device_transform, get_driver_view_transform(w, h, stream_width, stream_height));
     } else {
-      auto intrinsic_matrix = stream_type == VISION_STREAM_RGB_WIDE ? ecam_intrinsic_matrix : fcam_intrinsic_matrix;
+      auto intrinsic_matrix = stream_type == VISION_STREAM_RGB_WIDE_ROAD ? ecam_intrinsic_matrix : fcam_intrinsic_matrix;
       float zoom = ZOOM / intrinsic_matrix.v[0];
-      if (stream_type == VISION_STREAM_RGB_WIDE) {
+      if (stream_type == VISION_STREAM_RGB_WIDE_ROAD) {
         zoom *= 0.5;
       }
       float zx = zoom * 2 * intrinsic_matrix.v[2] / width();
diff --git a/selfdrive/ui/replay/camera.h b/selfdrive/ui/replay/camera.h
index 340a120e8c..9d72094d4c 100644
--- a/selfdrive/ui/replay/camera.h
+++ b/selfdrive/ui/replay/camera.h
@@ -32,9 +32,9 @@ protected:
   void cameraThread(Camera &cam);
 
   Camera cameras_[MAX_CAMERAS] = {
-      {.type = RoadCam, .rgb_type = VISION_STREAM_RGB_BACK, .yuv_type = VISION_STREAM_ROAD},
-      {.type = DriverCam, .rgb_type = VISION_STREAM_RGB_FRONT, .yuv_type = VISION_STREAM_DRIVER},
-      {.type = WideRoadCam, .rgb_type = VISION_STREAM_RGB_WIDE, .yuv_type = VISION_STREAM_WIDE_ROAD},
+      {.type = RoadCam, .rgb_type = VISION_STREAM_RGB_ROAD, .yuv_type = VISION_STREAM_ROAD},
+      {.type = DriverCam, .rgb_type = VISION_STREAM_RGB_DRIVER, .yuv_type = VISION_STREAM_DRIVER},
+      {.type = WideRoadCam, .rgb_type = VISION_STREAM_RGB_WIDE_ROAD, .yuv_type = VISION_STREAM_WIDE_ROAD},
   };
   std::atomic<int> publishing_ = 0;
   std::unique_ptr<VisionIpcServer> vipc_server_;
diff --git a/selfdrive/ui/watch3.cc b/selfdrive/ui/watch3.cc
index aeabfe110e..74c00fe18e 100644
--- a/selfdrive/ui/watch3.cc
+++ b/selfdrive/ui/watch3.cc
@@ -6,7 +6,7 @@
 #include "selfdrive/ui/qt/widgets/cameraview.h"
 
 int main(int argc, char *argv[]) {
-  setQtSurfaceFormat();
+  initApp(argc, argv);
 
   QApplication a(argc, argv);
   QWidget w;
@@ -20,14 +20,14 @@ int main(int argc, char *argv[]) {
     QHBoxLayout *hlayout = new QHBoxLayout();
     layout->addLayout(hlayout);
     hlayout->addWidget(new CameraViewWidget("navd", VISION_STREAM_RGB_MAP, false));
-    hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_RGB_BACK, false));
+    hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_RGB_ROAD, false));
   }
 
   {
     QHBoxLayout *hlayout = new QHBoxLayout();
     layout->addLayout(hlayout);
-    hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_RGB_FRONT, false));
-    hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_RGB_WIDE, false));
+    hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_RGB_DRIVER, false));
+    hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_RGB_WIDE_ROAD, false));
   }
 
   return a.exec();
diff --git a/third_party/acados/aarch64/lib/libacados.so b/third_party/acados/aarch64/lib/libacados.so
index fe961d8c50..640de2c2ac 100755
Binary files a/third_party/acados/aarch64/lib/libacados.so and b/third_party/acados/aarch64/lib/libacados.so differ
diff --git a/third_party/acados/aarch64/lib/libblasfeo.so b/third_party/acados/aarch64/lib/libblasfeo.so
index a59f3f79f8..fae141e9fc 100755
Binary files a/third_party/acados/aarch64/lib/libblasfeo.so and b/third_party/acados/aarch64/lib/libblasfeo.so differ
diff --git a/third_party/acados/build.sh b/third_party/acados/build.sh
index 9216032e98..96aa23419b 100755
--- a/third_party/acados/build.sh
+++ b/third_party/acados/build.sh
@@ -13,19 +13,19 @@ elif [ -f /EON ]; then
 fi
 
 if [ ! -d acados_repo/ ]; then
-  #git clone https://github.com/acados/acados.git $DIR/acados
-  git clone https://github.com/commaai/acados.git $DIR/acados_repo
+  git clone https://github.com/acados/acados.git $DIR/acados_repo
+  # git clone https://github.com/commaai/acados.git $DIR/acados_repo
 fi
 cd acados_repo
 git fetch
-git checkout 79e9e3e76f2751198858adf382c97837833ad31f
+git checkout 105e06df87f06ea02df4af825867c946b31defdd
 git submodule update --recursive --init
 
 # build
 mkdir -p build
 cd build
 cmake -DACADOS_WITH_QPOASES=ON -UBLASFEO_TARGET -DBLASFEO_TARGET=$BLAS_TARGET ..
-make -j4 install
+make -j20 install
 
 INSTALL_DIR="$DIR/$ARCHNAME"
 rm -rf $INSTALL_DIR
@@ -33,8 +33,10 @@ mkdir -p $INSTALL_DIR
 
 rm $DIR/acados_repo/lib/*.json
 
+rm -rf $DIR/include
 cp -r $DIR/acados_repo/include $DIR
 cp -r $DIR/acados_repo/lib $INSTALL_DIR
+rm -rf $DIR/../../pyextra/acados_template
 cp -r $DIR/acados_repo/interfaces/acados_template/acados_template $DIR/../../pyextra
 #pip3 install -e $DIR/acados/interfaces/acados_template
 
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h
index 64939b9ed1..4a7f19a07e 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h
@@ -234,11 +234,7 @@ typedef struct ocp_nlp_out
 
     // NOTE: the inequalities are internally organized in the following order:
     // [ lbu lbx lg lh lphi ubu ubx ug uh uphi; lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
-
-    int sqp_iter;
-    int qp_iter;
     double inf_norm_res;
-    double total_time;
 
     void *raw_memory; // Pointer to allocated memory, to be used for freeing
 
@@ -274,11 +270,16 @@ typedef struct ocp_nlp_opts
     double levenberg_marquardt;  // LM factor to be added to the hessian before regularization
     int reuse_workspace;
     int num_threads;
+    int print_level;
 
     // TODO: move to separate struct?
     ocp_nlp_globalization_t globalization;
+    int full_step_dual;
+    int line_search_use_sufficient_descent;
+    int globalization_use_SOC;
     double alpha_min;
     double alpha_reduction;
+    double eps_sufficient_descent;
 } ocp_nlp_opts;
 
 //
@@ -316,6 +317,8 @@ typedef struct ocp_nlp_res
 acados_size_t ocp_nlp_res_calculate_size(ocp_nlp_dims *dims);
 //
 ocp_nlp_res *ocp_nlp_res_assign(ocp_nlp_dims *dims, void *raw_memory);
+//
+void ocp_nlp_res_get_inf_norm(ocp_nlp_res *res, double *out);
 
 /************************************************
  * memory
@@ -351,7 +354,7 @@ typedef struct ocp_nlp_memory
     bool *set_sim_guess; // indicate if there is new explicitly provided guess for integration variables
     struct blasfeo_dvec *sim_guess;
 
-	int *sqp_iter; // pointer to iteration number
+    int *sqp_iter; // pointer to iteration number
 
 } ocp_nlp_memory;
 
@@ -375,8 +378,12 @@ typedef struct ocp_nlp_workspace
     void **cost;         // cost_workspace
     void **constraints;  // constraints_workspace
 
-	ocp_nlp_out *tmp_nlp_out;
-	ocp_nlp_out *weight_merit_fun;
+    // for globalization: -> move to module?!
+    ocp_nlp_out *tmp_nlp_out;
+    ocp_nlp_out *weight_merit_fun;
+    struct blasfeo_dvec tmp_nxu;
+    struct blasfeo_dvec tmp_ni;
+    struct blasfeo_dvec dxnext_dy;
 
 } ocp_nlp_workspace;
 
@@ -392,6 +399,8 @@ ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims
  * function
  ************************************************/
 
+void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
 //
 void ocp_nlp_initialize_qp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
             ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
@@ -409,7 +418,8 @@ void ocp_nlp_update_variables_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, oc
            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work, double alpha);
 //
 double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
-            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work,
+            int check_early_termination);
 //
 double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
           ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h
index 45dac20a25..f2128a8574 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h
@@ -84,6 +84,7 @@ typedef struct
     /* memory */
     acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
     void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
+    // get shooting node gap x_next(x_n, u_n) - x_{n+1}
     struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory_);
     struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory_);
     void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory_);
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h
index 8d2b6ecc13..c085e00d5d 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h
@@ -65,11 +65,6 @@ extern "C" {
  * options
  ************************************************/
 
-typedef struct
-{
-    int dummy;
-} ocp_nlp_reg_noreg_opts;
-
 //
 acados_size_t ocp_nlp_reg_noreg_opts_calculate_size(void);
 //
@@ -84,12 +79,6 @@ void ocp_nlp_reg_noreg_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opt
 /************************************************
  * memory
  ************************************************/
-
-typedef struct
-{
-    int dummy;
-} ocp_nlp_reg_noreg_memory;
-
 //
 acados_size_t ocp_nlp_reg_noreg_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
 //
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h
index 81a4afd030..5e4fc810d9 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h
@@ -68,7 +68,6 @@ typedef struct
     int qp_warm_start;   // qp_warm_start in all but the first sqp iterations
     bool warm_start_first_qp; // to set qp_warm_start in first iteration
     int rti_phase;       // only phase 0 at the moment 
-    int print_level;     // verbosity
     int initialize_t_slacks;  // 0-false or 1-true
 
 } ocp_nlp_sqp_opts;
@@ -107,6 +106,7 @@ typedef struct
     double time_sim;
     double time_sim_la;
     double time_sim_ad;
+    double time_solution_sensitivities;
 
     // statistics
     double *stat;
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h
index 6f16594d2b..af22c06a17 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h
@@ -64,7 +64,6 @@ typedef struct
     int qp_warm_start;        // NOTE: this is not actually setting the warm_start! Just for compatibility with sqp.
     bool warm_start_first_qp; // to set qp_warm_start in first iteration
     int rti_phase;            // phase of RTI. Possible values 1 (preparation), 2 (feedback) 0 (both)
-    int print_level;     // verbosity
 
 } ocp_nlp_sqp_rti_opts;
 
@@ -100,6 +99,7 @@ typedef struct
     double time_reg;
     double time_tot;
     double time_glob;
+    double time_solution_sensitivities;
 
     // statistics
     double *stat;
diff --git a/third_party/acados/include/acados/utils/print.h b/third_party/acados/include/acados/utils/print.h
index 2993447dbd..f8568afb26 100644
--- a/third_party/acados/include/acados/utils/print.h
+++ b/third_party/acados/include/acados/utils/print.h
@@ -69,28 +69,32 @@ void ocp_nlp_out_print(ocp_nlp_dims *dims, ocp_nlp_out *nlp_out);
 void ocp_nlp_res_print(ocp_nlp_dims *dims, ocp_nlp_res *nlp_res);
 
 // ocp qp
+// TODO: move printing routines below that print qp structures to HPIPM!
 void print_ocp_qp_dims(ocp_qp_dims *dims);
 
 // void print_dense_qp_dims(dense_qp_dims *dims);
 
 void print_ocp_qp_in(ocp_qp_in *qp_in);
 
+void print_ocp_qp_in_to_file(FILE *file, ocp_qp_in *qp_in);
+
 void print_ocp_qp_out(ocp_qp_out *qp_out);
 
+void print_ocp_qp_out_to_file(FILE *file, ocp_qp_out *qp_out);
+
+void print_ocp_qp_res(ocp_qp_res *qp_res);
+
+void print_dense_qp_in(dense_qp_in *qp_in);
 // void print_ocp_qp_in_to_string(char string_out[], ocp_qp_in *qp_in);
 
 // void print_ocp_qp_out_to_string(char string_out[], ocp_qp_out *qp_out);
 
-void print_ocp_qp_res(ocp_qp_res *qp_res);
-
 // void print_colmaj_ocp_qp_in(colmaj_ocp_qp_in *qp);
 
 // void print_colmaj_ocp_qp_in_to_file(colmaj_ocp_qp_in *qp);
 
 // void print_colmaj_ocp_qp_out(char *filename, colmaj_ocp_qp_in *qp, colmaj_ocp_qp_out *out);
 
-void print_dense_qp_in(dense_qp_in *qp_in);
-
 void print_qp_info(qp_info *info);
 
 // void acados_warning(char warning_string[]);
diff --git a/third_party/acados/include/acados_c/ocp_nlp_interface.h b/third_party/acados/include/acados_c/ocp_nlp_interface.h
index b6b893024b..0ec82a437a 100644
--- a/third_party/acados/include/acados_c/ocp_nlp_interface.h
+++ b/third_party/acados/include/acados_c/ocp_nlp_interface.h
@@ -107,13 +107,13 @@ typedef enum
 
 
 /// Structure to store the configuration of a non-linear program
-typedef struct ocp_nlp_plan
+typedef struct ocp_nlp_plan_t
 {
     /// QP solver configuration.
-    ocp_qp_solver_plan ocp_qp_solver_plan;
+    ocp_qp_solver_plan_t ocp_qp_solver_plan;
 
     /// Simulation solver configuration for each stage.
-    sim_solver_plan *sim_solver_plan;
+    sim_solver_plan_t *sim_solver_plan;
 
     /// Nlp solver type.
     ocp_nlp_solver_t nlp_solver;
@@ -133,7 +133,7 @@ typedef struct ocp_nlp_plan
     /// Horizon length.
     int N;
 
-} ocp_nlp_plan;
+} ocp_nlp_plan_t;
 
 
 /// Structure to store the state/configuration for the non-linear programming solver
@@ -151,7 +151,7 @@ typedef struct ocp_nlp_solver
 /// default/invalid state.
 ///
 /// \param N Horizon length
-ocp_nlp_plan *ocp_nlp_plan_create(int N);
+ocp_nlp_plan_t *ocp_nlp_plan_create(int N);
 
 /// Destructor for plan struct, frees memory.
 ///
@@ -162,7 +162,7 @@ void ocp_nlp_plan_destroy(void* plan_);
 /// Constructs an nlp configuration struct from a plan.
 ///
 /// \param plan The plan (user nlp configuration).
-ocp_nlp_config *ocp_nlp_config_create(ocp_nlp_plan plan);
+ocp_nlp_config *ocp_nlp_config_create(ocp_nlp_plan_t plan);
 
 /// Desctructor of the nlp configuration.
 ///
@@ -372,6 +372,14 @@ void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *
 //
 void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out);
 
+/// Computes the residuals.
+///
+/// \param solver The solver struct.
+/// \param nlp_in The inputs struct.
+/// \param nlp_out The output struct.
+void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out);
+
+
 //
 void ocp_nlp_eval_param_sens(ocp_nlp_solver *solver, char *field, int stage, int index, ocp_nlp_out *sens_nlp_out);
 
diff --git a/third_party/acados/include/acados_c/ocp_qp_interface.h b/third_party/acados/include/acados_c/ocp_qp_interface.h
index a567ebeb35..2582f142da 100644
--- a/third_party/acados/include/acados_c/ocp_qp_interface.h
+++ b/third_party/acados/include/acados_c/ocp_qp_interface.h
@@ -105,7 +105,7 @@ typedef enum {
 typedef struct
 {
     ocp_qp_solver_t qp_solver;
-} ocp_qp_solver_plan;
+} ocp_qp_solver_plan_t;
 
 
 /// Linear ocp configuration.
@@ -127,7 +127,7 @@ void ocp_qp_xcond_solver_config_initialize_from_plan(
 /// Constructs a qp solver config and Initializes with default values.
 ///
 /// \param plan The qp solver plan struct.
-ocp_qp_xcond_solver_config *ocp_qp_xcond_solver_config_create(ocp_qp_solver_plan plan);
+ocp_qp_xcond_solver_config *ocp_qp_xcond_solver_config_create(ocp_qp_solver_plan_t plan);
 
 /// Destructor for config struct, frees memory.
 ///
diff --git a/third_party/acados/include/acados_c/sim_interface.h b/third_party/acados/include/acados_c/sim_interface.h
index cc47b62ed4..92969e8017 100644
--- a/third_party/acados/include/acados_c/sim_interface.h
+++ b/third_party/acados/include/acados_c/sim_interface.h
@@ -57,7 +57,7 @@ typedef enum
 typedef struct
 {
     sim_solver_t sim_solver;
-} sim_solver_plan;
+} sim_solver_plan_t;
 
 
 
@@ -74,7 +74,7 @@ typedef struct
 
 /* config */
 //
-sim_config *sim_config_create(sim_solver_plan plan);
+sim_config *sim_config_create(sim_solver_plan_t plan);
 //
 void sim_config_destroy(void *config);
 
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_aux.h b/third_party/acados/include/blasfeo/include/blasfeo_d_aux.h
index 7a9415a59d..d4d805c3e4 100644
--- a/third_party/acados/include/blasfeo/include/blasfeo_d_aux.h
+++ b/third_party/acados/include/blasfeo/include/blasfeo_d_aux.h
@@ -185,9 +185,14 @@ void blasfeo_dveccp(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec
 void blasfeo_dvecsc(int m, double alpha, struct blasfeo_dvec *sx, int xi);
 // y <= alpha*x
 void blasfeo_dveccpsc(int m, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi);
+// z[idx] += alpha * x
 void blasfeo_dvecad_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
+// z[idx] <= alpha * x
 void blasfeo_dvecin_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
-void blasfeo_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int x, struct blasfeo_dvec *sz, int zi);
+// z <= alpha * x[idx]
+void blasfeo_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z += alpha * x[idx]
+void blasfeo_dvecexad_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
 
 void blasfeo_dveccl(int m,
 	struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi,
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ref.h b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ref.h
index 448234044a..1e007fa89e 100644
--- a/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ref.h
+++ b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ref.h
@@ -182,6 +182,8 @@ void blasfeo_ref_dveccpsc(int m, double alpha, struct blasfeo_dvec *sx, int xi,
 void blasfeo_ref_dvecad_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
 void blasfeo_ref_dvecin_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
 void blasfeo_ref_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int x, struct blasfeo_dvec *sz, int zi);
+// z += alpha * x[idx]
+void blasfeo_ref_dvecexad_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
 
 void blasfeo_ref_dveccl(int m,
 	struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi,
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_aux.h b/third_party/acados/include/blasfeo/include/blasfeo_s_aux.h
index 539268849c..f43f5e83ce 100644
--- a/third_party/acados/include/blasfeo/include/blasfeo_s_aux.h
+++ b/third_party/acados/include/blasfeo/include/blasfeo_s_aux.h
@@ -127,6 +127,8 @@ float blasfeo_svecex1(struct blasfeo_svec *sx, int xi);
 void blasfeo_svecad_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
 void blasfeo_svecin_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
 void blasfeo_svecex_sp(int m, float alpha, int *idx, struct blasfeo_svec *sx, int x, struct blasfeo_svec *sz, int zi);
+// z += alpha * x[idx]
+void blasfeo_svecexad_sp(int m, double alpha, int *idx, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
 void blasfeo_sveccl(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi);
 void blasfeo_sveccl_mask(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi, struct blasfeo_svec *sm, int mi);
 void blasfeo_svecze(int m, struct blasfeo_svec *sm, int mi, struct blasfeo_svec *sv, int vi, struct blasfeo_svec *se, int ei);
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ref.h b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ref.h
index f6e16a9577..998e1d8999 100644
--- a/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ref.h
+++ b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ref.h
@@ -128,6 +128,8 @@ float blasfeo_ref_svecex1(struct blasfeo_svec *sx, int xi);
 void blasfeo_ref_svecad_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
 void blasfeo_ref_svecin_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
 void blasfeo_ref_svecex_sp(int m, float alpha, int *idx, struct blasfeo_svec *sx, int x, struct blasfeo_svec *sz, int zi);
+// z += alpha * x[idx]
+void blasfeo_ref_svecexad_sp(int m, double alpha, int *idx, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
 void blasfeo_ref_sveccl(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi);
 void blasfeo_ref_sveccl_mask(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi, struct blasfeo_svec *sm, int mi);
 void blasfeo_ref_svecze(int m, struct blasfeo_svec *sm, int mi, struct blasfeo_svec *sv, int vi, struct blasfeo_svec *se, int ei);
diff --git a/third_party/acados/larch64/lib/libacados.so b/third_party/acados/larch64/lib/libacados.so
index 1f2b1f5c0b..c9448d9632 100644
Binary files a/third_party/acados/larch64/lib/libacados.so and b/third_party/acados/larch64/lib/libacados.so differ
diff --git a/third_party/acados/larch64/lib/libblasfeo.so b/third_party/acados/larch64/lib/libblasfeo.so
index 790282fa19..583769e982 100644
Binary files a/third_party/acados/larch64/lib/libblasfeo.so and b/third_party/acados/larch64/lib/libblasfeo.so differ
diff --git a/third_party/acados/x86_64/lib/libacados.so b/third_party/acados/x86_64/lib/libacados.so
index 00a5e2371a..5c3203b553 100644
Binary files a/third_party/acados/x86_64/lib/libacados.so and b/third_party/acados/x86_64/lib/libacados.so differ
diff --git a/third_party/acados/x86_64/lib/libblasfeo.so b/third_party/acados/x86_64/lib/libblasfeo.so
index 5a8399287a..262cf9f12a 100644
Binary files a/third_party/acados/x86_64/lib/libblasfeo.so and b/third_party/acados/x86_64/lib/libblasfeo.so differ
diff --git a/third_party/acados/x86_64/lib/libhpipm.so b/third_party/acados/x86_64/lib/libhpipm.so
index f52091d931..48421183fd 100644
Binary files a/third_party/acados/x86_64/lib/libhpipm.so and b/third_party/acados/x86_64/lib/libhpipm.so differ
diff --git a/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1 b/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1
index 6b553554fd..6aa7b4f8b5 100644
Binary files a/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1 and b/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1 differ
diff --git a/third_party/acados/x86_64/t_renderer b/third_party/acados/x86_64/t_renderer
index 181ef9c3d2..5e7871bf8a 100755
Binary files a/third_party/acados/x86_64/t_renderer and b/third_party/acados/x86_64/t_renderer differ
diff --git a/tools/camerastream/receive.py b/tools/camerastream/receive.py
new file mode 100755
index 0000000000..6f8d67c78f
--- /dev/null
+++ b/tools/camerastream/receive.py
@@ -0,0 +1,44 @@
+#!/usr/bin/env python
+import os
+import sys
+import numpy as np
+os.environ['ZMQ'] = '1'
+
+from common.window import Window
+import cereal.messaging as messaging
+
+# start camerad with 'SEND_ROAD=1 SEND_DRIVER=1 SEND_WIDE_ROAD=1 XMIN=771 XMAX=1156 YMIN=483 YMAX=724 ./camerad'
+# also start bridge
+# then run this "./receive.py <ip>"
+
+if "FULL" in os.environ:
+  SCALE = 2
+  XMIN, XMAX = 0, 1927
+  YMIN, YMAX = 0, 1207
+else:
+  SCALE = 1
+  XMIN = 771
+  XMAX = 1156
+  YMIN = 483
+  YMAX = 724
+H, W = ((YMAX-YMIN+1)//SCALE, (XMAX-XMIN+1)//SCALE)
+
+if __name__ == '__main__':
+  cameras = ['roadCameraState', 'wideRoadCameraState', 'driverCameraState']
+  if "CAM" in os.environ:
+    cam = int(os.environ['CAM'])
+    cameras = cameras[cam:cam+1]
+  sm = messaging.SubMaster(cameras, addr=sys.argv[1])
+  win = Window(W*len(cameras), H)
+  bdat = np.zeros((H, W*len(cameras), 3), dtype=np.uint8)
+
+  while 1:
+    sm.update()
+    for i,k in enumerate(cameras):
+      if sm.updated[k]:
+        #print("update", k)
+        bgr_raw = sm[k].image
+        dat = np.frombuffer(bgr_raw, dtype=np.uint8).reshape(H, W, 3)[:, :, [2,1,0]]
+        bdat[:, W*i:W*(i+1)] = dat
+    win.draw(bdat)
+
diff --git a/tools/lib/README.md b/tools/lib/README.md
index c681809eae..241232eae2 100644
--- a/tools/lib/README.md
+++ b/tools/lib/README.md
@@ -31,3 +31,21 @@ for msg in lr:
   if msg.which() == "carState":
     print(msg.carState.steeringAngleDeg)
 ```
+
+### MultiLogIterator
+
+`MultiLogIterator` is similar to `LogReader`, but reads multiple logs. 
+
+```python
+from tools.lib.route import Route
+from tools.lib.logreader import MultiLogIterator
+
+# setup a MultiLogIterator to read all the logs in the route
+r = Route("4cf7a6ad03080c90|2021-09-29--13-46-36")
+lr = MultiLogIterator(r.log_paths())
+
+# print all the steering angles values from all the logs in the route
+for msg in lr:
+  if msg.which() == "carState":
+    print(msg.carState.steeringAngleDeg)
+```
diff --git a/tools/lib/logreader.py b/tools/lib/logreader.py
index c8d506b4b3..5597a5ffda 100755
--- a/tools/lib/logreader.py
+++ b/tools/lib/logreader.py
@@ -5,8 +5,9 @@ import bz2
 import urllib.parse
 import capnp
 
-from tools.lib.filereader import FileReader
 from cereal import log as capnp_log
+from tools.lib.filereader import FileReader
+from tools.lib.route import Route, SegmentName
 
 # this is an iterator itself, and uses private variables from LogReader
 class MultiLogIterator:
@@ -99,6 +100,16 @@ class LogReader:
       else:
         yield ent
 
+
+def logreader_from_route_or_segment(r):
+  sn = SegmentName(r, allow_route_name=True)
+  route = Route(sn.route_name.canonical_name)
+  if sn.segment_num < 0:
+    return MultiLogIterator(route.log_paths())
+  else:
+    return LogReader(route.log_paths()[sn.segment_num])
+
+
 if __name__ == "__main__":
   import codecs
   # capnproto <= 0.8.0 throws errors converting byte data to string
diff --git a/tools/mac_setup.sh b/tools/mac_setup.sh
index 9140cc4335..b85981dc04 100755
--- a/tools/mac_setup.sh
+++ b/tools/mac_setup.sh
@@ -30,6 +30,7 @@ fi
 
 # TODO: remove protobuf,protobuf-c,swig when casadi can be pip installed
 brew bundle --file=- <<-EOS
+brew "catch2"
 brew "cmake"
 brew "cppcheck"
 brew "git-lfs"
diff --git a/tools/plotjuggler/README.md b/tools/plotjuggler/README.md
index 2e79aecbca..cd3b358e31 100644
--- a/tools/plotjuggler/README.md
+++ b/tools/plotjuggler/README.md
@@ -12,8 +12,7 @@ Once you've cloned and are in openpilot, this command will download PlotJuggler
 
 ```
 $ ./juggle.py -h
-usage: juggle.py [-h] [--demo] [--qlog] [--can] [--stream] [--layout [LAYOUT]] [--install]
-                 [route_or_segment_name] [segment_count]
+usage: juggle.py [-h] [--demo] [--qlog] [--can] [--stream] [--layout [LAYOUT]] [--install] [--dbc DBC] [route_or_segment_name] [segment_count]
 
 A helper to run PlotJuggler on openpilot routes
 
@@ -23,13 +22,15 @@ positional arguments:
   segment_count         The number of segments to plot (default: None)
 
 optional arguments:
-  -h, --help         show this help message and exit
-  --demo             Use the demo route instead of providing one (default: False)
-  --qlog             Use qlogs (default: False)
-  --can              Parse CAN data (default: False)
-  --stream           Start PlotJuggler in streaming mode (default: False)
-  --layout [LAYOUT]  Run PlotJuggler with a pre-defined layout (default: None)
-  --install          Install or update PlotJuggler + plugins (default: False)
+  -h, --help            show this help message and exit
+  --demo                Use the demo route instead of providing one (default: False)
+  --qlog                Use qlogs (default: False)
+  --can                 Parse CAN data (default: False)
+  --stream              Start PlotJuggler in streaming mode (default: False)
+  --layout [LAYOUT]     Run PlotJuggler with a pre-defined layout (default: None)
+  --install             Install or update PlotJuggler + plugins (default: False)
+  --dbc DBC             Set the DBC name to load for parsing CAN data. If not set, the DBC will be
+                        automatically inferred from the logs. (default: None)
 ```
 
 Examples using route name:
diff --git a/tools/plotjuggler/juggle.py b/tools/plotjuggler/juggle.py
index af69bd7c8b..6f7be73490 100755
--- a/tools/plotjuggler/juggle.py
+++ b/tools/plotjuggler/juggle.py
@@ -75,7 +75,7 @@ def start_juggler(fn=None, dbc=None, layout=None):
   subprocess.call(cmd, shell=True, env=env, cwd=juggle_dir)
 
 
-def juggle_route(route_or_segment_name, segment_count, qlog, can, layout):
+def juggle_route(route_or_segment_name, segment_count, qlog, can, layout, dbc=None):
   segment_start = 0
   if 'cabana' in route_or_segment_name:
     query = parse_qs(urlparse(route_or_segment_name).query)
@@ -113,14 +113,14 @@ def juggle_route(route_or_segment_name, segment_count, qlog, can, layout):
     all_data = [d for d in all_data if d.which() not in ['can', 'sendcan']]
 
   # Infer DBC name from logs
-  dbc = None
-  for cp in [m for m in all_data if m.which() == 'carParams']:
-    try:
-      DBC = __import__(f"selfdrive.car.{cp.carParams.carName}.values", fromlist=['DBC']).DBC
-      dbc = DBC[cp.carParams.carFingerprint]['pt']
-    except Exception:
-      pass
-    break
+  if dbc is None:
+    for cp in [m for m in all_data if m.which() == 'carParams']:
+      try:
+        DBC = __import__(f"selfdrive.car.{cp.carParams.carName}.values", fromlist=['DBC']).DBC
+        dbc = DBC[cp.carParams.carFingerprint]['pt']
+      except Exception:
+        pass
+      break
 
   with tempfile.NamedTemporaryFile(suffix='.rlog', dir=juggle_dir) as tmp:
     save_log(tmp.name, all_data, compress=False)
@@ -138,6 +138,7 @@ if __name__ == "__main__":
   parser.add_argument("--stream", action="store_true", help="Start PlotJuggler in streaming mode")
   parser.add_argument("--layout", nargs='?', help="Run PlotJuggler with a pre-defined layout")
   parser.add_argument("--install", action="store_true", help="Install or update PlotJuggler + plugins")
+  parser.add_argument("--dbc", help="Set the DBC name to load for parsing CAN data. If not set, the DBC will be automatically inferred from the logs.")
   parser.add_argument("route_or_segment_name", nargs='?', help="The route or segment name to plot (cabana share URL accepted)")
   parser.add_argument("segment_count", type=int, nargs='?', help="The number of segments to plot")
 
@@ -158,4 +159,4 @@ if __name__ == "__main__":
     start_juggler(layout=args.layout)
   else:
     route_or_segment_name = DEMO_ROUTE if args.demo else args.route_or_segment_name.strip()
-    juggle_route(route_or_segment_name, args.segment_count, args.qlog, args.can, args.layout)
+    juggle_route(route_or_segment_name, args.segment_count, args.qlog, args.can, args.layout, args.dbc)
diff --git a/tools/replay/lib/ui_helpers.py b/tools/replay/lib/ui_helpers.py
index 33a1e77e05..d66fe79306 100644
--- a/tools/replay/lib/ui_helpers.py
+++ b/tools/replay/lib/ui_helpers.py
@@ -10,8 +10,7 @@ from matplotlib.backends.backend_agg import FigureCanvasAgg
 from common.transformations.camera import (eon_f_frame_size, eon_f_focal_length,
                                            tici_f_frame_size, tici_f_focal_length,
                                            get_view_frame_from_calib_frame)
-from selfdrive.config import UIParams as UP
-from selfdrive.config import RADAR_TO_CAMERA
+from selfdrive.controls.lib.radar_helpers import RADAR_TO_CAMERA
 
 
 RED = (255, 0, 0)
@@ -24,6 +23,15 @@ WHITE = (255, 255, 255)
 _FULL_FRAME_SIZE = {
 }
 
+class UIParams:
+  lidar_x, lidar_y, lidar_zoom = 384, 960, 6
+  lidar_car_x, lidar_car_y = lidar_x / 2., lidar_y / 1.1
+  car_hwidth = 1.7272 / 2 * lidar_zoom
+  car_front = 2.6924 * lidar_zoom
+  car_back = 1.8796 * lidar_zoom
+  car_color = 110
+UP = UIParams
+
 _BB_TO_FULL_FRAME = {}
 _CALIB_BB_TO_FULL = {}
 _FULL_FRAME_TO_BB = {}
diff --git a/tools/replay/ui.py b/tools/replay/ui.py
index b39377ffdc..729d4cd0d8 100755
--- a/tools/replay/ui.py
+++ b/tools/replay/ui.py
@@ -10,8 +10,7 @@ import pygame  # pylint: disable=import-error
 import cereal.messaging as messaging
 from common.numpy_fast import clip
 from common.basedir import BASEDIR
-from selfdrive.config import UIParams as UP
-from tools.replay.lib.ui_helpers import (_BB_TO_FULL_FRAME,
+from tools.replay.lib.ui_helpers import (_BB_TO_FULL_FRAME, UP,
                                          _INTRINSICS, BLACK, GREEN,
                                          YELLOW, Calibration,
                                          get_blank_lid_overlay, init_plots,
@@ -100,7 +99,7 @@ def ui_thread(addr):
 
   draw_plots = init_plots(plot_arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles)
 
-  vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_RGB_BACK, True)
+  vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_RGB_ROAD, True)
   while 1:
     list(pygame.event.get())
 
diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py
index 2e19faefcd..ae6483fd28 100755
--- a/tools/sim/bridge.py
+++ b/tools/sim/bridge.py
@@ -67,7 +67,7 @@ class Camerad:
     self.vipc_server = VisionIpcServer("camerad")
 
     # TODO: remove RGB buffers once the last RGB vipc subscriber is removed
-    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_RGB_BACK, 4, True, W, H)
+    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_RGB_ROAD, 4, True, W, H)
     self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, W, H)
     self.vipc_server.start_listener()
 
@@ -97,7 +97,7 @@ class Camerad:
     eof = self.frame_id * 0.05
 
     # TODO: remove RGB send once the last RGB vipc subscriber is removed
-    self.vipc_server.send(VisionStreamType.VISION_STREAM_RGB_BACK, img.tobytes(), self.frame_id, eof, eof)
+    self.vipc_server.send(VisionStreamType.VISION_STREAM_RGB_ROAD, img.tobytes(), self.frame_id, eof, eof)
     self.vipc_server.send(VisionStreamType.VISION_STREAM_ROAD, yuv.data.tobytes(), self.frame_id, eof, eof)
 
     dat = messaging.new_message('roadCameraState')
diff --git a/update_requirements.sh b/update_requirements.sh
index ac9472dca2..94b14496f1 100755
--- a/update_requirements.sh
+++ b/update_requirements.sh
@@ -1,5 +1,4 @@
 #!/bin/bash
-
 set -e
 
 DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
@@ -45,10 +44,11 @@ echo "pip packages install..."
 pipenv install --dev --deploy --clear
 pyenv rehash
 
-if [ -f "$DIR/.pre-commit-config.yaml" ]; then
-  echo "precommit install ..."
-  $RUN pre-commit install
-  [ -d "./xx" ] && (cd xx && $RUN pre-commit install)
-  [ -d "./notebooks" ] && (cd notebooks && $RUN pre-commit install)
-  echo "pre-commit hooks installed"
-fi
+echo "pre-commit hooks install..."
+shopt -s nullglob
+for f in .pre-commit-config.yaml */.pre-commit-config.yaml; do
+  cd $DIR/$(dirname $f)
+  if [ -e ".git" ]; then
+    $RUN pre-commit install
+  fi
+done