diff --git a/.gitignore b/.gitignore
index 91aecd9a72..e414e4301a 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,6 @@
 venv/
+.venv/
+.ci_cache
 .env
 .clang-format
 .DS_Store
@@ -46,6 +48,7 @@ selfdrive/mapd/default_speeds_by_region.json
 system/proclogd/proclogd
 selfdrive/ui/_ui
 selfdrive/ui/translations/alerts_generated.h
+selfdrive/ui/translations/tmp
 selfdrive/test/longitudinal_maneuvers/out
 selfdrive/car/tests/cars_dump
 system/camerad/camerad
@@ -56,7 +59,6 @@ selfdrive/modeld/_dmonitoringmodeld
 /src/
 
 one
-openpilot
 notebooks
 xx
 yy
@@ -77,6 +79,7 @@ comma*.sh
 
 selfdrive/modeld/thneed/compile
 selfdrive/modeld/models/*.thneed
+selfdrive/modeld/models/*.pkl
 
 *.bz2
 
@@ -85,3 +88,4 @@ build/
 !**/.gitkeep
 
 poetry.toml
+Pipfile
diff --git a/Jenkinsfile b/Jenkinsfile
index 16a42413ea..1e1d485b10 100644
--- a/Jenkinsfile
+++ b/Jenkinsfile
@@ -1,4 +1,4 @@
-def phone(String ip, String step_label, String cmd) {
+def device(String ip, String step_label, String cmd) {
   withCredentials([file(credentialsId: 'id_rsa', variable: 'key_file')]) {
     def ssh_cmd = """
 ssh -tt -o StrictHostKeyChecking=no -i ${key_file} 'comma@${ip}' /usr/bin/bash <<'END'
@@ -6,6 +6,7 @@ ssh -tt -o StrictHostKeyChecking=no -i ${key_file} 'comma@${ip}' /usr/bin/bash <
 set -e
 
 export CI=1
+export PYTHONWARNINGS=error
 export LOGPRINT=debug
 export TEST_DIR=${env.TEST_DIR}
 export SOURCE_DIR=${env.SOURCE_DIR}
@@ -13,6 +14,8 @@ export GIT_BRANCH=${env.GIT_BRANCH}
 export GIT_COMMIT=${env.GIT_COMMIT}
 export AZURE_TOKEN='${env.AZURE_TOKEN}'
 export MAPBOX_TOKEN='${env.MAPBOX_TOKEN}'
+export PYTEST_ADDOPTS="-c selfdrive/test/pytest-tici.ini --rootdir ."
+
 
 export GIT_SSH_COMMAND="ssh -i /data/gitkey"
 
@@ -20,6 +23,8 @@ source ~/.bash_profile
 if [ -f /TICI ]; then
   source /etc/profile
 
+  rm -rf ~/.commacache
+
   if ! systemctl is-active --quiet systemd-resolved; then
     echo "restarting resolved"
     sudo systemctl start systemd-resolved
@@ -50,222 +55,192 @@ END"""
   }
 }
 
-def phone_steps(String device_type, steps) {
-  lock(resource: "", label: device_type, inversePrecedence: true, variable: 'device_ip', quantity: 1) {
-    timeout(time: 20, unit: 'MINUTES') {
-      phone(device_ip, "git checkout", readFile("selfdrive/test/setup_device_ci.sh"),)
-      steps.each { item ->
-        phone(device_ip, item[0], item[1])
-      }
+def deviceStage(String stageName, String deviceType, List env, def steps) {
+  stage(stageName) {
+    if (currentBuild.result != null) {
+        return
     }
-  }
-}
 
-pipeline {
-  agent none
-  environment {
-    CI = "1"
-    TEST_DIR = "/data/openpilot"
-    SOURCE_DIR = "/data/openpilot_source/"
-    AZURE_TOKEN = credentials('azure_token')
-    MAPBOX_TOKEN = credentials('mapbox_token')
-  }
-  options {
-    timeout(time: 3, unit: 'HOURS')
-    disableConcurrentBuilds(abortPrevious: env.BRANCH_NAME != 'master')
-  }
+    def extra = env.collect { "export ${it}" }.join('\n');
 
-  stages {
-    stage('build release3-staging') {
-      agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } }
-      when {
-        branch 'devel-staging'
-      }
-      steps {
-        phone_steps("tici-needs-can", [
-          ["build release3-staging & dashcam3-staging", "RELEASE_BRANCH=release3-staging DASHCAM_BRANCH=dashcam3-staging $SOURCE_DIR/release/build_release.sh"],
-        ])
+    docker.image('ghcr.io/commaai/alpine-ssh').inside('--user=root') {
+      lock(resource: "", label: deviceType, inversePrecedence: true, variable: 'device_ip', quantity: 1) {
+        timeout(time: 20, unit: 'MINUTES') {
+          retry (3) {
+            device(device_ip, "git checkout", extra + "\n" + readFile("selfdrive/test/setup_device_ci.sh"))
+          }
+          steps.each { item ->
+            device(device_ip, item[0], item[1])
+          }
+        }
       }
     }
+  }
+}
 
-    stage('build nightly') {
-      agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } }
-      when {
-        branch 'master-ci'
-      }
-      steps {
-        phone_steps("tici-needs-can", [
-          ["build nightly", "RELEASE_BRANCH=nightly $SOURCE_DIR/release/build_release.sh"],
-        ])
-      }
+def pcStage(String stageName, Closure body) {
+  node {
+  stage(stageName) {
+    if (currentBuild.result != null) {
+        return
     }
 
-    stage('openpilot tests') {
-      when {
-        not {
-          anyOf {
-            branch 'master-ci'; branch 'devel'; branch 'devel-staging';
-            branch 'release3'; branch 'release3-staging'; branch 'dashcam3'; branch 'dashcam3-staging';
-            branch 'testing-closet*'; branch 'hotfix-*'
-          }
+    checkout scm
+
+    def dockerArgs = "--user=batman -v /tmp/comma_download_cache:/tmp/comma_download_cache -v /tmp/scons_cache:/tmp/scons_cache -e PYTHONPATH=${env.WORKSPACE}";
+    docker.build("openpilot-base:build-${env.GIT_COMMIT}", "-f Dockerfile.openpilot_base .").inside(dockerArgs) {
+      timeout(time: 20, unit: 'MINUTES') {
+        try {
+          // TODO: remove these after all jenkins jobs are running as batman (merged with master)
+          sh "sudo chown -R batman:batman /tmp/scons_cache"
+          sh "sudo chown -R batman:batman /tmp/comma_download_cache"
+
+          sh "git config --global --add safe.directory '*'"
+          sh "git submodule update --init --recursive"
+          sh "git lfs pull"
+          body()
+        } finally {
+          sh "rm -rf ${env.WORKSPACE}/* || true"
+          sh "rm -rf .* || true"
         }
       }
+    }
+  }
+  }
+}
 
-      parallel {
-
-        /*
-        stage('simulator') {
-          agent {
-            dockerfile {
-              filename 'Dockerfile.sim_nvidia'
-              dir 'tools/sim'
-              args '--user=root'
-            }
-          }
-          steps {
-            sh "git config --global --add safe.directory '*'"
-            sh "git submodule update --init --recursive"
-            sh "git lfs pull"
-            lock(resource: "", label: "simulator", inversePrecedence: true, quantity: 1) {
-              sh "${WORKSPACE}/tools/sim/build_container.sh"
-              sh "DETACH=1 ${WORKSPACE}/tools/sim/start_carla.sh"
-              sh "${WORKSPACE}/tools/sim/start_openpilot_docker.sh"
-            }
-          }
-
-          post {
-            always {
-              sh "docker kill carla_sim || true"
-              sh "rm -rf ${WORKSPACE}/* || true"
-              sh "rm -rf .* || true"
-            }
-          }
-        }
-        */
-
-        stage('scons build test') {
-          agent {
-            dockerfile {
-              filename 'Dockerfile.openpilot_base'
-              args '--user=root'
-            }
-          }
-          steps {
-            sh "git config --global --add safe.directory '*'"
-            sh "git submodule update --init --depth=1 --recursive"
-            sh "scons --clean && scons --no-cache -j42"
-            sh "scons --clean && scons --no-cache --random -j42"
-          }
+def setupCredentials() {
+  withCredentials([
+    string(credentialsId: 'azure_token', variable: 'AZURE_TOKEN'),
+    string(credentialsId: 'mapbox_token', variable: 'MAPBOX_TOKEN')
+  ]) {
+    env.AZURE_TOKEN = "${AZURE_TOKEN}"
+    env.MAPBOX_TOKEN = "${MAPBOX_TOKEN}"
+  }
+}
 
-          post {
-            always {
-              sh "rm -rf ${WORKSPACE}/* || true"
-              sh "rm -rf .* || true"
-            }
-          }
-        }
+node {
+  env.CI = "1"
+  env.PYTHONWARNINGS = "error"
+  env.TEST_DIR = "/data/openpilot"
+  env.SOURCE_DIR = "/data/openpilot_source/"
+  setupCredentials()
 
-        stage('tizi-tests') {
-          agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } }
-          steps {
-            phone_steps("tizi", [
-              ["build openpilot", "cd selfdrive/manager && ./build.py"],
-              ["test boardd loopback", "SINGLE_PANDA=1 pytest selfdrive/boardd/tests/test_boardd_loopback.py"],
-              ["test pandad", "pytest selfdrive/boardd/tests/test_pandad.py"],
-              ["test sensord", "cd system/sensord/tests && pytest test_sensord.py"],
-              ["test camerad", "pytest system/camerad/test/test_camerad.py"],
-              ["test exposure", "pytest system/camerad/test/test_exposure.py"],
-              ["test amp", "pytest system/hardware/tici/tests/test_amplifier.py"],
-              ["test hw", "pytest system/hardware/tici/tests/test_hardware.py"],
-              ["test rawgpsd", "pytest system/sensord/rawgps/test_rawgps.py"],
-            ])
-          }
-        }
+  env.GIT_BRANCH = checkout(scm).GIT_BRANCH
+  env.GIT_COMMIT = checkout(scm).GIT_COMMIT
 
-        stage('build') {
-          agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } }
-          environment {
-            R3_PUSH = "${env.BRANCH_NAME == 'master' ? '1' : ' '}"
-          }
-          steps {
-            phone_steps("tici-needs-can", [
-              ["build master-ci", "cd $SOURCE_DIR/release && TARGET_DIR=$TEST_DIR EXTRA_FILES='tools/' ./build_devel.sh"],
-              ["build openpilot", "cd selfdrive/manager && ./build.py"],
-              ["check dirty", "release/check-dirty.sh"],
-              ["onroad tests", "cd selfdrive/test/ && ./test_onroad.py"],
-              ["time to onroad", "cd selfdrive/test/ && pytest test_time_to_onroad.py"],
-            ])
-          }
-        }
+  def excludeBranches = ['master-ci', 'devel', 'devel-staging', 'release3', 'release3-staging',
+                         'dashcam3', 'dashcam3-staging', 'testing-closet*', 'hotfix-*']
+  def excludeRegex = excludeBranches.join('|').replaceAll('\\*', '.*')
 
-        stage('loopback-tests') {
-          agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } }
-          steps {
-            phone_steps("tici-loopback", [
-              ["build openpilot", "cd selfdrive/manager && ./build.py"],
-              ["test boardd loopback", "pytest selfdrive/boardd/tests/test_boardd_loopback.py"],
-            ])
-          }
-        }
+  if (env.BRANCH_NAME != 'master') {
+    properties([
+        disableConcurrentBuilds(abortPrevious: true)
+    ])
+  }
 
-        stage('HW + Unit Tests') {
-          agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } }
-          steps {
-            phone_steps("tici-common", [
-              ["build", "cd selfdrive/manager && ./build.py"],
-              ["test pandad", "pytest selfdrive/boardd/tests/test_pandad.py"],
-              ["test power draw", "pytest system/hardware/tici/tests/test_power_draw.py"],
-              ["test loggerd", "pytest system/loggerd/tests/test_loggerd.py"],
-              ["test encoder", "LD_LIBRARY_PATH=/usr/local/lib pytest system/loggerd/tests/test_encoder.py"],
-              ["test pigeond", "pytest system/sensord/tests/test_pigeond.py"],
-              ["test manager", "pytest selfdrive/manager/test/test_manager.py"],
-              ["test nav", "pytest selfdrive/navd/tests/"],
-            ])
-          }
-        }
+  try {
+    if (env.BRANCH_NAME == 'devel-staging') {
+      deviceStage("build release3-staging", "tici-needs-can", [], [
+        ["build release3-staging & dashcam3-staging", "RELEASE_BRANCH=release3-staging DASHCAM_BRANCH=dashcam3-staging $SOURCE_DIR/release/build_release.sh"],
+      ])
+    }
 
-        stage('camerad') {
-          agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } }
-          steps {
-            phone_steps("tici-ar0231", [
-              ["build", "cd selfdrive/manager && ./build.py"],
-              ["test camerad", "pytest system/camerad/test/test_camerad.py"],
-              ["test exposure", "pytest system/camerad/test/test_exposure.py"],
-            ])
-            phone_steps("tici-ox03c10", [
-              ["build", "cd selfdrive/manager && ./build.py"],
-              ["test camerad", "pytest system/camerad/test/test_camerad.py"],
-              ["test exposure", "pytest system/camerad/test/test_exposure.py"],
-            ])
-          }
-        }
+    if (env.BRANCH_NAME == 'master-ci') {
+      deviceStage("build nightly", "tici-needs-can", [], [
+        ["build nightly", "RELEASE_BRANCH=nightly $SOURCE_DIR/release/build_release.sh"],
+      ])
+    }
 
-        stage('sensord') {
-          agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } }
-          steps {
-            phone_steps("tici-lsmc", [
-              ["build", "cd selfdrive/manager && ./build.py"],
-              ["test sensord", "cd system/sensord/tests && pytest test_sensord.py"],
-            ])
-            phone_steps("tici-bmx-lsm", [
-              ["build", "cd selfdrive/manager && ./build.py"],
-              ["test sensord", "cd system/sensord/tests && pytest test_sensord.py"],
-            ])
-          }
+    if (!env.BRANCH_NAME.matches(excludeRegex)) {
+    parallel (
+      // tici tests
+      'onroad tests': {
+        deviceStage("onroad", "tici-needs-can", ["SKIP_COPY=1"], [
+          ["build master-ci", "cd $SOURCE_DIR/release && TARGET_DIR=$TEST_DIR ./build_devel.sh"],
+          ["build openpilot", "cd selfdrive/manager && ./build.py"],
+          ["check dirty", "release/check-dirty.sh"],
+          ["onroad tests", "pytest selfdrive/test/test_onroad.py -s"],
+          ["time to onroad", "pytest selfdrive/test/test_time_to_onroad.py"],
+        ])
+      },
+      'HW + Unit Tests': {
+        deviceStage("tici", "tici-common", ["UNSAFE=1"], [
+          ["build", "cd selfdrive/manager && ./build.py"],
+          ["test pandad", "pytest selfdrive/boardd/tests/test_pandad.py"],
+          ["test power draw", "./system/hardware/tici/tests/test_power_draw.py"],
+          ["test encoder", "LD_LIBRARY_PATH=/usr/local/lib pytest system/loggerd/tests/test_encoder.py"],
+          ["test pigeond", "pytest system/sensord/tests/test_pigeond.py"],
+          ["test manager", "pytest selfdrive/manager/test/test_manager.py"],
+          ["test nav", "pytest selfdrive/navd/tests/"],
+        ])
+      },
+      'loopback': {
+        deviceStage("tici", "tici-loopback", ["UNSAFE=1"], [
+          ["build openpilot", "cd selfdrive/manager && ./build.py"],
+          ["test boardd loopback", "pytest selfdrive/boardd/tests/test_boardd_loopback.py"],
+        ])
+      },
+      'camerad': {
+        deviceStage("AR0231", "tici-ar0231", ["UNSAFE=1"], [
+          ["build", "cd selfdrive/manager && ./build.py"],
+          ["test camerad", "pytest system/camerad/test/test_camerad.py"],
+          ["test exposure", "pytest system/camerad/test/test_exposure.py"],
+        ])
+        deviceStage("OX03C10", "tici-ox03c10", ["UNSAFE=1"], [
+          ["build", "cd selfdrive/manager && ./build.py"],
+          ["test camerad", "pytest system/camerad/test/test_camerad.py"],
+          ["test exposure", "pytest system/camerad/test/test_exposure.py"],
+        ])
+      },
+      'sensord': {
+        deviceStage("LSM + MMC", "tici-lsmc", ["UNSAFE=1"], [
+          ["build", "cd selfdrive/manager && ./build.py"],
+          ["test sensord", "pytest system/sensord/tests/test_sensord.py"],
+        ])
+        deviceStage("BMX + LSM", "tici-bmx-lsm", ["UNSAFE=1"], [
+          ["build", "cd selfdrive/manager && ./build.py"],
+          ["test sensord", "pytest system/sensord/tests/test_sensord.py"],
+        ])
+      },
+      'replay': {
+        deviceStage("tici", "tici-replay", ["UNSAFE=1"], [
+          ["build", "cd selfdrive/manager && ./build.py"],
+          ["model replay", "selfdrive/test/process_replay/model_replay.py"],
+        ])
+      },
+      'tizi': {
+        deviceStage("tizi", "tizi", ["UNSAFE=1"], [
+          ["build openpilot", "cd selfdrive/manager && ./build.py"],
+          ["test boardd loopback", "SINGLE_PANDA=1 pytest selfdrive/boardd/tests/test_boardd_loopback.py"],
+          ["test pandad", "pytest selfdrive/boardd/tests/test_pandad.py"],
+          ["test amp", "pytest system/hardware/tici/tests/test_amplifier.py"],
+          ["test hw", "pytest system/hardware/tici/tests/test_hardware.py"],
+          ["test rawgpsd", "pytest system/sensord/rawgps/test_rawgps.py"],
+        ])
+      },
+
+      // *** PC tests ***
+      'PC tests': {
+        pcStage("PC tests") {
+          // tests that our build system's dependencies are configured properly,
+          // needs a machine with lots of cores
+          sh label: "test multi-threaded build", script: "scons --no-cache --random -j42"
         }
-
-        stage('replay') {
-          agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } }
-          steps {
-            phone_steps("tici-replay", [
-              ["build", "cd selfdrive/manager && ./build.py"],
-              ["model replay", "cd selfdrive/test/process_replay && ./model_replay.py"],
-            ])
-          }
+      },
+      'car tests': {
+        pcStage("car tests") {
+          sh label: "build", script: "selfdrive/manager/build.py"
+          sh label: "test_models.py", script: "INTERNAL_SEG_CNT=250 INTERNAL_SEG_LIST=selfdrive/car/tests/test_models_segs.txt FILEREADER_CACHE=1 \
+              pytest -n42 --dist=loadscope selfdrive/car/tests/test_models.py"
+          sh label: "test_car_interfaces.py", script: "MAX_EXAMPLES=100 pytest -n42 --dist=load selfdrive/car/tests/test_car_interfaces.py"
         }
+      },
 
-      }
+    )
     }
-
+  } catch (Exception e) {
+    currentBuild.result = 'FAILED'
+    throw e
   }
-}
+}
\ No newline at end of file
diff --git a/README.md b/README.md
old mode 100755
new mode 100644
index 5428ea7a95..7d051ada14
--- a/README.md
+++ b/README.md
@@ -1,4 +1,4 @@
-![](https://i.imgur.com/b0ZyIx5.jpg)
+![openpilot on the comma 3X](https://github.com/commaai/openpilot/assets/4038174/f1081737-8718-4241-a22a-3ceba526361a)
 
 Table of Contents
 =======================
@@ -39,13 +39,17 @@ Running on a dedicated device in a car
 ------
 
 To use openpilot in a car, you need four things
-* A supported device to run this software: a [comma three](https://comma.ai/shop/products/three).
-* This software. The setup procedure of the comma three allows the user to enter a URL for custom software.
-The URL, openpilot.comma.ai will install the release version of openpilot. To install openpilot master, you can use installer.comma.ai/commaai/master, and replacing commaai with another GitHub username can install a fork.
-* One of [the 250+ supported cars](docs/CARS.md). We support Honda, Toyota, Hyundai, Nissan, Kia, Chrysler, Lexus, Acura, Audi, VW, Ford and more. If your car is not supported but has adaptive cruise control and lane-keeping assist, it's likely able to run openpilot.
-* A [car harness](https://comma.ai/shop/products/car-harness) to connect to your car.
-
-We have detailed instructions for [how to mount the device in a car](https://comma.ai/setup).
+1. **Supported Device:** A comma 3/3X. You can purchase these devices from (https://comma.ai/shop/comma-3x)
+  
+2. **Software:** The setup procedure for the comma 3/3X allows users to enter a URL for custom software.
+  To install the release version of openpilot, use the URL `openpilot.comma.ai`.
+  To install openpilot master (for more advanced users), use the URL `installer.comma.ai/commaai/master`. You can replace "commaai" with another GitHub username to install a fork.
+
+3. **Supported Car:** Ensure that you have one of [the 250+ supported cars](docs/CARS.md). openpilot supports a wide range of car makes including Honda, Toyota, Hyundai, Nissan, Kia, Chrysler, Lexus, Acura, Audi, VW, Ford, and many more.
+  If your car is not officially listed as supported but has adaptive cruise control and lane-keeping assist, it's likely capable of running openpilot.
+  
+4. **Car Harness:** You will also need a [car harness](https://comma.ai/shop/car-harness) to connect your comma 3/3X to your car.
+  We have detailed instructions for [how to install the harness and device in a car](https://comma.ai/setup).
 
 Running on PC
 ------
@@ -105,7 +109,6 @@ Directory Structure
     ├── third_party         # External libraries
     └── system              # Generic services
         ├── camerad         # Driver to capture images from the camera sensors
-        ├── clocksd         # Broadcasts current time
         ├── hardware        # Hardware abstraction classes
         ├── logcatd         # systemd journal as a service
         ├── loggerd         # Logger and uploader of car data
diff --git a/RELEASES.md b/RELEASES.md
index 58b9593683..1cb6105680 100644
--- a/RELEASES.md
+++ b/RELEASES.md
@@ -1,5 +1,24 @@
+Version 0.9.5 (2023-11-17)
+========================
+* New driving model
+  * Improved navigate on openpilot performance using navigation instructions as an additional model input
+  * Do lateral planning inside the model
+  * New vision transformer architecture
+* Cadillac Escalade ESV 2019 support thanks to twilsonco!
+* Hyundai Azera 2022 support thanks to sunnyhaibin!
+* Hyundai Azera Hybrid 2020 support thanks to chanhojung and haram-KONA!
+* Hyundai Custin 2023 support thanks to sunnyhaibin and Saber422!
+* Hyundai Ioniq 6 2023 support thanks to sunnyhaibin and alamo3!
+* Hyundai Kona Electric 2023 (Korean version) support thanks to sunnyhaibin and haram-KONA!
+* Kia K8 Hybrid (with HDA II) 2023 support thanks to sunnyhaibin!
+* Kia Optima Hybrid 2019 support
+* Kia Sorento Hybrid 2023 support thanks to sunnyhaibin!
+* Lexus GS F 2016 support thanks to snyperifle!
+* Lexus IS 2023 support thanks to L3R5!
+
 Version 0.9.4 (2023-07-27)
 ========================
+* comma 3X support
 * Navigate on openpilot in Experimental mode
   * When navigation has a destination, openpilot will input the map information into the model, which provides useful context to help the model understand the scene
   * When navigating on openpilot, openpilot will keep left or right appropriately at forks and exits
diff --git a/SConstruct b/SConstruct
index ce497e2134..eb9cd737e2 100644
--- a/SConstruct
+++ b/SConstruct
@@ -14,10 +14,6 @@ AGNOS = TICI
 
 Decider('MD5-timestamp')
 
-AddOption('--extras',
-          action='store_true',
-          help='build misc extras, like setup and installer files')
-
 AddOption('--kaitai',
           action='store_true',
           help='Regenerate kaitai struct parsers')
@@ -30,6 +26,10 @@ AddOption('--ubsan',
           action='store_true',
           help='turn on UBSan')
 
+AddOption('--coverage',
+          action='store_true',
+          help='build with test coverage options')
+
 AddOption('--clazy',
           action='store_true',
           help='build with clazy')
@@ -37,6 +37,12 @@ AddOption('--clazy',
 AddOption('--compile_db',
           action='store_true',
           help='build clang compilation database')
+          
+AddOption('--ccflags',
+          action='store',
+          type='string',
+          default='',
+          help='pass arbitrary flags over the command line')
 
 AddOption('--snpe',
           action='store_true',
@@ -48,34 +54,34 @@ AddOption('--external-sconscript',
           dest='external_sconscript',
           help='add an external SConscript to the build')
 
-AddOption('--no-thneed',
-          action='store_true',
-          dest='no_thneed',
-          help='avoid using thneed')
-
 AddOption('--pc-thneed',
           action='store_true',
           dest='pc_thneed',
           help='use thneed on pc')
 
-AddOption('--no-test',
+AddOption('--minimal',
           action='store_false',
-          dest='test',
-          default=os.path.islink(Dir('#laika/').abspath),
-          help='skip building test files')
-
+          dest='extras',
+          default=os.path.islink(Dir('#rednose/').abspath), # minimal by default on release branch (where rednose is not a link)
+          help='the minimum build to run openpilot. no tests, tools, etc.')
+
+## Architecture name breakdown (arch)
+## - larch64: linux tici aarch64
+## - aarch64: linux pc aarch64
+## - x86_64:  linux pc x64
+## - Darwin:  mac x64 or arm64
 real_arch = arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip()
 if platform.system() == "Darwin":
   arch = "Darwin"
   brew_prefix = subprocess.check_output(['brew', '--prefix'], encoding='utf8').strip()
-
-if arch == "aarch64" and AGNOS:
+elif arch == "aarch64" and AGNOS:
   arch = "larch64"
+assert arch in ["larch64", "aarch64", "x86_64", "Darwin"]
 
 lenv = {
   "PATH": os.environ['PATH'],
   "LD_LIBRARY_PATH": [Dir(f"#third_party/acados/{arch}/lib").abspath],
-  "PYTHONPATH": Dir("#").abspath,
+  "PYTHONPATH": Dir("#").abspath + ':' + Dir(f"#third_party/acados").abspath,
 
   "ACADOS_SOURCE_DIR": Dir("#third_party/acados").abspath,
   "ACADOS_PYTHON_INTERFACE_PATH": Dir("#third_party/acados/acados_template").abspath,
@@ -117,9 +123,8 @@ else:
 
   # MacOS
   if arch == "Darwin":
-    yuv_dir = "mac" if real_arch != "arm64" else "mac_arm64"
     libpath = [
-      f"#third_party/libyuv/{yuv_dir}/lib",
+      f"#third_party/libyuv/{arch}/lib",
       f"#third_party/acados/{arch}/lib",
       f"{brew_prefix}/lib",
       f"{brew_prefix}/opt/openssl@3.0/lib",
@@ -133,21 +138,25 @@ else:
       f"{brew_prefix}/opt/openssl@3.0/include",
     ]
     lenv["DYLD_LIBRARY_PATH"] = lenv["LD_LIBRARY_PATH"]
-  # Linux 86_64
+  # Linux
   else:
     libpath = [
-      "#third_party/acados/x86_64/lib",
-      "#third_party/snpe/x86_64-linux-clang",
-      "#third_party/libyuv/x64/lib",
-      "#third_party/mapbox-gl-native-qt/x86_64",
+      f"#third_party/acados/{arch}/lib",
+      f"#third_party/libyuv/{arch}/lib",
+      f"#third_party/mapbox-gl-native-qt/{arch}",
       "#cereal",
       "#common",
       "/usr/lib",
       "/usr/local/lib",
     ]
-    rpath += [
-      Dir("#third_party/snpe/x86_64-linux-clang").abspath,
-    ]
+
+    if arch == "x86_64":
+      libpath += [
+        f"#third_party/snpe/{arch}"
+      ]
+      rpath += [
+        Dir(f"#third_party/snpe/{arch}").abspath,
+      ]
 
 if GetOption('asan'):
   ccflags = ["-fsanitize=address", "-fno-omit-frame-pointer"]
@@ -167,6 +176,10 @@ if arch != "Darwin":
 cflags += ['-DSWAGLOG="\\"common/swaglog.h\\""']
 cxxflags += ['-DSWAGLOG="\\"common/swaglog.h\\""']
 
+ccflags_option = GetOption('ccflags')
+if ccflags_option:
+  ccflags += ccflags_option.split(' ')
+
 env = Environment(
   ENV=lenv,
   CCFLAGS=[
@@ -193,7 +206,6 @@ env = Environment(
     "#third_party/catch2/include",
     "#third_party/libyuv/include",
     "#third_party/json11",
-    "#third_party/curl/include",
     "#third_party/linux/include",
     "#third_party/snpe/include",
     "#third_party/mapbox-gl-native-qt/include",
@@ -246,18 +258,6 @@ def progress_function(node):
 if os.environ.get('SCONS_PROGRESS'):
   Progress(progress_function, interval=node_interval)
 
-SHARED = False
-
-# TODO: this can probably be removed
-def abspath(x):
-  if arch == 'aarch64':
-    pth = os.path.join("/data/pythonpath", x[0].path)
-    env.Depends(pth, x)
-    return File(pth)
-  else:
-    # rpath works elsewhere
-    return x[0].path.rsplit("/", 1)[1][:-3]
-
 # Cython build environment
 py_include = sysconfig.get_paths()['include']
 envCython = env.Clone()
@@ -268,9 +268,6 @@ envCython["CCFLAGS"].remove("-Werror")
 envCython["LIBS"] = []
 if arch == "Darwin":
   envCython["LINKFLAGS"] = ["-bundle", "-undefined", "dynamic_lookup"] + darwin_rpath_link_flags
-elif arch == "aarch64":
-  envCython["LINKFLAGS"] = ["-shared"]
-  envCython["LIBS"] = [os.path.basename(py_include)]
 else:
   envCython["LINKFLAGS"] = ["-pthread", "-shared"]
 
@@ -330,7 +327,6 @@ qt_flags = [
 qt_env['CXXFLAGS'] += qt_flags
 qt_env['LIBPATH'] += ['#selfdrive/ui']
 qt_env['LIBS'] = qt_libs
-qt_env['QT_MOCHPREFIX'] = cache_dir + '/moc_files/moc_'
 
 if GetOption("clazy"):
   checks = [
@@ -343,33 +339,35 @@ if GetOption("clazy"):
   qt_env['ENV']['CLAZY_IGNORE_DIRS'] = qt_dirs[0]
   qt_env['ENV']['CLAZY_CHECKS'] = ','.join(checks)
 
-Export('env', 'qt_env', 'arch', 'real_arch', 'SHARED')
+Export('env', 'qt_env', 'arch', 'real_arch')
 
+# Build common module
 SConscript(['common/SConscript'])
 Import('_common', '_gpucommon')
 
-if SHARED:
-  common, gpucommon = abspath(common), abspath(gpucommon)
-else:
-  common = [_common, 'json11']
-  gpucommon = [_gpucommon]
+common = [_common, 'json11']
+gpucommon = [_gpucommon]
 
 Export('common', 'gpucommon')
 
-# cereal and messaging are shared with the system
+# Build cereal and messaging
 SConscript(['cereal/SConscript'])
-if SHARED:
-  cereal = abspath([File('cereal/libcereal_shared.so')])
-  messaging = abspath([File('cereal/libmessaging_shared.so')])
-else:
-  cereal = [File('#cereal/libcereal.a')]
-  messaging = [File('#cereal/libmessaging.a')]
-  visionipc = [File('#cereal/libvisionipc.a')]
 
-Export('cereal', 'messaging', 'visionipc')
+cereal = [File('#cereal/libcereal.a')]
+messaging = [File('#cereal/libmessaging.a')]
+visionipc = [File('#cereal/libvisionipc.a')]
+messaging_python = [File('#cereal/messaging/messaging_pyx.so')]
 
-# Build rednose library and ekf models
+Export('cereal', 'messaging', 'messaging_python', 'visionipc')
 
+# Build other submodules
+SConscript([
+  'body/board/SConscript',
+  'opendbc/can/SConscript',
+  'panda/SConscript',
+])
+
+# Build rednose library and ekf models
 rednose_deps = [
   "#selfdrive/locationd/models/constants.py",
   "#selfdrive/locationd/models/gnss_helpers.py",
@@ -398,7 +396,6 @@ SConscript(['rednose/SConscript'])
 
 # Build system services
 SConscript([
-  'system/clocksd/SConscript',
   'system/proclogd/SConscript',
   'system/ubloxd/SConscript',
   'system/loggerd/SConscript',
@@ -411,20 +408,8 @@ if arch != "Darwin":
   ])
 
 # Build openpilot
-
-# build submodules
-SConscript([
-  'body/board/SConscript',
-  'cereal/SConscript',
-  'opendbc/can/SConscript',
-  'panda/SConscript',
-])
-
 SConscript(['third_party/SConscript'])
 
-SConscript(['common/kalman/SConscript'])
-SConscript(['common/transformations/SConscript'])
-
 SConscript(['selfdrive/boardd/SConscript'])
 SConscript(['selfdrive/controls/lib/lateral_mpc_lib/SConscript'])
 SConscript(['selfdrive/controls/lib/longitudinal_mpc_lib/SConscript'])
@@ -433,7 +418,7 @@ SConscript(['selfdrive/navd/SConscript'])
 SConscript(['selfdrive/modeld/SConscript'])
 SConscript(['selfdrive/ui/SConscript'])
 
-if (arch in ['x86_64', 'Darwin'] and Dir('#tools/cabana/').exists()) or GetOption('extras'):
+if arch in ['x86_64', 'aarch64', 'Darwin'] and Dir('#tools/cabana/').exists() and GetOption('extras'):
   SConscript(['tools/replay/SConscript'])
   SConscript(['tools/cabana/SConscript'])
 
diff --git a/cereal/SConscript b/cereal/SConscript
index 192e42d0e8..a5d9d6bea1 100644
--- a/cereal/SConscript
+++ b/cereal/SConscript
@@ -69,7 +69,7 @@ else:
 envCython.Program('visionipc/visionipc_pyx.so', 'visionipc/visionipc_pyx.pyx',
                   LIBS=vipc_libs, FRAMEWORKS=vipc_frameworks)
 
-if GetOption('test'):
+if GetOption('extras'):
   env.Program('messaging/test_runner', ['messaging/test_runner.cc', 'messaging/msgq_tests.cc'], LIBS=[messaging_lib, common])
 
   env.Program('visionipc/test_runner', ['visionipc/test_runner.cc', 'visionipc/visionipc_tests.cc'],
diff --git a/cereal/car.capnp b/cereal/car.capnp
index 25f396ec93..29949a1480 100644
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@@ -63,7 +63,6 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     driverUnresponsive @45;
     belowSteerSpeed @46;
     lowBattery @48;
-    vehicleModelInvalid @50;
     accFaulted @51;
     sensorDataInvalid @52;
     commIssue @53;
@@ -106,7 +105,6 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     roadCameraError @100;
     driverCameraError @101;
     wideRoadCameraError @102;
-    localizerMalfunction @103;
     highCpuUsage @105;
     cruiseMismatch @106;
     lkasDisabled @107;
@@ -115,6 +113,10 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     resumeBlocked @113;
     steerTimeLimit @115;
     vehicleSensorsInvalid @116;
+    locationdTemporaryError @103;
+    locationdPermanentError @118;
+    paramsdTemporaryError @50;
+    paramsdPermanentError @119;
 
     radarCanErrorDEPRECATED @15;
     communityFeatureDisallowedDEPRECATED @62;
@@ -450,6 +452,7 @@ struct CarParams {
 
   # things we can derive
   rotationalInertia @22 :Float32;    # [kg*m2] body rotational inertia
+  tireStiffnessFactor @72 :Float32;  # scaling factor used in calculating tireStiffness[Front,Rear]
   tireStiffnessFront @23 :Float32;   # [N/rad] front tire coeff of stiff
   tireStiffnessRear @24 :Float32;    # [N/rad] rear tire coeff of stiff
 
@@ -457,8 +460,8 @@ struct CarParams {
   lateralParams @48 :LateralParams;
   lateralTuning :union {
     pid @26 :LateralPIDTuning;
-    indi @27 :LateralINDITuning;
-    lqr @40 :LateralLQRTuning;
+    indiDEPRECATED @27 :LateralINDITuning;
+    lqrDEPRECATED @40 :LateralLQRTuning;
     torque @67 :LateralTorqueTuning;
   }
 
@@ -591,6 +594,7 @@ struct CarParams {
     hongqi @26;
     body @27;
     hyundaiCanfd @28;
+    volkswagenMqbEvo @29;
   }
 
   enum SteerControlType {
diff --git a/cereal/log.capnp b/cereal/log.capnp
index 0646b9e9ef..12e25b14f3 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -46,6 +46,8 @@ struct InitData {
 
   commands @19 :Map(Text, Data);
 
+  wallTimeNanos @20 :UInt64;
+
   enum DeviceType {
     unknown @0;
     neo @1;
@@ -586,6 +588,7 @@ struct RadarState @0x9a185389d6fdd05f {
     aLeadTau @12 :Float32;
     modelProb @13 :Float32;
     radar @14 :Bool;
+    radarTrackId @15 :Int32 = -1;
 
     aLeadDEPRECATED @5 :Float32;
   }
@@ -871,6 +874,8 @@ struct ModelDataV2 {
   navEnabled @22 :Bool;
   locationMonoTime @24 :UInt64;
 
+  # e2e lateral planner
+  lateralPlannerSolution @25: LateralPlannerSolution;
 
   struct LeadDataV2 {
     prob @0 :Float32; # probability that car is your lead at time t
@@ -937,6 +942,18 @@ struct ModelDataV2 {
     transStd @2 :List(Float32); # std m/s in device frame
     rotStd @3 :List(Float32); # std rad/s in device frame
   }
+
+  struct LateralPlannerSolution {
+    x @0 :List(Float32);
+    y @1 :List(Float32);
+    yaw @2 :List(Float32);
+    yawRate @3 :List(Float32);
+    xStd @4 :List(Float32);
+    yStd @5 :List(Float32);
+    yawStd @6 :List(Float32);
+    yawRateStd @7 :List(Float32);
+  }
+
 }
 
 struct EncodeIndex {
@@ -2082,6 +2099,8 @@ struct NavInstruction {
   speedLimit @10 :Float32; # m/s
   speedLimitSign @11 :SpeedLimitSign;
 
+  allManeuvers @12 :List(Maneuver);
+
   struct Lane {
     directions @0 :List(Direction);
     active @1 :Bool;
@@ -2093,12 +2112,20 @@ struct NavInstruction {
     left @1;
     right @2;
     straight @3;
+    slightLeft @4;
+    slightRight @5;
   }
 
   enum SpeedLimitSign {
     mutcd @0; # US Style
     vienna @1; # EU Style
-    }
+  }
+
+  struct Maneuver {
+    distance @0 :Float32;
+    type @1 :Text;
+    modifier @2 :Text;
+  }
 }
 
 struct NavRoute {
@@ -2177,6 +2204,7 @@ struct Event {
     magnetometer @95 :SensorEventData;
     lightSensor @96 :SensorEventData;
     temperatureSensor @97 :SensorEventData;
+    temperatureSensor2 @123 :SensorEventData;
     pandaStates @81 :List(PandaState);
     peripheralState @80 :PeripheralState;
     radarState @13 :RadarState;
@@ -2253,6 +2281,10 @@ struct Event {
     livestreamWideRoadEncodeData @121 :EncodeData;
     livestreamDriverEncodeData @122 :EncodeData;
 
+    customReservedRawData0 @124 :Data;
+    customReservedRawData1 @125 :Data;
+    customReservedRawData2 @126 :Data;
+
     # *********** Custom: reserved for forks ***********
     customReserved0 @107 :Custom.CustomReserved0;
     customReserved1 @108 :Custom.CustomReserved1;
diff --git a/cereal/messaging/__init__.py b/cereal/messaging/__init__.py
index 5a863f130b..57675212f4 100644
--- a/cereal/messaging/__init__.py
+++ b/cereal/messaging/__init__.py
@@ -1,6 +1,7 @@
 # must be built with scons
-from .messaging_pyx import Context, Poller, SubSocket, PubSocket, SocketEventHandle, toggle_fake_events, set_fake_prefix, get_fake_prefix, delete_fake_prefix, wait_for_one_event  # pylint: disable=no-name-in-module, import-error
-from .messaging_pyx import MultiplePublishersError, MessagingError  # pylint: disable=no-name-in-module, import-error
+from .messaging_pyx import Context, Poller, SubSocket, PubSocket, SocketEventHandle, toggle_fake_events, \
+                                set_fake_prefix, get_fake_prefix, delete_fake_prefix, wait_for_one_event
+from .messaging_pyx import MultiplePublishersError, MessagingError
 
 import os
 import capnp
@@ -10,7 +11,7 @@ from typing import Optional, List, Union, Dict, Deque
 from collections import deque
 
 from cereal import log
-from cereal.services import service_list
+from cereal.services import SERVICE_LIST
 
 assert MultiplePublishersError
 assert MessagingError
@@ -23,13 +24,6 @@ assert wait_for_one_event
 NO_TRAVERSAL_LIMIT = 2**64-1
 AVG_FREQ_HISTORY = 100
 
-# sec_since_boot is faster, but allow to run standalone too
-try:
-  from common.realtime import sec_since_boot
-except ImportError:
-  sec_since_boot = time.time
-  print("Warning, using python time.time() instead of faster sec_since_boot")
-
 context = Context()
 
 
@@ -49,7 +43,7 @@ def log_from_bytes(dat: bytes) -> capnp.lib.capnp._DynamicStructReader:
 
 def new_message(service: Optional[str] = None, size: Optional[int] = None) -> capnp.lib.capnp._DynamicStructBuilder:
   dat = log.Event.new_message()
-  dat.logMonoTime = int(sec_since_boot() * 1e9)
+  dat.logMonoTime = int(time.monotonic() * 1e9)
   dat.valid = True
   if service is not None:
     if size is None:
@@ -186,7 +180,7 @@ class SubMaster:
       if addr is not None:
         p = self.poller if s not in self.non_polled_services else None
         self.sock[s] = sub_sock(s, poller=p, addr=addr, conflate=True)
-      self.freq[s] = service_list[s].frequency
+      self.freq[s] = SERVICE_LIST[s].frequency
 
       try:
         data = new_message(s)
@@ -212,7 +206,7 @@ class SubMaster:
     # non-blocking receive for non-polled sockets
     for s in self.non_polled_services:
       msgs.append(recv_one_or_none(self.sock[s]))
-    self.update_msgs(sec_since_boot(), msgs)
+    self.update_msgs(time.monotonic(), msgs)
 
   def update_msgs(self, cur_time: float, msgs: List[capnp.lib.capnp._DynamicStructReader]) -> None:
     self.frame += 1
@@ -293,8 +287,7 @@ class PubMaster:
       dat = dat.to_bytes()
     self.sock[s].send(dat)
 
-  def wait_for_readers_to_update(self, s: str, timeout: int) -> bool:
-    dt = 0.05
+  def wait_for_readers_to_update(self, s: str, timeout: int, dt: float = 0.05) -> bool:
     for _ in range(int(timeout*(1./dt))):
       if self.sock[s].all_readers_updated():
         return True
diff --git a/cereal/messaging/bridge.cc b/cereal/messaging/bridge.cc
index 2ab751ce25..4a5390caff 100644
--- a/cereal/messaging/bridge.cc
+++ b/cereal/messaging/bridge.cc
@@ -24,7 +24,7 @@ void sigpipe_handler(int sig) {
 static std::vector<std::string> get_services(std::string whitelist_str, bool zmq_to_msgq) {
   std::vector<std::string> service_list;
   for (const auto& it : services) {
-    std::string name = it.name;
+    std::string name = it.second.name;
     bool in_whitelist = whitelist_str.find(name) != std::string::npos;
     if (name == "plusFrame" || name == "uiLayoutState" || (zmq_to_msgq && !in_whitelist)) {
       continue;
diff --git a/cereal/messaging/impl_fake.h b/cereal/messaging/impl_fake.h
index 0b3f6eb41c..0ec8486a08 100644
--- a/cereal/messaging/impl_fake.h
+++ b/cereal/messaging/impl_fake.h
@@ -63,5 +63,5 @@ private:
 public:
   void registerSocket(SubSocket *socket) override;
   std::vector<SubSocket*> poll(int timeout) override;
-  ~FakePoller() {};
+  ~FakePoller() {}
 };
diff --git a/cereal/messaging/impl_msgq.cc b/cereal/messaging/impl_msgq.cc
index 18e936449a..8f2c10a087 100644
--- a/cereal/messaging/impl_msgq.cc
+++ b/cereal/messaging/impl_msgq.cc
@@ -17,12 +17,7 @@ void sig_handler(int signal) {
 }
 
 static bool service_exists(std::string path){
-  for (const auto& it : services) {
-    if (it.name == path) {
-      return true;
-    }
-  }
-  return false;
+  return services.count(path) > 0;
 }
 
 
diff --git a/cereal/messaging/impl_msgq.h b/cereal/messaging/impl_msgq.h
index fdad52f9e2..68235f0c6e 100644
--- a/cereal/messaging/impl_msgq.h
+++ b/cereal/messaging/impl_msgq.h
@@ -63,5 +63,5 @@ private:
 public:
   void registerSocket(SubSocket *socket);
   std::vector<SubSocket*> poll(int timeout);
-  ~MSGQPoller(){};
+  ~MSGQPoller(){}
 };
diff --git a/cereal/messaging/impl_zmq.cc b/cereal/messaging/impl_zmq.cc
index 5355c81109..7da9df1b8f 100644
--- a/cereal/messaging/impl_zmq.cc
+++ b/cereal/messaging/impl_zmq.cc
@@ -3,22 +3,13 @@
 #include <iostream>
 #include <cstdlib>
 #include <cerrno>
+#include <unistd.h>
 
 #include "cereal/services.h"
 #include "cereal/messaging/impl_zmq.h"
 
 static int get_port(std::string endpoint) {
-  int port = -1;
-  for (const auto& it : services) {
-    std::string name = it.name;
-    if (name == endpoint) {
-      port = it.port;
-      break;
-    }
-  }
-
-  assert(port >= 0);
-  return port;
+  return services.at(endpoint).port;
 }
 
 ZMQContext::ZMQContext() {
@@ -118,14 +109,19 @@ int ZMQPubSocket::connect(Context *context, std::string endpoint, bool check_end
     full_endpoint += endpoint;
   }
 
+  // ZMQ pub sockets cannot be shared between processes, so we need to ensure pid stays the same
+  pid = getpid();
+
   return zmq_bind(sock, full_endpoint.c_str());
 }
 
-int ZMQPubSocket::sendMessage(Message *message){
+int ZMQPubSocket::sendMessage(Message *message) {
+  assert(pid == getpid());
   return zmq_send(sock, message->getData(), message->getSize(), ZMQ_DONTWAIT);
 }
 
-int ZMQPubSocket::send(char *data, size_t size){
+int ZMQPubSocket::send(char *data, size_t size) {
+  assert(pid == getpid());
   return zmq_send(sock, data, size, ZMQ_DONTWAIT);
 }
 
diff --git a/cereal/messaging/impl_zmq.h b/cereal/messaging/impl_zmq.h
index deeac859c1..903875f630 100644
--- a/cereal/messaging/impl_zmq.h
+++ b/cereal/messaging/impl_zmq.h
@@ -46,6 +46,7 @@ class ZMQPubSocket : public PubSocket {
 private:
   void * sock;
   std::string full_endpoint;
+  int pid = -1;
 public:
   int connect(Context *context, std::string endpoint, bool check_endpoint=true);
   int sendMessage(Message *message);
@@ -63,5 +64,5 @@ private:
 public:
   void registerSocket(SubSocket *socket);
   std::vector<SubSocket*> poll(int timeout);
-  ~ZMQPoller(){};
+  ~ZMQPoller(){}
 };
diff --git a/cereal/messaging/messaging.h b/cereal/messaging/messaging.h
index 483b2465c2..91106514af 100644
--- a/cereal/messaging/messaging.h
+++ b/cereal/messaging/messaging.h
@@ -33,7 +33,7 @@ public:
   virtual void close() = 0;
   virtual size_t getSize() = 0;
   virtual char * getData() = 0;
-  virtual ~Message(){};
+  virtual ~Message(){}
 };
 
 
@@ -45,7 +45,7 @@ public:
   virtual void * getRawSocket() = 0;
   static SubSocket * create();
   static SubSocket * create(Context * context, std::string endpoint, std::string address="127.0.0.1", bool conflate=false, bool check_endpoint=true);
-  virtual ~SubSocket(){};
+  virtual ~SubSocket(){}
 };
 
 class PubSocket {
@@ -57,7 +57,7 @@ public:
   static PubSocket * create();
   static PubSocket * create(Context * context, std::string endpoint, bool check_endpoint=true);
   static PubSocket * create(Context * context, std::string endpoint, int port, bool check_endpoint=true);
-  virtual ~PubSocket(){};
+  virtual ~PubSocket(){}
 };
 
 class Poller {
@@ -66,7 +66,7 @@ public:
   virtual std::vector<SubSocket*> poll(int timeout) = 0;
   static Poller * create();
   static Poller * create(std::vector<SubSocket*> sockets);
-  virtual ~Poller(){};
+  virtual ~Poller(){}
 };
 
 class SubMaster {
@@ -116,6 +116,18 @@ public:
     return heapArray_.asBytes();
   }
 
+  size_t getSerializedSize() {
+    return capnp::computeSerializedSizeInWords(*this) * sizeof(capnp::word);
+  }
+
+  int serializeToBuffer(unsigned char *buffer, size_t buffer_size) {
+    size_t serialized_size = getSerializedSize();
+    if (serialized_size > buffer_size) { return -1; }
+    kj::ArrayOutputStream out(kj::ArrayPtr<capnp::byte>(buffer, buffer_size));
+    capnp::writeMessage(out, *this);
+    return serialized_size;
+  }
+
 private:
   kj::Array<capnp::word> heapArray_;
 };
diff --git a/cereal/messaging/socketmaster.cc b/cereal/messaging/socketmaster.cc
index 50614be36a..3054b4f7db 100644
--- a/cereal/messaging/socketmaster.cc
+++ b/cereal/messaging/socketmaster.cc
@@ -15,13 +15,6 @@ static inline uint64_t nanos_since_boot() {
   return t.tv_sec * 1000000000ULL + t.tv_nsec;
 }
 
-static const service *get_service(const char *name) {
-  for (const auto &it : services) {
-    if (strcmp(it.name, name) == 0) return &it;
-  }
-  return nullptr;
-}
-
 static inline bool inList(const std::vector<const char *> &list, const char *value) {
   for (auto &v : list) {
     if (strcmp(value, v) == 0) return true;
@@ -31,7 +24,7 @@ static inline bool inList(const std::vector<const char *> &list, const char *val
 
 class MessageContext {
 public:
-  MessageContext() : ctx_(nullptr) {};
+  MessageContext() : ctx_(nullptr) {}
   ~MessageContext() { delete ctx_; }
   inline Context *context() {
     std::call_once(init_flag, [=]() { ctx_ = Context::create(); });
@@ -61,8 +54,9 @@ SubMaster::SubMaster(const std::vector<const char *> &service_list, const std::v
                      const char *address, const std::vector<const char *> &ignore_alive) {
   poller_ = Poller::create();
   for (auto name : service_list) {
-    const service *serv = get_service(name);
-    assert(serv != nullptr);
+    assert(services.count(std::string(name)) > 0);
+
+    service serv = services.at(std::string(name));
     SubSocket *socket = SubSocket::create(message_context.context(), name, address ? address : "127.0.0.1", true);
     assert(socket != 0);
     bool is_polled = inList(poll, name) || poll.empty();
@@ -70,7 +64,7 @@ SubMaster::SubMaster(const std::vector<const char *> &service_list, const std::v
     SubMessage *m = new SubMessage{
       .name = name,
       .socket = socket,
-      .freq = serv->frequency,
+      .freq = serv.frequency,
       .ignore_alive = inList(ignore_alive, name),
       .allocated_msg_reader = malloc(sizeof(capnp::FlatArrayMessageReader)),
       .is_polled = is_polled};
@@ -184,7 +178,7 @@ uint64_t SubMaster::rcv_time(const char *name) const {
 
 cereal::Event::Reader &SubMaster::operator[](const char *name) const {
   return services_.at(name)->event;
-};
+}
 
 SubMaster::~SubMaster() {
   delete poller_;
@@ -199,7 +193,7 @@ SubMaster::~SubMaster() {
 
 PubMaster::PubMaster(const std::vector<const char *> &service_list) {
   for (auto name : service_list) {
-    assert(get_service(name) != nullptr);
+    assert(services.count(name) > 0);
     PubSocket *socket = PubSocket::create(message_context.context(), name);
     assert(socket);
     sockets_[name] = socket;
diff --git a/cereal/services.py b/cereal/services.py
index 1ff98cfe86..9c08e63525 100755
--- a/cereal/services.py
+++ b/cereal/services.py
@@ -18,21 +18,22 @@ class Service:
     self.decimation = decimation
 
 
-services = {
+services: dict[str, tuple] = {
   # service: (should_log, frequency, qlog decimation (optional))
   # note: the "EncodeIdx" packets will still be in the log
   "gyroscope": (True, 104., 104),
   "gyroscope2": (True, 100., 100),
   "accelerometer": (True, 104., 104),
   "accelerometer2": (True, 100., 100),
-  "magnetometer": (True, 100., 100),
+  "magnetometer": (True, 25., 25),
   "lightSensor": (True, 100., 100),
-  "temperatureSensor": (True, 100., 100),
+  "temperatureSensor": (True, 2., 200),
+  "temperatureSensor2": (True, 2., 200),
   "gpsNMEA": (True, 9.),
   "deviceState": (True, 2., 1),
   "can": (True, 100., 1223),  # decimation gives ~5 msgs in a full segment
   "controlsState": (True, 100., 10),
-  "pandaStates": (True, 2., 1),
+  "pandaStates": (True, 10., 1),
   "peripheralState": (True, 2., 1),
   "radarState": (True, 20., 5),
   "roadEncodeIdx": (False, 20., 1),
@@ -94,8 +95,11 @@ services = {
   "livestreamWideRoadEncodeData": (False, 20.),
   "livestreamRoadEncodeData": (False, 20.),
   "livestreamDriverEncodeData": (False, 20.),
+  "customReservedRawData0": (True, 0.),
+  "customReservedRawData1": (True, 0.),
+  "customReservedRawData2": (True, 0.),
 }
-service_list = {name: Service(new_port(idx), *vals) for  # type: ignore
+SERVICE_LIST = {name: Service(new_port(idx), *vals) for
                 idx, (name, vals) in enumerate(services.items())}
 
 
@@ -104,14 +108,19 @@ def build_header():
   h += "/* THIS IS AN AUTOGENERATED FILE, PLEASE EDIT services.py */\n"
   h += "#ifndef __SERVICES_H\n"
   h += "#define __SERVICES_H\n"
-  h += "struct service { char name[0x100]; int port; bool should_log; int frequency; int decimation; };\n"
-  h += "static struct service services[] = {\n"
-  for k, v in service_list.items():
+
+  h += "#include <map>\n"
+  h += "#include <string>\n"
+
+  h += "struct service { std::string name; int port; bool should_log; int frequency; int decimation; };\n"
+  h += "static std::map<std::string, service> services = {\n"
+  for k, v in SERVICE_LIST.items():
     should_log = "true" if v.should_log else "false"
     decimation = -1 if v.decimation is None else v.decimation
-    h += '  { "%s", %d, %s, %d, %d },\n' % \
-         (k, v.port, should_log, v.frequency, decimation)
+    h += '  { "%s", {"%s", %d, %s, %d, %d}},\n' % \
+         (k, k, v.port, should_log, v.frequency, decimation)
   h += "};\n"
+
   h += "#endif\n"
   return h
 
diff --git a/cereal/visionipc/__init__.py b/cereal/visionipc/__init__.py
index c6460600ce..7adfc58d14 100644
--- a/cereal/visionipc/__init__.py
+++ b/cereal/visionipc/__init__.py
@@ -1,4 +1,5 @@
-from cereal.visionipc.visionipc_pyx import VisionIpcClient, VisionIpcServer, VisionStreamType, get_endpoint_name  # pylint: disable=no-name-in-module, import-error
+from cereal.visionipc.visionipc_pyx import VisionBuf, VisionIpcClient, VisionIpcServer, VisionStreamType, get_endpoint_name
+assert VisionBuf
 assert VisionIpcClient
 assert VisionIpcServer
 assert VisionStreamType
diff --git a/cereal/visionipc/visionbuf_ion.cc b/cereal/visionipc/visionbuf_ion.cc
index ed446abfb9..f72e76cf74 100644
--- a/cereal/visionipc/visionbuf_ion.cc
+++ b/cereal/visionipc/visionbuf_ion.cc
@@ -40,30 +40,35 @@
 #define DEVICE_PAGE_SIZE_CL 4096
 #define PADDING_CL 0
 
-static int ion_fd = -1;
-static void ion_init() {
-  if (ion_fd == -1) {
-    ion_fd = open("/dev/ion", O_RDWR | O_NONBLOCK);
+struct IonFileHandle {
+  IonFileHandle() {
+    fd = open("/dev/ion", O_RDWR | O_NONBLOCK);
+    assert(fd >= 0);
   }
+  ~IonFileHandle() {
+    close(fd);
+  }
+  int fd = -1;
+};
+
+int ion_fd() {
+  static IonFileHandle fh;
+  return fh.fd;
 }
 
 void VisionBuf::allocate(size_t length) {
-  int err;
-
-  ion_init();
-
   struct ion_allocation_data ion_alloc = {0};
   ion_alloc.len = length + PADDING_CL + sizeof(uint64_t);
   ion_alloc.align = 4096;
   ion_alloc.heap_id_mask = 1 << ION_IOMMU_HEAP_ID;
   ion_alloc.flags = ION_FLAG_CACHED;
 
-  err = HANDLE_EINTR(ioctl(ion_fd, ION_IOC_ALLOC, &ion_alloc));
+  int err = HANDLE_EINTR(ioctl(ion_fd(), ION_IOC_ALLOC, &ion_alloc));
   assert(err == 0);
 
   struct ion_fd_data ion_fd_data = {0};
   ion_fd_data.handle = ion_alloc.handle;
-  err = HANDLE_EINTR(ioctl(ion_fd, ION_IOC_SHARE, &ion_fd_data));
+  err = HANDLE_EINTR(ioctl(ion_fd(), ION_IOC_SHARE, &ion_fd_data));
   assert(err == 0);
 
   void *mmap_addr = mmap(NULL, ion_alloc.len,
@@ -85,12 +90,10 @@ void VisionBuf::import(){
   int err;
   assert(this->fd >= 0);
 
-  ion_init();
-
   // Get handle
   struct ion_fd_data fd_data = {0};
   fd_data.fd = this->fd;
-  err = HANDLE_EINTR(ioctl(ion_fd, ION_IOC_IMPORT, &fd_data));
+  err = HANDLE_EINTR(ioctl(ion_fd(), ION_IOC_IMPORT, &fd_data));
   assert(err == 0);
 
   this->handle = fd_data.handle;
@@ -136,7 +139,7 @@ int VisionBuf::sync(int dir) {
      ION_IOC_INV_CACHES : ION_IOC_CLEAN_CACHES;
 
   custom_data.arg = (unsigned long)&flush_data;
-  return HANDLE_EINTR(ioctl(ion_fd, ION_IOC_CUSTOM, &custom_data));
+  return HANDLE_EINTR(ioctl(ion_fd(), ION_IOC_CUSTOM, &custom_data));
 }
 
 int VisionBuf::free() {
@@ -154,5 +157,5 @@ int VisionBuf::free() {
   if (err != 0) return err;
 
   struct ion_handle_data handle_data = {.handle = this->handle};
-  return HANDLE_EINTR(ioctl(ion_fd, ION_IOC_FREE, &handle_data));
+  return HANDLE_EINTR(ioctl(ion_fd(), ION_IOC_FREE, &handle_data));
 }
diff --git a/cereal/visionipc/visionipc.pxd b/cereal/visionipc/visionipc.pxd
index 32baa9cc13..3151dfc7d7 100644
--- a/cereal/visionipc/visionipc.pxd
+++ b/cereal/visionipc/visionipc.pxd
@@ -8,7 +8,12 @@ from libc.stdint cimport uint32_t, uint64_t
 from libcpp cimport bool, int
 
 cdef extern from "cereal/visionipc/visionbuf.h":
+  struct _cl_device_id
+  struct _cl_context
   struct _cl_mem
+
+  ctypedef _cl_device_id * cl_device_id
+  ctypedef _cl_context * cl_context
   ctypedef _cl_mem * cl_mem
 
   cdef enum VisionStreamType:
diff --git a/cereal/visionipc/visionipc_client.h b/cereal/visionipc/visionipc_client.h
index 8dff9b2e04..d67734d32f 100644
--- a/cereal/visionipc/visionipc_client.h
+++ b/cereal/visionipc/visionipc_client.h
@@ -19,8 +19,6 @@ private:
   cl_device_id device_id = nullptr;
   cl_context ctx = nullptr;
 
-  void init_msgq(bool conflate);
-
 public:
   bool connected = false;
   VisionStreamType type;
diff --git a/cereal/visionipc/visionipc_pyx.pxd b/cereal/visionipc/visionipc_pyx.pxd
index 6e2d5ed05e..ec431ceecd 100644
--- a/cereal/visionipc/visionipc_pyx.pxd
+++ b/cereal/visionipc/visionipc_pyx.pxd
@@ -2,6 +2,11 @@
 #cython: language_level=3
 
 from .visionipc cimport VisionBuf as cppVisionBuf
+from .visionipc cimport cl_device_id, cl_context
+
+cdef class CLContext:
+  cdef cl_device_id device_id
+  cdef cl_context context
 
 cdef class VisionBuf:
   cdef cppVisionBuf * buf
diff --git a/cereal/visionipc/visionipc_pyx.pyx b/cereal/visionipc/visionipc_pyx.pyx
index bcce534f87..0ff270efd5 100644
--- a/cereal/visionipc/visionipc_pyx.pyx
+++ b/cereal/visionipc/visionipc_pyx.pyx
@@ -98,8 +98,11 @@ cdef class VisionIpcClient:
   cdef cppVisionIpcClient * client
   cdef VisionIpcBufExtra extra
 
-  def __cinit__(self, string name, VisionStreamType stream, bool conflate):
-    self.client = new cppVisionIpcClient(name, stream, conflate, NULL, NULL)
+  def __cinit__(self, string name, VisionStreamType stream, bool conflate, CLContext context = None):
+    if context:
+      self.client = new cppVisionIpcClient(name, stream, conflate, context.device_id, context.context)
+    else:
+      self.client = new cppVisionIpcClient(name, stream, conflate, NULL, NULL)
 
   def __dealloc__(self):
     del self.client
diff --git a/cereal/visionipc/visionipc_server.h b/cereal/visionipc/visionipc_server.h
index 4c23ff6f07..6ad86f1469 100644
--- a/cereal/visionipc/visionipc_server.h
+++ b/cereal/visionipc/visionipc_server.h
@@ -23,7 +23,6 @@ class VisionIpcServer {
 
   std::map<VisionStreamType, std::atomic<size_t> > cur_idx;
   std::map<VisionStreamType, std::vector<VisionBuf*> > buffers;
-  std::map<VisionStreamType, std::map<VisionBuf*, size_t> > idxs;
 
   Context * msg_ctx;
   std::map<VisionStreamType, PubSocket*> sockets;
diff --git a/common/SConscript b/common/SConscript
index 5d6170611f..97322b248d 100644
--- a/common/SConscript
+++ b/common/SConscript
@@ -1,9 +1,4 @@
-Import('env', 'envCython', 'arch', 'SHARED')
-
-if SHARED:
-  fxn = env.SharedLibrary
-else:
-  fxn = env.Library
+Import('env', 'envCython', 'arch')
 
 common_libs = [
   'params.cc',
@@ -12,24 +7,35 @@ common_libs = [
   'util.cc',
   'i2c.cc',
   'watchdog.cc',
+  'ratekeeper.cc'
 ]
 
 if arch != "Darwin":
   common_libs.append('gpio.cc')
 
-_common = fxn('common', common_libs, LIBS="json11")
+_common = env.Library('common', common_libs, LIBS="json11")
 
 files = [
   'clutil.cc',
 ]
 
-_gpucommon = fxn('gpucommon', files)
+_gpucommon = env.Library('gpucommon', files)
 Export('_common', '_gpucommon')
 
-if GetOption('test'):
-  env.Program('tests/test_util', ['tests/test_util.cc'], LIBS=[_common])
-  env.Program('tests/test_swaglog', ['tests/test_swaglog.cc'], LIBS=[_common, 'json11', 'zmq', 'pthread'])
+if GetOption('extras'):
+  env.Program('tests/test_common',
+              ['tests/test_runner.cc', 'tests/test_util.cc', 'tests/test_swaglog.cc', 'tests/test_ratekeeper.cc'],
+              LIBS=[_common, 'json11', 'zmq', 'pthread'])
+
+# Cython bindings
+params_python = envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [_common, 'zmq', 'json11'])
+
+SConscript([
+  'kalman/SConscript',
+  'transformations/SConscript'  
+])
+
+Import('simple_kalman_python', 'transformations_python')
+common_python = [params_python, simple_kalman_python, transformations_python]
 
-# Cython
-envCython.Program('clock.so', 'clock.pyx')
-envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [_common, 'zmq', 'json11'])
+Export('common_python')
diff --git a/common/api/__init__.py b/common/api/__init__.py
index c1fa635bd6..0eb8aa7627 100644
--- a/common/api/__init__.py
+++ b/common/api/__init__.py
@@ -2,8 +2,8 @@ import jwt
 import os
 import requests
 from datetime import datetime, timedelta
-from common.basedir import PERSIST
-from system.version import get_version
+from openpilot.common.basedir import PERSIST
+from openpilot.system.version import get_version
 
 API_HOST = os.getenv('API_HOST', 'https://api.commadotai.com')
 
diff --git a/common/basedir.py b/common/basedir.py
index 371b54d3ef..b4486f9f08 100644
--- a/common/basedir.py
+++ b/common/basedir.py
@@ -1,7 +1,7 @@
 import os
 from pathlib import Path
 
-from system.hardware import PC
+from openpilot.system.hardware import PC
 
 BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../"))
 
diff --git a/common/clock.pyx b/common/clock.pyx
deleted file mode 100644
index 81333565c5..0000000000
--- a/common/clock.pyx
+++ /dev/null
@@ -1,24 +0,0 @@
-# distutils: language = c++
-# cython: language_level = 3
-from posix.time cimport clock_gettime, timespec, CLOCK_MONOTONIC_RAW, clockid_t
-
-IF UNAME_SYSNAME == "Darwin":
-  # Darwin doesn't have a CLOCK_BOOTTIME
-  CLOCK_BOOTTIME = CLOCK_MONOTONIC_RAW
-ELSE:
-  from posix.time cimport CLOCK_BOOTTIME
-
-cdef double readclock(clockid_t clock_id):
-  cdef timespec ts
-  cdef double current
-
-  clock_gettime(clock_id, &ts)
-  current = ts.tv_sec + (ts.tv_nsec / 1000000000.)
-  return current
-
-def monotonic_time():
-  return readclock(CLOCK_MONOTONIC_RAW)
-
-def sec_since_boot():
-  return readclock(CLOCK_BOOTTIME)
-
diff --git a/common/clutil.cc b/common/clutil.cc
index 3cfc8a8c8c..4f2a783d3e 100644
--- a/common/clutil.cc
+++ b/common/clutil.cc
@@ -38,8 +38,8 @@ void cl_print_info(cl_platform_id platform, cl_device_id device) {
   LOGD("extensions: %s", get_platform_info(platform, CL_PLATFORM_EXTENSIONS).c_str());
   LOGD("name: %s", get_device_info(device, CL_DEVICE_NAME).c_str());
   LOGD("device version: %s", get_device_info(device, CL_DEVICE_VERSION).c_str());
-  LOGD("max work group size: %d", work_group_size);
-  LOGD("type = %d = ", device_type, type_str);
+  LOGD("max work group size: %zu", work_group_size);
+  LOGD("type = %d, %s", (int)device_type, type_str);
 }
 
 void cl_print_build_errors(cl_program program, cl_device_id device) {
@@ -62,7 +62,7 @@ cl_device_id cl_get_device_id(cl_device_type device_type) {
   CL_CHECK(clGetPlatformIDs(num_platforms, &platform_ids[0], NULL));
 
   for (size_t i = 0; i < num_platforms; ++i) {
-    LOGD("platform[%d] CL_PLATFORM_NAME: %s", i, get_platform_info(platform_ids[i], CL_PLATFORM_NAME).c_str());
+    LOGD("platform[%zu] CL_PLATFORM_NAME: %s", i, get_platform_info(platform_ids[i], CL_PLATFORM_NAME).c_str());
 
     // Get first device
     if (cl_device_id device_id = NULL; clGetDeviceIDs(platform_ids[i], device_type, 1, &device_id, NULL) == 0 && device_id) {
@@ -75,6 +75,10 @@ cl_device_id cl_get_device_id(cl_device_type device_type) {
   return nullptr;
 }
 
+cl_context cl_create_context(cl_device_id device_id) {
+  return CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err));
+}
+
 cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args) {
   return cl_program_from_source(ctx, device_id, util::read_file(path), args);
 }
diff --git a/common/clutil.h b/common/clutil.h
index be1a07c332..af986d6434 100644
--- a/common/clutil.h
+++ b/common/clutil.h
@@ -22,6 +22,7 @@
   })
 
 cl_device_id cl_get_device_id(cl_device_type device_type);
+cl_context cl_create_context(cl_device_id device_id);
 cl_program cl_program_from_source(cl_context ctx, cl_device_id device_id, const std::string& src, const char* args = nullptr);
 cl_program cl_program_from_binary(cl_context ctx, cl_device_id device_id, const uint8_t* binary, size_t length, const char* args = nullptr);
 cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args);
diff --git a/common/file_helpers.py b/common/file_helpers.py
index 8a45fa313c..227d614d72 100644
--- a/common/file_helpers.py
+++ b/common/file_helpers.py
@@ -5,7 +5,7 @@ from atomicwrites import AtomicWriter
 
 
 def mkdirs_exists_ok(path):
-  if path.startswith('http://') or path.startswith('https://'):
+  if path.startswith(('http://', 'https://')):
     raise ValueError('URL path')
   try:
     os.makedirs(path)
diff --git a/common/gpio.cc b/common/gpio.cc
index 8a16cd3703..dd7ba34b6d 100644
--- a/common/gpio.cc
+++ b/common/gpio.cc
@@ -1,5 +1,7 @@
 #include "common/gpio.h"
 
+#include <string>
+
 #ifdef __APPLE__
 int gpio_init(int pin_nr, bool output) {
   return 0;
@@ -29,7 +31,7 @@ int gpio_init(int pin_nr, bool output) {
   char pin_dir_path[50];
   int pin_dir_path_len = snprintf(pin_dir_path, sizeof(pin_dir_path),
                            "/sys/class/gpio/gpio%d/direction", pin_nr);
-  if(pin_dir_path_len <= 0) {
+  if (pin_dir_path_len <= 0) {
     return -1;
   }
   const char *value = output ? "out" : "in";
@@ -40,7 +42,7 @@ int gpio_set(int pin_nr, bool high) {
   char pin_val_path[50];
   int pin_val_path_len = snprintf(pin_val_path, sizeof(pin_val_path),
                            "/sys/class/gpio/gpio%d/value", pin_nr);
-  if(pin_val_path_len <= 0) {
+  if (pin_val_path_len <= 0) {
     return -1;
   }
   return util::write_file(pin_val_path, (void*)(high ? "1" : "0"), 1);
@@ -68,7 +70,7 @@ int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int p
   rq.eventflags = GPIOEVENT_REQUEST_BOTH_EDGES;
 
   strncpy(rq.consumer_label, consumer_label, std::size(rq.consumer_label) - 1);
-  int ret = ioctl(fd, GPIO_GET_LINEEVENT_IOCTL, &rq);
+  int ret = util::safe_ioctl(fd, GPIO_GET_LINEEVENT_IOCTL, &rq);
   if (ret == -1) {
     LOGE("Unable to get line event from ioctl : %s", strerror(errno));
     close(fd);
diff --git a/common/gpio.h b/common/gpio.h
index b2f67f8ba3..89cdedd66c 100644
--- a/common/gpio.h
+++ b/common/gpio.h
@@ -5,7 +5,7 @@
   #define GPIO_HUB_RST_N        30
   #define GPIO_UBLOX_RST_N      32
   #define GPIO_UBLOX_SAFEBOOT_N 33
-  #define GPIO_UBLOX_PWR_EN     34
+  #define GPIO_GNSS_PWR_EN      34 /* SCHEMATIC LABEL: GPIO_UBLOX_PWR_EN */
   #define GPIO_STM_RST_N        124
   #define GPIO_STM_BOOT0        134
   #define GPIO_BMX_ACCEL_INT    21
@@ -17,7 +17,7 @@
   #define GPIO_HUB_RST_N        0
   #define GPIO_UBLOX_RST_N      0
   #define GPIO_UBLOX_SAFEBOOT_N 0
-  #define GPIO_UBLOX_PWR_EN     0
+  #define GPIO_GNSS_PWR_EN      0 /* SCHEMATIC LABEL: GPIO_UBLOX_PWR_EN */
   #define GPIO_STM_RST_N        0
   #define GPIO_STM_BOOT0        0
   #define GPIO_BMX_ACCEL_INT    0
diff --git a/common/gpio.py b/common/gpio.py
index 5ec23bf7b1..88a9479a60 100644
--- a/common/gpio.py
+++ b/common/gpio.py
@@ -1,3 +1,4 @@
+import os
 from functools import lru_cache
 from typing import Optional, List
 
@@ -26,6 +27,9 @@ def gpio_read(pin: int) -> Optional[bool]:
   return val
 
 def gpio_export(pin: int) -> None:
+  if os.path.isdir(f"/sys/class/gpio/gpio{pin}"):
+    return
+
   try:
     with open("/sys/class/gpio/export", 'w') as f:
       f.write(str(pin))
diff --git a/common/i2c.cc b/common/i2c.cc
index ef788ac9ea..3d6c79efc5 100644
--- a/common/i2c.cc
+++ b/common/i2c.cc
@@ -8,7 +8,6 @@
 #include <cstdio>
 #include <stdexcept>
 
-#include "common/util.h"
 #include "common/swaglog.h"
 #include "common/util.h"
 
@@ -26,36 +25,42 @@ I2CBus::I2CBus(uint8_t bus_id) {
   snprintf(bus_name, 20, "/dev/i2c-%d", bus_id);
 
   i2c_fd = HANDLE_EINTR(open(bus_name, O_RDWR));
-  if(i2c_fd < 0) {
+  if (i2c_fd < 0) {
     throw std::runtime_error("Failed to open I2C bus");
   }
 }
 
 I2CBus::~I2CBus() {
-  if(i2c_fd >= 0) { close(i2c_fd); }
+  if (i2c_fd >= 0) {
+    close(i2c_fd);
+  }
 }
 
 int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len) {
+  std::lock_guard lk(m);
+
   int ret = 0;
 
   ret = HANDLE_EINTR(ioctl(i2c_fd, I2C_SLAVE, device_address));
-  if(ret < 0) { goto fail; }
+  if (ret < 0) { goto fail; }
 
   ret = i2c_smbus_read_i2c_block_data(i2c_fd, register_address, len, buffer);
-  if((ret < 0) || (ret != len)) { goto fail; }
+  if ((ret < 0) || (ret != len)) { goto fail; }
 
 fail:
   return ret;
 }
 
 int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data) {
+  std::lock_guard lk(m);
+
   int ret = 0;
 
   ret = HANDLE_EINTR(ioctl(i2c_fd, I2C_SLAVE, device_address));
-  if(ret < 0) { goto fail; }
+  if (ret < 0) { goto fail; }
 
   ret = i2c_smbus_write_byte_data(i2c_fd, register_address, data);
-  if(ret < 0) { goto fail; }
+  if (ret < 0) { goto fail; }
 
 fail:
   return ret;
diff --git a/common/i2c.h b/common/i2c.h
index 0669116bb8..ca0d4635b8 100644
--- a/common/i2c.h
+++ b/common/i2c.h
@@ -1,12 +1,14 @@
 #pragma once
 
 #include <cstdint>
+#include <mutex>
 
 #include <sys/types.h>
 
 class I2CBus {
   private:
     int i2c_fd;
+    std::mutex m;
 
   public:
     I2CBus(uint8_t bus_id);
diff --git a/common/kalman/SConscript b/common/kalman/SConscript
index d60354c987..7cba4a9a61 100644
--- a/common/kalman/SConscript
+++ b/common/kalman/SConscript
@@ -1,3 +1,5 @@
 Import('envCython')
 
-envCython.Program('simple_kalman_impl.so', 'simple_kalman_impl.pyx')
+simple_kalman_python = envCython.Program('simple_kalman_impl.so', 'simple_kalman_impl.pyx')
+
+Export('simple_kalman_python')
diff --git a/common/kalman/simple_kalman.py b/common/kalman/simple_kalman.py
index 33289e4f50..5e1b6ce1fb 100644
--- a/common/kalman/simple_kalman.py
+++ b/common/kalman/simple_kalman.py
@@ -1,3 +1,12 @@
-# pylint: skip-file
-from common.kalman.simple_kalman_impl import KF1D as KF1D
+from openpilot.common.kalman.simple_kalman_impl import KF1D as KF1D
 assert KF1D
+import numpy as np
+
+def get_kalman_gain(dt, A, C, Q, R, iterations=100):
+  P = np.zeros_like(Q)
+  for _ in range(iterations):
+    P = A.dot(P).dot(A.T) + dt * Q
+    S = C.dot(P).dot(C.T) + R
+    K = P.dot(C.T).dot(np.linalg.inv(S))
+    P = (np.eye(len(P)) - K.dot(C)).dot(P)
+  return K
\ No newline at end of file
diff --git a/common/kalman/tests/test_simple_kalman.py b/common/kalman/tests/test_simple_kalman.py
index 96b2527655..32cc79fc3d 100644
--- a/common/kalman/tests/test_simple_kalman.py
+++ b/common/kalman/tests/test_simple_kalman.py
@@ -3,8 +3,8 @@ import random
 import timeit
 import numpy as np
 
-from common.kalman.simple_kalman import KF1D
-from common.kalman.simple_kalman_old import KF1D as KF1D_old
+from openpilot.common.kalman.simple_kalman import KF1D
+from openpilot.common.kalman.simple_kalman_old import KF1D as KF1D_old
 
 
 class TestSimpleKalman(unittest.TestCase):
@@ -54,8 +54,8 @@ class TestSimpleKalman(unittest.TestCase):
     setup = """
 import numpy as np
 
-from common.kalman.simple_kalman import KF1D
-from common.kalman.simple_kalman_old import KF1D as KF1D_old
+from openpilot.common.kalman.simple_kalman import KF1D
+from openpilot.common.kalman.simple_kalman_old import KF1D as KF1D_old
 
 dt = 0.01
 x0_0 = 0.0
diff --git a/common/logging_extra.py b/common/logging_extra.py
index 899ad7a391..5e0584c7bc 100644
--- a/common/logging_extra.py
+++ b/common/logging_extra.py
@@ -65,7 +65,7 @@ class SwagFormatter(logging.Formatter):
 
     return record_dict
 
-  def format(self, record):
+  def format(self, record): # noqa: A003
     if self.swaglogger is None:
       raise Exception("must set swaglogger before calling format()")
     return json_robust_dumps(self.format_dict(record))
@@ -95,7 +95,7 @@ class SwagLogFileFormatter(SwagFormatter):
       k += "$a"
     return k, v
 
-  def format(self, record):
+  def format(self, record): # noqa: A003
     if isinstance(record, str):
       v = json.loads(record)
     else:
@@ -197,7 +197,7 @@ class SwagLogger(logging.Logger):
       filename = os.path.normcase(co.co_filename)
 
       # TODO: is this pylint exception correct?
-      if filename == _srcfile:  # pylint: disable=comparison-with-callable
+      if filename == _srcfile:
         f = f.f_back
         continue
       sinfo = None
diff --git a/common/mat.h b/common/mat.h
index 626f3404fe..8e10d61971 100644
--- a/common/mat.h
+++ b/common/mat.h
@@ -1,7 +1,7 @@
 #pragma once
 
 typedef struct vec3 {
-	float v[3];
+  float v[3];
 } vec3;
 
 typedef struct vec4 {
@@ -9,7 +9,7 @@ typedef struct vec4 {
 } vec4;
 
 typedef struct mat3 {
-	float v[3*3];
+  float v[3*3];
 } mat3;
 
 typedef struct mat4 {
diff --git a/common/params.cc b/common/params.cc
index e8ab42c0b0..54168165a1 100644
--- a/common/params.cc
+++ b/common/params.cc
@@ -64,7 +64,9 @@ bool create_params_path(const std::string &param_path, const std::string &key_pa
 std::string ensure_params_path(const std::string &prefix, const std::string &path = {}) {
   std::string params_path = path.empty() ? Path::params() : path;
   if (!create_params_path(params_path, params_path + prefix)) {
-    throw std::runtime_error(util::string_format("Failed to ensure params path, errno=%d", errno));
+    throw std::runtime_error(util::string_format(
+        "Failed to ensure params path, errno=%d, path=%s, param_prefix=%s",
+        errno, params_path.c_str(), prefix.c_str()));
   }
   return params_path;
 }
@@ -86,7 +88,6 @@ private:
 std::unordered_map<std::string, uint32_t> keys = {
     {"AccessToken", CLEAR_ON_MANAGER_START | DONT_LOG},
     {"ApiCache_Device", PERSISTENT},
-    {"ApiCache_DriveStats", PERSISTENT},
     {"ApiCache_NavDestinations", PERSISTENT},
     {"AssistNowToken", PERSISTENT},
     {"AthenadPid", PERSISTENT},
@@ -98,6 +99,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"CarParamsCache", CLEAR_ON_MANAGER_START},
     {"CarParamsPersistent", PERSISTENT},
+    {"CarParamsPrevRoute", PERSISTENT},
     {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"CompletedTrainingVersion", PERSISTENT},
     {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
@@ -153,11 +155,11 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"LastUpdateException", CLEAR_ON_MANAGER_START},
     {"LastUpdateTime", PERSISTENT},
     {"LiveParameters", PERSISTENT},
-    {"LiveTorqueCarParams", PERSISTENT},
     {"LiveTorqueParameters", PERSISTENT | DONT_LOG},
     {"LongitudinalPersonality", PERSISTENT},
     {"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
     {"NavDestinationWaypoints", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
+    {"NavPastDestinations", PERSISTENT},
     {"NavSettingLeftSide", PERSISTENT},
     {"NavSettingTime24h", PERSISTENT},
     {"NavdRender", PERSISTENT},
@@ -178,7 +180,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START},
     {"OpenpilotEnabledToggle", PERSISTENT},
     {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
-    {"PandaLogState", PERSISTENT},
+    {"PandaSomResetTriggered", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
     {"PandaSignatures", CLEAR_ON_MANAGER_START},
     {"Passive", PERSISTENT},
     {"PrimeType", PERSISTENT},
diff --git a/common/params.h b/common/params.h
index 24b1bffeb1..fbe0bba6b0 100644
--- a/common/params.h
+++ b/common/params.h
@@ -15,7 +15,11 @@ enum ParamKeyType {
 
 class Params {
 public:
-  Params(const std::string &path = {});
+  explicit Params(const std::string &path = {});
+  // Not copyable.
+  Params(const Params&) = delete;
+  Params& operator=(const Params&) = delete;
+
   std::vector<std::string> allKeys() const;
   bool checkKey(const std::string &key);
   ParamKeyType getKeyType(const std::string &key);
diff --git a/common/params.py b/common/params.py
index b6be424d41..ea8ac7514a 100644
--- a/common/params.py
+++ b/common/params.py
@@ -1,4 +1,5 @@
-from common.params_pyx import Params, ParamKeyType, UnknownKeyName, put_nonblocking, put_bool_nonblocking # pylint: disable=no-name-in-module, import-error
+from openpilot.common.params_pyx import Params, ParamKeyType, UnknownKeyName, put_nonblocking, \
+                                        put_bool_nonblocking
 assert Params
 assert ParamKeyType
 assert UnknownKeyName
diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx
old mode 100755
new mode 100644
index abb3199d05..de52c490b3
--- a/common/params_pyx.pyx
+++ b/common/params_pyx.pyx
@@ -14,7 +14,7 @@ cdef extern from "common/params.h":
     ALL
 
   cdef cppclass c_Params "Params":
-    c_Params(string) nogil
+    c_Params(string) except + nogil
     string get(string, bool) nogil
     bool getBool(string, bool) nogil
     int remove(string) nogil
diff --git a/common/ratekeeper.cc b/common/ratekeeper.cc
new file mode 100644
index 0000000000..7e63815168
--- /dev/null
+++ b/common/ratekeeper.cc
@@ -0,0 +1,40 @@
+#include "common/ratekeeper.h"
+
+#include <algorithm>
+
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+
+RateKeeper::RateKeeper(const std::string &name, float rate, float print_delay_threshold)
+    : name(name),
+      print_delay_threshold(std::max(0.f, print_delay_threshold)) {
+  interval = 1 / rate;
+  last_monitor_time = seconds_since_boot();
+  next_frame_time = last_monitor_time + interval;
+}
+
+bool RateKeeper::keepTime() {
+  bool lagged = monitorTime();
+  if (remaining_ > 0) {
+    util::sleep_for(remaining_ * 1000);
+  }
+  return lagged;
+}
+
+bool RateKeeper::monitorTime() {
+  ++frame_;
+  last_monitor_time = seconds_since_boot();
+  remaining_ = next_frame_time - last_monitor_time;
+
+  bool lagged = remaining_ < 0;
+  if (lagged) {
+    if (print_delay_threshold > 0 && remaining_ < -print_delay_threshold) {
+      LOGW("%s lagging by %.2f ms", name.c_str(), -remaining_ * 1000);
+    }
+    next_frame_time = last_monitor_time + interval;
+  } else {
+    next_frame_time += interval;
+  }
+  return lagged;
+}
diff --git a/common/ratekeeper.h b/common/ratekeeper.h
new file mode 100644
index 0000000000..b6e8ac66a6
--- /dev/null
+++ b/common/ratekeeper.h
@@ -0,0 +1,23 @@
+#pragma once
+
+#include <cstdint>
+#include <string>
+
+class RateKeeper {
+public:
+  RateKeeper(const std::string &name, float rate, float print_delay_threshold = 0);
+  ~RateKeeper() {}
+  bool keepTime();
+  bool monitorTime();
+  inline double frame() const { return frame_; }
+  inline double remaining() const { return remaining_; }
+
+private:
+  double interval;
+  double next_frame_time;
+  double last_monitor_time;
+  double remaining_ = 0;
+  float print_delay_threshold = 0;
+  uint64_t frame_ = 0;
+  std::string name;
+};
diff --git a/common/realtime.py b/common/realtime.py
index 7dd2eb98a6..05e19e770f 100644
--- a/common/realtime.py
+++ b/common/realtime.py
@@ -5,10 +5,9 @@ import time
 from collections import deque
 from typing import Optional, List, Union
 
-from setproctitle import getproctitle  # pylint: disable=no-name-in-module
+from setproctitle import getproctitle
 
-from common.clock import sec_since_boot  # pylint: disable=no-name-in-module, import-error
-from system.hardware import PC
+from openpilot.system.hardware import PC
 
 
 # time step for each process
@@ -31,12 +30,12 @@ class Priority:
 
 def set_realtime_priority(level: int) -> None:
   if not PC:
-    os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level))  # pylint: disable=no-member
+    os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level))
 
 
 def set_core_affinity(cores: List[int]) -> None:
   if not PC:
-    os.sched_setaffinity(0, cores)  # pylint: disable=no-member
+    os.sched_setaffinity(0, cores)
 
 
 def config_realtime_process(cores: Union[int, List[int]], priority: int) -> None:
@@ -50,13 +49,13 @@ class Ratekeeper:
   def __init__(self, rate: float, print_delay_threshold: Optional[float] = 0.0) -> None:
     """Rate in Hz for ratekeeping. print_delay_threshold must be nonnegative."""
     self._interval = 1. / rate
-    self._next_frame_time = sec_since_boot() + self._interval
+    self._next_frame_time = time.monotonic() + self._interval
     self._print_delay_threshold = print_delay_threshold
     self._frame = 0
     self._remaining = 0.0
     self._process_name = getproctitle()
     self._dts = deque([self._interval], maxlen=100)
-    self._last_monitor_time = sec_since_boot()
+    self._last_monitor_time = time.monotonic()
 
   @property
   def frame(self) -> int:
@@ -82,11 +81,11 @@ class Ratekeeper:
   # this only monitor the cumulative lag, but does not enforce a rate
   def monitor_time(self) -> bool:
     prev = self._last_monitor_time
-    self._last_monitor_time = sec_since_boot()
+    self._last_monitor_time = time.monotonic()
     self._dts.append(self._last_monitor_time - prev)
 
     lagged = False
-    remaining = self._next_frame_time - sec_since_boot()
+    remaining = self._next_frame_time - time.monotonic()
     self._next_frame_time += self._interval
     if self._print_delay_threshold is not None and remaining < -self._print_delay_threshold:
       print(f"{self._process_name} lagging by {-remaining * 1000:.2f} ms")
diff --git a/common/spinner.py b/common/spinner.py
index 57242d644d..43d4bb2cc2 100644
--- a/common/spinner.py
+++ b/common/spinner.py
@@ -1,6 +1,6 @@
 import os
 import subprocess
-from common.basedir import BASEDIR
+from openpilot.common.basedir import BASEDIR
 
 
 class Spinner():
@@ -29,11 +29,11 @@ class Spinner():
 
   def close(self):
     if self.spinner_proc is not None:
+      self.spinner_proc.kill()
       try:
-        self.spinner_proc.stdin.close()
-      except BrokenPipeError:
-        pass
-      self.spinner_proc.terminate()
+        self.spinner_proc.communicate(timeout=2.)
+      except subprocess.TimeoutExpired:
+        print("WARNING: failed to kill spinner")
       self.spinner_proc = None
 
   def __del__(self):
diff --git a/common/swaglog.cc b/common/swaglog.cc
index 060090e18f..bcd597a257 100644
--- a/common/swaglog.cc
+++ b/common/swaglog.cc
@@ -12,7 +12,7 @@
 #include <string>
 
 #include <zmq.h>
-#include "json11.hpp"
+#include "third_party/json11/json11.hpp"
 
 #include "common/util.h"
 #include "common/version.h"
@@ -20,7 +20,7 @@
 
 class SwaglogState : public LogState {
  public:
-  SwaglogState() : LogState("ipc:///tmp/logmessage") {}
+  SwaglogState() : LogState(Path::swaglog_ipc().c_str()) {}
 
   json11::Json::object ctx_j;
 
@@ -64,8 +64,7 @@ static void log(int levelnum, const char* filename, int lineno, const char* func
   if (levelnum >= s.print_level) {
     printf("%s: %s\n", filename, msg);
   }
-  char levelnum_c = levelnum;
-  zmq_send(s.sock, (levelnum_c + log_s).c_str(), log_s.length() + 1, ZMQ_NOBLOCK);
+  zmq_send(s.sock, log_s.data(), log_s.length(), ZMQ_NOBLOCK);
 }
 
 static void cloudlog_common(int levelnum, const char* filename, int lineno, const char* func,
@@ -87,8 +86,11 @@ static void cloudlog_common(int levelnum, const char* filename, int lineno, cons
     log_j["msg"] = msg_j;
   }
 
-  std::string log_s = ((json11::Json)log_j).dump();
+  std::string log_s;
+  log_s += (char)levelnum;
+  ((json11::Json)log_j).dump(log_s);
   log(levelnum, filename, lineno, func, msg_buf, log_s);
+
   free(msg_buf);
 }
 
diff --git a/common/swaglog.h b/common/swaglog.h
index 25368501ac..06d45b1d98 100644
--- a/common/swaglog.h
+++ b/common/swaglog.h
@@ -9,14 +9,20 @@
 #define CLOUDLOG_CRITICAL 50
 
 
+#ifdef __GNUC__
+#define SWAG_LOG_CHECK_FMT(a, b) __attribute__ ((format (printf, a, b)))
+#else
+#define SWAG_LOG_CHECK_FMT(a, b)
+#endif
+
 void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func,
-                const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
+                const char* fmt, ...) SWAG_LOG_CHECK_FMT(5, 6);
 
 void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func,
-                 const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
+                 const char* fmt, ...) SWAG_LOG_CHECK_FMT(5, 6);
 
 void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func,
-                 uint32_t frame_id, const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
+                 uint32_t frame_id, const char* fmt, ...) SWAG_LOG_CHECK_FMT(6, 7);
 
 
 #define cloudlog(lvl, fmt, ...) cloudlog_e(lvl, __FILE__, __LINE__, \
@@ -38,7 +44,7 @@ void cloudlog_te(int levelnum, const char* filename, int lineno, const char* fun
   int __millis = (millis);                          \
   uint64_t __ts = nanos_since_boot();               \
                                                     \
-  if (!__begin) __begin = __ts;                     \
+  if (!__begin) { __begin = __ts; }                 \
                                                     \
   if (__begin + __millis*1000000ULL < __ts) {       \
     if (__missed) {                                 \
diff --git a/common/text_window.py b/common/text_window.py
index bea3a149f8..d2762ebf7d 100755
--- a/common/text_window.py
+++ b/common/text_window.py
@@ -2,7 +2,7 @@
 import os
 import time
 import subprocess
-from common.basedir import BASEDIR
+from openpilot.common.basedir import BASEDIR
 
 
 class TextWindow:
diff --git a/common/transformations/SConscript b/common/transformations/SConscript
index ee9b9a2b73..4ac73a165e 100644
--- a/common/transformations/SConscript
+++ b/common/transformations/SConscript
@@ -1,6 +1,5 @@
 Import('env', 'envCython')
 
 transformations = env.Library('transformations', ['orientation.cc', 'coordinates.cc'])
-Export('transformations')
-
-envCython.Program('transformations.so', 'transformations.pyx')
+transformations_python = envCython.Program('transformations.so', 'transformations.pyx')
+Export('transformations', 'transformations_python')
diff --git a/common/transformations/camera.py b/common/transformations/camera.py
index b20ed5c64b..c643cb5702 100644
--- a/common/transformations/camera.py
+++ b/common/transformations/camera.py
@@ -1,6 +1,6 @@
 import numpy as np
 
-import common.transformations.orientation as orient
+import openpilot.common.transformations.orientation as orient
 
 ## -- hardcoded hardware params --
 eon_f_focal_length = 910.0
@@ -61,14 +61,6 @@ device_frame_from_view_frame = np.array([
 view_frame_from_device_frame = device_frame_from_view_frame.T
 
 
-def get_calib_from_vp(vp):
-  vp_norm = normalize(vp)
-  yaw_calib = np.arctan(vp_norm[0])
-  pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib))
-  roll_calib = 0
-  return roll_calib, pitch_calib, yaw_calib
-
-
 # aka 'extrinsic_matrix'
 # road : x->forward, y -> left, z->up
 def get_view_frame_from_road_frame(roll, pitch, yaw, height):
@@ -131,6 +123,14 @@ def denormalize(img_pts, intrinsics=fcam_intrinsics, width=np.inf, height=np.inf
   return img_pts_denormalized[:, :2].reshape(input_shape)
 
 
+def get_calib_from_vp(vp, intrinsics=fcam_intrinsics):
+  vp_norm = normalize(vp, intrinsics)
+  yaw_calib = np.arctan(vp_norm[0])
+  pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib))
+  roll_calib = 0
+  return roll_calib, pitch_calib, yaw_calib
+
+
 def device_from_ecef(pos_ecef, orientation_ecef, pt_ecef):
   # device from ecef frame
   # device frame is x -> forward, y-> right, z -> down
diff --git a/common/transformations/coordinates.cc b/common/transformations/coordinates.cc
index b729ac3d87..5b00b53a4f 100644
--- a/common/transformations/coordinates.cc
+++ b/common/transformations/coordinates.cc
@@ -1,13 +1,11 @@
 #define _USE_MATH_DEFINES
 
+#include "common/transformations/coordinates.hpp"
+
 #include <iostream>
 #include <cmath>
 #include <eigen3/Eigen/Dense>
 
-#include "coordinates.hpp"
-
-
-
 double a = 6378137; // lgtm [cpp/short-global-name]
 double b = 6356752.3142; // lgtm [cpp/short-global-name]
 double esq = 6.69437999014 * 0.001; // lgtm [cpp/short-global-name]
diff --git a/common/transformations/coordinates.hpp b/common/transformations/coordinates.hpp
index f5ba0d3fe7..32ec2ff66e 100644
--- a/common/transformations/coordinates.hpp
+++ b/common/transformations/coordinates.hpp
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <eigen3/Eigen/Dense>
+
 #define DEG2RAD(x) ((x) * M_PI / 180.0)
 #define RAD2DEG(x) ((x) * 180.0 / M_PI)
 
diff --git a/common/transformations/coordinates.py b/common/transformations/coordinates.py
index 46cc0ded0d..696e7de2e5 100644
--- a/common/transformations/coordinates.py
+++ b/common/transformations/coordinates.py
@@ -1,8 +1,7 @@
-# pylint: skip-file
-from common.transformations.orientation import numpy_wrap
-from common.transformations.transformations import (ecef2geodetic_single,
+from openpilot.common.transformations.orientation import numpy_wrap
+from openpilot.common.transformations.transformations import (ecef2geodetic_single,
                                                     geodetic2ecef_single)
-from common.transformations.transformations import LocalCoord as LocalCoord_single
+from openpilot.common.transformations.transformations import LocalCoord as LocalCoord_single
 
 
 class LocalCoord(LocalCoord_single):
diff --git a/common/transformations/model.py b/common/transformations/model.py
index 811a17eafe..7e40767f63 100644
--- a/common/transformations/model.py
+++ b/common/transformations/model.py
@@ -1,7 +1,9 @@
 import numpy as np
 
-from common.transformations.camera import (FULL_FRAME_SIZE,
-                                           get_view_frame_from_calib_frame)
+from openpilot.common.transformations.orientation import rot_from_euler
+from openpilot.common.transformations.camera import (
+  FULL_FRAME_SIZE, get_view_frame_from_calib_frame, view_frame_from_device_frame,
+  eon_fcam_intrinsics, tici_ecam_intrinsics, tici_fcam_intrinsics)
 
 # segnet
 SEGNET_SIZE = (512, 384)
@@ -57,61 +59,20 @@ medmodel_frame_from_calib_frame = np.dot(medmodel_intrinsics,
 
 medmodel_frame_from_bigmodel_frame = np.dot(medmodel_intrinsics, np.linalg.inv(bigmodel_intrinsics))
 
+calib_from_medmodel = np.linalg.inv(medmodel_frame_from_calib_frame[:, :3])
+calib_from_sbigmodel = np.linalg.inv(sbigmodel_frame_from_calib_frame[:, :3])
 
-### This function mimics the update_calibration logic in modeld.cc
-### Manually verified to give similar results to xx.uncommon.utils.transform_img
-def get_warp_matrix(rpy_calib, wide_cam=False, big_model=False, tici=True):
-  from common.transformations.orientation import rot_from_euler
-  from common.transformations.camera import view_frame_from_device_frame, eon_fcam_intrinsics, tici_ecam_intrinsics, tici_fcam_intrinsics
-
-  if tici and wide_cam:
-    intrinsics = tici_ecam_intrinsics
-  elif tici:
-    intrinsics = tici_fcam_intrinsics
-  else:
-    intrinsics = eon_fcam_intrinsics
-
-  if big_model:
-    sbigmodel_from_calib = sbigmodel_frame_from_calib_frame[:, (0,1,2)]
-    calib_from_model = np.linalg.inv(sbigmodel_from_calib)
-  else:
-    medmodel_from_calib = medmodel_frame_from_calib_frame[:, (0,1,2)]
-    calib_from_model = np.linalg.inv(medmodel_from_calib)
-  device_from_calib = rot_from_euler(rpy_calib)
-  camera_from_calib = intrinsics.dot(view_frame_from_device_frame.dot(device_from_calib))
-  warp_matrix = camera_from_calib.dot(calib_from_model)
-  return warp_matrix
-
-
-### This is old, just for debugging
-def get_warp_matrix_old(rpy_calib, wide_cam=False, big_model=False, tici=True):
-  from common.transformations.orientation import rot_from_euler
-  from common.transformations.camera import view_frame_from_device_frame, eon_fcam_intrinsics, tici_ecam_intrinsics, tici_fcam_intrinsics
-
-
-  def get_view_frame_from_road_frame(roll, pitch, yaw, height):
-    device_from_road = rot_from_euler([roll, pitch, yaw]).dot(np.diag([1, -1, -1]))
-    view_from_road = view_frame_from_device_frame.dot(device_from_road)
-    return np.hstack((view_from_road, [[0], [height], [0]]))
-
-  if tici and wide_cam:
-    intrinsics = tici_ecam_intrinsics
+# This function is verified to give similar results to xx.uncommon.utils.transform_img
+def get_warp_matrix(device_from_calib_euler: np.ndarray, wide_camera: bool = False, bigmodel_frame: bool = False, tici: bool = True) -> np.ndarray:
+  if tici and wide_camera:
+    cam_intrinsics = tici_ecam_intrinsics
   elif tici:
-    intrinsics = tici_fcam_intrinsics
+    cam_intrinsics = tici_fcam_intrinsics
   else:
-    intrinsics = eon_fcam_intrinsics
+    cam_intrinsics = eon_fcam_intrinsics
 
-  model_height = 1.22
-  if big_model:
-    model_from_road = np.dot(sbigmodel_intrinsics,
-             get_view_frame_from_road_frame(0, 0, 0, model_height))
-  else:
-    model_from_road = np.dot(medmodel_intrinsics,
-             get_view_frame_from_road_frame(0, 0, 0, model_height))
-  ground_from_model = np.linalg.inv(model_from_road[:, (0, 1, 3)])
-
-  E = get_view_frame_from_road_frame(*rpy_calib, 1.22)
-  camera_frame_from_road_frame = intrinsics.dot(E)
-  camera_frame_from_ground = camera_frame_from_road_frame[:,(0,1,3)]
-  warp_matrix = camera_frame_from_ground .dot(ground_from_model)
+  calib_from_model = calib_from_sbigmodel if bigmodel_frame else calib_from_medmodel
+  device_from_calib = rot_from_euler(device_from_calib_euler)
+  camera_from_calib = cam_intrinsics @ view_frame_from_device_frame @ device_from_calib
+  warp_matrix: np.ndarray = camera_from_calib @ calib_from_model
   return warp_matrix
diff --git a/common/transformations/orientation.cc b/common/transformations/orientation.cc
index 7909c0affb..00888c3a92 100644
--- a/common/transformations/orientation.cc
+++ b/common/transformations/orientation.cc
@@ -4,8 +4,8 @@
 #include <cmath>
 #include <eigen3/Eigen/Dense>
 
-#include "orientation.hpp"
-#include "coordinates.hpp"
+#include "common/transformations/orientation.hpp"
+#include "common/transformations/coordinates.hpp"
 
 Eigen::Quaterniond ensure_unique(Eigen::Quaterniond quat){
   if (quat.w() > 0){
diff --git a/common/transformations/orientation.hpp b/common/transformations/orientation.hpp
index ebd7da0aee..150b12cade 100644
--- a/common/transformations/orientation.hpp
+++ b/common/transformations/orientation.hpp
@@ -1,6 +1,6 @@
 #pragma once
 #include <eigen3/Eigen/Dense>
-#include "coordinates.hpp"
+#include "common/transformations/coordinates.hpp"
 
 
 Eigen::Quaterniond ensure_unique(Eigen::Quaterniond quat);
diff --git a/common/transformations/orientation.py b/common/transformations/orientation.py
index 134442b624..ce4378738d 100644
--- a/common/transformations/orientation.py
+++ b/common/transformations/orientation.py
@@ -1,8 +1,7 @@
-# pylint: skip-file
 import numpy as np
 from typing import Callable
 
-from common.transformations.transformations import (ecef_euler_from_ned_single,
+from openpilot.common.transformations.transformations import (ecef_euler_from_ned_single,
                                                     euler2quat_single,
                                                     euler2rot_single,
                                                     ned_euler_from_ecef_single,
diff --git a/common/transformations/transformations.pyx b/common/transformations/transformations.pyx
index ce80d90d29..c5cb9e0056 100644
--- a/common/transformations/transformations.pyx
+++ b/common/transformations/transformations.pyx
@@ -1,20 +1,20 @@
 # distutils: language = c++
 # cython: language_level = 3
-from common.transformations.transformations cimport Matrix3, Vector3, Quaternion
-from common.transformations.transformations cimport ECEF, NED, Geodetic
-
-from common.transformations.transformations cimport euler2quat as euler2quat_c
-from common.transformations.transformations cimport quat2euler as quat2euler_c
-from common.transformations.transformations cimport quat2rot as quat2rot_c
-from common.transformations.transformations cimport rot2quat as rot2quat_c
-from common.transformations.transformations cimport euler2rot as euler2rot_c
-from common.transformations.transformations cimport rot2euler as rot2euler_c
-from common.transformations.transformations cimport rot_matrix as rot_matrix_c
-from common.transformations.transformations cimport ecef_euler_from_ned as ecef_euler_from_ned_c
-from common.transformations.transformations cimport ned_euler_from_ecef as ned_euler_from_ecef_c
-from common.transformations.transformations cimport geodetic2ecef as geodetic2ecef_c
-from common.transformations.transformations cimport ecef2geodetic as ecef2geodetic_c
-from common.transformations.transformations cimport LocalCoord_c
+from openpilot.common.transformations.transformations cimport Matrix3, Vector3, Quaternion
+from openpilot.common.transformations.transformations cimport ECEF, NED, Geodetic
+
+from openpilot.common.transformations.transformations cimport euler2quat as euler2quat_c
+from openpilot.common.transformations.transformations cimport quat2euler as quat2euler_c
+from openpilot.common.transformations.transformations cimport quat2rot as quat2rot_c
+from openpilot.common.transformations.transformations cimport rot2quat as rot2quat_c
+from openpilot.common.transformations.transformations cimport euler2rot as euler2rot_c
+from openpilot.common.transformations.transformations cimport rot2euler as rot2euler_c
+from openpilot.common.transformations.transformations cimport rot_matrix as rot_matrix_c
+from openpilot.common.transformations.transformations cimport ecef_euler_from_ned as ecef_euler_from_ned_c
+from openpilot.common.transformations.transformations cimport ned_euler_from_ecef as ned_euler_from_ecef_c
+from openpilot.common.transformations.transformations cimport geodetic2ecef as geodetic2ecef_c
+from openpilot.common.transformations.transformations cimport ecef2geodetic as ecef2geodetic_c
+from openpilot.common.transformations.transformations cimport LocalCoord_c
 
 
 import cython
diff --git a/common/util.cc b/common/util.cc
index 55a8b1fb3e..8af9f5884c 100644
--- a/common/util.cc
+++ b/common/util.cc
@@ -2,7 +2,7 @@
 
 #include <sys/ioctl.h>
 #include <sys/stat.h>
-#include <dirent.h>
+#include <sys/resource.h>
 
 #include <cassert>
 #include <cerrno>
@@ -60,6 +60,20 @@ int set_core_affinity(std::vector<int> cores) {
 #endif
 }
 
+int set_file_descriptor_limit(uint64_t limit_val) {
+  struct rlimit limit;
+  int status;
+
+  if ((status = getrlimit(RLIMIT_NOFILE, &limit)) < 0)
+    return status;
+
+  limit.rlim_cur = limit_val;
+  if ((status = setrlimit(RLIMIT_NOFILE, &limit)) < 0)
+    return status;
+
+  return 0;
+}
+
 std::string read_file(const std::string& fn) {
   std::ifstream f(fn, std::ios::binary | std::ios::in);
   if (f.is_open()) {
@@ -213,10 +227,17 @@ std::string hexdump(const uint8_t* in, const size_t size) {
   return ss.str();
 }
 
+int random_int(int min, int max) {
+  std::random_device dev;
+  std::mt19937 rng(dev());
+  std::uniform_int_distribution<std::mt19937::result_type> dist(min, max);
+  return dist(rng);
+}
+
 std::string random_string(std::string::size_type length) {
-  const char* chrs = "0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ";
+  const std::string chrs = "0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ";
   std::mt19937 rg{std::random_device{}()};
-  std::uniform_int_distribution<std::string::size_type> pick(0, sizeof(chrs) - 2);
+  std::uniform_int_distribution<std::string::size_type> pick(0, chrs.length() - 1);
   std::string s;
   s.reserve(length);
   while (length--) {
@@ -231,6 +252,14 @@ std::string dir_name(std::string const &path) {
   return path.substr(0, pos);
 }
 
+bool starts_with(const std::string &s1, const std::string &s2) {
+  return strncmp(s1.c_str(), s2.c_str(), s2.size()) == 0;
+}
+
+bool ends_with(const std::string &s1, const std::string &s2) {
+  return strcmp(s1.c_str() + (s1.size() - s2.size()), s2.c_str()) == 0;
+}
+
 std::string check_output(const std::string& command) {
   char buffer[128];
   std::string result;
diff --git a/common/util.h b/common/util.h
index 34721700e7..5c0ef7fec5 100644
--- a/common/util.h
+++ b/common/util.h
@@ -44,6 +44,7 @@ namespace util {
 void set_thread_name(const char* name);
 int set_realtime_priority(int level);
 int set_core_affinity(std::vector<int> cores);
+int set_file_descriptor_limit(uint64_t limit);
 
 // ***** Time helpers *****
 struct tm get_time();
@@ -75,10 +76,15 @@ int getenv(const char* key, int default_val);
 float getenv(const char* key, float default_val);
 
 std::string hexdump(const uint8_t* in, const size_t size);
-std::string random_string(std::string::size_type length);
 std::string dir_name(std::string const& path);
+bool starts_with(const std::string &s1, const std::string &s2);
+bool ends_with(const std::string &s1, const std::string &s2);
+
+// ***** random helpers *****
+int random_int(int min, int max);
+std::string random_string(std::string::size_type length);
 
-// **** file fhelpers *****
+// **** file helpers *****
 std::string read_file(const std::string& fn);
 std::map<std::string, std::string> read_files_in_dir(const std::string& path);
 int write_file(const char* path, const void* data, size_t size, int flags = O_WRONLY, mode_t mode = 0664);
@@ -111,7 +117,7 @@ public:
 #ifndef __APPLE__
     std::signal(SIGPWR, (sighandler_t)set_do_exit);
 #endif
-  };
+  }
   inline static std::atomic<bool> power_failure = false;
   inline static std::atomic<int> signal = 0;
   inline operator bool() { return do_exit; }
@@ -147,12 +153,18 @@ struct unique_fd {
 
 class FirstOrderFilter {
 public:
-  FirstOrderFilter(float x0, float ts, float dt) {
+  FirstOrderFilter(float x0, float ts, float dt, bool initialized = true) {
     k_ = (dt / ts) / (1.0 + dt / ts);
     x_ = x0;
+    initialized_ = initialized;
   }
   inline float update(float x) {
-    x_ = (1. - k_) * x_ + k_ * x;
+    if (initialized_) {
+      x_ = (1. - k_) * x_ + k_ * x;
+    } else {
+      initialized_ = true;
+      x_ = x;
+    }
     return x_;
   }
   inline void reset(float x) { x_ = x; }
@@ -160,12 +172,13 @@ public:
 
 private:
   float x_, k_;
+  bool initialized_;
 };
 
 template<typename T>
 void update_max_atomic(std::atomic<T>& max, T const& value) {
   T prev = max;
-  while(prev < value && !max.compare_exchange_weak(prev, value)) {}
+  while (prev < value && !max.compare_exchange_weak(prev, value)) {}
 }
 
 class LogState {
@@ -175,9 +188,9 @@ class LogState {
   void *zctx = nullptr;
   void *sock = nullptr;
   int print_level;
-  const char* endpoint;
+  std::string endpoint;
 
-  LogState(const char* _endpoint) {
+  LogState(std::string _endpoint) {
     endpoint = _endpoint;
   }
 
@@ -189,7 +202,7 @@ class LogState {
     int timeout = 100;
     zmq_setsockopt(sock, ZMQ_LINGER, &timeout, sizeof(timeout));
 
-    zmq_connect(sock, endpoint);
+    zmq_connect(sock, endpoint.c_str());
     initialized = true;
   }
 
diff --git a/common/version.h b/common/version.h
index 6af006aabb..946f6a1fec 100644
--- a/common/version.h
+++ b/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "0.9.4"
+#define COMMA_VERSION "0.9.5"
diff --git a/common/watchdog.cc b/common/watchdog.cc
index 920df4030a..3483ad21c2 100644
--- a/common/watchdog.cc
+++ b/common/watchdog.cc
@@ -1,3 +1,5 @@
+#include <string>
+
 #include "common/watchdog.h"
 #include "common/util.h"
 
diff --git a/docs/CARS.md b/docs/CARS.md
index 96eecd4860..044db1f0ec 100644
--- a/docs/CARS.md
+++ b/docs/CARS.md
@@ -2,280 +2,293 @@
 
 # Supported Cars
 
-A supported vehicle is one that just works when you install a comma three. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified.
+A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified.
 
-# 255 Supported Cars
+# 268 Supported Cars
 
 |Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|<a href="##"><img width=2000></a>Hardware Needed<br>&nbsp;|Video|
 |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
-|Acura|ILX 2016-19|AcuraWatch Plus|openpilot|25 mph|25 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Acura&model=ILX 2016-19">Buy Here</a></sub></details>||
-|Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Acura&model=RDX 2016-18">Buy Here</a></sub></details>||
-|Acura|RDX 2019-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Acura&model=RDX 2019-22">Buy Here</a></sub></details>||
-|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Audi&model=A3 2014-19">Buy Here</a></sub></details>||
-|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Audi&model=A3 Sportback e-tron 2017-18">Buy Here</a></sub></details>||
-|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Audi&model=Q2 2018">Buy Here</a></sub></details>||
-|Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Audi&model=Q3 2019-23">Buy Here</a></sub></details>||
-|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Audi&model=RS3 2018">Buy Here</a></sub></details>||
-|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Audi&model=S3 2015-17">Buy Here</a></sub></details>||
-|Buick|LaCrosse 2017-19[<sup>4</sup>](#footnotes)|Driver Confidence Package 2|openpilot|18 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 OBD-II connector<br>- 1 comma three<br>- 2 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Buick&model=LaCrosse 2017-19">Buy Here</a></sub></details>||
-|Cadillac|Escalade 2017[<sup>4</sup>](#footnotes)|Driver Assist Package|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 OBD-II connector<br>- 1 comma three<br>- 2 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Cadillac&model=Escalade 2017">Buy Here</a></sub></details>||
-|Cadillac|Escalade ESV 2016[<sup>4</sup>](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 OBD-II connector<br>- 1 comma three<br>- 2 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Cadillac&model=Escalade ESV 2016">Buy Here</a></sub></details>||
-|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 GM connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Chevrolet&model=Bolt EUV 2022-23">Buy Here</a></sub></details>|<a href="https://youtu.be/xvwzGMUA210" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Chevrolet|Bolt EV 2022-23|2LT Trim with Adaptive Cruise Control Package|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 GM connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Chevrolet&model=Bolt EV 2022-23">Buy Here</a></sub></details>||
-|Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 GM connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Chevrolet&model=Silverado 1500 2020-21">Buy Here</a></sub></details>||
-|Chevrolet|Trailblazer 2021-22|Adaptive Cruise Control (ACC)|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 GM connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Chevrolet&model=Trailblazer 2021-22">Buy Here</a></sub></details>||
-|Chevrolet|Volt 2017-18[<sup>4</sup>](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 OBD-II connector<br>- 1 comma three<br>- 2 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Chevrolet&model=Volt 2017-18">Buy Here</a></sub></details>|<a href="https://youtu.be/QeMCN_4TFfQ" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Chrysler|Pacifica 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 FCA connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Chrysler&model=Pacifica 2017-18">Buy Here</a></sub></details>||
-|Chrysler|Pacifica 2019-20|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 FCA connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Chrysler&model=Pacifica 2019-20">Buy Here</a></sub></details>||
-|Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 FCA connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Chrysler&model=Pacifica 2021">Buy Here</a></sub></details>||
-|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 FCA connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Chrysler&model=Pacifica Hybrid 2017-18">Buy Here</a></sub></details>||
-|Chrysler|Pacifica Hybrid 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 FCA connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Chrysler&model=Pacifica Hybrid 2019-23">Buy Here</a></sub></details>||
+|Acura|ILX 2016-19|AcuraWatch Plus|openpilot|25 mph|25 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Acura&model=ILX 2016-19">Buy Here</a></sub></details>||
+|Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Acura&model=RDX 2016-18">Buy Here</a></sub></details>||
+|Acura|RDX 2019-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Acura&model=RDX 2019-22">Buy Here</a></sub></details>||
+|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Audi&model=A3 2014-19">Buy Here</a></sub></details>||
+|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Audi&model=A3 Sportback e-tron 2017-18">Buy Here</a></sub></details>||
+|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Audi&model=Q2 2018">Buy Here</a></sub></details>||
+|Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Audi&model=Q3 2019-23">Buy Here</a></sub></details>||
+|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Audi&model=RS3 2018">Buy Here</a></sub></details>||
+|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Audi&model=S3 2015-17">Buy Here</a></sub></details>||
+|Buick|LaCrosse 2017-19[<sup>4</sup>](#footnotes)|Driver Confidence Package 2|openpilot|18 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-II connector<br>- 1 comma 3X<br>- 2 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Buick&model=LaCrosse 2017-19">Buy Here</a></sub></details>||
+|Cadillac|Escalade 2017[<sup>4</sup>](#footnotes)|Driver Assist Package|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-II connector<br>- 1 comma 3X<br>- 2 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Cadillac&model=Escalade 2017">Buy Here</a></sub></details>||
+|Cadillac|Escalade ESV 2016[<sup>4</sup>](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-II connector<br>- 1 comma 3X<br>- 2 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Cadillac&model=Escalade ESV 2016">Buy Here</a></sub></details>||
+|Cadillac|Escalade ESV 2019[<sup>4</sup>](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-II connector<br>- 1 comma 3X<br>- 2 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Cadillac&model=Escalade ESV 2019">Buy Here</a></sub></details>||
+|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 GM connector<br>- 1 comma 3X<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Chevrolet&model=Bolt EUV 2022-23">Buy Here</a></sub></details>|<a href="https://youtu.be/xvwzGMUA210" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Chevrolet|Bolt EV 2022-23|2LT Trim with Adaptive Cruise Control Package|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 GM connector<br>- 1 comma 3X<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Chevrolet&model=Bolt EV 2022-23">Buy Here</a></sub></details>||
+|Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 GM connector<br>- 1 comma 3X<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Chevrolet&model=Silverado 1500 2020-21">Buy Here</a></sub></details>||
+|Chevrolet|Trailblazer 2021-22|Adaptive Cruise Control (ACC)|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 GM connector<br>- 1 comma 3X<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Chevrolet&model=Trailblazer 2021-22">Buy Here</a></sub></details>||
+|Chevrolet|Volt 2017-18[<sup>4</sup>](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-II connector<br>- 1 comma 3X<br>- 2 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Chevrolet&model=Volt 2017-18">Buy Here</a></sub></details>|<a href="https://youtu.be/QeMCN_4TFfQ" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Chrysler|Pacifica 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Chrysler&model=Pacifica 2017-18">Buy Here</a></sub></details>||
+|Chrysler|Pacifica 2019-20|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Chrysler&model=Pacifica 2019-20">Buy Here</a></sub></details>||
+|Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Chrysler&model=Pacifica 2021">Buy Here</a></sub></details>||
+|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Chrysler&model=Pacifica Hybrid 2017-18">Buy Here</a></sub></details>||
+|Chrysler|Pacifica Hybrid 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Chrysler&model=Pacifica Hybrid 2019-23">Buy Here</a></sub></details>||
 |comma|body|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|None||
-|Ford|Bronco Sport 2021-22|Co-Pilot360 Assist+|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Ford Q3 connector<br>- 1 RJ45 cable (7 ft)<br>- 1 angled mount (8 degrees)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Bronco Sport 2021-22">Buy Here</a></sub></details>||
-|Ford|Escape 2020-22|Co-Pilot360 Assist+|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Ford Q3 connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Escape 2020-22">Buy Here</a></sub></details>||
-|Ford|Explorer 2020-22|Co-Pilot360 Assist+|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Ford Q3 connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Explorer 2020-22">Buy Here</a></sub></details>||
-|Ford|Focus 2018[<sup>3</sup>](#footnotes)|Adaptive Cruise Control with Lane Centering|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Ford Q3 connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Focus 2018">Buy Here</a></sub></details>||
-|Ford|Kuga 2020-22|Adaptive Cruise Control with Lane Centering|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Ford Q3 connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Kuga 2020-22">Buy Here</a></sub></details>||
-|Ford|Maverick 2022-23|Co-Pilot360 Assist|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Ford Q3 connector<br>- 1 RJ45 cable (7 ft)<br>- 1 angled mount (8 degrees)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Maverick 2022-23">Buy Here</a></sub></details>||
-|Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai F connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G70 2018-19">Buy Here</a></sub></details>||
-|Genesis|G70 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai F connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G70 2020">Buy Here</a></sub></details>||
-|Genesis|G80 2017|All|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai J connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G80 2017">Buy Here</a></sub></details>||
-|Genesis|G80 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai H connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G80 2018-19">Buy Here</a></sub></details>||
-|Genesis|G90 2017-18|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai C connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G90 2017-18">Buy Here</a></sub></details>||
-|Genesis|GV60 (Advanced Trim) 2023[<sup>6</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV60 (Advanced Trim) 2023">Buy Here</a></sub></details>||
-|Genesis|GV60 (Performance Trim) 2023[<sup>6</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV60 (Performance Trim) 2023">Buy Here</a></sub></details>||
-|Genesis|GV70 (2.5T Trim) 2022-23[<sup>6</sup>](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai L connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV70 (2.5T Trim) 2022-23">Buy Here</a></sub></details>||
-|Genesis|GV70 (3.5T Trim) 2022-23[<sup>6</sup>](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai M connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV70 (3.5T Trim) 2022-23">Buy Here</a></sub></details>||
-|Genesis|GV80 2023[<sup>6</sup>](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai M connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV80 2023">Buy Here</a></sub></details>||
-|GMC|Acadia 2018[<sup>4</sup>](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 OBD-II connector<br>- 1 comma three<br>- 2 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=GMC&model=Acadia 2018">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=0ZN6DdsBUZo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 GM connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=GMC&model=Sierra 1500 2020-21">Buy Here</a></sub></details>|<a href="https://youtu.be/5HbNoBLzRwE" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Honda|Accord 2018-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Accord 2018-22">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=mrUwlj3Mi58" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Honda|Accord Hybrid 2018-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Accord Hybrid 2018-22">Buy Here</a></sub></details>||
-|Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Civic 2016-18">Buy Here</a></sub></details>|<a href="https://youtu.be/-IkImTe1NYE" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Honda|Civic 2019-21|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|2 mph[<sup>5</sup>](#footnotes)|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Civic 2019-21">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=4Iz1Mz5LGF8" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Honda|Civic 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Bosch B connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Civic 2022">Buy Here</a></sub></details>|<a href="https://youtu.be/ytiOT5lcp6Q" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Honda|Civic Hatchback 2017-21|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Civic Hatchback 2017-21">Buy Here</a></sub></details>||
-|Honda|Civic Hatchback 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Bosch B connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Civic Hatchback 2022">Buy Here</a></sub></details>|<a href="https://youtu.be/ytiOT5lcp6Q" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Honda|CR-V 2015-16|Touring Trim|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=CR-V 2015-16">Buy Here</a></sub></details>||
-|Honda|CR-V 2017-22|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=CR-V 2017-22">Buy Here</a></sub></details>||
-|Honda|CR-V Hybrid 2017-19|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=CR-V Hybrid 2017-19">Buy Here</a></sub></details>||
-|Honda|e 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=e 2020">Buy Here</a></sub></details>||
-|Honda|Fit 2018-20|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Fit 2018-20">Buy Here</a></sub></details>||
-|Honda|Freed 2020|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Freed 2020">Buy Here</a></sub></details>||
-|Honda|HR-V 2019-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=HR-V 2019-22">Buy Here</a></sub></details>||
-|Honda|HR-V 2023|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Bosch B connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=HR-V 2023">Buy Here</a></sub></details>||
-|Honda|Insight 2019-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Insight 2019-22">Buy Here</a></sub></details>||
-|Honda|Inspire 2018|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Inspire 2018">Buy Here</a></sub></details>||
-|Honda|Odyssey 2018-20|Honda Sensing|openpilot|25 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Odyssey 2018-20">Buy Here</a></sub></details>||
-|Honda|Passport 2019-23|All|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Passport 2019-23">Buy Here</a></sub></details>||
-|Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Pilot 2016-22">Buy Here</a></sub></details>||
-|Honda|Ridgeline 2017-23|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Ridgeline 2017-23">Buy Here</a></sub></details>||
-|Hyundai|Elantra 2017-19|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai B connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Elantra 2017-19">Buy Here</a></sub></details>||
-|Hyundai|Elantra 2021-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Elantra 2021-23">Buy Here</a></sub></details>|<a href="https://youtu.be/_EdYQtV52-c" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Hyundai|Elantra GT 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Elantra GT 2017-19">Buy Here</a></sub></details>||
-|Hyundai|Elantra Hybrid 2021-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Elantra Hybrid 2021-23">Buy Here</a></sub></details>|<a href="https://youtu.be/_EdYQtV52-c" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai J connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Genesis 2015-16">Buy Here</a></sub></details>||
-|Hyundai|i30 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=i30 2017-19">Buy Here</a></sub></details>||
-|Hyundai|Ioniq 5 (Southeast Asia only) 2022-23[<sup>6</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai Q connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq 5 (Southeast Asia only) 2022-23">Buy Here</a></sub></details>||
-|Hyundai|Ioniq 5 (with HDA II) 2022-23[<sup>6</sup>](#footnotes)|Highway Driving Assist II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai Q connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq 5 (with HDA II) 2022-23">Buy Here</a></sub></details>||
-|Hyundai|Ioniq 5 (without HDA II) 2022-23[<sup>6</sup>](#footnotes)|Highway Driving Assist|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq 5 (without HDA II) 2022-23">Buy Here</a></sub></details>||
-|Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai C connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq Electric 2019">Buy Here</a></sub></details>||
-|Hyundai|Ioniq Electric 2020|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai H connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq Electric 2020">Buy Here</a></sub></details>||
-|Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai C connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq Hybrid 2017-19">Buy Here</a></sub></details>||
-|Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai H connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq Hybrid 2020-22">Buy Here</a></sub></details>||
-|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai C connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq Plug-in Hybrid 2019">Buy Here</a></sub></details>||
-|Hyundai|Ioniq Plug-in Hybrid 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai H connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq Plug-in Hybrid 2020-22">Buy Here</a></sub></details>||
-|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai B connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Kona 2020">Buy Here</a></sub></details>||
-|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai G connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Kona Electric 2018-21">Buy Here</a></sub></details>||
-|Hyundai|Kona Electric 2022|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai O connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Kona Electric 2022">Buy Here</a></sub></details>||
-|Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai I connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Kona Hybrid 2020">Buy Here</a></sub></details>|<a href="https://youtu.be/0dwpAHiZgFo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Hyundai|Palisade 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai H connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Palisade 2020-22">Buy Here</a></sub></details>|<a href="https://youtu.be/TAnDqjF4fDY?t=456" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Hyundai|Santa Cruz 2022-23[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai N connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Santa Cruz 2022-23">Buy Here</a></sub></details>||
-|Hyundai|Santa Fe 2019-20|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai D connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Santa Fe 2019-20">Buy Here</a></sub></details>||
-|Hyundai|Santa Fe 2021-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai L connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Santa Fe 2021-23">Buy Here</a></sub></details>|<a href="https://youtu.be/VnHzSTygTS4" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Hyundai|Santa Fe Hybrid 2022-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai L connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Santa Fe Hybrid 2022-23">Buy Here</a></sub></details>||
-|Hyundai|Santa Fe Plug-in Hybrid 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai L connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Santa Fe Plug-in Hybrid 2022">Buy Here</a></sub></details>||
-|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Sonata 2018-19">Buy Here</a></sub></details>||
-|Hyundai|Sonata 2020-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Sonata 2020-23">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=ix63r9kE3Fw" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Hyundai|Sonata Hybrid 2020-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Sonata Hybrid 2020-23">Buy Here</a></sub></details>||
-|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai L connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Tucson 2021">Buy Here</a></sub></details>||
-|Hyundai|Tucson 2022[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai N connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Tucson 2022">Buy Here</a></sub></details>||
-|Hyundai|Tucson 2023[<sup>6</sup>](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai N connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Tucson 2023">Buy Here</a></sub></details>||
-|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai L connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Tucson Diesel 2019">Buy Here</a></sub></details>||
-|Hyundai|Tucson Hybrid 2022-23[<sup>6</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai N connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Tucson Hybrid 2022-23">Buy Here</a></sub></details>||
-|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Veloster 2019-20">Buy Here</a></sub></details>||
-|Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 FCA connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Jeep&model=Grand Cherokee 2016-18">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=eLR9o2JkuRk" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 FCA connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Jeep&model=Grand Cherokee 2019-21">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=jBe4lWnRSu4" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Kia|Carnival 2023[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Carnival 2023">Buy Here</a></sub></details>||
-|Kia|Carnival (China only) 2023[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Carnival (China only) 2023">Buy Here</a></sub></details>||
-|Kia|Ceed 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Ceed 2019">Buy Here</a></sub></details>||
-|Kia|EV6 (Southeast Asia only) 2022-23[<sup>6</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai P connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=EV6 (Southeast Asia only) 2022-23">Buy Here</a></sub></details>||
-|Kia|EV6 (with HDA II) 2022-23[<sup>6</sup>](#footnotes)|Highway Driving Assist II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai P connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=EV6 (with HDA II) 2022-23">Buy Here</a></sub></details>||
-|Kia|EV6 (without HDA II) 2022-23[<sup>6</sup>](#footnotes)|Highway Driving Assist|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai L connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=EV6 (without HDA II) 2022-23">Buy Here</a></sub></details>||
-|Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai G connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Forte 2019-21">Buy Here</a></sub></details>||
-|Kia|Forte 2023|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Forte 2023">Buy Here</a></sub></details>||
-|Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=K5 2021-22">Buy Here</a></sub></details>||
-|Kia|K5 Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=K5 Hybrid 2020">Buy Here</a></sub></details>||
-|Kia|Niro EV 2019|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai H connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro EV 2019">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Kia|Niro EV 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai F connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro EV 2020">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Kia|Niro EV 2021|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai C connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro EV 2021">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Kia|Niro EV 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai H connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro EV 2022">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Kia|Niro EV 2023[<sup>6</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro EV 2023">Buy Here</a></sub></details>||
-|Kia|Niro Hybrid 2021-22|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai F connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro Hybrid 2021-22">Buy Here</a></sub></details>||
-|Kia|Niro Hybrid 2023[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro Hybrid 2023">Buy Here</a></sub></details>||
-|Kia|Niro Plug-in Hybrid 2018-19|All|openpilot available[<sup>1</sup>](#footnotes)|10 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai C connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro Plug-in Hybrid 2018-19">Buy Here</a></sub></details>||
-|Kia|Niro Plug-in Hybrid 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai D connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro Plug-in Hybrid 2020">Buy Here</a></sub></details>||
-|Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai B connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Optima 2017">Buy Here</a></sub></details>||
-|Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai G connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Optima 2019-20">Buy Here</a></sub></details>||
-|Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Seltos 2021">Buy Here</a></sub></details>||
-|Kia|Sorento 2018|Advanced Smart Cruise Control|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai C connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento 2018">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=Fkh3s6WHJz8" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento 2019">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=Fkh3s6WHJz8" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Kia|Sorento 2021-23[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento 2021-23">Buy Here</a></sub></details>||
-|Kia|Sorento Plug-in Hybrid 2022-23[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento Plug-in Hybrid 2022-23">Buy Here</a></sub></details>||
-|Kia|Sportage 2023[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai N connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sportage 2023">Buy Here</a></sub></details>||
-|Kia|Sportage Hybrid 2023[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai N connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sportage Hybrid 2023">Buy Here</a></sub></details>||
-|Kia|Stinger 2018-20|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai C connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Stinger 2018-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=MJ94qoofYw0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Kia|Stinger 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Stinger 2022">Buy Here</a></sub></details>||
-|Kia|Telluride 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Hyundai H connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Telluride 2020-22">Buy Here</a></sub></details>||
-|Lexus|CT Hybrid 2017-18|Lexus Safety System+|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=CT Hybrid 2017-18">Buy Here</a></sub></details>||
-|Lexus|ES 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=ES 2017-18">Buy Here</a></sub></details>||
-|Lexus|ES 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=ES 2019-22">Buy Here</a></sub></details>||
-|Lexus|ES Hybrid 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=ES Hybrid 2017-18">Buy Here</a></sub></details>||
-|Lexus|ES Hybrid 2019-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=ES Hybrid 2019-23">Buy Here</a></sub></details>|<a href="https://youtu.be/BZ29osRVJeg?t=12" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=IS 2017-19">Buy Here</a></sub></details>||
-|Lexus|NX 2018-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=NX 2018-19">Buy Here</a></sub></details>||
-|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=NX 2020-21">Buy Here</a></sub></details>||
-|Lexus|NX Hybrid 2018-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=NX Hybrid 2018-19">Buy Here</a></sub></details>||
-|Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=NX Hybrid 2020-21">Buy Here</a></sub></details>||
-|Lexus|RC 2018-20|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RC 2018-20">Buy Here</a></sub></details>||
-|Lexus|RX 2016|Lexus Safety System+|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX 2016">Buy Here</a></sub></details>||
-|Lexus|RX 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX 2017-19">Buy Here</a></sub></details>||
-|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX 2020-22">Buy Here</a></sub></details>||
-|Lexus|RX Hybrid 2016|Lexus Safety System+|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX Hybrid 2016">Buy Here</a></sub></details>||
-|Lexus|RX Hybrid 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX Hybrid 2017-19">Buy Here</a></sub></details>||
-|Lexus|RX Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX Hybrid 2020-22">Buy Here</a></sub></details>||
-|Lexus|UX Hybrid 2019-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=UX Hybrid 2019-23">Buy Here</a></sub></details>||
-|Lincoln|Aviator 2020-21|Co-Pilot360 Plus|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Ford Q3 connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Lincoln&model=Aviator 2020-21">Buy Here</a></sub></details>||
-|MAN|eTGE 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 angled mount (8 degrees)<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=MAN&model=eTGE 2020-23">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|MAN|TGE 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 angled mount (8 degrees)<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=MAN&model=TGE 2017-23">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Mazda|CX-5 2022-23|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Mazda connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Mazda&model=CX-5 2022-23">Buy Here</a></sub></details>||
-|Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 Mazda connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Mazda&model=CX-9 2021-23">Buy Here</a></sub></details>|<a href="https://youtu.be/dA3duO4a0O4" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 Nissan B connector<br>- 1 RJ45 cable (7 ft)<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Nissan&model=Altima 2019-20">Buy Here</a></sub></details>||
-|Nissan|Leaf 2018-23|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 Nissan A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Nissan&model=Leaf 2018-23">Buy Here</a></sub></details>|<a href="https://youtu.be/vaMbtAh_0cY" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 Nissan A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Nissan&model=Rogue 2018-20">Buy Here</a></sub></details>||
-|Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 Nissan A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Nissan&model=X-Trail 2017">Buy Here</a></sub></details>||
-|Ram|1500 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Ram connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Ram&model=1500 2019-23">Buy Here</a></sub></details>||
-|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=SEAT&model=Ateca 2018">Buy Here</a></sub></details>||
-|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=SEAT&model=Leon 2014-20">Buy Here</a></sub></details>||
-|Subaru|Ascent 2019-21|All[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Ascent 2019-21">Buy Here</a></sub></details>||
-|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Crosstrek 2018-19">Buy Here</a></sub></details>|<a href="https://youtu.be/Agww7oE1k-s?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Crosstrek 2020-23">Buy Here</a></sub></details>||
-|Subaru|Forester 2019-21|All[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Forester 2019-21">Buy Here</a></sub></details>||
-|Subaru|Impreza 2017-19|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Impreza 2017-19">Buy Here</a></sub></details>||
-|Subaru|Impreza 2020-22|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Impreza 2020-22">Buy Here</a></sub></details>||
-|Subaru|Legacy 2020-22|All[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru B connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Legacy 2020-22">Buy Here</a></sub></details>||
-|Subaru|Outback 2020-22|All[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru B connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Outback 2020-22">Buy Here</a></sub></details>||
-|Subaru|XV 2018-19|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=XV 2018-19">Buy Here</a></sub></details>|<a href="https://youtu.be/Agww7oE1k-s?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Subaru|XV 2020-21|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=XV 2020-21">Buy Here</a></sub></details>||
-|Škoda|Fabia 2022-23[<sup>11</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Fabia 2022-23">Buy Here</a></sub></details>[<sup>13</sup>](#footnotes)||
-|Škoda|Kamiq 2021[<sup>9,11</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Kamiq 2021">Buy Here</a></sub></details>[<sup>13</sup>](#footnotes)||
-|Škoda|Karoq 2019-21[<sup>11</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Karoq 2019-21">Buy Here</a></sub></details>||
-|Škoda|Kodiaq 2017-23[<sup>11</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Kodiaq 2017-23">Buy Here</a></sub></details>||
-|Škoda|Octavia 2015, 2018-19[<sup>11</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Octavia 2015, 2018-19">Buy Here</a></sub></details>||
-|Škoda|Octavia RS 2016[<sup>11</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Octavia RS 2016">Buy Here</a></sub></details>||
-|Škoda|Scala 2020-23[<sup>11</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Scala 2020-23">Buy Here</a></sub></details>[<sup>13</sup>](#footnotes)||
-|Škoda|Superb 2015-22[<sup>11</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Superb 2015-22">Buy Here</a></sub></details>||
-|Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Alphard 2019-20">Buy Here</a></sub></details>||
-|Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Alphard Hybrid 2021">Buy Here</a></sub></details>||
-|Toyota|Avalon 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Avalon 2016">Buy Here</a></sub></details>||
-|Toyota|Avalon 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Avalon 2017-18">Buy Here</a></sub></details>||
-|Toyota|Avalon 2019-21|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Avalon 2019-21">Buy Here</a></sub></details>||
-|Toyota|Avalon 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Avalon 2022">Buy Here</a></sub></details>||
-|Toyota|Avalon Hybrid 2019-21|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Avalon Hybrid 2019-21">Buy Here</a></sub></details>||
-|Toyota|Avalon Hybrid 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Avalon Hybrid 2022">Buy Here</a></sub></details>||
-|Toyota|C-HR 2017-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=C-HR 2017-20">Buy Here</a></sub></details>||
-|Toyota|C-HR 2021|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=C-HR 2021">Buy Here</a></sub></details>||
-|Toyota|C-HR Hybrid 2017-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=C-HR Hybrid 2017-20">Buy Here</a></sub></details>||
-|Toyota|C-HR Hybrid 2021-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=C-HR Hybrid 2021-22">Buy Here</a></sub></details>||
-|Toyota|Camry 2018-20|All|Stock|0 mph[<sup>8</sup>](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Camry 2018-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=fkcjviZY9CM" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Toyota|Camry 2021-23|All|openpilot|0 mph[<sup>8</sup>](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Camry 2021-23">Buy Here</a></sub></details>||
-|Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Camry Hybrid 2018-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=Q2DYY0AWKgk" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Toyota|Camry Hybrid 2021-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Camry Hybrid 2021-23">Buy Here</a></sub></details>||
-|Toyota|Corolla 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla 2017-19">Buy Here</a></sub></details>||
-|Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla 2020-22">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=_66pXk0CBYA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Toyota|Corolla Cross (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla Cross (Non-US only) 2020-23">Buy Here</a></sub></details>||
-|Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla Cross Hybrid (Non-US only) 2020-22">Buy Here</a></sub></details>||
-|Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla Hatchback 2019-22">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=_66pXk0CBYA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla Hybrid 2020-22">Buy Here</a></sub></details>||
-|Toyota|Corolla Hybrid (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla Hybrid (Non-US only) 2020-23">Buy Here</a></sub></details>||
-|Toyota|Highlander 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Highlander 2017-19">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=0wS0wXSLzoo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Toyota|Highlander 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Highlander 2020-23">Buy Here</a></sub></details>||
-|Toyota|Highlander Hybrid 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Highlander Hybrid 2017-19">Buy Here</a></sub></details>||
-|Toyota|Highlander Hybrid 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Highlander Hybrid 2020-23">Buy Here</a></sub></details>||
-|Toyota|Mirai 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Mirai 2021">Buy Here</a></sub></details>||
-|Toyota|Prius 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Prius 2016">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=8zopPJI8XQ0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Toyota|Prius 2017-20|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Prius 2017-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=8zopPJI8XQ0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Toyota|Prius 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Prius 2021-22">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=J58TvCpUd4U" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Toyota|Prius Prime 2017-20|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Prius Prime 2017-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=8zopPJI8XQ0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Toyota|Prius Prime 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Prius Prime 2021-22">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=J58TvCpUd4U" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Toyota|Prius v 2017|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Prius v 2017">Buy Here</a></sub></details>||
-|Toyota|RAV4 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 2016">Buy Here</a></sub></details>||
-|Toyota|RAV4 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 2017-18">Buy Here</a></sub></details>||
-|Toyota|RAV4 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 2019-21">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=wJxjDd42gGA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Toyota|RAV4 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 2022">Buy Here</a></sub></details>||
-|Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 Hybrid 2016">Buy Here</a></sub></details>|<a href="https://youtu.be/LhT5VzJVfNI?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Toyota|RAV4 Hybrid 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 Hybrid 2017-18">Buy Here</a></sub></details>|<a href="https://youtu.be/LhT5VzJVfNI?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 Hybrid 2019-21">Buy Here</a></sub></details>||
-|Toyota|RAV4 Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 Hybrid 2022">Buy Here</a></sub></details>|<a href="https://youtu.be/U0nH9cnrFB0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Toyota|Sienna 2018-20|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota connector<br>- 1 comma power v2<br>- 1 comma three<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Sienna 2018-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=q1UPOo4Sh68" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Volkswagen|Arteon 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Arteon 2018-23">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Volkswagen|Arteon eHybrid 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Arteon eHybrid 2020-23">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Volkswagen|Arteon R 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Arteon R 2020-23">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Volkswagen|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Atlas 2018-23">Buy Here</a></sub></details>||
-|Volkswagen|Atlas Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Atlas Cross Sport 2021-22">Buy Here</a></sub></details>||
-|Volkswagen|California 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 angled mount (8 degrees)<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=California 2021">Buy Here</a></sub></details>||
-|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 angled mount (8 degrees)<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Caravelle 2020">Buy Here</a></sub></details>||
-|Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=CC 2018-22">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Volkswagen|Crafter 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 angled mount (8 degrees)<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Crafter 2017-23">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Volkswagen|e-Crafter 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 angled mount (8 degrees)<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=e-Crafter 2018-23">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=e-Golf 2014-20">Buy Here</a></sub></details>||
-|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf 2015-20">Buy Here</a></sub></details>||
-|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf Alltrack 2015-19">Buy Here</a></sub></details>||
-|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf GTD 2015-20">Buy Here</a></sub></details>||
-|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf GTE 2015-20">Buy Here</a></sub></details>||
-|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf GTI 2015-21">Buy Here</a></sub></details>||
-|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf R 2015-19">Buy Here</a></sub></details>||
-|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf SportsVan 2015-20">Buy Here</a></sub></details>||
-|Volkswagen|Grand California 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 angled mount (8 degrees)<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Grand California 2019-23">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Volkswagen|Jetta 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Jetta 2018-22">Buy Here</a></sub></details>||
-|Volkswagen|Jetta GLI 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Jetta GLI 2021-22">Buy Here</a></sub></details>||
-|Volkswagen|Passat 2015-22[<sup>10</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Passat 2015-22">Buy Here</a></sub></details>||
-|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Passat Alltrack 2015-22">Buy Here</a></sub></details>||
-|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Passat GTE 2015-22">Buy Here</a></sub></details>||
-|Volkswagen|Polo 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Polo 2018-23">Buy Here</a></sub></details>[<sup>13</sup>](#footnotes)||
-|Volkswagen|Polo GTI 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Polo GTI 2018-23">Buy Here</a></sub></details>[<sup>13</sup>](#footnotes)||
-|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=T-Cross 2021">Buy Here</a></sub></details>[<sup>13</sup>](#footnotes)||
-|Volkswagen|T-Roc 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=T-Roc 2021">Buy Here</a></sub></details>[<sup>13</sup>](#footnotes)||
-|Volkswagen|Taos 2022-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Taos 2022-23">Buy Here</a></sub></details>||
-|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Teramont 2018-22">Buy Here</a></sub></details>||
-|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Teramont Cross Sport 2021-22">Buy Here</a></sub></details>||
-|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Teramont X 2021-22">Buy Here</a></sub></details>||
-|Volkswagen|Tiguan 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Tiguan 2018-23">Buy Here</a></sub></details>||
-|Volkswagen|Tiguan eHybrid 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Tiguan eHybrid 2021-23">Buy Here</a></sub></details>||
-|Volkswagen|Touran 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>View</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma three<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Touran 2016-23">Buy Here</a></sub></details>||
+|Ford|Bronco Sport 2021-22|Co-Pilot360 Assist+|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 RJ45 cable (7 ft)<br>- 1 angled mount (8 degrees)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Ford&model=Bronco Sport 2021-22">Buy Here</a></sub></details>||
+|Ford|Escape 2020-22|Co-Pilot360 Assist+|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Ford&model=Escape 2020-22">Buy Here</a></sub></details>||
+|Ford|Explorer 2020-22|Co-Pilot360 Assist+|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Ford&model=Explorer 2020-22">Buy Here</a></sub></details>||
+|Ford|Focus 2018[<sup>3</sup>](#footnotes)|Adaptive Cruise Control with Lane Centering|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Ford&model=Focus 2018">Buy Here</a></sub></details>||
+|Ford|Kuga 2020-22|Adaptive Cruise Control with Lane Centering|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Ford&model=Kuga 2020-22">Buy Here</a></sub></details>||
+|Ford|Maverick 2022|LARIAT Luxury|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 RJ45 cable (7 ft)<br>- 1 angled mount (8 degrees)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Ford&model=Maverick 2022">Buy Here</a></sub></details>||
+|Ford|Maverick 2023|Co-Pilot360 Assist|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 RJ45 cable (7 ft)<br>- 1 angled mount (8 degrees)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Ford&model=Maverick 2023">Buy Here</a></sub></details>||
+|Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai F connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Genesis&model=G70 2018-19">Buy Here</a></sub></details>||
+|Genesis|G70 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai F connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Genesis&model=G70 2020">Buy Here</a></sub></details>||
+|Genesis|G80 2017|All|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai J connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Genesis&model=G80 2017">Buy Here</a></sub></details>||
+|Genesis|G80 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Genesis&model=G80 2018-19">Buy Here</a></sub></details>||
+|Genesis|G90 2017-18|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai C connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Genesis&model=G90 2017-18">Buy Here</a></sub></details>||
+|Genesis|GV60 (Advanced Trim) 2023[<sup>6</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Genesis&model=GV60 (Advanced Trim) 2023">Buy Here</a></sub></details>||
+|Genesis|GV60 (Performance Trim) 2023[<sup>6</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Genesis&model=GV60 (Performance Trim) 2023">Buy Here</a></sub></details>||
+|Genesis|GV70 (2.5T Trim) 2022-23[<sup>6</sup>](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Genesis&model=GV70 (2.5T Trim) 2022-23">Buy Here</a></sub></details>||
+|Genesis|GV70 (3.5T Trim) 2022-23[<sup>6</sup>](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai M connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Genesis&model=GV70 (3.5T Trim) 2022-23">Buy Here</a></sub></details>||
+|Genesis|GV80 2023[<sup>6</sup>](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai M connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Genesis&model=GV80 2023">Buy Here</a></sub></details>||
+|GMC|Acadia 2018[<sup>4</sup>](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-II connector<br>- 1 comma 3X<br>- 2 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=GMC&model=Acadia 2018">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=0ZN6DdsBUZo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 GM connector<br>- 1 comma 3X<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=GMC&model=Sierra 1500 2020-21">Buy Here</a></sub></details>|<a href="https://youtu.be/5HbNoBLzRwE" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Honda|Accord 2018-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=Accord 2018-22">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=mrUwlj3Mi58" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Honda|Accord Hybrid 2018-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=Accord Hybrid 2018-22">Buy Here</a></sub></details>||
+|Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=Civic 2016-18">Buy Here</a></sub></details>|<a href="https://youtu.be/-IkImTe1NYE" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Honda|Civic 2019-21|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|2 mph[<sup>5</sup>](#footnotes)|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=Civic 2019-21">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=4Iz1Mz5LGF8" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Honda|Civic 2022-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch B connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=Civic 2022-23">Buy Here</a></sub></details>|<a href="https://youtu.be/ytiOT5lcp6Q" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Honda|Civic Hatchback 2017-21|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=Civic Hatchback 2017-21">Buy Here</a></sub></details>||
+|Honda|Civic Hatchback 2022-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch B connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=Civic Hatchback 2022-23">Buy Here</a></sub></details>|<a href="https://youtu.be/ytiOT5lcp6Q" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Honda|CR-V 2015-16|Touring Trim|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=CR-V 2015-16">Buy Here</a></sub></details>||
+|Honda|CR-V 2017-22|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=CR-V 2017-22">Buy Here</a></sub></details>||
+|Honda|CR-V Hybrid 2017-19|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=CR-V Hybrid 2017-19">Buy Here</a></sub></details>||
+|Honda|e 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=e 2020">Buy Here</a></sub></details>||
+|Honda|Fit 2018-20|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=Fit 2018-20">Buy Here</a></sub></details>||
+|Honda|Freed 2020|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=Freed 2020">Buy Here</a></sub></details>||
+|Honda|HR-V 2019-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=HR-V 2019-22">Buy Here</a></sub></details>||
+|Honda|HR-V 2023|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch B connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=HR-V 2023">Buy Here</a></sub></details>||
+|Honda|Insight 2019-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=Insight 2019-22">Buy Here</a></sub></details>||
+|Honda|Inspire 2018|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=Inspire 2018">Buy Here</a></sub></details>||
+|Honda|Odyssey 2018-20|Honda Sensing|openpilot|25 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=Odyssey 2018-20">Buy Here</a></sub></details>||
+|Honda|Passport 2019-23|All|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=Passport 2019-23">Buy Here</a></sub></details>||
+|Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=Pilot 2016-22">Buy Here</a></sub></details>||
+|Honda|Ridgeline 2017-23|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Honda&model=Ridgeline 2017-23">Buy Here</a></sub></details>||
+|Hyundai|Azera 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Azera 2022">Buy Here</a></sub></details>||
+|Hyundai|Azera Hybrid 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Azera Hybrid 2020">Buy Here</a></sub></details>||
+|Hyundai|Custin 2023|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Custin 2023">Buy Here</a></sub></details>||
+|Hyundai|Elantra 2017-18|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai B connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra 2017-18">Buy Here</a></sub></details>||
+|Hyundai|Elantra 2019|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai G connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra 2019">Buy Here</a></sub></details>||
+|Hyundai|Elantra 2021-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra 2021-23">Buy Here</a></sub></details>|<a href="https://youtu.be/_EdYQtV52-c" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Hyundai|Elantra GT 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra GT 2017-19">Buy Here</a></sub></details>||
+|Hyundai|Elantra Hybrid 2021-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra Hybrid 2021-23">Buy Here</a></sub></details>|<a href="https://youtu.be/_EdYQtV52-c" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai J connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Genesis 2015-16">Buy Here</a></sub></details>||
+|Hyundai|i30 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=i30 2017-19">Buy Here</a></sub></details>||
+|Hyundai|Ioniq 5 (Southeast Asia only) 2022-23[<sup>6</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai Q connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Ioniq 5 (Southeast Asia only) 2022-23">Buy Here</a></sub></details>||
+|Hyundai|Ioniq 5 (with HDA II) 2022-23[<sup>6</sup>](#footnotes)|Highway Driving Assist II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai Q connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Ioniq 5 (with HDA II) 2022-23">Buy Here</a></sub></details>||
+|Hyundai|Ioniq 5 (without HDA II) 2022-23[<sup>6</sup>](#footnotes)|Highway Driving Assist|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Ioniq 5 (without HDA II) 2022-23">Buy Here</a></sub></details>||
+|Hyundai|Ioniq 6 (with HDA II) 2023[<sup>6</sup>](#footnotes)|Highway Driving Assist II|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai P connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Ioniq 6 (with HDA II) 2023">Buy Here</a></sub></details>||
+|Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai C connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Ioniq Electric 2019">Buy Here</a></sub></details>||
+|Hyundai|Ioniq Electric 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Ioniq Electric 2020">Buy Here</a></sub></details>||
+|Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai C connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Ioniq Hybrid 2017-19">Buy Here</a></sub></details>||
+|Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Ioniq Hybrid 2020-22">Buy Here</a></sub></details>||
+|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai C connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Ioniq Plug-in Hybrid 2019">Buy Here</a></sub></details>||
+|Hyundai|Ioniq Plug-in Hybrid 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Ioniq Plug-in Hybrid 2020-22">Buy Here</a></sub></details>||
+|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai B connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Kona 2020">Buy Here</a></sub></details>||
+|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai G connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Kona Electric 2018-21">Buy Here</a></sub></details>||
+|Hyundai|Kona Electric 2022-23|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai O connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Kona Electric 2022-23">Buy Here</a></sub></details>||
+|Hyundai|Kona Electric (with HDA II, Korea only) 2023[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai R connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Kona Electric (with HDA II, Korea only) 2023">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=U2fOCmcQ8hw" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai I connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Kona Hybrid 2020">Buy Here</a></sub></details>|<a href="https://youtu.be/0dwpAHiZgFo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Hyundai|Palisade 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Palisade 2020-22">Buy Here</a></sub></details>|<a href="https://youtu.be/TAnDqjF4fDY?t=456" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Hyundai|Santa Cruz 2022-23[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Santa Cruz 2022-23">Buy Here</a></sub></details>||
+|Hyundai|Santa Fe 2019-20|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai D connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Santa Fe 2019-20">Buy Here</a></sub></details>||
+|Hyundai|Santa Fe 2021-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Santa Fe 2021-23">Buy Here</a></sub></details>|<a href="https://youtu.be/VnHzSTygTS4" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Hyundai|Santa Fe Hybrid 2022-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Santa Fe Hybrid 2022-23">Buy Here</a></sub></details>||
+|Hyundai|Santa Fe Plug-in Hybrid 2022-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Santa Fe Plug-in Hybrid 2022-23">Buy Here</a></sub></details>||
+|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Sonata 2018-19">Buy Here</a></sub></details>||
+|Hyundai|Sonata 2020-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Sonata 2020-23">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=ix63r9kE3Fw" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Hyundai|Sonata Hybrid 2020-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Sonata Hybrid 2020-23">Buy Here</a></sub></details>||
+|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Tucson 2021">Buy Here</a></sub></details>||
+|Hyundai|Tucson 2022[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Tucson 2022">Buy Here</a></sub></details>||
+|Hyundai|Tucson 2023[<sup>6</sup>](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Tucson 2023">Buy Here</a></sub></details>||
+|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Tucson Diesel 2019">Buy Here</a></sub></details>||
+|Hyundai|Tucson Hybrid 2022-24[<sup>6</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Tucson Hybrid 2022-24">Buy Here</a></sub></details>||
+|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Veloster 2019-20">Buy Here</a></sub></details>||
+|Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Jeep&model=Grand Cherokee 2016-18">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=eLR9o2JkuRk" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Jeep&model=Grand Cherokee 2019-21">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=jBe4lWnRSu4" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Kia|Carnival 2023-24[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Carnival 2023-24">Buy Here</a></sub></details>||
+|Kia|Carnival (China only) 2023[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Carnival (China only) 2023">Buy Here</a></sub></details>||
+|Kia|Ceed 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Ceed 2019">Buy Here</a></sub></details>||
+|Kia|EV6 (Southeast Asia only) 2022-23[<sup>6</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai P connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=EV6 (Southeast Asia only) 2022-23">Buy Here</a></sub></details>||
+|Kia|EV6 (with HDA II) 2022-23[<sup>6</sup>](#footnotes)|Highway Driving Assist II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai P connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=EV6 (with HDA II) 2022-23">Buy Here</a></sub></details>||
+|Kia|EV6 (without HDA II) 2022-23[<sup>6</sup>](#footnotes)|Highway Driving Assist|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=EV6 (without HDA II) 2022-23">Buy Here</a></sub></details>||
+|Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai G connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Forte 2019-21">Buy Here</a></sub></details>||
+|Kia|Forte 2023|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Forte 2023">Buy Here</a></sub></details>||
+|Kia|K5 2021-24|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=K5 2021-24">Buy Here</a></sub></details>||
+|Kia|K5 Hybrid 2020-22|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=K5 Hybrid 2020-22">Buy Here</a></sub></details>||
+|Kia|K8 Hybrid (with HDA II) 2023[<sup>6</sup>](#footnotes)|Highway Driving Assist II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai Q connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=K8 Hybrid (with HDA II) 2023">Buy Here</a></sub></details>||
+|Kia|Niro EV 2019|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Niro EV 2019">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Kia|Niro EV 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai F connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Niro EV 2020">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Kia|Niro EV 2021|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai C connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Niro EV 2021">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Kia|Niro EV 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Niro EV 2022">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Kia|Niro EV 2023[<sup>6</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Niro EV 2023">Buy Here</a></sub></details>||
+|Kia|Niro Hybrid 2021-22|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai F connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Niro Hybrid 2021-22">Buy Here</a></sub></details>||
+|Kia|Niro Hybrid 2023[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Niro Hybrid 2023">Buy Here</a></sub></details>||
+|Kia|Niro Plug-in Hybrid 2018-19|All|Stock|10 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai C connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Niro Plug-in Hybrid 2018-19">Buy Here</a></sub></details>||
+|Kia|Niro Plug-in Hybrid 2020|All|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai D connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Niro Plug-in Hybrid 2020">Buy Here</a></sub></details>||
+|Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai B connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Optima 2017">Buy Here</a></sub></details>||
+|Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai G connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Optima 2019-20">Buy Here</a></sub></details>||
+|Kia|Optima Hybrid 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Optima Hybrid 2019">Buy Here</a></sub></details>||
+|Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Seltos 2021">Buy Here</a></sub></details>||
+|Kia|Sorento 2018|Advanced Smart Cruise Control & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Sorento 2018">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=Fkh3s6WHJz8" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Sorento 2019">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=Fkh3s6WHJz8" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Kia|Sorento 2021-23[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Sorento 2021-23">Buy Here</a></sub></details>||
+|Kia|Sorento Hybrid 2021-23[<sup>6</sup>](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Sorento Hybrid 2021-23">Buy Here</a></sub></details>||
+|Kia|Sorento Plug-in Hybrid 2022-23[<sup>6</sup>](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Sorento Plug-in Hybrid 2022-23">Buy Here</a></sub></details>||
+|Kia|Sportage 2023[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Sportage 2023">Buy Here</a></sub></details>||
+|Kia|Sportage Hybrid 2023[<sup>6</sup>](#footnotes)|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Sportage Hybrid 2023">Buy Here</a></sub></details>||
+|Kia|Stinger 2018-20|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai C connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Stinger 2018-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=MJ94qoofYw0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Kia|Stinger 2022-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Stinger 2022-23">Buy Here</a></sub></details>||
+|Kia|Telluride 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Kia&model=Telluride 2020-22">Buy Here</a></sub></details>||
+|Lexus|CT Hybrid 2017-18|Lexus Safety System+|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=CT Hybrid 2017-18">Buy Here</a></sub></details>||
+|Lexus|ES 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=ES 2017-18">Buy Here</a></sub></details>||
+|Lexus|ES 2019-24|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=ES 2019-24">Buy Here</a></sub></details>||
+|Lexus|ES Hybrid 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=ES Hybrid 2017-18">Buy Here</a></sub></details>||
+|Lexus|ES Hybrid 2019-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=ES Hybrid 2019-23">Buy Here</a></sub></details>|<a href="https://youtu.be/BZ29osRVJeg?t=12" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Lexus|GS F 2016|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=GS F 2016">Buy Here</a></sub></details>||
+|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=IS 2017-19">Buy Here</a></sub></details>||
+|Lexus|IS 2022-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=IS 2022-23">Buy Here</a></sub></details>||
+|Lexus|NX 2018-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=NX 2018-19">Buy Here</a></sub></details>||
+|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=NX 2020-21">Buy Here</a></sub></details>||
+|Lexus|NX Hybrid 2018-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=NX Hybrid 2018-19">Buy Here</a></sub></details>||
+|Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=NX Hybrid 2020-21">Buy Here</a></sub></details>||
+|Lexus|RC 2018-20|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=RC 2018-20">Buy Here</a></sub></details>||
+|Lexus|RX 2016|Lexus Safety System+|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=RX 2016">Buy Here</a></sub></details>||
+|Lexus|RX 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=RX 2017-19">Buy Here</a></sub></details>||
+|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=RX 2020-22">Buy Here</a></sub></details>||
+|Lexus|RX Hybrid 2016|Lexus Safety System+|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=RX Hybrid 2016">Buy Here</a></sub></details>||
+|Lexus|RX Hybrid 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=RX Hybrid 2017-19">Buy Here</a></sub></details>||
+|Lexus|RX Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=RX Hybrid 2020-22">Buy Here</a></sub></details>||
+|Lexus|UX Hybrid 2019-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=UX Hybrid 2019-23">Buy Here</a></sub></details>||
+|Lincoln|Aviator 2020-21|Co-Pilot360 Plus|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lincoln&model=Aviator 2020-21">Buy Here</a></sub></details>||
+|MAN|eTGE 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 angled mount (8 degrees)<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=MAN&model=eTGE 2020-23">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|MAN|TGE 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 angled mount (8 degrees)<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=MAN&model=TGE 2017-23">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Mazda|CX-5 2022-24|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Mazda connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Mazda&model=CX-5 2022-24">Buy Here</a></sub></details>||
+|Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Mazda connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Mazda&model=CX-9 2021-23">Buy Here</a></sub></details>|<a href="https://youtu.be/dA3duO4a0O4" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Nissan B connector<br>- 1 RJ45 cable (7 ft)<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Nissan&model=Altima 2019-20">Buy Here</a></sub></details>||
+|Nissan|Leaf 2018-23|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Nissan A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Nissan&model=Leaf 2018-23">Buy Here</a></sub></details>|<a href="https://youtu.be/vaMbtAh_0cY" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Nissan A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Nissan&model=Rogue 2018-20">Buy Here</a></sub></details>||
+|Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Nissan A connector<br>- 1 RJ45 cable (7 ft)<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Nissan&model=X-Trail 2017">Buy Here</a></sub></details>||
+|Ram|1500 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Ram connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Ram&model=1500 2019-23">Buy Here</a></sub></details>||
+|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=SEAT&model=Ateca 2018">Buy Here</a></sub></details>||
+|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=SEAT&model=Leon 2014-20">Buy Here</a></sub></details>||
+|Subaru|Ascent 2019-21|All[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Subaru&model=Ascent 2019-21">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>||
+|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Subaru&model=Crosstrek 2018-19">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|<a href="https://youtu.be/Agww7oE1k-s?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Subaru&model=Crosstrek 2020-23">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>||
+|Subaru|Forester 2019-21|All[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Subaru&model=Forester 2019-21">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>||
+|Subaru|Impreza 2017-19|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Subaru&model=Impreza 2017-19">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>||
+|Subaru|Impreza 2020-22|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Subaru&model=Impreza 2020-22">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>||
+|Subaru|Legacy 2020-22|All[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru B connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Subaru&model=Legacy 2020-22">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>||
+|Subaru|Outback 2020-22|All[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru B connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Subaru&model=Outback 2020-22">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>||
+|Subaru|XV 2018-19|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Subaru&model=XV 2018-19">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|<a href="https://youtu.be/Agww7oE1k-s?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Subaru|XV 2020-21|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Subaru A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Subaru&model=XV 2020-21">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>||
+|Škoda|Fabia 2022-23[<sup>11</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Škoda&model=Fabia 2022-23">Buy Here</a></sub></details>[<sup>13</sup>](#footnotes)||
+|Škoda|Kamiq 2021-23[<sup>9,11</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Škoda&model=Kamiq 2021-23">Buy Here</a></sub></details>[<sup>13</sup>](#footnotes)||
+|Škoda|Karoq 2019-23[<sup>11</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Škoda&model=Karoq 2019-23">Buy Here</a></sub></details>||
+|Škoda|Kodiaq 2017-23[<sup>11</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Škoda&model=Kodiaq 2017-23">Buy Here</a></sub></details>||
+|Škoda|Octavia 2015-19[<sup>11</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Škoda&model=Octavia 2015-19">Buy Here</a></sub></details>||
+|Škoda|Octavia RS 2016[<sup>11</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Škoda&model=Octavia RS 2016">Buy Here</a></sub></details>||
+|Škoda|Scala 2020-23[<sup>11</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Škoda&model=Scala 2020-23">Buy Here</a></sub></details>[<sup>13</sup>](#footnotes)||
+|Škoda|Superb 2015-22[<sup>11</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Škoda&model=Superb 2015-22">Buy Here</a></sub></details>||
+|Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Alphard 2019-20">Buy Here</a></sub></details>||
+|Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Alphard Hybrid 2021">Buy Here</a></sub></details>||
+|Toyota|Avalon 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Avalon 2016">Buy Here</a></sub></details>||
+|Toyota|Avalon 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Avalon 2017-18">Buy Here</a></sub></details>||
+|Toyota|Avalon 2019-21|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Avalon 2019-21">Buy Here</a></sub></details>||
+|Toyota|Avalon 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Avalon 2022">Buy Here</a></sub></details>||
+|Toyota|Avalon Hybrid 2019-21|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Avalon Hybrid 2019-21">Buy Here</a></sub></details>||
+|Toyota|Avalon Hybrid 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Avalon Hybrid 2022">Buy Here</a></sub></details>||
+|Toyota|C-HR 2017-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=C-HR 2017-20">Buy Here</a></sub></details>||
+|Toyota|C-HR 2021|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=C-HR 2021">Buy Here</a></sub></details>||
+|Toyota|C-HR Hybrid 2017-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=C-HR Hybrid 2017-20">Buy Here</a></sub></details>||
+|Toyota|C-HR Hybrid 2021-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=C-HR Hybrid 2021-22">Buy Here</a></sub></details>||
+|Toyota|Camry 2018-20|All|Stock|0 mph[<sup>8</sup>](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Camry 2018-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=fkcjviZY9CM" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Toyota|Camry 2021-24|All|openpilot|0 mph[<sup>8</sup>](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Camry 2021-24">Buy Here</a></sub></details>||
+|Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Camry Hybrid 2018-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=Q2DYY0AWKgk" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Toyota|Camry Hybrid 2021-24|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Camry Hybrid 2021-24">Buy Here</a></sub></details>||
+|Toyota|Corolla 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Corolla 2017-19">Buy Here</a></sub></details>||
+|Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Corolla 2020-22">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=_66pXk0CBYA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Toyota|Corolla Cross (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Corolla Cross (Non-US only) 2020-23">Buy Here</a></sub></details>||
+|Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Corolla Cross Hybrid (Non-US only) 2020-22">Buy Here</a></sub></details>||
+|Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Corolla Hatchback 2019-22">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=_66pXk0CBYA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Corolla Hybrid 2020-22">Buy Here</a></sub></details>||
+|Toyota|Corolla Hybrid (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Corolla Hybrid (Non-US only) 2020-23">Buy Here</a></sub></details>||
+|Toyota|Highlander 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Highlander 2017-19">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=0wS0wXSLzoo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Toyota|Highlander 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Highlander 2020-23">Buy Here</a></sub></details>||
+|Toyota|Highlander Hybrid 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Highlander Hybrid 2017-19">Buy Here</a></sub></details>||
+|Toyota|Highlander Hybrid 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Highlander Hybrid 2020-23">Buy Here</a></sub></details>||
+|Toyota|Mirai 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Mirai 2021">Buy Here</a></sub></details>||
+|Toyota|Prius 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Prius 2016">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=8zopPJI8XQ0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Toyota|Prius 2017-20|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Prius 2017-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=8zopPJI8XQ0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Toyota|Prius 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Prius 2021-22">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=J58TvCpUd4U" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Toyota|Prius Prime 2017-20|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Prius Prime 2017-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=8zopPJI8XQ0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Toyota|Prius Prime 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Prius Prime 2021-22">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=J58TvCpUd4U" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Toyota|Prius v 2017|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Prius v 2017">Buy Here</a></sub></details>||
+|Toyota|RAV4 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=RAV4 2016">Buy Here</a></sub></details>||
+|Toyota|RAV4 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=RAV4 2017-18">Buy Here</a></sub></details>||
+|Toyota|RAV4 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=RAV4 2019-21">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=wJxjDd42gGA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Toyota|RAV4 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=RAV4 2022">Buy Here</a></sub></details>||
+|Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=RAV4 Hybrid 2016">Buy Here</a></sub></details>|<a href="https://youtu.be/LhT5VzJVfNI?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Toyota|RAV4 Hybrid 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=RAV4 Hybrid 2017-18">Buy Here</a></sub></details>|<a href="https://youtu.be/LhT5VzJVfNI?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=RAV4 Hybrid 2019-21">Buy Here</a></sub></details>||
+|Toyota|RAV4 Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=RAV4 Hybrid 2022">Buy Here</a></sub></details>|<a href="https://youtu.be/U0nH9cnrFB0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Toyota|Sienna 2018-20|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Toyota&model=Sienna 2018-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=q1UPOo4Sh68" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Volkswagen|Arteon 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Arteon 2018-23">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Volkswagen|Arteon eHybrid 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Arteon eHybrid 2020-23">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Volkswagen|Arteon R 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Arteon R 2020-23">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Volkswagen|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Atlas 2018-23">Buy Here</a></sub></details>||
+|Volkswagen|Atlas Cross Sport 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Atlas Cross Sport 2020-22">Buy Here</a></sub></details>||
+|Volkswagen|California 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 angled mount (8 degrees)<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=California 2021-23">Buy Here</a></sub></details>||
+|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 angled mount (8 degrees)<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Caravelle 2020">Buy Here</a></sub></details>||
+|Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=CC 2018-22">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Volkswagen|Crafter 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 angled mount (8 degrees)<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Crafter 2017-23">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Volkswagen|e-Crafter 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 angled mount (8 degrees)<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=e-Crafter 2018-23">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=e-Golf 2014-20">Buy Here</a></sub></details>||
+|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Golf 2015-20">Buy Here</a></sub></details>||
+|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Golf Alltrack 2015-19">Buy Here</a></sub></details>||
+|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Golf GTD 2015-20">Buy Here</a></sub></details>||
+|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Golf GTE 2015-20">Buy Here</a></sub></details>||
+|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Golf GTI 2015-21">Buy Here</a></sub></details>||
+|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Golf R 2015-19">Buy Here</a></sub></details>||
+|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Golf SportsVan 2015-20">Buy Here</a></sub></details>||
+|Volkswagen|Grand California 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 angled mount (8 degrees)<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Grand California 2019-23">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
+|Volkswagen|Jetta 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Jetta 2018-22">Buy Here</a></sub></details>||
+|Volkswagen|Jetta GLI 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Jetta GLI 2021-22">Buy Here</a></sub></details>||
+|Volkswagen|Passat 2015-22[<sup>10</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Passat 2015-22">Buy Here</a></sub></details>||
+|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Passat Alltrack 2015-22">Buy Here</a></sub></details>||
+|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Passat GTE 2015-22">Buy Here</a></sub></details>||
+|Volkswagen|Polo 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Polo 2018-23">Buy Here</a></sub></details>[<sup>13</sup>](#footnotes)||
+|Volkswagen|Polo GTI 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Polo GTI 2018-23">Buy Here</a></sub></details>[<sup>13</sup>](#footnotes)||
+|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=T-Cross 2021">Buy Here</a></sub></details>[<sup>13</sup>](#footnotes)||
+|Volkswagen|T-Roc 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=T-Roc 2018-22">Buy Here</a></sub></details>[<sup>13</sup>](#footnotes)||
+|Volkswagen|Taos 2022-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Taos 2022-23">Buy Here</a></sub></details>||
+|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Teramont 2018-22">Buy Here</a></sub></details>||
+|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Teramont Cross Sport 2021-22">Buy Here</a></sub></details>||
+|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Teramont X 2021-22">Buy Here</a></sub></details>||
+|Volkswagen|Tiguan 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Tiguan 2018-23">Buy Here</a></sub></details>||
+|Volkswagen|Tiguan eHybrid 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Tiguan eHybrid 2021-23">Buy Here</a></sub></details>||
+|Volkswagen|Touran 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 J533 connector<br>- 1 USB-C coupler<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Volkswagen&model=Touran 2016-23">Buy Here</a></sub></details>||
 
 ### Footnotes
-<sup>1</sup>Experimental openpilot longitudinal control is available behind a toggle; the toggle is only available in non-release branches such as `devel` or `master-ci`.  <br />
+<sup>1</sup>openpilot Longitudinal Control (Alpha) is available behind a toggle; the toggle is only available in non-release branches such as `devel` or `master-ci`. <br />
 <sup>2</sup>By default, this car will use the stock Adaptive Cruise Control (ACC) for longitudinal control. If the Driver Support Unit (DSU) is disconnected, openpilot ACC will replace stock ACC. <b><i>NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).</i></b> <br />
 <sup>3</sup>Refers only to the Focus Mk4 (C519) available in Europe/China/Taiwan/Australasia, not the Focus Mk3 (C346) in North and South America/Southeast Asia. <br />
 <sup>4</sup>Requires a <a href="https://github.com/commaai/openpilot/wiki/GM#hardware" target="_blank">community built ASCM harness</a>. <b><i>NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).</i></b> <br />
 <sup>5</sup>2019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph. <br />
-<sup>6</sup>Requires a <a href="https://comma.ai/shop/panda" target="_blank">red panda</a> for this <a href="https://en.wikipedia.org/wiki/CAN_FD" target="_blank">CAN FD car</a>. All the hardware needed is sold in the <a href="https://comma.ai/shop/can-fd-panda-kit" target="_blank">CAN FD kit</a>. <br />
+<sup>6</sup>Requires a <a href="https://comma.ai/shop/can-fd-panda-kit" target="_blank">CAN FD panda kit</a> if not using comma 3X for this <a href="https://en.wikipedia.org/wiki/CAN_FD" target="_blank">CAN FD car</a>. <br />
 <sup>7</sup>In the non-US market, openpilot requires the car to come equipped with EyeSight with Lane Keep Assistance. <br />
 <sup>8</sup>openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control. <br />
 <sup>9</sup>Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform. <br />
 <sup>10</sup>Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets. <br />
-<sup>11</sup>Some Škoda vehicles are equipped with heated windshields, which are known to block GPS signal needed for some comma three functionality. <br />
+<sup>11</sup>Some Škoda vehicles are equipped with heated windshields, which are known to block GPS signal needed for some comma 3X functionality. <br />
 <sup>12</sup>Only available for vehicles using a gateway (J533) harness. At this time, vehicles using a camera harness are limited to using stock ACC. <br />
 <sup>13</sup>Model-years 2022 and beyond may have a combined CAN gateway and BCM, which is supported by openpilot in software, but doesn't yet have a harness available from the comma store. <br />
 
@@ -306,12 +319,12 @@ If your car has the following packages or features, then it's a good candidate f
 
 ### FlexRay
 
-All the cars that openpilot supports use a [CAN bus](https://en.wikipedia.org/wiki/CAN_bus) for communication between all the car's computers, however a CAN bus isn't the only way that the cars in your computer can communicate. Most, if not all, vehicles from the following manufacturers use [FlexRay](https://en.wikipedia.org/wiki/FlexRay) instead of a CAN bus: **BMW, Mercedes, Audi, Land Rover, and some Volvo**. These cars may one day be supported, but we have no immediate plans to support FlexRay.
+All the cars that openpilot supports use a [CAN bus](https://en.wikipedia.org/wiki/CAN_bus) for communication between all the car's computers, however a CAN bus isn't the only way that the computers in your car can communicate. Most, if not all, vehicles from the following manufacturers use [FlexRay](https://en.wikipedia.org/wiki/FlexRay) instead of a CAN bus: **BMW, Mercedes, Audi, Land Rover, and some Volvo**. These cars may one day be supported, but we have no immediate plans to support FlexRay.
 
 ### Toyota Security
 
 openpilot does not yet support these Toyota models due to a new message authentication method.
-[Vote](https://comma.ai/shop/products/vote) if you'd like to see openpilot support on these models.
+[Vote](https://comma.ai/shop#toyota-security) if you'd like to see openpilot support on these models.
 
 * Toyota RAV4 Prime 2021+
 * Toyota Sienna 2021+
diff --git a/laika/__init__.py b/laika/__init__.py
deleted file mode 100644
index 1c9762b4b2..0000000000
--- a/laika/__init__.py
+++ /dev/null
@@ -1,9 +0,0 @@
-import logging
-import os
-
-from .astro_dog import AstroDog
-assert AstroDog is not None
-
-# setup logging
-LOGLEVEL = os.environ.get('LOGLEVEL', 'INFO').upper()
-logging.basicConfig(level=LOGLEVEL, format='%(message)s')
diff --git a/laika/astro_dog.py b/laika/astro_dog.py
deleted file mode 100644
index a2d01946ad..0000000000
--- a/laika/astro_dog.py
+++ /dev/null
@@ -1,396 +0,0 @@
-from collections import defaultdict
-from concurrent.futures import ThreadPoolExecutor
-from typing import DefaultDict, Dict, Iterable, List, Optional, Union
-
-from .constants import SECS_IN_DAY, SECS_IN_HR
-from .helpers import ConstellationId, get_constellation, get_closest, get_el_az, TimeRangeHolder
-from .ephemeris import Ephemeris, EphemerisType, GLONASSEphemeris, GPSEphemeris, PolyEphemeris, parse_sp3_orbits, parse_rinex_nav_msg_gps, \
-  parse_rinex_nav_msg_glonass
-from .downloader import download_orbits_gps, download_orbits_russia_src, download_nav, download_ionex, download_dcb, download_prediction_orbits_russia_src
-from .downloader import download_cors_station
-from .trop import saast
-from .iono import IonexMap, parse_ionex, get_slant_delay
-from .dcb import DCB, parse_dcbs
-from .gps_time import GPSTime
-from .dgps import get_closest_station_names, parse_dgps
-from . import constants
-
-MAX_DGPS_DISTANCE = 100_000  # in meters, because we're not barbarians
-
-
-class AstroDog:
-  '''
-  auto_update: flag indicating whether laika should fetch files from web automatically
-  cache_dir:   directory where data files are downloaded to and cached
-  dgps:        flag indicating whether laika should use dgps (CORS)
-               data to calculate pseudorange corrections
-  valid_const: list of constellation identifiers laika will try process
-  valid_ephem_types: set of ephemeris types that are allowed to use and download.
-                Default is set to use all orbit ephemeris types
-  clear_old_ephemeris: flag indicating if ephemeris for an individual satellite should be overwritten when new ephemeris is added.
-  '''
-
-  def __init__(self, auto_update=True,
-               cache_dir='/tmp/gnss/',
-               dgps=False,
-               valid_const=(ConstellationId.GPS, ConstellationId.GLONASS),
-               valid_ephem_types=EphemerisType.all_orbits(),
-               clear_old_ephemeris=False):
-
-    for const in valid_const:
-      if not isinstance(const, ConstellationId):
-        raise TypeError(f"valid_const must be a list of ConstellationId, got {const}")
-  
-    self.auto_update = auto_update
-    self.cache_dir = cache_dir
-    self.clear_old_ephemeris = clear_old_ephemeris
-    self.dgps = dgps
-    if not isinstance(valid_ephem_types, Iterable):
-      valid_ephem_types = [valid_ephem_types]
-    self.pull_orbit = len(set(EphemerisType.all_orbits()) & set(valid_ephem_types)) > 0
-    self.pull_nav = EphemerisType.NAV in valid_ephem_types
-    self.use_qcom_poly = EphemerisType.QCOM_POLY in valid_ephem_types
-    self.valid_const = valid_const
-    self.valid_ephem_types = valid_ephem_types
-
-    self.orbit_fetched_times = TimeRangeHolder()
-    self.navs_fetched_times = TimeRangeHolder()
-    self.dcbs_fetched_times = TimeRangeHolder()
-
-    self.dgps_delays = []
-    self.ionex_maps: List[IonexMap] = []
-    self.orbits: DefaultDict[str, List[PolyEphemeris]] = defaultdict(list)
-    self.qcom_polys: DefaultDict[str, List[PolyEphemeris]] = defaultdict(list)
-    self.navs: DefaultDict[str, List[Union[GPSEphemeris, GLONASSEphemeris]]] = defaultdict(list)
-    self.dcbs: DefaultDict[str, List[DCB]] = defaultdict(list)
-
-    self.cached_ionex: Optional[IonexMap] = None
-    self.cached_dgps = None
-    self.cached_orbit: DefaultDict[str, Optional[PolyEphemeris]] = defaultdict(lambda: None)
-    self.cached_qcom_polys: DefaultDict[str, Optional[PolyEphemeris]] = defaultdict(lambda: None)
-    self.cached_nav: DefaultDict[str, Union[GPSEphemeris, GLONASSEphemeris, None]] = defaultdict(lambda: None)
-    self.cached_dcb: DefaultDict[str, Optional[DCB]] = defaultdict(lambda: None)
-
-  def get_ionex(self, time) -> Optional[IonexMap]:
-    ionex: Optional[IonexMap] = self._get_latest_valid_data(self.ionex_maps, self.cached_ionex, self.get_ionex_data, time)
-    if ionex is None:
-      if self.auto_update:
-        raise RuntimeError("Pulled ionex, but still can't get valid for time " + str(time))
-    else:
-      self.cached_ionex = ionex
-    return ionex
-
-  def get_nav(self, prn, time):
-    skip_download = time in self.navs_fetched_times
-    nav = self._get_latest_valid_data(self.navs[prn], self.cached_nav[prn], self.get_nav_data, time, skip_download)
-    if nav is not None:
-      self.cached_nav[prn] = nav
-    return nav
-
-  @staticmethod
-  def _select_valid_temporal_items(item_dict, time, cache):
-    '''Returns only valid temporal item for specific time from currently fetched
-    data.'''
-    result = {}
-    for prn, temporal_objects in item_dict.items():
-      cached = cache[prn]
-      if cached is not None and cached.valid(time):
-        obj = cached
-      else:
-        obj = get_closest(time, temporal_objects)
-        if obj is None or not obj.valid(time):
-          continue
-        cache[prn] = obj
-      result[prn] = obj
-    return result
-
-  def get_all_ephem_prns(self):
-    return set(self.orbits.keys()).union(set(self.navs.keys())).union(set(self.qcom_polys.keys()))
-
-  def get_navs(self, time):
-    if time not in self.navs_fetched_times:
-      self.get_nav_data(time)
-    return AstroDog._select_valid_temporal_items(self.navs, time, self.cached_nav)
-
-  def get_orbit(self, prn: str, time: GPSTime):
-    skip_download = time in self.orbit_fetched_times
-    orbit = self._get_latest_valid_data(self.orbits[prn], self.cached_orbit[prn], self.get_orbit_data, time, skip_download)
-    if orbit is not None:
-      self.cached_orbit[prn] = orbit
-    return orbit
-
-  def get_qcom_poly(self, prn: str, time: GPSTime):
-    poly = self._get_latest_valid_data(self.qcom_polys[prn], self.cached_qcom_polys[prn], None, time, True)
-    if poly is not None:
-      self.cached_qcom_polys[prn] = poly
-    return poly
-
-  def get_orbits(self, time):
-    if time not in self.orbit_fetched_times:
-      self.get_orbit_data(time)
-    return AstroDog._select_valid_temporal_items(self.orbits, time, self.cached_orbit)
-
-  def get_dcb(self, prn, time):
-    skip_download = time in self.dcbs_fetched_times
-    dcb = self._get_latest_valid_data(self.dcbs[prn], self.cached_dcb[prn], self.get_dcb_data, time, skip_download)
-    if dcb is not None:
-      self.cached_dcb[prn] = dcb
-    return dcb
-
-  def get_dgps_corrections(self, time, recv_pos):
-    latest_data = self._get_latest_valid_data(self.dgps_delays, self.cached_dgps, self.get_dgps_data, time, recv_pos=recv_pos)
-    if latest_data is None:
-      if self.auto_update:
-        raise RuntimeError("Pulled dgps, but still can't get valid for time " + str(time))
-    else:
-      self.cached_dgps = latest_data
-    return latest_data
-
-  def add_qcom_polys(self, new_ephems: Dict[str, List[Ephemeris]]):
-    self._add_ephems(new_ephems, self.qcom_polys)
-
-  def add_orbits(self, new_ephems: Dict[str, List[Ephemeris]]):
-    self._add_ephems(new_ephems, self.orbits)
-
-  def add_navs(self, new_ephems: Dict[str, List[Ephemeris]]):
-    self._add_ephems(new_ephems, self.navs)
-
-  def _add_ephems(self, new_ephems: Dict[str, List[Ephemeris]], ephems_dict):
-    for k, v in new_ephems.items():
-      if len(v) > 0:
-        if self.clear_old_ephemeris:
-          ephems_dict[k] = v
-        else:
-          ephems_dict[k].extend(v)
-
-  def add_ephem_fetched_time(self, ephems, fetched_times):
-    min_epochs = []
-    max_epochs = []
-    for v in ephems.values():
-      if len(v) > 0:
-        min_ephem, max_ephem = self.get_epoch_range(v)
-        min_epochs.append(min_ephem)
-        max_epochs.append(max_ephem)
-    if len(min_epochs) > 0:
-      min_epoch = min(min_epochs)
-      max_epoch = max(max_epochs)
-      fetched_times.add(min_epoch, max_epoch)
-
-  def get_nav_data(self, time):
-    def download_and_parse(constellation, parse_rinex_nav_func):
-      file_path = download_nav(time, cache_dir=self.cache_dir, constellation=constellation)
-      return parse_rinex_nav_func(file_path) if file_path else {}
-
-    fetched_ephems = {}
-
-    if ConstellationId.GPS in self.valid_const:
-      fetched_ephems = download_and_parse(ConstellationId.GPS, parse_rinex_nav_msg_gps)
-    if ConstellationId.GLONASS in self.valid_const:
-      for k, v in download_and_parse(ConstellationId.GLONASS, parse_rinex_nav_msg_glonass).items():
-        fetched_ephems.setdefault(k, []).extend(v)
-    self.add_navs(fetched_ephems)
-
-    if sum([len(v) for v in fetched_ephems.values()]) == 0:
-      begin_day = GPSTime(time.week, SECS_IN_DAY * (time.tow // SECS_IN_DAY))
-      end_day = GPSTime(time.week, SECS_IN_DAY * (1 + (time.tow // SECS_IN_DAY)))
-      self.navs_fetched_times.add(begin_day, end_day)
-
-  def download_parse_orbit(self, gps_time: GPSTime, skip_before_epoch=None) -> Dict[str, List[PolyEphemeris]]:
-    # Download multiple days to be able to polyfit at the start-end of the day
-    time_steps = [gps_time - SECS_IN_DAY, gps_time, gps_time + SECS_IN_DAY]
-    with ThreadPoolExecutor() as executor:
-      futures_other = [executor.submit(download_orbits_russia_src, t, self.cache_dir, self.valid_ephem_types) for t in time_steps]
-      futures_gps = None
-      if ConstellationId.GPS in self.valid_const:
-        futures_gps = [executor.submit(download_orbits_gps, t, self.cache_dir, self.valid_ephem_types) for t in time_steps]
-
-      ephems_other = parse_sp3_orbits([f.result() for f in futures_other if f.result()], self.valid_const, skip_before_epoch)
-      ephems_us = parse_sp3_orbits([f.result() for f in futures_gps if f.result()], self.valid_const, skip_before_epoch) if futures_gps else {}
-
-    return {k: ephems_other.get(k, []) + ephems_us.get(k, []) for k in set(list(ephems_other.keys()) + list(ephems_us.keys()))}
-
-  def download_parse_prediction_orbit(self, gps_time: GPSTime):
-    assert EphemerisType.ULTRA_RAPID_ORBIT in self.valid_ephem_types
-    skip_until_epoch = gps_time - 2 * SECS_IN_HR
-
-    result = download_prediction_orbits_russia_src(gps_time, self.cache_dir)
-    if result is not None:
-      result = [result]
-    elif ConstellationId.GPS in self.valid_const:
-      # Slower fallback. Russia src prediction orbits are published from 2022
-      result = [download_orbits_gps(t, self.cache_dir, self.valid_ephem_types) for t in [gps_time - SECS_IN_DAY, gps_time]]
-    if result is None:
-      return {}
-    return parse_sp3_orbits(result, self.valid_const, skip_until_epoch=skip_until_epoch)
-
-  def get_orbit_data(self, time: GPSTime, only_predictions=False):
-    if only_predictions:
-      ephems_sp3 = self.download_parse_prediction_orbit(time)
-    else:
-      ephems_sp3 = self.download_parse_orbit(time)
-    if sum([len(v) for v in ephems_sp3.values()]) < 5:
-      raise RuntimeError(f'No orbit data found. For Time {time.as_datetime()} constellations {self.valid_const} valid ephem types {self.valid_ephem_types}')
-    self.add_ephem_fetched_time(ephems_sp3, self.orbit_fetched_times)
-    self.add_orbits(ephems_sp3)
-
-  def get_dcb_data(self, time):
-    file_path_dcb = download_dcb(time, cache_dir=self.cache_dir)
-    dcbs = parse_dcbs(file_path_dcb, self.valid_const)
-    for dcb in dcbs:
-      self.dcbs[dcb.prn].append(dcb)
-
-    if len(dcbs) != 0:
-      min_epoch, max_epoch = self.get_epoch_range(dcbs)
-      self.dcbs_fetched_times.add(min_epoch, max_epoch)
-
-  def get_epoch_range(self, new_ephems):
-    min_ephem = min(new_ephems, key=lambda e: e.epoch)
-    max_ephem = max(new_ephems, key=lambda e: e.epoch)
-    min_epoch = min_ephem.epoch - min_ephem.max_time_diff
-    max_epoch = max_ephem.epoch + max_ephem.max_time_diff
-    return min_epoch, max_epoch
-
-  def get_ionex_data(self, time):
-    file_path_ionex = download_ionex(time, cache_dir=self.cache_dir)
-    ionex_maps = parse_ionex(file_path_ionex)
-    for im in ionex_maps:
-      self.ionex_maps.append(im)
-
-  def get_dgps_data(self, time, recv_pos):
-    station_names = get_closest_station_names(recv_pos, k=8, max_distance=MAX_DGPS_DISTANCE, cache_dir=self.cache_dir)
-    for station_name in station_names:
-      file_path_station = download_cors_station(time, station_name, cache_dir=self.cache_dir)
-      if file_path_station:
-        dgps = parse_dgps(station_name, file_path_station,
-                          self, max_distance=MAX_DGPS_DISTANCE,
-                          required_constellations=self.valid_const)
-        if dgps is not None:
-          self.dgps_delays.append(dgps)
-          break
-
-  def get_tgd_from_nav(self, prn, time):
-    if get_constellation(prn) not in self.valid_const:
-      return None
-
-    eph = self.get_nav(prn, time)
-
-    if eph:
-      return eph.get_tgd()
-    return None
-
-  def get_eph(self, prn, time):
-    if get_constellation(prn) not in self.valid_const:
-      return None
-    eph = None
-    if self.pull_orbit:
-      eph = self.get_orbit(prn, time)
-    if not eph and self.pull_nav:
-      eph = self.get_nav(prn, time)
-    if not eph and self.use_qcom_poly:
-      eph = self.get_qcom_poly(prn, time)
-    return eph
-
-  def get_sat_info(self, prn, time):
-    eph = self.get_eph(prn, time)
-    if eph:
-      return eph.get_sat_info(time)
-    return None
-
-  def get_all_sat_info(self, time):
-    ephs = {}
-    if self.pull_orbit:
-      ephs = self.get_orbits(time)
-    if len(ephs) == 0 and self.pull_nav:
-      ephs = self.get_navs(time)
-
-    return {prn: eph.get_sat_info(time) for prn, eph in ephs.items()}
-
-  def get_glonass_channel(self, prn, time):
-    nav = self.get_nav(prn, time)
-    if nav:
-      return nav.channel
-    return None
-
-  def get_frequency(self, prn, time, signal='C1C'):
-    if get_constellation(prn) == ConstellationId.GPS:
-      switch = {'1': constants.GPS_L1,
-                '2': constants.GPS_L2,
-                '5': constants.GPS_L5,
-                '6': constants.GALILEO_E6,
-                '7': constants.GALILEO_E5B,
-                '8': constants.GALILEO_E5AB}
-      freq = switch.get(signal[1])
-      if freq:
-        return freq
-      raise NotImplementedError("Dont know this GPS frequency: ", signal, prn)
-    elif get_constellation(prn) == ConstellationId.GLONASS:
-      n = self.get_glonass_channel(prn, time)
-      if n is None:
-        return None
-      switch = {'1': constants.GLONASS_L1 + n * constants.GLONASS_L1_DELTA,
-                '2': constants.GLONASS_L2 + n * constants.GLONASS_L2_DELTA,
-                '5': constants.GLONASS_L5 + n * constants.GLONASS_L5_DELTA,
-                '6': constants.GALILEO_E6,
-                '7': constants.GALILEO_E5B,
-                '8': constants.GALILEO_E5AB}
-      freq = switch.get(signal[1])
-      if freq:
-        return freq
-      raise NotImplementedError("Dont know this GLONASS frequency: ", signal, prn)
-
-  def get_delay(self, prn, time, rcv_pos, no_dgps=False, signal='C1C', freq=None):
-    sat_info = self.get_sat_info(prn, time)
-    if sat_info is None:
-      return None
-    sat_pos = sat_info[0]
-    el, az = get_el_az(rcv_pos, sat_pos)
-    if el < 0.2:
-      return None
-    if self.dgps and not no_dgps:
-      return self._get_delay_dgps(prn, rcv_pos, time)
-
-    ionex = self.get_ionex(time)
-    if not freq and ionex is not None:
-      freq = self.get_frequency(prn, time, signal)
-    dcb = self.get_dcb(prn, time)
-    # When using internet we expect all data or return None
-    if self.auto_update and (ionex is None or dcb is None or freq is None):
-      return None
-    if ionex is not None:
-      iono_delay = ionex.get_delay(rcv_pos, az, el, sat_pos, time, freq)
-    else:
-      # 5m vertical delay is a good default
-      iono_delay = get_slant_delay(rcv_pos, az, el, sat_pos, time, freq, vertical_delay=5.0)
-    trop_delay = saast(rcv_pos, el)
-    code_bias = dcb.get_delay(signal) if dcb is not None else 0.
-    return iono_delay + trop_delay + code_bias
-
-  def _get_delay_dgps(self, prn, rcv_pos, time):
-    dgps_corrections = self.get_dgps_corrections(time, rcv_pos)
-    if dgps_corrections is None:
-      return None
-    return dgps_corrections.get_delay(prn, time)
-
-  def _get_latest_valid_data(self, data, latest_data, download_data_func, time, skip_download=False, recv_pos=None):
-    def is_valid(latest_data):
-      if recv_pos is None:
-        return latest_data is not None and latest_data.valid(time)
-      else:
-        return latest_data is not None and latest_data.valid(time, recv_pos)
-
-    if is_valid(latest_data):
-      return latest_data
-
-    latest_data = get_closest(time, data, recv_pos=recv_pos)
-    if is_valid(latest_data):
-      return latest_data
-    if skip_download or not self.auto_update:
-      return None
-    if recv_pos is not None:
-      download_data_func(time, recv_pos)
-    else:
-      download_data_func(time)
-    latest_data = get_closest(time, data, recv_pos=recv_pos)
-    if is_valid(latest_data):
-      return latest_data
-    return None
diff --git a/laika/constants.py b/laika/constants.py
deleted file mode 100644
index 6b6d33110a..0000000000
--- a/laika/constants.py
+++ /dev/null
@@ -1,34 +0,0 @@
-# These are all from IS-GPS-200G unless otherwise noted
-
-SPEED_OF_LIGHT = 2.99792458e8  # m/s
-
-# Physical parameters of the Earth
-EARTH_GM = 3.986005e14  # m^3/s^2 (gravitational constant * mass of earth)
-EARTH_RADIUS = 6.3781e6  # m
-EARTH_ROTATION_RATE = 7.2921151467e-005  # rad/s (WGS84 earth rotation rate)
-
-# GPS system parameters:
-GPS_L1 = l1 = 1.57542e9  # Hz
-GPS_L2 = l2 = 1.22760e9  # Hz
-GPS_L5 = l5 = 1.17645e9  # Hz Also E5
-
-#GLONASS system parameters
-#TODO this is old convention
-GLONASS_L1 = 1.602e9
-GLONASS_L1_DELTA = 0.5625e6
-GLONASS_L2 = 1.246e9
-GLONASS_L2_DELTA = 0.4375e6
-GLONASS_L5 = 1.201e9
-GLONASS_L5_DELTA = 0.4375e6
-
-#Galileo system parameters:  # Has additional frequencies on E6
-#Source RINEX 2.11 document
-GALILEO_E5B = 1.207140e9  # Hz
-GALILEO_E5AB = 1.191795e9  # Hz
-GALILEO_E6 = 1.27875e9  # Hz
-
-SECS_IN_MIN = 60
-SECS_IN_HR = 60*SECS_IN_MIN
-SECS_IN_DAY = 24*SECS_IN_HR
-SECS_IN_WEEK = 7*SECS_IN_DAY
-SECS_IN_YEAR = 365*SECS_IN_DAY
diff --git a/laika/dcb.py b/laika/dcb.py
deleted file mode 100644
index 06017ff4cd..0000000000
--- a/laika/dcb.py
+++ /dev/null
@@ -1,84 +0,0 @@
-from datetime import datetime
-from .constants import SECS_IN_HR, SECS_IN_WEEK, \
-                      SPEED_OF_LIGHT, GPS_L1, GPS_L2
-from .gps_time import GPSTime
-from .helpers import get_constellation
-import warnings
-
-
-class DCB:
-  def __init__(self, prn, data):
-    self.max_time_diff = 2*SECS_IN_WEEK
-    self.prn = prn
-    self.epoch = data['epoch']
-    self.healthy = True
-    if 'C1W_C2W' in data:
-      self.C1W_C2W = data['C1W_C2W']
-    elif 'C1P_C2P' in data:
-      self.C1W_C2W = data['C1P_C2P']
-    else:
-      self.healthy = False
-    if 'C1C_C1W' in data:
-      self.C1C_C1W = data['C1C_C1W']
-    elif 'C1C_C1P' in data:
-      self.C1C_C1W = data['C1C_C1P']
-    else:
-      self.healthy = False
-
-  def valid(self, time):
-    return abs(time - self.epoch) <= self.max_time_diff and self.healthy
-
-  def get_delay(self, signal):
-    if signal == 'C1C':
-      return (- SPEED_OF_LIGHT*1e-9*self.C1W_C2W*GPS_L2**2/(GPS_L1**2 - GPS_L2**2)
-              + SPEED_OF_LIGHT*1e-9*self.C1C_C1W)
-    if signal == 'C2P':
-      return (- SPEED_OF_LIGHT*1e-9*self.C1W_C2W*GPS_L1**2/(GPS_L1**2 - GPS_L2**2))
-    if signal == 'C1P':
-      return (SPEED_OF_LIGHT*1e-9*self.C1C_C1W)
-    ## Todo: update dcb database and get delay to include additional signals
-    if signal == 'C2C':
-      warnings.warn("Differential code bias not implemented for signal C2C", UserWarning)
-      return 0
-    if signal == 'C5C':
-      warnings.warn("Differential code bias not implemented for signal C5C", UserWarning)
-      return 0
-    if signal == 'C6C':
-      warnings.warn("Differential code bias not implemented for signal C6C", UserWarning)
-      return 0
-    if signal == 'C7C':
-      warnings.warn("Differential code bias not implemented for signal C7C", UserWarning)
-      return 0
-    if signal == 'C8C':
-      warnings.warn("Differential code bias not implemented for signal C8C", UserWarning)
-      return 0
-
-
-def parse_dcbs(file_name, SUPPORTED_CONSTELLATIONS):
-  with open(file_name, 'r+') as DCB_file:
-    contents = DCB_file.readlines()
-  data_started = False
-  dcbs_dict = {}
-  for line in contents:
-    if not data_started:
-      if line[1:4] == 'DSB':
-        data_started = True
-      else:
-        continue
-    line_components = line.split()
-    if len(line_components[2]) < 3:
-      break
-    prn = line_components[2]
-    if get_constellation(prn) not in SUPPORTED_CONSTELLATIONS:
-      continue
-    dcb_type = line_components[3] + '_' + line_components[4]
-    epoch = GPSTime.from_datetime(datetime.strptime(line_components[5], '%Y:%j:%f')) + 12*SECS_IN_HR
-    if prn not in dcbs_dict:
-      dcbs_dict[prn] = {}
-    dcbs_dict[prn][dcb_type] = float(line_components[8])
-    dcbs_dict[prn]['epoch'] = epoch
-
-  dcbs = []
-  for prn in dcbs_dict:
-    dcbs.append(DCB(prn, dcbs_dict[prn]))
-  return dcbs
diff --git a/laika/dgps.py b/laika/dgps.py
deleted file mode 100644
index 60d4393b2f..0000000000
--- a/laika/dgps.py
+++ /dev/null
@@ -1,161 +0,0 @@
-import os
-import numpy as np
-from datetime import datetime
-
-from .gps_time import GPSTime
-from .constants import SECS_IN_YEAR
-from . import raw_gnss as raw
-from . import opt
-from .rinex_file import RINEXFile
-from .downloader import download_cors_coords
-from .helpers import get_constellation, ConstellationId
-
-def mean_filter(delay):
-  d2 = delay.copy()
-  max_step = 10
-  for i in range(max_step, len(delay) - max_step):
-    finite_idxs = np.where(np.isfinite(delay[i - max_step:i + max_step]))
-    if max_step in finite_idxs[0]:
-      step = min([max_step, finite_idxs[0][-1] - max_step, max_step - finite_idxs[0][0]])
-      d2[i] = np.nanmean(delay[i - step:i + step + 1])
-  return d2
-
-
-def download_and_parse_station_postions(cors_station_positions_path, cache_dir):
-  if not os.path.isfile(cors_station_positions_path):
-    cors_stations = {}
-    coord_file_paths = download_cors_coords(cache_dir=cache_dir)
-    for coord_file_path in coord_file_paths:
-      try:
-        station_id = coord_file_path.split('/')[-1][:4]
-        with open(coord_file_path, 'r+') as coord_file:
-          contents = coord_file.readlines()
-        phase_center = False
-        for line_number in range(len(contents)):
-          if 'L1 Phase Center' in contents[line_number]:
-            phase_center = True
-          if not phase_center and 'ITRF2014 POSITION' in contents[line_number]:
-            velocity = [float(contents[line_number+8].split()[3]),
-                        float(contents[line_number+9].split()[3]),
-                        float(contents[line_number+10].split()[3])]
-          if phase_center and 'ITRF2014 POSITION' in contents[line_number]:
-            epoch = GPSTime.from_datetime(datetime(2005,1,1))
-            position = [float(contents[line_number+2].split()[3]),
-                        float(contents[line_number+3].split()[3]),
-                        float(contents[line_number+4].split()[3])]
-            cors_stations[station_id] = [epoch, position, velocity]
-            break
-      except ValueError:
-        pass
-    cors_station_positions_file = open(cors_station_positions_path, 'wb')
-    np.save(cors_station_positions_file, cors_stations)
-    cors_station_positions_file.close()
-
-
-def get_closest_station_names(pos, k=5, max_distance=100000, cache_dir='/tmp/gnss/'):
-  from scipy.spatial import cKDTree
-
-  cors_station_positions_dict = load_cors_station_positions(cache_dir)
-  station_ids = list(cors_station_positions_dict.keys())
-  station_positions = []
-  for station_id in station_ids:
-    station_positions.append(cors_station_positions_dict[station_id][1])
-  tree = cKDTree(station_positions)
-  distances, idxs = tree.query(pos, k=k, distance_upper_bound=max_distance)
-  return np.array(station_ids)[idxs]
-
-
-def load_cors_station_positions(cache_dir):
-  cors_station_positions_path = cache_dir + 'cors_coord/cors_station_positions'
-  download_and_parse_station_postions(cors_station_positions_path, cache_dir)
-  with open(cors_station_positions_path, 'rb') as f:
-    return np.load(f, allow_pickle=True).item()  # pylint: disable=unexpected-keyword-arg
-
-
-def get_station_position(station_id, cache_dir='/tmp/gnss/', time=GPSTime.from_datetime(datetime.utcnow())):
-  cors_station_positions_dict = load_cors_station_positions(cache_dir)
-  epoch, pos, vel = cors_station_positions_dict[station_id]
-  return ((time - epoch)/SECS_IN_YEAR)*np.array(vel) + np.array(pos)
-
-
-def parse_dgps(station_id, station_obs_file_path, dog, max_distance=100000, required_constellations=[ConstellationId.GPS]):
-  station_pos = get_station_position(station_id, cache_dir=dog.cache_dir)
-  obsdata = RINEXFile(station_obs_file_path)
-  measurements = raw.read_rinex_obs(obsdata)
-
-  # if not all constellations in first 100 epochs bail
-  detected_constellations = set()
-  for m in sum(measurements[:100],[]):
-    detected_constellations.add(get_constellation(m.prn))
-  for constellation in required_constellations:
-    if constellation not in detected_constellations:
-      return None
-
-  proc_measurements = []
-  for measurement in measurements:
-    proc_measurements.append(raw.process_measurements(measurement, dog=dog))
-  # sample at 30s
-  if len(proc_measurements) > 2880:
-    proc_measurements = proc_measurements[::int(len(proc_measurements)/2880)]
-  if len(proc_measurements) != 2880:
-    return None
-
-  station_delays = {}
-  n = len(proc_measurements)
-  for signal in ['C1C', 'C2P']:
-    times = []
-    station_delays[signal] = {}
-    for i, proc_measurement in enumerate(proc_measurements):
-      times.append(proc_measurement[0].recv_time)
-      Fx_pos = opt.pr_residual(proc_measurement, signal=signal)
-      residual, _ = Fx_pos(list(station_pos) + [0,0])
-      residual = -np.array(residual)
-      for j, m in enumerate(proc_measurement):
-        prn = m.prn
-        if prn not in station_delays[signal]:
-          station_delays[signal][prn] = np.nan*np.ones(n)
-        station_delays[signal][prn][i] = residual[j]
-  assert len(times) == n
-
-  # TODO crude way to get dgps station's clock errors,
-  # could this be biased? Only use GPS for convenience.
-  model_delays = {}
-  for prn in station_delays['C1C']:
-    if get_constellation(prn) == ConstellationId.GPS:
-      model_delays[prn] = np.nan*np.zeros(n)
-      for i in range(n):
-        model_delays[prn][i] = dog.get_delay(prn, times[i], station_pos, no_dgps=True)
-  station_clock_errs = np.zeros(n)
-  for i in range(n):
-    station_clock_errs[i] = np.nanmean([(station_delays['C1C'][prn][i] - model_delays[prn][i]) for prn in model_delays])
-
-  # remove clock errors and smooth out signal
-  for prn in station_delays['C1C']:
-    station_delays['C1C'][prn] = mean_filter(station_delays['C1C'][prn] - station_clock_errs)
-  for prn in station_delays['C2P']:
-    station_delays['C2P'][prn] = station_delays['C2P'][prn] - station_clock_errs
-
-  return DGPSDelay(station_id, station_pos, station_delays,
-                   times, max_distance)
-
-
-class DGPSDelay:
-  def __init__(self, station_id, station_pos,
-               station_delays, station_delays_t, max_distance):
-    self.id = station_id
-    self.pos = station_pos
-    self.delays = station_delays
-    self.delays_t = station_delays_t
-    self.max_distance = max_distance
-
-  def get_delay(self, prn, time, signal='C1C'):
-    time_index = int((time - self.delays_t[0])/30)
-    assert abs(self.delays_t[time_index] - time) < 30
-    if prn in self.delays[signal] and np.isfinite(self.delays[signal][prn][time_index]):
-      return self.delays[signal][prn][time_index]
-    return None
-
-  def valid(self, time, recv_pos):
-    return (np.linalg.norm(recv_pos - self.pos) <= self.max_distance and
-            time - self.delays_t[0] > -30 and
-            self.delays_t[-1] - time > -30)
diff --git a/laika/downloader.py b/laika/downloader.py
deleted file mode 100644
index c0bbb584fc..0000000000
--- a/laika/downloader.py
+++ /dev/null
@@ -1,483 +0,0 @@
-import certifi
-import ftplib
-import hatanaka
-import os
-import urllib.request
-import urllib.error
-import pycurl
-import re
-import time
-import socket
-import logging
-
-from datetime import datetime, timedelta
-from urllib.parse import urlparse
-from io import BytesIO
-from ftplib import FTP_TLS
-
-from atomicwrites import atomic_write
-
-from laika.ephemeris import EphemerisType
-from .constants import SECS_IN_HR, SECS_IN_DAY, SECS_IN_WEEK
-from .gps_time import GPSTime, tow_to_datetime
-from .helpers import ConstellationId
-
-dir_path = os.path.dirname(os.path.realpath(__file__))
-
-class DownloadFailed(Exception):
-  pass
-
-
-def retryable(f):
-  """
-  Decorator to allow us to pass multiple URLs from which to download.
-  Automatically retry the request with the next URL on failure
-  """
-  def wrapped(url_bases, *args, **kwargs):
-    if isinstance(url_bases, str):
-      # only one url passed, don't do the retry thing
-      return f(url_bases, *args, **kwargs)
-
-    # not a string, must be a list of url_bases
-    for url_base in url_bases:
-      try:
-        return f(url_base, *args, **kwargs)
-      except DownloadFailed as e:
-        logging.warning(e)
-    # none of them succeeded
-    raise DownloadFailed("Multiple URL failures attempting to pull file(s)")
-  return wrapped
-
-
-def ftp_connect(url):
-  parsed = urlparse(url)
-  assert parsed.scheme == 'ftp'
-  try:
-    domain = parsed.netloc
-    ftp = ftplib.FTP(domain, timeout=10)
-    ftp.login()
-  except (OSError, ftplib.error_perm):
-    raise DownloadFailed("Could not connect/auth to: " + domain)
-  try:
-    ftp.cwd(parsed.path)
-  except ftplib.error_perm:
-    raise DownloadFailed("Permission failure with folder: " + url)
-  return ftp
-
-
-@retryable
-def list_dir(url):
-  parsed = urlparse(url)
-  if parsed.scheme == 'ftp':
-    try:
-      ftp = ftp_connect(url)
-      return ftp.nlst()
-    except ftplib.error_perm:
-      raise DownloadFailed("Permission failure listing folder: " + url)
-  else:
-    # just connect and do simple url parsing
-    listing = https_download_file(url)
-    urls = re.findall(b"<a href=\"([^\"]+)\">", listing)
-    # decode the urls to normal strings. If they are complicated paths, ignore them
-    return [name.decode("latin1") for name in urls if name and b"/" not in name[1:]]
-
-
-def ftp_download_files(url_base, folder_path, cacheDir, filenames):
-  """
-  Like download file, but more of them. Keeps a persistent FTP connection open
-  to be more efficient.
-  """
-  folder_path_abs = os.path.join(cacheDir, folder_path)
-
-  ftp = ftp_connect(url_base + folder_path)
-
-  filepaths = []
-  for filename in filenames:
-    # os.path.join will be dumb if filename has a leading /
-    # if there is a / in the filename, then it's using a different folder
-    filename = filename.lstrip("/")
-    if "/" in filename:
-      continue
-    filepath = os.path.join(folder_path_abs, filename)
-    logging.debug("pulling from", url_base, "to", filepath)
-
-    if not os.path.isfile(filepath):
-      os.makedirs(folder_path_abs, exist_ok=True)
-      try:
-        ftp.retrbinary('RETR ' + filename, open(filepath, 'wb').write)
-      except (ftplib.error_perm):
-        raise DownloadFailed("Could not download file from: " + url_base + folder_path + filename)
-      except (socket.timeout):
-        raise DownloadFailed("Read timed out from: " + url_base + folder_path + filename)
-      filepaths.append(filepath)
-    else:
-      filepaths.append(filepath)
-  return filepaths
-
-
-def http_download_files(url_base, folder_path, cacheDir, filenames):
-  """
-  Similar to ftp_download_files, attempt to download multiple files faster than
-  just downloading them one-by-one.
-  Returns a list of filepaths instead of the raw data
-  """
-  folder_path_abs = os.path.join(cacheDir, folder_path)
-
-  def write_function(disk_path, handle):
-    def do_write(data):
-      open(disk_path, "wb").write(data)
-
-    return do_write
-
-  fetcher = pycurl.CurlMulti()
-  fetcher.setopt(pycurl.M_PIPELINING, 3)
-  fetcher.setopt(pycurl.M_MAX_HOST_CONNECTIONS, 64)
-  fetcher.setopt(pycurl.M_MAX_TOTAL_CONNECTIONS, 64)
-  filepaths = []
-  for filename in filenames:
-    # os.path.join will be dumb if filename has a leading /
-    # if there is a / in the filename, then it's using a different folder
-    filename = filename.lstrip("/")
-    if "/" in filename:
-      continue
-    filepath = os.path.join(folder_path_abs, filename)
-    if not os.path.isfile(filepath):
-      logging.debug("pulling from", url_base, "to", filepath)
-      os.makedirs(folder_path_abs, exist_ok=True)
-      url_path = url_base + folder_path + filename
-      handle = pycurl.Curl()
-      handle.setopt(pycurl.URL, url_path)
-      handle.setopt(pycurl.CONNECTTIMEOUT, 10)
-      handle.setopt(pycurl.WRITEFUNCTION, write_function(filepath, handle))
-      fetcher.add_handle(handle)
-      filepaths.append(filepath)
-
-  requests_processing = len(filepaths)
-  timeout = 10.0  # after 10 seconds of nothing happening, restart
-  deadline = time.time() + timeout
-  while requests_processing and time.time() < deadline:
-    while True:
-      ret, cur_requests_processing = fetcher.perform()
-      if ret != pycurl.E_CALL_MULTI_PERFORM:
-        _, success, failed = fetcher.info_read()
-        break
-    if requests_processing > cur_requests_processing:
-      deadline = time.time() + timeout
-      requests_processing = cur_requests_processing
-
-    if fetcher.select(1) < 0:
-      continue
-
-  # if there are downloads left to be done, repeat, and don't overwrite
-  _, requests_processing = fetcher.perform()
-  if requests_processing > 0:
-    logging.warning("some requests stalled, retrying them")
-    return http_download_files(url_base, folder_path, cacheDir, filenames)
-
-  return filepaths
-
-
-def https_download_file(url):
-  crl = pycurl.Curl()
-  crl.setopt(crl.CAINFO, certifi.where())
-  crl.setopt(crl.URL, url)
-  crl.setopt(crl.FOLLOWLOCATION, True)
-  crl.setopt(crl.SSL_CIPHER_LIST, 'DEFAULT@SECLEVEL=1')
-  crl.setopt(crl.COOKIEJAR, '/tmp/cddis_cookies')
-  crl.setopt(pycurl.CONNECTTIMEOUT, 10)
-  
-  buf = BytesIO()
-  crl.setopt(crl.WRITEDATA, buf)
-  crl.perform()
-  response = crl.getinfo(pycurl.RESPONSE_CODE)
-  crl.close()
-
-  if response != 200:
-    raise DownloadFailed('HTTPS error ' + str(response))
-  return buf.getvalue()
-
-
-def ftp_download_file(url):
-  try:
-    urlf = urllib.request.urlopen(url, timeout=10)
-    data_zipped = urlf.read()
-    urlf.close()
-    return data_zipped
-  except urllib.error.URLError as e:
-    raise DownloadFailed(e)
-
-def ftps_download_file(url):
-  parsed = urlparse(url)
-  try:
-    buf = BytesIO()
-    with FTP_TLS(parsed.hostname) as ftps:
-      ftps.login(user='anonymous')
-      ftps.prot_p()
-      ftps.retrbinary('RETR ' + parsed.path, buf.write)
-    return buf.getvalue()
-  except ftplib.all_errors as e:
-    raise DownloadFailed(e)
-
-@retryable
-def download_files(url_base, folder_path, cacheDir, filenames):
-  parsed = urlparse(url_base)
-  if parsed.scheme == 'ftp':
-    return ftp_download_files(url_base, folder_path, cacheDir, filenames)
-  else:
-    return http_download_files(url_base, folder_path, cacheDir, filenames)
-
-
-@retryable
-def download_file(url_base, folder_path, filename_zipped):
-  url = url_base + folder_path + filename_zipped
-  logging.debug('Downloading ' + url)
-  if url.startswith('https://'):
-    return https_download_file(url)
-  elif url.startswith('ftp://'):
-    return ftp_download_file(url)
-  elif url.startswith('sftp://'):
-    return ftps_download_file(url)
-  raise NotImplementedError('Did not find supported url scheme')
-
-
-def download_and_cache_file_return_first_success(url_bases, folder_and_file_names, cache_dir, compression='', overwrite=False, raise_error=False):
-  last_error = None
-  for folder_path, filename in folder_and_file_names:
-    try:
-      file = download_and_cache_file(url_bases, folder_path, cache_dir, filename, compression, overwrite)
-      return file
-    except DownloadFailed as e:
-      last_error = e
-
-  if last_error and raise_error:
-    raise last_error
-
-
-def download_and_cache_file(url_base, folder_path: str, cache_dir: str, filename: str, compression='', overwrite=False):
-  filename_zipped = filename + compression
-  folder_path_abs = os.path.join(cache_dir, folder_path)
-  filepath = str(hatanaka.get_decompressed_path(os.path.join(folder_path_abs, filename)))
-
-  filepath_attempt = filepath + '.attempt_time'
-
-  if os.path.exists(filepath_attempt):
-    with open(filepath_attempt, 'r') as rf:
-      last_attempt_time = float(rf.read())
-    if time.time() - last_attempt_time < SECS_IN_HR:
-      raise DownloadFailed(f"Too soon to try downloading {folder_path + filename_zipped} from {url_base} again since last attempt")
-  if not os.path.isfile(filepath) or overwrite:
-    try:
-      data_zipped = download_file(url_base, folder_path, filename_zipped)
-    except (DownloadFailed, pycurl.error, socket.timeout):
-      unix_time = time.time()
-      os.makedirs(folder_path_abs, exist_ok=True)
-      with atomic_write(filepath_attempt, mode='w', overwrite=True) as wf:
-        wf.write(str(unix_time))
-      raise DownloadFailed(f"Could not download {folder_path + filename_zipped} from {url_base} ")
-
-    os.makedirs(folder_path_abs, exist_ok=True)
-    ephem_bytes = hatanaka.decompress(data_zipped)
-    try:
-      with atomic_write(filepath, mode='wb', overwrite=overwrite) as f:
-        f.write(ephem_bytes)
-    except FileExistsError:
-      # Only happens when same file is downloaded in parallel by another process.
-      pass
-  return filepath
-
-
-# Currently, only GPS and Glonass are supported for daily and hourly data.
-CONSTELLATION_NASA_CHAR = {ConstellationId.GPS: 'n', ConstellationId.GLONASS: 'g'}
-
-
-def download_nav(time: GPSTime, cache_dir, constellation: ConstellationId):
-  t = time.as_datetime()
-  try:
-    if constellation not in CONSTELLATION_NASA_CHAR:
-      return None
-    c = CONSTELLATION_NASA_CHAR[constellation]
-    if GPSTime.from_datetime(datetime.utcnow()) - time > SECS_IN_DAY:
-      url_bases = (
-        'https://github.com/commaai/gnss-data/raw/master/gnss/data/daily/',
-        'sftp://gdc.cddis.eosdis.nasa.gov/gnss/data/daily/',
-      )
-      filename = t.strftime(f"brdc%j0.%y{c}")
-      folder_path = t.strftime(f'%Y/%j/%y{c}/')
-      compression = '.gz' if folder_path >= '2020/335/' else '.Z'
-      return download_and_cache_file(url_bases, folder_path, cache_dir+'daily_nav/', filename, compression)
-    else:
-      url_bases = (
-        'https://github.com/commaai/gnss-data-hourly/raw/master/',
-        'sftp://gdc.cddis.eosdis.nasa.gov/gnss/data/hourly/',
-      )
-      times = [t, (t - timedelta(hours=1))]
-      folder_and_filenames = [(t.strftime('%Y/%j/'), t.strftime(f"hour%j0.%y{c}")) for t in times]
-      compression = '.gz' if folder_and_filenames[0][0] >= '2020/336/' else '.Z'
-      # always overwrite as this file is appended
-      return download_and_cache_file_return_first_success(url_bases,
-        folder_and_filenames, cache_dir+'hourly_nav/', compression, overwrite=True)
-  except DownloadFailed:
-    pass
-
-
-def download_orbits_gps_cod0(time, cache_dir, ephem_types):
-  url_bases = (
-    'https://github.com/commaai/gnss-data/raw/master/gnss/products/',
-    'sftp://gdc.cddis.eosdis.nasa.gov/gnss/products/',
-  )
-
-  if EphemerisType.ULTRA_RAPID_ORBIT not in ephem_types:
-    # TODO: raise error here
-    return None
-
-  tm = tow_to_datetime(time.tow, time.week).timetuple()
-  doy = str(tm.tm_yday).zfill(3)
-  filename = f"COD0OPSULT_{tm.tm_year}{doy}0000_02D_05M_ORB.SP3"
-  # TODO: add hour management
-
-  folder_path = "%i/" % time.week
-  folder_file_names = [(folder_path, filename)]
-  return download_and_cache_file_return_first_success(url_bases, folder_file_names, cache_dir+'cddis_products/', compression='.gz')
-
-
-def download_orbits_gps(time, cache_dir, ephem_types):
-  url_bases = (
-    'https://github.com/commaai/gnss-data/raw/master/gnss/products/',
-    'sftp://gdc.cddis.eosdis.nasa.gov/gnss/products/',
-    'ftp://igs.ign.fr/pub/igs/products/',
-  )
-  folder_path = "%i/" % time.week
-  filenames = []
-  time_str = "%i%i" % (time.week, time.day)
-  # Download filenames in order of quality. Final -> Rapid -> Ultra-Rapid(newest first)
-  if EphemerisType.FINAL_ORBIT in ephem_types and GPSTime.from_datetime(datetime.utcnow()) - time > 3 * SECS_IN_WEEK:
-    filenames.append(f"igs{time_str}.sp3")
-  if EphemerisType.RAPID_ORBIT in ephem_types:
-    filenames.append(f"igr{time_str}.sp3")
-  if EphemerisType.ULTRA_RAPID_ORBIT in ephem_types:
-    filenames.extend([f"igu{time_str}_18.sp3",
-                      f"igu{time_str}_12.sp3",
-                      f"igu{time_str}_06.sp3",
-                      f"igu{time_str}_00.sp3"])
-  folder_file_names = [(folder_path, filename) for filename in filenames]
-  ret = download_and_cache_file_return_first_success(url_bases, folder_file_names, cache_dir+'cddis_products/', compression='.Z')
-  if ret is not None:
-    return ret
-
-  # fallback to COD0 Ultra Rapid Orbits
-  return download_orbits_gps_cod0(time, cache_dir, ephem_types)
-
-
-def download_prediction_orbits_russia_src(gps_time, cache_dir):
-  # Download single file that contains Ultra_Rapid predictions for GPS, GLONASS and other constellations
-  t = gps_time.as_datetime()
-  # Files exist starting at 29-01-2022
-  if t < datetime(2022, 1, 29):
-    return None
-  url_bases = 'https://github.com/commaai/gnss-data-alt/raw/master/MCC/PRODUCTS/'
-  folder_path = t.strftime('%y%j/ultra/')
-  file_prefix = "Stark_1D_" + t.strftime('%y%m%d')
-
-  # Predictions are 24H so previous day can also be used.
-  prev_day = (t - timedelta(days=1))
-  file_prefix_prev = "Stark_1D_" + prev_day.strftime('%y%m%d')
-  folder_path_prev = prev_day.strftime('%y%j/ultra/')
-
-  current_day = GPSTime.from_datetime(datetime(t.year, t.month, t.day))
-  # Ultra-Orbit is published in gnss-data-alt every 10th minute past the 5,11,17,23 hour.
-  # Predictions published are delayed by around 10 hours.
-  # Download latest file that includes gps_time with 20 minutes margin.:
-  if gps_time > current_day + 23.5 * SECS_IN_HR:
-    prev_day, current_day = [], [6, 12]
-  elif gps_time > current_day + 17.5 * SECS_IN_HR:
-    prev_day, current_day = [], [0, 6]
-  elif gps_time > current_day + 11.5 * SECS_IN_HR:
-    prev_day, current_day = [18], [0]
-  elif gps_time > current_day + 5.5 * SECS_IN_HR:
-    prev_day, current_day = [12, 18], []
-  else:
-    prev_day, current_day = [6, 12], []
-  # Example: Stark_1D_22060100.sp3
-  folder_and_file_names = [(folder_path, file_prefix + f"{h:02}.sp3") for h in reversed(current_day)] + \
-                          [(folder_path_prev, file_prefix_prev + f"{h:02}.sp3") for h in reversed(prev_day)]
-  return download_and_cache_file_return_first_success(url_bases, folder_and_file_names, cache_dir+'russian_products/', raise_error=True)
-
-
-def download_orbits_russia_src(time, cache_dir, ephem_types):
-  # Orbits from russian source. Contains GPS, GLONASS, GALILEO, BEIDOU
-  url_bases = (
-    'https://github.com/commaai/gnss-data-alt/raw/master/MCC/PRODUCTS/',
-    'ftp://ftp.glonass-iac.ru/MCC/PRODUCTS/',
-  )
-  t = time.as_datetime()
-  folder_paths = []
-  current_gps_time = GPSTime.from_datetime(datetime.utcnow())
-  filename = "Sta%i%i.sp3" % (time.week, time.day)
-  if EphemerisType.FINAL_ORBIT in ephem_types and current_gps_time - time > 2 * SECS_IN_WEEK:
-    folder_paths.append(t.strftime('%y%j/final/'))
-  if EphemerisType.RAPID_ORBIT in ephem_types:
-    folder_paths.append(t.strftime('%y%j/rapid/'))
-  if EphemerisType.ULTRA_RAPID_ORBIT in ephem_types:
-    folder_paths.append(t.strftime('%y%j/ultra/'))
-  folder_file_names = [(folder_path, filename) for folder_path in folder_paths]
-  return download_and_cache_file_return_first_success(url_bases, folder_file_names, cache_dir+'russian_products/')
-
-
-def download_ionex(time, cache_dir):
-  t = time.as_datetime()
-  url_bases = (
-    'https://github.com/commaai/gnss-data/raw/master/gnss/products/ionex/',
-    'sftp://gdc.cddis.eosdis.nasa.gov/gnss/products/ionex/',
-    'ftp://igs.ensg.ign.fr/pub/igs/products/ionosphere/',
-    'ftp://gssc.esa.int/gnss/products/ionex/',
-  )
-  folder_path = t.strftime('%Y/%j/')
-  filenames = [t.strftime("codg%j0.%yi"), t.strftime("c1pg%j0.%yi"), t.strftime("c2pg%j0.%yi")]
-
-  folder_file_names = [(folder_path, f) for f in filenames]
-  return download_and_cache_file_return_first_success(url_bases, folder_file_names, cache_dir+'ionex/', compression='.Z', raise_error=True)
-
-
-def download_dcb(time, cache_dir):
-  filenames = []
-  folder_paths = []
-  url_bases = (
-    'https://github.com/commaai/gnss-data/raw/master/gnss/products/bias/',
-    'sftp://gdc.cddis.eosdis.nasa.gov/gnss/products/bias/',
-    'ftp://igs.ign.fr/pub/igs/products/mgex/dcb/',
-  )
-  # seem to be a lot of data missing, so try many days
-  for time_step in [time - i * SECS_IN_DAY for i in range(14)]:
-    t = time_step.as_datetime()
-    folder_paths.append(t.strftime('%Y/'))
-    filenames.append(t.strftime("CAS0MGXRAP_%Y%j0000_01D_01D_DCB.BSX"))
-
-  return download_and_cache_file_return_first_success(url_bases, list(zip(folder_paths, filenames)), cache_dir+'dcb/', compression='.gz', raise_error=True)
-
-
-def download_cors_coords(cache_dir):
-  cache_subdir = cache_dir + 'cors_coord/'
-  url_bases = (
-    'https://geodesy.noaa.gov/corsdata/coord/coord_14/',
-    'https://alt.ngs.noaa.gov/corsdata/coord/coord_14/',
-  )
-  file_names = list_dir(url_bases)
-  file_names = [file_name for file_name in file_names if file_name.endswith('coord.txt')]
-  filepaths = download_files(url_bases, '', cache_subdir, file_names)
-  return filepaths
-
-
-def download_cors_station(time, station_name, cache_dir):
-  t = time.as_datetime()
-  folder_path = t.strftime('%Y/%j/') + station_name + '/'
-  filename = station_name + t.strftime("%j0.%yd")
-  url_bases = (
-    'https://geodesy.noaa.gov/corsdata/rinex/',
-    'https://alt.ngs.noaa.gov/corsdata/rinex/',
-  )
-  try:
-    filepath = download_and_cache_file(url_bases, folder_path, cache_dir+'cors_obs/', filename, compression='.gz')
-    return filepath
-  except DownloadFailed:
-    logging.warning("File not downloaded, check availability on server.")
-    return None
diff --git a/laika/ephemeris.capnp b/laika/ephemeris.capnp
deleted file mode 100644
index 5404286392..0000000000
--- a/laika/ephemeris.capnp
+++ /dev/null
@@ -1,106 +0,0 @@
-@0xb3ca6d2462778bb1;
-
-struct Ephemeris {
-  # This is according to the rinex (2?) format
-  svId @0 :UInt16;
-  year @1 :UInt16;
-  month @2 :UInt16;
-  day @3 :UInt16;
-  hour @4 :UInt16;
-  minute @5 :UInt16;
-  second @6 :Float32;
-  af0 @7 :Float64;
-  af1 @8 :Float64;
-  af2 @9 :Float64;
-
-  iode @10 :Float64;
-  crs @11 :Float64;
-  deltaN @12 :Float64;
-  m0 @13 :Float64;
-
-  cuc @14 :Float64;
-  ecc @15 :Float64;
-  cus @16 :Float64;
-  a @17 :Float64; # note that this is not the root!!
-
-  toe @18 :Float64;
-  cic @19 :Float64;
-  omega0 @20 :Float64;
-  cis @21 :Float64;
-
-  i0 @22 :Float64;
-  crc @23 :Float64;
-  omega @24 :Float64;
-  omegaDot @25 :Float64;
-
-  iDot @26 :Float64;
-  codesL2 @27 :Float64;
-  gpsWeekDEPRECATED @28 :Float64;
-  l2 @29 :Float64;
-
-  svAcc @30 :Float64;
-  svHealth @31 :Float64;
-  tgd @32 :Float64;
-  iodc @33 :Float64;
-
-  transmissionTime @34 :Float64;
-  fitInterval @35 :Float64;
-
-  toc @36 :Float64;
-
-  ionoCoeffsValid @37 :Bool;
-  ionoAlpha @38 :List(Float64);
-  ionoBeta @39 :List(Float64);
-
-  towCount @40 :UInt32;
-  toeWeek @41 :UInt16;
-  tocWeek @42 :UInt16;
-}
-
-struct GlonassEphemeris {
-  svId @0 :UInt16;
-  year @1 :UInt16;
-  dayInYear @2 :UInt16;
-  hour @3 :UInt16;
-  minute @4 :UInt16;
-  second @5 :Float32;
-
-  x @6 :Float64;
-  xVel @7 :Float64;
-  xAccel @8 :Float64;
-  y @9 :Float64;
-  yVel @10 :Float64;
-  yAccel @11 :Float64;
-  z @12 :Float64;
-  zVel @13 :Float64;
-  zAccel @14 :Float64;
-
-  svType @15 :UInt8;
-  svURA @16 :Float32;
-  age @17 :UInt8;
-
-  svHealth @18 :UInt8;
-  tkDEPRECATED @19 :UInt16;
-  tb @20 :UInt16;
-
-  tauN @21 :Float64;
-  deltaTauN @22 :Float64;
-  gammaN @23 :Float64;
-
-  p1 @24 :UInt8;
-  p2 @25 :UInt8;
-  p3 @26 :UInt8;
-  p4 @27 :UInt8;
-
-  freqNumDEPRECATED @28 :UInt32;
-
-  n4 @29 :UInt8;
-  nt @30 :UInt16;
-  freqNum @31 :Int16;
-  tkSeconds @32 :UInt32;
-}
-
-struct EphemerisCache {
-  gpsEphemerides @0 :List(Ephemeris);
-  glonassEphemerides @1 :List(GlonassEphemeris);
-}
\ No newline at end of file
diff --git a/laika/ephemeris.py b/laika/ephemeris.py
deleted file mode 100644
index fa8dbdf8d6..0000000000
--- a/laika/ephemeris.py
+++ /dev/null
@@ -1,498 +0,0 @@
-import warnings
-from abc import ABC, abstractmethod
-from collections import defaultdict
-from enum import IntEnum
-from typing import Dict, List, Optional
-
-import numpy as np
-import numpy.polynomial.polynomial as poly
-from datetime import datetime
-from math import sin, cos, sqrt, fabs, atan2
-
-from .gps_time import GPSTime, utc_to_gpst
-from .constants import SPEED_OF_LIGHT, SECS_IN_MIN, SECS_IN_HR, SECS_IN_DAY, \
-                       EARTH_ROTATION_RATE, EARTH_GM
-from .helpers import get_constellation, get_prn_from_nmea_id
-
-import capnp
-import os
-capnp.remove_import_hook()
-capnp_path = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "ephemeris.capnp"))
-ephemeris_structs = capnp.load(capnp_path)
-
-
-def read4(f, rinex_ver):
-  line = f.readline()[:-1]
-  if rinex_ver == 2:
-    line = ' ' + line  # Shift 1 char to the right
-  line = line.replace('D', 'E')  # Handle bizarro float format
-  return float(line[4:23]), float(line[23:42]), float(line[42:61]), float(line[61:80])
-
-
-class EphemerisType(IntEnum):
-  # Matches the enum in log.capnp
-  NAV = 0
-  FINAL_ORBIT = 1
-  RAPID_ORBIT = 2
-  ULTRA_RAPID_ORBIT = 3
-  QCOM_POLY = 4
-
-  @staticmethod
-  def all_orbits():
-    return EphemerisType.FINAL_ORBIT, EphemerisType.RAPID_ORBIT, EphemerisType.ULTRA_RAPID_ORBIT
-
-  @classmethod
-  def from_file_name(cls, file_name: str):
-    if "/final" in file_name or "/igs" in file_name:
-      return EphemerisType.FINAL_ORBIT
-    if "/rapid" in file_name or "/igr" in file_name:
-      return EphemerisType.RAPID_ORBIT
-    if "/ultra" in file_name or "/igu" in file_name or "COD0OPSULT" in file_name:
-      return EphemerisType.ULTRA_RAPID_ORBIT
-    raise RuntimeError(f"Ephemeris type not found in filename: {file_name}")
-
-
-class Ephemeris(ABC):
-
-  def __init__(self, prn: str, epoch: GPSTime, eph_type: EphemerisType, healthy: bool, max_time_diff: float,
-               file_epoch: Optional[GPSTime] = None, file_name=None):
-    self.prn = prn
-    self.epoch = epoch
-    self.eph_type = eph_type
-    self.healthy = healthy
-    self.max_time_diff = max_time_diff
-    self.file_epoch = file_epoch
-    self.file_name = file_name
-    self.file_source = '' if file_name is None else file_name.split('/')[-1][:3]  # File source for the ephemeris (e.g. igu, igr, Sta)
-
-  def valid(self, time):
-    return abs(time - self.epoch) <= self.max_time_diff
-
-  def __repr__(self):
-    time = self.epoch.as_datetime().strftime('%Y-%m-%dT%H:%M:%S.%f')
-    return f"<{self.__class__.__name__} from {self.prn} at {time}>"
-
-  def get_sat_info(self, time: GPSTime):
-    """
-    Returns: (pos, vel, clock_err, clock_rate_err, ephemeris)
-    """
-    if not self.healthy:
-      return None
-    return list(self._get_sat_info(time)) + [self]
-
-  @abstractmethod
-  def _get_sat_info(self, time):
-    pass
-
-
-class GLONASSEphemeris(Ephemeris):
-  def __init__(self, data, file_name=None):
-    self.epoch = GPSTime.from_glonass(data.n4, data.nt, data.tb*15*SECS_IN_MIN)
-    super().__init__('R%02i' % data.svId, self.epoch, EphemerisType.NAV, data.svHealth==0, max_time_diff=25*SECS_IN_MIN, file_name=file_name)
-    self.data = data
-    self.epoch =  GPSTime.from_glonass(data.n4, data.nt, data.tb*15 * SECS_IN_MIN)
-    self.channel = data.freqNum
-
-  def _get_sat_info(self, time: GPSTime):
-    # see the russian doc for this:
-    # http://gauss.gge.unb.ca/GLONASS.ICD.pdf
-
-    eph = self.data
-    tdiff = time - self.epoch
-    # Clock correction (except for general relativity which is applied later)
-    clock_err = -eph.tauN + tdiff * eph.gammaN
-    clock_rate_err = eph.gammaN
-
-    def glonass_diff_eq(state, acc):
-      J2 = 1.0826257e-3
-      mu = 3.9860044e14
-      omega = 7.292115e-5
-      ae = 6378136.0
-      r = np.sqrt(state[0]**2 + state[1]**2 + state[2]**2)
-      ders = np.zeros(6)
-      if r**2 < 0:
-        return ders
-      a = 1.5 * J2 * mu * (ae**2)/ (r**5)
-      b = 5 * (state[2]**2) / (r**2)
-      c = -mu/(r**3) - a*(1-b)
-
-      ders[0:3] = state[3:6]
-      ders[3] = (c + omega**2)*state[0] + 2*omega*state[4] + acc[0]
-      ders[4] = (c + omega**2)*state[1] - 2*omega*state[3] + acc[1]
-      ders[5] = (c - 2*a)*state[2] + acc[2]
-      return ders
-
-    init_state = np.empty(6)
-    init_state[0] = eph.x
-    init_state[1] = eph.y
-    init_state[2] = eph.z
-    init_state[3] = eph.xVel
-    init_state[4] = eph.yVel
-    init_state[5] = eph.zVel
-    init_state = 1000*init_state
-    acc = 1000*np.array([eph.xAccel, eph.yAccel, eph.zAccel])
-    state = init_state
-    tstep = 90
-    if tdiff < 0:
-      tt = -tstep
-    elif tdiff > 0:
-      tt = tstep
-    while abs(tdiff) > 1e-9:
-      if abs(tdiff) < tstep:
-        tt = tdiff
-      k1 = glonass_diff_eq(state, acc)
-      k2 = glonass_diff_eq(state + k1*tt/2, -acc)
-      k3 = glonass_diff_eq(state + k2*tt/2, -acc)
-      k4 = glonass_diff_eq(state + k3*tt, -acc)
-      state += (k1 + 2*k2 + 2*k3 + k4)*tt/6.0
-      tdiff -= tt
-
-    pos = state[0:3]
-    vel = state[3:6]
-    return pos, vel, clock_err, clock_rate_err
-
-
-class PolyEphemeris(Ephemeris):
-  def __init__(self, prn: str, data, epoch: GPSTime, ephem_type: EphemerisType,
-               file_epoch: Optional[GPSTime] = None, file_name: Optional[str] = None, healthy=True, tgd=0,
-               max_time_diff: int=SECS_IN_HR):
-    super().__init__(prn, epoch, ephem_type, healthy, max_time_diff=max_time_diff, file_epoch=file_epoch, file_name=file_name)
-    self.data = data
-    self.tgd = tgd
-
-  def _get_sat_info(self, time: GPSTime):
-    dt = time - self.data['t0']
-    deg = self.data['deg']
-    deg_t = self.data['deg_t']
-    indices = np.arange(deg+1)[:,np.newaxis]
-    sat_pos = np.sum((dt**indices)*self.data['xyz'], axis=0)
-    indices = indices[1:]
-    sat_vel = np.sum(indices*(dt**(indices-1)*self.data['xyz'][1:]), axis=0)
-    time_err = sum((dt**p)*self.data['clock'][deg_t-p] for p in range(deg_t+1))
-    time_err_rate = sum(p*(dt**(p-1))*self.data['clock'][deg_t-p] for p in range(1,deg_t+1))
-    time_err_with_rel = time_err - 2*np.inner(sat_pos, sat_vel)/SPEED_OF_LIGHT**2
-    return sat_pos, sat_vel, time_err_with_rel, time_err_rate
-
-
-class GPSEphemeris(Ephemeris):
-  def __init__(self, data, file_name=None):
-    self.toe = GPSTime(data.toeWeek, data.toe)
-    self.toc = GPSTime(data.tocWeek, data.toc)
-    self.epoch = self.toc
-
-    super().__init__('G%02i' % data.svId, self.epoch, EphemerisType.NAV, data.svHealth==0, max_time_diff=2*SECS_IN_HR, file_name=file_name)
-    self.max_time_diff_tgd = SECS_IN_DAY
-    self.data = data
-    self.sqrta = np.sqrt(data.a)
-
-  def get_tgd(self):
-    return self.datatgd
-
-  def _get_sat_info(self, time: GPSTime):
-    eph = self.data
-    tdiff = time - self.toc  # Time of clock
-    clock_err = eph.af0 + tdiff * (eph.af1 + tdiff * eph.af2)
-    clock_rate_err = eph.af1 + 2 * tdiff * eph.af2\
-
-    # Orbit propagation
-    tdiff = time - self.toe  # Time of ephemeris (might be different from time of clock)
-
-    # Calculate position per IS-GPS-200D p 97 Table 20-IV
-    a = self.sqrta * self.sqrta # [m] Semi-major axis
-    ma_dot = sqrt(EARTH_GM / (a * a * a)) + eph.deltaN  # [rad/sec] Corrected mean motion
-    ma = eph.m0 + ma_dot * tdiff  # [rad] Corrected mean anomaly
-
-    # Iteratively solve for the Eccentric Anomaly (from Keith Alter and David Johnston)
-    ea = ma  # Starting value for E
-
-    ea_old = 2222
-    while fabs(ea - ea_old) > 1.0E-14:
-      ea_old = ea
-      tempd1 = 1.0 - eph.ecc * cos(ea_old)
-      ea = ea + (ma - ea_old + eph.ecc * sin(ea_old)) / tempd1
-    ea_dot = ma_dot / tempd1
-
-    # Relativistic correction term
-    einstein = -4.442807633E-10 * eph.ecc * self.sqrta * sin(ea)
-
-    # Begin calc for True Anomaly and Argument of Latitude
-    tempd2 = sqrt(1.0 - eph.ecc * eph.ecc)
-    # [rad] Argument of Latitude = True Anomaly + Argument of Perigee
-    al = atan2(tempd2 * sin(ea), cos(ea) - eph.ecc) + eph.omega
-    al_dot = tempd2 * ea_dot / tempd1
-
-    # Calculate corrected argument of latitude based on position
-    cal = al + eph.cus * sin(2.0 * al) + eph.cuc * cos(2.0 * al)
-    cal_dot = al_dot * (1.0 + 2.0 * (eph.cus * cos(2.0 * al) -
-                                     eph.cuc * sin(2.0 * al)))
-
-    # Calculate corrected radius based on argument of latitude
-    r = a * tempd1 + eph.crc * cos(2.0 * al) + eph.crs * sin(2.0 * al)
-    r_dot = (a * eph.ecc * sin(ea) * ea_dot +
-             2.0 * al_dot * (eph.crs * cos(2.0 * al) -
-                             eph.crc * sin(2.0 * al)))
-
-    # Calculate inclination based on argument of latitude
-    inc = (eph.i0 + eph.iDot * tdiff +
-           eph.cic * cos(2.0 * al) +
-           eph.cis * sin(2.0 * al))
-    inc_dot = (eph.iDot +
-               2.0 * al_dot * (eph.cis * cos(2.0 * al) -
-                               eph.cic * sin(2.0 * al)))
-
-    # Calculate position and velocity in orbital plane
-    x = r * cos(cal)
-    y = r * sin(cal)
-    x_dot = r_dot * cos(cal) - y * cal_dot
-    y_dot = r_dot * sin(cal) + x * cal_dot
-
-    # Corrected longitude of ascending node
-    om_dot = eph.omegaDot - EARTH_ROTATION_RATE
-    om = eph.omega0 + tdiff * om_dot - EARTH_ROTATION_RATE * self.toe.tow
-
-    # Compute the satellite's position in Earth-Centered Earth-Fixed coordinates
-    pos = np.empty(3)
-    pos[0] = x * cos(om) - y * cos(inc) * sin(om)
-    pos[1] = x * sin(om) + y * cos(inc) * cos(om)
-    pos[2] = y * sin(inc)
-
-    tempd3 = y_dot * cos(inc) - y * sin(inc) * inc_dot
-
-    # Compute the satellite's velocity in Earth-Centered Earth-Fixed coordinates
-    vel = np.empty(3)
-    vel[0] = -om_dot * pos[1] + x_dot * cos(om) - tempd3 * sin(om)
-    vel[1] = om_dot * pos[0] + x_dot * sin(om) + tempd3 * cos(om)
-    vel[2] = y * cos(inc) * inc_dot + y_dot * sin(inc)
-
-    clock_err += einstein
-
-    return pos, vel, clock_err, clock_rate_err
-
-
-def parse_sp3_orbits(file_names, supported_constellations, skip_until_epoch: Optional[GPSTime] = None) -> Dict[str, List[PolyEphemeris]]:
-  if skip_until_epoch is None:
-    skip_until_epoch = GPSTime(0, 0)
-  data: Dict[str, List] = {}
-  for file_name in file_names:
-    if file_name is None:
-      continue
-    with open(file_name) as f:
-      ephem_type = EphemerisType.from_file_name(file_name)
-      file_epoch = None
-      while True:
-        line = f.readline()[:-1]
-        if not line:
-          break
-        # epoch header
-        if line[0:2] == '* ':
-          year = int(line[3:7])
-          month = int(line[8:10])
-          day = int(line[11:13])
-          hour = int(line[14:16])
-          minute = int(line[17:19])
-          second = int(float(line[20:31]))
-          epoch = GPSTime.from_datetime(datetime(year, month, day, hour, minute, second))
-          if file_epoch is None:
-            file_epoch = epoch
-        # pos line
-        elif line[0] == 'P':
-          # Skipping data can reduce the time significantly when parsing the ephemeris
-          if epoch < skip_until_epoch:
-            continue
-          prn = line[1:4].replace(' ', '0')
-          # In old SP3 files vehicle ID doesn't contain constellation
-          # identifier. We assume that constellation is GPS when missing.
-          if prn[0] == '0':
-            prn = 'G' + prn[1:]
-          if get_constellation(prn) not in supported_constellations:
-            continue
-          if prn not in data:
-            data[prn] = []
-          #TODO this is a crappy way to deal with overlapping ultra rapid
-          if len(data[prn]) < 1 or epoch - data[prn][-1][1] > 0:
-            parsed = [(ephem_type, file_epoch, file_name),
-                      epoch,
-                      1e3 * float(line[4:18]),
-                      1e3 * float(line[18:32]),
-                      1e3 * float(line[32:46]),
-                      1e-6 * float(line[46:60])]
-            if (np.array(parsed[2:]) != 0).all():
-              data[prn].append(parsed)
-  ephems = {}
-  for prn in data:
-    ephems[prn] = read_prn_data(data, prn)
-  return ephems
-
-
-def read_prn_data(data, prn, deg=16, deg_t=1):
-  np_data_prn = np.array(data[prn], dtype=object)
-  # Currently, don't even bother with satellites that have unhealthy times
-  if len(np_data_prn) == 0 or (np_data_prn[:, 5] > .99).any():
-    return []
-  ephems = []
-  for i in range(len(np_data_prn) - deg):
-    epoch_index = i + deg // 2
-    epoch = np_data_prn[epoch_index][1]
-    measurements = np_data_prn[i:i + deg + 1, 1:5]
-
-    times = (measurements[:, 0] - epoch).astype(float)
-    if not (np.diff(times) != 900).any() and not (np.diff(times) != 300).any():
-      continue
-
-    poly_data = {}
-    poly_data['t0'] = epoch
-    with warnings.catch_warnings():
-      warnings.simplefilter("ignore")  # Ignores: UserWarning: The value of the smallest subnormal for <class 'numpy.float64'> type is zero.
-      poly_data['xyz'] = poly.polyfit(times, measurements[:, 1:].astype(float), deg)
-    poly_data['clock'] = [(np_data_prn[epoch_index + 1][5] - np_data_prn[epoch_index - 1][5]) / 1800, np_data_prn[epoch_index][5]]
-    poly_data['deg'] = deg
-    poly_data['deg_t'] = deg_t
-    # It can happen that a mix of orbit ephemeris types are used in the polyfit.
-    ephem_type, file_epoch, file_name = np_data_prn[epoch_index][0]
-    ephems.append(PolyEphemeris(prn, poly_data, epoch, ephem_type, file_epoch, file_name, healthy=True))
-  return ephems
-
-
-def parse_rinex_nav_msg_gps(file_name):
-  ephems = defaultdict(list)
-  got_header = False
-  rinex_ver = None
-  #ion_alpha = None
-  #ion_beta = None
-  f = open(file_name)
-  while True:
-    line = f.readline()[:-1]
-    if not line:
-      break
-    if not got_header:
-      if rinex_ver is None:
-        if line[60:80] != "RINEX VERSION / TYPE":
-          raise RuntimeError("Doesn't appear to be a RINEX file")
-        rinex_ver = int(float(line[0:9]))
-        if line[20] != "N":
-          raise RuntimeError("Doesn't appear to be a Navigation Message file")
-      #if line[60:69] == "ION ALPHA":
-      #  line = line.replace('D', 'E')  # Handle bizarro float format
-      #  ion_alpha= [float(line[3:14]), float(line[15:26]), float(line[27:38]), float(line[39:50])]
-      #if line[60:68] == "ION BETA":
-      #  line = line.replace('D', 'E')  # Handle bizarro float format
-      #  ion_beta= [float(line[3:14]), float(line[15:26]), float(line[27:38]), float(line[39:50])]
-      if line[60:73] == "END OF HEADER":
-        #ion = ion_alpha + ion_beta
-        got_header = True
-      continue
-    if rinex_ver == 3:
-      if line[0] != 'G':
-        continue
-    if rinex_ver == 3:
-      sv_id = int(line[1:3])
-      epoch = GPSTime.from_datetime(datetime.strptime(line[4:23], "%y %m %d %H %M %S"))
-    elif rinex_ver == 2:
-      sv_id = int(line[0:2])
-      # 2000 year is in RINEX file as 0, but Python requires two digit year: 00
-      epoch_str = line[3:20]
-      if epoch_str[0] == ' ':
-        epoch_str = '0' + epoch_str[1:]
-      epoch = GPSTime.from_datetime(datetime.strptime(epoch_str, "%y %m %d %H %M %S"))
-      line = ' ' + line  # Shift 1 char to the right
-
-    line = line.replace('D', 'E')  # Handle bizarro float format
-    e = {'svId': sv_id}
-    # TODO are TOC and TOE the same?
-    e['toc'] = epoch.tow
-    e['tocWeek'] = epoch.week
-    e['af0'] = float(line[23:42])
-    e['af1'] = float(line[42:61])
-    e['af2'] = float(line[61:80])
-
-    e['iode'], e['crs'], e['deltaN'], e['m0'] = read4(f, rinex_ver)
-    e['cuc'], e['ecc'], e['cus'], sqrta = read4(f, rinex_ver)
-    e['a'] = sqrta ** 2
-    e['toe'], e['cic'], e['omega0'], e['cis'] = read4(f, rinex_ver)
-    e['i0'], e['crc'], e['omega'], e['omegaDot'] = read4(f, rinex_ver)
-    e['iDot'], e['codesL2'], e['toeWeek'], l2_pflag = read4(f, rinex_ver)
-    e['svAcc'], e['svHealth'], e['tgd'], e['iodc'] = read4(f, rinex_ver)
-    f.readline()  # Discard last row
-
-    data_struct = ephemeris_structs.Ephemeris.new_message(**e)
-
-    ephem = GPSEphemeris(data_struct, file_name=file_name)
-    ephems[ephem.prn].append(ephem)
-  f.close()
-  return ephems
-
-
-def parse_rinex_nav_msg_glonass(file_name):
-  ephems = defaultdict(list)
-  f = open(file_name)
-  got_header = False
-  rinex_ver = None
-  while True:
-    line = f.readline()[:-1]
-    if not line:
-      break
-    if not got_header:
-      if rinex_ver is None:
-        if line[60:80] != "RINEX VERSION / TYPE":
-          raise RuntimeError("Doesn't appear to be a RINEX file")
-        rinex_ver = int(float(line[0:9]))
-        if line[20] != "G":
-          raise RuntimeError("Doesn't appear to be a Navigation Message file")
-      if line[60:73] == "END OF HEADER":
-        got_header = True
-      continue
-    if rinex_ver == 3:
-      sv_id = int(line[1:3])
-
-      epoch = utc_to_gpst(GPSTime.from_datetime(datetime.strptime(line[4:23], "%y %m %d %H %M %S")))
-    elif rinex_ver == 2:
-      sv_id = int(line[0:2])
-      epoch = utc_to_gpst(GPSTime.from_datetime(datetime.strptime(line[3:20], "%y %m %d %H %M %S")))
-      line = ' ' + line  # Shift 1 char to the right
-
-    line = line.replace('D', 'E')  # Handle bizarro float format
-    e = {'svId': sv_id}
-    e['n4'], e['nt'], toe_seconds = epoch.as_glonass()
-    tb = toe_seconds / (15 * SECS_IN_MIN)
-
-  
-    e['tb'] = tb
-
-    e['tauN'] = -float(line[23:42])
-    e['gammaN'] = float(line[42:61])
-    e['tkSeconds'] = float(line[61:80])
-
-    e['x'], e['xVel'], e['xAccel'], e['svHealth'] = read4(f, rinex_ver)
-    e['y'], e['yVel'], e['yAccel'], e['freqNum'] = read4(f, rinex_ver)
-    e['z'], e['zVel'], e['zAccel'], e['age'] = read4(f, rinex_ver)
-    
-    # TODO unclear why glonass sometimes has nav messages 3s after correct one
-    if abs(tb - int(tb)) > 1e-3:
-      continue
-
-    
-    data_struct = ephemeris_structs.GlonassEphemeris.new_message(**e)
-    ephem = GLONASSEphemeris(data_struct, file_name=file_name)
-
-    ephems[ephem.prn].append(ephem)
-  f.close()
-  return ephems
-
-
-def parse_qcom_ephem(qcom_poly):
-  svId = qcom_poly.svId
-  prn = get_prn_from_nmea_id(svId)
-  epoch = GPSTime(qcom_poly.gpsWeek, qcom_poly.gpsTow)
-
-  data = qcom_poly
-  poly_data = {}
-  poly_data['t0'] = epoch
-  poly_data['xyz'] = np.array([
-                      [data.xyz0[0], data.xyzN[0], data.xyzN[1], data.xyzN[2]],
-                      [data.xyz0[1], data.xyzN[3], data.xyzN[4], data.xyzN[5]],
-                      [data.xyz0[2], data.xyzN[6], data.xyzN[7], data.xyzN[8]] ]).T
-
-  poly_data['clock'] = [1e-3*data.other[3], 1e-3*data.other[2], 1e-3*data.other[1], 1e-3*data.other[0]]
-  poly_data['deg'] = 3
-  poly_data['deg_t'] = 3
-  return PolyEphemeris(prn, poly_data, epoch, ephem_type=EphemerisType.QCOM_POLY, max_time_diff=300, file_name='qcom')
diff --git a/laika/gps_time.py b/laika/gps_time.py
deleted file mode 100644
index 293ff45482..0000000000
--- a/laika/gps_time.py
+++ /dev/null
@@ -1,203 +0,0 @@
-import datetime
-
-
-def datetime_to_tow(t):
-    """
-    Convert a Python datetime object to GPS Week and Time Of Week.
-    Does *not* convert from UTC to GPST.
-    Fractional seconds are supported.
-
-    Parameters
-    ----------
-    t : datetime
-      A time to be converted, on the GPST timescale.
-    mod1024 : bool, optional
-      If True (default), the week number will be output in 10-bit form.
-
-    Returns
-    -------
-    week, tow : tuple (int, float)
-      The GPS week number and time-of-week.
-    """
-    # DateTime to GPS week and TOW
-    wk_ref = datetime.datetime(2014, 2, 16, 0, 0, 0, 0, None)
-    refwk = 1780
-    wk = (t - wk_ref).days // 7 + refwk
-    tow = ((t - wk_ref) - datetime.timedelta((wk - refwk) * 7.0)).total_seconds()
-    return wk, tow
-
-
-def tow_to_datetime(tow, week):
-    """
-    Convert a GPS Week and Time Of Week to Python datetime object.
-    Does *not* convert from GPST to UTC.
-    Fractional seconds are supported.
-
-    Parameters
-    ----------
-    tow : time of week in seconds
-
-    weeks : gps week
-
-
-    Returns
-    -------
-    t : datetime
-      Python datetime
-    """
-    #  GPS week and TOW to DateTime
-    t = datetime.datetime(1980, 1, 6, 0, 0, 0, 0, None)
-    t += datetime.timedelta(seconds=tow)
-    t += datetime.timedelta(weeks=week)
-    return t
-
-
-def get_leap_seconds(time):
-  # TODO use library for this
-  if time <= GPSTime.from_datetime(datetime.datetime(2006, 1, 1)):
-    raise ValueError("Don't know how many leap seconds to use before 2006")
-  elif time <= GPSTime.from_datetime(datetime.datetime(2009, 1, 1)):
-    return 14
-  elif time <= GPSTime.from_datetime(datetime.datetime(2012, 7, 1)):
-    return 15
-  elif time <= GPSTime.from_datetime(datetime.datetime(2015, 7, 1)):
-    return 16
-  elif time <= GPSTime.from_datetime(datetime.datetime(2017, 1, 1)):
-    return 17
-  else:
-    return 18
-
-
-def gpst_to_utc(t_gpst):
-    t_utc = t_gpst - get_leap_seconds(t_gpst)
-    if utc_to_gpst(t_utc) - t_gpst != 0:
-      return t_utc + 1
-    else:
-      return t_utc
-
-
-def utc_to_gpst(t_utc):
-    t_gpst = t_utc + get_leap_seconds(t_utc)
-    return t_gpst
-
-
-class GPSTime:
-  """
-  GPS time class to add and subtract [week, tow]
-  """
-  def __init__(self, week, tow):
-    self.week = week
-    self.tow = tow
-    self.seconds_in_week = 604800
-
-  @classmethod
-  def from_datetime(cls, datetime):
-    week, tow = datetime_to_tow(datetime)
-    return cls(week, tow)
-
-  @classmethod
-  def from_glonass(cls, cycle, days, tow):
-    # https://en.wikipedia.org/wiki/GLONASS
-    # Day number (1 to 1461) within a four-year interval
-    # starting on 1 January of the last leap year
-    t = datetime.datetime(1992, 1, 1, 0, 0, 0, 0, None)
-    t += datetime.timedelta(days=cycle*(365*4+1)+(days-1))
-    # according to Moscow decree time.
-    t -= datetime.timedelta(hours=3)
-    t += datetime.timedelta(seconds=tow)
-    ret = cls.from_datetime(t)
-    return utc_to_gpst(ret)
-
-  @classmethod
-  def from_meas(cls, meas):
-    return cls(meas[1], meas[2])
-
-  def __sub__(self, other):
-    if isinstance(other, type(self)):
-      return (self.week - other.week)*self.seconds_in_week + self.tow - other.tow
-    elif isinstance(other, float) or isinstance(other, int):
-      new_week = self.week
-      new_tow = self.tow - other
-      while new_tow < 0:
-        new_tow += self.seconds_in_week
-        new_week -= 1
-      return GPSTime(new_week, new_tow)
-    raise NotImplementedError(f"subtracting {other} from {self}")
-
-  def __add__(self, other):
-    if isinstance(other, float) or isinstance(other, int):
-      new_week = self.week
-      new_tow = self.tow + other
-      while new_tow >= self.seconds_in_week:
-        new_tow -= self.seconds_in_week
-        new_week += 1
-      return GPSTime(new_week, new_tow)
-    raise NotImplementedError(f"adding {other} from {self}")
-
-  def __lt__(self, other):
-    return self - other < 0
-
-  def __gt__(self, other):
-    return self - other > 0
-
-  def __le__(self, other):
-    return self - other <= 0
-
-  def __ge__(self, other):
-    return self - other >= 0
-
-  def __eq__(self, other):
-    return self - other == 0
-
-  def as_datetime(self):
-    return tow_to_datetime(self.tow, self.week)
-
-  def as_glonass(self):
-    time_utc = gpst_to_utc(self)
-    datetime_utc = time_utc.as_datetime()
-    datetime_glonass = datetime_utc + datetime.timedelta(hours=3)
-
-    year = datetime_glonass.year
-    cycle = (year - 1992) // 4
-    days = (datetime_glonass - datetime.datetime(1992 + cycle*4, 1, 1)).days + 1
-    tod = (datetime_glonass - datetime_glonass.replace(hour=0, minute=0, second=0, microsecond=0)).total_seconds()
-    return cycle, days, tod
-
-  def as_unix_timestamp(self):
-    return (gpst_to_utc(self).as_datetime() - datetime.datetime(1970, 1, 1)).total_seconds()
-
-  @property
-  def day(self):
-    return int(self.tow/(24*3600))
-
-  def __repr__(self):
-    return f"GPSTime(week={self.week}, tow={self.tow})"
-
-
-class TimeSyncer:
-  """
-  Converts logmonotime to gps_time and vice versa
-  """
-  def __init__(self, mono_time, gps_time):
-    self.ref_mono_time = mono_time
-    self.ref_gps_time = gps_time
-
-  @classmethod
-  def from_datetime(cls, datetime):
-    week, tow = datetime_to_tow(datetime)
-    return cls(week, tow)
-
-  @classmethod
-  def from_logs(cls, raw_qcom_measurement_report, clocks):
-    #TODO
-    #return cls(week, mono_time, gps_time)
-    return None
-
-  def mono2gps(self, mono_time):
-    return self.ref_gps_time + mono_time - self.ref_mono_time
-
-  def gps2mono(self, gps_time):
-    return gps_time - self.ref_gps_time + self.ref_mono_time
-
-  def __str__(self):
-      return f"Reference mono time: {self.ref_mono_time} \n  Reference gps time: {self.ref_gps_time}"
diff --git a/laika/helpers.py b/laika/helpers.py
deleted file mode 100644
index 0647081561..0000000000
--- a/laika/helpers.py
+++ /dev/null
@@ -1,221 +0,0 @@
-from enum import IntEnum
-from typing import Dict
-
-import numpy as np
-from .lib.coordinates import LocalCoord
-
-
-class ConstellationId(IntEnum):
-  # Int values match Ublox gnssid version 8
-  GPS = 0
-  SBAS = 1
-  GALILEO = 2
-  BEIDOU = 3
-  IMES = 4
-  QZNSS = 5
-  GLONASS = 6
-  # Not supported by Ublox:
-  IRNSS = 7
-
-  def to_rinex_char(self) -> str:
-    # returns single character id
-    return RINEX_CONSTELLATION_TO_ID[self]
-
-  @classmethod
-  def from_rinex_char(cls, c: str):
-    if c in RINEX_ID_TO_CONSTELLATION:
-      return RINEX_ID_TO_CONSTELLATION[c]
-    else:
-      raise ValueError("Unknown rinex constellation id: ", c)
-
-  @classmethod
-  def from_qcom_source(cls, report_source: int):
-    if report_source == 0:
-      return ConstellationId.GPS
-    if report_source == 1:
-      return ConstellationId.GLONASS
-    if report_source == 2:
-      return ConstellationId.BEIDOU
-    if report_source == 6:
-      return ConstellationId.SBAS
-    raise NotImplementedError('Only GPS (0), GLONASS (1), BEIDOU (2) and SBAS (6) are supported from qcom, not:', {report_source})
-
-
-# From https://gpsd.gitlab.io/gpsd/NMEA.html#_satellite_ids
-# NmeaId is the unique 3 digits id for every satellite globally. (Example: 001, 201)
-# SvId is the 2 digits satellite id that is unique within a constellation. (Get the unique satellite with the constellation id. Examples: G01, R01)
-CONSTELLATION_TO_NMEA_RANGES = {
-  # NmeaId ranges for each constellation with its svId offset.
-  # constellation: [(start, end, svIdOffset)]
-  # svId = nmeaId + offset
-  ConstellationId.GPS: [(1, 32, 0)],  # svId [1,32]
-  ConstellationId.SBAS: [(33, 64, -32), (120, 158, -87)],  # svId [1,71]
-  ConstellationId.GLONASS: [(65, 96, -64)],  # svId [1,31]
-  ConstellationId.IMES: [(173, 182, -172)],  # svId [1,9]
-  ConstellationId.QZNSS: [(193, 200, -192)],  # svId [1,28]  # todo should be QZSS
-  ConstellationId.BEIDOU: [(201, 235, -200), (401, 437, -365)],  # svId 1-72
-  ConstellationId.GALILEO: [(301, 336, -300)]  # svId 1-36
-}
-#
-# # Source: RINEX 3.04
-RINEX_CONSTELLATION_TO_ID: Dict[ConstellationId, str] = {
-  ConstellationId.GPS: 'G',
-  ConstellationId.GLONASS: 'R',
-  ConstellationId.SBAS: 'S',
-  ConstellationId.GALILEO: 'E',
-  ConstellationId.BEIDOU: 'C',
-  ConstellationId.QZNSS: 'J',
-  ConstellationId.IRNSS: 'I'
-}
-
-# Make above dictionary bidirectional map:
-# Now you can ask for constellation using:
-# >>> RINEX_CONSTELLATION_IDENTIFIERS['R']
-#     "GLONASS"
-RINEX_ID_TO_CONSTELLATION: Dict[str, ConstellationId] = {id: con for con, id in RINEX_CONSTELLATION_TO_ID.items()}
-
-
-def get_el_az(pos, sat_pos):
-  converter = LocalCoord.from_ecef(pos)
-  sat_ned = converter.ecef2ned(sat_pos)
-  sat_range = np.linalg.norm(sat_ned)
-
-  el = np.arcsin(-sat_ned[2] / sat_range)  # pylint: disable=unsubscriptable-object
-  az = np.arctan2(sat_ned[1], sat_ned[0])  # pylint: disable=unsubscriptable-object
-  return el, az
-
-
-def get_closest(time, candidates, recv_pos=None):
-  if recv_pos is None:
-    # Takes a list of object that have an epoch(GPSTime) value
-    # and return the one that is closest the given time (GPSTime)
-    return min(candidates, key=lambda candidate: abs(time - candidate.epoch), default=None)
-
-  return min(
-    (candidate for candidate in candidates if candidate.valid(time, recv_pos)),
-    key=lambda candidate: np.linalg.norm(recv_pos - candidate.pos),
-    default=None,
-  )
-
-def get_constellation(prn: str):
-  identifier = prn[0]
-  return ConstellationId.from_rinex_char(identifier)
-
-def get_sv_id(prn: str):
-  return int(prn[1:])
-
-def get_constellation_and_sv_id(nmea_id):
-  for c, ranges in CONSTELLATION_TO_NMEA_RANGES.items():
-    for (start, end, sv_id_offset) in ranges:
-      if start <= nmea_id <= end:
-        sv_id = nmea_id + sv_id_offset
-        return c, sv_id
-
-  raise ValueError(f"constellation not found for nmeaid {nmea_id}")
-
-
-def get_prn_from_nmea_id(nmea_id: int):
-  c_id, sv_id = get_constellation_and_sv_id(nmea_id)
-  return "%s%02d" % (c_id.to_rinex_char(), sv_id)
-
-
-def get_nmea_id_from_prn(prn: str):
-  constellation = get_constellation(prn)
-  sv_id = int(prn[1:])  # satellite id
-  return get_nmea_id_from_constellation_and_svid(constellation, sv_id)
-
-
-def get_nmea_id_from_constellation_and_svid(constellation: ConstellationId, sv_id: int):
-  ranges = CONSTELLATION_TO_NMEA_RANGES[constellation]
-  for (start, end, sv_id_offset) in ranges:
-    new_nmea_id = sv_id - sv_id_offset
-    if start <= new_nmea_id <= end:
-      return new_nmea_id
-
-  raise ValueError(f"NMEA ID not found for constellation {constellation.name} with satellite id {sv_id}")
-
-
-def rinex3_obs_from_rinex2_obs(observable):
-  if observable == 'P2':
-    return 'C2P'
-  if len(observable) == 2:
-    return observable + 'C'
-  raise NotImplementedError("Don't know this: " + observable)
-
-
-class TimeRangeHolder:
-  '''Class to support test if date is in any of the multiple, sparse ranges'''
-
-  def __init__(self):
-    # Sorted list
-    self._ranges = []
-
-  def _previous_and_contains_index(self, time):
-    prev = None
-    current = None
-
-    for idx, (start, end) in enumerate(self._ranges):
-      # Time may be in next range
-      if time > end:
-        continue
-
-      # Time isn't in any next range
-      if time < start:
-        prev = idx - 1
-        current = None
-      # Time is in current range
-      else:
-        prev = idx - 1
-        current = idx
-      break
-
-    # Break in last loop
-    if prev is None:
-      prev = len(self._ranges) - 1
-
-    return prev, current
-
-  def add(self, start_time, end_time):
-    prev_start, current_start = self._previous_and_contains_index(start_time)
-    _, current_end = self._previous_and_contains_index(end_time)
-
-    # Merge ranges
-    if current_start is not None and current_end is not None:
-      # If ranges are different then merge
-      if current_start != current_end:
-        new_start, _ = self._ranges[current_start]
-        _, new_end = self._ranges[current_end]
-        new_range = (new_start, new_end)
-        # Required reversed order to correct remove
-        del self._ranges[current_end]
-        del self._ranges[current_start]
-        self._ranges.insert(current_start, new_range)
-    # Extend range - left
-    elif current_start is not None:
-      new_start, _ = self._ranges[current_start]
-      new_range = (new_start, end_time)
-      del self._ranges[current_start]
-      self._ranges.insert(current_start, new_range)
-    # Extend range - right
-    elif current_end is not None:
-      _, new_end = self._ranges[current_end]
-      new_range = (start_time, new_end)
-      del self._ranges[current_end]
-      self._ranges.insert(prev_start + 1, new_range)
-    # Create new range
-    else:
-      new_range = (start_time, end_time)
-      self._ranges.insert(prev_start + 1, new_range)
-
-  def __contains__(self, time):
-    for start, end in self._ranges:
-      # Time may be in next range
-      if time > end:
-        continue
-
-      # Time isn't in any next range
-      if time < start:
-        return False
-      # Time is in current range
-      return True
-    return False
diff --git a/laika/iono.py b/laika/iono.py
deleted file mode 100644
index 0558fdbeac..0000000000
--- a/laika/iono.py
+++ /dev/null
@@ -1,255 +0,0 @@
-import datetime as dt
-import numpy as np
-import re
-from math import cos, sin, pi, floor
-from .constants import SECS_IN_MIN, SECS_IN_HR, EARTH_RADIUS
-from .lib.coordinates import LocalCoord
-from .gps_time import GPSTime
-
-# Altitude of Ionospheric-pierce-point
-IPP_ALT = 6821000
-
-def get_alpha_beta(rcv_pos, el):
-    geocentric_alt = np.linalg.norm(rcv_pos)
-    alpha = np.pi/2 + el
-    beta = np.arcsin(geocentric_alt*np.sin(alpha)/IPP_ALT)
-    return alpha, beta
-
-def get_slant_delay(rcv_pos, az, el, sat_pos, time, freq, vertical_delay):
-    alpha, beta = get_alpha_beta(rcv_pos, el)
-    slant_delay = vertical_delay * ((1 - ((EARTH_RADIUS * np.sin(beta)) /
-                                          (EARTH_RADIUS + 3.5e5))**2)**(-0.5))
-    return slant_delay
-
-def closest_in_list(lst, val, num=2):
-  """
-    Returns two (`num` in general) closest values of `val` in list `lst`
-    """
-  idxs = sorted(lst, key=lambda x: abs(x - val))[:num]
-  return sorted(list(lst).index(x) for x in idxs)
-
-
-def get_header_line(headr, proprty):
-    """
-    :param headr: the header of the RINEX-file
-    :param proprty: string-like property to search for (e.g. 'delta-utc')
-    :return: the string of the ``headr`` containing ``property``
-    """
-    pattern = re.compile(proprty, re.IGNORECASE)
-    for d in headr:
-        if pattern.search(d):
-            return d
-
-
-def get_header_body(file_path):
-    """
-    Opens `file_path`, reads file and returns header and body
-    separated with "END OF HEADER"
-    :param file_path: path to RINEX-like file
-    :return: header, body (arrays of lines)
-    """
-    with open(file_path) as fd:
-        data = fd.readlines()
-        for j, d in enumerate(data):
-            if "END OF HEADER" in d:
-                header_end = j
-                break
-    return data[:header_end], data[header_end + 1:]
-
-
-def get_int_from_header(hdr, seq):
-    """
-    Returns the first int from the line that contains `seq` of lines `hdr`.
-    In fact, _header_ here may not be header of RINEX/IONEX, just some set of lines.
-    """
-    return int(get_header_line(hdr, seq).split()[0])
-
-def compute_grid_lats_lons(data):
-  grid = np.array([], dtype='uint16')
-  lats = np.array([])
-  for j, line in enumerate(data[1:]):
-    if "LAT" in line:
-      lat, lon1, lon2, dlon, h = (float(line[x:x + 6]) for x in range(2, 32, 6))
-      lats = np.append(lats, lat)
-      row_length = (lon2 - lon1) / dlon + 1  # total number of values of longitudes
-      next_lines_with_numbers = int(np.ceil(row_length / 16))
-      elems_in_row = [
-        min(16, int(row_length - i * 16)) for i in range(next_lines_with_numbers)
-      ]
-      row = np.array([], dtype='int16')
-      for i, elem in enumerate(elems_in_row):
-        row = np.append(
-          row,
-          np.array(
-            [int(data[j + 2 + i][5 * x:5 * x + 5]) for x in range(elem)],
-            dtype='int16',
-          ),
-        )
-      if len(grid) > 0:
-        grid = np.vstack((grid, row))
-      else:
-        grid = np.append(grid, row)
-  lons = np.linspace(lon1, lon2, int(row_length))
-  return (grid, lats, lons)
-
-
-class IonexMap:
-  def __init__(self, exp, data1, data2):
-    self.exp = exp
-    self.t1 = GPSTime.from_datetime(dt.datetime(*[int(d) for d in data1[0].split()[:6]]))
-    self.t2 = GPSTime.from_datetime(dt.datetime(*[int(d) for d in data2[0].split()[:6]]))
-    assert self.t2 - self.t1 == SECS_IN_HR
-    assert len(data1) == len(data2)
-
-    self.max_time_diff = SECS_IN_MIN*30
-    self.epoch = self.t1 + self.max_time_diff
-
-    self.grid_TEC1, self.lats, self.lons = compute_grid_lats_lons(data1)
-    self.grid_TEC2, self.lats, self.lons = compute_grid_lats_lons(data2)
-
-  def valid(self, time):
-    return abs(time - self.epoch) <= self.max_time_diff
-
-  @staticmethod
-  def find_nearest(lst, val):
-    return (np.abs(lst - val)).argmin()
-
-  def get_TEC(self, pos, time):
-    """
-        Returns TEC in a position `pos`  of ionosphere
-        :param pos: (lat, lon) [deg, deg]
-        :return:
-        """
-    if pos[0] in self.lats and pos[1] in self.lons:
-      lat = self.find_nearest(self.lats, pos[0])
-      lon = self.find_nearest(self.lons, pos[1])
-      E = self.grid_TEC1[lat][lon] + self.grid_TEC2[lat][lon]
-      return E
-    lat_idxs = closest_in_list(self.lats, pos[0])
-    lon_idxs = closest_in_list(self.lons, pos[1])
-    lat0, lat1 = self.lats[lat_idxs[0]], self.lats[lat_idxs[1]]
-    lon0, lon1 = self.lons[lon_idxs[0]], self.lons[lon_idxs[1]]
-    dlat = lat1 - lat0
-    dlon = lon1 - lon0
-    p = float(pos[0] - lat0) / dlat
-    q = float(pos[1] - lon0) / dlon
-
-    (E00, E10), (E01, E11) = self.grid_TEC1[lat_idxs[0]:lat_idxs[1] + 1, lon_idxs[0]:lon_idxs[1] + 1]
-    TEC_1 = ((1 - p) * (1 - q) * E00 + p * (1 - q) * E01 + (1 - p) * q * E10 + p * q * E11)
-    (E00, E10), (E01, E11) = self.grid_TEC2[lat_idxs[0]:lat_idxs[1] + 1, lon_idxs[0]:lon_idxs[1] + 1]
-    TEC_2 = ((1 - p) * (1 - q) * E00 + p * (1 - q) * E01 + (1 - p) * q * E10 + p * q * E11)
-
-    return (1 - (time - self.t1)/SECS_IN_HR)*TEC_1 + ((time - self.t1)/SECS_IN_HR)*TEC_2
-
-  def get_delay(self, rcv_pos, az, el, sat_pos, time, freq):
-    # To get a delay from a TEC map, we need to calculate
-    # the ionospheric pierce point, geometry described here
-    # https://en.wikipedia.org/wiki/Ionospheric_pierce_point
-    alpha, beta = get_alpha_beta(rcv_pos, el)
-    conv = LocalCoord.from_ecef(rcv_pos)
-    gamma = np.pi - alpha - beta
-    geocentric_alt = np.linalg.norm(rcv_pos)
-    ipp_dist = geocentric_alt*np.sin(gamma)/np.sin(beta)
-    ipp_ned = conv.ecef2ned(sat_pos)*(ipp_dist)/np.linalg.norm(sat_pos)
-    ipp_geo = conv.ned2geodetic(ipp_ned)
-    factor = 40.30E16 / (freq**2) * 10**(self.exp)
-    vertical_delay = self.get_TEC(ipp_geo, time) * factor
-    slant_delay = get_slant_delay(rcv_pos, az, el, sat_pos, time, freq, vertical_delay)
-    return slant_delay
-
-  @staticmethod
-  def round_to_grid(number, base):
-    return int(base * round(float(number) / base))
-
-
-def parse_ionex(ionex_file):
-  """
-    :param ionex_file: path to the IONEX file
-    :return: TEC interpolation function `f( (lat,lon), datetime )`
-    """
-  header, body = get_header_body(ionex_file)
-
-  exponent = get_int_from_header(header, "EXPONENT")
-  maps_count = get_int_from_header(header, "MAPS IN FILE")
-  # =============
-  # Separate maps
-  # =============
-  map_start_idx = []
-  map_end_idx = []
-
-  for j, line in enumerate(body):
-    if "START OF TEC MAP" in line:
-      map_start_idx += [j]
-    elif "END OF TEC MAP" in line:
-      map_end_idx += [j]
-  if maps_count != len(map_start_idx):
-    raise LookupError("Parsing error: the number of maps in the header "
-                      "is not equal to the number of maps in the body.")
-  if len(map_start_idx) != len(map_end_idx):
-    raise IndexError("Starts end ends numbers are not equal.")
-  map_dates = []
-  for i in range(maps_count):
-    date_components = body[map_start_idx[i] + 1].split()[:6]
-    map_dates.append(dt.datetime(*[int(d) for d in date_components]))
-
-  maps = []
-  iono_map = iono_map_prev = None
-  for m in range(maps_count):
-    iono_map_prev = iono_map
-    iono_map = body[map_start_idx[m] + 1:map_end_idx[m]]
-    if iono_map and iono_map_prev:
-      maps += [IonexMap(exponent, iono_map_prev, iono_map)]
-  return maps
-
-
-def klobuchar(pos, az, el, time, iono_coeffs):
-  """
-    Details are taken from [5]: IS-GPS-200H, Fig. 20-4
-    Note: result is referred to the GPS L₁ frequency;
-    if the user is operating on the GPS L₂ frequency, the correction term must
-    be multiplied by γ = f₂²/f₁¹ = 0.6071850227694382
-    :param pos: [lat, lon, alt] in radians and meters
-    """
-
-  tow = time.tow
-  if pos[2] < -1E3 or el < 0:
-    return 0.0
-  if len(iono_coeffs) < 8:
-    return None
-
-  # earth centered angle (semi-circle)
-  psi = 0.0137 / (el / pi + 0.11) - 0.022
-
-  # subionospheric latitude/longitude (semi-circle)
-  phi = pos[0] / pi + psi * cos(az)
-  if phi > 0.416:
-    phi = 0.416
-  elif phi < -0.416:
-    phi = -0.416
-  lam = pos[1] / pi + psi * sin(az) / cos(phi * pi)
-
-  # geomagnetic latitude (semi-circle) */
-  phi += 0.064 * cos((lam - 1.617) * pi)
-
-  # local time (s)
-  tt = 43200.0 * lam + tow
-  tt -= floor(tt / 86400.0) * 86400.0  # 0<=tt<86400
-
-  # slant factor
-  f = 1.0 + 16.0 * pow(0.53 - el / pi, 3.0)
-
-  # ionospheric delay
-  amp = iono_coeffs[0] + phi * (iono_coeffs[1] + phi *
-                                (iono_coeffs[2] + phi * iono_coeffs[3]))
-  per = iono_coeffs[4] + phi * (iono_coeffs[5] + phi *
-                                (iono_coeffs[6] + phi * iono_coeffs[7]))
-  if amp < 0.0:
-    amp = 0.
-  if per < 72000.0:
-    per = 72000.0
-  x = 2.0 * pi * (tt - 50400.0) / per
-
-  mul = 5E-9
-  if abs(x) < 1.57:
-    mul = (5E-9 + amp * (1.0 + x * x * (-0.5 + x * x / 24.0)))
-  return 2.99792458E8 * f * mul
diff --git a/laika/lib/coordinates.py b/laika/lib/coordinates.py
deleted file mode 100644
index c6e584f834..0000000000
--- a/laika/lib/coordinates.py
+++ /dev/null
@@ -1,106 +0,0 @@
-import numpy as np
-"""
-Coordinate transformation module. All methods accept arrays as input
-with each row as a position.
-"""
-
-
-a = 6378137
-b = 6356752.3142
-esq = 6.69437999014 * 0.001
-e1sq = 6.73949674228 * 0.001
-
-
-def geodetic2ecef(geodetic, radians=False):
-  geodetic = np.array(geodetic)
-  input_shape = geodetic.shape
-  geodetic = np.atleast_2d(geodetic)
-
-  ratio = 1.0 if radians else (np.pi / 180.0)
-  lat = ratio*geodetic[:,0]
-  lon = ratio*geodetic[:,1]
-  alt = geodetic[:,2]
-
-  xi = np.sqrt(1 - esq * np.sin(lat)**2)
-  x = (a / xi + alt) * np.cos(lat) * np.cos(lon)
-  y = (a / xi + alt) * np.cos(lat) * np.sin(lon)
-  z = (a / xi * (1 - esq) + alt) * np.sin(lat)
-  ecef = np.array([x, y, z]).T
-  return ecef.reshape(input_shape)
-
-
-def ecef2geodetic(ecef, radians=False):
-  """
-  Convert ECEF coordinates to geodetic using ferrari's method
-  """
-  # Save shape and export column
-  ecef = np.atleast_1d(ecef)
-  input_shape = ecef.shape
-  ecef = np.atleast_2d(ecef)
-  x, y, z = ecef[:, 0], ecef[:, 1], ecef[:, 2]
-
-  ratio = 1.0 if radians else (180.0 / np.pi)
-
-  # Conver from ECEF to geodetic using Ferrari's methods
-  # https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#Ferrari.27s_solution
-  r = np.sqrt(x * x + y * y)
-  Esq = a * a - b * b
-  F = 54 * b * b * z * z
-  G = r * r + (1 - esq) * z * z - esq * Esq
-  C = (esq * esq * F * r * r) / (pow(G, 3))
-  S = np.cbrt(1 + C + np.sqrt(C * C + 2 * C))
-  P = F / (3 * pow((S + 1 / S + 1), 2) * G * G)
-  Q = np.sqrt(1 + 2 * esq * esq * P)
-  r_0 = -(P * esq * r) / (1 + Q) + np.sqrt(0.5 * a * a*(1 + 1.0 / Q) -
-                                           P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r)
-  U = np.sqrt(pow((r - esq * r_0), 2) + z * z)
-  V = np.sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z)
-  Z_0 = b * b * z / (a * V)
-  h = U * (1 - b * b / (a * V))
-  lat = ratio*np.arctan((z + e1sq * Z_0) / r)
-  lon = ratio*np.arctan2(y, x)
-
-  # stack the new columns and return to the original shape
-  geodetic = np.column_stack((lat, lon, h))
-  return geodetic.reshape(input_shape)
-
-class LocalCoord:
-  """
-   Allows conversions to local frames. In this case NED.
-   That is: North East Down from the start position in
-   meters.
-  """
-  def __init__(self, init_geodetic, init_ecef):
-    self.init_ecef = init_ecef
-    lat, lon, _ = (np.pi/180)*np.array(init_geodetic)
-    self.ned2ecef_matrix = np.array([[-np.sin(lat)*np.cos(lon), -np.sin(lon), -np.cos(lat)*np.cos(lon)],
-                                     [-np.sin(lat)*np.sin(lon), np.cos(lon), -np.cos(lat)*np.sin(lon)],
-                                     [np.cos(lat), 0, -np.sin(lat)]])
-    self.ecef2ned_matrix = self.ned2ecef_matrix.T
-
-  @classmethod
-  def from_geodetic(cls, init_geodetic):
-    init_ecef = geodetic2ecef(init_geodetic)
-    return LocalCoord(init_geodetic, init_ecef)
-
-  @classmethod
-  def from_ecef(cls, init_ecef):
-    init_geodetic = ecef2geodetic(init_ecef)
-    return LocalCoord(init_geodetic, init_ecef)
-
-  def ecef2ned(self, ecef):
-    ecef = np.array(ecef)
-    return np.dot(self.ecef2ned_matrix, (ecef - self.init_ecef).T).T
-
-  def ned2ecef(self, ned):
-    ned = np.array(ned)
-    # Transpose so that init_ecef will broadcast correctly for 1d or 2d ned.
-    return (np.dot(self.ned2ecef_matrix, ned.T).T + self.init_ecef)
-
-  def geodetic2ned(self, geodetic):
-    ecef = geodetic2ecef(geodetic)
-    return self.ecef2ned(ecef)
-
-  def ned2geodetic(self, ned):
-    ecef = self.ned2ecef(ned)
-    return ecef2geodetic(ecef)
diff --git a/laika/lib/orientation.py b/laika/lib/orientation.py
deleted file mode 100644
index 5f8d5d2b32..0000000000
--- a/laika/lib/orientation.py
+++ /dev/null
@@ -1,291 +0,0 @@
-import numpy as np
-from numpy import dot, inner, array, linalg
-from .coordinates import LocalCoord
-
-
-'''
-Vectorized functions that transform between
-rotation matrices, euler angles and quaternions.
-All support lists, array or array of arrays as inputs.
-Supports both x2y and y_from_x format (y_from_x preferred!).
-'''
-
-def euler2quat(eulers):
-  eulers = array(eulers)
-  if len(eulers.shape) > 1:
-    output_shape = (-1,4)
-  else:
-    output_shape = (4,)
-  eulers = np.atleast_2d(eulers)
-  gamma, theta, psi = eulers[:,0],  eulers[:,1],  eulers[:,2]
-
-  cos_half_gamma = np.cos(gamma / 2)
-  cos_half_theta = np.cos(theta / 2)
-  cos_half_psi = np.cos(psi / 2)
-  sin_half_gamma = np.sin(gamma / 2)
-  sin_half_theta = np.sin(theta / 2)
-  sin_half_psi = np.sin(psi / 2)
-  q0 = cos_half_gamma * cos_half_theta * cos_half_psi + sin_half_gamma * sin_half_theta * sin_half_psi
-  q1 = sin_half_gamma * cos_half_theta * cos_half_psi - cos_half_gamma * sin_half_theta * sin_half_psi
-  q2 = cos_half_gamma * sin_half_theta * cos_half_psi + sin_half_gamma * cos_half_theta * sin_half_psi
-  q3 = cos_half_gamma * cos_half_theta * sin_half_psi - sin_half_gamma * sin_half_theta * cos_half_psi
-
-  quats = array([q0, q1, q2, q3]).T
-  for i in range(len(quats)):
-    if quats[i,0] < 0:
-      quats[i] = -quats[i]
-  return quats.reshape(output_shape)
-
-
-def quat2euler(quats):
-  quats = array(quats)
-  if len(quats.shape) > 1:
-    output_shape = (-1,3)
-  else:
-    output_shape = (3,)
-  quats = np.atleast_2d(quats)
-  q0, q1, q2, q3 = quats[:,0], quats[:,1], quats[:,2], quats[:,3]
-
-  gamma = np.arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2))
-  theta = np.arcsin(2 * (q0 * q2 - q3 * q1))
-  psi = np.arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2))
-
-  eulers = array([gamma, theta, psi]).T
-  return eulers.reshape(output_shape)
-
-
-def quat2rot(quats):
-  quats = array(quats)
-  input_shape = quats.shape
-  quats = np.atleast_2d(quats)
-  Rs = np.zeros((quats.shape[0], 3, 3))
-  q0 = quats[:, 0]
-  q1 = quats[:, 1]
-  q2 = quats[:, 2]
-  q3 = quats[:, 3]
-  Rs[:, 0, 0] = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3
-  Rs[:, 0, 1] = 2 * (q1 * q2 - q0 * q3)
-  Rs[:, 0, 2] = 2 * (q0 * q2 + q1 * q3)
-  Rs[:, 1, 0] = 2 * (q1 * q2 + q0 * q3)
-  Rs[:, 1, 1] = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3
-  Rs[:, 1, 2] = 2 * (q2 * q3 - q0 * q1)
-  Rs[:, 2, 0] = 2 * (q1 * q3 - q0 * q2)
-  Rs[:, 2, 1] = 2 * (q0 * q1 + q2 * q3)
-  Rs[:, 2, 2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3
-
-  if len(input_shape) < 2:
-    return Rs[0]
-  return Rs
-
-
-def rot2quat(rots):
-  input_shape = rots.shape
-  if len(input_shape) < 3:
-    rots = array([rots])
-  K3 = np.empty((len(rots), 4, 4))
-  K3[:, 0, 0] = (rots[:, 0, 0] - rots[:, 1, 1] - rots[:, 2, 2]) / 3.0
-  K3[:, 0, 1] = (rots[:, 1, 0] + rots[:, 0, 1]) / 3.0
-  K3[:, 0, 2] = (rots[:, 2, 0] + rots[:, 0, 2]) / 3.0
-  K3[:, 0, 3] = (rots[:, 1, 2] - rots[:, 2, 1]) / 3.0
-  K3[:, 1, 0] = K3[:, 0, 1]
-  K3[:, 1, 1] = (rots[:, 1, 1] - rots[:, 0, 0] - rots[:, 2, 2]) / 3.0
-  K3[:, 1, 2] = (rots[:, 2, 1] + rots[:, 1, 2]) / 3.0
-  K3[:, 1, 3] = (rots[:, 2, 0] - rots[:, 0, 2]) / 3.0
-  K3[:, 2, 0] = K3[:, 0, 2]
-  K3[:, 2, 1] = K3[:, 1, 2]
-  K3[:, 2, 2] = (rots[:, 2, 2] - rots[:, 0, 0] - rots[:, 1, 1]) / 3.0
-  K3[:, 2, 3] = (rots[:, 0, 1] - rots[:, 1, 0]) / 3.0
-  K3[:, 3, 0] = K3[:, 0, 3]
-  K3[:, 3, 1] = K3[:, 1, 3]
-  K3[:, 3, 2] = K3[:, 2, 3]
-  K3[:, 3, 3] = (rots[:, 0, 0] + rots[:, 1, 1] + rots[:, 2, 2]) / 3.0
-  q = np.empty((len(rots), 4))
-  for i in range(len(rots)):
-    _, eigvecs = linalg.eigh(K3[i].T)
-    eigvecs = eigvecs[:,3:]
-    q[i, 0] = eigvecs[-1]
-    q[i, 1:] = -eigvecs[:-1].flatten()
-    if q[i, 0] < 0:
-      q[i] = -q[i]
-
-  if len(input_shape) < 3:
-    return q[0]
-  return q
-
-
-def euler2rot(eulers):
-  return rotations_from_quats(euler2quat(eulers))
-
-
-def rot2euler(rots):
-  return quat2euler(quats_from_rotations(rots))
-
-
-quats_from_rotations = rot2quat
-quat_from_rot = rot2quat
-rotations_from_quats = quat2rot
-rot_from_quat= quat2rot
-rot_from_quat= quat2rot
-euler_from_rot = rot2euler
-euler_from_quat = quat2euler
-rot_from_euler = euler2rot
-quat_from_euler = euler2quat
-
-
-'''
-Random helpers below
-'''
-
-
-def quat_product(q, r):
-  t = np.zeros(4)
-  t[0] = r[0] * q[0] - r[1] * q[1] - r[2] * q[2] - r[3] * q[3]
-  t[1] = r[0] * q[1] + r[1] * q[0] - r[2] * q[3] + r[3] * q[2]
-  t[2] = r[0] * q[2] + r[1] * q[3] + r[2] * q[0] - r[3] * q[1]
-  t[3] = r[0] * q[3] - r[1] * q[2] + r[2] * q[1] + r[3] * q[0]
-  return t
-
-
-def rot_matrix(roll, pitch, yaw):
-  cr, sr = np.cos(roll), np.sin(roll)
-  cp, sp = np.cos(pitch), np.sin(pitch)
-  cy, sy = np.cos(yaw), np.sin(yaw)
-  rr = array([[1,0,0],[0, cr,-sr],[0, sr, cr]])
-  rp = array([[cp,0,sp],[0, 1,0],[-sp, 0, cp]])
-  ry = array([[cy,-sy,0],[sy, cy,0],[0, 0, 1]])
-  return ry.dot(rp.dot(rr))
-
-
-def rot(axis, angle):
-  # Rotates around an arbitrary axis
-  ret_1 = (1 - np.cos(angle)) * array([[axis[0]**2, axis[0] * axis[1], axis[0] * axis[2]], [
-    axis[1] * axis[0], axis[1]**2, axis[1] * axis[2]
-  ], [axis[2] * axis[0], axis[2] * axis[1], axis[2]**2]])
-  ret_2 = np.cos(angle) * np.eye(3)
-  ret_3 = np.sin(angle) * array([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]],
-                              [-axis[1], axis[0], 0]])
-  return ret_1 + ret_2 + ret_3
-
-
-def ecef_euler_from_ned(ned_ecef_init, ned_pose):
-  '''
-  Got it from here:
-  Using Rotations to Build Aerospace Coordinate Systems
-  -Don Koks
-  '''
-  converter = LocalCoord.from_ecef(ned_ecef_init)
-  x0 = converter.ned2ecef([1, 0, 0]) - converter.ned2ecef([0, 0, 0])
-  y0 = converter.ned2ecef([0, 1, 0]) - converter.ned2ecef([0, 0, 0])
-  z0 = converter.ned2ecef([0, 0, 1]) - converter.ned2ecef([0, 0, 0])
-
-  x1 = rot(z0, ned_pose[2]).dot(x0)
-  y1 = rot(z0, ned_pose[2]).dot(y0)
-  z1 = rot(z0, ned_pose[2]).dot(z0)
-
-  x2 = rot(y1, ned_pose[1]).dot(x1)
-  y2 = rot(y1, ned_pose[1]).dot(y1)
-  z2 = rot(y1, ned_pose[1]).dot(z1)
-
-  x3 = rot(x2, ned_pose[0]).dot(x2)
-  y3 = rot(x2, ned_pose[0]).dot(y2)
-  #z3 = rot(x2, ned_pose[0]).dot(z2)
-
-  x0 = array([1, 0, 0])
-  y0 = array([0, 1, 0])
-  z0 = array([0, 0, 1])
-
-  psi = np.arctan2(inner(x3, y0), inner(x3, x0))
-  theta = np.arctan2(-inner(x3, z0), np.sqrt(inner(x3, x0)**2 + inner(x3, y0)**2))
-  y2 = rot(z0, psi).dot(y0)
-  z2 = rot(y2, theta).dot(z0)
-  phi = np.arctan2(inner(y3, z2), inner(y3, y2))
-
-  ret = array([phi, theta, psi])
-  return ret
-
-
-def ned_euler_from_ecef(ned_ecef_init, ecef_poses):
-  '''
-  Got the math from here:
-  Using Rotations to Build Aerospace Coordinate Systems
-  -Don Koks
-
-  Also accepts array of ecef_poses and array of ned_ecef_inits.
-  Where each row is a pose and an ecef_init.
-  '''
-  ned_ecef_init = array(ned_ecef_init)
-  ecef_poses = array(ecef_poses)
-  output_shape = ecef_poses.shape
-  ned_ecef_init = np.atleast_2d(ned_ecef_init)
-  if ned_ecef_init.shape[0] == 1:
-    ned_ecef_init = np.tile(ned_ecef_init[0], (output_shape[0], 1))
-  ecef_poses = np.atleast_2d(ecef_poses)
-
-  ned_poses = np.zeros(ecef_poses.shape)
-  for i, ecef_pose in enumerate(ecef_poses):
-    converter = LocalCoord.from_ecef(ned_ecef_init[i])
-    x0 = array([1, 0, 0])
-    y0 = array([0, 1, 0])
-    z0 = array([0, 0, 1])
-
-    x1 = rot(z0, ecef_pose[2]).dot(x0)
-    y1 = rot(z0, ecef_pose[2]).dot(y0)
-    z1 = rot(z0, ecef_pose[2]).dot(z0)
-
-    x2 = rot(y1, ecef_pose[1]).dot(x1)
-    y2 = rot(y1, ecef_pose[1]).dot(y1)
-    z2 = rot(y1, ecef_pose[1]).dot(z1)
-
-    x3 = rot(x2, ecef_pose[0]).dot(x2)
-    y3 = rot(x2, ecef_pose[0]).dot(y2)
-    #z3 = rot(x2, ecef_pose[0]).dot(z2)
-
-    x0 = converter.ned2ecef([1, 0, 0]) - converter.ned2ecef([0, 0, 0])
-    y0 = converter.ned2ecef([0, 1, 0]) - converter.ned2ecef([0, 0, 0])
-    z0 = converter.ned2ecef([0, 0, 1]) - converter.ned2ecef([0, 0, 0])
-
-    psi = np.arctan2(inner(x3, y0), inner(x3, x0))
-    theta = np.arctan2(-inner(x3, z0), np.sqrt(inner(x3, x0)**2 + inner(x3, y0)**2))
-    y2 = rot(z0, psi).dot(y0)
-    z2 = rot(y2, theta).dot(z0)
-    phi = np.arctan2(inner(y3, z2), inner(y3, y2))
-    ned_poses[i] = array([phi, theta, psi])
-
-  return ned_poses.reshape(output_shape)
-
-
-def ecef2car(car_ecef, psi, theta, points_ecef, ned_converter):
-  """
-  TODO: add roll rotation
-  Converts an array of points in ecef coordinates into
-  x-forward, y-left, z-up coordinates
-  Parameters
-  ----------
-  psi: yaw, radian
-  theta: pitch, radian
-  Returns
-  -------
-  [x, y, z] coordinates in car frame
-  """
-
-  # input is an array of points in ecef cocrdinates
-  # output is an array of points in car's coordinate (x-front, y-left, z-up)
-
-  # convert points to NED
-  points_ned = []
-  for p in points_ecef:
-    points_ned.append(ned_converter.ecef2ned_matrix.dot(array(p) - car_ecef))
-
-  points_ned = np.vstack(points_ned).T
-
-  # n, e, d -> x, y, z
-  # Calculate relative positions and rotate wrt to heading and pitch of car
-  invert_R = array([[1., 0., 0.], [0., -1., 0.], [0., 0., -1.]])
-
-  c, s = np.cos(psi), np.sin(psi)
-  yaw_R = array([[c, s, 0.], [-s, c, 0.], [0., 0., 1.]])
-
-  c, s = np.cos(theta), np.sin(theta)
-  pitch_R = array([[c, 0., -s], [0., 1., 0.], [s, 0., c]])
-
-  return dot(pitch_R, dot(yaw_R, dot(invert_R, points_ned)))
diff --git a/laika/opt.py b/laika/opt.py
deleted file mode 100644
index dfa8ae5ca8..0000000000
--- a/laika/opt.py
+++ /dev/null
@@ -1,192 +0,0 @@
-import sympy
-import numpy as np
-from typing import List
-
-from .constants import EARTH_ROTATION_RATE, SPEED_OF_LIGHT
-from .helpers import ConstellationId
-from .raw_gnss import GNSSMeasurement
-
-
-def gauss_newton(fun, b, M, xtol=1e-8, max_n=25):
-
-  W = np.linalg.inv(M)
-  for _ in range(max_n):
-    # Compute function and jacobian on current estimate
-    r, J = fun(b)
-
-    # Update estimate, WLS https://en.wikipedia.org/wiki/Weighted_least_squares
-    delta = np.linalg.pinv(J.T.dot(W).dot(J)).dot(J.T).dot(W) @ r
-    b -= delta
-
-    # Check step size for stopping condition
-    if np.linalg.norm(delta) < xtol:
-      break
-
-  r, J = fun(b)
-  Mb = np.linalg.pinv(J.T.dot(W).dot(J))
-  x_std = np.sqrt(np.diagonal(Mb))
-  return b, r, x_std
-
-
-def calc_pos_fix(measurements, posfix_functions=None, x0=None, signal='C1C', min_measurements=5):
-  '''
-  Calculates gps fix using gauss newton method
-  To solve the problem a minimal of 4 measurements are required.
-    If Glonass is included 5 are required to solve for the additional free variable.
-  returns:
-  0 -> list with positions
-  1 -> pseudorange errs
-  '''
-  if x0 is None:
-    x0 = [0, 0, 0, 0, 0]
-
-  if len(measurements) < min_measurements:
-    return [],[],[]
-
-  Fx_pos = pr_residual(measurements, posfix_functions, signal=signal, no_nans=True)
-  meas_cov = np.diag([meas.observables_std[signal]**2 for meas in measurements])
-
-  x, residual, x_std = gauss_newton(Fx_pos, x0, meas_cov)
-  return x.tolist(), residual.tolist(), x_std
-
-
-def calc_vel_fix(measurements, est_pos, velfix_function=None, v0=None, signal='D1C', min_measurements=5):
-  '''
-  Calculates gps velocity fix using gauss newton method
-  returns:
-  0 -> list with velocities
-  1 -> pseudorange_rate errs
-  '''
-  if v0 is None:
-    v0 = [0, 0, 0, 0]
-
-  if len(measurements) < min_measurements:
-    return [], [], []
-
-  Fx_vel = prr_residual(measurements, est_pos, velfix_function, signal=signal, no_nans=True)
-  meas_cov = np.diag([meas.observables_std[signal]**2 for meas in measurements])
-
-  v, residual, x_std = gauss_newton(Fx_vel, v0, meas_cov)
-  return v.tolist(), residual.tolist(), x_std
-
-
-def get_posfix_sympy_fun(constellation):
-  # Unknowns
-  x, y, z = sympy.Symbol('x'), sympy.Symbol('y'), sympy.Symbol('z')
-  bc = sympy.Symbol('bc')
-  bg = sympy.Symbol('bg')
-  zero_theta = sympy.Symbol('zero_theta')
-  var = [x, y, z, bc, bg]
-
-  # Knowns
-  pr = sympy.Symbol('pr')
-  sat_x, sat_y, sat_z = sympy.Symbol('sat_x'), sympy.Symbol('sat_y'), sympy.Symbol('sat_z')
-
-  theta = (EARTH_ROTATION_RATE * (pr - bc) / SPEED_OF_LIGHT)*zero_theta
-  val = sympy.sqrt(
-    (sat_x * sympy.cos(theta) + sat_y * sympy.sin(theta) - x) ** 2 +
-    (sat_y * sympy.cos(theta) - sat_x * sympy.sin(theta) - y) ** 2 +
-    (sat_z - z) ** 2
-  )
-
-  if constellation == ConstellationId.GLONASS:
-    res = val - (pr - bc - bg)
-  elif constellation == ConstellationId.GPS:
-    res = val - (pr - bc)
-  else:
-    raise NotImplementedError(f"Constellation {constellation} not supported")
-
-  res = [res] + [sympy.diff(res, v) for v in var]
-
-  return sympy.lambdify([x, y, z, bc, bg, pr, zero_theta, sat_x, sat_y, sat_z], res, modules=["numpy"])
-
-
-def get_velfix_sympy_func():
-  # implementing this without sympy.Matrix gives a 2x speedup at generation
-
-  # knowns, receiver position, satellite position, satellite velocity
-  ep_x, ep_y, ep_z = sympy.Symbol('ep_x'), sympy.Symbol('ep_y'), sympy.Symbol('ep_z')
-  est_pos = np.array([ep_x, ep_y, ep_z])
-  sp_x, sp_y, sp_z = sympy.Symbol('sp_x'), sympy.Symbol('sp_y'), sympy.Symbol('sp_z')
-  sat_pos = np.array([sp_x, sp_y, sp_z])
-  sv_x, sv_y, sv_z = sympy.Symbol('sv_x'), sympy.Symbol('sv_y'), sympy.Symbol('sv_z')
-  sat_vel = np.array([sv_x, sv_y, sv_z])
-  observables = sympy.Symbol('observables')
-
-  # unknown, receiver velocity
-  v_x, v_y, v_z = sympy.Symbol('v_x'), sympy.Symbol('v_y'), sympy.Symbol('v_z')
-  vel = np.array([v_x, v_y, v_z])
-  vel_o = sympy.Symbol('vel_o')
-
-  loss = sat_pos - est_pos
-  loss /= sympy.sqrt(loss.dot(loss))
-  res = loss.dot(sat_vel - vel) - (observables - vel_o)
-
-  res = [res] + [sympy.diff(res, v) for v in [v_x, v_y, v_z, vel_o]]
-
-  return sympy.lambdify([
-      ep_x, ep_y, ep_z, sp_x, sp_y, sp_z,
-      sv_x, sv_y, sv_z, observables,
-      v_x, v_y, v_z, vel_o
-    ],
-    res, modules=["numpy"])
-
-
-def pr_residual(measurements: List[GNSSMeasurement], posfix_functions=None, signal='C1C', no_nans=False):
-
-  if posfix_functions is None:
-    posfix_functions = {constellation: get_posfix_sympy_fun(constellation) for constellation in (ConstellationId.GPS, ConstellationId.GLONASS)}
-
-  def Fx_pos(inp):
-    vals, gradients = [], []
-
-    for meas in measurements:
-      if signal in meas.observables_final and np.isfinite(meas.observables_final[signal]):
-        pr = meas.observables_final[signal]
-        sat_pos = meas.sat_pos_final
-        zero_theta = 0
-      elif signal in meas.observables and np.isfinite(meas.observables[signal]) and meas.processed:
-        pr = meas.observables[signal]
-        pr += meas.sat_clock_err * SPEED_OF_LIGHT
-        sat_pos = meas.sat_pos
-        zero_theta = 1
-      else:
-        if not no_nans:
-          vals.append(np.nan)
-          gradients.append(np.nan)
-        continue
-
-      val, *gradient = posfix_functions[meas.constellation_id](*inp, pr, zero_theta, *sat_pos)
-      vals.append(val)
-      gradients.append(gradient)
-    return np.asarray(vals), np.asarray(gradients)
-  return Fx_pos
-
-
-def prr_residual(measurements: List[GNSSMeasurement], est_pos, velfix_function=None, signal='D1C', no_nans=False):
-
-  if velfix_function is None:
-    velfix_function = get_velfix_sympy_func()
-
-  def Fx_vel(vel):
-    vals, gradients = [], []
-
-    for meas in measurements:
-      if signal not in meas.observables or not np.isfinite(meas.observables[signal]):
-        if not no_nans:
-          vals.append(np.nan)
-          gradients.append(np.nan)
-        continue
-
-      sat_pos = meas.sat_pos_final if meas.corrected else meas.sat_pos
-
-      val, *gradient = velfix_function(est_pos[0], est_pos[1], est_pos[2],
-                                       sat_pos[0], sat_pos[1], sat_pos[2],
-                                       meas.sat_vel[0], meas.sat_vel[1], meas.sat_vel[2],
-                                       meas.observables[signal],
-                                       vel[0], vel[1], vel[2], vel[3])
-      vals.append(val)
-      gradients.append(gradient)
-
-    return np.asarray(vals), np.asarray(gradients)
-  return Fx_vel
diff --git a/laika/raw_gnss.py b/laika/raw_gnss.py
deleted file mode 100644
index 405a96ef20..0000000000
--- a/laika/raw_gnss.py
+++ /dev/null
@@ -1,334 +0,0 @@
-from math import sqrt
-from typing import Dict, List, Optional, Union
-
-import numpy as np
-import datetime
-import struct
-
-from . import constants
-from .ephemeris import Ephemeris
-from .lib.coordinates import LocalCoord
-from .gps_time import GPSTime
-from .helpers import ConstellationId, get_constellation_and_sv_id, get_nmea_id_from_constellation_and_svid, \
-  rinex3_obs_from_rinex2_obs
-
-
-def array_from_normal_meas(meas):
-  return np.concatenate(([meas.get_nmea_id()],
-                         [meas.recv_time_week],
-                         [meas.recv_time_sec],
-                         [meas.glonass_freq],
-                         [meas.observables['C1C']],
-                         [meas.observables_std['C1C']],
-                         [meas.observables['D1C']],
-                         [meas.observables_std['D1C']],
-                         [meas.observables['S1C']],
-                         [meas.observables['L1C']]))
-
-
-def normal_meas_from_array(arr):
-  observables, observables_std = {}, {}
-  observables['C1C'] = arr[4]
-  observables_std['C1C'] = arr[5]
-  observables['D1C'] = arr[6]
-  observables_std['D1C'] = arr[7]
-  observables['S1C'] = arr[8]
-  observables['L1C'] = arr[9]
-  constellation_id, sv_id = get_constellation_and_sv_id(nmea_id=arr[0])
-  return GNSSMeasurement(constellation_id, sv_id, arr[1], arr[2],
-                         observables, observables_std, arr[3])
-
-
-class GNSSMeasurement:
-  PRN = 0
-  RECV_TIME_WEEK = 1
-  RECV_TIME_SEC = 2
-  GLONASS_FREQ = 3
-
-  PR = 4
-  PR_STD = 5
-  PRR = 6
-  PRR_STD = 7
-
-  SAT_POS = slice(8, 11)
-  SAT_VEL = slice(11, 14)
-
-  def __init__(self, constellation_id: ConstellationId, sv_id: int, recv_time_week: int, recv_time_sec: float, observables: Dict[str, float], observables_std: Dict[str, float],
-               glonass_freq: Union[int, float, None] = None):
-    # Metadata
-    # prn: unique satellite id
-    self.prn = "%s%02d" % (constellation_id.to_rinex_char(), sv_id)  # satellite ID in rinex convention
-    self.constellation_id = constellation_id
-    self.sv_id = sv_id  # satellite id per constellation
-
-    self.recv_time_week = recv_time_week
-    self.recv_time_sec = recv_time_sec
-    self.recv_time = GPSTime(recv_time_week, recv_time_sec)
-    self.glonass_freq = glonass_freq  # glonass channel
-
-    # Measurements
-    self.observables = observables
-    self.observables_std = observables_std
-
-    # flags
-    self.processed = False
-    self.corrected = False
-
-    # sat info
-    self.sat_pos = np.array([np.nan, np.nan, np.nan])
-    self.sat_vel = np.array([np.nan, np.nan, np.nan])
-    self.sat_clock_err = np.nan
-    self.sat_ephemeris: Optional[Ephemeris] = None
-
-    self.sat_pos_final = np.array([np.nan, np.nan, np.nan])  # sat_pos in receiver time's ECEF frame instead of satellite time's ECEF frame
-    self.observables_final: Dict[str, float] = {}
-
-  def process(self, dog):
-    sat_time = self.recv_time - self.observables['C1C']/constants.SPEED_OF_LIGHT
-    sat_info = dog.get_sat_info(self.prn, sat_time)
-    if sat_info is None:
-      return False
-    self.sat_pos, self.sat_vel, self.sat_clock_err, _, self.sat_ephemeris = sat_info
-    self.processed = True
-    return True
-
-  def correct(self, est_pos, dog, correct_delay=True):
-    for obs in self.observables:
-      if obs[0] == 'C':  # or obs[0] == 'L':
-        if correct_delay:
-          delay = dog.get_delay(self.prn, self.recv_time, est_pos, signal=obs)
-        else:
-          delay = 0.0
-        if delay is not None:
-          self.observables_final[obs] = (self.observables[obs] +
-                                         self.sat_clock_err*constants.SPEED_OF_LIGHT -
-                                         delay)
-      else:
-        self.observables_final[obs] = self.observables[obs]
-    if 'C1C' in self.observables_final and 'C2P' in self.observables_final:
-      self.observables_final['IOF'] = (((constants.GPS_L1**2)*self.observables_final['C1C'] -
-                                        (constants.GPS_L2**2)*self.observables_final['C2P'])/
-                                       (constants.GPS_L1**2 - constants.GPS_L2**2))
-
-    geometric_range = np.linalg.norm(self.sat_pos - est_pos)
-    theta_1 = constants.EARTH_ROTATION_RATE * geometric_range / constants.SPEED_OF_LIGHT
-    self.sat_pos_final = np.array([self.sat_pos[0] * np.cos(theta_1) + self.sat_pos[1] * np.sin(theta_1),
-                                   self.sat_pos[1] * np.cos(theta_1) - self.sat_pos[0] * np.sin(theta_1),
-                                   self.sat_pos[2]])
-    if 'C1C' in self.observables_final and np.isfinite(self.observables_final['C1C']):
-      self.corrected = True
-      return True
-    return False
-
-  def as_array(self, only_corrected=True):
-    observables = self.observables_final
-    sat_pos = self.sat_pos_final
-    if not self.corrected:
-      if only_corrected:
-        raise NotImplementedError('Only corrected measurements can be put into arrays')
-      else:
-        observables = self.observables
-        sat_pos = self.sat_pos
-    ret = np.array([self.get_nmea_id(), self.recv_time_week, self.recv_time_sec, self.glonass_freq,
-                    observables['C1C'], self.observables_std['C1C'],
-                    observables['D1C'], self.observables_std['D1C']])
-    return np.concatenate((ret, sat_pos, self.sat_vel))
-
-  def __repr__(self):
-    time = self.recv_time.as_datetime().strftime('%Y-%m-%dT%H:%M:%S.%f')
-    return f"<GNSSMeasurement from {self.prn} at {time}>"
-
-  def get_nmea_id(self):
-    return get_nmea_id_from_constellation_and_svid(self.constellation_id, self.sv_id)
-
-
-def process_measurements(measurements: List[GNSSMeasurement], dog) -> List[GNSSMeasurement]:
-  proc_measurements = []
-  for meas in measurements:
-    if meas.process(dog):
-      proc_measurements.append(meas)
-  return proc_measurements
-
-
-def correct_measurements(measurements: List[GNSSMeasurement], est_pos, dog, correct_delay=True) -> List[GNSSMeasurement]:
-  corrected_measurements = []
-  for meas in measurements:
-    if meas.correct(est_pos, dog, correct_delay=correct_delay):
-      corrected_measurements.append(meas)
-  return corrected_measurements
-
-
-def group_measurements_by_epoch(measurements):
-  meas_filt_by_t = [[measurements[0]]]
-  for m in measurements[1:]:
-    if abs(m.recv_time - meas_filt_by_t[-1][-1].recv_time) > 1e-9:
-      meas_filt_by_t.append([])
-    meas_filt_by_t[-1].append(m)
-  return meas_filt_by_t
-
-
-def group_measurements_by_sat(measurements):
-  measurements_by_sat = {}
-  sats = {m.prn for m in measurements}
-  for sat in sats:
-    measurements_by_sat[sat] = [m for m in measurements if m.prn == sat]
-  return measurements_by_sat
-
-
-def read_raw_qcom(report):
-  dr = 'DrMeasurementReport' in str(report.schema)
-  # Only gps/sbas and glonass are supported
-  constellation_id = ConstellationId.from_qcom_source(report.source)
-  if constellation_id in [ConstellationId.GPS, ConstellationId.SBAS]:  # gps/sbas
-    if dr:
-      recv_tow = report.gpsMilliseconds / 1000.0  # seconds
-      time_bias_ms = struct.unpack("f", struct.pack("I", report.gpsTimeBiasMs))[0]
-    else:
-      recv_tow = report.milliseconds / 1000.0  # seconds
-      time_bias_ms = report.timeBias
-    recv_time = GPSTime(report.gpsWeek, recv_tow)
-  elif constellation_id == ConstellationId.GLONASS:
-    if dr:
-      recv_tow = report.glonassMilliseconds / 1000.0  # seconds
-      recv_time = GPSTime.from_glonass(report.glonassYear, report.glonassDay, recv_tow)
-      time_bias_ms = report.glonassTimeBias
-    else:
-      recv_tow = report.milliseconds / 1000.0  # seconds
-      recv_time = GPSTime.from_glonass(report.glonassCycleNumber, report.glonassNumberOfDays, recv_tow)
-      time_bias_ms = report.timeBias
-  else:
-    raise NotImplementedError('Only GPS (0), SBAS (1) and GLONASS (6) are supported from qcom, not:', {report.source})
-  # logging.debug(recv_time, report.source, time_bias_ms, dr)
-  measurements = []
-  for i in report.sv:
-    nmea_id = i.svId  # todo change svId to nmea_id in cereal message. Or better: change the publisher to publish correct svId's, since constellation id is also given
-    if nmea_id == 255:
-      # TODO nmea_id is not valid. Fix publisher
-      continue
-    _, sv_id = get_constellation_and_sv_id(nmea_id)
-    if not i.measurementStatus.measurementNotUsable and i.measurementStatus.satelliteTimeIsKnown:
-      sat_tow = (i.unfilteredMeasurementIntegral + i.unfilteredMeasurementFraction + i.latency + time_bias_ms) / 1000
-      observables, observables_std = {}, {}
-      observables['C1C'] = (recv_tow - sat_tow)*constants.SPEED_OF_LIGHT
-      observables_std['C1C'] = i.unfilteredTimeUncertainty * 1e-3 * constants.SPEED_OF_LIGHT
-      if i.measurementStatus.fineOrCoarseVelocity:
-        # about 10x better, perhaps filtered with carrier phase?
-        observables['D1C'] = i.fineSpeed
-        observables_std['D1C'] = i.fineSpeedUncertainty
-      else:
-        observables['D1C'] = i.unfilteredSpeed
-        observables_std['D1C'] = i.unfilteredSpeedUncertainty
-      observables['S1C'] = (i.carrierNoise/100.) if i.carrierNoise != 0 else np.nan
-      observables['L1C'] = np.nan
-      # logging.debug("  %.5f %3d %10.2f %7.2f %7.2f %.2f %d" % (recv_time.tow, nmea_id,
-      # observables['C1C'], observables_std['C1C'],
-      # observables_std['D1C'], observables['S1C'], i.latency), i.observationState, i.measurementStatus.fineOrCoarseVelocity)
-      glonass_freq = (i.glonassFrequencyIndex - 7) if constellation_id == ConstellationId.GLONASS else np.nan
-      measurements.append(GNSSMeasurement(constellation_id, sv_id,
-                                          recv_time.week,
-                                          recv_time.tow,
-                                          observables,
-                                          observables_std,
-                                          glonass_freq))
-  return measurements
-
-
-def read_raw_ublox(report) -> List[GNSSMeasurement]:
-  recv_tow = report.rcvTow  # seconds
-  recv_week = report.gpsWeek
-  measurements = []
-  for i in report.measurements:
-    # only add Gps and Glonass fixes
-    if i.gnssId in [ConstellationId.GPS, ConstellationId.GLONASS]:
-      if i.svId > 32 or i.pseudorange > 2**32:
-        continue
-      observables = {}
-      observables_std = {}
-      if i.trackingStatus.pseudorangeValid and i.sigId == 0:
-        observables['C1C'] = i.pseudorange
-        # Empirically it seems obvious ublox's std is
-        # actually a variation
-        observables_std['C1C'] = sqrt(i.pseudorangeStdev)*10
-        if i.gnssId == ConstellationId.GLONASS:
-          glonass_freq = i.glonassFrequencyIndex - 7
-          observables['D1C'] = -(constants.SPEED_OF_LIGHT / (constants.GLONASS_L1 + glonass_freq * constants.GLONASS_L1_DELTA)) * i.doppler
-        else:  # GPS
-          glonass_freq = np.nan
-          observables['D1C'] = -(constants.SPEED_OF_LIGHT / constants.GPS_L1) * i.doppler
-        observables_std['D1C'] = (constants.SPEED_OF_LIGHT / constants.GPS_L1) * i.dopplerStdev
-        observables['S1C'] = i.cno
-        if i.trackingStatus.carrierPhaseValid:
-          observables['L1C'] = i.carrierCycles
-        else:
-          observables['L1C'] = np.nan
-
-        measurements.append(GNSSMeasurement(ConstellationId(i.gnssId), i.svId, recv_week, recv_tow,
-                                            observables, observables_std, glonass_freq))
-  return measurements
-
-
-def read_rinex_obs(obsdata) -> List[List[GNSSMeasurement]]:
-  measurements: List[List[GNSSMeasurement]] = []
-  obsdata_keys = list(obsdata.data.keys())
-  first_sat = obsdata_keys[0]
-  n = len(obsdata.data[first_sat]['Epochs'])
-  for i in range(n):
-    recv_time_datetime = obsdata.data[first_sat]['Epochs'][i]
-    recv_time_datetime = recv_time_datetime.astype(datetime.datetime)
-    recv_time = GPSTime.from_datetime(recv_time_datetime)
-    measurements.append([])
-    for sat_str in obsdata_keys:
-      if np.isnan(obsdata.data[sat_str]['C1'][i]):
-        continue
-      observables, observables_std = {}, {}
-      for obs in obsdata.data[sat_str]:
-        if obs == 'Epochs':
-          continue
-        rinex3_obs_key = rinex3_obs_from_rinex2_obs(obs)
-        observables[rinex3_obs_key] = obsdata.data[sat_str][obs][i]
-        observables_std[rinex3_obs_key] = 1.
-
-      constellation_id, sv_id = get_constellation_and_sv_id(int(sat_str))
-      measurements[-1].append(GNSSMeasurement(constellation_id, sv_id,
-                                              recv_time.week, recv_time.tow,
-                                              observables, observables_std))
-  return measurements
-
-
-def get_Q(recv_pos, sat_positions):
-  local = LocalCoord.from_ecef(recv_pos)
-  sat_positions_rel = local.ecef2ned(sat_positions)
-  sat_distances = np.linalg.norm(sat_positions_rel, axis=1)
-  A = np.column_stack((sat_positions_rel[:,0]/sat_distances,  # pylint: disable=unsubscriptable-object
-                       sat_positions_rel[:,1]/sat_distances,  # pylint: disable=unsubscriptable-object
-                       sat_positions_rel[:,2]/sat_distances,  # pylint: disable=unsubscriptable-object
-                       -np.ones(len(sat_distances))))
-  if A.shape[0] < 4 or np.linalg.matrix_rank(A) < 4:
-    return np.inf*np.ones((4,4))
-  Q = np.linalg.inv(A.T.dot(A))
-  return Q
-
-
-def get_DOP(recv_pos, sat_positions):
-  Q = get_Q(recv_pos, sat_positions)
-  return np.sqrt(np.trace(Q))
-
-
-def get_HDOP(recv_pos, sat_positions):
-  Q = get_Q(recv_pos, sat_positions)
-  return np.sqrt(np.trace(Q[:2,:2]))
-
-
-def get_VDOP(recv_pos, sat_positions):
-  Q = get_Q(recv_pos, sat_positions)
-  return np.sqrt(Q[2,2])
-
-
-def get_TDOP(recv_pos, sat_positions):
-  Q = get_Q(recv_pos, sat_positions)
-  return np.sqrt(Q[3,3])
-
-
-def get_PDOP(recv_pos, sat_positions):
-  Q = get_Q(recv_pos, sat_positions)
-  return np.sqrt(np.trace(Q[:3,:3]))
diff --git a/laika/rinex_file.py b/laika/rinex_file.py
deleted file mode 100644
index f0697edde6..0000000000
--- a/laika/rinex_file.py
+++ /dev/null
@@ -1,252 +0,0 @@
-#!/usr/bin/env python
-
-# Copyright (C) 2014 Swift Navigation Inc.
-#
-# This source is subject to the license found in the file 'LICENSE' which must
-# be be distributed together with this source. All other rights reserved.
-#
-# THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
-# EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
-# WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
-
-import datetime
-import numpy as np
-import logging
-
-def floatornan(x):
-  if x == '' or x[-1] == ' ':
-    return np.NaN
-  return float(x)
-
-
-def digitorzero(x):
-  if x == ' ' or x == '':
-    return 0
-  return int(x)
-
-
-def padline(l, n=16):
-  x = len(l)
-  x_ = n * ((x + n - 1) // n)
-  padded = l + ' ' * (x_ - x)
-  while len(padded) < 70:
-    padded += ' ' * 16
-  return padded
-
-
-TOTAL_SATS = 132  # Increased to support Galileo
-
-
-class DownloadError(Exception):
-  pass
-
-
-class RINEXFile:
-  def __init__(self, filename, rate=None):
-    self.rate = rate
-    try:
-      with open(filename) as f:
-        self._read_header(f)
-        self._read_data(f)
-    except TypeError:
-      logging.exception("TypeError, file likely not downloaded.")
-      raise DownloadError("file download failure")
-    except FileNotFoundError:
-      logging.exception("File not found in directory.")
-      raise DownloadError("file missing in download cache")
-  def _read_header(self, f):
-    version_line = padline(f.readline(), 80)
-
-    self.version = float(version_line[0:9])
-    if (self.version > 2.11):
-      raise ValueError(
-        f"RINEX file versions > 2.11 not supported (file version {self.version:f})")
-
-    self.filetype = version_line[20]
-    if self.filetype not in "ONGM":  # Check valid file type
-      raise ValueError(f"RINEX file type '{self.filetype}' not supported")
-    if self.filetype != 'O':
-      raise ValueError("Only 'OBSERVATION DATA' RINEX files are currently supported")
-
-    self.gnss = version_line[40]
-    if self.gnss not in " GRSEM":  # Check valid satellite system
-      raise ValueError(f"Satellite system '{self.filetype}' not supported")
-    if self.gnss == ' ':
-      self.gnss = 'G'
-    if self.gnss != 'G':
-      #raise ValueError("Only GPS data currently supported")
-      pass
-
-    self.comment = ""
-    while True:  # Read the rest of the header
-      line = padline(f.readline(), 80)
-      label = line[60:80].rstrip()
-      if label == "END OF HEADER":
-        break
-      if label == "COMMENT":
-        self.comment += line[:60] + '\n'
-      if label == "MARKER NAME":
-        self.marker_name = line[:60].rstrip()
-        if self.marker_name == '':
-          self.marker_name = 'UNKNOWN'
-      if label == "# / TYPES OF OBSERV":
-        # RINEX files can have multiple line headers
-        # This code handles the case
-        try:
-          n_obs = int(line[0:6])
-          self.obs_types = []
-        except ValueError:
-          pass
-
-        if n_obs <= 9:
-          for i in range(0, n_obs):
-            self.obs_types.append(line[10 + 6 * i:12 + 6 * i])
-        if n_obs > 9:
-          for i in range(0, 9):
-            self.obs_types.append(line[10 + 6 * i:12 + 6 * i])
-          n_obs -= 9
-
-  def _read_next_non_comment(self, f):
-    line = f.readline()
-    while line and line.find('COMMENT') != -1:
-      line = f.readline()
-    return line
-
-  def _read_epoch_header(self, f):
-    epoch_hdr = self._read_next_non_comment(f)
-    if epoch_hdr == '':
-      return None
-    # ignore any line with these three strings
-    skippable = ('0.0000000  4  5', 'MARKER NUMBER', '          4  1')
-    while any(skip in epoch_hdr for skip in skippable):
-      epoch_hdr = self._read_next_non_comment(f)
-
-    if epoch_hdr == '':
-      return None
-
-    year = int(epoch_hdr[1:3])
-    if year >= 80:
-      year += 1900
-    else:
-      year += 2000
-    month = int(epoch_hdr[4:6])
-    day = int(epoch_hdr[7:9])
-    hour = int(epoch_hdr[10:12])
-    minute = int(epoch_hdr[13:15])
-    second = int(epoch_hdr[15:18])
-    microsecond = int(
-      epoch_hdr[19:25])  # Discard the least sig. fig. (use microseconds only).
-    epoch = datetime.datetime(year, month, day, hour, minute, second, microsecond)
-
-    flag = int(epoch_hdr[28])
-    allowed_flags = {0, 3, 4}
-    if flag not in allowed_flags:
-      raise ValueError("Don't know how to handle epoch flag %d in epoch header:\n%s" %
-                       (flag, epoch_hdr))
-
-    n_sats = int(epoch_hdr[29:32])
-    if flag > 1:  # event flag: nsats is number of records
-      for i in range(n_sats):
-        f.readline()
-      return None
-
-    sats = []
-    for i in range(0, n_sats):
-      if ((i % 12) == 0) and (i > 0):
-        epoch_hdr = f.readline()
-      sats.append(epoch_hdr[(32 + (i % 12) * 3):(35 + (i % 12) * 3)])
-
-    return epoch, flag, sats
-
-  def _read_obs(self, f, n_sat, sat_map):
-    obs = np.empty((TOTAL_SATS, len(self.obs_types)), dtype=np.float64) * np.NaN
-    lli = np.zeros((TOTAL_SATS, len(self.obs_types)), dtype=np.uint8)
-    signal_strength = np.zeros((TOTAL_SATS, len(self.obs_types)), dtype=np.uint8)
-
-    for i in range(n_sat):
-      # Join together observations for a single satellite if split across lines.
-      obs_line = ''.join(
-        padline(f.readline()[:-1], 16) for _ in range((len(self.obs_types) + 4) // 5))
-      for j in range(len(self.obs_types)):
-        obs_record = obs_line[16 * j:16 * (j + 1)]
-        obs[int(sat_map[i]), j] = floatornan(obs_record[0:14])
-        lli[int(sat_map[i]), j] = digitorzero(obs_record[14:15])
-        signal_strength[int(sat_map[i]), j] = digitorzero(obs_record[15:16])
-
-    return obs, lli, signal_strength
-
-  def _skip_obs(self, f, n_sat):
-    for i in range(n_sat):
-      for _ in range((len(self.obs_types) + 4) // 5):
-        f.readline()
-
-  def _read_data_chunk(self, f, CHUNK_SIZE=10000):
-    obss = np.empty(
-      (CHUNK_SIZE, TOTAL_SATS, len(self.obs_types)), dtype=np.float64) * np.NaN
-    llis = np.zeros((CHUNK_SIZE, TOTAL_SATS, len(self.obs_types)), dtype=np.uint8)
-    signal_strengths = np.zeros(
-      (CHUNK_SIZE, TOTAL_SATS, len(self.obs_types)), dtype=np.uint8)
-    epochs = np.zeros(CHUNK_SIZE, dtype='datetime64[us]')
-    flags = np.zeros(CHUNK_SIZE, dtype=np.uint8)
-
-    i = 0
-    while True:
-      hdr = self._read_epoch_header(f)
-      if hdr is None:
-        break
-      # data faster than desired rate: ignore it
-      if self.rate and (hdr[0].microsecond or hdr[0].second % self.rate != 0):
-        self._skip_obs(f, len(hdr[2]))
-        continue
-      epoch, flags[i], sats = hdr
-      epochs[i] = np.datetime64(epoch)
-      sat_map = np.ones(len(sats)) * -1
-      for n, sat in enumerate(sats):
-        if sat[0] == 'G':
-          sat_map[n] = int(sat[1:]) - 1
-        if sat[0] == 'R':
-          sat_map[n] = int(sat[1:]) - 1 + 64
-      obss[i], llis[i], signal_strengths[i] = self._read_obs(f, len(sats), sat_map)
-      i += 1
-      if i >= CHUNK_SIZE:
-        break
-
-    return obss[:i], llis[:i], signal_strengths[:i], epochs[:i], flags[:i]
-
-  def _read_data(self, f):
-    self.data = {}
-    while True:
-      obss, llis, signal_strengths, epochs, flags = self._read_data_chunk(f)
-      if obss.shape[0] == 0:
-        break
-
-      for i, sv in enumerate(['%02d' % d for d in range(1, TOTAL_SATS+1)]):
-        if sv not in self.data:
-          self.data[sv] = {}
-        for j, obs_type in enumerate(self.obs_types):
-          if obs_type in self.data[sv]:
-            self.data[sv][obs_type] = np.append(self.data[sv][obs_type], obss[:, i, j])
-          else:
-            self.data[sv][obs_type] = obss[:, i, j]
-        if 'Epochs' in self.data[sv]:
-          self.data[sv]['Epochs'] = np.append(self.data[sv]['Epochs'], epochs)
-        else:
-          self.data[sv]['Epochs'] = epochs
-    for sat in list(self.data.keys()):
-      if np.all(np.isnan(self.data[sat]['C1'])):
-        del self.data[sat]
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/laika/trop.py b/laika/trop.py
deleted file mode 100644
index ff224663f6..0000000000
--- a/laika/trop.py
+++ /dev/null
@@ -1,34 +0,0 @@
-#!/usr/bin/python
-
-from numpy import cos, exp, pi
-from .lib.coordinates import ecef2geodetic
-
-
-def saast(pos, el, humi=0.75, temp0=15.0):
-  """
-    Function from RTKlib: https://github.com/tomojitakasu/RTKLIB/blob/master/src/rtkcmn.c#L3362-3362
-        with no changes
-    :param time:    time
-    :param pos:     receiver position {ecef} m)
-    :param el:    azimuth/elevation angle {az,el} (rad) -- we do not use az
-    :param humi:    relative humidity
-    :param temp0:   temperature (Celsius)
-    :return:        tropospheric delay (m)
-    """
-  pos_rad = ecef2geodetic(pos, radians=True)
-  if pos_rad[2] < -1E3 or 1E4 < pos_rad[2] or el <= 0:
-    return 0.0
-
-  # /* standard atmosphere */
-  hgt = 0.0 if pos_rad[2] < 0.0 else pos_rad[2]
-
-  pres = 1013.25 * pow(1.0 - 2.2557E-5 * hgt, 5.2568)
-  temp = temp0 - 6.5E-3 * hgt + 273.16
-  e = 6.108 * humi * exp((17.15 * temp - 4684.0) / (temp - 38.45))
-
-  # /* saastamoninen model */
-  z = pi / 2.0 - el
-  trph = 0.0022768 * pres / (
-    1.0 - 0.00266 * cos(2.0 * pos_rad[0]) - 0.00028 * hgt / 1E3) / cos(z)
-  trpw = 0.002277 * (1255.0 / temp + 0.05) * e / cos(z)
-  return trph + trpw
diff --git a/opendbc/can/common.cc b/opendbc/can/common.cc
index 0df9e04b90..37b48fa6aa 100644
--- a/opendbc/can/common.cc
+++ b/opendbc/can/common.cc
@@ -29,7 +29,7 @@ unsigned int subaru_checksum(uint32_t address, const Signal &sig, const std::vec
   while (address) { s += address & 0xFF; address >>= 8; }
 
   // skip checksum in first byte
-  for (int i = 1; i < d.size(); i++) { s += d[i]; };
+  for (int i = 1; i < d.size(); i++) { s += d[i]; }
 
   return s & 0xFF;
 }
diff --git a/opendbc/can/common.h b/opendbc/can/common.h
index 0031d787f0..03b99e5598 100644
--- a/opendbc/can/common.h
+++ b/opendbc/can/common.h
@@ -76,8 +76,7 @@ public:
   uint64_t can_invalid_cnt = CAN_INVALID_CNT;
 
   CANParser(int abus, const std::string& dbc_name,
-            const std::vector<MessageParseOptions> &options,
-            const std::vector<SignalParseOptions> &sigoptions);
+            const std::vector<std::pair<uint32_t, int>> &messages);
   CANParser(int abus, const std::string& dbc_name, bool ignore_checksum, bool ignore_counter);
   #ifndef DYNAMIC_CAPNP
   void update_string(const std::string &data, bool sendcan);
diff --git a/opendbc/can/common.pxd b/opendbc/can/common.pxd
index aeb1f0a92e..477f71b2d3 100644
--- a/opendbc/can/common.pxd
+++ b/opendbc/can/common.pxd
@@ -3,6 +3,7 @@
 
 from libc.stdint cimport uint8_t, uint16_t, uint32_t, uint64_t
 from libcpp cimport bool
+from libcpp.pair cimport pair
 from libcpp.string cimport string
 from libcpp.vector cimport vector
 
@@ -48,14 +49,6 @@ cdef extern from "common_dbc.h":
     vector[Msg] msgs
     vector[Val] vals
 
-  cdef struct SignalParseOptions:
-    uint32_t address
-    string name
-
-  cdef struct MessageParseOptions:
-    uint32_t address
-    int check_frequency
-
   cdef struct SignalValue:
     uint32_t address
     uint64_t ts_nanos
@@ -74,8 +67,8 @@ cdef extern from "common.h":
   cdef cppclass CANParser:
     bool can_valid
     bool bus_timeout
-    CANParser(int, string, vector[MessageParseOptions], vector[SignalParseOptions])
-    void update_strings(vector[string]&, vector[SignalValue]&, bool)
+    CANParser(int, string, vector[pair[uint32_t, int]]) except +
+    void update_strings(vector[string]&, vector[SignalValue]&, bool) except +
 
   cdef cppclass CANPacker:
    CANPacker(string)
diff --git a/opendbc/can/common_dbc.h b/opendbc/can/common_dbc.h
index 1e0f034520..ef4c98c803 100644
--- a/opendbc/can/common_dbc.h
+++ b/opendbc/can/common_dbc.h
@@ -5,23 +5,11 @@
 #include <string>
 #include <vector>
 
-#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
-
 struct SignalPackValue {
   std::string name;
   double value;
 };
 
-struct SignalParseOptions {
-  uint32_t address;
-  std::string name;
-};
-
-struct MessageParseOptions {
-  uint32_t address;
-  int check_frequency;
-};
-
 struct SignalValue {
   uint32_t address;
   uint64_t ts_nanos;
diff --git a/opendbc/can/packer.cc b/opendbc/can/packer.cc
index ad762828bc..87fa0a8201 100644
--- a/opendbc/can/packer.cc
+++ b/opendbc/can/packer.cc
@@ -35,7 +35,7 @@ CANPacker::CANPacker(const std::string& dbc_name) {
   for (const auto& msg : dbc->msgs) {
     message_lookup[msg.address] = msg;
     for (const auto& sig : msg.sigs) {
-      signal_lookup[std::make_pair(msg.address, std::string(sig.name))] = sig;
+      signal_lookup[std::make_pair(msg.address, sig.name)] = sig;
     }
   }
   init_crc_lookup_tables();
diff --git a/opendbc/can/packer_pyx.pyx b/opendbc/can/packer_pyx.pyx
index 3304339086..5133fcc990 100644
--- a/opendbc/can/packer_pyx.pyx
+++ b/opendbc/can/packer_pyx.pyx
@@ -40,7 +40,7 @@ cdef class CANPacker:
 
   cpdef make_can_msg(self, name_or_addr, bus, values):
     cdef int addr
-    if type(name_or_addr) == int:
+    if isinstance(name_or_addr, int):
       addr = name_or_addr
     else:
       addr = self.name_to_address[name_or_addr.encode("utf8")]
diff --git a/opendbc/can/parser.cc b/opendbc/can/parser.cc
index db6d4e65e3..bf2406ec3b 100644
--- a/opendbc/can/parser.cc
+++ b/opendbc/can/parser.cc
@@ -2,6 +2,8 @@
 #include <cassert>
 #include <cstring>
 #include <limits>
+#include <stdexcept>
+#include <sstream>
 
 #include <unistd.h>
 #include <fcntl.h>
@@ -89,9 +91,7 @@ bool MessageState::update_counter_generic(int64_t v, int cnt_size) {
 }
 
 
-CANParser::CANParser(int abus, const std::string& dbc_name,
-          const std::vector<MessageParseOptions> &options,
-          const std::vector<SignalParseOptions> &sigoptions)
+CANParser::CANParser(int abus, const std::string& dbc_name, const std::vector<std::pair<uint32_t, int>> &messages)
   : bus(abus), aligned_buf(kj::heapArray<capnp::word>(1024)) {
   dbc = dbc_lookup(dbc_name);
   assert(dbc);
@@ -99,14 +99,21 @@ CANParser::CANParser(int abus, const std::string& dbc_name,
 
   bus_timeout_threshold = std::numeric_limits<uint64_t>::max();
 
-  for (const auto& op : options) {
-    MessageState &state = message_states[op.address];
-    state.address = op.address;
+  for (const auto& [address, frequency] : messages) {
+    // disallow duplicate message checks
+    if (message_states.find(address) != message_states.end()) { 
+      std::stringstream is;
+      is << "Duplicate Message Check: " << address;
+      throw std::runtime_error(is.str());
+    }
+
+    MessageState &state = message_states[address];
+    state.address = address;
     // state.check_frequency = op.check_frequency,
 
     // msg is not valid if a message isn't received for 10 consecutive steps
-    if (op.check_frequency > 0) {
-      state.check_threshold = (1000000000ULL / op.check_frequency) * 10;
+    if (frequency > 0) {
+      state.check_threshold = (1000000000ULL / frequency) * 10;
 
       // bus timeout threshold should be 10x the fastest msg
       bus_timeout_threshold = std::min(bus_timeout_threshold, state.check_threshold);
@@ -114,13 +121,13 @@ CANParser::CANParser(int abus, const std::string& dbc_name,
 
     const Msg* msg = NULL;
     for (const auto& m : dbc->msgs) {
-      if (m.address == op.address) {
+      if (m.address == address) {
         msg = &m;
         break;
       }
     }
     if (!msg) {
-      fprintf(stderr, "CANParser: could not find message 0x%X in DBC %s\n", op.address, dbc_name.c_str());
+      fprintf(stderr, "CANParser: could not find message 0x%X in DBC %s\n", address, dbc_name.c_str());
       assert(false);
     }
 
@@ -128,28 +135,10 @@ CANParser::CANParser(int abus, const std::string& dbc_name,
     state.size = msg->size;
     assert(state.size <= 64);  // max signal size is 64 bytes
 
-    // track checksums and counters for this message
-    for (const auto& sig : msg->sigs) {
-      if (sig.type != SignalType::DEFAULT) {
-        state.parse_sigs.push_back(sig);
-        state.vals.push_back(0);
-        state.all_vals.push_back({});
-      }
-    }
-
-    // track requested signals for this message
-    for (const auto& sigop : sigoptions) {
-      if (sigop.address != op.address) continue;
-
-      for (const auto& sig : msg->sigs) {
-        if (sig.name == sigop.name && sig.type == SignalType::DEFAULT) {
-          state.parse_sigs.push_back(sig);
-          state.vals.push_back(0);
-          state.all_vals.push_back({});
-          break;
-        }
-      }
-    }
+    // track all signals for this message
+    state.parse_sigs = msg->sigs;
+    state.vals.resize(msg->sigs.size());
+    state.all_vals.resize(msg->sigs.size());
   }
 }
 
diff --git a/opendbc/can/parser_pyx.pyx b/opendbc/can/parser_pyx.pyx
index f9cf87e04c..808610e87a 100644
--- a/opendbc/can/parser_pyx.pyx
+++ b/opendbc/can/parser_pyx.pyx
@@ -2,14 +2,14 @@
 # cython: c_string_encoding=ascii, language_level=3
 
 from cython.operator cimport dereference as deref, preincrement as preinc
+from libcpp.pair cimport pair
 from libcpp.string cimport string
 from libcpp.vector cimport vector
 from libcpp.unordered_set cimport unordered_set
 from libc.stdint cimport uint32_t
-from libcpp.map cimport map
 
 from .common cimport CANParser as cpp_CANParser
-from .common cimport SignalParseOptions, MessageParseOptions, dbc_lookup, SignalValue, DBC
+from .common cimport dbc_lookup, SignalValue, DBC
 
 import numbers
 from collections import defaultdict
@@ -19,7 +19,6 @@ cdef class CANParser:
   cdef:
     cpp_CANParser *can
     const DBC *dbc
-    map[uint32_t, string] address_to_msg_name
     vector[SignalValue] can_values
 
   cdef readonly:
@@ -28,10 +27,7 @@ cdef class CANParser:
     dict ts_nanos
     string dbc_name
 
-  def __init__(self, dbc_name, signals, checks=None, bus=0, enforce_checks=True):
-    if checks is None:
-      checks = []
-
+  def __init__(self, dbc_name, messages, bus=0):
     self.dbc_name = dbc_name
     self.dbc = dbc_lookup(dbc_name)
     if not self.dbc:
@@ -41,71 +37,33 @@ cdef class CANParser:
     self.vl_all = {}
     self.ts_nanos = {}
     msg_name_to_address = {}
-    msg_address_to_signals = {}
+    address_to_msg_name = {}
 
     for i in range(self.dbc[0].msgs.size()):
       msg = self.dbc[0].msgs[i]
       name = msg.name.decode("utf8")
 
       msg_name_to_address[name] = msg.address
-      msg_address_to_signals[msg.address] = set()
-      for sig in msg.sigs:
-        msg_address_to_signals[msg.address].add(sig.name.decode("utf8"))
-
-      self.address_to_msg_name[msg.address] = name
-      self.vl[msg.address] = {}
-      self.vl[name] = self.vl[msg.address]
-      self.vl_all[msg.address] = {}
-      self.vl_all[name] = self.vl_all[msg.address]
-      self.ts_nanos[msg.address] = {}
-      self.ts_nanos[name] = self.ts_nanos[msg.address]
-
-    # Convert message names into addresses
-    for i in range(len(signals)):
-      s = signals[i]
-      address = s[1] if isinstance(s[1], numbers.Number) else msg_name_to_address.get(s[1])
-      if address not in msg_address_to_signals:
-        raise RuntimeError(f"could not find message {repr(s[1])} in DBC {self.dbc_name}")
-      if s[0] not in msg_address_to_signals[address]:
-        raise RuntimeError(f"could not find signal {repr(s[0])} in {repr(s[1])}, DBC {self.dbc_name}")
-
-      signals[i] = (s[0], address)
-
-    for i in range(len(checks)):
-      c = checks[i]
-      if not isinstance(c[0], numbers.Number):
-        if c[0] not in msg_name_to_address:
-          print(msg_name_to_address)
-          raise RuntimeError(f"could not find message {repr(c[0])} in DBC {self.dbc_name}")
-        c = (msg_name_to_address[c[0]], c[1])
-        checks[i] = c
-
-    if enforce_checks:
-      checked_addrs = {c[0] for c in checks}
-      signal_addrs = {s[1] for s in signals}
-      unchecked = signal_addrs - checked_addrs
-      if len(unchecked):
-        err_msg = ", ".join(f"{self.address_to_msg_name[addr].decode()} ({hex(addr)})" for addr in unchecked)
-        raise RuntimeError(f"Unchecked addrs: {err_msg}")
-
-    cdef vector[SignalParseOptions] signal_options_v
-    cdef SignalParseOptions spo
-    for sig_name, sig_address in signals:
-      spo.address = sig_address
-      spo.name = sig_name
-      signal_options_v.push_back(spo)
-
-    message_options = dict((address, 0) for _, address in signals)
-    message_options.update(dict(checks))
-
-    cdef vector[MessageParseOptions] message_options_v
-    cdef MessageParseOptions mpo
-    for msg_address, freq in message_options.items():
-      mpo.address = msg_address
-      mpo.check_frequency = freq
-      message_options_v.push_back(mpo)
-
-    self.can = new cpp_CANParser(bus, dbc_name, message_options_v, signal_options_v)
+      address_to_msg_name[msg.address] = name
+
+    # Convert message names into addresses and check existence in DBC
+    cdef vector[pair[uint32_t, int]] message_v
+    for i in range(len(messages)):
+      c = messages[i]
+      address = c[0] if isinstance(c[0], numbers.Number) else msg_name_to_address.get(c[0])
+      if address not in address_to_msg_name:
+        raise RuntimeError(f"could not find message {repr(c[0])} in DBC {self.dbc_name}")
+      message_v.push_back((address, c[1]))
+
+      name = address_to_msg_name[address]
+      self.vl[address] = {}
+      self.vl[name] = self.vl[address]
+      self.vl_all[address] = {}
+      self.vl_all[name] = self.vl_all[address]
+      self.ts_nanos[address] = {}
+      self.ts_nanos[name] = self.ts_nanos[address]
+
+    self.can = new cpp_CANParser(bus, dbc_name, message_v)
     self.update_strings([])
 
   def update_strings(self, strings, sendcan=False):
diff --git a/opendbc/gm_global_a_powertrain_generated.dbc b/opendbc/gm_global_a_powertrain_generated.dbc
index d83f035dc9..6ecb36f2fa 100644
--- a/opendbc/gm_global_a_powertrain_generated.dbc
+++ b/opendbc/gm_global_a_powertrain_generated.dbc
@@ -176,12 +176,13 @@ BO_ 489 EBCMVehicleDynamic: 8 K17_EBCM
  SG_ YawRate : 35|12@0- (0.625,0) [0|1] "" NEO
  SG_ YawRate2 : 51|12@0- (0.0625,0) [-2047|2047] "grad/s" NEO
 
-BO_ 352 VehicleIgnition: 5 XXX
- SG_ Ignition : 7|32@0+ (1,0) [0|4294967295] "" XXX
+BO_ 352 BCMImmobilizer: 5 K9_BCM
+ SG_ ImmobilizerInfo : 7|32@0+ (1,0) [0|4294967295] "" XXX
 
-BO_ 497 VehicleIgnitionAlt: 8 XXX
- SG_ Ignition : 5|1@0+ (1,0) [0|1] "" XXX
- SG_ ParkBrake : 36|1@0+ (1,0) [0|3] "" XXX
+BO_ 497 BCMGeneralPlatformStatus: 8 K9_BCM
+ SG_ SystemPowerMode : 1|2@0+ (1,0) [0|3] "" XXX
+ SG_ SystemBackUpPowerMode : 5|2@0+ (1,0) [0|3] "" XXX
+ SG_ ParkBrakeSwActive : 36|1@0+ (1,0) [0|3] "" XXX
 
 BO_ 501 ECMPRDNL2: 8 K20_ECM
  SG_ TransmissionState : 48|4@1+ (1,0) [0|7] "" NEO
@@ -194,7 +195,7 @@ BO_ 532 BRAKE_RELATED: 6 XXX
 BO_ 560 EPBStatus: 8 EPB
  SG_ EPBClosed : 12|1@0+ (1,0) [0|1] ""  NEO
 
-BO_ 562 EBCMFrictionBrakeStatus: 8 XXX
+BO_ 562 EBCMFrictionBrakeStatus: 8 K17_EBCM
  SG_ FrictionBrakeUnavailable : 46|1@0+ (1,0) [0|1] "" XXX
 
 BO_ 608 SPEED_RELATED: 8 XXX
@@ -316,11 +317,12 @@ CM_ BU_ K124_ASCM "Active Safety Control Module";
 CM_ SG_ 381 MSG17D_AccPower "Need to investigate";
 CM_ BO_ 190 "Length varies from 6 to 8 bytes by car";
 CM_ SG_ 190 GasPedalAndAcc "ACC baseline is 62";
-CM_ SG_ 352 Ignition "Non-zero when ignition is on";
+CM_ SG_ 352 ImmobilizerInfo "Non-zero when ignition or accessory mode";
 CM_ SG_ 451 GasPedalAndAcc2 "ACC baseline is 62";
 CM_ SG_ 481 ACCAlwaysOne "Usually 1 if the car is equipped with ACC";
 CM_ SG_ 562 FrictionBrakeUnavailable "1 when ACC brake control is unavailable. Stays high if brake command messages are blocked for a period of time";
-CM_ SG_ 497 Ignition "Describes ignition + preconditioning mode, noisy";
+CM_ SG_ 497 SystemPowerMode "Describes ignition";
+CM_ SG_ 497 SystemBackUpPowerMode "Describes ignition + preconditioning mode, noisy";
 CM_ SG_ 501 PRNDL2 "When ManualMode is Active, Value is 13=L1 12=L2 11=L3 ... 4=L10";
 CM_ SG_ 532 UserBrakePressure "can be lower than other brake position signals when the brakes are pre-filled from ACC braking and the user presses on the brakes. user-only pressure?";
 CM_ SG_ 608 ClusterSpeed "Cluster speed signal seems to match dash on newer cars, but is a lower rate and can be noisier.";
@@ -335,6 +337,8 @@ BA_DEF_DEF_  "BusType" "";
 BA_ "BusType" "CAN";
 BA_ "ProtocolType" "GMLAN";
 BA_ "UseGMParameterIDs" 0;
+VAL_ 497 SystemPowerMode 3 "Crank Request" 2 "Run" 1 "Accessory" 0 "Off";
+VAL_ 497 SystemBackUpPowerMode 3 "Crank Request" 2 "Run" 1 "Accessory" 0 "Off";
 VAL_ 481 DistanceButton 1 "Active" 0 "Inactive" ;
 VAL_ 481 LKAButton 1 "Active" 0 "Inactive" ;
 VAL_ 481 ACCButtons 6 "Cancel" 5 "Main" 3 "Set" 2 "Resume" 1 "None" ;
diff --git a/opendbc/hyundai_canfd.dbc b/opendbc/hyundai_canfd.dbc
index 2ce413c09d..8d27594528 100644
--- a/opendbc/hyundai_canfd.dbc
+++ b/opendbc/hyundai_canfd.dbc
@@ -65,7 +65,7 @@ BO_ 80 LKAS: 16 XXX
  SG_ LKA_ASSIST : 62|1@1+ (1,0) [0|1] "" XXX
  SG_ LKA_MODE : 24|3@1+ (1,0) [0|7] "" XXX
  SG_ NEW_SIGNAL_2 : 70|2@0+ (1,0) [0|3] "" XXX
- SG_ SET_ME_1 : 80|1@0+ (1,0) [0|1] "" XXX
+ SG_ HAS_LANE_SAFETY : 80|1@0+ (1,0) [0|1] "" XXX
  SG_ NEW_SIGNAL_3 : 111|8@0+ (1,0) [0|255] "" XXX
  SG_ FCA_SYSWARN : 40|1@0+ (1,0) [0|1] "" XXX
 
@@ -98,10 +98,10 @@ BO_ 160 WHEEL_SPEEDS: 24 XXX
  SG_ MOVING_BACKWARD : 57|1@0+ (1,0) [0|1] "" XXX
  SG_ MOVING_FORWARD2 : 58|1@0+ (1,0) [0|1] "" XXX
  SG_ MOVING_BACKWARD2 : 59|1@0+ (1,0) [0|1] "" XXX
- SG_ WHEEL_SPEED_1 : 64|16@1+ (0.03125,0) [0|65535] "m/s" XXX
- SG_ WHEEL_SPEED_2 : 80|16@1+ (0.03125,0) [0|65535] "m/s" XXX
- SG_ WHEEL_SPEED_3 : 96|16@1+ (0.03125,0) [0|65535] "m/s" XXX
- SG_ WHEEL_SPEED_4 : 112|16@1+ (0.03125,0) [0|65535] "m/s" XXX
+ SG_ WHEEL_SPEED_1 : 64|16@1+ (0.03125,0) [0|65535] "kph" XXX
+ SG_ WHEEL_SPEED_2 : 80|16@1+ (0.03125,0) [0|65535] "kph" XXX
+ SG_ WHEEL_SPEED_3 : 96|16@1+ (0.03125,0) [0|65535] "kph" XXX
+ SG_ WHEEL_SPEED_4 : 112|16@1+ (0.03125,0) [0|65535] "kph" XXX
 
 BO_ 234 MDPS: 24 XXX
  SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
@@ -124,6 +124,23 @@ BO_ 261 ACCELERATOR_ALT: 32 XXX
  SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
  SG_ ACCELERATOR_PEDAL : 103|10@1+ (0.25,0) [0|1022] "" XXX
 
+BO_ 272 LKAS_ALT: 32 XXX
+ SG_ STEER_REQ : 52|1@1+ (1,0) [0|1] "" XXX
+ SG_ TORQUE_REQUEST : 41|11@1+ (1,-1024) [0|4095] "" XXX
+ SG_ LKA_ICON : 38|2@1+ (1,0) [0|255] "" XXX
+ SG_ NEW_SIGNAL_1 : 27|2@1+ (1,0) [0|255] "" XXX
+ SG_ LFA_BUTTON : 56|1@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
+ SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
+ SG_ STEER_MODE : 65|3@1+ (1,0) [0|1] "" XXX
+ SG_ LKA_WARNING : 32|1@1+ (1,0) [0|1] "" XXX
+ SG_ LKA_ASSIST : 62|1@1+ (1,0) [0|1] "" XXX
+ SG_ LKA_MODE : 24|3@1+ (1,0) [0|7] "" XXX
+ SG_ NEW_SIGNAL_2 : 70|2@0+ (1,0) [0|3] "" XXX
+ SG_ HAS_LANE_SAFETY : 80|1@0+ (1,0) [0|1] "" XXX
+ SG_ NEW_SIGNAL_3 : 111|8@0+ (1,0) [0|255] "" XXX
+ SG_ FCA_SYSWARN : 40|1@0+ (1,0) [0|1] "" XXX
+
 BO_ 293 STEERING_SENSORS: 16 XXX
  SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
  SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
@@ -143,7 +160,7 @@ BO_ 298 LFA: 16 ADRV
  SG_ LKA_ASSIST : 62|1@1+ (1,0) [0|1] "" XXX
  SG_ LKA_MODE : 24|3@1+ (1,0) [0|7] "" XXX
  SG_ NEW_SIGNAL_2 : 70|2@0+ (1,0) [0|3] "" XXX
- SG_ SET_ME_1 : 80|1@0+ (1,0) [0|1] "" XXX
+ SG_ HAS_LANE_SAFETY : 80|1@0+ (1,0) [0|1] "" XXX
  SG_ NEW_SIGNAL_3 : 111|8@0+ (1,0) [0|255] "" XXX
 
 BO_ 304 GEAR_SHIFTER: 16 XXX
@@ -453,7 +470,10 @@ BO_ 676 CAM_0x2a4: 24 XXX
  SG_ BYTE4 : 32|8@1+ (1,0) [0|255] "" XXX
  SG_ BYTE5 : 40|8@1+ (1,0) [0|255] "" XXX
  SG_ BYTE6 : 48|8@1+ (1,0) [0|255] "" XXX
- SG_ BYTE7 : 56|8@1+ (1,0) [0|255] "" XXX
+ SG_ LEFT_LANE_LINE : 56|2@1+ (1,0) [0|3] "" XXX
+ SG_ SET_ME_0 : 58|2@1+ (1,0) [0|3] "" XXX
+ SG_ RIGHT_LANE_LINE : 60|2@1+ (1,0) [0|3] "" XXX
+ SG_ SET_ME_0_2 : 62|2@1+ (1,0) [0|3] "" XXX
  SG_ BYTE8 : 64|8@1+ (1,0) [0|255] "" XXX
  SG_ BYTE9 : 72|8@1+ (1,0) [0|255] "" XXX
  SG_ BYTE10 : 80|8@1+ (1,0) [0|255] "" XXX
@@ -487,11 +507,55 @@ BO_ 702 CAM_0x2be: 32 CAMERA
  SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
  SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
 
+BO_ 736 MANUAL_SPEED_LIMIT_ASSIST: 32 XXX
+ SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
+ SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
+ SG_ MSLA_STATUS : 26|2@1+ (1,0) [0|3] "" XXX
+ SG_ MSLA_ENABLED : 38|1@1+ (1,0) [0|1] "" XXX
+ SG_ MAX_SPEED : 55|8@0+ (1,0) [0|255] "" XXX
+ SG_ MAX_SPEED_COPY : 144|8@1+ (1,0) [0|255] "" XXX
+
 BO_ 837 ADRV_0x345: 8 ADRV
  SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
  SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
  SG_ SET_ME_15 : 24|8@1+ (1,0) [0|255] "" XXX
 
+BO_ 866 CAM_0x362: 32 CAMERA
+ SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
+ SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE3 : 24|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE4 : 32|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE5 : 40|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE6 : 48|8@1+ (1,0) [0|255] "" XXX
+ SG_ LEFT_LANE_LINE : 56|2@1+ (1,0) [0|3] "" XXX
+ SG_ SET_ME_0 : 58|2@1+ (1,0) [0|3] "" XXX
+ SG_ RIGHT_LANE_LINE : 60|2@1+ (1,0) [0|3] "" XXX
+ SG_ SET_ME_0_2 : 62|2@1+ (1,0) [0|3] "" XXX
+ SG_ BYTE8 : 64|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE9 : 72|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE10 : 80|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE11 : 88|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE12 : 96|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE13 : 104|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE14 : 112|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE15 : 120|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE16 : 128|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE17 : 136|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE18 : 144|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE19 : 152|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE20 : 160|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE21 : 168|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE22 : 176|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE23 : 184|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE24 : 192|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE25 : 200|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE26 : 208|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE27 : 216|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE28 : 224|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE29 : 232|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE30 : 240|8@1+ (1,0) [0|255] "" XXX
+ SG_ BYTE31 : 248|8@1+ (1,0) [0|255] "" XXX
+
 BO_ 961 BLINKER_STALKS: 8 XXX
  SG_ COUNTER_ALT : 15|4@0+ (1,0) [0|15] "" XXX
  SG_ CHECKSUM_MAYBE : 7|8@0+ (1,0) [0|255] "" XXX
@@ -512,9 +576,14 @@ BO_ 1041 DOORS_SEATBELTS: 8 XXX
  SG_ PASSENGER_SEATBELT : 36|1@0+ (1,0) [0|1] "" XXX
 
 BO_ 1043 BLINKERS: 8 XXX
+ SG_ LEFT_STALK : 8|1@0+ (1,0) [0|1] "" XXX
+ SG_ RIGHT_STALK : 10|1@0+ (1,0) [0|1] "" XXX
  SG_ COUNTER_ALT : 15|4@0+ (1,0) [0|15] "" XXX
  SG_ LEFT_LAMP : 20|1@0+ (1,0) [0|1] "" XXX
  SG_ RIGHT_LAMP : 22|1@0+ (1,0) [0|1] "" XXX
+ SG_ LEFT_LAMP_ALT : 59|1@0+ (1,0) [0|1] "" XXX
+ SG_ RIGHT_LAMP_ALT : 61|1@0+ (1,0) [0|1] "" XXX
+ SG_ USE_ALT_LAMP : 62|1@0+ (1,0) [0|1] "" XXX
 
 BO_ 1240 CLUSTER_INFO: 8 XXX
  SG_ DISTANCE_UNIT : 0|1@1+ (1,0) [0|1] "" XXX
@@ -589,7 +658,12 @@ BO_ 1264 LOCAL_TIME: 8 XXX
  SG_ MINUTES : 21|6@0+ (1,0) [0|63] "" XXX
  SG_ SECONDS : 31|8@0+ (1,0) [0|59] "" XXX
 
+CM_ 272 "Alternative LKAS message, used on cars such as 2023 Ioniq 6, 2nd gen Kona. Matches LKAS except size is 32 bytes";
+CM_ 676 "Contains signals with detailed lane line information. Used by ADAS ECU on HDA 2 vehicles to operate LFA.";
+CM_ 866 "Contains signals with detailed lane line information. Used by ADAS ECU on HDA 2 vehicles to operate LFA. Used on cars that use message 272.";
+CM_ 1043 "Lamp signals do not seem universal on cars that use LKAS_ALT, but stalk signals do.";
 
+CM_ SG_ 80 HAS_LANE_SAFETY "If 0, hides LKAS 'Lane Safety' menu from vehicle settings";
 CM_ SG_ 96 BRAKE_PRESSURE "User applied brake pedal pressure. Ramps from computer applied pressure on falling edge of cruise. Cruise cancels if !=0";
 CM_ SG_ 101 BRAKE_POSITION "User applied brake pedal position, max is ~700. Signed on some vehicles";
 CM_ SG_ 373 PROBABLY_EQUIP "aeb equip?";
@@ -598,9 +672,15 @@ CM_ SG_ 373 DriverBraking "Likely derived from BRAKE->BRAKE_POSITION";
 CM_ SG_ 373 DriverBrakingLowSens "Higher threshold version of DriverBraking";
 CM_ SG_ 352 SET_ME_9 "has something to do with AEB settings";
 CM_ SG_ 416 VSetDis "set speed in display units";
+CM_ SG_ 676 LEFT_LANE_LINE "Left lane line confidence";
+CM_ SG_ 676 RIGHT_LANE_LINE "Right lane line confidence";
+CM_ SG_ 736 MAX_SPEED "Display units. Restricts car from driving above this speed unless accelerator pedal is depressed beyond pressure point";
+CM_ SG_ 866 LEFT_LANE_LINE "Left lane line confidence";
+CM_ SG_ 866 RIGHT_LANE_LINE "Right lane line confidence";
 CM_ SG_ 961 COUNTER_ALT "only increments on change";
 CM_ SG_ 1041 COUNTER_ALT "only increments on change";
 CM_ SG_ 1043 COUNTER_ALT "only increments on change";
+CM_ SG_ 1043 USE_ALT_LAMP "likely 1 on cars that use alt lamp signals";
 VAL_ 53 GEAR 0 "P" 5 "D" 6 "N" 7 "R" ;
 VAL_ 64 GEAR 0 "P" 5 "D" 6 "N" 7 "R" ;
 VAL_ 69 GEAR 0 "P" 5 "D" 6 "N" 7 "R" ;
@@ -609,6 +689,8 @@ VAL_ 80 LKA_ICON 0 "hidden" 1 "grey" 2 "green" 3 "flashing green" ;
 VAL_ 80 LKA_MODE 1 "warning only" 2 "assist" 6 "off" ;
 VAL_ 96 TRACTION_AND_STABILITY_CONTROL 0 "On" 5 "Limited" 1 "Off";
 VAL_ 234 LKA_FAULT 0 "ok" 1 "lka fault" ;
+VAL_ 272 LKA_ICON 0 "hidden" 1 "grey" 2 "green" 3 "flashing green" ;
+VAL_ 272 LKA_MODE 1 "warning only" 2 "assist" 6 "off" ;
 VAL_ 298 LKA_ICON 0 "hidden" 1 "grey" 2 "green" 3 "flashing green" ;
 VAL_ 298 LKA_MODE 1 "warning only" 2 "assist" 6 "off" ;
 VAL_ 304 PARK_BUTTON 1 "Pressed" 2 "Not Pressed";
@@ -622,6 +704,11 @@ VAL_ 426 CRUISE_BUTTONS 0 "none" 1 "res_accel" 2 "set_decel" 3 "gap_distance" 4
 VAL_ 463 CRUISE_BUTTONS 0 "none" 1 "res_accel" 2 "set_decel" 3 "gap_distance" 4 "pause_resume" ;
 VAL_ 463 RIGHT_PADDLE 0 "Not Pulled" 1 "Pulled";
 VAL_ 463 LEFT_PADDLE 0 "Not Pulled" 1 "Pulled";
+VAL_ 676 LEFT_LANE_LINE 0 "Not Detected" 1 "Low Confidence" 2 "Medium Confidence" 3 "High Confidence";
+VAL_ 676 RIGHT_LANE_LINE 0 "Not Detected" 1 "Low Confidence" 2 "Medium Confidence" 3 "High Confidence";
+VAL_ 736 MSLA_STATUS 0 "disabled" 1 "active" 2 "paused";
+VAL_ 866 LEFT_LANE_LINE 0 "Not Detected" 1 "Low Confidence" 2 "Medium Confidence" 3 "High Confidence";
+VAL_ 866 RIGHT_LANE_LINE 0 "Not Detected" 1 "Low Confidence" 2 "Medium Confidence" 3 "High Confidence";
 VAL_ 1041 DRIVER_DOOR 0 "Closed" 1 "Opened";
 VAL_ 1041 PASSENGER_DOOR 0 "Closed" 1 "Opened";
 VAL_ 1041 DRIVER_REAR_DOOR 0 "Closed" 1 "Opened";
diff --git a/opendbc/hyundai_kia_generic.dbc b/opendbc/hyundai_kia_generic.dbc
index f1af8f9d2e..3031c4bc22 100644
--- a/opendbc/hyundai_kia_generic.dbc
+++ b/opendbc/hyundai_kia_generic.dbc
@@ -1645,6 +1645,7 @@ BO_ 1348 Navi_HU: 8 XXX
 
 CM_ "BO_ E_EMS11: All (plug-in) hybrids use this gas signal: CR_Vcu_AccPedDep_Pos, and all EVs use the Accel_Pedal_Pos signal. See hyundai/values.py for a specific car list";
 CM_ SG_ 871 CF_Lvr_IsgState "Idle Stop and Go";
+CM_ SG_ 1056 SCCInfoDisplay "Goes to 1 for a second while transitioning from Cruise Control to No Message";
 CM_ SG_ 1348 SpeedLim_Nav_Clu "Speed limit displayed on Nav, Cluster and HUD";
 
 VAL_ 274 CUR_GR 1 "D" 2 "D" 3 "D" 4 "D" 5 "D" 6 "D" 7 "D" 8 "D" 14 "R" 0 "P";
@@ -1654,6 +1655,7 @@ VAL_ 882 Elect_Gear_Shifter 5 "D" 8 "S" 6 "N" 7 "R" 0 "P";
 VAL_ 905 ACCMode 0 "off" 1 "enabled" 2 "driver_override" 3 "off_maybe_fault" 4 "cancelled";
 VAL_ 909 CF_VSM_Warn 2 "FCW" 3 "AEB";
 VAL_ 916 ACCEnable 0 "SCC ready" 1 "SCC temp fault" 2 "SCC permanent fault" 3 "SCC permanent fault, communication issue";
+VAL_ 1056 SCCInfoDisplay 0 "No Message" 2 "Cruise Control" 3 "Lost Lead" 4 "Standstill";
 VAL_ 1057 ACCMode 0 "off" 1 "enabled" 2 "driver_override" 3 "off_maybe_fault";
 VAL_ 1157 HDA_Icon_State 0 "no_hda" 1 "white_hda" 2 "green_hda";
 VAL_ 1157 LFA_SysWarning 0 "no_message" 1 "switching_to_hda" 2 "switching_to_scc" 3 "lfa_error" 4 "check_hda" 5 "keep_hands_on_wheel_orange" 6 "keep_hands_on_wheel_red";
diff --git a/opendbc/mazda_2017.dbc b/opendbc/mazda_2017.dbc
index 339894659a..d2bb77b351 100644
--- a/opendbc/mazda_2017.dbc
+++ b/opendbc/mazda_2017.dbc
@@ -209,6 +209,8 @@ BO_ 1088 CAM_LANEINFO: 8 XXX
  SG_ LINE_VISIBLE : 0|1@0+ (1,0) [0|3] "" XXX
  SG_ LDW_WARN_RL : 58|1@0+ (1,0) [0|1] "" XXX
  SG_ LDW_WARN_LL : 57|1@0+ (1,0) [0|1] "" XXX
+ SG_ TJA : 38|3@0+ (1,0) [0|7] "" XXX
+ SG_ TJA_TRANSITION : 27|2@0+ (1,0) [0|63] "" XXX
 
 BO_ 1479 NEW_MSG_470: 8 XXX
 
@@ -554,6 +556,8 @@ BO_ 535 CURVE_CTRS: 8 XXX
 
 BO_ 540 CRZ_CTRL: 8 XXX
  SG_ NEW_SIGNAL_6 : 10|1@0+ (1,0) [0|1] "" XXX
+ SG_ NEW_SIGNAL_9 : 31|1@0+ (1,0) [0|255] "" XXX
+ SG_ ACC_GAS_MAYBE2 : 29|1@0+ (1,0) [0|1] "" XXX
  SG_ HANDS_OFF_STEERING : 48|1@0+ (1,0) [0|1] "" XXX
  SG_ HANDS_ON_STEER_WARN : 59|4@0+ (1,0) [0|255] "" XXX
  SG_ CRZ_ACTIVE : 3|1@0+ (1,0) [0|1] "" XXX
@@ -561,7 +565,9 @@ BO_ 540 CRZ_CTRL: 8 XXX
  SG_ DISTANCE_SETTING : 20|3@0+ (1,0) [0|7] "" XXX
  SG_ MSG_1_INV : 1|1@0+ (1,0) [0|1] "" XXX
  SG_ MSG_1_COPY : 9|1@0+ (1,0) [0|1] "" XXX
+ SG_ ACC_GAS_MAYBE : 23|1@0+ (1,0) [0|31] "" XXX
  SG_ ACC_ACTIVE_2 : 52|1@0+ (1,0) [0|1] "" XXX
+ SG_ NEW_SIGNAL_10 : 30|1@0+ (1,0) [0|1] "" XXX
  SG_ MSG_1 : 0|1@0+ (1,0) [0|3] "" XXX
  SG_ 5_SEC_DISABLE_TIMER : 45|3@0+ (1,0) [0|7] "" XXX
  SG_ NEW_SIGNAL_3 : 13|1@0+ (1,0) [0|3] "" XXX
@@ -701,6 +707,12 @@ BO_ 1143 BSM: 8 XXX
  SG_ NEW_SIGNAL_1 : 32|1@0+ (1,0) [0|1] "" XXX
  SG_ REAR_CT_ALERT : 23|5@0+ (1,0) [0|63] "" XXX
 
+BO_ 480 ACCEL_TEST: 8 XXX
+ SG_ ACCEL_COMMAND : 7|32@0- (1,0) [-2147483647|2147483647] "" XXX
+ SG_ ENABLED : 32|1@0+ (1,0) [0|1] "" XXX
+ SG_ STARTING : 40|1@0+ (1,0) [0|1] "" XXX
+ SG_ STOPPING : 48|1@0+ (1,0) [0|1] "" XXX
+
 BO_ 1361 KEY_POSITION: 8 XXX
 
 BO_ 1283 KEY_POSITION2: 8 XXX
@@ -760,6 +772,8 @@ CM_ SG_ 1157 LKAS_SENSETIVITY "0 low, 1 high";
 CM_ SG_ 1157 LANEE_DEPARTURE_ALERT "1 off, 2 on";
 CM_ SG_ 1157 WARNING "1 Rare, 0 often";
 CM_ SG_ 1088 LANE_LINES "0 LKAS disabled, 1 no lines, 2 two lines, 3 left line, 4 right line";
+CM_ SG_ 1088 TJA "2: crz not active,  3: TJA not allowed,  4: TJA allowed";
+CM_ SG_ 1088 TJA_TRANSITION "3: if TJA signal is 3, otherwise set to 0";
 CM_ SG_ 1045 ABS_MALFUNCTION "off: 0, solid: 1, slow blink: 2, fast blink: 3";
 CM_ SG_ 120 VEHICLE_ACC_X "Vehicle acceleration of X-axis wrt. NED frame.";
 CM_ SG_ 120 VEHICLE_ACC_Y "Vehicle acceleration of Y-axis wrt. NED frame.";
diff --git a/opendbc/subaru_forester_2017_generated.dbc b/opendbc/subaru_forester_2017_generated.dbc
index 1455c691e9..3ba563e817 100644
--- a/opendbc/subaru_forester_2017_generated.dbc
+++ b/opendbc/subaru_forester_2017_generated.dbc
@@ -127,10 +127,11 @@ BO_ 338 Stalk: 8 XXX
 
 BO_ 352 ES_Brake: 8 XXX
  SG_ Brake_Pressure : 0|16@1+ (1,0) [0|255] "" XXX
- SG_ Brake_Light : 20|1@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_Brake_Lights : 20|1@1+ (1,0) [0|1] "" XXX
  SG_ Cruise_Fault : 21|1@1+ (1,0) [0|1] "" XXX
- SG_ Brake_On : 22|1@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_Brake_Active : 22|1@1+ (1,0) [0|1] "" XXX
  SG_ Cruise_Activated : 23|1@1+ (1,0) [0|1] "" XXX
+ SG_ SET_1 : 45|1@0+ (1,0) [0|1] "" XXX
  SG_ COUNTER : 48|3@1+ (1,0) [0|7] "" XXX
  SG_ Checksum : 56|8@1+ (1,0) [0|255] "" XXX
 
@@ -139,7 +140,7 @@ BO_ 353 ES_Distance: 8 XXX
  SG_ Signal1 : 12|4@1+ (1,0) [0|15] "" XXX
  SG_ Car_Follow : 16|1@1+ (1,0) [0|1] "" XXX
  SG_ Signal2 : 17|3@1+ (1,0) [0|7] "" XXX
- SG_ Brake_On : 20|1@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_Brake_Active : 20|1@1+ (1,0) [0|1] "" XXX
  SG_ Distance_Swap : 21|1@1+ (1,0) [0|1] "" XXX
  SG_ Standstill : 22|1@1+ (1,0) [0|1] "" XXX
  SG_ Signal3 : 23|1@1+ (1,0) [0|1] "" XXX
@@ -154,10 +155,10 @@ BO_ 353 ES_Distance: 8 XXX
  SG_ Signal7 : 51|5@1+ (1,0) [0|31] "" XXX
  SG_ Checksum : 56|8@1+ (1,0) [0|255] "" XXX
 
-BO_ 354 ES_RPM: 8 XXX
+BO_ 354 ES_Status: 8 XXX
  SG_ Brake : 8|1@1+ (1,0) [0|1] "" XXX
  SG_ Cruise_Activated : 9|1@1+ (1,0) [0|1] "" XXX
- SG_ RPM : 16|16@1+ (1,0) [0|65535] "" XXX
+ SG_ Cruise_RPM : 16|16@1+ (1,0) [0|65535] "" XXX
  SG_ Checksum : 32|8@1+ (1,0) [0|255] "" XXX
  SG_ COUNTER : 48|3@1+ (1,0) [0|7] "" XXX
 
diff --git a/opendbc/subaru_global_2017_generated.dbc b/opendbc/subaru_global_2017_generated.dbc
index 75f15ea946..319764fc9b 100644
--- a/opendbc/subaru_global_2017_generated.dbc
+++ b/opendbc/subaru_global_2017_generated.dbc
@@ -140,11 +140,11 @@ BO_ 290 ES_LKAS: 8 XXX
  SG_ LKAS_Output : 16|13@1- (-1,0) [-8191|8191] "" XXX
  SG_ LKAS_Request : 29|1@0+ (1,0) [0|1] "" XXX
 
-BO_ 292 ES_LKAS_ALT: 8 XXX
+BO_ 292 ES_LKAS_ANGLE: 8 XXX
  SG_ CHECKSUM : 0|8@1+ (1,0) [0|1] "" XXX
  SG_ COUNTER : 8|4@1+ (1,0) [0|1] "" XXX
  SG_ LKAS_Request : 12|1@1+ (1,0) [0|1] "" XXX
- SG_ LKAS_Output : 40|17@1- (-1,0) [0|1] "" XXX
+ SG_ LKAS_Output : 40|17@1- (-0.01,0) [0|1] "deg" XXX
  SG_ SET_3 : 60|2@1+ (1,0) [0|1] "" XXX
 
 BO_ 544 ES_Brake: 8 XXX
@@ -162,9 +162,9 @@ BO_ 544 ES_Brake: 8 XXX
 BO_ 577 Cruise_Status: 8 XXX
  SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
  SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ Cruise_Set_Speed : 51|12@0+ (1,0) [0|120] "" XXX
  SG_ Cruise_On : 54|1@1+ (1,0) [0|1] "" XXX
  SG_ Cruise_Activated : 55|1@1+ (1,0) [0|1] "" XXX
- SG_ Cruise_Active : 57|4@1+ (1,0) [0|15] "" XXX
 
 BO_ 552 BSD_RCTA: 8 XXX
  SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
@@ -246,6 +246,14 @@ BO_ 802 ES_LKAS_State: 8 XXX
  SG_ LKAS_Alert : 32|5@1+ (1,0) [0|31] "" XXX
  SG_ Signal3 : 37|27@1+ (1,0) [0|1] "" XXX
 
+BO_ 803 ES_Infotainment: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ LKAS_Blue_Lines : 15|4@0+ (1,0) [0|15] "" XXX
+ SG_ Signal1 : 19|4@0+ (1,0) [0|15] "" XXX
+ SG_ LKAS_State_Infotainment : 22|3@0+ (1,0) [0|7] "" XXX
+ SG_ Signal2 : 24|1@0+ (1,0) [0|1] "" XXX
+
 BO_ 722 AC_State: 8 XXX
  SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
  SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
@@ -257,6 +265,21 @@ BO_ 1677 Dash_State: 8 XXX
  SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
  SG_ Units : 29|3@1+ (1,0) [0|7] "" XXX
 
+BO_ 554 ES_HighBeamAssist: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ HBA_Available : 13|1@0+ (1,0) [0|1] "" XXX
+
+BO_ 805 ES_STATIC_1: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ SET_3 : 23|2@0+ (1,0) [0|3] "" XXX
+
+BO_ 289 ES_STATIC_2: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ SET_3 : 15|2@1+ (1,0) [0|3] "" XXX
+
 CM_ SG_ 64 Throttle_Combo "Throttle Cruise + Pedal";
 CM_ SG_ 313 Brake_Lights "Driver or Cruise Brake on";
 CM_ SG_ 544 Cruise_Brake_Lights "1 = switch on brake lights";
@@ -287,7 +310,7 @@ BO_ 72 Transmission: 8 XXX
  SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
  SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
  SG_ Gear : 24|8@1+ (1,0) [0|255] "" XXX
- SG_ RPM : 40|16@1+ (1,0) [0|65535] "" XXX
+ SG_ RPM : 40|15@1+ (1,0) [0|65535] "" XXX
 
 BO_ 73 CVT: 8 XXX
  SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
@@ -302,7 +325,7 @@ BO_ 545 ES_Distance: 8 XXX
  SG_ Cruise_Throttle : 16|12@1+ (1,0) [0|4095] "" XXX
  SG_ Signal2 : 28|4@1+ (1,0) [0|15] "" XXX
  SG_ Car_Follow : 32|1@1+ (1,0) [0|1] "" XXX
- SG_ Signal3 : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ Low_Speed_Follow : 33|1@1+ (1,0) [0|1] "" XXX
  SG_ Cruise_Soft_Disable : 34|1@1+ (1,0) [0|1] "" XXX
  SG_ Signal7 : 35|1@1+ (1,0) [0|1] "" XXX
  SG_ Cruise_Brake_Active : 36|1@1+ (1,0) [0|1] "" XXX
@@ -336,14 +359,6 @@ BO_ 576 CruiseControl: 8 XXX
  SG_ Cruise_Activated : 41|1@1+ (1,0) [0|1] "" XXX
  SG_ Signal2 : 42|22@1+ (1,0) [0|4194303] "" XXX
 
-BO_ 803 ES_Infotainment: 8 XXX
- SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
- SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
- SG_ LKAS_Blue_Lines : 15|4@0+ (1,0) [0|15] "" XXX
- SG_ Signal1 : 19|4@0+ (1,0) [0|15] "" XXX
- SG_ LKAS_State_Infotainment : 22|3@0+ (1,0) [0|7] "" XXX
- SG_ Signal2 : 24|1@0+ (1,0) [0|1] "" XXX
-
 CM_ SG_ 545 Cruise_Throttle "RPM-like output signal";
 CM_ SG_ 545 Cruise_EPB "1 = Electric Parking Brake set";
 CM_ SG_ 545 Distance_Swap "Switch from Close to Far distance";
diff --git a/opendbc/subaru_global_2020_hybrid_generated.dbc b/opendbc/subaru_global_2020_hybrid_generated.dbc
new file mode 100644
index 0000000000..99087eb228
--- /dev/null
+++ b/opendbc/subaru_global_2020_hybrid_generated.dbc
@@ -0,0 +1,324 @@
+CM_ "AUTOGENERATED FILE, DO NOT EDIT";
+
+
+CM_ "Imported file _subaru_global.dbc starts here";
+VERSION ""
+
+
+NS_ :
+    NS_DESC_
+    CM_
+    BA_DEF_
+    BA_
+    VAL_
+    CAT_DEF_
+    CAT_
+    FILTER
+    BA_DEF_DEF_
+    EV_DATA_
+    ENVVAR_DATA_
+    SGTYPE_
+    SGTYPE_VAL_
+    BA_DEF_SGTYPE_
+    BA_SGTYPE_
+    SIG_TYPE_REF_
+    VAL_TABLE_
+    SIG_GROUP_
+    SIG_VALTYPE_
+    SIGTYPE_VALTYPE_
+    BO_TX_BU_
+    BA_DEF_REL_
+    BA_REL_
+    BA_DEF_DEF_REL_
+    BU_SG_REL_
+    BU_EV_REL_
+    BU_BO_REL_
+    SG_MUL_VAL_
+
+BS_:
+
+BU_: XXX X
+
+
+BO_ 2 Steering: 8 XXX
+ SG_ Steering_Angle : 7|16@0- (0.1,0) [0|65535] "" XXX
+ SG_ COUNTER : 25|3@1+ (1,0) [0|7] "" XXX
+ SG_ CHECKSUM : 32|8@1+ (1,0) [0|255] "" XXX
+
+BO_ 64 Throttle: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ Signal1 : 12|4@1+ (1,0) [0|15] "" XXX
+ SG_ Engine_RPM : 16|12@1+ (1,0) [0|4095] "" XXX
+ SG_ Signal2 : 28|4@1+ (1,0) [0|15] "" XXX
+ SG_ Throttle_Pedal : 32|8@1+ (1,0) [0|255] "" XXX
+ SG_ Throttle_Cruise : 40|8@1+ (1,0) [0|255] "" XXX
+ SG_ Throttle_Combo : 48|8@1+ (1,0) [0|255] "" XXX
+ SG_ Signal3 : 56|4@1+ (1,0) [0|15] "" XXX
+ SG_ Off_Accel : 60|4@1+ (1,0) [0|15] "" XXX
+
+BO_ 316 Brake_Status: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ Signal1 : 12|46@1+ (1,0) [0|1] "" XXX
+ SG_ ES_Brake : 58|1@1+ (1,0) [0|1] "" XXX
+ SG_ Signal2 : 59|3@1+ (1,0) [0|1] "" XXX
+ SG_ Brake : 62|1@1+ (1,0) [0|1] "" XXX
+ SG_ Signal3 : 63|1@1+ (1,0) [0|1] "" XXX
+
+BO_ 326 Cruise_Buttons: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ Signal1 : 12|30@1+ (1,0) [0|1073741823] "" XXX
+ SG_ Main : 42|1@1+ (1,0) [0|1] "" XXX
+ SG_ Set : 43|1@1+ (1,0) [0|1] "" XXX
+ SG_ Resume : 44|1@1+ (1,0) [0|1] "" XXX
+ SG_ Signal2 : 45|19@1+ (1,0) [0|524287] "" XXX
+
+BO_ 315 G_Sensor: 8 XXX
+ SG_ Lateral : 48|8@1- (-0.1,0) [0|255] "m/s2" XXX
+ SG_ Longitudinal : 56|8@1- (-0.1,0) [0|255] "m/s2" XXX
+
+BO_ 314 Wheel_Speeds: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ FR : 12|13@1+ (0.057,0) [0|255] "kph" XXX
+ SG_ RR : 25|13@1+ (0.057,0) [0|255] "kph" XXX
+ SG_ FL : 51|13@1+ (0.057,0) [0|255] "kph" XXX
+ SG_ RL : 38|13@1+ (0.057,0) [0|255] "kph" XXX
+
+BO_ 280 Steering_Torque_2: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ Steer_Torque_Output : 13|11@1- (-10,0) [0|255] "" XXX
+ SG_ Signal1 : 24|8@1+ (1,0) [0|511] "" XXX
+ SG_ Steer_Torque_Sensor : 45|11@1- (-1,0) [0|255] "" XXX
+ SG_ Steering_Active : 61|1@0+ (1,0) [0|1] "" XXX
+ SG_ Steering_Disabled : 63|1@1+ (1,0) [0|1] "" XXX
+
+BO_ 281 Steering_Torque: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ Steer_Error_1 : 12|1@0+ (1,0) [0|1] "" XXX
+ SG_ Steer_Torque_Sensor : 16|11@1- (-1,0) [-1000|1000] "" XXX
+ SG_ Steer_Error_2 : 28|1@1+ (1,0) [0|1] "" XXX
+ SG_ Steer_Warning : 29|1@1+ (1,0) [0|1] "" XXX
+ SG_ Steering_Angle : 32|16@1- (-0.0217,0) [-600|600] "" X
+ SG_ Steer_Torque_Output : 48|11@1- (-10,0) [-1000|1000] "" XXX
+
+BO_ 282 Steering_2: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|1] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|1] "" XXX
+ SG_ Steering_Angle : 24|17@1- (-0.01,0) [0|1] "" XXX
+
+BO_ 312 Brake_Pressure_L_R: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ Brake_1 : 48|8@1+ (1,0) [0|255] "" XXX
+ SG_ Brake_2 : 56|8@1+ (1,0) [0|255] "" XXX
+
+BO_ 313 Brake_Pedal: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ Signal1 : 12|4@1+ (1,0) [0|15] "" XXX
+ SG_ Speed : 16|12@1+ (0.05625,0) [0|255] "kph" XXX
+ SG_ Signal2 : 28|6@1+ (1,0) [0|63] "" XXX
+ SG_ Brake_Lights : 34|1@1+ (1,0) [0|1] "" XXX
+ SG_ Signal3 : 35|1@1+ (1,0) [0|1] "" XXX
+ SG_ Brake_Pedal : 36|12@1+ (1,0) [0|4095] "" XXX
+ SG_ Signal4 : 48|16@1+ (1,0) [0|65535] "" XXX
+
+BO_ 372 Engine_Stop_Start: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ Counter : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ STOP_START_STATE : 39|2@0+ (1,0) [0|3] "" XXX
+
+BO_ 290 ES_LKAS: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ SET_1 : 12|1@0+ (1,0) [0|1] "" XXX
+ SG_ LKAS_Output : 16|13@1- (-1,0) [-8191|8191] "" XXX
+ SG_ LKAS_Request : 29|1@0+ (1,0) [0|1] "" XXX
+
+BO_ 292 ES_LKAS_ANGLE: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|1] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|1] "" XXX
+ SG_ LKAS_Request : 12|1@1+ (1,0) [0|1] "" XXX
+ SG_ LKAS_Output : 40|17@1- (-0.01,0) [0|1] "deg" XXX
+ SG_ SET_3 : 60|2@1+ (1,0) [0|1] "" XXX
+
+BO_ 544 ES_Brake: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ Signal1 : 12|4@1+ (1,0) [0|15] "" XXX
+ SG_ Brake_Pressure : 16|16@1+ (1,0) [0|65535] "" XXX
+ SG_ AEB_Status : 32|4@1+ (1,0) [0|15] "" XXX
+ SG_ Cruise_Brake_Lights : 36|1@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_Brake_Fault : 37|1@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_Brake_Active : 38|1@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_Activated : 39|1@1+ (1,0) [0|1] "" XXX
+ SG_ Signal3 : 40|24@1+ (1,0) [0|16777215] "" XXX
+
+BO_ 577 Cruise_Status: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ Cruise_Set_Speed : 51|12@0+ (1,0) [0|120] "" XXX
+ SG_ Cruise_On : 54|1@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_Activated : 55|1@1+ (1,0) [0|1] "" XXX
+
+BO_ 552 BSD_RCTA: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ R_ADJACENT : 48|1@1+ (1,0) [0|1] "" XXX
+ SG_ L_ADJACENT : 49|1@1+ (1,0) [0|1] "" XXX
+ SG_ R_APPROACHING : 58|1@1+ (1,0) [0|1] "" XXX
+ SG_ L_APPROACHING : 59|1@1+ (1,0) [0|1] "" XXX
+
+BO_ 912 Dashlights: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ UNITS : 24|1@1+ (1,0) [0|1] "" XXX
+ SG_ ICY_ROAD : 32|2@1+ (1,0) [0|3] "" XXX
+ SG_ SEATBELT_FL : 48|1@1+ (1,0) [0|1] "" XXX
+ SG_ LEFT_BLINKER : 50|1@1+ (1,0) [0|1] "" XXX
+ SG_ RIGHT_BLINKER : 51|1@1+ (1,0) [0|1] "" XXX
+ SG_ STOP_START : 54|1@0+ (1,0) [0|1] "" XXX
+
+BO_ 940 BodyInfo: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ DOOR_OPEN_FL : 32|1@1+ (1,0) [0|1] "" XXX
+ SG_ DOOR_OPEN_FR : 33|1@1+ (1,0) [0|1] "" XXX
+ SG_ DOOR_OPEN_RL : 34|1@1+ (1,0) [0|1] "" XXX
+ SG_ DOOR_OPEN_RR : 35|1@1+ (1,0) [0|1] "" XXX
+ SG_ DOOR_OPEN_TRUNK : 36|1@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE : 54|1@1+ (1,0) [0|1] "" XXX
+ SG_ DASH_BTN_LIGHTS : 56|1@0+ (1,0) [0|1] "" XXX
+ SG_ LOWBEAM : 57|1@1+ (1,0) [0|1] "" XXX
+ SG_ HIGHBEAM : 58|1@1+ (1,0) [0|1] "" XXX
+ SG_ FOG_LIGHTS : 60|1@1+ (1,0) [0|1] "" XXX
+ SG_ WIPERS : 62|1@0+ (1,0) [0|1] "" XXX
+
+BO_ 801 ES_DashStatus: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ PCB_Off : 12|1@1+ (1,0) [0|1] "" XXX
+ SG_ LDW_Off : 13|1@1+ (1,0) [0|1] "" XXX
+ SG_ Signal1 : 14|2@1+ (1,0) [0|3] "" XXX
+ SG_ Cruise_State_Msg : 16|4@1+ (1,0) [0|15] "" XXX
+ SG_ LKAS_State_Msg : 20|3@1+ (1,0) [0|7] "" XXX
+ SG_ Signal2 : 23|1@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_Soft_Disable : 24|1@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_Status_Msg : 25|2@1+ (1,0) [0|3] "" XXX
+ SG_ Signal3 : 27|1@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_Distance : 28|3@1+ (1,0) [0|7] "" XXX
+ SG_ Signal4 : 31|1@1+ (1,0) [0|1] "" XXX
+ SG_ Conventional_Cruise : 32|1@1+ (1,0) [0|1] "" XXX
+ SG_ Signal5 : 33|2@1+ (1,0) [0|3] "" XXX
+ SG_ Cruise_Disengaged : 35|1@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_Activated : 36|1@1+ (1,0) [0|1] "" XXX
+ SG_ Signal6 : 37|3@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_Set_Speed : 40|8@1+ (1,0) [0|255] "" XXX
+ SG_ Cruise_Fault : 48|1@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_On : 49|1@1+ (1,0) [0|1] "" XXX
+ SG_ Display_Own_Car : 50|1@1+ (1,0) [0|1] "" XXX
+ SG_ Brake_Lights : 51|1@1+ (1,0) [0|1] "" XXX
+ SG_ Car_Follow : 52|1@1+ (1,0) [0|1] "" XXX
+ SG_ Signal7 : 53|3@1+ (1,0) [0|1] "" XXX
+ SG_ Far_Distance : 56|4@1+ (5,0) [0|75] "m" XXX
+ SG_ Cruise_State : 60|4@1+ (1,0) [0|15] "" XXX
+
+BO_ 802 ES_LKAS_State: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ LKAS_Alert_Msg : 12|3@1+ (1,0) [0|7] "" XXX
+ SG_ Signal1 : 15|2@1+ (1,0) [0|3] "" XXX
+ SG_ LKAS_ACTIVE : 17|1@1+ (1,0) [0|1] "" XXX
+ SG_ LKAS_Dash_State : 18|2@1+ (1,0) [0|2] "" XXX
+ SG_ Signal2 : 20|3@1+ (1,0) [0|7] "" XXX
+ SG_ Backward_Speed_Limit_Menu : 23|1@1+ (1,0) [0|1] "" XXX
+ SG_ LKAS_Left_Line_Enable : 24|1@1+ (1,0) [0|1] "" XXX
+ SG_ LKAS_Left_Line_Light_Blink : 25|1@1+ (1,0) [0|1] "" XXX
+ SG_ LKAS_Right_Line_Enable : 26|1@1+ (1,0) [0|1] "" XXX
+ SG_ LKAS_Right_Line_Light_Blink : 27|1@1+ (1,0) [0|1] "" XXX
+ SG_ LKAS_Left_Line_Visible : 28|2@1+ (1,0) [0|3] "" XXX
+ SG_ LKAS_Right_Line_Visible : 30|2@1+ (1,0) [0|3] "" XXX
+ SG_ LKAS_Alert : 32|5@1+ (1,0) [0|31] "" XXX
+ SG_ Signal3 : 37|27@1+ (1,0) [0|1] "" XXX
+
+BO_ 803 ES_Infotainment: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ LKAS_Blue_Lines : 15|4@0+ (1,0) [0|15] "" XXX
+ SG_ Signal1 : 19|4@0+ (1,0) [0|15] "" XXX
+ SG_ LKAS_State_Infotainment : 22|3@0+ (1,0) [0|7] "" XXX
+ SG_ Signal2 : 24|1@0+ (1,0) [0|1] "" XXX
+
+BO_ 722 AC_State: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ AC_Mode : 37|3@1+ (1,0) [0|1] "" XXX
+ SG_ AC_ON : 24|2@1+ (1,0) [0|1] "" XXX
+
+BO_ 1677 Dash_State: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ Units : 29|3@1+ (1,0) [0|7] "" XXX
+
+BO_ 554 ES_HighBeamAssist: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ HBA_Available : 13|1@0+ (1,0) [0|1] "" XXX
+
+BO_ 805 ES_STATIC_1: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ SET_3 : 23|2@0+ (1,0) [0|3] "" XXX
+
+BO_ 289 ES_STATIC_2: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ SET_3 : 15|2@1+ (1,0) [0|3] "" XXX
+
+CM_ SG_ 64 Throttle_Combo "Throttle Cruise + Pedal";
+CM_ SG_ 313 Brake_Lights "Driver or Cruise Brake on";
+CM_ SG_ 544 Cruise_Brake_Lights "1 = switch on brake lights";
+CM_ SG_ 544 Brake_Pressure "Winds down after cruise disabled. Also can be non-zero when likely preparing for AEB";
+CM_ SG_ 544 Signal3 "Usually goes to 2 if AEB_Status is 4";
+CM_ SG_ 544 AEB_Status "Occasionally is 4 instead of 8 while Brake_Pressure is non-zero, unsure why";
+CM_ SG_ 801 PCB_Off "Pre-Collision Braking off";
+CM_ SG_ 801 Brake_Lights "Driver or Cruise brake on";
+CM_ SG_ 801 Cruise_State "0 = Normal, 1 = Hold+User Brake, 2 = Ready, 3 = Hold";
+CM_ SG_ 801 Far_Distance "1=0-5m, 2=5-10m, 3=10-15m, 4=15-20m, 5=20-25m, 6=25-30m, 7=30-35m, 8=35-40m, 9=40-45m, 10=45-50m, 11=50-55m, 12=55-60m, 13=60-65m, 14=65-70m, 15=75m+";
+CM_ SG_ 801 LKAS_State_Msg "1 = LKAS_Off_Sharp_Curve, 2 = Keep_Hands_On_Steering_wheel_disabled, 3 = LKAS_Off, 4 = LKAS_Off_Too_Slow, 5 = LKAS_Off_Too_Fast";
+CM_ SG_ 801 Cruise_State_Msg "1 = Cruise_Off_Steep_Slope, 2 = Cruise_lvl1_eco, 3 = Cruise_lvl2_comfort, 4 = Cruise_off_empty_reason, 5 = Cruise_off, 6 = Cruise_Unable_to_set, 7 = Cruise_Unable_to_set_brakes_applied, 8 = Eyesight_not_ready, 9 = Cruise_lvl3_standard, 10 = Cruise_lvl4_dynamic, 11 = Cruise_Unable_to_set_steep_slope";
+CM_ SG_ 801 Cruise_Soft_Disable "Eyesight soft disable (eg direct sunlight)";
+CM_ SG_ 801 Cruise_Status_Msg "1 = Disabled_Bad_Visibility, 2 = Disabled_Check_Manual";
+CM_ SG_ 802 LKAS_ACTIVE "Turns on the full LKAS dash display";
+CM_ SG_ 802 LKAS_Alert_Msg "1 = Keep_Hands_On_Wheel, 6 = Pre_Collision_Braking, 7 = Keep_Hands_On_Wheel_Off";
+CM_ SG_ 802 LKAS_Alert "1 = FCW_Cont_Beep, 2 = FCW_Repeated_Beep, 3 = Throttle_Management_Activated_Warning, 4 = Throttle_Management_Activated_Alert, 5 = Pre_Collision_Activated_Alert, 8 = Traffic_Light_Ahead, 9 = Apply_Brake_to_Hold Position, 11 = LDW_Right, 12 = LDW_Left, 13 = Stay_Alert, 14 = Lead_Vehicle_Start_Alert, 18 = Keep_Hands_On_Steering_Alert, 24 = Audio_Beep, 25 = Audio_Lead_Car_Change, 26 = Audio_ACC_Disengaged, 27 = Audio_LKAS_disabled, 28 = Audio_Ding_Ding, 30 = Audio_Repeated_Beep";
+CM_ SG_ 802 LKAS_Left_Line_Visible "0 = Off, 1 = White, 2 = Green, 3 = Orange";
+CM_ SG_ 802 LKAS_Dash_State "0 = Off, 1 = Ready, 2 = Active";
+CM_ SG_ 802 LKAS_Right_Line_Visible "0 = Off, 1 = White, 2 = Green, 3 = Orange";
+CM_ SG_ 912 UNITS "0 = Metric, 1 = Imperial";
+CM_ SG_ 912 ICY_ROAD "1 = DASHLIGHT ON, 2 = WARNING, 3 = OFF";
+VAL_ 544 AEB_Status 12 "AEB related" 8 "AEB actuation" 4 "AEB related" 0 "No AEB actuation";
+
+CM_ "subaru_global_2020_hybrid.dbc starts here";
+
+BO_ 39 Cruise_Status_2: 8 XXX
+ SG_ Cruise_Activated : 37|1@0+ (1,0) [0|1] "" XXX
+
+BO_ 295 Transmission: 8 XXX
+ SG_ Gear : 44|4@1+ (1,0) [0|15] "" XXX
+
+BO_ 360 Throttle_Hybrid: 8 XXX
+ SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
+ SG_ Throttle_Pedal : 32|8@1+ (1,0) [0|255] "" XXX
+
+BO_ 550 Brake_Hybrid: 8 XXX
+ SG_ Brake_Pedal : 24|8@1+ (1,0) [0|1] "" XXX
+ SG_ Brake : 37|1@1+ (1,0) [0|1] "" XXX
+
+VAL_ 295 Gear 0 "P" 1 "R" 3 "D";
diff --git a/opendbc/subaru_outback_2015_generated.dbc b/opendbc/subaru_outback_2015_generated.dbc
index 6f97c89088..4a6a0d3c03 100644
--- a/opendbc/subaru_outback_2015_generated.dbc
+++ b/opendbc/subaru_outback_2015_generated.dbc
@@ -127,10 +127,11 @@ BO_ 338 Stalk: 8 XXX
 
 BO_ 352 ES_Brake: 8 XXX
  SG_ Brake_Pressure : 0|16@1+ (1,0) [0|255] "" XXX
- SG_ Brake_Light : 20|1@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_Brake_Lights : 20|1@1+ (1,0) [0|1] "" XXX
  SG_ Cruise_Fault : 21|1@1+ (1,0) [0|1] "" XXX
- SG_ Brake_On : 22|1@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_Brake_Active : 22|1@1+ (1,0) [0|1] "" XXX
  SG_ Cruise_Activated : 23|1@1+ (1,0) [0|1] "" XXX
+ SG_ SET_1 : 45|1@0+ (1,0) [0|1] "" XXX
  SG_ COUNTER : 48|3@1+ (1,0) [0|7] "" XXX
  SG_ Checksum : 56|8@1+ (1,0) [0|255] "" XXX
 
@@ -139,7 +140,7 @@ BO_ 353 ES_Distance: 8 XXX
  SG_ Signal1 : 12|4@1+ (1,0) [0|15] "" XXX
  SG_ Car_Follow : 16|1@1+ (1,0) [0|1] "" XXX
  SG_ Signal2 : 17|3@1+ (1,0) [0|7] "" XXX
- SG_ Brake_On : 20|1@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_Brake_Active : 20|1@1+ (1,0) [0|1] "" XXX
  SG_ Distance_Swap : 21|1@1+ (1,0) [0|1] "" XXX
  SG_ Standstill : 22|1@1+ (1,0) [0|1] "" XXX
  SG_ Signal3 : 23|1@1+ (1,0) [0|1] "" XXX
@@ -154,10 +155,10 @@ BO_ 353 ES_Distance: 8 XXX
  SG_ Signal7 : 51|5@1+ (1,0) [0|31] "" XXX
  SG_ Checksum : 56|8@1+ (1,0) [0|255] "" XXX
 
-BO_ 354 ES_RPM: 8 XXX
+BO_ 354 ES_Status: 8 XXX
  SG_ Brake : 8|1@1+ (1,0) [0|1] "" XXX
  SG_ Cruise_Activated : 9|1@1+ (1,0) [0|1] "" XXX
- SG_ RPM : 16|16@1+ (1,0) [0|65535] "" XXX
+ SG_ Cruise_RPM : 16|16@1+ (1,0) [0|65535] "" XXX
  SG_ Checksum : 32|8@1+ (1,0) [0|255] "" XXX
  SG_ COUNTER : 48|3@1+ (1,0) [0|7] "" XXX
 
diff --git a/opendbc/subaru_outback_2019_generated.dbc b/opendbc/subaru_outback_2019_generated.dbc
index 908a9fdd3f..2493ad4d3a 100644
--- a/opendbc/subaru_outback_2019_generated.dbc
+++ b/opendbc/subaru_outback_2019_generated.dbc
@@ -127,10 +127,11 @@ BO_ 338 Stalk: 8 XXX
 
 BO_ 352 ES_Brake: 8 XXX
  SG_ Brake_Pressure : 0|16@1+ (1,0) [0|255] "" XXX
- SG_ Brake_Light : 20|1@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_Brake_Lights : 20|1@1+ (1,0) [0|1] "" XXX
  SG_ Cruise_Fault : 21|1@1+ (1,0) [0|1] "" XXX
- SG_ Brake_On : 22|1@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_Brake_Active : 22|1@1+ (1,0) [0|1] "" XXX
  SG_ Cruise_Activated : 23|1@1+ (1,0) [0|1] "" XXX
+ SG_ SET_1 : 45|1@0+ (1,0) [0|1] "" XXX
  SG_ COUNTER : 48|3@1+ (1,0) [0|7] "" XXX
  SG_ Checksum : 56|8@1+ (1,0) [0|255] "" XXX
 
@@ -139,7 +140,7 @@ BO_ 353 ES_Distance: 8 XXX
  SG_ Signal1 : 12|4@1+ (1,0) [0|15] "" XXX
  SG_ Car_Follow : 16|1@1+ (1,0) [0|1] "" XXX
  SG_ Signal2 : 17|3@1+ (1,0) [0|7] "" XXX
- SG_ Brake_On : 20|1@1+ (1,0) [0|1] "" XXX
+ SG_ Cruise_Brake_Active : 20|1@1+ (1,0) [0|1] "" XXX
  SG_ Distance_Swap : 21|1@1+ (1,0) [0|1] "" XXX
  SG_ Standstill : 22|1@1+ (1,0) [0|1] "" XXX
  SG_ Signal3 : 23|1@1+ (1,0) [0|1] "" XXX
@@ -154,10 +155,10 @@ BO_ 353 ES_Distance: 8 XXX
  SG_ Signal7 : 51|5@1+ (1,0) [0|31] "" XXX
  SG_ Checksum : 56|8@1+ (1,0) [0|255] "" XXX
 
-BO_ 354 ES_RPM: 8 XXX
+BO_ 354 ES_Status: 8 XXX
  SG_ Brake : 8|1@1+ (1,0) [0|1] "" XXX
  SG_ Cruise_Activated : 9|1@1+ (1,0) [0|1] "" XXX
- SG_ RPM : 16|16@1+ (1,0) [0|65535] "" XXX
+ SG_ Cruise_RPM : 16|16@1+ (1,0) [0|65535] "" XXX
  SG_ Checksum : 32|8@1+ (1,0) [0|255] "" XXX
  SG_ COUNTER : 48|3@1+ (1,0) [0|7] "" XXX
 
diff --git a/opendbc/toyota_new_mc_pt_generated.dbc b/opendbc/toyota_new_mc_pt_generated.dbc
index b5667101d5..04910dcfc2 100644
--- a/opendbc/toyota_new_mc_pt_generated.dbc
+++ b/opendbc/toyota_new_mc_pt_generated.dbc
@@ -252,6 +252,7 @@ BO_ 1005 REVERSE_CAMERA_STATE: 2 BGM
  SG_ REVERSE_CAMERA_GUIDELINES : 9|2@0+ (1,0) [1|3] "" XXX
 
 BO_ 1009 PCM_CRUISE_ALT: 8 XXX
+ SG_ PCM_FOLLOW_DISTANCE : 4|2@1+ (1,0) [0|3] "" XXX
  SG_ MAIN_ON : 13|1@0+ (1,0) [0|3] "" XXX
  SG_ CRUISE_STATE : 10|1@0+ (1,0) [0|1] "" XXX
  SG_ UI_SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX
@@ -259,7 +260,7 @@ BO_ 1009 PCM_CRUISE_ALT: 8 XXX
 BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
-BO_ 1041 ACC_HUD: 8 DSU
+BO_ 1041 PCS_HUD: 8 DSU
  SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
@@ -425,7 +426,7 @@ BO_ 1592 DOOR_LOCKS: 8 XXX
  SG_ LOCKED_VIA_KEYFOB : 23|1@0+ (1,0) [0|1] "" XXX
 
 CM_ SG_ 36 YAW_RATE "verify";
-CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
+CM_ SG_ 36 ACCEL_X "x-axis accel";
 CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
 CM_ SG_ 37 STEER_RATE "factor is tbd";
 CM_ SG_ 466 NEUTRAL_FORCE "force in newtons the engine/electric motors are applying without any acceleration commands or user input";
@@ -440,7 +441,7 @@ CM_ SG_ 581 GAS_PEDAL "it seems slightly filtered";
 CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
 CM_ SG_ 608 STEER_OVERRIDE "set when driver torque exceeds a certain value";
 CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active";
-CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
+CM_ SG_ 643 _COUNTER "only used on cars that use this msg for cruise control";
 CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
 CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
 CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
@@ -527,6 +528,7 @@ VAL_ 956 ECON_ON 0 "off" 1 "on";
 VAL_ 956 B_GEAR_ENGAGED 0 "off" 1 "on";
 VAL_ 956 DRIVE_ENGAGED 0 "off" 1 "on";
 VAL_ 1005 REVERSE_CAMERA_GUIDELINES 3 "No guidelines" 2 "Static guidelines" 1 "Active guidelines";
+VAL_ 1009 PCM_FOLLOW_DISTANCE 1 "far" 2 "medium" 3 "close";
 VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
 VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
diff --git a/opendbc/toyota_nodsu_pt_generated.dbc b/opendbc/toyota_nodsu_pt_generated.dbc
index 2ea426557f..c03949164c 100644
--- a/opendbc/toyota_nodsu_pt_generated.dbc
+++ b/opendbc/toyota_nodsu_pt_generated.dbc
@@ -252,6 +252,7 @@ BO_ 1005 REVERSE_CAMERA_STATE: 2 BGM
  SG_ REVERSE_CAMERA_GUIDELINES : 9|2@0+ (1,0) [1|3] "" XXX
 
 BO_ 1009 PCM_CRUISE_ALT: 8 XXX
+ SG_ PCM_FOLLOW_DISTANCE : 4|2@1+ (1,0) [0|3] "" XXX
  SG_ MAIN_ON : 13|1@0+ (1,0) [0|3] "" XXX
  SG_ CRUISE_STATE : 10|1@0+ (1,0) [0|1] "" XXX
  SG_ UI_SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX
@@ -259,7 +260,7 @@ BO_ 1009 PCM_CRUISE_ALT: 8 XXX
 BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
-BO_ 1041 ACC_HUD: 8 DSU
+BO_ 1041 PCS_HUD: 8 DSU
  SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
@@ -425,7 +426,7 @@ BO_ 1592 DOOR_LOCKS: 8 XXX
  SG_ LOCKED_VIA_KEYFOB : 23|1@0+ (1,0) [0|1] "" XXX
 
 CM_ SG_ 36 YAW_RATE "verify";
-CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
+CM_ SG_ 36 ACCEL_X "x-axis accel";
 CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
 CM_ SG_ 37 STEER_RATE "factor is tbd";
 CM_ SG_ 466 NEUTRAL_FORCE "force in newtons the engine/electric motors are applying without any acceleration commands or user input";
@@ -440,7 +441,7 @@ CM_ SG_ 581 GAS_PEDAL "it seems slightly filtered";
 CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
 CM_ SG_ 608 STEER_OVERRIDE "set when driver torque exceeds a certain value";
 CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active";
-CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
+CM_ SG_ 643 _COUNTER "only used on cars that use this msg for cruise control";
 CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
 CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
 CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
@@ -527,6 +528,7 @@ VAL_ 956 ECON_ON 0 "off" 1 "on";
 VAL_ 956 B_GEAR_ENGAGED 0 "off" 1 "on";
 VAL_ 956 DRIVE_ENGAGED 0 "off" 1 "on";
 VAL_ 1005 REVERSE_CAMERA_GUIDELINES 3 "No guidelines" 2 "Static guidelines" 1 "Active guidelines";
+VAL_ 1009 PCM_FOLLOW_DISTANCE 1 "far" 2 "medium" 3 "close";
 VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
 VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
diff --git a/opendbc/toyota_tnga_k_pt_generated.dbc b/opendbc/toyota_tnga_k_pt_generated.dbc
index c16b9f1234..f54f7df852 100644
--- a/opendbc/toyota_tnga_k_pt_generated.dbc
+++ b/opendbc/toyota_tnga_k_pt_generated.dbc
@@ -252,6 +252,7 @@ BO_ 1005 REVERSE_CAMERA_STATE: 2 BGM
  SG_ REVERSE_CAMERA_GUIDELINES : 9|2@0+ (1,0) [1|3] "" XXX
 
 BO_ 1009 PCM_CRUISE_ALT: 8 XXX
+ SG_ PCM_FOLLOW_DISTANCE : 4|2@1+ (1,0) [0|3] "" XXX
  SG_ MAIN_ON : 13|1@0+ (1,0) [0|3] "" XXX
  SG_ CRUISE_STATE : 10|1@0+ (1,0) [0|1] "" XXX
  SG_ UI_SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX
@@ -259,7 +260,7 @@ BO_ 1009 PCM_CRUISE_ALT: 8 XXX
 BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
-BO_ 1041 ACC_HUD: 8 DSU
+BO_ 1041 PCS_HUD: 8 DSU
  SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
@@ -425,7 +426,7 @@ BO_ 1592 DOOR_LOCKS: 8 XXX
  SG_ LOCKED_VIA_KEYFOB : 23|1@0+ (1,0) [0|1] "" XXX
 
 CM_ SG_ 36 YAW_RATE "verify";
-CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
+CM_ SG_ 36 ACCEL_X "x-axis accel";
 CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
 CM_ SG_ 37 STEER_RATE "factor is tbd";
 CM_ SG_ 466 NEUTRAL_FORCE "force in newtons the engine/electric motors are applying without any acceleration commands or user input";
@@ -440,7 +441,7 @@ CM_ SG_ 581 GAS_PEDAL "it seems slightly filtered";
 CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
 CM_ SG_ 608 STEER_OVERRIDE "set when driver torque exceeds a certain value";
 CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active";
-CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
+CM_ SG_ 643 _COUNTER "only used on cars that use this msg for cruise control";
 CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
 CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
 CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
@@ -527,6 +528,7 @@ VAL_ 956 ECON_ON 0 "off" 1 "on";
 VAL_ 956 B_GEAR_ENGAGED 0 "off" 1 "on";
 VAL_ 956 DRIVE_ENGAGED 0 "off" 1 "on";
 VAL_ 1005 REVERSE_CAMERA_GUIDELINES 3 "No guidelines" 2 "Static guidelines" 1 "Active guidelines";
+VAL_ 1009 PCM_FOLLOW_DISTANCE 1 "far" 2 "medium" 3 "close";
 VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
 VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
diff --git a/opendbc/vw_golf_mk4.dbc b/opendbc/vw_golf_mk4.dbc
index c562b9e056..131a4b763c 100644
--- a/opendbc/vw_golf_mk4.dbc
+++ b/opendbc/vw_golf_mk4.dbc
@@ -1178,7 +1178,7 @@ BO_ 870 AWV: 8 XXX
  SG_ AWV_1_Freigabe : 16|1@1+ (1,0) [0|1] "" Vector__XXX
  SG_ AWV_1_Prefill : 17|1@1+ (1,0) [0|1] "" Vector__XXX
  SG_ AWV_1_Parameter : 18|2@1+ (1,0) [0|3] "" Vector__XXX
- SG_ AWV_only : 20|4@1+ (1,0) [0|0] "" Vector__XXX
+ SG_ AWV_only : 20|1@1+ (1,0) [0|1] "" Vector__XXX
  SG_ AWV_CityANB_Auspraegung : 21|1@1+ (1,0) [0|1] "" Vector__XXX
  SG_ AWV_Halten : 22|1@1+ (1,0) [0|1] "" Vector__XXX
  SG_ ANB_Teilbremsung_Freigabe : 23|1@1+ (1,0) [0|1] "" Vector__XXX
diff --git a/openpilot/__init__.py b/openpilot/__init__.py
new file mode 100644
index 0000000000..b28b04f643
--- /dev/null
+++ b/openpilot/__init__.py
@@ -0,0 +1,3 @@
+
+
+
diff --git a/openpilot/common b/openpilot/common
new file mode 120000
index 0000000000..60d3b0a6a8
--- /dev/null
+++ b/openpilot/common
@@ -0,0 +1 @@
+../common
\ No newline at end of file
diff --git a/openpilot/selfdrive b/openpilot/selfdrive
new file mode 120000
index 0000000000..e005fd0d04
--- /dev/null
+++ b/openpilot/selfdrive
@@ -0,0 +1 @@
+../selfdrive/
\ No newline at end of file
diff --git a/openpilot/system b/openpilot/system
new file mode 120000
index 0000000000..16f8cc2b23
--- /dev/null
+++ b/openpilot/system
@@ -0,0 +1 @@
+../system/
\ No newline at end of file
diff --git a/openpilot/third_party b/openpilot/third_party
new file mode 120000
index 0000000000..d838c05a86
--- /dev/null
+++ b/openpilot/third_party
@@ -0,0 +1 @@
+../third_party
\ No newline at end of file
diff --git a/openpilot/tools b/openpilot/tools
new file mode 120000
index 0000000000..4887d6e0c9
--- /dev/null
+++ b/openpilot/tools
@@ -0,0 +1 @@
+../tools
\ No newline at end of file
diff --git a/panda/.gitignore b/panda/.gitignore
index a3f6520bfd..2dfa094768 100644
--- a/panda/.gitignore
+++ b/panda/.gitignore
@@ -12,7 +12,7 @@ a.out
 .#*
 dist/
 pandacan.egg-info/
-board/obj/
+obj/
 examples/output.csv
 .DS_Store
 .vscode*
@@ -22,3 +22,9 @@ nosetests.xml
 
 # CTU info files generated by Cppcheck
 *.*.ctu-info
+
+# safety coverage-related files
+*.gcda
+*.gcno
+tests/safety/coverage-out
+tests/safety/coverage.info
diff --git a/panda/SConscript b/panda/SConscript
index ed3aff754d..34fddc49e5 100644
--- a/panda/SConscript
+++ b/panda/SConscript
@@ -1,6 +1,190 @@
+import os
+import subprocess
+
+
+PREFIX = "arm-none-eabi-"
+BUILDER = "DEV"
+
+common_flags = []
+
+panda_root = Dir('.').abspath
+
+if os.getenv("RELEASE"):
+  BUILD_TYPE = "RELEASE"
+  cert_fn = os.getenv("CERT")
+  assert cert_fn is not None, 'No certificate file specified. Please set CERT env variable'
+  assert os.path.exists(cert_fn), 'Certificate file not found. Please specify absolute path'
+else:
+  BUILD_TYPE = "DEBUG"
+  cert_fn = File("./certs/debug").srcnode().abspath
+  common_flags += ["-DALLOW_DEBUG"]
+
+  if os.getenv("DEBUG"):
+    common_flags += ["-DDEBUG"]
+
+def objcopy(source, target, env, for_signature):
+    return '$OBJCOPY -O binary %s %s' % (source[0], target[0])
+
+def get_version(builder, build_type):
+  try:
+    git = subprocess.check_output(["git", "rev-parse", "--short=8", "HEAD"], encoding='utf8').strip()
+  except subprocess.CalledProcessError:
+    git = "unknown"
+  return f"{builder}-{git}-{build_type}"
+
+def get_key_header(name):
+  from Crypto.PublicKey import RSA
+
+  public_fn = File(f'./certs/{name}.pub').srcnode().abspath
+  with open(public_fn) as f:
+    rsa = RSA.importKey(f.read())
+  assert(rsa.size_in_bits() == 1024)
+
+  rr = pow(2**1024, 2, rsa.n)
+  n0inv = 2**32 - pow(rsa.n, -1, 2**32)
+
+  r = [
+    f"RSAPublicKey {name}_rsa_key = {{",
+    f"  .len = 0x20,",
+    f"  .n0inv = {n0inv}U,",
+    f"  .n = {to_c_uint32(rsa.n)},",
+    f"  .rr = {to_c_uint32(rr)},",
+    f"  .exponent = {rsa.e},",
+    f"}};",
+  ]
+  return r
+
+def to_c_uint32(x):
+  nums = []
+  for _ in range(0x20):
+    nums.append(x % (2**32))
+    x //= (2**32)
+  return "{" + 'U,'.join(map(str, nums)) + "U}"
+
+
+def build_project(project_name, project, extra_flags):
+  linkerscript_fn = File(project["LINKER_SCRIPT"]).srcnode().abspath
+
+  flags = project["PROJECT_FLAGS"] + extra_flags + common_flags + [
+    "-Wall",
+    "-Wextra",
+    "-Wstrict-prototypes",
+    "-Werror",
+    "-mlittle-endian",
+    "-mthumb",
+    "-nostdlib",
+    "-fno-builtin",
+    "-std=gnu11",
+    "-fmax-errors=3",
+    f"-T{linkerscript_fn}",
+  ]
+
+  includes = [
+    '.',
+    '..',
+    panda_root,
+    f"{panda_root}/board/",
+    f"{panda_root}/board/stm32fx/inc",
+    f"{panda_root}/board/stm32h7/inc",
+  ]
+
+  env = Environment(
+    ENV=os.environ,
+    CC=PREFIX + 'gcc',
+    AS=PREFIX + 'gcc',
+    OBJCOPY=PREFIX + 'objcopy',
+    OBJDUMP=PREFIX + 'objdump',
+    CFLAGS=flags,
+    ASFLAGS=flags,
+    LINKFLAGS=flags,
+    CPPPATH=includes,
+    ASCOM="$AS $ASFLAGS -o $TARGET -c $SOURCES",
+    BUILDERS={
+      'Objcopy': Builder(generator=objcopy, suffix='.bin', src_suffix='.elf')
+    }
+  )
+
+  startup = env.Object(f"obj/startup_{project_name}", project["STARTUP_FILE"])
+
+  # Bootstub
+  crypto_obj = [
+    env.Object(f"rsa-{project_name}", f"{panda_root}/crypto/rsa.c"),
+    env.Object(f"sha-{project_name}", f"{panda_root}/crypto/sha.c")
+  ]
+  bootstub_obj = env.Object(f"bootstub-{project_name}", File(project.get("BOOTSTUB", f"{panda_root}/board/bootstub.c")))
+  bootstub_elf = env.Program(f"obj/bootstub.{project_name}.elf",
+                                     [startup] + crypto_obj + [bootstub_obj])
+  env.Objcopy(f"obj/bootstub.{project_name}.bin", bootstub_elf)
+
+  # Build main
+  main_obj = env.Object(f"main-{project_name}", project["MAIN"])
+  main_elf = env.Program(f"obj/{project_name}.elf", [startup, main_obj],
+    LINKFLAGS=[f"-Wl,--section-start,.isr_vector={project['APP_START_ADDRESS']}"] + flags)
+  main_bin = env.Objcopy(f"obj/{project_name}.bin", main_elf)
+
+  # Sign main
+  sign_py = File(f"{panda_root}/crypto/sign.py").srcnode().abspath
+  env.Command(f"obj/{project_name}.bin.signed", main_bin, f"SETLEN=1 {sign_py} $SOURCE $TARGET {cert_fn}")
+
+
+base_project_f4 = {
+  "MAIN": "main.c",
+  "STARTUP_FILE": File("./board/stm32fx/startup_stm32f413xx.s"),
+  "LINKER_SCRIPT": File("./board/stm32fx/stm32f4_flash.ld"),
+  "APP_START_ADDRESS": "0x8004000",
+  "PROJECT_FLAGS": [
+    "-mcpu=cortex-m4",
+    "-mhard-float",
+    "-DSTM32F4",
+    "-DSTM32F413xx",
+    "-mfpu=fpv4-sp-d16",
+    "-fsingle-precision-constant",
+    "-Os",
+    "-g",
+  ],
+}
+
+base_project_h7 = {
+  "MAIN": "main.c",
+  "STARTUP_FILE": File("./board/stm32h7/startup_stm32h7x5xx.s"),
+  "LINKER_SCRIPT": File("./board/stm32h7/stm32h7x5_flash.ld"),
+  "APP_START_ADDRESS": "0x8020000",
+  "PROJECT_FLAGS": [
+    "-mcpu=cortex-m7",
+    "-mhard-float",
+    "-DSTM32H7",
+    "-DSTM32H725xx",
+    "-mfpu=fpv5-d16",
+    "-fsingle-precision-constant",
+    "-Os",
+    "-g",
+  ],
+}
+
+Export('base_project_f4', 'base_project_h7', 'build_project')
+
+
+# Common autogenerated includes
+with open("board/obj/gitversion.h", "w") as f:
+  f.write(f'const uint8_t gitversion[] = "{get_version(BUILDER, BUILD_TYPE)}";\n')
+
+with open("board/obj/version", "w") as f:
+  f.write(f'{get_version(BUILDER, BUILD_TYPE)}')
+
+certs = [get_key_header(n) for n in ["debug", "release"]]
+with open("board/obj/cert.h", "w") as f:
+  for cert in certs:
+    f.write("\n".join(cert) + "\n")
+
 # panda fw
 SConscript('board/SConscript')
 
+# pedal fw
+SConscript('board/pedal/SConscript')
+
+# panda jungle fw
+SConscript('board/jungle/SConscript')
+
 # test files
-if GetOption('test'):
+if GetOption('extras'):
   SConscript('tests/libpanda/SConscript')
diff --git a/panda/__init__.py b/panda/__init__.py
index dfb2bbf1c9..bf487fddcc 100644
--- a/panda/__init__.py
+++ b/panda/__init__.py
@@ -1,6 +1,11 @@
 from .python.constants import McuType, BASEDIR, FW_PATH, USBPACKET_MAX_SIZE  # noqa: F401
-from .python.spi import PandaSpiException, PandaProtocolMismatch  # noqa: F401
+from .python.spi import PandaSpiException, PandaProtocolMismatch, STBootloaderSPIHandle  # noqa: F401
 from .python.serial import PandaSerial  # noqa: F401
+from .python.canhandle import CanHandle # noqa: F401
 from .python import (Panda, PandaDFU, # noqa: F401
-                     pack_can_buffer, unpack_can_buffer, calculate_checksum, unpack_log,
+                     pack_can_buffer, unpack_can_buffer, calculate_checksum,
                      DLC_TO_LEN, LEN_TO_DLC, ALTERNATIVE_EXPERIENCE, CANPACKET_HEAD_SIZE)
+
+
+# panda jungle
+from .board.jungle import PandaJungle, PandaJungleDFU # noqa: F401
diff --git a/panda/board/SConscript b/panda/board/SConscript
index dc095e712a..45fa0f416e 100644
--- a/panda/board/SConscript
+++ b/panda/board/SConscript
@@ -1,194 +1,18 @@
 import os
 import copy
-import subprocess
 
-PREFIX = "arm-none-eabi-"
-BUILDER = "DEV"
+Import('build_project', 'base_project_f4', 'base_project_h7')
 
-common_flags = []
-build_projects = {}
-
-build_projects["pedal"] = {
-  "MAIN": "pedal/main.c",
-  "STARTUP_FILE": "stm32fx/startup_stm32f205xx.s",
-  "LINKER_SCRIPT": "stm32fx/stm32f2_flash.ld",
-  "APP_START_ADDRESS": "0x8004000",
-  "PROJECT_FLAGS": [
-    "-mcpu=cortex-m3",
-    "-msoft-float",
-    "-DSTM32F2",
-    "-DSTM32F205xx",
-    "-O2",
-    "-DPEDAL",
-  ],
-}
-
-build_projects["pedal_usb"] = copy.deepcopy(build_projects["pedal"])
-build_projects["pedal_usb"]["PROJECT_FLAGS"].append("-DPEDAL_USB")
-
-build_projects["panda"] = {
-  "MAIN": "main.c",
-  "STARTUP_FILE": "stm32fx/startup_stm32f413xx.s",
-  "LINKER_SCRIPT": "stm32fx/stm32f4_flash.ld",
-  "APP_START_ADDRESS": "0x8004000",
-  "PROJECT_FLAGS": [
-    "-mcpu=cortex-m4",
-    "-mhard-float",
-    "-DSTM32F4",
-    "-DSTM32F413xx",
-    "-mfpu=fpv4-sp-d16",
-    "-fsingle-precision-constant",
-    "-Os",
-    "-g",
-    "-DPANDA",
-  ],
+build_projects = {
+  "panda": base_project_f4,
+  "panda_h7": base_project_h7,
 }
 
-build_projects["panda_h7"] = {
-  "MAIN": "main.c",
-  "STARTUP_FILE": "stm32h7/startup_stm32h7x5xx.s",
-  "LINKER_SCRIPT": "stm32h7/stm32h7x5_flash.ld",
-  "APP_START_ADDRESS": "0x8020000",
-  "PROJECT_FLAGS": [
-    "-mcpu=cortex-m7",
-    "-mhard-float",
-    "-DSTM32H7",
-    "-DSTM32H725xx",
-    "-mfpu=fpv5-d16",
-    "-fsingle-precision-constant",
-    "-Os",
-    "-g",
+for project_name, project in build_projects.items():
+  flags = [
     "-DPANDA",
-  ],
-}
-
-if os.getenv("RELEASE"):
-  BUILD_TYPE = "RELEASE"
-  cert_fn = os.getenv("CERT")
-  assert cert_fn is not None, 'No certificate file specified. Please set CERT env variable'
-  assert os.path.exists(cert_fn), 'Certificate file not found. Please specify absolute path'
-else:
-  BUILD_TYPE = "DEBUG"
-  cert_fn = File("../certs/debug").srcnode().abspath
-  common_flags += ["-DALLOW_DEBUG"]
-
-if os.getenv("DEBUG"):
-  common_flags += ["-DDEBUG"]
-
-includes = [
-  "stm32fx/inc",
-  "stm32h7/inc",
-  "..",
-  ".",
-]
-
-def get_version(builder, build_type):
-  try:
-    git = subprocess.check_output(["git", "rev-parse", "--short=8", "HEAD"], encoding='utf8').strip()
-  except subprocess.CalledProcessError:
-    git = "unknown"
-  return f"{builder}-{git}-{build_type}"
-
-
-def to_c_uint32(x):
-  nums = []
-  for _ in range(0x20):
-    nums.append(x % (2**32))
-    x //= (2**32)
-  return "{" + 'U,'.join(map(str, nums)) + "U}"
-
-
-def get_key_header(name):
-  from Crypto.PublicKey import RSA
-
-  public_fn = File(f'../certs/{name}.pub').srcnode().abspath
-  with open(public_fn) as f:
-    rsa = RSA.importKey(f.read())
-  assert(rsa.size_in_bits() == 1024)
-
-  rr = pow(2**1024, 2, rsa.n)
-  n0inv = 2**32 - pow(rsa.n, -1, 2**32)
-
-  r = [
-    f"RSAPublicKey {name}_rsa_key = {{",
-    f"  .len = 0x20,",
-    f"  .n0inv = {n0inv}U,",
-    f"  .n = {to_c_uint32(rsa.n)},",
-    f"  .rr = {to_c_uint32(rr)},",
-    f"  .exponent = {rsa.e},",
-    f"}};",
   ]
-  return r
-
-def objcopy(source, target, env, for_signature):
-    return '$OBJCOPY -O binary %s %s' % (source[0], target[0])
-
-# Common autogenerated includes
-with open("obj/gitversion.h", "w") as f:
-  f.write(f'const uint8_t gitversion[] = "{get_version(BUILDER, BUILD_TYPE)}";\n')
-
-with open("obj/version", "w") as f:
-  f.write(f'{get_version(BUILDER, BUILD_TYPE)}')
-
-certs = [get_key_header(n) for n in ["debug", "release"]]
-with open("obj/cert.h", "w") as f:
-  for cert in certs:
-    f.write("\n".join(cert) + "\n")
-
-
-for project_name in build_projects:
-  project = build_projects[project_name]
-  linkerscript_fn = File(project["LINKER_SCRIPT"]).srcnode().abspath
-
-  flags = [
-    "-Wall",
-    "-Wextra",
-    "-Wstrict-prototypes",
-    "-Werror",
-    "-mlittle-endian",
-    "-mthumb",
-    "-nostdlib",
-    "-fno-builtin",
-    f"-T{linkerscript_fn}",
-    "-std=gnu11",
-  ] + project["PROJECT_FLAGS"] + common_flags
-
   if ("ENABLE_SPI" in os.environ or "h7" in project_name) and not project_name.startswith('pedal'):
     flags.append('-DENABLE_SPI')
 
-  project_env = Environment(
-    ENV=os.environ,
-    CC=PREFIX + 'gcc',
-    AS=PREFIX + 'gcc',
-    OBJCOPY=PREFIX + 'objcopy',
-    OBJDUMP=PREFIX + 'objdump',
-    ASCOM="$AS $ASFLAGS -o $TARGET -c $SOURCES",
-    CFLAGS=flags,
-    ASFLAGS=flags,
-    LINKFLAGS=flags,
-    CPPPATH=includes,
-    BUILDERS={
-      'Objcopy': Builder(generator=objcopy, suffix='.bin', src_suffix='.elf')
-    }
-  )
-
-  startup = project_env.Object(f"obj/startup_{project_name}", project["STARTUP_FILE"])
-
-  # Bootstub
-  crypto_obj = [
-    project_env.Object(f"rsa-{project_name}", "../crypto/rsa.c"),
-    project_env.Object(f"sha-{project_name}", "../crypto/sha.c")
-  ]
-  bootstub_obj = project_env.Object(f"bootstub-{project_name}", "bootstub.c")
-  bootstub_elf = project_env.Program(f"obj/bootstub.{project_name}.elf", [startup] + crypto_obj + [bootstub_obj])
-  bootstub_bin = project_env.Objcopy(f"obj/bootstub.{project_name}.bin", bootstub_elf)
-
-  # Build main
-  main_obj = project_env.Object(f"main-{project_name}", project["MAIN"])
-  main_elf = project_env.Program(f"obj/{project_name}.elf", [startup, main_obj],
-    LINKFLAGS=[f"-Wl,--section-start,.isr_vector={project['APP_START_ADDRESS']}"] + flags)
-  main_bin = project_env.Objcopy(f"obj/{project_name}.bin", main_elf)
-
-  # Sign main
-  sign_py = File("../crypto/sign.py").srcnode().abspath
-  panda_bin_signed = project_env.Command(f"obj/{project_name}.bin.signed", main_bin, f"SETLEN=1 {sign_py} $SOURCE $TARGET {cert_fn}")
+  build_project(project_name, project, flags)
diff --git a/panda/board/boards/black.h b/panda/board/boards/black.h
index 216def774f..caeffc3174 100644
--- a/panda/board/boards/black.h
+++ b/panda/board/boards/black.h
@@ -49,37 +49,13 @@ void black_set_led(uint8_t color, bool enabled) {
   }
 }
 
-void black_set_gps_load_switch(bool enabled) {
-  set_gpio_output(GPIOC, 12, enabled);
-}
-
 void black_set_usb_load_switch(bool enabled) {
   set_gpio_output(GPIOB, 1, !enabled);
 }
 
-void black_set_gps_mode(uint8_t mode) {
-  switch (mode) {
-    case GPS_DISABLED:
-      // GPS OFF
-      set_gpio_output(GPIOC, 12, 0);
-      set_gpio_output(GPIOC, 5, 0);
-      break;
-    case GPS_ENABLED:
-      // GPS ON
-      set_gpio_output(GPIOC, 12, 1);
-      set_gpio_output(GPIOC, 5, 1);
-      break;
-    case GPS_BOOTMODE:
-      set_gpio_output(GPIOC, 12, 1);
-      set_gpio_output(GPIOC, 5, 0);
-      break;
-    default:
-      print("Invalid GPS mode\n");
-      break;
-  }
-}
-
-void black_set_can_mode(uint8_t mode){
+void black_set_can_mode(uint8_t mode) {
+  black_enable_can_transceiver(2U, false);
+  black_enable_can_transceiver(4U, false);
   switch (mode) {
     case CAN_MODE_NORMAL:
     case CAN_MODE_OBD_CAN2:
@@ -91,6 +67,8 @@ void black_set_can_mode(uint8_t mode){
         // B5,B6: normal CAN2 mode
         set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2);
         set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2);
+        black_enable_can_transceiver(2U, true);
+
       } else {
         // B5,B6: disable normal CAN2 mode
         set_gpio_mode(GPIOB, 5, MODE_INPUT);
@@ -99,6 +77,7 @@ void black_set_can_mode(uint8_t mode){
         // B12,B13: OBD mode
         set_gpio_alternate(GPIOB, 12, GPIO_AF9_CAN2);
         set_gpio_alternate(GPIOB, 13, GPIO_AF9_CAN2);
+        black_enable_can_transceiver(4U, true);
       }
       break;
     default:
@@ -124,8 +103,9 @@ void black_init(void) {
   set_gpio_mode(GPIOC, 0, MODE_ANALOG);
   set_gpio_mode(GPIOC, 3, MODE_ANALOG);
 
-  // Set default state of GPS
-  current_board->set_gps_mode(GPS_ENABLED);
+  // GPS OFF
+  set_gpio_output(GPIOC, 5, 0);
+  set_gpio_output(GPIOC, 12, 0);
 
   // C10: OBD_SBU1_RELAY (harness relay driving output)
   // C11: OBD_SBU2_RELAY (harness relay driving output)
@@ -136,9 +116,6 @@ void black_init(void) {
   set_gpio_output(GPIOC, 10, 1);
   set_gpio_output(GPIOC, 11, 1);
 
-  // Turn on GPS load switch.
-  black_set_gps_load_switch(true);
-
   // Turn on USB load switch.
   black_set_usb_load_switch(true);
 
@@ -165,6 +142,12 @@ void black_init(void) {
   }
 }
 
+void black_init_bootloader(void) {
+  // GPS OFF
+  set_gpio_output(GPIOC, 5, 0);
+  set_gpio_output(GPIOC, 12, 0);
+}
+
 const harness_configuration black_harness_config = {
   .has_harness = true,
   .GPIO_SBU1 = GPIOC,
@@ -181,9 +164,8 @@ const harness_configuration black_harness_config = {
 
 const board board_black = {
   .board_type = "Black",
-  .board_tick = unused_board_tick,
+  .set_bootkick = unused_set_bootkick,
   .harness_config = &black_harness_config,
-  .has_gps = true,
   .has_hw_gmlan = false,
   .has_obd = true,
   .has_lin = false,
@@ -195,10 +177,10 @@ const board board_black = {
   .fan_stall_recovery = false,
   .fan_enable_cooldown_time = 0U,
   .init = black_init,
+  .init_bootloader = black_init_bootloader,
   .enable_can_transceiver = black_enable_can_transceiver,
   .enable_can_transceivers = black_enable_can_transceivers,
   .set_led = black_set_led,
-  .set_gps_mode = black_set_gps_mode,
   .set_can_mode = black_set_can_mode,
   .check_ignition = black_check_ignition,
   .read_current = unused_read_current,
diff --git a/panda/board/boards/board_declarations.h b/panda/board/boards/board_declarations.h
index c0c6436906..fbf23783e0 100644
--- a/panda/board/boards/board_declarations.h
+++ b/panda/board/boards/board_declarations.h
@@ -1,9 +1,15 @@
 // ******************** Prototypes ********************
+typedef enum {
+  BOOT_STANDBY,
+  BOOT_BOOTKICK,
+  BOOT_RESET,
+} BootState;
+
 typedef void (*board_init)(void);
+typedef void (*board_init_bootloader)(void);
 typedef void (*board_enable_can_transceiver)(uint8_t transceiver, bool enabled);
 typedef void (*board_enable_can_transceivers)(bool enabled);
 typedef void (*board_set_led)(uint8_t color, bool enabled);
-typedef void (*board_set_gps_mode)(uint8_t mode);
 typedef void (*board_set_can_mode)(uint8_t mode);
 typedef bool (*board_check_ignition)(void);
 typedef uint32_t (*board_read_current)(void);
@@ -11,13 +17,12 @@ typedef void (*board_set_ir_power)(uint8_t percentage);
 typedef void (*board_set_fan_enabled)(bool enabled);
 typedef void (*board_set_phone_power)(bool enabled);
 typedef void (*board_set_siren)(bool enabled);
-typedef bool (*board_board_tick)(bool ignition, bool usb_enum, bool heartbeat_seen, bool harness_inserted);
+typedef void (*board_set_bootkick)(BootState state);
 typedef bool (*board_read_som_gpio)(void);
 
 struct board {
   const char *board_type;
   const harness_configuration *harness_config;
-  const bool has_gps;
   const bool has_hw_gmlan;
   const bool has_obd;
   const bool has_lin;
@@ -29,10 +34,10 @@ struct board {
   const bool fan_stall_recovery;
   const uint8_t fan_enable_cooldown_time;
   board_init init;
+  board_init_bootloader init_bootloader;
   board_enable_can_transceiver enable_can_transceiver;
   board_enable_can_transceivers enable_can_transceivers;
   board_set_led set_led;
-  board_set_gps_mode set_gps_mode;
   board_set_can_mode set_can_mode;
   board_check_ignition check_ignition;
   board_read_current read_current;
@@ -40,7 +45,7 @@ struct board {
   board_set_fan_enabled set_fan_enabled;
   board_set_phone_power set_phone_power;
   board_set_siren set_siren;
-  board_board_tick board_tick;
+  board_set_bootkick set_bootkick;
   board_read_som_gpio read_som_gpio;
 };
 
@@ -68,11 +73,6 @@ struct board {
 #define USB_POWER_CDP 2U
 #define USB_POWER_DCP 3U
 
-// GPS modes
-#define GPS_DISABLED 0U
-#define GPS_ENABLED 1U
-#define GPS_BOOTMODE 2U
-
 // CAN modes
 #define CAN_MODE_NORMAL 0U
 #define CAN_MODE_GMLAN_CAN2 1U
diff --git a/panda/board/boards/dos.h b/panda/board/boards/dos.h
index 0178ff016f..bdbf1723eb 100644
--- a/panda/board/boards/dos.h
+++ b/panda/board/boards/dos.h
@@ -49,26 +49,13 @@ void dos_set_led(uint8_t color, bool enabled) {
   }
 }
 
-void dos_set_bootkick(bool enabled){
-  set_gpio_output(GPIOC, 4, !enabled);
+void dos_set_bootkick(BootState state) {
+  set_gpio_output(GPIOC, 4, state != BOOT_BOOTKICK);
 }
 
-bool dos_board_tick(bool ignition, bool usb_enum, bool heartbeat_seen, bool harness_inserted) {
-  bool ret = false;
-  if ((ignition && !usb_enum) || harness_inserted) {
-    // enable bootkick if ignition seen or if plugged into a harness
-    ret = true;
-    dos_set_bootkick(true);
-  } else if (heartbeat_seen) {
-    // disable once openpilot is up
-    dos_set_bootkick(false);
-  } else {
-
-  }
-  return ret;
-}
-
-void dos_set_can_mode(uint8_t mode){
+void dos_set_can_mode(uint8_t mode) {
+  dos_enable_can_transceiver(2U, false);
+  dos_enable_can_transceiver(4U, false);
   switch (mode) {
     case CAN_MODE_NORMAL:
     case CAN_MODE_OBD_CAN2:
@@ -80,6 +67,7 @@ void dos_set_can_mode(uint8_t mode){
         // B5,B6: normal CAN2 mode
         set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2);
         set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2);
+        dos_enable_can_transceiver(2U, true);
       } else {
         // B5,B6: disable normal CAN2 mode
         set_gpio_mode(GPIOB, 5, MODE_INPUT);
@@ -88,6 +76,7 @@ void dos_set_can_mode(uint8_t mode){
         // B12,B13: OBD mode
         set_gpio_alternate(GPIOB, 12, GPIO_AF9_CAN2);
         set_gpio_alternate(GPIOB, 13, GPIO_AF9_CAN2);
+        dos_enable_can_transceiver(4U, true);
       }
       break;
     default:
@@ -204,9 +193,7 @@ const harness_configuration dos_harness_config = {
 
 const board board_dos = {
   .board_type = "Dos",
-  .board_tick = dos_board_tick,
   .harness_config = &dos_harness_config,
-  .has_gps = false,
   .has_hw_gmlan = false,
   .has_obd = true,
   .has_lin = false,
@@ -222,10 +209,10 @@ const board board_dos = {
   .fan_stall_recovery = true,
   .fan_enable_cooldown_time = 3U,
   .init = dos_init,
+  .init_bootloader = unused_init_bootloader,
   .enable_can_transceiver = dos_enable_can_transceiver,
   .enable_can_transceivers = dos_enable_can_transceivers,
   .set_led = dos_set_led,
-  .set_gps_mode = unused_set_gps_mode,
   .set_can_mode = dos_set_can_mode,
   .check_ignition = dos_check_ignition,
   .read_current = unused_read_current,
@@ -233,5 +220,6 @@ const board board_dos = {
   .set_ir_power = dos_set_ir_power,
   .set_phone_power = unused_set_phone_power,
   .set_siren = dos_set_siren,
+  .set_bootkick = dos_set_bootkick,
   .read_som_gpio = dos_read_som_gpio
 };
diff --git a/panda/board/boards/grey.h b/panda/board/boards/grey.h
index 0adb5e2a1c..1aa2fdc3d7 100644
--- a/panda/board/boards/grey.h
+++ b/panda/board/boards/grey.h
@@ -4,40 +4,10 @@
 
 // Most hardware functionality is similar to white panda
 
-void grey_init(void) {
-  white_grey_common_init();
-
-  // Set default state of GPS
-  current_board->set_gps_mode(GPS_ENABLED);
-}
-
-void grey_set_gps_mode(uint8_t mode) {
-  switch (mode) {
-    case GPS_DISABLED:
-      // GPS OFF
-      set_gpio_output(GPIOC, 14, 0);
-      set_gpio_output(GPIOC, 5, 0);
-      break;
-    case GPS_ENABLED:
-      // GPS ON
-      set_gpio_output(GPIOC, 14, 1);
-      set_gpio_output(GPIOC, 5, 1);
-      break;
-    case GPS_BOOTMODE:
-      set_gpio_output(GPIOC, 14, 1);
-      set_gpio_output(GPIOC, 5, 0);
-      break;
-    default:
-      print("Invalid ESP/GPS mode\n");
-      break;
-  }
-}
-
 const board board_grey = {
   .board_type = "Grey",
-  .board_tick = unused_board_tick,
+  .set_bootkick = unused_set_bootkick,
   .harness_config = &white_harness_config,
-  .has_gps = true,
   .has_hw_gmlan = true,
   .has_obd = false,
   .has_lin = true,
@@ -48,11 +18,11 @@ const board board_grey = {
   .avdd_mV = 3300U,
   .fan_stall_recovery = false,
   .fan_enable_cooldown_time = 0U,
-  .init = grey_init,
+  .init = white_grey_init,
+  .init_bootloader = white_grey_init_bootloader,
   .enable_can_transceiver = white_enable_can_transceiver,
   .enable_can_transceivers = white_enable_can_transceivers,
   .set_led = white_set_led,
-  .set_gps_mode = grey_set_gps_mode,
   .set_can_mode = white_set_can_mode,
   .check_ignition = white_check_ignition,
   .read_current = white_read_current,
diff --git a/panda/board/boards/pedal.h b/panda/board/boards/pedal.h
index d545a9faf1..42dd8f7efe 100644
--- a/panda/board/boards/pedal.h
+++ b/panda/board/boards/pedal.h
@@ -30,11 +30,6 @@ void pedal_set_led(uint8_t color, bool enabled) {
   }
 }
 
-void pedal_set_gps_mode(uint8_t mode) {
-  UNUSED(mode);
-  print("Trying to set ESP/GPS mode on pedal. This is not supported.\n");
-}
-
 void pedal_set_can_mode(uint8_t mode){
   switch (mode) {
     case CAN_MODE_NORMAL:
@@ -73,9 +68,8 @@ const harness_configuration pedal_harness_config = {
 
 const board board_pedal = {
   .board_type = "Pedal",
-  .board_tick = unused_board_tick,
+  .set_bootkick = unused_set_bootkick,
   .harness_config = &pedal_harness_config,
-  .has_gps = false,
   .has_hw_gmlan = false,
   .has_obd = false,
   .has_lin = false,
@@ -87,10 +81,10 @@ const board board_pedal = {
   .fan_stall_recovery = false,
   .fan_enable_cooldown_time = 0U,
   .init = pedal_init,
+  .init_bootloader = unused_init_bootloader,
   .enable_can_transceiver = pedal_enable_can_transceiver,
   .enable_can_transceivers = pedal_enable_can_transceivers,
   .set_led = pedal_set_led,
-  .set_gps_mode = pedal_set_gps_mode,
   .set_can_mode = pedal_set_can_mode,
   .check_ignition = pedal_check_ignition,
   .read_current = unused_read_current,
diff --git a/panda/board/boards/red.h b/panda/board/boards/red.h
index e7dccc00ed..16f68161a7 100644
--- a/panda/board/boards/red.h
+++ b/panda/board/boards/red.h
@@ -50,6 +50,8 @@ void red_set_led(uint8_t color, bool enabled) {
 }
 
 void red_set_can_mode(uint8_t mode) {
+  red_enable_can_transceiver(2U, false);
+  red_enable_can_transceiver(4U, false);
   switch (mode) {
     case CAN_MODE_NORMAL:
     case CAN_MODE_OBD_CAN2:
@@ -67,6 +69,7 @@ void red_set_can_mode(uint8_t mode) {
 
         set_gpio_pullup(GPIOB, 6, PULL_NONE);
         set_gpio_alternate(GPIOB, 6, GPIO_AF9_FDCAN2);
+        red_enable_can_transceiver(2U, true);
       } else {
         // B5,B6: disable normal mode
         set_gpio_pullup(GPIOB, 5, PULL_NONE);
@@ -80,6 +83,7 @@ void red_set_can_mode(uint8_t mode) {
 
         set_gpio_pullup(GPIOB, 13, PULL_NONE);
         set_gpio_alternate(GPIOB, 13, GPIO_AF9_FDCAN2);
+        red_enable_can_transceiver(4U, true);
       }
       break;
     default:
@@ -168,9 +172,8 @@ const harness_configuration red_harness_config = {
 
 const board board_red = {
   .board_type = "Red",
-  .board_tick = unused_board_tick,
+  .set_bootkick = unused_set_bootkick,
   .harness_config = &red_harness_config,
-  .has_gps = false,
   .has_hw_gmlan = false,
   .has_obd = true,
   .has_lin = false,
@@ -182,10 +185,10 @@ const board board_red = {
   .fan_stall_recovery = false,
   .fan_enable_cooldown_time = 0U,
   .init = red_init,
+  .init_bootloader = unused_init_bootloader,
   .enable_can_transceiver = red_enable_can_transceiver,
   .enable_can_transceivers = red_enable_can_transceivers,
   .set_led = red_set_led,
-  .set_gps_mode = unused_set_gps_mode,
   .set_can_mode = red_set_can_mode,
   .check_ignition = red_check_ignition,
   .read_current = unused_read_current,
diff --git a/panda/board/boards/red_chiplet.h b/panda/board/boards/red_chiplet.h
index 8a3f8dbd98..1301cd3e54 100644
--- a/panda/board/boards/red_chiplet.h
+++ b/panda/board/boards/red_chiplet.h
@@ -35,6 +35,48 @@ void red_chiplet_enable_can_transceivers(bool enabled) {
   }
 }
 
+void red_chiplet_set_can_mode(uint8_t mode) {
+  red_chiplet_enable_can_transceiver(2U, false);
+  red_chiplet_enable_can_transceiver(4U, false);
+  switch (mode) {
+    case CAN_MODE_NORMAL:
+    case CAN_MODE_OBD_CAN2:
+      if ((bool)(mode == CAN_MODE_NORMAL) != (bool)(harness.status == HARNESS_STATUS_FLIPPED)) {
+        // B12,B13: disable normal mode
+        set_gpio_pullup(GPIOB, 12, PULL_NONE);
+        set_gpio_mode(GPIOB, 12, MODE_ANALOG);
+
+        set_gpio_pullup(GPIOB, 13, PULL_NONE);
+        set_gpio_mode(GPIOB, 13, MODE_ANALOG);
+
+        // B5,B6: FDCAN2 mode
+        set_gpio_pullup(GPIOB, 5, PULL_NONE);
+        set_gpio_alternate(GPIOB, 5, GPIO_AF9_FDCAN2);
+
+        set_gpio_pullup(GPIOB, 6, PULL_NONE);
+        set_gpio_alternate(GPIOB, 6, GPIO_AF9_FDCAN2);
+        red_chiplet_enable_can_transceiver(2U, true);
+      } else {
+        // B5,B6: disable normal mode
+        set_gpio_pullup(GPIOB, 5, PULL_NONE);
+        set_gpio_mode(GPIOB, 5, MODE_ANALOG);
+
+        set_gpio_pullup(GPIOB, 6, PULL_NONE);
+        set_gpio_mode(GPIOB, 6, MODE_ANALOG);
+        // B12,B13: FDCAN2 mode
+        set_gpio_pullup(GPIOB, 12, PULL_NONE);
+        set_gpio_alternate(GPIOB, 12, GPIO_AF9_FDCAN2);
+
+        set_gpio_pullup(GPIOB, 13, PULL_NONE);
+        set_gpio_alternate(GPIOB, 13, GPIO_AF9_FDCAN2);
+        red_chiplet_enable_can_transceiver(4U, true);
+      }
+      break;
+    default:
+      break;
+  }
+}
+
 void red_chiplet_set_fan_or_usb_load_switch(bool enabled) {
   set_gpio_output(GPIOD, 3, enabled);
 }
@@ -89,7 +131,7 @@ void red_chiplet_init(void) {
   red_set_led(LED_BLUE, false);
 
   // Set normal CAN mode
-  red_set_can_mode(CAN_MODE_NORMAL);
+  red_chiplet_set_can_mode(CAN_MODE_NORMAL);
 
   // flip CAN0 and CAN2 if we are flipped
   if (harness.status == HARNESS_STATUS_FLIPPED) {
diff --git a/panda/board/boards/red_v2.h b/panda/board/boards/red_v2.h
index d6724d8bb7..6554ef4e1e 100644
--- a/panda/board/boards/red_v2.h
+++ b/panda/board/boards/red_v2.h
@@ -12,9 +12,8 @@ void red_panda_v2_init(void) {
 
 const board board_red_v2 = {
   .board_type = "Red_v2",
-  .board_tick = unused_board_tick,
+  .set_bootkick = unused_set_bootkick,
   .harness_config = &red_chiplet_harness_config,
-  .has_gps = false,
   .has_hw_gmlan = false,
   .has_obd = true,
   .has_lin = false,
@@ -26,11 +25,11 @@ const board board_red_v2 = {
   .fan_stall_recovery = false,
   .fan_enable_cooldown_time = 0U,
   .init = red_panda_v2_init,
+  .init_bootloader = unused_init_bootloader,
   .enable_can_transceiver = red_chiplet_enable_can_transceiver,
   .enable_can_transceivers = red_chiplet_enable_can_transceivers,
   .set_led = red_set_led,
-  .set_gps_mode = unused_set_gps_mode,
-  .set_can_mode = red_set_can_mode,
+  .set_can_mode = red_chiplet_set_can_mode,
   .check_ignition = red_check_ignition,
   .read_current = unused_read_current,
   .set_fan_enabled = unused_set_fan_enabled,
diff --git a/panda/board/boards/tres.h b/panda/board/boards/tres.h
index 97dac7aa0f..da1cc7e68b 100644
--- a/panda/board/boards/tres.h
+++ b/panda/board/boards/tres.h
@@ -14,26 +14,9 @@ void tres_set_ir_power(uint8_t percentage){
   pwm_set(TIM3, 4, percentage);
 }
 
-void tres_set_bootkick(bool enabled){
-  set_gpio_output(GPIOA, 0, !enabled);
-}
-
-bool tres_ignition_prev = false;
-bool tres_board_tick(bool ignition, bool usb_enum, bool heartbeat_seen, bool harness_inserted) {
-  UNUSED(usb_enum);
-  bool ret = false;
-  if ((ignition && !tres_ignition_prev) || harness_inserted) {
-    // enable bootkick on rising edge of ignition
-    ret = true;
-    tres_set_bootkick(true);
-  } else if (heartbeat_seen) {
-    // disable once openpilot is up
-    tres_set_bootkick(false);
-  } else {
-
-  }
-  tres_ignition_prev = ignition;
-  return ret;
+void tres_set_bootkick(BootState state) {
+  set_gpio_output(GPIOA, 0, state != BOOT_BOOTKICK);
+  set_gpio_output(GPIOC, 12, state != BOOT_RESET);
 }
 
 void tres_set_fan_enabled(bool enabled) {
@@ -42,7 +25,7 @@ void tres_set_fan_enabled(bool enabled) {
   tres_update_fan_ir_power();
 }
 
-bool tres_read_som_gpio (void){
+bool tres_read_som_gpio (void) {
   return (get_gpio_input(GPIOC, 2) != 0);
 }
 
@@ -58,7 +41,9 @@ void tres_init(void) {
   set_gpio_mode(GPIOC, 2, MODE_INPUT);
   set_gpio_pullup(GPIOC, 2, PULL_DOWN);
 
-  tres_set_bootkick(true);
+  // SOM bootkick + reset lines
+  set_gpio_mode(GPIOC, 12, MODE_OUTPUT);
+  tres_set_bootkick(BOOT_BOOTKICK);
 
   // SOM debugging UART
   gpio_uart7_init();
@@ -87,9 +72,7 @@ void tres_init(void) {
 
 const board board_tres = {
   .board_type = "Tres",
-  .board_tick = tres_board_tick,
   .harness_config = &red_chiplet_harness_config,
-  .has_gps = false,
   .has_hw_gmlan = false,
   .has_obd = true,
   .has_lin = false,
@@ -101,16 +84,17 @@ const board board_tres = {
   .fan_stall_recovery = false,
   .fan_enable_cooldown_time = 3U,
   .init = tres_init,
+  .init_bootloader = unused_init_bootloader,
   .enable_can_transceiver = red_chiplet_enable_can_transceiver,
   .enable_can_transceivers = red_chiplet_enable_can_transceivers,
   .set_led = red_set_led,
-  .set_gps_mode = unused_set_gps_mode,
-  .set_can_mode = red_set_can_mode,
+  .set_can_mode = red_chiplet_set_can_mode,
   .check_ignition = red_check_ignition,
   .read_current = unused_read_current,
   .set_fan_enabled = tres_set_fan_enabled,
   .set_ir_power = tres_set_ir_power,
   .set_phone_power = unused_set_phone_power,
   .set_siren = fake_siren_set,
+  .set_bootkick = tres_set_bootkick,
   .read_som_gpio = tres_read_som_gpio
 };
diff --git a/panda/board/boards/uno.h b/panda/board/boards/uno.h
index ad773f6e49..6fe6177399 100644
--- a/panda/board/boards/uno.h
+++ b/panda/board/boards/uno.h
@@ -1,8 +1,6 @@
 // ///////////// //
 // Uno + Harness //
 // ///////////// //
-#define BOOTKICK_TIME 3U
-uint8_t bootkick_timer = 0U;
 
 void uno_enable_can_transceiver(uint8_t transceiver, bool enabled) {
   switch (transceiver){
@@ -51,12 +49,8 @@ void uno_set_led(uint8_t color, bool enabled) {
   }
 }
 
-void uno_set_gps_load_switch(bool enabled) {
-  set_gpio_output(GPIOC, 12, enabled);
-}
-
-void uno_set_bootkick(bool enabled){
-  if (enabled) {
+void uno_set_bootkick(BootState state) {
+  if (state == BOOT_BOOTKICK) {
     set_gpio_output(GPIOB, 14, false);
   } else {
     // We want the pin to be floating, not forced high!
@@ -64,41 +58,13 @@ void uno_set_bootkick(bool enabled){
   }
 }
 
-void uno_bootkick(void) {
-  bootkick_timer = BOOTKICK_TIME;
-  uno_set_bootkick(true);
-}
-
 void uno_set_phone_power(bool enabled){
   set_gpio_output(GPIOB, 4, enabled);
 }
 
-void uno_set_gps_mode(uint8_t mode) {
-  switch (mode) {
-    case GPS_DISABLED:
-      // GPS OFF
-      set_gpio_output(GPIOB, 1, 0);
-      set_gpio_output(GPIOC, 5, 0);
-      uno_set_gps_load_switch(false);
-      break;
-    case GPS_ENABLED:
-      // GPS ON
-      set_gpio_output(GPIOB, 1, 1);
-      set_gpio_output(GPIOC, 5, 1);
-      uno_set_gps_load_switch(true);
-      break;
-    case GPS_BOOTMODE:
-      set_gpio_output(GPIOB, 1, 1);
-      set_gpio_output(GPIOC, 5, 0);
-      uno_set_gps_load_switch(true);
-      break;
-    default:
-      print("Invalid ESP/GPS mode\n");
-      break;
-  }
-}
-
-void uno_set_can_mode(uint8_t mode){
+void uno_set_can_mode(uint8_t mode) {
+  uno_enable_can_transceiver(2U, false);
+  uno_enable_can_transceiver(4U, false);
   switch (mode) {
     case CAN_MODE_NORMAL:
     case CAN_MODE_OBD_CAN2:
@@ -110,6 +76,7 @@ void uno_set_can_mode(uint8_t mode){
         // B5,B6: normal CAN2 mode
         set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2);
         set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2);
+        uno_enable_can_transceiver(2U, true);
       } else {
         // B5,B6: disable normal CAN2 mode
         set_gpio_mode(GPIOB, 5, MODE_INPUT);
@@ -118,6 +85,7 @@ void uno_set_can_mode(uint8_t mode){
         // B12,B13: OBD mode
         set_gpio_alternate(GPIOB, 12, GPIO_AF9_CAN2);
         set_gpio_alternate(GPIOB, 13, GPIO_AF9_CAN2);
+        uno_enable_can_transceiver(4U, true);
       }
       break;
     default:
@@ -126,19 +94,6 @@ void uno_set_can_mode(uint8_t mode){
   }
 }
 
-bool uno_board_tick(bool ignition, bool usb_enum, bool heartbeat_seen, bool harness_inserted) {
-  UNUSED(ignition);
-  UNUSED(usb_enum);
-  UNUSED(heartbeat_seen);
-  UNUSED(harness_inserted);
-  if (bootkick_timer != 0U) {
-    bootkick_timer--;
-  } else {
-    uno_set_bootkick(false);
-  }
-  return false;
-}
-
 bool uno_check_ignition(void){
   // ignition is checked through harness
   return harness_check_ignition();
@@ -168,8 +123,10 @@ void uno_init(void) {
   set_gpio_mode(GPIOC, 0, MODE_ANALOG);
   set_gpio_mode(GPIOC, 3, MODE_ANALOG);
 
-  // Set default state of GPS
-  current_board->set_gps_mode(GPS_ENABLED);
+  // GPS off
+  set_gpio_output(GPIOB, 1, 0);
+  set_gpio_output(GPIOC, 5, 0);
+  set_gpio_output(GPIOC, 12, 0);
 
   // C10: OBD_SBU1_RELAY (harness relay driving output)
   // C11: OBD_SBU2_RELAY (harness relay driving output)
@@ -183,9 +140,6 @@ void uno_init(void) {
   // C8: FAN PWM aka TIM3_CH3
   set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3);
 
-  // Turn on GPS load switch.
-  uno_set_gps_load_switch(true);
-
   // Turn on phone regulator
   uno_set_phone_power(true);
 
@@ -224,7 +178,14 @@ void uno_init(void) {
   }
 
   // Bootkick phone
-  uno_bootkick();
+  uno_set_bootkick(BOOT_BOOTKICK);
+}
+
+void uno_init_bootloader(void) {
+  // GPS off
+  set_gpio_output(GPIOB, 1, 0);
+  set_gpio_output(GPIOC, 5, 0);
+  set_gpio_output(GPIOC, 12, 0);
 }
 
 const harness_configuration uno_harness_config = {
@@ -243,9 +204,7 @@ const harness_configuration uno_harness_config = {
 
 const board board_uno = {
   .board_type = "Uno",
-  .board_tick = uno_board_tick,
   .harness_config = &uno_harness_config,
-  .has_gps = true,
   .has_hw_gmlan = false,
   .has_obd = true,
   .has_lin = false,
@@ -257,10 +216,10 @@ const board board_uno = {
   .fan_stall_recovery = false,
   .fan_enable_cooldown_time = 0U,
   .init = uno_init,
+  .init_bootloader = uno_init_bootloader,
   .enable_can_transceiver = uno_enable_can_transceiver,
   .enable_can_transceivers = uno_enable_can_transceivers,
   .set_led = uno_set_led,
-  .set_gps_mode = uno_set_gps_mode,
   .set_can_mode = uno_set_can_mode,
   .check_ignition = uno_check_ignition,
   .read_current = unused_read_current,
@@ -268,5 +227,6 @@ const board board_uno = {
   .set_ir_power = uno_set_ir_power,
   .set_phone_power = uno_set_phone_power,
   .set_siren = unused_set_siren,
+  .set_bootkick = uno_set_bootkick,
   .read_som_gpio = unused_read_som_gpio
 };
diff --git a/panda/board/boards/unused_funcs.h b/panda/board/boards/unused_funcs.h
index 4e6214e6a7..edc83e3e05 100644
--- a/panda/board/boards/unused_funcs.h
+++ b/panda/board/boards/unused_funcs.h
@@ -1,5 +1,4 @@
-void unused_set_gps_mode(uint8_t mode) {
-  UNUSED(mode);
+void unused_init_bootloader(void) {
 }
 
 void unused_set_ir_power(uint8_t percentage) {
@@ -22,12 +21,8 @@ uint32_t unused_read_current(void) {
   return 0U;
 }
 
-bool unused_board_tick(bool ignition, bool usb_enum, bool heartbeat_seen, bool harness_inserted) {
-  UNUSED(ignition);
-  UNUSED(usb_enum);
-  UNUSED(heartbeat_seen);
-  UNUSED(harness_inserted);
-  return false;
+void unused_set_bootkick(BootState state) {
+  UNUSED(state);
 }
 
 bool unused_read_som_gpio(void) {
diff --git a/panda/board/boards/white.h b/panda/board/boards/white.h
index 45d860181b..4bd4381a51 100644
--- a/panda/board/boards/white.h
+++ b/panda/board/boards/white.h
@@ -65,23 +65,6 @@ void white_set_usb_power_mode(uint8_t mode){
   }
 }
 
-void white_set_gps_mode(uint8_t mode) {
-  switch (mode) {
-    case GPS_DISABLED:
-      // ESP OFF
-      set_gpio_output(GPIOC, 14, 0);
-      set_gpio_output(GPIOC, 5, 0);
-      break;
-    case GPS_BOOTMODE:
-      set_gpio_output(GPIOC, 14, 1);
-      set_gpio_output(GPIOC, 5, 0);
-      break;
-    default:
-      print("Invalid ESP/GPS mode\n");
-      break;
-  }
-}
-
 void white_set_can_mode(uint8_t mode){
   switch (mode) {
     case CAN_MODE_NORMAL:
@@ -150,7 +133,7 @@ bool white_check_ignition(void){
   return !get_gpio_input(GPIOA, 1);
 }
 
-void white_grey_common_init(void) {
+void white_grey_init(void) {
   common_init_gpio();
 
   // C3: current sense
@@ -222,13 +205,16 @@ void white_grey_common_init(void) {
   } else {
     white_set_usb_power_mode(USB_POWER_CLIENT);
   }
-}
 
-void white_init(void) {
-  white_grey_common_init();
+  // ESP/GPS off
+  set_gpio_output(GPIOC, 5, 0);
+  set_gpio_output(GPIOC, 14, 0);
+}
 
-  // Set ESP off by default
-  current_board->set_gps_mode(GPS_DISABLED);
+void white_grey_init_bootloader(void) {
+  // ESP/GPS off
+  set_gpio_output(GPIOC, 5, 0);
+  set_gpio_output(GPIOC, 14, 0);
 }
 
 const harness_configuration white_harness_config = {
@@ -237,9 +223,8 @@ const harness_configuration white_harness_config = {
 
 const board board_white = {
   .board_type = "White",
-  .board_tick = unused_board_tick,
+  .set_bootkick = unused_set_bootkick,
   .harness_config = &white_harness_config,
-  .has_gps = false,
   .has_hw_gmlan = true,
   .has_obd = false,
   .has_lin = true,
@@ -250,11 +235,11 @@ const board board_white = {
   .avdd_mV = 3300U,
   .fan_stall_recovery = false,
   .fan_enable_cooldown_time = 0U,
-  .init = white_init,
+  .init = white_grey_init,
+  .init_bootloader = white_grey_init_bootloader,
   .enable_can_transceiver = white_enable_can_transceiver,
   .enable_can_transceivers = white_enable_can_transceivers,
   .set_led = white_set_led,
-  .set_gps_mode = white_set_gps_mode,
   .set_can_mode = white_set_can_mode,
   .check_ignition = white_check_ignition,
   .read_current = white_read_current,
diff --git a/panda/board/bootstub.c b/panda/board/bootstub.c
index a2bf7dad7a..e2977e938c 100644
--- a/panda/board/bootstub.c
+++ b/panda/board/bootstub.c
@@ -41,6 +41,10 @@ int main(void) {
   clock_init();
   detect_board_type();
 
+#ifdef PANDA_JUNGLE
+  current_board->set_panda_power(true);
+#endif
+
   if (enter_bootloader_mode == ENTER_SOFTLOADER_MAGIC) {
     enter_bootloader_mode = 0;
     soft_flasher_start();
diff --git a/panda/board/config.h b/panda/board/config.h
index 903161876c..507d19e7d3 100644
--- a/panda/board/config.h
+++ b/panda/board/config.h
@@ -21,10 +21,18 @@
 // USB definitions
 #define USB_VID 0xBBAAU
 
-#ifdef BOOTSTUB
-  #define USB_PID 0xDDEEU
+#ifdef PANDA_JUNGLE
+  #ifdef BOOTSTUB
+    #define USB_PID 0xDDEFU
+  #else
+    #define USB_PID 0xDDCFU
+  #endif
 #else
-  #define USB_PID 0xDDCCU
+  #ifdef BOOTSTUB
+    #define USB_PID 0xDDEEU
+  #else
+    #define USB_PID 0xDDCCU
+  #endif
 #endif
 
 // platform includes
diff --git a/panda/board/debug/README.md b/panda/board/debug/README.md
new file mode 100644
index 0000000000..d89eab818a
--- /dev/null
+++ b/panda/board/debug/README.md
@@ -0,0 +1,26 @@
+# In-circuit debugging using openocd and gdb
+
+## Hardware
+Connect an ST-Link V2 programmer to the SWD pins on the board. The pins that need to be connected are:
+- GND
+- VTref
+- SWDIO
+- SWCLK
+- NRST
+
+Make sure you're using a genuine one for boards that do not have a 3.3V panda power rail. For example, the tres runs at 1.8V, which is not supported by the clones.
+
+## Openocd
+Install openocd. For Ubuntu 20.04, the one in the package manager works fine: `sudo apt install openocd`.
+
+To run, use `./debug_f4.sh (TODO)` or `./debug_h7.sh` depending on the panda.
+
+## GDB
+You need `gdb-multiarch`.
+
+Once openocd is running, you can connect from gdb as follows:
+```
+$ gdb-multiarch
+(gdb) target ext :3333
+```
+To reset and break, use `monitor reset halt`.
diff --git a/panda/board/debug/debug_h7.sh b/panda/board/debug/debug_h7.sh
new file mode 100755
index 0000000000..a2bd88434e
--- /dev/null
+++ b/panda/board/debug/debug_h7.sh
@@ -0,0 +1,3 @@
+#!/bin/bash -e
+
+sudo openocd -f "interface/stlink.cfg" -c "transport select hla_swd" -f "target/stm32h7x.cfg" -c "init"
diff --git a/panda/board/drivers/bootkick.h b/panda/board/drivers/bootkick.h
new file mode 100644
index 0000000000..acae3616af
--- /dev/null
+++ b/panda/board/drivers/bootkick.h
@@ -0,0 +1,67 @@
+bool bootkick_ign_prev = false;
+BootState boot_state = BOOT_BOOTKICK;
+uint8_t bootkick_harness_status_prev = HARNESS_STATUS_NC;
+
+uint8_t boot_reset_countdown = 0;
+uint8_t waiting_to_boot_countdown = 0;
+bool bootkick_reset_triggered = false;
+uint16_t bootkick_last_serial_ptr = 0;
+
+void bootkick_tick(bool ignition, bool recent_heartbeat) {
+  BootState boot_state_prev = boot_state;
+  const bool harness_inserted = (harness.status != bootkick_harness_status_prev) && (harness.status != HARNESS_STATUS_NC);
+
+  if ((ignition && !bootkick_ign_prev) || harness_inserted) {
+    // bootkick on rising edge of ignition or harness insertion
+    boot_state = BOOT_BOOTKICK;
+  } else if (recent_heartbeat) {
+    // disable bootkick once openpilot is up
+    boot_state = BOOT_STANDBY;
+  } else {
+
+  }
+
+  /*
+    Ensure SOM boots in case it goes into QDL mode. Reset behavior:
+    * shouldn't trigger on the first boot after power-on
+    * only try reset once per bootkick, i.e. don't keep trying until booted
+    * only try once per panda boot, since openpilot will reset panda on startup
+    * once BOOT_RESET is triggered, it stays until countdown is finished
+  */
+  if (!bootkick_reset_triggered && (boot_state == BOOT_BOOTKICK) && (boot_state_prev == BOOT_STANDBY)) {
+    waiting_to_boot_countdown = 45U;
+  }
+  if (waiting_to_boot_countdown > 0U) {
+    bool serial_activity = uart_ring_som_debug.w_ptr_tx != bootkick_last_serial_ptr;
+    if (serial_activity || current_board->read_som_gpio() || (boot_state != BOOT_BOOTKICK)) {
+      waiting_to_boot_countdown = 0U;
+    } else {
+      // try a reset
+      if (waiting_to_boot_countdown == 1U) {
+        boot_reset_countdown = 5U;
+      }
+    }
+  }
+
+  // handle reset state
+  if (boot_reset_countdown > 0U) {
+    boot_state = BOOT_RESET;
+    bootkick_reset_triggered = true;
+  } else {
+    if (boot_state == BOOT_RESET) {
+      boot_state = BOOT_BOOTKICK;
+    }
+  }
+
+  // update state
+  bootkick_ign_prev = ignition;
+  bootkick_harness_status_prev = harness.status;
+  bootkick_last_serial_ptr = uart_ring_som_debug.w_ptr_tx;
+  if (waiting_to_boot_countdown > 0U) {
+    waiting_to_boot_countdown--;
+  }
+  if (boot_reset_countdown > 0U) {
+    boot_reset_countdown--;
+  }
+  current_board->set_bootkick(boot_state);
+}
diff --git a/panda/board/drivers/bxcan.h b/panda/board/drivers/bxcan.h
index 478bd5858b..080c9f4a9d 100644
--- a/panda/board/drivers/bxcan.h
+++ b/panda/board/drivers/bxcan.h
@@ -11,11 +11,11 @@ uint8_t can_irq_number[3][3] = {
 
 bool can_set_speed(uint8_t can_number) {
   bool ret = true;
-  CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
+  CAN_TypeDef *CANx = CANIF_FROM_CAN_NUM(can_number);
   uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
 
   ret &= llcan_set_speed(
-    CAN,
+    CANx,
     bus_config[bus_number].can_speed,
     can_loopback,
     (unsigned int)(can_silent) & (1U << can_number)
@@ -74,8 +74,8 @@ void can_set_gmlan(uint8_t bus) {
 }
 
 void update_can_health_pkt(uint8_t can_number, uint32_t ir_reg) {
-  CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
-  uint32_t esr_reg = CAN->ESR;
+  CAN_TypeDef *CANx = CANIF_FROM_CAN_NUM(can_number);
+  uint32_t esr_reg = CANx->ESR;
 
   can_health[can_number].bus_off = ((esr_reg & CAN_ESR_BOFF) >> CAN_ESR_BOFF_Pos);
   can_health[can_number].bus_off_cnt += can_health[can_number].bus_off;
@@ -98,12 +98,12 @@ void update_can_health_pkt(uint8_t can_number, uint32_t ir_reg) {
     can_health[can_number].total_error_cnt += 1U;
 
     // RX message lost due to FIFO overrun
-    if ((CAN->RF0R & (CAN_RF0R_FOVR0)) != 0) {
+    if ((CANx->RF0R & (CAN_RF0R_FOVR0)) != 0) {
       can_health[can_number].total_rx_lost_cnt += 1U;
-      CAN->RF0R &= ~(CAN_RF0R_FOVR0);
+      CANx->RF0R &= ~(CAN_RF0R_FOVR0);
     }
     can_health[can_number].can_core_reset_cnt += 1U;
-    llcan_clear_send(CAN);
+    llcan_clear_send(CANx);
   }
 }
 
@@ -119,28 +119,28 @@ void process_can(uint8_t can_number) {
 
     ENTER_CRITICAL();
 
-    CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
+    CAN_TypeDef *CANx = CANIF_FROM_CAN_NUM(can_number);
     uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
 
     // check for empty mailbox
     CANPacket_t to_send;
-    if ((CAN->TSR & (CAN_TSR_TERR0 | CAN_TSR_ALST0)) != 0) { // last TX failed due to error arbitration lost
+    if ((CANx->TSR & (CAN_TSR_TERR0 | CAN_TSR_ALST0)) != 0) { // last TX failed due to error arbitration lost
       can_health[can_number].total_tx_lost_cnt += 1U;
-      CAN->TSR |= (CAN_TSR_TERR0 | CAN_TSR_ALST0);
+      CANx->TSR |= (CAN_TSR_TERR0 | CAN_TSR_ALST0);
     }
-    if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
+    if ((CANx->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
       // add successfully transmitted message to my fifo
-      if ((CAN->TSR & CAN_TSR_RQCP0) == CAN_TSR_RQCP0) {
-        if ((CAN->TSR & CAN_TSR_TXOK0) == CAN_TSR_TXOK0) {
+      if ((CANx->TSR & CAN_TSR_RQCP0) == CAN_TSR_RQCP0) {
+        if ((CANx->TSR & CAN_TSR_TXOK0) == CAN_TSR_TXOK0) {
           CANPacket_t to_push;
           to_push.returned = 1U;
           to_push.rejected = 0U;
-          to_push.extended = (CAN->sTxMailBox[0].TIR >> 2) & 0x1U;
-          to_push.addr = (to_push.extended != 0U) ? (CAN->sTxMailBox[0].TIR >> 3) : (CAN->sTxMailBox[0].TIR >> 21);
-          to_push.data_len_code = CAN->sTxMailBox[0].TDTR & 0xFU;
+          to_push.extended = (CANx->sTxMailBox[0].TIR >> 2) & 0x1U;
+          to_push.addr = (to_push.extended != 0U) ? (CANx->sTxMailBox[0].TIR >> 3) : (CANx->sTxMailBox[0].TIR >> 21);
+          to_push.data_len_code = CANx->sTxMailBox[0].TDTR & 0xFU;
           to_push.bus = bus_number;
-          WORD_TO_BYTE_ARRAY(&to_push.data[0], CAN->sTxMailBox[0].TDLR);
-          WORD_TO_BYTE_ARRAY(&to_push.data[4], CAN->sTxMailBox[0].TDHR);
+          WORD_TO_BYTE_ARRAY(&to_push.data[0], CANx->sTxMailBox[0].TDLR);
+          WORD_TO_BYTE_ARRAY(&to_push.data[4], CANx->sTxMailBox[0].TDHR);
           can_set_checksum(&to_push);
 
           rx_buffer_overflow += can_push(&can_rx_q, &to_push) ? 0U : 1U;
@@ -148,19 +148,19 @@ void process_can(uint8_t can_number) {
 
         // clear interrupt
         // careful, this can also be cleared by requesting a transmission
-        CAN->TSR |= CAN_TSR_RQCP0;
+        CANx->TSR |= CAN_TSR_RQCP0;
       }
 
       if (can_pop(can_queues[bus_number], &to_send)) {
         if (can_check_checksum(&to_send)) {
           can_health[can_number].total_tx_cnt += 1U;
           // only send if we have received a packet
-          CAN->sTxMailBox[0].TIR = ((to_send.extended != 0U) ? (to_send.addr << 3) : (to_send.addr << 21)) | (to_send.extended << 2);
-          CAN->sTxMailBox[0].TDTR = to_send.data_len_code;
-          BYTE_ARRAY_TO_WORD(CAN->sTxMailBox[0].TDLR, &to_send.data[0]);
-          BYTE_ARRAY_TO_WORD(CAN->sTxMailBox[0].TDHR, &to_send.data[4]);
+          CANx->sTxMailBox[0].TIR = ((to_send.extended != 0U) ? (to_send.addr << 3) : (to_send.addr << 21)) | (to_send.extended << 2);
+          CANx->sTxMailBox[0].TDTR = to_send.data_len_code;
+          BYTE_ARRAY_TO_WORD(CANx->sTxMailBox[0].TDLR, &to_send.data[0]);
+          BYTE_ARRAY_TO_WORD(CANx->sTxMailBox[0].TDHR, &to_send.data[4]);
           // Send request TXRQ
-          CAN->sTxMailBox[0].TIR |= 0x1U;
+          CANx->sTxMailBox[0].TIR |= 0x1U;
         } else {
           can_health[can_number].total_tx_checksum_error_cnt += 1U;
         }
@@ -176,10 +176,10 @@ void process_can(uint8_t can_number) {
 // CANx_RX0 IRQ Handler
 // blink blue when we are receiving CAN messages
 void can_rx(uint8_t can_number) {
-  CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
+  CAN_TypeDef *CANx = CANIF_FROM_CAN_NUM(can_number);
   uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
 
-  while ((CAN->RF0R & CAN_RF0R_FMP0) != 0) {
+  while ((CANx->RF0R & CAN_RF0R_FMP0) != 0) {
     can_health[can_number].total_rx_cnt += 1U;
 
     // can is live
@@ -190,12 +190,12 @@ void can_rx(uint8_t can_number) {
 
     to_push.returned = 0U;
     to_push.rejected = 0U;
-    to_push.extended = (CAN->sFIFOMailBox[0].RIR >> 2) & 0x1U;
-    to_push.addr = (to_push.extended != 0U) ? (CAN->sFIFOMailBox[0].RIR >> 3) : (CAN->sFIFOMailBox[0].RIR >> 21);
-    to_push.data_len_code = CAN->sFIFOMailBox[0].RDTR & 0xFU;
+    to_push.extended = (CANx->sFIFOMailBox[0].RIR >> 2) & 0x1U;
+    to_push.addr = (to_push.extended != 0U) ? (CANx->sFIFOMailBox[0].RIR >> 3) : (CANx->sFIFOMailBox[0].RIR >> 21);
+    to_push.data_len_code = CANx->sFIFOMailBox[0].RDTR & 0xFU;
     to_push.bus = bus_number;
-    WORD_TO_BYTE_ARRAY(&to_push.data[0], CAN->sFIFOMailBox[0].RDLR);
-    WORD_TO_BYTE_ARRAY(&to_push.data[4], CAN->sFIFOMailBox[0].RDHR);
+    WORD_TO_BYTE_ARRAY(&to_push.data[0], CANx->sFIFOMailBox[0].RDLR);
+    WORD_TO_BYTE_ARRAY(&to_push.data[4], CANx->sFIFOMailBox[0].RDHR);
     can_set_checksum(&to_push);
 
     // forwarding (panda only)
@@ -223,7 +223,7 @@ void can_rx(uint8_t can_number) {
     rx_buffer_overflow += can_push(&can_rx_q, &to_push) ? 0U : 1U;
 
     // next
-    CAN->RF0R |= CAN_RF0R_RFOM0;
+    CANx->RF0R |= CAN_RF0R_RFOM0;
   }
 }
 
@@ -253,9 +253,9 @@ bool can_init(uint8_t can_number) {
   REGISTER_INTERRUPT(CAN3_SCE_IRQn, CAN3_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_3)
 
   if (can_number != 0xffU) {
-    CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
+    CAN_TypeDef *CANx = CANIF_FROM_CAN_NUM(can_number);
     ret &= can_set_speed(can_number);
-    ret &= llcan_init(CAN);
+    ret &= llcan_init(CANx);
     // in case there are queued up messages
     process_can(can_number);
   }
diff --git a/panda/board/drivers/can_common.h b/panda/board/drivers/can_common.h
index be6b5b2a5d..5c53862877 100644
--- a/panda/board/drivers/can_common.h
+++ b/panda/board/drivers/can_common.h
@@ -8,6 +8,7 @@ typedef struct {
 typedef struct {
   uint8_t bus_lookup;
   uint8_t can_num_lookup;
+  int8_t forwarding_bus;
   uint32_t can_speed;
   uint32_t can_data_speed;
   bool canfd_enabled;
@@ -51,17 +52,22 @@ void process_can(uint8_t can_number);
   CANPacket_t elems_##x[size]; \
   can_ring can_##x = { .w_ptr = 0, .r_ptr = 0, .fifo_size = (size), .elems = (CANPacket_t *)&(elems_##x) };
 
+#define CAN_RX_BUFFER_SIZE 4096U
+#define CAN_TX_BUFFER_SIZE 416U
+#define GMLAN_TX_BUFFER_SIZE 416U
+
 #ifdef STM32H7
-__attribute__((section(".ram_d1"))) can_buffer(rx_q, 0x1000)
-__attribute__((section(".ram_d1"))) can_buffer(tx2_q, 0x1A0)
-__attribute__((section(".ram_d2"))) can_buffer(txgmlan_q, 0x1A0)
+// ITCM RAM and DTCM RAM are the fastest for Cortex-M7 core access
+__attribute__((section(".axisram"))) can_buffer(rx_q, CAN_RX_BUFFER_SIZE)
+__attribute__((section(".itcmram"))) can_buffer(tx1_q, CAN_TX_BUFFER_SIZE)
+__attribute__((section(".itcmram"))) can_buffer(tx2_q, CAN_TX_BUFFER_SIZE)
 #else
-can_buffer(rx_q, 0x1000)
-can_buffer(tx2_q, 0x1A0)
-can_buffer(txgmlan_q, 0x1A0)
+can_buffer(rx_q, CAN_RX_BUFFER_SIZE)
+can_buffer(tx1_q, CAN_TX_BUFFER_SIZE)
+can_buffer(tx2_q, CAN_TX_BUFFER_SIZE)
 #endif
-can_buffer(tx1_q, 0x1A0)
-can_buffer(tx3_q, 0x1A0)
+can_buffer(tx3_q, CAN_TX_BUFFER_SIZE)
+can_buffer(txgmlan_q, GMLAN_TX_BUFFER_SIZE)
 // FIXME:
 // cppcheck-suppress misra-c2012-9.3
 can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q, &can_tx3_q, &can_txgmlan_q};
@@ -157,14 +163,15 @@ void can_clear(can_ring *q) {
 // can number: numeric lookup for MCU CAN interfaces (0 = CAN1, 1 = CAN2, etc);
 // bus_lookup: Translates from 'can number' to 'bus number'.
 // can_num_lookup: Translates from 'bus number' to 'can number'.
+// forwarding bus: If >= 0, forward all messages from this bus to the specified bus.
 
 // Helpers
 // Panda:       Bus 0=CAN1   Bus 1=CAN2   Bus 2=CAN3
 bus_config_t bus_config[] = {
-  { .bus_lookup = 0U, .can_num_lookup = 0U, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false },
-  { .bus_lookup = 1U, .can_num_lookup = 1U, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false },
-  { .bus_lookup = 2U, .can_num_lookup = 2U, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false },
-  { .bus_lookup = 0xFFU, .can_num_lookup = 0xFFU, .can_speed = 333U, .can_data_speed = 333U, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false },
+  { .bus_lookup = 0U, .can_num_lookup = 0U, .forwarding_bus = -1, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false },
+  { .bus_lookup = 1U, .can_num_lookup = 1U, .forwarding_bus = -1, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false },
+  { .bus_lookup = 2U, .can_num_lookup = 2U, .forwarding_bus = -1, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false },
+  { .bus_lookup = 0xFFU, .can_num_lookup = 0xFFU, .forwarding_bus = -1, .can_speed = 333U, .can_data_speed = 333U, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false },
 };
 
 #define CANIF_FROM_CAN_NUM(num) (cans[num])
@@ -190,6 +197,10 @@ void can_flip_buses(uint8_t bus1, uint8_t bus2){
   bus_config[bus2].can_num_lookup = bus1;
 }
 
+void can_set_forwarding(uint8_t from, uint8_t to) {
+  bus_config[from].forwarding_bus = to;
+}
+
 void ignition_can_hook(CANPacket_t *to_push) {
   int bus = GET_BUS(to_push);
   int addr = GET_ADDR(to_push);
@@ -197,9 +208,9 @@ void ignition_can_hook(CANPacket_t *to_push) {
 
   if (bus == 0) {
     // GM exception
-    if ((addr == 0x160) && (len == 5)) {
-      // this message isn't all zeros when ignition is on
-      ignition_can = GET_BYTES(to_push, 0, 4) != 0U;
+    if ((addr == 0x1F1) && (len == 8)) {
+      // SystemPowerMode (2=Run, 3=Crank Request)
+      ignition_can = (GET_BYTE(to_push, 0) & 0x2U) != 0U;
       ignition_can_cnt = 0U;
     }
 
diff --git a/panda/board/drivers/clock_source.h b/panda/board/drivers/clock_source.h
index d1b5724f16..11b2fa3247 100644
--- a/panda/board/drivers/clock_source.h
+++ b/panda/board/drivers/clock_source.h
@@ -1,6 +1,10 @@
 #define CLOCK_SOURCE_PERIOD_MS           50U
 #define CLOCK_SOURCE_PULSE_LEN_MS        2U
 
+void clock_source_set_period(uint8_t period) {
+  register_set(&(TIM1->ARR), ((period*10U) - 1U), 0xFFFFU);
+}
+
 void clock_source_init(void) {
   // Setup timer
   register_set(&(TIM1->PSC), ((APB2_TIMER_FREQ*100U)-1U), 0xFFFFU);           // Tick on 0.1 ms
diff --git a/panda/board/drivers/fdcan.h b/panda/board/drivers/fdcan.h
index 7cab232d36..5104f3fec9 100644
--- a/panda/board/drivers/fdcan.h
+++ b/panda/board/drivers/fdcan.h
@@ -21,11 +21,11 @@ uint8_t can_irq_number[3][2] = {
 
 bool can_set_speed(uint8_t can_number) {
   bool ret = true;
-  FDCAN_GlobalTypeDef *CANx = CANIF_FROM_CAN_NUM(can_number);
+  FDCAN_GlobalTypeDef *FDCANx = CANIF_FROM_CAN_NUM(can_number);
   uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
 
   ret &= llcan_set_speed(
-    CANx,
+    FDCANx,
     bus_config[bus_number].can_speed,
     bus_config[bus_number].can_data_speed,
     bus_config[bus_number].canfd_non_iso,
@@ -41,9 +41,9 @@ void can_set_gmlan(uint8_t bus) {
 }
 
 void update_can_health_pkt(uint8_t can_number, uint32_t ir_reg) {
-  FDCAN_GlobalTypeDef *CANx = CANIF_FROM_CAN_NUM(can_number);
-  uint32_t psr_reg = CANx->PSR;
-  uint32_t ecr_reg = CANx->ECR;
+  FDCAN_GlobalTypeDef *FDCANx = CANIF_FROM_CAN_NUM(can_number);
+  uint32_t psr_reg = FDCANx->PSR;
+  uint32_t ecr_reg = FDCANx->ECR;
 
   can_health[can_number].bus_off = ((psr_reg & FDCAN_PSR_BO) >> FDCAN_PSR_BO_Pos);
   can_health[can_number].bus_off_cnt += can_health[can_number].bus_off;
@@ -69,34 +69,36 @@ void update_can_health_pkt(uint8_t can_number, uint32_t ir_reg) {
 
   if (ir_reg != 0U) {
     // Clear error interrupts
-    CANx->IR |= (FDCAN_IR_PED | FDCAN_IR_PEA | FDCAN_IR_EP | FDCAN_IR_BO | FDCAN_IR_RF0L);
+    FDCANx->IR |= (FDCAN_IR_PED | FDCAN_IR_PEA | FDCAN_IR_EP | FDCAN_IR_BO | FDCAN_IR_RF0L);
     can_health[can_number].total_error_cnt += 1U;
     // Check for RX FIFO overflow
     if ((ir_reg & (FDCAN_IR_RF0L)) != 0) {
       can_health[can_number].total_rx_lost_cnt += 1U;
     }
-    // While multiplexing between buses 1 and 3 we are getting ACK errors that overwhelm CAN core
-    // By resseting CAN core when no ACK is detected for a while(until TEC counter reaches 127) it can recover faster
-    if (((can_health[can_number].last_error == CAN_ACK_ERROR) || (can_health[can_number].last_data_error == CAN_ACK_ERROR)) && (can_health[can_number].transmit_error_cnt > 127U)) {
+    // Cases:
+    // 1. while multiplexing between buses 1 and 3 we are getting ACK errors that overwhelm CAN core, by resetting it recovers faster
+    // 2. H7 gets stuck in bus off recovery state indefinitely
+    if ((((can_health[can_number].last_error == CAN_ACK_ERROR) || (can_health[can_number].last_data_error == CAN_ACK_ERROR)) && (can_health[can_number].transmit_error_cnt > 127U)) ||
+     ((ir_reg & FDCAN_IR_BO) != 0)) {
       can_health[can_number].can_core_reset_cnt += 1U;
-      can_health[can_number].total_tx_lost_cnt += (FDCAN_TX_FIFO_EL_CNT - (CANx->TXFQS & FDCAN_TXFQS_TFFL)); // TX FIFO msgs will be lost after reset
-      llcan_clear_send(CANx);
+      can_health[can_number].total_tx_lost_cnt += (FDCAN_TX_FIFO_EL_CNT - (FDCANx->TXFQS & FDCAN_TXFQS_TFFL)); // TX FIFO msgs will be lost after reset
+      llcan_clear_send(FDCANx);
     }
   }
 }
 
 // ***************************** CAN *****************************
-// FDCANx_IT1 IRQ Handler (TX)
+// FDFDCANx_IT1 IRQ Handler (TX)
 void process_can(uint8_t can_number) {
   if (can_number != 0xffU) {
     ENTER_CRITICAL();
 
-    FDCAN_GlobalTypeDef *CANx = CANIF_FROM_CAN_NUM(can_number);
+    FDCAN_GlobalTypeDef *FDCANx = CANIF_FROM_CAN_NUM(can_number);
     uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
 
-    CANx->IR |= FDCAN_IR_TFE; // Clear Tx FIFO Empty flag
+    FDCANx->IR |= FDCAN_IR_TFE; // Clear Tx FIFO Empty flag
 
-    if ((CANx->TXFQS & FDCAN_TXFQS_TFQF) == 0) {
+    if ((FDCANx->TXFQS & FDCAN_TXFQS_TFQF) == 0) {
       CANPacket_t to_send;
       if (can_pop(can_queues[bus_number], &to_send)) {
         if (can_check_checksum(&to_send)) {
@@ -104,7 +106,7 @@ void process_can(uint8_t can_number) {
 
           uint32_t TxFIFOSA = FDCAN_START_ADDRESS + (can_number * FDCAN_OFFSET) + (FDCAN_RX_FIFO_0_EL_CNT * FDCAN_RX_FIFO_0_EL_SIZE);
           // get the index of the next TX FIFO element (0 to FDCAN_TX_FIFO_EL_CNT - 1)
-          uint8_t tx_index = (CANx->TXFQS >> FDCAN_TXFQS_TFQPI_Pos) & 0x1F;
+          uint8_t tx_index = (FDCANx->TXFQS >> FDCAN_TXFQS_TFQPI_Pos) & 0x1F;
           // only send if we have received a packet
           canfd_fifo *fifo;
           fifo = (canfd_fifo *)(TxFIFOSA + (tx_index * FDCAN_TX_FIFO_EL_SIZE));
@@ -118,7 +120,7 @@ void process_can(uint8_t can_number) {
             BYTE_ARRAY_TO_WORD(fifo->data_word[i], &to_send.data[i*4U]);
           }
 
-          CANx->TXBAR = (1UL << tx_index);
+          FDCANx->TXBAR = (1UL << tx_index);
 
           // Send back to USB
           CANPacket_t to_push;
@@ -144,27 +146,27 @@ void process_can(uint8_t can_number) {
   }
 }
 
-// FDCANx_IT0 IRQ Handler (RX and errors)
+// FDFDCANx_IT0 IRQ Handler (RX and errors)
 // blink blue when we are receiving CAN messages
 void can_rx(uint8_t can_number) {
-  FDCAN_GlobalTypeDef *CANx = CANIF_FROM_CAN_NUM(can_number);
+  FDCAN_GlobalTypeDef *FDCANx = CANIF_FROM_CAN_NUM(can_number);
   uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
 
-  uint32_t ir_reg = CANx->IR;
+  uint32_t ir_reg = FDCANx->IR;
 
   // Clear all new messages from Rx FIFO 0
-  CANx->IR |= FDCAN_IR_RF0N;
-  while((CANx->RXF0S & FDCAN_RXF0S_F0FL) != 0) {
+  FDCANx->IR |= FDCAN_IR_RF0N;
+  while((FDCANx->RXF0S & FDCAN_RXF0S_F0FL) != 0) {
     can_health[can_number].total_rx_cnt += 1U;
 
     // can is live
     pending_can_live = 1;
 
     // get the index of the next RX FIFO element (0 to FDCAN_RX_FIFO_0_EL_CNT - 1)
-    uint8_t rx_fifo_idx = (uint8_t)((CANx->RXF0S >> FDCAN_RXF0S_F0GI_Pos) & 0x3F);
+    uint8_t rx_fifo_idx = (uint8_t)((FDCANx->RXF0S >> FDCAN_RXF0S_F0GI_Pos) & 0x3F);
 
     // Recommended to offset get index by at least +1 if RX FIFO is in overwrite mode and full (datasheet)
-    if((CANx->RXF0S & FDCAN_RXF0S_F0F) == FDCAN_RXF0S_F0F) {
+    if((FDCANx->RXF0S & FDCAN_RXF0S_F0F) == FDCAN_RXF0S_F0F) {
       rx_fifo_idx = ((rx_fifo_idx + 1U) >= FDCAN_RX_FIFO_0_EL_CNT) ? 0U : (rx_fifo_idx + 1U);
       can_health[can_number].total_rx_lost_cnt += 1U; // At least one message was lost
     }
@@ -195,6 +197,9 @@ void can_rx(uint8_t can_number) {
 
     // forwarding (panda only)
     int bus_fwd_num = safety_fwd_hook(bus_number, to_push.addr);
+    if (bus_fwd_num < 0) {
+      bus_fwd_num = bus_config[can_number].forwarding_bus;
+    }
     if (bus_fwd_num != -1) {
       CANPacket_t to_send;
 
@@ -226,7 +231,7 @@ void can_rx(uint8_t can_number) {
     }
 
     // update read index
-    CANx->RXF0A = rx_fifo_idx;
+    FDCANx->RXF0A = rx_fifo_idx;
   }
 
   // Error handling
@@ -255,9 +260,9 @@ bool can_init(uint8_t can_number) {
   REGISTER_INTERRUPT(FDCAN3_IT1_IRQn, FDCAN3_IT1_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_3)
 
   if (can_number != 0xffU) {
-    FDCAN_GlobalTypeDef *CANx = CANIF_FROM_CAN_NUM(can_number);
+    FDCAN_GlobalTypeDef *FDCANx = CANIF_FROM_CAN_NUM(can_number);
     ret &= can_set_speed(can_number);
-    ret &= llcan_init(CANx);
+    ret &= llcan_init(FDCANx);
     // in case there are queued up messages
     process_can(can_number);
   }
diff --git a/panda/board/drivers/gmlan_alt.h b/panda/board/drivers/gmlan_alt.h
index 53f46c2e02..1c20e1ed79 100644
--- a/panda/board/drivers/gmlan_alt.h
+++ b/panda/board/drivers/gmlan_alt.h
@@ -121,23 +121,23 @@ int get_bit_message(char *out, CANPacket_t *to_bang) {
   return len;
 }
 
-void TIM12_IRQ_Handler(void);
+void GMLAN_BITBANG_IRQ_Handler(void);
 
 void setup_timer(void) {
   // register interrupt
-  REGISTER_INTERRUPT(TIM8_BRK_TIM12_IRQn, TIM12_IRQ_Handler, 40000U, FAULT_INTERRUPT_RATE_GMLAN)
+  REGISTER_INTERRUPT(GMLAN_BITBANG_TIMER_IRQ, GMLAN_BITBANG_IRQ_Handler, 40000U, FAULT_INTERRUPT_RATE_GMLAN)
 
   // setup
-  register_set(&(TIM12->PSC), (48-1), 0xFFFFU);    // Tick on 1 us
-  register_set(&(TIM12->CR1), TIM_CR1_CEN, 0x3FU); // Enable
-  register_set(&(TIM12->ARR), (30-1), 0xFFFFU);   // 33.3 kbps
+  register_set(&(GMLAN_BITBANG_TIMER->PSC), (APB1_TIMER_FREQ-1U), 0xFFFFU);    // Tick on 1 us
+  register_set(&(GMLAN_BITBANG_TIMER->CR1), TIM_CR1_CEN, 0x3FU); // Enable
+  register_set(&(GMLAN_BITBANG_TIMER->ARR), (30U-1U), 0xFFFFU);   // 33.3 kbps
 
   // in case it's disabled
-  NVIC_EnableIRQ(TIM8_BRK_TIM12_IRQn);
+  NVIC_EnableIRQ(GMLAN_BITBANG_TIMER_IRQ);
 
   // run the interrupt
-  register_set(&(TIM12->DIER), TIM_DIER_UIE, 0x5F5FU); // Update interrupt
-  TIM12->SR = 0;
+  register_set(&(GMLAN_BITBANG_TIMER->DIER), TIM_DIER_UIE, 0x5F5FU); // Update interrupt
+  GMLAN_BITBANG_TIMER->SR = 0;
 }
 
 int gmlan_timeout_counter = GMLAN_TICKS_PER_TIMEOUT_TICKLE; //GMLAN transceiver times out every 17ms held high; tickle every 15ms
@@ -191,9 +191,9 @@ int gmlan_fail_count = 0;
 #define REQUIRED_SILENT_TIME 10
 #define MAX_FAIL_COUNT 10
 
-void TIM12_IRQ_Handler(void) {
+void GMLAN_BITBANG_IRQ_Handler(void) {
   if (gmlan_alt_mode == BITBANG) {
-    if ((TIM12->SR & TIM_SR_UIF) && (gmlan_sendmax != -1)) {
+    if ((GMLAN_BITBANG_TIMER->SR & TIM_SR_UIF) && (gmlan_sendmax != -1)) {
       int read = get_gpio_input(GPIOB, 12);
       if (gmlan_silent_count < REQUIRED_SILENT_TIME) {
         if (read == 0) {
@@ -235,13 +235,13 @@ void TIM12_IRQ_Handler(void) {
       if ((gmlan_sending == gmlan_sendmax) || (gmlan_fail_count == MAX_FAIL_COUNT)) {
         set_bitbanged_gmlan(1); // recessive
         set_gpio_mode(GPIOB, 13, MODE_INPUT);
-        register_clear_bits(&(TIM12->DIER), TIM_DIER_UIE); // No update interrupt
-        register_set(&(TIM12->CR1), 0U, 0x3FU); // Disable timer
+        register_clear_bits(&(GMLAN_BITBANG_TIMER->DIER), TIM_DIER_UIE); // No update interrupt
+        register_set(&(GMLAN_BITBANG_TIMER->CR1), 0U, 0x3FU); // Disable timer
         gmlan_sendmax = -1;   // exit
       }
     }
   } else if (gmlan_alt_mode == GPIO_SWITCH) {
-    if ((TIM12->SR & TIM_SR_UIF) && (gmlan_switch_below_timeout != -1)) {
+    if ((GMLAN_BITBANG_TIMER->SR & TIM_SR_UIF) && (gmlan_switch_below_timeout != -1)) {
       if ((can_timeout_counter == 0) && gmlan_switch_timeout_enable) {
         //it has been more than 1 second since timeout was reset; disable timer and restore the GMLAN output
         set_gpio_output(GPIOB, 13, GMLAN_LOW);
@@ -265,14 +265,13 @@ void TIM12_IRQ_Handler(void) {
   } else {
     // Invalid GMLAN mode. Do not put a print statement here, way too fast to keep up with
   }
-  TIM12->SR = 0;
+  GMLAN_BITBANG_TIMER->SR = 0;
 }
 
 bool bitbang_gmlan(CANPacket_t *to_bang) {
   gmlan_send_ok = true;
   gmlan_alt_mode = BITBANG;
 
-#ifndef STM32H7
   if (gmlan_sendmax == -1) {
     int len = get_bit_message(pkt_stuffed, to_bang);
     gmlan_fail_count = 0;
@@ -286,8 +285,5 @@ bool bitbang_gmlan(CANPacket_t *to_bang) {
     // 33kbps
     setup_timer();
   }
-#else
-  UNUSED(to_bang);
-#endif
   return gmlan_send_ok;
 }
diff --git a/panda/board/drivers/gpio.h b/panda/board/drivers/gpio.h
index 77af87658c..fe3194f3ba 100644
--- a/panda/board/drivers/gpio.h
+++ b/panda/board/drivers/gpio.h
@@ -10,6 +10,11 @@
 #define OUTPUT_TYPE_PUSH_PULL 0U
 #define OUTPUT_TYPE_OPEN_DRAIN 1U
 
+typedef struct {
+  GPIO_TypeDef *bank;
+  uint8_t pin;
+} gpio_t;
+
 void set_gpio_mode(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode) {
   ENTER_CRITICAL();
   uint32_t tmp = GPIO->MODER;
@@ -63,6 +68,18 @@ int get_gpio_input(GPIO_TypeDef *GPIO, unsigned int pin) {
   return (GPIO->IDR & (1U << pin)) == (1U << pin);
 }
 
+void gpio_set_all_output(const gpio_t *pins, uint8_t num_pins, bool enabled) {
+  for (uint8_t i = 0; i < num_pins; i++) {
+    set_gpio_output(pins[i].bank, pins[i].pin, enabled);
+  }
+}
+
+void gpio_set_bitmask(const gpio_t *pins, uint8_t num_pins, uint32_t bitmask) {
+  for (uint8_t i = 0; i < num_pins; i++) {
+    set_gpio_output(pins[i].bank, pins[i].pin, (bitmask >> i) & 1U);
+  }
+}
+
 // Detection with internal pullup
 #define PULL_EFFECTIVE_DELAY 4096
 bool detect_with_pull(GPIO_TypeDef *GPIO, int pin, int mode) {
diff --git a/panda/board/drivers/harness.h b/panda/board/drivers/harness.h
index 6bf3fe1b9c..36f73f2573 100644
--- a/panda/board/drivers/harness.h
+++ b/panda/board/drivers/harness.h
@@ -25,7 +25,8 @@ struct harness_configuration {
   uint8_t adc_channel_SBU2;
 };
 
-void set_intercept_relay(bool intercept) {
+// The ignition relay is only used for testing purposes
+void set_intercept_relay(bool intercept, bool ignition_relay) {
   if (current_board->harness_config->has_harness) {
     bool drive_relay = intercept;
     if (harness.status == HARNESS_STATUS_NC) {
@@ -33,7 +34,7 @@ void set_intercept_relay(bool intercept) {
       drive_relay = false;
     }
 
-    if (drive_relay) {
+    if (drive_relay || ignition_relay) {
       harness.relay_driven = true;
     }
 
@@ -41,14 +42,14 @@ void set_intercept_relay(bool intercept) {
     while (harness.sbu_adc_lock == true) {}
 
     if (harness.status == HARNESS_STATUS_NORMAL) {
-      set_gpio_output(current_board->harness_config->GPIO_relay_SBU1, current_board->harness_config->pin_relay_SBU1, true);
+      set_gpio_output(current_board->harness_config->GPIO_relay_SBU1, current_board->harness_config->pin_relay_SBU1, !ignition_relay);
       set_gpio_output(current_board->harness_config->GPIO_relay_SBU2, current_board->harness_config->pin_relay_SBU2, !drive_relay);
     } else {
       set_gpio_output(current_board->harness_config->GPIO_relay_SBU1, current_board->harness_config->pin_relay_SBU1, !drive_relay);
-      set_gpio_output(current_board->harness_config->GPIO_relay_SBU2, current_board->harness_config->pin_relay_SBU2, true);
+      set_gpio_output(current_board->harness_config->GPIO_relay_SBU2, current_board->harness_config->pin_relay_SBU2, !ignition_relay);
     }
 
-    if (!drive_relay) {
+    if (!(drive_relay || ignition_relay)) {
       harness.relay_driven = false;
     }
   }
@@ -129,5 +130,5 @@ void harness_init(void) {
   }
 
   // keep buses connected by default
-  set_intercept_relay(false);
+  set_intercept_relay(false, false);
 }
diff --git a/panda/board/drivers/logging.h b/panda/board/drivers/logging.h
deleted file mode 100644
index 28e4275230..0000000000
--- a/panda/board/drivers/logging.h
+++ /dev/null
@@ -1,193 +0,0 @@
-
-#include "logging_definitions.h"
-
-#define BANK_SIZE LOGGING_FLASH_SECTOR_SIZE
-#define BANK_LOG_CAPACITY (BANK_SIZE / sizeof(log_t))
-#define TOTAL_LOG_CAPACITY (BANK_LOG_CAPACITY * 2U)
-
-#define LOGGING_MAX_LOGS_PER_MINUTE 10U
-
-struct logging_state_t {
-  uint16_t read_index;
-  uint16_t write_index;
-  uint16_t last_id;
-
-  uint8_t rate_limit_counter;
-  uint8_t rate_limit_log_count;
-};
-struct logging_state_t log_state = { 0 };
-log_t *log_arr = (log_t *) LOGGING_FLASH_BASE_A;
-
-uint16_t logging_next_id(uint16_t id) {
-  return (id + 1U) % 0xFFFEU;
-}
-
-uint16_t logging_next_index(uint16_t index) {
-  return (index + 1U) % TOTAL_LOG_CAPACITY;
-}
-
-void logging_erase_bank(uint8_t flash_sector) {
-  print("erasing sector "); puth(flash_sector); print("\n");
-  flash_unlock();
-  if (!flash_erase_sector(flash_sector)) {
-    print("failed to erase sector "); puth(flash_sector); print("\n");
-  }
-  flash_lock();
-}
-
-void logging_erase(void) {
-  logging_erase_bank(LOGGING_FLASH_SECTOR_A);
-  logging_erase_bank(LOGGING_FLASH_SECTOR_B);
-  log_state.read_index = 0U;
-  log_state.write_index = 0U;
-}
-
-void logging_find_read_index(uint16_t last_id) {
-  // Figure out the read index by the last empty slot
-  log_state.read_index = BANK_LOG_CAPACITY;
-  for (uint16_t i = 0U; i < TOTAL_LOG_CAPACITY; i++) {
-    if (log_arr[i].id == last_id) {
-      log_state.read_index = logging_next_index(i);
-    }
-  }
-}
-
-void logging_init_read_index(void) {
-  return logging_find_read_index(0xFFFFU);
-}
-
-void logging_init(void) {
-  COMPILE_TIME_ASSERT(sizeof(log_t) == 64U);
-  COMPILE_TIME_ASSERT((LOGGING_FLASH_BASE_A + BANK_SIZE) == LOGGING_FLASH_BASE_B);
-
-  // Make sure all empty-ID logs are fully empty
-  log_t empty_log;
-  (void) memset(&empty_log, 0xFF, sizeof(log_t));
-
-  for (uint16_t i = 0U; i < TOTAL_LOG_CAPACITY; i++) {
-    if ((log_arr[i].id == 0xFFFFU) && (memcmp(&log_arr[i], &empty_log, sizeof(log_t)) != 0)) {
-      logging_erase();
-      break;
-    }
-  }
-
-  logging_init_read_index();
-
-  // At initialization, the read index should always be at the beginning of a bank
-  // If not, clean slate
-  if ((log_state.read_index != 0U) && (log_state.read_index != BANK_LOG_CAPACITY)) {
-    logging_erase();
-  }
-
-  // Figure out the write index
-  log_state.write_index = log_state.read_index;
-  log_state.last_id = log_arr[log_state.write_index].id - 1U;
-  for (uint16_t i = 0U; i < TOTAL_LOG_CAPACITY; i++) {
-    bool done = false;
-    if (log_arr[log_state.write_index].id == 0xFFFFU) {
-      // Found the first empty slot after the read pointer
-      done = true;
-    } else if (log_arr[log_state.write_index].id != logging_next_id(log_state.last_id)) {
-      // Discontinuity in the index, shouldn't happen!
-      logging_erase();
-      done = true;
-    } else {
-      log_state.last_id = log_arr[log_state.write_index].id;
-      log_state.write_index = logging_next_index(log_state.write_index);
-    }
-
-    if (done) {
-      break;
-    }
-  }
-
-  // Reset rate limit
-  log_state.rate_limit_counter = 0U;
-  log_state.rate_limit_log_count = 0U;
-}
-
-// Call at 1Hz
-void logging_tick(void) {
-  flush_write_buffer();
-
-  log_state.rate_limit_counter++;
-  if (log_state.rate_limit_counter >= 60U) {
-    log_state.rate_limit_counter = 0U;
-    log_state.rate_limit_log_count = 0U;
-  }
-}
-
-void log(const char* msg){
-  if (log_state.rate_limit_log_count < LOGGING_MAX_LOGS_PER_MINUTE) {
-    ENTER_CRITICAL();
-    log_t new_log = {0};
-    new_log.id = logging_next_id(log_state.last_id);
-    log_state.last_id = new_log.id;
-    new_log.uptime = uptime_cnt;
-    if (current_board->has_rtc_battery) {
-      new_log.timestamp = rtc_get_time();
-    }
-
-    uint8_t i = 0U;
-    for (const char *in = msg; *in; in++) {
-      new_log.msg[i] = *in;
-      i++;
-      if (i >= sizeof(new_log.msg)) {
-        print("log message too long\n");
-        break;
-      }
-    }
-
-    // If we are at the beginning of a bank, erase it first and move the read pointer if needed
-    switch (log_state.write_index) {
-      case ((2U * BANK_LOG_CAPACITY) - 1U):
-        logging_erase_bank(LOGGING_FLASH_SECTOR_A);
-        if ((log_state.read_index < BANK_LOG_CAPACITY)) {
-          log_state.read_index = BANK_LOG_CAPACITY;
-        }
-        break;
-      case (BANK_LOG_CAPACITY - 1U):
-        // beginning to write in bank B
-        logging_erase_bank(LOGGING_FLASH_SECTOR_B);
-        if ((log_state.read_index > BANK_LOG_CAPACITY)) {
-          log_state.read_index = 0U;
-        }
-        break;
-      default:
-        break;
-    }
-
-    // Write!
-    void *addr = &log_arr[log_state.write_index];
-    uint32_t data[sizeof(log_t) / sizeof(uint32_t)];
-    (void) memcpy(data, &new_log, sizeof(log_t));
-
-    flash_unlock();
-    for (uint8_t j = 0U; j < sizeof(log_t) / sizeof(uint32_t); j++) {
-      flash_write_word(&((uint32_t *) addr)[j], data[j]);
-    }
-    flash_lock();
-
-    // Update the write index
-    log_state.write_index = logging_next_index(log_state.write_index);
-    EXIT_CRITICAL();
-
-    log_state.rate_limit_log_count++;
-  } else {
-    fault_occurred(FAULT_LOGGING_RATE_LIMIT);
-  }
-}
-
-uint8_t logging_read(uint8_t *buffer) {
-  uint8_t ret = 0U;
-  if ((log_arr[log_state.read_index].id != 0xFFFFU) && (log_state.read_index != log_state.write_index)) {
-    // Read the log
-    (void) memcpy(buffer, &log_arr[log_state.read_index], sizeof(log_t));
-
-    // Update the read index
-    log_state.read_index = logging_next_index(log_state.read_index);
-
-    ret = sizeof(log_t);
-  }
-  return ret;
-}
diff --git a/panda/board/drivers/logging_definitions.h b/panda/board/drivers/logging_definitions.h
deleted file mode 100644
index 9c1bc02e1c..0000000000
--- a/panda/board/drivers/logging_definitions.h
+++ /dev/null
@@ -1,9 +0,0 @@
-
-// Flash is writable in 32-byte lines, this struct is designed to fit in two lines.
-// This also matches the USB transfer size.
-typedef struct __attribute__((packed)) log_t {
-  uint16_t id;
-  timestamp_t timestamp;
-  uint32_t uptime;
-  char msg[50];
-} log_t;
diff --git a/panda/board/drivers/rtc.h b/panda/board/drivers/rtc.h
index 231d88f2bf..df121e3e89 100644
--- a/panda/board/drivers/rtc.h
+++ b/panda/board/drivers/rtc.h
@@ -1,8 +1,15 @@
-
-#include "rtc_definitions.h"
-
 #define YEAR_OFFSET 2000U
 
+typedef struct __attribute__((packed)) timestamp_t {
+  uint16_t year;
+  uint8_t month;
+  uint8_t day;
+  uint8_t weekday;
+  uint8_t hour;
+  uint8_t minute;
+  uint8_t second;
+} timestamp_t;
+
 uint8_t to_bcd(uint16_t value){
   return (((value / 10U) & 0x0FU) << 4U) | ((value % 10U) & 0x0FU);
 }
@@ -43,6 +50,14 @@ void rtc_set_time(timestamp_t time){
 
 timestamp_t rtc_get_time(void){
   timestamp_t result;
+  // Init with zero values in case there is no RTC running
+  result.year = 0U;
+  result.month = 0U;
+  result.day = 0U;
+  result.weekday = 0U;
+  result.hour = 0U;
+  result.minute = 0U;
+  result.second = 0U;
 
   // Wait until the register sync flag is set
   while((RTC->ISR & RTC_ISR_RSF) == 0){}
diff --git a/panda/board/drivers/rtc_definitions.h b/panda/board/drivers/rtc_definitions.h
deleted file mode 100644
index d308eb7467..0000000000
--- a/panda/board/drivers/rtc_definitions.h
+++ /dev/null
@@ -1,9 +0,0 @@
-typedef struct __attribute__((packed)) timestamp_t {
-  uint16_t year;
-  uint8_t month;
-  uint8_t day;
-  uint8_t weekday;
-  uint8_t hour;
-  uint8_t minute;
-  uint8_t second;
-} timestamp_t;
diff --git a/panda/board/drivers/spi.h b/panda/board/drivers/spi.h
index af9dd8a574..6d0beda9ad 100644
--- a/panda/board/drivers/spi.h
+++ b/panda/board/drivers/spi.h
@@ -10,8 +10,9 @@
 
 #ifdef STM32H7
 #define SPI_BUF_SIZE 2048U
-__attribute__((section(".ram_d1"))) uint8_t spi_buf_rx[SPI_BUF_SIZE];
-__attribute__((section(".ram_d2"))) uint8_t spi_buf_tx[SPI_BUF_SIZE];
+// H7 DMA2 located in D2 domain, so we need to use SRAM1/SRAM2
+__attribute__((section(".sram12"))) uint8_t spi_buf_rx[SPI_BUF_SIZE];
+__attribute__((section(".sram12"))) uint8_t spi_buf_tx[SPI_BUF_SIZE];
 #else
 #define SPI_BUF_SIZE 1024U
 uint8_t spi_buf_rx[SPI_BUF_SIZE];
diff --git a/panda/board/drivers/uart.h b/panda/board/drivers/uart.h
index 824e7d7592..cde8137402 100644
--- a/panda/board/drivers/uart.h
+++ b/panda/board/drivers/uart.h
@@ -1,8 +1,7 @@
-// IRQs: USART1, USART2, USART3, UART5
+// IRQs: USART2, USART3, UART5
 
 // ***************************** Definitions *****************************
 #define FIFO_SIZE_INT 0x400U
-#define FIFO_SIZE_DMA 0x1000U
 
 typedef struct uart_ring {
   volatile uint16_t w_ptr_tx;
@@ -15,11 +14,10 @@ typedef struct uart_ring {
   uint32_t rx_fifo_size;
   USART_TypeDef *uart;
   void (*callback)(struct uart_ring*);
-  bool dma_rx;
   bool overwrite;
 } uart_ring;
 
-#define UART_BUFFER(x, size_rx, size_tx, uart_ptr, callback_ptr, rx_dma, overwrite_mode) \
+#define UART_BUFFER(x, size_rx, size_tx, uart_ptr, callback_ptr, overwrite_mode) \
   uint8_t elems_rx_##x[size_rx]; \
   uint8_t elems_tx_##x[size_tx]; \
   uart_ring uart_ring_##x = {  \
@@ -33,7 +31,6 @@ typedef struct uart_ring {
     .rx_fifo_size = (size_rx), \
     .uart = (uart_ptr), \
     .callback = (callback_ptr), \
-    .dma_rx = (rx_dma), \
     .overwrite = (overwrite_mode) \
   };
 
@@ -44,23 +41,20 @@ void uart_send_break(uart_ring *u);
 
 // ******************************** UART buffers ********************************
 
-// gps = USART1
-UART_BUFFER(gps, FIFO_SIZE_DMA, FIFO_SIZE_INT, USART1, NULL, true, false)
-
 // lin1, K-LINE = UART5
 // lin2, L-LINE = USART3
-UART_BUFFER(lin1, FIFO_SIZE_INT, FIFO_SIZE_INT, UART5, NULL, false, false)
-UART_BUFFER(lin2, FIFO_SIZE_INT, FIFO_SIZE_INT, USART3, NULL, false, false)
+UART_BUFFER(lin1, FIFO_SIZE_INT, FIFO_SIZE_INT, UART5, NULL, false)
+UART_BUFFER(lin2, FIFO_SIZE_INT, FIFO_SIZE_INT, USART3, NULL, false)
 
 // debug = USART2
-UART_BUFFER(debug, FIFO_SIZE_INT, FIFO_SIZE_INT, USART2, debug_ring_callback, false, true)
+UART_BUFFER(debug, FIFO_SIZE_INT, FIFO_SIZE_INT, USART2, debug_ring_callback, true)
 
 // SOM debug = UART7
 #ifdef STM32H7
-  UART_BUFFER(som_debug, FIFO_SIZE_INT, FIFO_SIZE_INT, UART7, NULL, false, true)
+  UART_BUFFER(som_debug, FIFO_SIZE_INT, FIFO_SIZE_INT, UART7, NULL, true)
 #else
   // UART7 is not available on F4
-  UART_BUFFER(som_debug, 1U, 1U, NULL, NULL, false, true)
+  UART_BUFFER(som_debug, 1U, 1U, NULL, NULL, true)
 #endif
 
 uart_ring *get_ring_by_number(int a) {
@@ -69,9 +63,6 @@ uart_ring *get_ring_by_number(int a) {
     case 0:
       ring = &uart_ring_debug;
       break;
-    case 1:
-      ring = &uart_ring_gps;
-      break;
     case 2:
       ring = &uart_ring_lin1;
       break;
diff --git a/panda/board/drivers/usb.h b/panda/board/drivers/usb.h
index d10bed15e3..c291d7e64c 100644
--- a/panda/board/drivers/usb.h
+++ b/panda/board/drivers/usb.h
@@ -198,8 +198,13 @@ uint16_t string_product_desc[] = {
 
 // default serial number when we're not a panda
 uint16_t string_serial_desc[] = {
+#ifdef PEDAL
+  STRING_DESCRIPTOR_HEADER(5),
+  'p', 'e', 'd', 'a', 'l'
+#else
   STRING_DESCRIPTOR_HEADER(4),
   'n', 'o', 'n', 'e'
+#endif
 };
 
 // a string containing the default configuration index
diff --git a/panda/board/early_init.h b/panda/board/early_init.h
index 6c60b20fbb..ae652aebce 100644
--- a/panda/board/early_init.h
+++ b/panda/board/early_init.h
@@ -51,9 +51,9 @@ void early_initialization(void) {
   detect_board_type();
 
   if (enter_bootloader_mode == ENTER_BOOTLOADER_MAGIC) {
-  #ifdef PANDA
-    current_board->set_gps_mode(GPS_DISABLED);
-  #endif
+    #ifdef PANDA
+    current_board->init_bootloader();
+    #endif
     current_board->set_led(LED_GREEN, 1);
     jump_to_bootloader();
   }
diff --git a/panda/board/fake_stm.h b/panda/board/fake_stm.h
index e6709c78e0..b73a4e8985 100644
--- a/panda/board/fake_stm.h
+++ b/panda/board/fake_stm.h
@@ -2,10 +2,8 @@
 #include <stdio.h>
 #include <stdint.h>
 #include <stdlib.h>
-#include <stdbool.h>
 
 #include "utils.h"
-#include "drivers/rtc_definitions.h"
 
 #define CANFD
 #define ALLOW_DEBUG
@@ -33,64 +31,3 @@ uint32_t microsecond_timer_get(void);
 uint32_t microsecond_timer_get(void) {
   return MICROSECOND_TIMER->CNT;
 }
-
-// Register functions
-void register_set_bits(volatile uint32_t *addr, uint32_t val) {}
-
-// RTC
-timestamp_t rtc_get_time() {
-  timestamp_t result;
-  result.year = 1996;
-  result.month = 4;
-  result.day = 23;
-  result.weekday = 2;
-  result.hour = 4;
-  result.minute = 20;
-  result.second = 20;
-  return result;
-}
-
-// Logging and flash
-uint8_t fake_logging_bank[0x40000] __attribute__ ((aligned (4)));
-#define LOGGING_FLASH_BASE_A (&fake_logging_bank[0])
-#define LOGGING_FLASH_BASE_B (&fake_logging_bank[0x20000])
-#define LOGGING_FLASH_SECTOR_A 5
-#define LOGGING_FLASH_SECTOR_B 6
-#define LOGGING_FLASH_SECTOR_SIZE 0x20000U
-
-bool flash_locked = true;
-void flash_unlock(void) {
-  flash_locked = false;
-}
-void flash_lock(void) {
-  flash_locked = true;
-}
-
-void *memset(void *str, int c, unsigned int n);
-
-bool flash_erase_sector(uint8_t sector) {
-  if (flash_locked) {
-    return false;
-  }
-
-  switch (sector) {
-    case LOGGING_FLASH_SECTOR_A:
-      memset(LOGGING_FLASH_BASE_A, 0xFF, sizeof(fake_logging_bank)/2);
-      return true;
-    case LOGGING_FLASH_SECTOR_B:
-      memset(LOGGING_FLASH_BASE_B, 0xFF, sizeof(fake_logging_bank)/2);
-      return true;
-    default:
-      return false;
-  }
-}
-
-void flash_write_word(void *prog_ptr, uint32_t data) {
-  if (flash_locked || prog_ptr < (void *) LOGGING_FLASH_BASE_A || prog_ptr >= (void *) (LOGGING_FLASH_BASE_A + sizeof(fake_logging_bank))) {
-    return;
-  }
-
-  *(uint32_t *)prog_ptr = data;
-}
-
-void flush_write_buffer(void) {}
\ No newline at end of file
diff --git a/panda/board/faults.h b/panda/board/faults.h
index a741ea1c41..6c6bef475d 100644
--- a/panda/board/faults.h
+++ b/panda/board/faults.h
@@ -30,7 +30,6 @@
 #define FAULT_INTERRUPT_RATE_UART_7         (1U << 24)
 #define FAULT_SIREN_MALFUNCTION             (1U << 25)
 #define FAULT_HEARTBEAT_LOOP_WATCHDOG       (1U << 26)
-#define FAULT_LOGGING_RATE_LIMIT            (1U << 27)
 
 // Permanent faults
 #define PERMANENT_FAULTS 0U
diff --git a/panda/board/flasher.h b/panda/board/flasher.h
index 351a899915..495d9f1691 100644
--- a/panda/board/flasher.h
+++ b/panda/board/flasher.h
@@ -1,5 +1,6 @@
 // flasher state variables
 uint32_t *prog_ptr = NULL;
+bool unlocked = false;
 
 void spi_init(void);
 
@@ -32,12 +33,13 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
         resp[1] = 0xff;
       }
       current_board->set_led(LED_GREEN, 1);
+      unlocked = true;
       prog_ptr = (uint32_t *)APP_START_ADDRESS;
       break;
     // **** 0xb2: erase sector
     case 0xb2:
       sec = req->param1;
-      if (flash_erase_sector(sec)) {
+      if (flash_erase_sector(sec, unlocked)) {
         resp[1] = 0xff;
       }
       break;
@@ -148,14 +150,14 @@ int spi_cb_rx(uint8_t *data, int len, uint8_t *data_out) {
 #ifdef PEDAL
 
 #include "stm32fx/llbxcan.h"
-#define CAN CAN1
+#define CANx CAN1
 
 #define CAN_BL_INPUT 0x1
 #define CAN_BL_OUTPUT 0x2
 
 void CAN1_TX_IRQ_Handler(void) {
   // clear interrupt
-  CAN->TSR |= CAN_TSR_RQCP0;
+  CANx->TSR |= CAN_TSR_RQCP0;
 }
 
 #define ISOTP_BUF_SIZE 0x110
@@ -171,21 +173,21 @@ int isotp_buf_out_idx = 0;
 
 void bl_can_send(uint8_t *odat) {
   // wait for send
-  while (!(CAN->TSR & CAN_TSR_TME0));
+  while (!(CANx->TSR & CAN_TSR_TME0));
 
   // send continue
-  CAN->sTxMailBox[0].TDLR = ((uint32_t*)odat)[0];
-  CAN->sTxMailBox[0].TDHR = ((uint32_t*)odat)[1];
-  CAN->sTxMailBox[0].TDTR = 8;
-  CAN->sTxMailBox[0].TIR = (CAN_BL_OUTPUT << 21) | 1;
+  CANx->sTxMailBox[0].TDLR = ((uint32_t*)odat)[0];
+  CANx->sTxMailBox[0].TDHR = ((uint32_t*)odat)[1];
+  CANx->sTxMailBox[0].TDTR = 8;
+  CANx->sTxMailBox[0].TIR = (CAN_BL_OUTPUT << 21) | 1;
 }
 
 void CAN1_RX0_IRQ_Handler(void) {
-  while (CAN->RF0R & CAN_RF0R_FMP0) {
-    if ((CAN->sFIFOMailBox[0].RIR>>21) == CAN_BL_INPUT) {
+  while (CANx->RF0R & CAN_RF0R_FMP0) {
+    if ((CANx->sFIFOMailBox[0].RIR>>21) == CAN_BL_INPUT) {
       uint8_t dat[8];
       for (int i = 0; i < 8; i++) {
-        dat[i] = GET_MAILBOX_BYTE(&CAN->sFIFOMailBox[0], i);
+        dat[i] = GET_MAILBOX_BYTE(&CANx->sFIFOMailBox[0], i);
       }
       uint8_t odat[8];
       uint8_t type = dat[0] & 0xF0;
@@ -193,7 +195,7 @@ void CAN1_RX0_IRQ_Handler(void) {
         // continue
         while (isotp_buf_out_remain > 0) {
           // wait for send
-          while (!(CAN->TSR & CAN_TSR_TME0));
+          while (!(CANx->TSR & CAN_TSR_TME0));
 
           odat[0] = 0x20 | isotp_buf_out_idx;
           memcpy(odat+1, isotp_buf_out_ptr, 7);
@@ -251,12 +253,12 @@ void CAN1_RX0_IRQ_Handler(void) {
       }
     }
     // next
-    CAN->RF0R |= CAN_RF0R_RFOM0;
+    CANx->RF0R |= CAN_RF0R_RFOM0;
   }
 }
 
 void CAN1_SCE_IRQ_Handler(void) {
-  llcan_clear_send(CAN);
+  llcan_clear_send(CANx);
 }
 
 #endif
@@ -284,8 +286,8 @@ void soft_flasher_start(void) {
   current_board->enable_can_transceiver(1, true);
 
   // init can
-  llcan_set_speed(CAN1, 5000, false, false);
-  llcan_init(CAN1);
+  llcan_set_speed(CANx, 5000, false, false);
+  llcan_init(CANx);
 #endif
 
   gpio_usart2_init();
diff --git a/panda/board/health.h b/panda/board/health.h
index 9d9dcfb381..a1b7ed952f 100644
--- a/panda/board/health.h
+++ b/panda/board/health.h
@@ -29,6 +29,7 @@ struct __attribute__((packed)) health_t {
   uint8_t fan_stall_count;
   uint16_t sbu1_voltage_mV;
   uint16_t sbu2_voltage_mV;
+  uint8_t som_reset_triggered;
 };
 
 #define CAN_HEALTH_PACKET_VERSION 5
diff --git a/panda/board/jungle/README.md b/panda/board/jungle/README.md
new file mode 100644
index 0000000000..4c271cbf21
--- /dev/null
+++ b/panda/board/jungle/README.md
@@ -0,0 +1,26 @@
+Welcome to the jungle
+======
+
+Firmware for the Panda Jungle testing board.
+Available for purchase at the [comma shop](https://comma.ai/shop/panda-jungle).
+
+## udev rules
+
+To make the jungle usable without root permissions, you might need to setup udev rules for it.
+On ubuntu, this should do the trick:
+``` bash
+sudo tee /etc/udev/rules.d/12-panda_jungle.rules <<EOF
+SUBSYSTEM=="usb", ATTRS{idVendor}=="bbaa", ATTRS{idProduct}=="ddcf", MODE="0666"
+SUBSYSTEM=="usb", ATTRS{idVendor}=="bbaa", ATTRS{idProduct}=="ddef", MODE="0666"
+EOF
+sudo udevadm control --reload-rules && sudo udevadm trigger
+```
+
+## updating the firmware
+Updating the firmware is easy! In the `board/jungle/` folder, run:
+``` bash
+./flash.py
+```
+
+If you somehow bricked your jungle, you'll need a [comma key](https://comma.ai/shop/products/comma-key) to put the microcontroller in DFU mode for the V1.
+For V2, the onboard button serves this purpose. When powered on while holding the button to put it in DFU mode, running `./recover.sh` in `board/` should unbrick it.
diff --git a/panda/board/jungle/SConscript b/panda/board/jungle/SConscript
new file mode 100644
index 0000000000..3e40611a64
--- /dev/null
+++ b/panda/board/jungle/SConscript
@@ -0,0 +1,18 @@
+import os
+import copy
+
+Import('build_project', 'base_project_f4', 'base_project_h7')
+
+build_projects = {
+  "panda_jungle": base_project_f4,
+  "panda_jungle_h7": base_project_h7,
+}
+
+for project_name, project in build_projects.items():
+  flags = [
+    "-DPANDA_JUNGLE",
+  ]
+  if os.getenv("FINAL_PROVISIONING"):
+    flags += ["-DFINAL_PROVISIONING"]
+
+  build_project(project_name, project, flags)
diff --git a/panda/board/jungle/__init__.py b/panda/board/jungle/__init__.py
new file mode 100644
index 0000000000..77a6e3c7cc
--- /dev/null
+++ b/panda/board/jungle/__init__.py
@@ -0,0 +1,160 @@
+# python library to interface with panda
+import os
+import struct
+from functools import wraps
+from typing import Optional
+
+from panda import Panda, PandaDFU
+from panda.python.constants import McuType
+
+BASEDIR = os.path.dirname(os.path.realpath(__file__))
+FW_PATH = os.path.join(BASEDIR, "obj/")
+
+
+def ensure_jungle_health_packet_version(fn):
+  @wraps(fn)
+  def wrapper(self, *args, **kwargs):
+    if self.health_version != self.HEALTH_PACKET_VERSION:
+      raise RuntimeError(f"Jungle firmware ({self.health_version}) doesn't match the \
+                           library's health packet version ({self.HEALTH_PACKET_VERSION}). \
+                           Reflash jungle.")
+    return fn(self, *args, **kwargs)
+  return wrapper
+
+
+class PandaJungleDFU(PandaDFU):
+  def recover(self):
+    fn = os.path.join(FW_PATH, self._mcu_type.config.bootstub_fn.replace("panda", "panda_jungle"))
+    with open(fn, "rb") as f:
+      code = f.read()
+    self.program_bootstub(code)
+    self.reset()
+
+
+class PandaJungle(Panda):
+  USB_PIDS = (0xddef, 0xddcf)
+
+  HW_TYPE_UNKNOWN = b'\x00'
+  HW_TYPE_V1 = b'\x01'
+  HW_TYPE_V2 = b'\x02'
+
+  F4_DEVICES = [HW_TYPE_V1, ]
+  H7_DEVICES = [HW_TYPE_V2, ]
+
+  HEALTH_PACKET_VERSION = 1
+  HEALTH_STRUCT = struct.Struct("<IffffffHHHHHHHHHHHH")
+
+  HARNESS_ORIENTATION_NONE = 0
+  HARNESS_ORIENTATION_1 = 1
+  HARNESS_ORIENTATION_2 = 2
+
+  @classmethod
+  def spi_connect(cls, serial, ignore_version=False):
+    return None, None, None, None, None
+
+  def flash(self, fn=None, code=None, reconnect=True):
+    if not fn:
+      fn = os.path.join(FW_PATH, self._mcu_type.config.app_fn.replace("panda", "panda_jungle"))
+    super().flash(fn=fn, code=code, reconnect=reconnect)
+
+  def recover(self, timeout: Optional[int] = 60, reset: bool = True) -> bool:
+    dfu_serial = self.get_dfu_serial()
+
+    if reset:
+      self.reset(enter_bootstub=True)
+      self.reset(enter_bootloader=True)
+
+    if not self.wait_for_dfu(dfu_serial, timeout=timeout):
+      return False
+
+    dfu = PandaJungleDFU(dfu_serial)
+    dfu.recover()
+
+    # reflash after recover
+    self.connect(True, True)
+    self.flash()
+    return True
+
+  def get_mcu_type(self) -> McuType:
+    hw_type = self.get_type()
+    if hw_type in PandaJungle.F4_DEVICES:
+      return McuType.F4
+    elif hw_type in PandaJungle.H7_DEVICES:
+      return McuType.H7
+    raise ValueError(f"unknown HW type: {hw_type}")
+
+  def up_to_date(self, fn=None) -> bool:
+    if fn is None:
+      fn = os.path.join(FW_PATH, self.get_mcu_type().config.app_fn.replace("panda", "panda_jungle"))
+    return super().up_to_date(fn=fn)
+
+  # ******************* health *******************
+
+  @ensure_jungle_health_packet_version
+  def health(self):
+    dat = self._handle.controlRead(PandaJungle.REQUEST_IN, 0xd2, 0, 0, self.HEALTH_STRUCT.size)
+    a = self.HEALTH_STRUCT.unpack(dat)
+    return {
+      "uptime": a[0],
+      "ch1_power": a[1],
+      "ch2_power": a[2],
+      "ch3_power": a[3],
+      "ch4_power": a[4],
+      "ch5_power": a[5],
+      "ch6_power": a[6],
+      "ch1_sbu1_voltage": a[7] / 1000.0,
+      "ch1_sbu2_voltage": a[8] / 1000.0,
+      "ch2_sbu1_voltage": a[9] / 1000.0,
+      "ch2_sbu2_voltage": a[10] / 1000.0,
+      "ch3_sbu1_voltage": a[11] / 1000.0,
+      "ch3_sbu2_voltage": a[12] / 1000.0,
+      "ch4_sbu1_voltage": a[13] / 1000.0,
+      "ch4_sbu2_voltage": a[14] / 1000.0,
+      "ch5_sbu1_voltage": a[15] / 1000.0,
+      "ch5_sbu2_voltage": a[16] / 1000.0,
+      "ch6_sbu1_voltage": a[17] / 1000.0,
+      "ch6_sbu2_voltage": a[18] / 1000.0,
+    }
+
+  # ******************* control *******************
+
+  # Returns tuple with health packet version and CAN packet/USB packet version
+  def get_packets_versions(self):
+    dat = self._handle.controlRead(PandaJungle.REQUEST_IN, 0xdd, 0, 0, 3)
+    if dat and len(dat) == 3:
+      a = struct.unpack("BBB", dat)
+      return (a[0], a[1], a[2])
+    return (-1, -1, -1)
+
+  # ******************* jungle stuff *******************
+
+  def set_panda_power(self, enabled):
+    self._handle.controlWrite(PandaJungle.REQUEST_OUT, 0xa0, int(enabled), 0, b'')
+
+  def set_panda_individual_power(self, port, enabled):
+    self._handle.controlWrite(PandaJungle.REQUEST_OUT, 0xa3, int(port), int(enabled), b'')
+
+  def set_harness_orientation(self, mode):
+    self._handle.controlWrite(PandaJungle.REQUEST_OUT, 0xa1, int(mode), 0, b'')
+
+  def set_ignition(self, enabled):
+    self._handle.controlWrite(PandaJungle.REQUEST_OUT, 0xa2, int(enabled), 0, b'')
+
+  def set_can_silent(self, silent):
+    self._handle.controlWrite(PandaJungle.REQUEST_OUT, 0xf5, int(silent), 0, b'')
+
+  # ******************* serial *******************
+
+  def debug_read(self):
+    ret = []
+    while 1:
+      lret = bytes(self._handle.controlRead(PandaJungle.REQUEST_IN, 0xe0, 0, 0, 0x40))
+      if len(lret) == 0:
+        break
+      ret.append(lret)
+    return b''.join(ret)
+
+  # ******************* header pins *******************
+
+  def set_header_pin(self, pin_num, enabled):
+    self._handle.controlWrite(Panda.REQUEST_OUT, 0xf7, int(pin_num), int(enabled), b'')
diff --git a/panda/board/jungle/boards/board_declarations.h b/panda/board/jungle/boards/board_declarations.h
new file mode 100644
index 0000000000..6e016af071
--- /dev/null
+++ b/panda/board/jungle/boards/board_declarations.h
@@ -0,0 +1,84 @@
+// ******************** Prototypes ********************
+typedef void (*board_init)(void);
+typedef void (*board_set_led)(uint8_t color, bool enabled);
+typedef void (*board_board_tick)(void);
+typedef bool (*board_get_button)(void);
+typedef void (*board_set_panda_power)(bool enabled);
+typedef void (*board_set_panda_individual_power)(uint8_t port_num, bool enabled);
+typedef void (*board_set_ignition)(bool enabled);
+typedef void (*board_set_individual_ignition)(uint8_t bitmask);
+typedef void (*board_set_harness_orientation)(uint8_t orientation);
+typedef void (*board_set_can_mode)(uint8_t mode);
+typedef void (*board_enable_can_transciever)(uint8_t transciever, bool enabled);
+typedef void (*board_enable_header_pin)(uint8_t pin_num, bool enabled);
+typedef float (*board_get_channel_power)(uint8_t channel);
+typedef uint16_t (*board_get_sbu_mV)(uint8_t channel, uint8_t sbu);
+
+struct board {
+  const char *board_type;
+  const bool has_canfd;
+  const bool has_sbu_sense;
+  const uint16_t avdd_mV;
+  board_init init;
+  board_set_led set_led;
+  board_board_tick board_tick;
+  board_get_button get_button;
+  board_set_panda_power set_panda_power;
+  board_set_panda_individual_power set_panda_individual_power;
+  board_set_ignition set_ignition;
+  board_set_individual_ignition set_individual_ignition;
+  board_set_harness_orientation set_harness_orientation;
+  board_set_can_mode set_can_mode;
+  board_enable_can_transciever enable_can_transciever;
+  board_enable_header_pin enable_header_pin;
+  board_get_channel_power get_channel_power;
+  board_get_sbu_mV get_sbu_mV;
+
+  // TODO: shouldn't need these
+  bool has_spi;
+  bool has_hw_gmlan;
+};
+
+// ******************* Definitions ********************
+#define HW_TYPE_UNKNOWN 0U
+#define HW_TYPE_V1 1U
+#define HW_TYPE_V2 2U
+
+// LED colors
+#define LED_RED 0U
+#define LED_GREEN 1U
+#define LED_BLUE 2U
+
+// CAN modes
+#define CAN_MODE_NORMAL 0U
+#define CAN_MODE_GMLAN_CAN2 1U
+#define CAN_MODE_GMLAN_CAN3 2U
+#define CAN_MODE_OBD_CAN2 3U
+
+// Harness states
+#define HARNESS_ORIENTATION_NONE 0U
+#define HARNESS_ORIENTATION_1 1U
+#define HARNESS_ORIENTATION_2 2U
+
+#define SBU1 0U
+#define SBU2 1U
+
+// ********************* Globals **********************
+uint8_t harness_orientation = HARNESS_ORIENTATION_NONE;
+uint8_t can_mode = CAN_MODE_NORMAL;
+uint8_t ignition = 0U;
+
+
+void unused_set_individual_ignition(uint8_t bitmask) {
+  UNUSED(bitmask);
+}
+
+void unused_board_enable_header_pin(uint8_t pin_num, bool enabled) {
+  UNUSED(pin_num);
+  UNUSED(enabled);
+}
+
+void unused_set_panda_individual_power(uint8_t port_num, bool enabled) {
+  UNUSED(port_num);
+  UNUSED(enabled);
+}
diff --git a/panda/board/jungle/boards/board_v1.h b/panda/board/jungle/boards/board_v1.h
new file mode 100644
index 0000000000..d0ad7843eb
--- /dev/null
+++ b/panda/board/jungle/boards/board_v1.h
@@ -0,0 +1,176 @@
+
+void board_v1_set_led(uint8_t color, bool enabled) {
+  switch (color) {
+    case LED_RED:
+      set_gpio_output(GPIOC, 9, !enabled);
+      break;
+     case LED_GREEN:
+      set_gpio_output(GPIOC, 7, !enabled);
+      break;
+    case LED_BLUE:
+      set_gpio_output(GPIOC, 6, !enabled);
+      break;
+    default:
+      break;
+  }
+}
+
+void board_v1_enable_can_transciever(uint8_t transciever, bool enabled) {
+  switch (transciever) {
+    case 1U:
+      set_gpio_output(GPIOC, 1, !enabled);
+      break;
+    case 2U:
+      set_gpio_output(GPIOC, 13, !enabled);
+      break;
+    case 3U:
+      set_gpio_output(GPIOA, 0, !enabled);
+      break;
+    case 4U:
+      set_gpio_output(GPIOB, 10, !enabled);
+      break;
+    default:
+      print("Invalid CAN transciever ("); puth(transciever); print("): enabling failed\n");
+      break;
+  }
+}
+
+void board_v1_set_can_mode(uint8_t mode) {
+  board_v1_enable_can_transciever(2U, false);
+  board_v1_enable_can_transciever(4U, false);
+  switch (mode) {
+    case CAN_MODE_NORMAL:
+      print("Setting normal CAN mode\n");
+      // B12,B13: disable OBD mode
+      set_gpio_mode(GPIOB, 12, MODE_INPUT);
+      set_gpio_mode(GPIOB, 13, MODE_INPUT);
+
+      // B5,B6: normal CAN2 mode
+      set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2);
+      set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2);
+      can_mode = CAN_MODE_NORMAL;
+      board_v1_enable_can_transciever(2U, true);
+      break;
+    case CAN_MODE_OBD_CAN2:
+      print("Setting OBD CAN mode\n");
+      // B5,B6: disable normal CAN2 mode
+      set_gpio_mode(GPIOB, 5, MODE_INPUT);
+      set_gpio_mode(GPIOB, 6, MODE_INPUT);
+
+      // B12,B13: OBD mode
+      set_gpio_alternate(GPIOB, 12, GPIO_AF9_CAN2);
+      set_gpio_alternate(GPIOB, 13, GPIO_AF9_CAN2);
+      can_mode = CAN_MODE_OBD_CAN2;
+      board_v1_enable_can_transciever(4U, true);
+      break;
+    default:
+      print("Tried to set unsupported CAN mode: "); puth(mode); print("\n");
+      break;
+  }
+}
+
+void board_v1_set_harness_orientation(uint8_t orientation) {
+  switch (orientation) {
+    case HARNESS_ORIENTATION_NONE:
+      set_gpio_output(GPIOA, 2, false);
+      set_gpio_output(GPIOA, 3, false);
+      set_gpio_output(GPIOA, 4, false);
+      set_gpio_output(GPIOA, 5, false);
+      harness_orientation = orientation;
+      break;
+    case HARNESS_ORIENTATION_1:
+      set_gpio_output(GPIOA, 2, false);
+      set_gpio_output(GPIOA, 3, (ignition != 0U));
+      set_gpio_output(GPIOA, 4, true);
+      set_gpio_output(GPIOA, 5, false);
+      harness_orientation = orientation;
+      break;
+    case HARNESS_ORIENTATION_2:
+      set_gpio_output(GPIOA, 2, (ignition != 0U));
+      set_gpio_output(GPIOA, 3, false);
+      set_gpio_output(GPIOA, 4, false);
+      set_gpio_output(GPIOA, 5, true);
+      harness_orientation = orientation;
+      break;
+    default:
+      print("Tried to set an unsupported harness orientation: "); puth(orientation); print("\n");
+      break;
+  }
+}
+
+bool panda_power = false;
+void board_v1_set_panda_power(bool enable) {
+  panda_power = enable;
+  set_gpio_output(GPIOB, 14, enable);
+}
+
+bool board_v1_get_button(void) {
+  return get_gpio_input(GPIOC, 8);
+}
+
+void board_v1_set_ignition(bool enabled) {
+  ignition = enabled ? 0xFFU : 0U;
+  board_v1_set_harness_orientation(harness_orientation);
+}
+
+float board_v1_get_channel_power(uint8_t channel) {
+  UNUSED(channel);
+  return 0.0f;
+}
+
+uint16_t board_v1_get_sbu_mV(uint8_t channel, uint8_t sbu) {
+  UNUSED(channel); UNUSED(sbu);
+  return 0U;
+}
+
+void board_v1_init(void) {
+  common_init_gpio();
+
+  // A8,A15: normal CAN3 mode
+  set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3);
+  set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3);
+
+  board_v1_set_can_mode(CAN_MODE_NORMAL);
+
+  // Enable CAN transcievers
+  for(uint8_t i = 1; i <= 4; i++) {
+    board_v1_enable_can_transciever(i, true);
+  }
+
+  // Disable LEDs
+  board_v1_set_led(LED_RED, false);
+  board_v1_set_led(LED_GREEN, false);
+  board_v1_set_led(LED_BLUE, false);
+
+  // Set normal CAN mode
+  board_v1_set_can_mode(CAN_MODE_NORMAL);
+
+  // Set to no harness orientation
+  board_v1_set_harness_orientation(HARNESS_ORIENTATION_NONE);
+
+  // Enable panda power by default
+  board_v1_set_panda_power(true);
+}
+
+void board_v1_tick(void) {}
+
+const board board_v1 = {
+  .board_type = "V1",
+  .has_canfd = false,
+  .has_sbu_sense = false,
+  .avdd_mV = 3300U,
+  .init = &board_v1_init,
+  .set_led = &board_v1_set_led,
+  .board_tick = &board_v1_tick,
+  .get_button = &board_v1_get_button,
+  .set_panda_power = &board_v1_set_panda_power,
+  .set_panda_individual_power = &unused_set_panda_individual_power,
+  .set_ignition = &board_v1_set_ignition,
+  .set_individual_ignition = &unused_set_individual_ignition,
+  .set_harness_orientation = &board_v1_set_harness_orientation,
+  .set_can_mode = &board_v1_set_can_mode,
+  .enable_can_transciever = &board_v1_enable_can_transciever,
+  .enable_header_pin = &unused_board_enable_header_pin,
+  .get_channel_power = &board_v1_get_channel_power,
+  .get_sbu_mV = &board_v1_get_sbu_mV,
+};
diff --git a/panda/board/jungle/boards/board_v2.h b/panda/board/jungle/boards/board_v2.h
new file mode 100644
index 0000000000..25ea2fd422
--- /dev/null
+++ b/panda/board/jungle/boards/board_v2.h
@@ -0,0 +1,326 @@
+
+const gpio_t power_pins[] = {
+  {.bank = GPIOA, .pin = 0},
+  {.bank = GPIOA, .pin = 1},
+  {.bank = GPIOF, .pin = 12},
+  {.bank = GPIOA, .pin = 5},
+  {.bank = GPIOC, .pin = 5},
+  {.bank = GPIOB, .pin = 2},
+};
+
+const gpio_t sbu1_ignition_pins[] = {
+  {.bank = GPIOD, .pin = 0},
+  {.bank = GPIOD, .pin = 5},
+  {.bank = GPIOD, .pin = 12},
+  {.bank = GPIOD, .pin = 14},
+  {.bank = GPIOE, .pin = 5},
+  {.bank = GPIOE, .pin = 9},
+};
+
+const gpio_t sbu1_relay_pins[] = {
+  {.bank = GPIOD, .pin = 1},
+  {.bank = GPIOD, .pin = 6},
+  {.bank = GPIOD, .pin = 11},
+  {.bank = GPIOD, .pin = 15},
+  {.bank = GPIOE, .pin = 6},
+  {.bank = GPIOE, .pin = 10},
+};
+
+const gpio_t sbu2_ignition_pins[] = {
+  {.bank = GPIOD, .pin = 3},
+  {.bank = GPIOD, .pin = 8},
+  {.bank = GPIOD, .pin = 9},
+  {.bank = GPIOE, .pin = 0},
+  {.bank = GPIOE, .pin = 7},
+  {.bank = GPIOE, .pin = 11},
+};
+
+const gpio_t sbu2_relay_pins[] = {
+  {.bank = GPIOD, .pin = 4},
+  {.bank = GPIOD, .pin = 10},
+  {.bank = GPIOD, .pin = 13},
+  {.bank = GPIOE, .pin = 1},
+  {.bank = GPIOE, .pin = 8},
+  {.bank = GPIOE, .pin = 12},
+};
+
+const adc_channel_t sbu1_channels[] = {
+  {.adc = ADC3, .channel = 12},
+  {.adc = ADC3, .channel = 2},
+  {.adc = ADC3, .channel = 4},
+  {.adc = ADC3, .channel = 6},
+  {.adc = ADC3, .channel = 8},
+  {.adc = ADC3, .channel = 10},
+};
+
+const adc_channel_t sbu2_channels[] = {
+  {.adc = ADC1, .channel = 13},
+  {.adc = ADC3, .channel = 3},
+  {.adc = ADC3, .channel = 5},
+  {.adc = ADC3, .channel = 7},
+  {.adc = ADC3, .channel = 9},
+  {.adc = ADC3, .channel = 11},
+};
+
+void board_v2_set_led(uint8_t color, bool enabled) {
+  switch (color) {
+    case LED_RED:
+      set_gpio_output(GPIOE, 4, !enabled);
+      break;
+     case LED_GREEN:
+      set_gpio_output(GPIOE, 3, !enabled);
+      break;
+    case LED_BLUE:
+      set_gpio_output(GPIOE, 2, !enabled);
+      break;
+    default:
+      break;
+  }
+}
+
+void board_v2_set_harness_orientation(uint8_t orientation) {
+  switch (orientation) {
+    case HARNESS_ORIENTATION_NONE:
+      gpio_set_all_output(sbu1_ignition_pins, sizeof(sbu1_ignition_pins) / sizeof(gpio_t), false);
+      gpio_set_all_output(sbu1_relay_pins, sizeof(sbu1_relay_pins) / sizeof(gpio_t), false);
+      gpio_set_all_output(sbu2_ignition_pins, sizeof(sbu2_ignition_pins) / sizeof(gpio_t), false);
+      gpio_set_all_output(sbu2_relay_pins, sizeof(sbu2_relay_pins) / sizeof(gpio_t), false);
+      harness_orientation = orientation;
+      break;
+    case HARNESS_ORIENTATION_1:
+      gpio_set_all_output(sbu1_ignition_pins, sizeof(sbu1_ignition_pins) / sizeof(gpio_t), false);
+      gpio_set_all_output(sbu1_relay_pins, sizeof(sbu1_relay_pins) / sizeof(gpio_t), true);
+      gpio_set_bitmask(sbu2_ignition_pins, sizeof(sbu2_ignition_pins) / sizeof(gpio_t), ignition);
+      gpio_set_all_output(sbu2_relay_pins, sizeof(sbu2_relay_pins) / sizeof(gpio_t), false);
+      harness_orientation = orientation;
+      break;
+    case HARNESS_ORIENTATION_2:
+      gpio_set_bitmask(sbu1_ignition_pins, sizeof(sbu1_ignition_pins) / sizeof(gpio_t), ignition);
+      gpio_set_all_output(sbu1_relay_pins, sizeof(sbu1_relay_pins) / sizeof(gpio_t), false);
+      gpio_set_all_output(sbu2_ignition_pins, sizeof(sbu2_ignition_pins) / sizeof(gpio_t), false);
+      gpio_set_all_output(sbu2_relay_pins, sizeof(sbu2_relay_pins) / sizeof(gpio_t), true);
+      harness_orientation = orientation;
+      break;
+    default:
+      print("Tried to set an unsupported harness orientation: "); puth(orientation); print("\n");
+      break;
+  }
+}
+
+void board_v2_enable_can_transciever(uint8_t transciever, bool enabled) {
+  switch (transciever) {
+    case 1U:
+      set_gpio_output(GPIOG, 11, !enabled);
+      break;
+    case 2U:
+      set_gpio_output(GPIOB, 3, !enabled);
+      break;
+    case 3U:
+      set_gpio_output(GPIOD, 7, !enabled);
+      break;
+    case 4U:
+      set_gpio_output(GPIOB, 4, !enabled);
+      break;
+    default:
+      print("Invalid CAN transciever ("); puth(transciever); print("): enabling failed\n");
+      break;
+  }
+}
+
+void board_v2_enable_header_pin(uint8_t pin_num, bool enabled) {
+  if (pin_num < 8U) {
+    set_gpio_output(GPIOG, pin_num, enabled);
+  } else {
+    print("Invalid pin number ("); puth(pin_num); print("): enabling failed\n");
+  }
+}
+
+void board_v2_set_can_mode(uint8_t mode) {
+  board_v2_enable_can_transciever(2U, false);
+  board_v2_enable_can_transciever(4U, false);
+  switch (mode) {
+    case CAN_MODE_NORMAL:
+      // B12,B13: disable normal mode
+      set_gpio_pullup(GPIOB, 12, PULL_NONE);
+      set_gpio_mode(GPIOB, 12, MODE_ANALOG);
+
+      set_gpio_pullup(GPIOB, 13, PULL_NONE);
+      set_gpio_mode(GPIOB, 13, MODE_ANALOG);
+
+      // B5,B6: FDCAN2 mode
+      set_gpio_pullup(GPIOB, 5, PULL_NONE);
+      set_gpio_alternate(GPIOB, 5, GPIO_AF9_FDCAN2);
+
+      set_gpio_pullup(GPIOB, 6, PULL_NONE);
+      set_gpio_alternate(GPIOB, 6, GPIO_AF9_FDCAN2);
+      can_mode = CAN_MODE_NORMAL;
+      board_v2_enable_can_transciever(2U, true);
+      break;
+    case CAN_MODE_OBD_CAN2:
+      // B5,B6: disable normal mode
+      set_gpio_pullup(GPIOB, 5, PULL_NONE);
+      set_gpio_mode(GPIOB, 5, MODE_ANALOG);
+
+      set_gpio_pullup(GPIOB, 6, PULL_NONE);
+      set_gpio_mode(GPIOB, 6, MODE_ANALOG);
+      // B12,B13: FDCAN2 mode
+      set_gpio_pullup(GPIOB, 12, PULL_NONE);
+      set_gpio_alternate(GPIOB, 12, GPIO_AF9_FDCAN2);
+
+      set_gpio_pullup(GPIOB, 13, PULL_NONE);
+      set_gpio_alternate(GPIOB, 13, GPIO_AF9_FDCAN2);
+      can_mode = CAN_MODE_OBD_CAN2;
+      board_v2_enable_can_transciever(4U, true);
+      break;
+    default:
+      break;
+  }
+}
+
+bool panda_power = false;
+uint8_t panda_power_bitmask = 0U;
+void board_v2_set_panda_power(bool enable) {
+  panda_power = enable;
+  gpio_set_all_output(power_pins, sizeof(power_pins) / sizeof(gpio_t), enable);
+  if (enable) {
+    panda_power_bitmask = 0xFFU;
+  } else {
+    panda_power_bitmask = 0U;
+  }
+}
+
+void board_v2_set_panda_individual_power(uint8_t port_num, bool enable) {
+  port_num -= 1U;
+  if (port_num < 6U) {
+    panda_power_bitmask &= ~(1U << port_num);
+    panda_power_bitmask |= (enable ? 1U : 0U) << port_num;
+  } else {
+    print("Invalid port number ("); puth(port_num); print("): enabling failed\n");
+  }
+  gpio_set_bitmask(power_pins, sizeof(power_pins) / sizeof(gpio_t), (uint32_t)panda_power_bitmask);
+}
+
+bool board_v2_get_button(void) {
+  return get_gpio_input(GPIOG, 15);
+}
+
+void board_v2_set_ignition(bool enabled) {
+  ignition = enabled ? 0xFFU : 0U;
+  board_v2_set_harness_orientation(harness_orientation);
+}
+
+void board_v2_set_individual_ignition(uint8_t bitmask) {
+  ignition = bitmask;
+  board_v2_set_harness_orientation(harness_orientation);
+}
+
+float board_v2_get_channel_power(uint8_t channel) {
+  float ret = 0.0f;
+  if ((channel >= 1U) && (channel <= 6U)) {
+    uint16_t readout = adc_get_mV(ADC1, channel - 1U); // these are mapped nicely in hardware
+
+    ret = (((float) readout / 33e6) - 0.8e-6) / 52e-6 * 12.0f;
+  } else {
+    print("Invalid channel ("); puth(channel); print(")\n");
+  }
+  return ret;
+}
+
+uint16_t board_v2_get_sbu_mV(uint8_t channel, uint8_t sbu) {
+  uint16_t ret = 0U;
+  if ((channel >= 1U) && (channel <= 6U)) {
+    switch(sbu){
+      case SBU1:
+        ret = adc_get_mV(sbu1_channels[channel - 1U].adc, sbu1_channels[channel - 1U].channel);
+        break;
+      case SBU2:
+        ret = adc_get_mV(sbu2_channels[channel - 1U].adc, sbu2_channels[channel - 1U].channel);
+        break;
+      default:
+        print("Invalid SBU ("); puth(sbu); print(")\n");
+        break;
+    }
+  } else {
+    print("Invalid channel ("); puth(channel); print(")\n");
+  }
+  return ret;
+}
+
+void board_v2_init(void) {
+  common_init_gpio();
+
+  // Disable LEDs
+  board_v2_set_led(LED_RED, false);
+  board_v2_set_led(LED_GREEN, false);
+  board_v2_set_led(LED_BLUE, false);
+
+  // Normal CAN mode
+  board_v2_set_can_mode(CAN_MODE_NORMAL);
+
+  // Enable CAN transcievers
+  for(uint8_t i = 1; i <= 4; i++) {
+    board_v2_enable_can_transciever(i, true);
+  }
+
+  // Set to no harness orientation
+  board_v2_set_harness_orientation(HARNESS_ORIENTATION_NONE);
+
+  // Enable panda power by default
+  board_v2_set_panda_power(true);
+
+  // Current monitor channels
+  adc_init(ADC1);
+  register_set_bits(&SYSCFG->PMCR, SYSCFG_PMCR_PA0SO | SYSCFG_PMCR_PA1SO); // open up analog switches for PA0_C and PA1_C
+  set_gpio_mode(GPIOF, 11, MODE_ANALOG);
+  set_gpio_mode(GPIOA, 6, MODE_ANALOG);
+  set_gpio_mode(GPIOC, 4, MODE_ANALOG);
+  set_gpio_mode(GPIOB, 1, MODE_ANALOG);
+
+  // SBU channels
+  adc_init(ADC3);
+  set_gpio_mode(GPIOC, 2, MODE_ANALOG);
+  set_gpio_mode(GPIOC, 3, MODE_ANALOG);
+  set_gpio_mode(GPIOF, 9, MODE_ANALOG);
+  set_gpio_mode(GPIOF, 7, MODE_ANALOG);
+  set_gpio_mode(GPIOF, 5, MODE_ANALOG);
+  set_gpio_mode(GPIOF, 3, MODE_ANALOG);
+  set_gpio_mode(GPIOF, 10, MODE_ANALOG);
+  set_gpio_mode(GPIOF, 8, MODE_ANALOG);
+  set_gpio_mode(GPIOF, 6, MODE_ANALOG);
+  set_gpio_mode(GPIOF, 4, MODE_ANALOG);
+  set_gpio_mode(GPIOC, 0, MODE_ANALOG);
+  set_gpio_mode(GPIOC, 1, MODE_ANALOG);
+
+  // Header pins
+  set_gpio_mode(GPIOG, 0, MODE_OUTPUT);
+  set_gpio_mode(GPIOG, 1, MODE_OUTPUT);
+  set_gpio_mode(GPIOG, 2, MODE_OUTPUT);
+  set_gpio_mode(GPIOG, 3, MODE_OUTPUT);
+  set_gpio_mode(GPIOG, 4, MODE_OUTPUT);
+  set_gpio_mode(GPIOG, 5, MODE_OUTPUT);
+  set_gpio_mode(GPIOG, 6, MODE_OUTPUT);
+  set_gpio_mode(GPIOG, 7, MODE_OUTPUT);
+}
+
+void board_v2_tick(void) {}
+
+const board board_v2 = {
+  .board_type = "V2",
+  .has_canfd = true,
+  .has_sbu_sense = true,
+  .avdd_mV = 3300U,
+  .init = &board_v2_init,
+  .set_led = &board_v2_set_led,
+  .board_tick = &board_v2_tick,
+  .get_button = &board_v2_get_button,
+  .set_panda_power = &board_v2_set_panda_power,
+  .set_panda_individual_power = &board_v2_set_panda_individual_power,
+  .set_ignition = &board_v2_set_ignition,
+  .set_individual_ignition = &board_v2_set_individual_ignition,
+  .set_harness_orientation = &board_v2_set_harness_orientation,
+  .set_can_mode = &board_v2_set_can_mode,
+  .enable_can_transciever = &board_v2_enable_can_transciever,
+  .enable_header_pin = &board_v2_enable_header_pin,
+  .get_channel_power = &board_v2_get_channel_power,
+  .get_sbu_mV = &board_v2_get_sbu_mV,
+};
diff --git a/panda/board/jungle/flash.py b/panda/board/jungle/flash.py
new file mode 100755
index 0000000000..75a7f0c8ee
--- /dev/null
+++ b/panda/board/jungle/flash.py
@@ -0,0 +1,17 @@
+#!/usr/bin/env python3
+import os
+import subprocess
+
+from panda import PandaJungle
+
+board_path = os.path.dirname(os.path.realpath(__file__))
+
+if __name__ == "__main__":
+  subprocess.check_call(f"scons -C {board_path}/.. -u -j$(nproc) {board_path}", shell=True)
+
+  serials = PandaJungle.list()
+  print(f"found {len(serials)} panda jungle(s) - {serials}")
+  for s in serials:
+    print("flashing", s)
+    with PandaJungle(serial=s) as p:
+      p.flash()
diff --git a/panda/board/jungle/jungle_health.h b/panda/board/jungle/jungle_health.h
new file mode 100644
index 0000000000..931ed3715e
--- /dev/null
+++ b/panda/board/jungle/jungle_health.h
@@ -0,0 +1,24 @@
+// When changing these structs, python/__init__.py needs to be kept up to date!
+
+#define JUNGLE_HEALTH_PACKET_VERSION 1
+struct __attribute__((packed)) jungle_health_t {
+  uint32_t uptime_pkt;
+  float ch1_power;
+  float ch2_power;
+  float ch3_power;
+  float ch4_power;
+  float ch5_power;
+  float ch6_power;
+  uint16_t ch1_sbu1_mV;
+  uint16_t ch1_sbu2_mV;
+  uint16_t ch2_sbu1_mV;
+  uint16_t ch2_sbu2_mV;
+  uint16_t ch3_sbu1_mV;
+  uint16_t ch3_sbu2_mV;
+  uint16_t ch4_sbu1_mV;
+  uint16_t ch4_sbu2_mV;
+  uint16_t ch5_sbu1_mV;
+  uint16_t ch5_sbu2_mV;
+  uint16_t ch6_sbu1_mV;
+  uint16_t ch6_sbu2_mV;
+};
diff --git a/panda/board/jungle/main.c b/panda/board/jungle/main.c
new file mode 100644
index 0000000000..3ab39ba1d6
--- /dev/null
+++ b/panda/board/jungle/main.c
@@ -0,0 +1,216 @@
+// ********************* Includes *********************
+#include "board/config.h"
+
+#include "board/safety.h"
+#include "board/drivers/gmlan_alt.h"
+
+#include "board/drivers/pwm.h"
+#include "board/drivers/usb.h"
+
+#include "board/early_init.h"
+#include "board/provision.h"
+
+#include "board/health.h"
+#include "jungle_health.h"
+
+#include "board/drivers/can_common.h"
+
+#ifdef STM32H7
+  #include "board/drivers/fdcan.h"
+#else
+  #include "board/drivers/bxcan.h"
+#endif
+
+#include "board/obj/gitversion.h"
+
+#include "board/can_comms.h"
+#include "main_comms.h"
+
+
+// ********************* Serial debugging *********************
+
+void debug_ring_callback(uart_ring *ring) {
+  char rcv;
+  while (getc(ring, &rcv)) {
+    (void)injectc(ring, rcv);
+  }
+}
+
+// ***************************** main code *****************************
+
+// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
+void __initialize_hardware_early(void) {
+  early_initialization();
+}
+
+void __attribute__ ((noinline)) enable_fpu(void) {
+  // enable the FPU
+  SCB->CPACR |= ((3UL << (10U * 2U)) | (3UL << (11U * 2U)));
+}
+
+// called at 8Hz
+uint32_t loop_counter = 0U;
+uint16_t button_press_cnt = 0U;
+void tick_handler(void) {
+  if (TICK_TIMER->SR != 0) {
+    // tick drivers at 8Hz
+    usb_tick();
+
+    // decimated to 1Hz
+    if ((loop_counter % 8) == 0U) {
+      #ifdef DEBUG
+        print("** blink ");
+        print("rx:"); puth4(can_rx_q.r_ptr); print("-"); puth4(can_rx_q.w_ptr); print("  ");
+        print("tx1:"); puth4(can_tx1_q.r_ptr); print("-"); puth4(can_tx1_q.w_ptr); print("  ");
+        print("tx2:"); puth4(can_tx2_q.r_ptr); print("-"); puth4(can_tx2_q.w_ptr); print("  ");
+        print("tx3:"); puth4(can_tx3_q.r_ptr); print("-"); puth4(can_tx3_q.w_ptr); print("\n");
+      #endif
+
+      current_board->board_tick();
+
+      // check registers
+      check_registers();
+
+      // turn off the blue LED, turned on by CAN
+      current_board->set_led(LED_BLUE, false);
+
+      // Blink and OBD CAN
+#ifdef FINAL_PROVISIONING
+      current_board->set_can_mode(can_mode == CAN_MODE_NORMAL ? CAN_MODE_OBD_CAN2 : CAN_MODE_NORMAL);
+#endif
+
+      // on to the next one
+      uptime_cnt += 1U;
+    }
+
+    current_board->set_led(LED_GREEN, green_led_enabled);
+
+    // Check on button
+    bool current_button_status = current_board->get_button();
+
+    if (current_button_status && button_press_cnt == 10) {
+      current_board->set_panda_power(!panda_power);
+    }
+
+#ifdef FINAL_PROVISIONING
+    // Ignition blinking
+    uint8_t ignition_bitmask = 0U;
+    for (uint8_t i = 0U; i < 6U; i++) {
+      ignition_bitmask |= ((loop_counter % 12U) < ((uint32_t) i + 2U)) << i;
+    }
+    current_board->set_individual_ignition(ignition_bitmask);
+
+    // SBU voltage reporting
+    if (current_board->has_sbu_sense) {
+      for (uint8_t i = 0U; i < 6U; i++) {
+        CANPacket_t pkt = { 0 };
+        pkt.data_len_code = 8U;
+        pkt.addr = 0x100U + i;
+        *(uint16_t *) &pkt.data[0] = current_board->get_sbu_mV(i + 1U, SBU1);
+        *(uint16_t *) &pkt.data[2] = current_board->get_sbu_mV(i + 1U, SBU2);
+        pkt.data[4] = (ignition_bitmask >> i) & 1U;
+        can_set_checksum(&pkt);
+        can_send(&pkt, 0U, false);
+      }
+    }
+#else
+    // toggle ignition on button press
+    static bool prev_button_status = false;
+    if (!current_button_status && prev_button_status && button_press_cnt < 10){
+      current_board->set_ignition(!ignition);
+    }
+    prev_button_status = current_button_status;
+#endif
+
+    button_press_cnt = current_button_status ? button_press_cnt + 1 : 0;
+
+    loop_counter++;
+  }
+  TICK_TIMER->SR = 0;
+}
+
+
+int main(void) {
+  // Init interrupt table
+  init_interrupts(true);
+
+  // shouldn't have interrupts here, but just in case
+  disable_interrupts();
+
+  // init early devices
+  clock_init();
+  peripherals_init();
+  detect_board_type();
+  // red+green leds enabled until succesful USB init, as a debug indicator
+  current_board->set_led(LED_RED, true);
+  current_board->set_led(LED_GREEN, true);
+
+  // print hello
+  print("\n\n\n************************ MAIN START ************************\n");
+
+  // check for non-supported board types
+  if (hw_type == HW_TYPE_UNKNOWN) {
+    print("Unsupported board type\n");
+    while (1) { /* hang */ }
+  }
+
+  print("Config:\n");
+  print("  Board type: "); print(current_board->board_type); print("\n");
+
+  // init board
+  current_board->init();
+
+  // we have an FPU, let's use it!
+  enable_fpu();
+
+  microsecond_timer_init();
+
+  // 8Hz timer
+  REGISTER_INTERRUPT(TICK_TIMER_IRQ, tick_handler, 10U, FAULT_INTERRUPT_RATE_TICK)
+  tick_timer_init();
+
+#ifdef DEBUG
+  print("DEBUG ENABLED\n");
+#endif
+  // enable USB (right before interrupts or enum can fail!)
+  usb_init();
+
+  current_board->set_led(LED_RED, false);
+  current_board->set_led(LED_GREEN, false);
+
+  print("**** INTERRUPTS ON ****\n");
+  enable_interrupts();
+
+  can_silent = ALL_CAN_LIVE;
+  set_safety_hooks(SAFETY_ALLOUTPUT, 0U);
+
+  can_init_all();
+  current_board->set_harness_orientation(HARNESS_ORIENTATION_1);
+
+#ifdef FINAL_PROVISIONING
+  print("---- FINAL PROVISIONING BUILD ---- \n");
+  can_set_forwarding(0, 2);
+  can_set_forwarding(1, 2);
+#endif
+
+  // LED should keep on blinking all the time
+  uint64_t cnt = 0;
+  for (cnt=0;;cnt++) {
+    // useful for debugging, fade breaks = panda is overloaded
+    for (uint32_t fade = 0U; fade < MAX_LED_FADE; fade += 1U) {
+      current_board->set_led(LED_RED, true);
+      delay(fade >> 4);
+      current_board->set_led(LED_RED, false);
+      delay((MAX_LED_FADE - fade) >> 4);
+    }
+
+    for (uint32_t fade = MAX_LED_FADE; fade > 0U; fade -= 1U) {
+      current_board->set_led(LED_RED, true);
+      delay(fade >> 4);
+      current_board->set_led(LED_RED, false);
+      delay((MAX_LED_FADE - fade) >> 4);
+    }
+  }
+
+  return 0;
+}
diff --git a/panda/board/jungle/main_comms.h b/panda/board/jungle/main_comms.h
new file mode 100644
index 0000000000..49d16e5745
--- /dev/null
+++ b/panda/board/jungle/main_comms.h
@@ -0,0 +1,261 @@
+extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used
+
+int get_jungle_health_pkt(void *dat) {
+  COMPILE_TIME_ASSERT(sizeof(struct jungle_health_t) <= USBPACKET_MAX_SIZE);
+  struct jungle_health_t * health = (struct jungle_health_t*)dat;
+
+  health->uptime_pkt = uptime_cnt;
+  health->ch1_power = current_board->get_channel_power(1U);
+  health->ch2_power = current_board->get_channel_power(2U);
+  health->ch3_power = current_board->get_channel_power(3U);
+  health->ch4_power = current_board->get_channel_power(4U);
+  health->ch5_power = current_board->get_channel_power(5U);
+  health->ch6_power = current_board->get_channel_power(6U);
+
+  health->ch1_sbu1_mV = current_board->get_sbu_mV(1U, SBU1);
+  health->ch1_sbu2_mV = current_board->get_sbu_mV(1U, SBU2);
+  health->ch2_sbu1_mV = current_board->get_sbu_mV(2U, SBU1);
+  health->ch2_sbu2_mV = current_board->get_sbu_mV(2U, SBU2);
+  health->ch3_sbu1_mV = current_board->get_sbu_mV(3U, SBU1);
+  health->ch3_sbu2_mV = current_board->get_sbu_mV(3U, SBU2);
+  health->ch4_sbu1_mV = current_board->get_sbu_mV(4U, SBU1);
+  health->ch4_sbu2_mV = current_board->get_sbu_mV(4U, SBU2);
+  health->ch5_sbu1_mV = current_board->get_sbu_mV(5U, SBU1);
+  health->ch5_sbu2_mV = current_board->get_sbu_mV(5U, SBU2);
+  health->ch6_sbu1_mV = current_board->get_sbu_mV(6U, SBU1);
+  health->ch6_sbu2_mV = current_board->get_sbu_mV(6U, SBU2);
+
+  return sizeof(*health);
+}
+
+// send on serial, first byte to select the ring
+void comms_endpoint2_write(uint8_t *data, uint32_t len) {
+  UNUSED(data);
+  UNUSED(len);
+}
+
+int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
+  unsigned int resp_len = 0;
+  uint32_t time;
+
+#ifdef DEBUG_COMMS
+  print("raw control request: "); hexdump(req, sizeof(ControlPacket_t)); print("\n");
+  print("- request "); puth(req->request); print("\n");
+  print("- param1 "); puth(req->param1); print("\n");
+  print("- param2 "); puth(req->param2); print("\n");
+#endif
+
+  switch (req->request) {
+    // **** 0xa0: Set panda power.
+    case 0xa0:
+      current_board->set_panda_power((req->param1 == 1U));
+      break;
+    // **** 0xa1: Set harness orientation.
+    case 0xa1:
+      current_board->set_harness_orientation(req->param1);
+      break;
+    // **** 0xa2: Set ignition.
+    case 0xa2:
+      current_board->set_ignition((req->param1 == 1U));
+      break;
+    // **** 0xa0: Set panda power per channel by bitmask.
+    case 0xa3:
+      current_board->set_panda_individual_power(req->param1, (req->param2 > 0U));
+      break;
+    // **** 0xa8: get microsecond timer
+    case 0xa8:
+      time = microsecond_timer_get();
+      resp[0] = (time & 0x000000FFU);
+      resp[1] = ((time & 0x0000FF00U) >> 8U);
+      resp[2] = ((time & 0x00FF0000U) >> 16U);
+      resp[3] = ((time & 0xFF000000U) >> 24U);
+      resp_len = 4U;
+      break;
+    // **** 0xc0: reset communications
+    case 0xc0:
+      comms_can_reset();
+      break;
+    // **** 0xc1: get hardware type
+    case 0xc1:
+      resp[0] = hw_type;
+      resp_len = 1;
+      break;
+    // **** 0xc2: CAN health stats
+    case 0xc2:
+      COMPILE_TIME_ASSERT(sizeof(can_health_t) <= USBPACKET_MAX_SIZE);
+      if (req->param1 < 3U) {
+        update_can_health_pkt(req->param1, 0U);
+        can_health[req->param1].can_speed = (bus_config[req->param1].can_speed / 10U);
+        can_health[req->param1].can_data_speed = (bus_config[req->param1].can_data_speed / 10U);
+        can_health[req->param1].canfd_enabled = bus_config[req->param1].canfd_enabled;
+        can_health[req->param1].brs_enabled = bus_config[req->param1].brs_enabled;
+        can_health[req->param1].canfd_non_iso = bus_config[req->param1].canfd_non_iso;
+        resp_len = sizeof(can_health[req->param1]);
+        (void)memcpy(resp, &can_health[req->param1], resp_len);
+      }
+      break;
+    // **** 0xc3: fetch MCU UID
+    case 0xc3:
+      (void)memcpy(resp, ((uint8_t *)UID_BASE), 12);
+      resp_len = 12;
+      break;
+    // **** 0xd0: fetch serial (aka the provisioned dongle ID)
+    case 0xd0:
+      // addresses are OTP
+      if (req->param1 == 1U) {
+        (void)memcpy(resp, (uint8_t *)DEVICE_SERIAL_NUMBER_ADDRESS, 0x10);
+        resp_len = 0x10;
+      } else {
+        get_provision_chunk(resp);
+        resp_len = PROVISION_CHUNK_LEN;
+      }
+      break;
+    // **** 0xd1: enter bootloader mode
+    case 0xd1:
+      // this allows reflashing of the bootstub
+      switch (req->param1) {
+        case 0:
+          // only allow bootloader entry on debug builds
+          #ifdef ALLOW_DEBUG
+            print("-> entering bootloader\n");
+            enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
+            NVIC_SystemReset();
+          #endif
+          break;
+        case 1:
+          print("-> entering softloader\n");
+          enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC;
+          NVIC_SystemReset();
+          break;
+        default:
+          print("Bootloader mode invalid\n");
+          break;
+      }
+      break;
+    // **** 0xd2: get health packet
+    case 0xd2:
+      resp_len = get_jungle_health_pkt(resp);
+      break;
+    // **** 0xd3: get first 64 bytes of signature
+    case 0xd3:
+      {
+        resp_len = 64;
+        char * code = (char*)_app_start;
+        int code_len = _app_start[0];
+        (void)memcpy(resp, &code[code_len], resp_len);
+      }
+      break;
+    // **** 0xd4: get second 64 bytes of signature
+    case 0xd4:
+      {
+        resp_len = 64;
+        char * code = (char*)_app_start;
+        int code_len = _app_start[0];
+        (void)memcpy(resp, &code[code_len + 64], resp_len);
+      }
+      break;
+    // **** 0xd6: get version
+    case 0xd6:
+      COMPILE_TIME_ASSERT(sizeof(gitversion) <= USBPACKET_MAX_SIZE);
+      (void)memcpy(resp, gitversion, sizeof(gitversion));
+      resp_len = sizeof(gitversion) - 1U;
+      break;
+    // **** 0xd8: reset ST
+    case 0xd8:
+      NVIC_SystemReset();
+      break;
+    // **** 0xdb: set OBD CAN multiplexing mode
+    case 0xdb:
+      if (req->param1 == 1U) {
+        // Enable OBD CAN
+        current_board->set_can_mode(CAN_MODE_OBD_CAN2);
+      } else {
+        // Disable OBD CAN
+        current_board->set_can_mode(CAN_MODE_NORMAL);
+      }
+      break;
+    // **** 0xdd: get healthpacket and CANPacket versions
+    case 0xdd:
+      resp[0] = JUNGLE_HEALTH_PACKET_VERSION;
+      resp[1] = CAN_PACKET_VERSION;
+      resp[2] = CAN_HEALTH_PACKET_VERSION;
+      resp_len = 3;
+      break;
+    // **** 0xde: set can bitrate
+    case 0xde:
+      if ((req->param1 < PANDA_BUS_CNT) && is_speed_valid(req->param2, speeds, sizeof(speeds)/sizeof(speeds[0]))) {
+        bus_config[req->param1].can_speed = req->param2;
+        bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1));
+        UNUSED(ret);
+      }
+      break;
+    // **** 0xe0: debug read
+    case 0xe0:
+      // read
+      while ((resp_len < MIN(req->length, USBPACKET_MAX_SIZE)) && getc(get_ring_by_number(0), (char*)&resp[resp_len])) {
+        ++resp_len;
+      }
+      break;
+    // **** 0xe5: set CAN loopback (for testing)
+    case 0xe5:
+      can_loopback = (req->param1 > 0U);
+      can_init_all();
+      break;
+    // **** 0xf1: Clear CAN ring buffer.
+    case 0xf1:
+      if (req->param1 == 0xFFFFU) {
+        print("Clearing CAN Rx queue\n");
+        can_clear(&can_rx_q);
+      } else if (req->param1 < PANDA_BUS_CNT) {
+        print("Clearing CAN Tx queue\n");
+        can_clear(can_queues[req->param1]);
+      } else {
+        print("Clearing CAN CAN ring buffer failed: wrong bus number\n");
+      }
+      break;
+    // **** 0xf2: Clear debug ring buffer.
+    case 0xf2:
+      print("Clearing debug queue.\n");
+      clear_uart_buff(get_ring_by_number(0));
+      break;
+    // **** 0xf4: Set CAN transceiver enable pin
+    case 0xf4:
+      current_board->enable_can_transciever(req->param1, req->param2 > 0U);
+      break;
+    // **** 0xf5: Set CAN silent mode
+    case 0xf5:
+      can_silent = (req->param1 > 0U) ? ALL_CAN_SILENT : ALL_CAN_LIVE;
+      can_init_all();
+      break;
+    // **** 0xf7: enable/disable header pin by number
+    case 0xf7:
+      current_board->enable_header_pin(req->param1, req->param2 > 0U);
+      break;
+    // **** 0xf9: set CAN FD data bitrate
+    case 0xf9:
+      if ((req->param1 < PANDA_CAN_CNT) &&
+           current_board->has_canfd &&
+           is_speed_valid(req->param2, data_speeds, sizeof(data_speeds)/sizeof(data_speeds[0]))) {
+        bus_config[req->param1].can_data_speed = req->param2;
+        bus_config[req->param1].canfd_enabled = (req->param2 >= bus_config[req->param1].can_speed);
+        bus_config[req->param1].brs_enabled = (req->param2 > bus_config[req->param1].can_speed);
+        bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1));
+        UNUSED(ret);
+      }
+      break;
+    // **** 0xfc: set CAN FD non-ISO mode
+    case 0xfc:
+      if ((req->param1 < PANDA_CAN_CNT) && current_board->has_canfd) {
+        bus_config[req->param1].canfd_non_iso = (req->param2 != 0U);
+        bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1));
+        UNUSED(ret);
+      }
+      break;
+    default:
+      print("NO HANDLER ");
+      puth(req->request);
+      print("\n");
+      break;
+  }
+  return resp_len;
+}
diff --git a/panda/board/jungle/recover.py b/panda/board/jungle/recover.py
new file mode 100755
index 0000000000..98afb06748
--- /dev/null
+++ b/panda/board/jungle/recover.py
@@ -0,0 +1,26 @@
+#!/usr/bin/env python3
+import os
+import time
+import subprocess
+
+from panda import PandaJungle, PandaJungleDFU
+
+board_path = os.path.dirname(os.path.realpath(__file__))
+
+if __name__ == "__main__":
+  subprocess.check_call(f"scons -C {board_path}/.. -u -j$(nproc) {board_path}", shell=True)
+
+  for s in PandaJungle.list():
+    print("putting", s, "in DFU mode")
+    with PandaJungle(serial=s) as p:
+      p.reset(enter_bootstub=True)
+      p.reset(enter_bootloader=True)
+
+  # wait for reset panda jungles to come back up
+  time.sleep(1)
+
+  dfu_serials = PandaJungleDFU.list()
+  print(f"found {len(dfu_serials)} panda jungle(s) in DFU - {dfu_serials}")
+  for s in dfu_serials:
+    print("flashing", s)
+    PandaJungleDFU(s).recover()
diff --git a/panda/board/jungle/scripts/can_health.py b/panda/board/jungle/scripts/can_health.py
new file mode 100755
index 0000000000..ff068b5baa
--- /dev/null
+++ b/panda/board/jungle/scripts/can_health.py
@@ -0,0 +1,13 @@
+#!/usr/bin/env python3
+
+import time
+from panda import PandaJungle
+
+if __name__ == "__main__":
+  jungle = PandaJungle()
+
+  while True:
+    for bus in range(3):
+      print(bus, jungle.can_health(bus))
+    print()
+    time.sleep(1)
diff --git a/panda/board/jungle/scripts/can_printer.py b/panda/board/jungle/scripts/can_printer.py
new file mode 100755
index 0000000000..675fc508a1
--- /dev/null
+++ b/panda/board/jungle/scripts/can_printer.py
@@ -0,0 +1,35 @@
+#!/usr/bin/env python3
+import os
+import time
+from collections import defaultdict
+import binascii
+
+from panda import PandaJungle
+
+# fake
+def sec_since_boot():
+  return time.time()
+
+def can_printer():
+  p = PandaJungle()
+
+  start = sec_since_boot()
+  lp = sec_since_boot()
+  msgs = defaultdict(list)
+  canbus = int(os.getenv("CAN", "0"))
+  while True:
+    can_recv = p.can_recv()
+    for address, _, dat, src  in can_recv:
+      if src == canbus:
+        msgs[address].append(dat)
+
+    if sec_since_boot() - lp > 0.1:
+      dd = chr(27) + "[2J"
+      dd += "%5.2f\n" % (sec_since_boot() - start)
+      for k,v in sorted(zip(list(msgs.keys()), [binascii.hexlify(x[-1]) for x in list(msgs.values())], strict=True)):
+        dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k,k),len(msgs[k]), v)
+      print(dd)
+      lp = sec_since_boot()
+
+if __name__ == "__main__":
+  can_printer()
diff --git a/panda/board/jungle/scripts/debug_console.py b/panda/board/jungle/scripts/debug_console.py
new file mode 100755
index 0000000000..f35388796f
--- /dev/null
+++ b/panda/board/jungle/scripts/debug_console.py
@@ -0,0 +1,38 @@
+#!/usr/bin/env python3
+import os
+import sys
+import time
+
+from panda import PandaJungle
+
+setcolor = ["\033[1;32;40m", "\033[1;31;40m"]
+unsetcolor = "\033[00m"
+
+if __name__ == "__main__":
+  while True:
+    try:
+      claim = os.getenv("CLAIM") is not None
+
+      serials = PandaJungle.list()
+      if os.getenv("SERIAL"):
+        serials = [x for x in serials if x==os.getenv("SERIAL")]
+
+      panda_jungles = [PandaJungle(x, claim=claim) for x in serials]
+
+      if not len(panda_jungles):
+        sys.exit("no panda jungles found")
+
+      while True:
+        for i, panda_jungle in enumerate(panda_jungles):
+          while True:
+            ret = panda_jungle.debug_read()
+            if len(ret) > 0:
+              sys.stdout.write(setcolor[i] + ret.decode('ascii') + unsetcolor)
+              sys.stdout.flush()
+            else:
+              break
+          time.sleep(0.01)
+    except Exception as e:
+      print(e)
+      print("panda jungle disconnected!")
+      time.sleep(0.5)
diff --git a/panda/board/jungle/scripts/echo_loopback_test.py b/panda/board/jungle/scripts/echo_loopback_test.py
new file mode 100755
index 0000000000..78b65b5341
--- /dev/null
+++ b/panda/board/jungle/scripts/echo_loopback_test.py
@@ -0,0 +1,68 @@
+#!/usr/bin/env python3
+import os
+import time
+import contextlib
+import random
+from termcolor import cprint
+
+from panda import PandaJungle
+
+# This script is intended to be used in conjunction with the echo.py test script from panda.
+# It sends messages on bus 0, 1, 2 and checks for a reversed response to be sent back.
+
+#################################################################
+############################# UTILS #############################
+#################################################################
+def print_colored(text, color):
+  cprint(text + " "*40, color, end="\r")
+
+def get_test_string():
+  return b"test"+os.urandom(4)
+
+#################################################################
+############################# TEST ##############################
+#################################################################
+
+def test_loopback():
+  for bus in range(3):
+    # Clear can
+    jungle.can_clear(bus)
+    # Send a random message
+    address = random.randint(1, 2000)
+    data = get_test_string()
+    jungle.can_send(address, data, bus)
+    time.sleep(0.1)
+
+    # Make sure it comes back reversed
+    incoming = jungle.can_recv()
+    found = False
+    for message in incoming:
+      incomingAddress, _, incomingData, incomingBus = message
+      if incomingAddress == address and incomingData == data[::-1] and incomingBus == bus:
+        found = True
+        break
+    if not found:
+      cprint("\nFAILED", "red")
+      raise AssertionError
+
+#################################################################
+############################# MAIN ##############################
+#################################################################
+jungle = None
+counter = 0
+
+if __name__ == "__main__":
+  # Connect to jungle silently
+  print_colored("Connecting to jungle", "blue")
+  with open(os.devnull, "w") as devnull:
+    with contextlib.redirect_stdout(devnull):
+      jungle = PandaJungle()
+  jungle.set_panda_power(True)
+  jungle.set_ignition(False)
+
+  # Run test
+  while True:
+    jungle.can_clear(0xFFFF)
+    test_loopback()
+    counter += 1
+    print_colored(str(counter) + " loopback cycles complete", "blue")
diff --git a/panda/board/jungle/scripts/get_version.py b/panda/board/jungle/scripts/get_version.py
new file mode 100755
index 0000000000..ad4a1c4264
--- /dev/null
+++ b/panda/board/jungle/scripts/get_version.py
@@ -0,0 +1,9 @@
+#!/usr/bin/env python3
+from panda import PandaJungle
+
+if __name__ == "__main__":
+  for p in PandaJungle.list():
+    pp = PandaJungle(p)
+    print("%s: %s" % (pp.get_serial()[0], pp.get_version()))
+
+
diff --git a/panda/board/jungle/scripts/health_test.py b/panda/board/jungle/scripts/health_test.py
new file mode 100755
index 0000000000..039f840e0b
--- /dev/null
+++ b/panda/board/jungle/scripts/health_test.py
@@ -0,0 +1,21 @@
+#!/usr/bin/env python3
+import time
+from pprint import pprint
+
+from panda import PandaJungle
+
+if __name__ == "__main__":
+  i = 0
+  pi = 0
+
+  pj = PandaJungle()
+  while True:
+    st = time.monotonic()
+    while time.monotonic() - st < 1:
+      pj.health()
+      pj.can_health(0)
+      i += 1
+    pprint(pj.health())
+    print(f"Speed: {i - pi}Hz")
+    pi = i
+
diff --git a/panda/board/jungle/scripts/loopback_test.py b/panda/board/jungle/scripts/loopback_test.py
new file mode 100755
index 0000000000..10caf42cc4
--- /dev/null
+++ b/panda/board/jungle/scripts/loopback_test.py
@@ -0,0 +1,140 @@
+#!/usr/bin/env python3
+import os
+import time
+import contextlib
+import random
+from termcolor import cprint
+
+from panda import Panda, PandaJungle
+
+NUM_PANDAS_PER_TEST = 1
+FOR_RELEASE_BUILDS = os.getenv("RELEASE") is not None       # Release builds do not have ALLOUTPUT mode
+
+BUS_SPEEDS = [125, 500, 1000]
+
+#################################################################
+############################# UTILS #############################
+#################################################################
+# To suppress the connection text
+def silent_panda_connect(serial):
+  with open(os.devnull, "w") as devnull:
+    with contextlib.redirect_stdout(devnull):
+      panda = Panda(serial)
+  return panda
+
+def print_colored(text, color):
+  cprint(text + " "*40, color, end="\r")
+
+def connect_to_pandas():
+  print_colored("Connecting to pandas", "blue")
+  # Connect to pandas
+  pandas = []
+  for serial in panda_serials:
+    pandas.append(silent_panda_connect(serial))
+  print_colored("Connected", "blue")
+
+def start_with_orientation(orientation):
+  print_colored("Restarting pandas with orientation " + str(orientation), "blue")
+  jungle.set_panda_power(False)
+  jungle.set_harness_orientation(orientation)
+  time.sleep(4)
+  jungle.set_panda_power(True)
+  time.sleep(2)
+  connect_to_pandas()
+
+def can_loopback(sender):
+  receivers = list(filter((lambda p: (p != sender)), [jungle] + pandas))
+
+  for bus in range(4):
+    obd = False
+    if bus == 3:
+      obd = True
+      bus = 1
+
+    # Clear buses
+    for receiver in receivers:
+      receiver.set_obd(obd)
+      receiver.can_clear(bus)     # TX
+      receiver.can_clear(0xFFFF)  # RX
+
+    # Send a random string
+    addr = 0x18DB33F1 if FOR_RELEASE_BUILDS else random.randint(1, 2000)
+    string = b"test"+os.urandom(4)
+    sender.set_obd(obd)
+    time.sleep(0.2)
+    sender.can_send(addr, string, bus)
+    time.sleep(0.2)
+
+    # Check if all receivers have indeed received them in their receiving buffers
+    for receiver in receivers:
+      content = receiver.can_recv()
+
+      # Check amount of messages
+      if len(content) != 1:
+        raise Exception("Amount of received CAN messages (" + str(len(content)) + ") does not equal 1. Bus: " + str(bus) +" OBD: " + str(obd))
+
+      # Check content
+      if content[0][0] != addr or content[0][2] != string:
+        raise Exception("Received CAN message content or address does not match")
+
+      # Check bus
+      if content[0][3] != bus:
+        raise Exception("Received CAN message bus does not match")
+
+#################################################################
+############################# TEST ##############################
+#################################################################
+
+def test_loopback():
+  # disable safety modes
+  for panda in pandas:
+    panda.set_safety_mode(Panda.SAFETY_ELM327 if FOR_RELEASE_BUILDS else Panda.SAFETY_ALLOUTPUT)
+
+  # perform loopback with jungle as a sender
+  can_loopback(jungle)
+
+  # perform loopback with each possible panda as a sender
+  for panda in pandas:
+    can_loopback(panda)
+
+  # enable safety modes
+  for panda in pandas:
+    panda.set_safety_mode(Panda.SAFETY_SILENT)
+
+#################################################################
+############################# MAIN ##############################
+#################################################################
+jungle = None
+pandas = []  # type: ignore
+panda_serials = []
+counter = 0
+
+if __name__ == "__main__":
+  # Connect to jungle silently
+  print_colored("Connecting to jungle", "blue")
+  with open(os.devnull, "w") as devnull:
+    with contextlib.redirect_stdout(devnull):
+      jungle = PandaJungle()
+  jungle.set_panda_power(True)
+  jungle.set_ignition(False)
+
+  # Connect to <NUM_PANDAS_PER_TEST> new pandas before starting tests
+  print_colored("Waiting for " + str(NUM_PANDAS_PER_TEST) + " pandas to be connected", "yellow")
+  while True:
+    connected_serials = Panda.list()
+    if len(connected_serials) == NUM_PANDAS_PER_TEST:
+      panda_serials = connected_serials
+      break
+
+  start_with_orientation(PandaJungle.HARNESS_ORIENTATION_1)
+
+  # Set bus speeds
+  for device in pandas + [jungle]:
+    for bus in range(len(BUS_SPEEDS)):
+      device.set_can_speed_kbps(bus, BUS_SPEEDS[bus])
+
+  # Run test
+  while True:
+    test_loopback()
+    counter += 1
+    print_colored(str(counter) + " loopback cycles complete", "blue")
diff --git a/panda/board/jungle/scripts/panda_identification_test.py b/panda/board/jungle/scripts/panda_identification_test.py
new file mode 100755
index 0000000000..a61b0d608b
--- /dev/null
+++ b/panda/board/jungle/scripts/panda_identification_test.py
@@ -0,0 +1,45 @@
+#!/usr/bin/env python3
+import os
+import time
+import random
+import contextlib
+
+from panda import PandaJungle
+from panda import Panda
+
+PANDA_UNDER_TEST = Panda.HW_TYPE_UNO
+
+panda_jungle = PandaJungle()
+
+def silent_panda_connect():
+  with open(os.devnull, "w") as devnull:
+    with contextlib.redirect_stdout(devnull):
+      panda = Panda()
+  return panda
+
+def reboot_panda(harness_orientation=PandaJungle.HARNESS_ORIENTATION_NONE, ignition=False):
+  print(f"Restarting panda with harness orientation: {harness_orientation} and ignition: {ignition}")
+  panda_jungle.set_panda_power(False)
+  panda_jungle.set_harness_orientation(harness_orientation)
+  panda_jungle.set_ignition(ignition)
+  time.sleep(2)
+  panda_jungle.set_panda_power(True)
+  time.sleep(2)
+
+count = 0
+if __name__ == "__main__":
+  while True:
+    ignition = random.randint(0, 1)
+    harness_orientation = random.randint(0, 2)
+    reboot_panda(harness_orientation, ignition)
+
+    p = silent_panda_connect()
+    assert p.get_type() == PANDA_UNDER_TEST
+    assert p.health()['car_harness_status'] == harness_orientation
+    if harness_orientation != PandaJungle.HARNESS_ORIENTATION_NONE:
+      assert p.health()['ignition_line'] == ignition
+
+    count += 1
+    print(f"Passed {count} loops")
+
+
diff --git a/panda/board/jungle/scripts/start.py b/panda/board/jungle/scripts/start.py
new file mode 100755
index 0000000000..76afd14ac4
--- /dev/null
+++ b/panda/board/jungle/scripts/start.py
@@ -0,0 +1,18 @@
+#!/usr/bin/env python
+import sys
+
+from panda import PandaJungle
+
+if __name__ == "__main__":
+  jungle = PandaJungle()
+
+  # If first argument specified, it sets the ignition (0 or 1)
+  if len(sys.argv) > 1:
+    if sys.argv[1] == '1':
+      jungle.set_ignition(True)
+    else:
+      jungle.set_ignition(False)
+  jungle.set_harness_orientation(1)
+  jungle.set_panda_power(True)
+
+
diff --git a/panda/board/jungle/stm32fx/board.h b/panda/board/jungle/stm32fx/board.h
new file mode 100644
index 0000000000..0adf7923b2
--- /dev/null
+++ b/panda/board/jungle/stm32fx/board.h
@@ -0,0 +1,7 @@
+#include "boards/board_declarations.h"
+#include "boards/board_v1.h"
+
+void detect_board_type(void) {
+  hw_type = HW_TYPE_V1;
+  current_board = &board_v1;
+}
diff --git a/panda/board/jungle/stm32h7/board.h b/panda/board/jungle/stm32h7/board.h
new file mode 100644
index 0000000000..4fe4fa4637
--- /dev/null
+++ b/panda/board/jungle/stm32h7/board.h
@@ -0,0 +1,9 @@
+#include "boards/board_declarations.h"
+
+#include "stm32h7/lladc.h"
+#include "boards/board_v2.h"
+
+void detect_board_type(void) {
+  hw_type = HW_TYPE_V2;
+  current_board = &board_v2;
+}
diff --git a/panda/board/jungle/stm32h7/lladc.h b/panda/board/jungle/stm32h7/lladc.h
new file mode 100644
index 0000000000..06b742dd38
--- /dev/null
+++ b/panda/board/jungle/stm32h7/lladc.h
@@ -0,0 +1,54 @@
+
+typedef struct {
+  ADC_TypeDef *adc;
+  uint8_t channel;
+} adc_channel_t;
+
+void adc_init(ADC_TypeDef *adc) {
+  adc->CR &= ~(ADC_CR_DEEPPWD); // Reset deep-power-down mode
+  adc->CR |= ADC_CR_ADVREGEN; // Enable ADC regulator
+  while(!(adc->ISR & ADC_ISR_LDORDY) && (adc != ADC3));
+
+  if (adc != ADC3) {
+    adc->CR &= ~(ADC_CR_ADCALDIF); // Choose single-ended calibration
+    adc->CR |= ADC_CR_ADCALLIN; // Lineriality calibration
+  }
+  adc->CR |= ADC_CR_ADCAL; // Start calibrtation
+  while((adc->CR & ADC_CR_ADCAL) != 0);
+
+  adc->ISR |= ADC_ISR_ADRDY;
+  adc->CR |= ADC_CR_ADEN;
+  while(!(adc->ISR & ADC_ISR_ADRDY));
+}
+
+uint16_t adc_get_raw(ADC_TypeDef *adc, uint8_t channel) {
+  adc->SQR1 &= ~(ADC_SQR1_L);
+  adc->SQR1 = ((uint32_t) channel << 6U);
+
+  if (channel < 10U) {
+    adc->SMPR1 = (0x7U << (channel * 3U));
+  } else {
+    adc->SMPR2 = (0x7U << ((channel - 10U) * 3U));
+  }
+  adc->PCSEL_RES0 = (0x1U << channel);
+
+  adc->CR |= ADC_CR_ADSTART;
+  while (!(adc->ISR & ADC_ISR_EOC));
+
+  uint16_t res = adc->DR;
+
+  while (!(adc->ISR & ADC_ISR_EOS));
+  adc->ISR |= ADC_ISR_EOS;
+
+  return res;
+}
+
+uint16_t adc_get_mV(ADC_TypeDef *adc, uint8_t channel) {
+  uint16_t ret = 0;
+  if ((adc == ADC1) || (adc == ADC2)) {
+    ret = (adc_get_raw(adc, channel) * current_board->avdd_mV) / 65535U;
+  } else if (adc == ADC3) {
+    ret = (adc_get_raw(adc, channel) * current_board->avdd_mV) / 4095U;
+  } else {}
+  return ret;
+}
diff --git a/panda/board/main.c b/panda/board/main.c
index e9e42e2be8..0d692d0e91 100644
--- a/panda/board/main.c
+++ b/panda/board/main.c
@@ -6,7 +6,7 @@
 #include "drivers/gmlan_alt.h"
 #include "drivers/kline_init.h"
 #include "drivers/simple_watchdog.h"
-#include "drivers/logging.h"
+#include "drivers/bootkick.h"
 
 #include "early_init.h"
 #include "provision.h"
@@ -82,21 +82,21 @@ void set_safety_mode(uint16_t mode, uint16_t param) {
 
   switch (mode_copy) {
     case SAFETY_SILENT:
-      set_intercept_relay(false);
+      set_intercept_relay(false, false);
       if (current_board->has_obd) {
         current_board->set_can_mode(CAN_MODE_NORMAL);
       }
       can_silent = ALL_CAN_SILENT;
       break;
     case SAFETY_NOOUTPUT:
-      set_intercept_relay(false);
+      set_intercept_relay(false, false);
       if (current_board->has_obd) {
         current_board->set_can_mode(CAN_MODE_NORMAL);
       }
       can_silent = ALL_CAN_LIVE;
       break;
     case SAFETY_ELM327:
-      set_intercept_relay(false);
+      set_intercept_relay(false, false);
       heartbeat_counter = 0U;
       heartbeat_lost = false;
       if (current_board->has_obd) {
@@ -109,7 +109,7 @@ void set_safety_mode(uint16_t mode, uint16_t param) {
       can_silent = ALL_CAN_LIVE;
       break;
     default:
-      set_intercept_relay(true);
+      set_intercept_relay(true, false);
       heartbeat_counter = 0U;
       heartbeat_lost = false;
       if (current_board->has_obd) {
@@ -146,9 +146,6 @@ void __attribute__ ((noinline)) enable_fpu(void) {
 
 // called at 8Hz
 uint8_t loop_counter = 0U;
-uint8_t previous_harness_status = HARNESS_STATUS_NC;
-uint32_t waiting_to_boot_count = 0;
-bool waiting_to_boot = false;
 void tick_handler(void) {
   if (TICK_TIMER->SR != 0) {
     // siren
@@ -157,6 +154,7 @@ void tick_handler(void) {
     // tick drivers at 8Hz
     fan_tick();
     usb_tick();
+    harness_tick();
     simple_watchdog_kick();
 
     // decimated to 1Hz
@@ -184,32 +182,10 @@ void tick_handler(void) {
       // unless we are in power saving mode
       current_board->set_led(LED_BLUE, (uptime_cnt & 1U) && (power_save_status == POWER_SAVE_STATUS_ENABLED));
 
-      // tick drivers at 1Hz
-      harness_tick();
-      logging_tick();
-
       const bool recent_heartbeat = heartbeat_counter == 0U;
-      const bool harness_inserted = (harness.status != previous_harness_status) && (harness.status != HARNESS_STATUS_NC);
-      const bool just_bootkicked = current_board->board_tick(check_started(), usb_enumerated, recent_heartbeat, harness_inserted);
-      previous_harness_status = harness.status;
-
-      // log device boot time
-      const bool som_running = current_board->read_som_gpio();
-      if (just_bootkicked && !som_running) {
-        log("bootkick");
-        waiting_to_boot = true;
-      }
-      if (waiting_to_boot) {
-        if (som_running) {
-          log("device booted");
-          waiting_to_boot = false;
-        } else if (waiting_to_boot_count == 10U) {
-          log("not booted after 10s");
-        } else {
 
-        }
-        waiting_to_boot_count += 1U;
-      }
+      // tick drivers at 1Hz
+      bootkick_tick(check_started(), recent_heartbeat);
 
       // increase heartbeat counter and cap it at the uint32 limit
       if (heartbeat_counter < __UINT32_MAX__) {
@@ -237,7 +213,7 @@ void tick_handler(void) {
       if (controls_allowed && !heartbeat_engaged) {
         heartbeat_engaged_mismatches += 1U;
         if (heartbeat_engaged_mismatches >= 3U) {
-          controls_allowed = 0U;
+          controls_allowed = false;
         }
       } else {
         heartbeat_engaged_mismatches = 0U;
@@ -348,8 +324,10 @@ int main(void) {
   clock_init();
   peripherals_init();
   detect_board_type();
+  // red+green leds enabled until succesful USB/SPI init, as a debug indicator
+  current_board->set_led(LED_RED, true);
+  current_board->set_led(LED_GREEN, true);
   adc_init();
-  logging_init();
 
   // print hello
   print("\n\n\n************************ MAIN START ************************\n");
@@ -369,15 +347,6 @@ int main(void) {
   // panda has an FPU, let's use it!
   enable_fpu();
 
-  log("main start");
-
-  if (current_board->has_gps) {
-    uart_init(&uart_ring_gps, 9600);
-  } else {
-    // enable ESP uart
-    uart_init(&uart_ring_gps, 115200);
-  }
-
   if (current_board->has_lin) {
     // enable LIN
     uart_init(&uart_ring_lin1, 10400);
@@ -417,6 +386,9 @@ int main(void) {
   }
 #endif
 
+  current_board->set_led(LED_RED, false);
+  current_board->set_led(LED_GREEN, false);
+
   print("**** INTERRUPTS ON ****\n");
   enable_interrupts();
 
diff --git a/panda/board/main_comms.h b/panda/board/main_comms.h
index c1591a0608..c821f4cfc1 100644
--- a/panda/board/main_comms.h
+++ b/panda/board/main_comms.h
@@ -44,6 +44,8 @@ int get_health_pkt(void *dat) {
   health->sbu1_voltage_mV = harness.sbu1_voltage_mV;
   health->sbu2_voltage_mV = harness.sbu2_voltage_mV;
 
+  health->som_reset_triggered = bootkick_reset_triggered;
+
   return sizeof(*health);
 }
 
@@ -182,8 +184,8 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
       (void)memcpy(resp, ((uint8_t *)UID_BASE), 12);
       resp_len = 12;
       break;
+    // **** 0xc4: get interrupt call rate
     case 0xc4:
-      // **** 0xc4: get interrupt call rate
       if (req->param1 < NUM_INTERRUPTS) {
         uint32_t load = interrupts[req->param1].call_rate;
         resp[0] = (load & 0x000000FFU);
@@ -193,6 +195,15 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
         resp_len = 4U;
       }
       break;
+    // **** 0xc5: DEBUG: drive relay
+    case 0xc5:
+      set_intercept_relay((req->param1 & 0x1U), (req->param1 & 0x2U));
+      break;
+    // **** 0xc6: DEBUG: read SOM GPIO
+    case 0xc6:
+      resp[0] = current_board->read_som_gpio();
+      resp_len = 1;
+      break;
     // **** 0xd0: fetch serial (aka the provisioned dongle ID)
     case 0xd0:
       // addresses are OTP
@@ -258,28 +269,6 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
     case 0xd8:
       NVIC_SystemReset();
       break;
-    // **** 0xd9: set ESP power
-    case 0xd9:
-      if (req->param1 == 1U) {
-        current_board->set_gps_mode(GPS_ENABLED);
-      } else if (req->param1 == 2U) {
-        current_board->set_gps_mode(GPS_BOOTMODE);
-      } else {
-        current_board->set_gps_mode(GPS_DISABLED);
-      }
-      break;
-    // **** 0xda: reset ESP, with optional boot mode
-    case 0xda:
-      current_board->set_gps_mode(GPS_DISABLED);
-      delay(1000000);
-      if (req->param1 == 1U) {
-        current_board->set_gps_mode(GPS_BOOTMODE);
-      } else {
-        current_board->set_gps_mode(GPS_ENABLED);
-      }
-      delay(1000000);
-      current_board->set_gps_mode(GPS_ENABLED);
-      break;
     // **** 0xdb: set GMLAN (white/grey) or OBD CAN (black) multiplexing mode
     case 0xdb:
       if(current_board->has_obd){
@@ -339,11 +328,6 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
         break;
       }
 
-      // TODO: Remove this again and fix boardd code to hande the message bursts instead of single chars
-      if (ur == &uart_ring_gps) {
-        dma_pointer_handler(ur, DMA2_Stream5->NDTR);
-      }
-
       // read
       while ((resp_len < MIN(req->length, USBPACKET_MAX_SIZE)) &&
                          getc(ur, (char*)&resp[resp_len])) {
@@ -396,6 +380,10 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
       can_loopback = (req->param1 > 0U);
       can_init_all();
       break;
+    // **** 0xe6: set custom clock source period
+    case 0xe6:
+      clock_source_set_period(req->param1);
+      break;
     // **** 0xe7: set power save state
     case 0xe7:
       set_power_save_state(req->param1);
@@ -490,18 +478,6 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
         UNUSED(ret);
       }
       break;
-    // *** 0xfd: read logs
-    case 0xfd:
-      if (req->param1 == 1U) {
-        logging_init_read_index();
-      }
-
-      if (req->param2 != 0xFFFFU) {
-        logging_find_read_index(req->param2);
-      }
-
-      resp_len = logging_read(resp);
-      break;
     default:
       print("NO HANDLER ");
       puth(req->request);
diff --git a/panda/board/pedal/README b/panda/board/pedal/README.md
similarity index 100%
rename from panda/board/pedal/README
rename to panda/board/pedal/README.md
diff --git a/panda/board/pedal/SConscript b/panda/board/pedal/SConscript
new file mode 100644
index 0000000000..bfa3c2c19d
--- /dev/null
+++ b/panda/board/pedal/SConscript
@@ -0,0 +1,28 @@
+import copy
+
+Import('build_project')
+
+build_projects = {}
+
+build_projects["pedal"] = {
+  "MAIN": "main.c",
+  "BOOTSTUB": "../bootstub.c",
+  "STARTUP_FILE": "../stm32fx/startup_stm32f205xx.s",
+  "LINKER_SCRIPT": "../stm32fx/stm32f2_flash.ld",
+  "APP_START_ADDRESS": "0x8004000",
+  "PROJECT_FLAGS": [
+    "-mcpu=cortex-m3",
+    "-msoft-float",
+    "-DSTM32F2",
+    "-DSTM32F205xx",
+    "-O2",
+    "-DPEDAL",
+  ],
+}
+
+# build with the USB driver enabled
+build_projects["pedal_usb"] = copy.deepcopy(build_projects["pedal"])
+build_projects["pedal_usb"]["PROJECT_FLAGS"].append("-DPEDAL_USB")
+
+for project_name, project in build_projects.items():
+  build_project(project_name, project, [])
diff --git a/panda/board/pedal/flash_can.sh b/panda/board/pedal/flash_can.sh
index 0225bf8dff..b9edf25f12 100755
--- a/panda/board/pedal/flash_can.sh
+++ b/panda/board/pedal/flash_can.sh
@@ -2,7 +2,7 @@
 set -e
 
 cd ..
-PEDAL=1 scons -u -j$(nproc)
+scons -u -j$(nproc)
 cd pedal
 
-../../tests/pedal/enter_canloader.py ../obj/pedal.bin.signed
+../../tests/pedal/enter_canloader.py obj/pedal.bin.signed
diff --git a/panda/board/pedal/main.c b/panda/board/pedal/main.c
index eb40466106..e0020f1123 100644
--- a/panda/board/pedal/main.c
+++ b/panda/board/pedal/main.c
@@ -266,7 +266,7 @@ int main(void) {
   REGISTER_INTERRUPT(CAN1_TX_IRQn, CAN1_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
   REGISTER_INTERRUPT(CAN1_RX0_IRQn, CAN1_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
   REGISTER_INTERRUPT(CAN1_SCE_IRQn, CAN1_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
-  
+
   // Should run at around 732Hz (see init below)
   REGISTER_INTERRUPT(TIM3_IRQn, TIM3_IRQ_Handler, 1000U, FAULT_INTERRUPT_RATE_TIM3)
 
@@ -290,12 +290,12 @@ int main(void) {
   adc_init();
 
   // init can
-  bool llcan_speed_set = llcan_set_speed(CAN1, 5000, false, false);
+  bool llcan_speed_set = llcan_set_speed(CAN, 5000, false, false);
   if (!llcan_speed_set) {
     print("Failed to set llcan speed");
   }
 
-  bool ret = llcan_init(CAN1);
+  bool ret = llcan_init(CAN);
   UNUSED(ret);
 
   // 48mhz / 65536 ~= 732
diff --git a/panda/board/pedal/recover.sh b/panda/board/pedal/recover.sh
index 98c5e19862..d7fe0aff5f 100755
--- a/panda/board/pedal/recover.sh
+++ b/panda/board/pedal/recover.sh
@@ -4,8 +4,8 @@ set -e
 DFU_UTIL="dfu-util"
 
 cd ..
-PEDAL=1 scons -u -j$(nproc)
+scons -u -j$(nproc)
 cd pedal
 
-$DFU_UTIL -d 0483:df11 -a 0 -s 0x08004000 -D ../obj/pedal.bin.signed
-$DFU_UTIL -d 0483:df11 -a 0 -s 0x08000000:leave -D ../obj/bootstub.pedal.bin
+$DFU_UTIL -d 0483:df11 -a 0 -s 0x08004000 -D obj/pedal.bin.signed
+$DFU_UTIL -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.pedal.bin
diff --git a/panda/board/power_saving.h b/panda/board/power_saving.h
index 64baad0320..2b7d7c9872 100644
--- a/panda/board/power_saving.h
+++ b/panda/board/power_saving.h
@@ -13,11 +13,7 @@ void set_power_save_state(int state) {
     bool enable = false;
     if (state == POWER_SAVE_STATUS_ENABLED) {
       print("enable power savings\n");
-      if (current_board->has_gps) {
-        const char UBLOX_SLEEP_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x08\x00\x17\x78";
-        uart_ring *ur = get_ring_by_number(1);
-        for (unsigned int i = 0; i < sizeof(UBLOX_SLEEP_MSG) - 1U; i++) while (!putc(ur, UBLOX_SLEEP_MSG[i]));
-      }
+
       // Disable CAN interrupts
       if (harness.status == HARNESS_STATUS_FLIPPED) {
         llcan_irq_disable(cans[0]);
@@ -27,11 +23,6 @@ void set_power_save_state(int state) {
       llcan_irq_disable(cans[1]);
     } else {
       print("disable power savings\n");
-      if (current_board->has_gps) {
-        const char UBLOX_WAKE_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x09\x00\x18\x7a";
-        uart_ring *ur = get_ring_by_number(1);
-        for (unsigned int i = 0; i < sizeof(UBLOX_WAKE_MSG) - 1U; i++) while (!putc(ur, UBLOX_WAKE_MSG[i]));
-      }
 
       if (harness.status == HARNESS_STATUS_FLIPPED) {
         llcan_irq_enable(cans[0]);
@@ -45,13 +36,6 @@ void set_power_save_state(int state) {
 
     current_board->enable_can_transceivers(enable);
 
-    // Switch EPS/GPS
-    if (enable) {
-      current_board->set_gps_mode(GPS_ENABLED);
-    } else {
-      current_board->set_gps_mode(GPS_DISABLED);
-    }
-
     if(current_board->has_hw_gmlan){
       // turn on GMLAN
       set_gpio_output(GPIOB, 14, enable);
diff --git a/panda/board/safety.h b/panda/board/safety.h
index 4d7efd2a0f..a2a3e861d8 100644
--- a/panda/board/safety.h
+++ b/panda/board/safety.h
@@ -174,7 +174,7 @@ void safety_tick(const addr_checks *rx_checks) {
       bool lagging = elapsed_time > MAX(rx_checks->check[i].msg[rx_checks->check[i].index].expected_timestep * MAX_MISSED_MSGS, 1e6);
       rx_checks->check[i].lagging = lagging;
       if (lagging) {
-        controls_allowed = 0;
+        controls_allowed = false;
       }
 
       if (lagging || !is_msg_valid(rx_checks->check, i)) {
@@ -190,7 +190,7 @@ void update_counter(AddrCheckStruct addr_list[], int index, uint8_t counter) {
   if (index != -1) {
     uint8_t expected_counter = (addr_list[index].last_counter + 1U) % (addr_list[index].msg[addr_list[index].index].max_counter + 1U);
     addr_list[index].wrong_counters += (expected_counter == counter) ? -1 : 1;
-    addr_list[index].wrong_counters = MAX(MIN(addr_list[index].wrong_counters, MAX_WRONG_COUNTERS), 0);
+    addr_list[index].wrong_counters = CLAMP(addr_list[index].wrong_counters, 0, MAX_WRONG_COUNTERS);
     addr_list[index].last_counter = counter;
   }
 }
@@ -200,7 +200,7 @@ bool is_msg_valid(AddrCheckStruct addr_list[], int index) {
   if (index != -1) {
     if (!addr_list[index].valid_checksum || !addr_list[index].valid_quality_flag || (addr_list[index].wrong_counters >= MAX_WRONG_COUNTERS)) {
       valid = false;
-      controls_allowed = 0;
+      controls_allowed = false;
     }
   }
   return valid;
@@ -254,19 +254,19 @@ bool addr_safety_check(CANPacket_t *to_push,
 void generic_rx_checks(bool stock_ecu_detected) {
   // exit controls on rising edge of gas press
   if (gas_pressed && !gas_pressed_prev && !(alternative_experience & ALT_EXP_DISABLE_DISENGAGE_ON_GAS)) {
-    controls_allowed = 0;
+    controls_allowed = false;
   }
   gas_pressed_prev = gas_pressed;
 
   // exit controls on rising edge of brake press
   if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
-    controls_allowed = 0;
+    controls_allowed = false;
   }
   brake_pressed_prev = brake_pressed;
 
   // exit controls on rising edge of regen paddle
   if (regen_braking && (!regen_braking_prev || vehicle_moving)) {
-    controls_allowed = 0;
+    controls_allowed = false;
   }
   regen_braking_prev = regen_braking;
 
@@ -344,14 +344,11 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
   valid_steer_req_count = 0;
   invalid_steer_req_count = 0;
 
-  vehicle_speed.min = 0;
-  vehicle_speed.max = 0;
-  torque_meas.min = 0;
-  torque_meas.max = 0;
-  torque_driver.min = 0;
-  torque_driver.max = 0;
-  angle_meas.min = 0;
-  angle_meas.max = 0;
+  // reset samples
+  reset_sample(&vehicle_speed);
+  reset_sample(&torque_meas);
+  reset_sample(&torque_driver);
+  reset_sample(&angle_meas);
 
   controls_allowed = false;
   relay_malfunction_reset();
@@ -389,8 +386,7 @@ int to_signed(int d, int bits) {
 
 // given a new sample, update the sample_t struct
 void update_sample(struct sample_t *sample, int sample_new) {
-  int sample_size = sizeof(sample->values) / sizeof(sample->values[0]);
-  for (int i = sample_size - 1; i > 0; i--) {
+  for (int i = MAX_SAMPLE_VALS - 1; i > 0; i--) {
     sample->values[i] = sample->values[i-1];
   }
   sample->values[0] = sample_new;
@@ -398,7 +394,7 @@ void update_sample(struct sample_t *sample, int sample_new) {
   // get the minimum and maximum measured samples
   sample->min = sample->values[0];
   sample->max = sample->values[0];
-  for (int i = 1; i < sample_size; i++) {
+  for (int i = 1; i < MAX_SAMPLE_VALS; i++) {
     if (sample->values[i] < sample->min) {
       sample->min = sample->values[i];
     }
@@ -408,6 +404,14 @@ void update_sample(struct sample_t *sample, int sample_new) {
   }
 }
 
+// resets values and min/max for sample_t struct
+void reset_sample(struct sample_t *sample) {
+  for (int i = 0; i < MAX_SAMPLE_VALS; i++) {
+    sample->values[i] = 0;
+  }
+  update_sample(sample, 0);
+}
+
 bool max_limit_check(int val, const int MAX_VAL, const int MIN_VAL) {
   return (val > MAX_VAL) || (val < MIN_VAL);
 }
@@ -509,6 +513,12 @@ bool longitudinal_speed_checks(int desired_speed, const LongitudinalLimits limit
   return !get_longitudinal_allowed() && (desired_speed != limits.inactive_speed);
 }
 
+bool longitudinal_transmission_rpm_checks(int desired_transmission_rpm, const LongitudinalLimits limits) {
+  bool transmission_rpm_valid = get_longitudinal_allowed() && !max_limit_check(desired_transmission_rpm, limits.max_transmission_rpm, limits.min_transmission_rpm);
+  bool transmission_rpm_inactive = desired_transmission_rpm == limits.inactive_transmission_rpm;
+  return !(transmission_rpm_valid || transmission_rpm_inactive);
+}
+
 bool longitudinal_gas_checks(int desired_gas, const LongitudinalLimits limits) {
   bool gas_valid = get_longitudinal_allowed() && !max_limit_check(desired_gas, limits.max_gas, limits.min_gas);
   bool gas_inactive = desired_gas == limits.inactive_gas;
diff --git a/panda/board/safety/safety_body.h b/panda/board/safety/safety_body.h
index 61ebc46dc2..30906381e4 100644
--- a/panda/board/safety/safety_body.h
+++ b/panda/board/safety/safety_body.h
@@ -11,7 +11,12 @@ static int body_rx_hook(CANPacket_t *to_push) {
 
   bool valid = addr_safety_check(to_push, &body_rx_checks, NULL, NULL, NULL, NULL);
 
-  controls_allowed = valid;
+  // body is never at standstill
+  vehicle_moving = true;
+
+  if (valid && (GET_ADDR(to_push) == 0x201U)) {
+    controls_allowed = true;
+  }
 
   return valid;
 }
@@ -20,6 +25,7 @@ static int body_tx_hook(CANPacket_t *to_send) {
 
   int tx = 0;
   int addr = GET_ADDR(to_send);
+  int len = GET_LEN(to_send);
 
   // CAN flasher
   if (addr == 0x1) {
@@ -30,8 +36,9 @@ static int body_tx_hook(CANPacket_t *to_send) {
     tx = 1;
   }
 
-  // Allow going into CAN flashing mode even if controls are not allowed
-  if (!controls_allowed && (GET_BYTES(to_send, 0, 4) == 0xdeadfaceU) && (GET_BYTES(to_send, 4, 4) == 0x0ab00b1eU)) {
+  // Allow going into CAN flashing mode for base & knee even if controls are not allowed
+  bool flash_msg = ((addr == 0x250) || (addr == 0x350)) && (len == 8);
+  if (!controls_allowed && (GET_BYTES(to_send, 0, 4) == 0xdeadfaceU) && (GET_BYTES(to_send, 4, 4) == 0x0ab00b1eU) && flash_msg) {
     tx = 1;
   }
 
diff --git a/panda/board/safety/safety_chrysler.h b/panda/board/safety/safety_chrysler.h
index 0caaa2c649..324f73a22d 100644
--- a/panda/board/safety/safety_chrysler.h
+++ b/panda/board/safety/safety_chrysler.h
@@ -41,38 +41,38 @@ typedef struct {
 
 // CAN messages for Chrysler/Jeep platforms
 const ChryslerAddrs CHRYSLER_ADDRS = {
-  .EPS_2            = 544,  // EPS driver input torque
-  .ESP_1            = 320,  // Brake pedal and vehicle speed
-  .ESP_8            = 284,  // Brake pedal and vehicle speed
-  .ECM_5            = 559,  // Throttle position sensor
-  .DAS_3            = 500,  // ACC engagement states from DASM
-  .DAS_6            = 678,  // LKAS HUD and auto headlight control from DASM
-  .LKAS_COMMAND     = 658,  // LKAS controls from DASM
-  .CRUISE_BUTTONS   = 571,  // Cruise control buttons
+  .EPS_2            = 0x220,  // EPS driver input torque
+  .ESP_1            = 0x140,  // Brake pedal and vehicle speed
+  .ESP_8            = 0x11C,  // Brake pedal and vehicle speed
+  .ECM_5            = 0x22F,  // Throttle position sensor
+  .DAS_3            = 0x1F4,  // ACC engagement states from DASM
+  .DAS_6            = 0x2A6,  // LKAS HUD and auto headlight control from DASM
+  .LKAS_COMMAND     = 0x292,  // LKAS controls from DASM
+  .CRUISE_BUTTONS   = 0x23B,  // Cruise control buttons
 };
 
 // CAN messages for the 5th gen RAM DT platform
 const ChryslerAddrs CHRYSLER_RAM_DT_ADDRS = {
-  .EPS_2            = 49,   // EPS driver input torque
-  .ESP_1            = 131,  // Brake pedal and vehicle speed
-  .ESP_8            = 121,  // Brake pedal and vehicle speed
-  .ECM_5            = 157,  // Throttle position sensor
-  .DAS_3            = 153,  // ACC engagement states from DASM
-  .DAS_6            = 250,  // LKAS HUD and auto headlight control from DASM
-  .LKAS_COMMAND     = 166,  // LKAS controls from DASM
-  .CRUISE_BUTTONS   = 177,  // Cruise control buttons
+  .EPS_2            = 0x31,   // EPS driver input torque
+  .ESP_1            = 0x83,   // Brake pedal and vehicle speed
+  .ESP_8            = 0x79,   // Brake pedal and vehicle speed
+  .ECM_5            = 0x9D,   // Throttle position sensor
+  .DAS_3            = 0x99,   // ACC engagement states from DASM
+  .DAS_6            = 0xFA,   // LKAS HUD and auto headlight control from DASM
+  .LKAS_COMMAND     = 0xA6,   // LKAS controls from DASM
+  .CRUISE_BUTTONS   = 0xB1,   // Cruise control buttons
 };
 
 // CAN messages for the 5th gen RAM HD platform
 const ChryslerAddrs CHRYSLER_RAM_HD_ADDRS = {
-  .EPS_2            = 544,  // EPS driver input torque
-  .ESP_1            = 320,  // Brake pedal and vehicle speed
-  .ESP_8            = 284,  // Brake pedal and vehicle speed
-  .ECM_5            = 559,  // Throttle position sensor
-  .DAS_3            = 500,  // ACC engagement states from DASM
-  .DAS_6            = 629,  // LKAS HUD and auto headlight control from DASM
-  .LKAS_COMMAND     = 630,  // LKAS controls from DASM
-  .CRUISE_BUTTONS   = 570,  // Cruise control buttons
+  .EPS_2            = 0x220,  // EPS driver input torque
+  .ESP_1            = 0x140,  // Brake pedal and vehicle speed
+  .ESP_8            = 0x11C,  // Brake pedal and vehicle speed
+  .ECM_5            = 0x22F,  // Throttle position sensor
+  .DAS_3            = 0x1F4,  // ACC engagement states from DASM
+  .DAS_6            = 0x275,  // LKAS HUD and auto headlight control from DASM
+  .LKAS_COMMAND     = 0x276,  // LKAS controls from DASM
+  .CRUISE_BUTTONS   = 0x23A,  // Cruise control buttons
 };
 
 const CanMsg CHRYSLER_TX_MSGS[] = {
@@ -247,7 +247,9 @@ static int chrysler_tx_hook(CANPacket_t *to_send) {
 
     const SteeringLimits limits = (chrysler_platform == CHRYSLER_PACIFICA) ? CHRYSLER_STEERING_LIMITS :
                                   (chrysler_platform == CHRYSLER_RAM_DT) ? CHRYSLER_RAM_DT_STEERING_LIMITS : CHRYSLER_RAM_HD_STEERING_LIMITS;
-    if (steer_torque_cmd_checks(desired_torque, -1, limits)) {
+                                
+    bool steer_req = (chrysler_platform == CHRYSLER_PACIFICA) ? (GET_BIT(to_send, 4U) != 0U) : ((GET_BYTE(to_send, 3) & 0x7U) == 2U);
+    if (steer_torque_cmd_checks(desired_torque, steer_req, limits)) {
       tx = 0;
     }
   }
diff --git a/panda/board/safety/safety_ford.h b/panda/board/safety/safety_ford.h
index 4f71650c90..4a1825f087 100644
--- a/panda/board/safety/safety_ford.h
+++ b/panda/board/safety/safety_ford.h
@@ -77,18 +77,14 @@ addr_checks ford_rx_checks = {ford_addr_checks, FORD_ADDR_CHECK_LEN};
 static uint8_t ford_get_counter(CANPacket_t *to_push) {
   int addr = GET_ADDR(to_push);
 
-  uint8_t cnt;
+  uint8_t cnt = 0;
   if (addr == FORD_BrakeSysFeatures) {
     // Signal: VehVActlBrk_No_Cnt
     cnt = (GET_BYTE(to_push, 2) >> 2) & 0xFU;
-  } else if (addr == FORD_EngVehicleSpThrottle2) {
-    // Signal: VehVActlEng_No_Cnt
-    cnt = (GET_BYTE(to_push, 2) >> 3) & 0xFU;
   } else if (addr == FORD_Yaw_Data_FD1) {
     // Signal: VehRollYaw_No_Cnt
     cnt = GET_BYTE(to_push, 5);
   } else {
-    cnt = 0;
   }
   return cnt;
 }
@@ -96,7 +92,7 @@ static uint8_t ford_get_counter(CANPacket_t *to_push) {
 static uint32_t ford_get_checksum(CANPacket_t *to_push) {
   int addr = GET_ADDR(to_push);
 
-  uint8_t chksum;
+  uint8_t chksum = 0;
   if (addr == FORD_BrakeSysFeatures) {
     // Signal: VehVActlBrk_No_Cs
     chksum = GET_BYTE(to_push, 3);
@@ -107,7 +103,6 @@ static uint32_t ford_get_checksum(CANPacket_t *to_push) {
     // Signal: VehRollYawW_No_Cs
     chksum = GET_BYTE(to_push, 4);
   } else {
-    chksum = 0;
   }
   return chksum;
 }
@@ -237,7 +232,7 @@ static int ford_rx_hook(CANPacket_t *to_push) {
       // Signal: Veh_V_ActlEng
       float filtered_pcm_speed = ((GET_BYTE(to_push, 6) << 8) | GET_BYTE(to_push, 7)) * 0.01 / 3.6;
       if (ABS(filtered_pcm_speed - ((float)vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR)) > FORD_MAX_SPEED_DELTA) {
-        controls_allowed = 0;
+        controls_allowed = false;
       }
     }
 
@@ -270,7 +265,11 @@ static int ford_rx_hook(CANPacket_t *to_push) {
 
     // If steering controls messages are received on the destination bus, it's an indication
     // that the relay might be malfunctioning.
-    generic_rx_checks(ford_lkas_msg_check(addr));
+    bool stock_ecu_detected = ford_lkas_msg_check(addr);
+    if (ford_longitudinal) {
+      stock_ecu_detected = stock_ecu_detected || (addr == FORD_ACCDATA);
+    }
+    generic_rx_checks(stock_ecu_detected);
   }
 
   return valid;
diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h
index 76e55f0bc5..82c154765a 100644
--- a/panda/board/safety/safety_gm.h
+++ b/panda/board/safety/safety_gm.h
@@ -27,27 +27,27 @@ const LongitudinalLimits *gm_long_limits;
 
 const int GM_STANDSTILL_THRSLD = 10;  // 0.311kph
 
-const CanMsg GM_ASCM_TX_MSGS[] = {{384, 0, 4}, {1033, 0, 7}, {1034, 0, 7}, {715, 0, 8}, {880, 0, 6},  // pt bus
-                                  {161, 1, 7}, {774, 1, 8}, {776, 1, 7}, {784, 1, 2},   // obs bus
-                                  {789, 2, 5},  // ch bus
+const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4}, {0x409, 0, 7}, {0x40A, 0, 7}, {0x2CB, 0, 8}, {0x370, 0, 6},  // pt bus
+                                  {0xA1, 1, 7}, {0x306, 1, 8}, {0x308, 1, 7}, {0x310, 1, 2},   // obs bus
+                                  {0x315, 2, 5},  // ch bus
                                   {0x104c006c, 3, 3}, {0x10400060, 3, 5}};  // gmlan
 
-const CanMsg GM_CAM_TX_MSGS[] = {{384, 0, 4},  // pt bus
-                                 {481, 2, 7}, {388, 2, 8}};  // camera bus
+const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4},  // pt bus
+                                 {0x1E1, 2, 7}, {0x184, 2, 8}};  // camera bus
 
-const CanMsg GM_CAM_LONG_TX_MSGS[] = {{384, 0, 4}, {789, 0, 5}, {715, 0, 8}, {880, 0, 6},  // pt bus
-                                      {388, 2, 8}};  // camera bus
+const CanMsg GM_CAM_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x315, 0, 5}, {0x2CB, 0, 8}, {0x370, 0, 6},  // pt bus
+                                      {0x184, 2, 8}};  // camera bus
 
 // TODO: do checksum and counter checks. Add correct timestep, 0.1s for now.
 AddrCheckStruct gm_addr_checks[] = {
-  {.msg = {{388, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}},
-  {.msg = {{842, 0, 5, .expected_timestep = 100000U}, { 0 }, { 0 }}},
-  {.msg = {{481, 0, 7, .expected_timestep = 100000U}, { 0 }, { 0 }}},
-  {.msg = {{190, 0, 6, .expected_timestep = 100000U},    // Volt, Silverado, Acadia Denali
-           {190, 0, 7, .expected_timestep = 100000U},    // Bolt EUV
-           {190, 0, 8, .expected_timestep = 100000U}}},  // Escalade
-  {.msg = {{452, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}},
-  {.msg = {{201, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}},
+  {.msg = {{0x184, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}},
+  {.msg = {{0x34A, 0, 5, .expected_timestep = 100000U}, { 0 }, { 0 }}},
+  {.msg = {{0x1E1, 0, 7, .expected_timestep = 100000U}, { 0 }, { 0 }}},
+  {.msg = {{0xBE, 0, 6, .expected_timestep = 100000U},    // Volt, Silverado, Acadia Denali
+           {0xBE, 0, 7, .expected_timestep = 100000U},    // Bolt EUV
+           {0xBE, 0, 8, .expected_timestep = 100000U}}},  // Escalade
+  {.msg = {{0x1C4, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}},
+  {.msg = {{0xC9, 0, 8, .expected_timestep = 100000U}, { 0 }, { 0 }}},
 };
 #define GM_RX_CHECK_LEN (sizeof(gm_addr_checks) / sizeof(gm_addr_checks[0]))
 addr_checks gm_rx_checks = {gm_addr_checks, GM_RX_CHECK_LEN};
@@ -73,7 +73,7 @@ static int gm_rx_hook(CANPacket_t *to_push) {
   if (valid && (GET_BUS(to_push) == 0U)) {
     int addr = GET_ADDR(to_push);
 
-    if (addr == 388) {
+    if (addr == 0x184) {
       int torque_driver_new = ((GET_BYTE(to_push, 6) & 0x7U) << 8) | GET_BYTE(to_push, 7);
       torque_driver_new = to_signed(torque_driver_new, 11);
       // update array of samples
@@ -81,26 +81,26 @@ static int gm_rx_hook(CANPacket_t *to_push) {
     }
 
     // sample rear wheel speeds
-    if (addr == 842) {
+    if (addr == 0x34A) {
       int left_rear_speed = (GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1);
       int right_rear_speed = (GET_BYTE(to_push, 2) << 8) | GET_BYTE(to_push, 3);
       vehicle_moving = (left_rear_speed > GM_STANDSTILL_THRSLD) || (right_rear_speed > GM_STANDSTILL_THRSLD);
     }
 
     // ACC steering wheel buttons (GM_CAM is tied to the PCM)
-    if ((addr == 481) && !gm_pcm_cruise) {
+    if ((addr == 0x1E1) && !gm_pcm_cruise) {
       int button = (GET_BYTE(to_push, 5) & 0x70U) >> 4;
 
       // enter controls on falling edge of set or rising edge of resume (avoids fault)
       bool set = (button != GM_BTN_SET) && (cruise_button_prev == GM_BTN_SET);
       bool res = (button == GM_BTN_RESUME) && (cruise_button_prev != GM_BTN_RESUME);
       if (set || res) {
-        controls_allowed = 1;
+        controls_allowed = true;
       }
 
       // exit controls on cancel press
       if (button == GM_BTN_CANCEL) {
-        controls_allowed = 0;
+        controls_allowed = false;
       }
 
       cruise_button_prev = button;
@@ -108,15 +108,15 @@ static int gm_rx_hook(CANPacket_t *to_push) {
 
     // Reference for brake pressed signals:
     // https://github.com/commaai/openpilot/blob/master/selfdrive/car/gm/carstate.py
-    if ((addr == 190) && (gm_hw == GM_ASCM)) {
+    if ((addr == 0xBE) && (gm_hw == GM_ASCM)) {
       brake_pressed = GET_BYTE(to_push, 1) >= 8U;
     }
 
-    if ((addr == 201) && (gm_hw == GM_CAM)) {
+    if ((addr == 0xC9) && (gm_hw == GM_CAM)) {
       brake_pressed = GET_BIT(to_push, 40U) != 0U;
     }
 
-    if (addr == 452) {
+    if (addr == 0x1C4) {
       gas_pressed = GET_BYTE(to_push, 5) != 0U;
 
       // enter controls on rising edge of ACC, exit controls when ACC off
@@ -126,14 +126,14 @@ static int gm_rx_hook(CANPacket_t *to_push) {
       }
     }
 
-    if (addr == 189) {
+    if (addr == 0xBD) {
       regen_braking = (GET_BYTE(to_push, 0) >> 4) != 0U;
     }
 
-    bool stock_ecu_detected = (addr == 384);  // ASCMLKASteeringCmd
+    bool stock_ecu_detected = (addr == 0x180);  // ASCMLKASteeringCmd
 
     // Check ASCMGasRegenCmd only if we're blocking it
-    if (!gm_pcm_cruise && (addr == 715)) {
+    if (!gm_pcm_cruise && (addr == 0x2CB)) {
       stock_ecu_detected = true;
     }
     generic_rx_checks(stock_ecu_detected);
@@ -163,7 +163,7 @@ static int gm_tx_hook(CANPacket_t *to_send) {
   }
 
   // BRAKE: safety check
-  if (addr == 789) {
+  if (addr == 0x315) {
     int brake = ((GET_BYTE(to_send, 0) & 0xFU) << 8) + GET_BYTE(to_send, 1);
     brake = (0x1000 - brake) & 0xFFF;
     if (longitudinal_brake_checks(brake, *gm_long_limits)) {
@@ -172,17 +172,19 @@ static int gm_tx_hook(CANPacket_t *to_send) {
   }
 
   // LKA STEER: safety check
-  if (addr == 384) {
+  if (addr == 0x180) {
     int desired_torque = ((GET_BYTE(to_send, 0) & 0x7U) << 8) + GET_BYTE(to_send, 1);
     desired_torque = to_signed(desired_torque, 11);
 
-    if (steer_torque_cmd_checks(desired_torque, -1, GM_STEERING_LIMITS)) {
+    bool steer_req = (GET_BIT(to_send, 3U) != 0U);
+
+    if (steer_torque_cmd_checks(desired_torque, steer_req, GM_STEERING_LIMITS)) {
       tx = 0;
     }
   }
 
   // GAS/REGEN: safety check
-  if (addr == 715) {
+  if (addr == 0x2CB) {
     bool apply = GET_BIT(to_send, 0U) != 0U;
     int gas_regen = ((GET_BYTE(to_send, 2) & 0x7FU) << 5) + ((GET_BYTE(to_send, 3) & 0xF8U) >> 3);
 
@@ -197,7 +199,7 @@ static int gm_tx_hook(CANPacket_t *to_send) {
   }
 
   // BUTTONS: used for resume spamming and cruise cancellation with stock longitudinal
-  if ((addr == 481) && gm_pcm_cruise) {
+  if ((addr == 0x1E1) && gm_pcm_cruise) {
     int button = (GET_BYTE(to_send, 5) >> 4) & 0x7U;
 
     bool allowed_cancel = (button == 6) && cruise_engaged_prev;
@@ -217,7 +219,7 @@ static int gm_fwd_hook(int bus_num, int addr) {
   if (gm_hw == GM_CAM) {
     if (bus_num == 0) {
       // block PSCMStatus; forwarded through openpilot to hide an alert from the camera
-      bool is_pscm_msg = (addr == 388);
+      bool is_pscm_msg = (addr == 0x184);
       if (!is_pscm_msg) {
         bus_fwd = 2;
       }
@@ -225,8 +227,8 @@ static int gm_fwd_hook(int bus_num, int addr) {
 
     if (bus_num == 2) {
       // block lkas message and acc messages if gm_cam_long, forward all others
-      bool is_lkas_msg = (addr == 384);
-      bool is_acc_msg = (addr == 789) || (addr == 715) || (addr == 880);
+      bool is_lkas_msg = (addr == 0x180);
+      bool is_acc_msg = (addr == 0x315) || (addr == 0x2CB) || (addr == 0x370);
       int block_msg = is_lkas_msg || (is_acc_msg && gm_cam_long);
       if (!block_msg) {
         bus_fwd = 0;
diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h
index e8100cad72..415a379076 100644
--- a/panda/board/safety/safety_honda.h
+++ b/panda/board/safety/safety_honda.h
@@ -142,7 +142,7 @@ static int honda_rx_hook(CANPacket_t *to_push) {
     if ((addr == 0x326) || (addr == 0x1A6)) {
       acc_main_on = GET_BIT(to_push, ((addr == 0x326) ? 28U : 47U));
       if (!acc_main_on) {
-        controls_allowed = 0;
+        controls_allowed = false;
       }
     }
 
@@ -151,13 +151,13 @@ static int honda_rx_hook(CANPacket_t *to_push) {
       const bool cruise_engaged = GET_BIT(to_push, 38U) != 0U;
       // engage on rising edge
       if (cruise_engaged && !cruise_engaged_prev) {
-        controls_allowed = 1;
+        controls_allowed = true;
       }
 
       // Since some Nidec cars can brake down to 0 after the PCM disengages,
       // we don't disengage when the PCM does.
       if (!cruise_engaged && (honda_hw != HONDA_NIDEC)) {
-        controls_allowed = 0;
+        controls_allowed = false;
       }
       cruise_engaged_prev = cruise_engaged;
     }
@@ -167,16 +167,16 @@ static int honda_rx_hook(CANPacket_t *to_push) {
     if (((addr == 0x1A6) || (addr == 0x296)) && (bus == pt_bus)) {
       int button = (GET_BYTE(to_push, 0) & 0xE0U) >> 5;
 
-      // exit controls once main or cancel are pressed
-      if ((button == HONDA_BTN_MAIN) || (button == HONDA_BTN_CANCEL)) {
-        controls_allowed = 0;
-      }
-
       // enter controls on the falling edge of set or resume
-      bool set = (button == HONDA_BTN_NONE) && (cruise_button_prev == HONDA_BTN_SET);
-      bool res = (button == HONDA_BTN_NONE) && (cruise_button_prev == HONDA_BTN_RESUME);
+      bool set = (button != HONDA_BTN_SET) && (cruise_button_prev == HONDA_BTN_SET);
+      bool res = (button != HONDA_BTN_RESUME) && (cruise_button_prev == HONDA_BTN_RESUME);
       if (acc_main_on && !pcm_cruise && (set || res)) {
-        controls_allowed = 1;
+        controls_allowed = true;
+      }
+
+      // exit controls once main or cancel are pressed
+      if ((button == HONDA_BTN_MAIN) || (button == HONDA_BTN_CANCEL)) {
+        controls_allowed = false;
       }
       cruise_button_prev = button;
     }
@@ -217,8 +217,8 @@ static int honda_rx_hook(CANPacket_t *to_push) {
     // disable stock Honda AEB in alternative experience
     if (!(alternative_experience & ALT_EXP_DISABLE_STOCK_AEB)) {
       if ((bus == 2) && (addr == 0x1FA)) {
-        bool honda_stock_aeb = GET_BYTE(to_push, 3) & 0x20U;
-        int honda_stock_brake = (GET_BYTE(to_push, 0) << 2) + ((GET_BYTE(to_push, 1) >> 6) & 0x3U);
+        bool honda_stock_aeb = GET_BIT(to_push, 29U) != 0U;
+        int honda_stock_brake = (GET_BYTE(to_push, 0) << 2) | (GET_BYTE(to_push, 1) >> 6);
 
         // Forward AEB when stock braking is higher than openpilot braking
         // only stop forwarding when AEB event is over
@@ -235,20 +235,18 @@ static int honda_rx_hook(CANPacket_t *to_push) {
     int bus_rdr_car = (honda_hw == HONDA_BOSCH) ? 0 : 2;  // radar bus, car side
     bool stock_ecu_detected = false;
 
-    if (safety_mode_cnt > RELAY_TRNS_TIMEOUT) {
-      // If steering controls messages are received on the destination bus, it's an indication
-      // that the relay might be malfunctioning
-      if ((addr == 0xE4) || (addr == 0x194)) {
-        if (((honda_hw != HONDA_NIDEC) && (bus == bus_rdr_car)) || ((honda_hw == HONDA_NIDEC) && (bus == 0))) {
-          stock_ecu_detected = true;
-        }
-      }
-      // If Honda Bosch longitudinal mode is selected we need to ensure the radar is turned off
-      // Verify this by ensuring ACC_CONTROL (0x1DF) is not received on the PT bus
-      if (honda_bosch_long && !honda_bosch_radarless && (bus == pt_bus) && (addr == 0x1DF)) {
+    // If steering controls messages are received on the destination bus, it's an indication
+    // that the relay might be malfunctioning
+    if ((addr == 0xE4) || (addr == 0x194)) {
+      if (((honda_hw != HONDA_NIDEC) && (bus == bus_rdr_car)) || ((honda_hw == HONDA_NIDEC) && (bus == 0))) {
         stock_ecu_detected = true;
       }
     }
+    // If Honda Bosch longitudinal mode is selected we need to ensure the radar is turned off
+    // Verify this by ensuring ACC_CONTROL (0x1DF) is not received on the PT bus
+    if (honda_bosch_long && !honda_bosch_radarless && (bus == pt_bus) && (addr == 0x1DF)) {
+      stock_ecu_detected = true;
+    }
 
     generic_rx_checks(stock_ecu_detected);
   }
@@ -379,8 +377,9 @@ static int honda_tx_hook(CANPacket_t *to_send) {
 }
 
 static const addr_checks* honda_nidec_init(uint16_t param) {
-  gas_interceptor_detected = 0;
   honda_hw = HONDA_NIDEC;
+  honda_brake = 0;
+  honda_fwd_brake = false;
   honda_alt_brake_msg = false;
   honda_bosch_long = false;
   honda_bosch_radarless = false;
diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h
index 8ba252a1b1..29a42c7a01 100644
--- a/panda/board/safety/safety_hyundai.h
+++ b/panda/board/safety/safety_hyundai.h
@@ -26,88 +26,81 @@ const LongitudinalLimits HYUNDAI_LONG_LIMITS = {
 };
 
 const CanMsg HYUNDAI_TX_MSGS[] = {
-  {832, 0, 8},  // LKAS11 Bus 0
-  {1265, 0, 4}, // CLU11 Bus 0
-  {1157, 0, 4}, // LFAHDA_MFC Bus 0
+  {0x340, 0, 8}, // LKAS11 Bus 0
+  {0x4F1, 0, 4}, // CLU11 Bus 0
+  {0x485, 0, 4}, // LFAHDA_MFC Bus 0
 };
 
 const CanMsg HYUNDAI_LONG_TX_MSGS[] = {
-  {832, 0, 8},  // LKAS11 Bus 0
-  {1265, 0, 4}, // CLU11 Bus 0
-  {1157, 0, 4}, // LFAHDA_MFC Bus 0
-  {1056, 0, 8}, // SCC11 Bus 0
-  {1057, 0, 8}, // SCC12 Bus 0
-  {1290, 0, 8}, // SCC13 Bus 0
-  {905, 0, 8},  // SCC14 Bus 0
-  {1186, 0, 2}, // FRT_RADAR11 Bus 0
-  {909, 0, 8},  // FCA11 Bus 0
-  {1155, 0, 8}, // FCA12 Bus 0
-  {2000, 0, 8}, // radar UDS TX addr Bus 0 (for radar disable)
+  {0x340, 0, 8}, // LKAS11 Bus 0
+  {0x4F1, 0, 4}, // CLU11 Bus 0
+  {0x485, 0, 4}, // LFAHDA_MFC Bus 0
+  {0x420, 0, 8}, // SCC11 Bus 0
+  {0x421, 0, 8}, // SCC12 Bus 0
+  {0x50A, 0, 8}, // SCC13 Bus 0
+  {0x389, 0, 8}, // SCC14 Bus 0
+  {0x4A2, 0, 2}, // FRT_RADAR11 Bus 0
+  {0x38D, 0, 8}, // FCA11 Bus 0
+  {0x483, 0, 8}, // FCA12 Bus 0
+  {0x7D0, 0, 8}, // radar UDS TX addr Bus 0 (for radar disable)
 };
 
 const CanMsg HYUNDAI_CAMERA_SCC_TX_MSGS[] = {
-  {832, 0, 8},  // LKAS11 Bus 0
-  {1265, 2, 4}, // CLU11 Bus 2
-  {1157, 0, 4}, // LFAHDA_MFC Bus 0
+  {0x340, 0, 8}, // LKAS11 Bus 0
+  {0x4F1, 2, 4}, // CLU11 Bus 2
+  {0x485, 0, 4}, // LFAHDA_MFC Bus 0
 };
 
+#define HYUNDAI_COMMON_ADDR_CHECKS(legacy)                                                                                              \
+  {.msg = {{0x260, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U},                                       \
+           {0x371, 0, 8, .expected_timestep = 10000U}, { 0 }}},                                                                         \
+  {.msg = {{0x386, 0, 8, .check_checksum = !(legacy), .max_counter = (legacy) ? 0U : 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}}, \
+  {.msg = {{0x394, 0, 8, .check_checksum = !(legacy), .max_counter = (legacy) ? 0U : 7U, .expected_timestep = 10000U}, { 0 }, { 0 }}},  \
+
+#define HYUNDAI_SCC12_ADDR_CHECK(scc_bus)                                                                                  \
+  {.msg = {{0x421, (scc_bus), 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}}, \
+
 AddrCheckStruct hyundai_addr_checks[] = {
-  {.msg = {{608, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U},
-           {881, 0, 8, .expected_timestep = 10000U}, { 0 }}},
-  {.msg = {{902, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
-  {.msg = {{916, 0, 8, .check_checksum = true, .max_counter = 7U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
-  {.msg = {{1057, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
+   HYUNDAI_COMMON_ADDR_CHECKS(false)
+   HYUNDAI_SCC12_ADDR_CHECK(0)
 };
-#define HYUNDAI_ADDR_CHECK_LEN (sizeof(hyundai_addr_checks) / sizeof(hyundai_addr_checks[0]))
 
 AddrCheckStruct hyundai_cam_scc_addr_checks[] = {
-  {.msg = {{608, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U},
-           {881, 0, 8, .expected_timestep = 10000U}, { 0 }}},
-  {.msg = {{902, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
-  {.msg = {{916, 0, 8, .check_checksum = true, .max_counter = 7U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
-  {.msg = {{1057, 2, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
+  HYUNDAI_COMMON_ADDR_CHECKS(false)
+  HYUNDAI_SCC12_ADDR_CHECK(2)
 };
-#define HYUNDAI_CAM_SCC_ADDR_CHECK_LEN (sizeof(hyundai_cam_scc_addr_checks) / sizeof(hyundai_cam_scc_addr_checks[0]))
 
 AddrCheckStruct hyundai_long_addr_checks[] = {
-  {.msg = {{608, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U},
-           {881, 0, 8, .expected_timestep = 10000U}, { 0 }}},
-  {.msg = {{902, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
-  {.msg = {{916, 0, 8, .check_checksum = true, .max_counter = 7U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
-  {.msg = {{1265, 0, 4, .check_checksum = false, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
+  HYUNDAI_COMMON_ADDR_CHECKS(false)
+  // Use CLU11 (buttons) to manage controls allowed instead of SCC cruise state
+  {.msg = {{0x4F1, 0, 4, .check_checksum = false, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
 };
-#define HYUNDAI_LONG_ADDR_CHECK_LEN (sizeof(hyundai_long_addr_checks) / sizeof(hyundai_long_addr_checks[0]))
 
 // older hyundai models have less checks due to missing counters and checksums
 AddrCheckStruct hyundai_legacy_addr_checks[] = {
-  {.msg = {{608, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U},
-           {881, 0, 8, .expected_timestep = 10000U}, { 0 }}},
-  {.msg = {{902, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}},
-  {.msg = {{916, 0, 8, .expected_timestep = 10000U}, { 0 }, { 0 }}},
-  {.msg = {{1057, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
+  HYUNDAI_COMMON_ADDR_CHECKS(true)
+  HYUNDAI_SCC12_ADDR_CHECK(0)
 };
-#define HYUNDAI_LEGACY_ADDR_CHECK_LEN (sizeof(hyundai_legacy_addr_checks) / sizeof(hyundai_legacy_addr_checks[0]))
 
 bool hyundai_legacy = false;
 
-addr_checks hyundai_rx_checks = {hyundai_addr_checks, HYUNDAI_ADDR_CHECK_LEN};
+addr_checks hyundai_rx_checks = SET_ADDR_CHECKS(hyundai_addr_checks);
 
 static uint8_t hyundai_get_counter(CANPacket_t *to_push) {
   int addr = GET_ADDR(to_push);
 
-  uint8_t cnt;
-  if (addr == 608) {
+  uint8_t cnt = 0;
+  if (addr == 0x260) {
     cnt = (GET_BYTE(to_push, 7) >> 4) & 0x3U;
-  } else if (addr == 902) {
+  } else if (addr == 0x386) {
     cnt = ((GET_BYTE(to_push, 3) >> 6) << 2) | (GET_BYTE(to_push, 1) >> 6);
-  } else if (addr == 916) {
+  } else if (addr == 0x394) {
     cnt = (GET_BYTE(to_push, 1) >> 5) & 0x7U;
-  } else if (addr == 1057) {
+  } else if (addr == 0x421) {
     cnt = GET_BYTE(to_push, 7) & 0xFU;
-  } else if (addr == 1265) {
+  } else if (addr == 0x4F1) {
     cnt = (GET_BYTE(to_push, 3) >> 4) & 0xFU;
   } else {
-    cnt = 0;
   }
   return cnt;
 }
@@ -115,17 +108,16 @@ static uint8_t hyundai_get_counter(CANPacket_t *to_push) {
 static uint32_t hyundai_get_checksum(CANPacket_t *to_push) {
   int addr = GET_ADDR(to_push);
 
-  uint8_t chksum;
-  if (addr == 608) {
+  uint8_t chksum = 0;
+  if (addr == 0x260) {
     chksum = GET_BYTE(to_push, 7) & 0xFU;
-  } else if (addr == 902) {
+  } else if (addr == 0x386) {
     chksum = ((GET_BYTE(to_push, 7) >> 6) << 2) | (GET_BYTE(to_push, 5) >> 6);
-  } else if (addr == 916) {
+  } else if (addr == 0x394) {
     chksum = GET_BYTE(to_push, 6) & 0xFU;
-  } else if (addr == 1057) {
+  } else if (addr == 0x421) {
     chksum = GET_BYTE(to_push, 7) >> 4;
   } else {
-    chksum = 0;
   }
   return chksum;
 }
@@ -134,7 +126,7 @@ static uint32_t hyundai_compute_checksum(CANPacket_t *to_push) {
   int addr = GET_ADDR(to_push);
 
   uint8_t chksum = 0;
-  if (addr == 902) {
+  if (addr == 0x386) {
     // count the bits
     for (int i = 0; i < 8; i++) {
       uint8_t b = GET_BYTE(to_push, i);
@@ -151,12 +143,12 @@ static uint32_t hyundai_compute_checksum(CANPacket_t *to_push) {
   } else {
     // sum of nibbles
     for (int i = 0; i < 8; i++) {
-      if ((addr == 916) && (i == 7)) {
+      if ((addr == 0x394) && (i == 7)) {
         continue; // exclude
       }
       uint8_t b = GET_BYTE(to_push, i);
-      if (((addr == 608) && (i == 7)) || ((addr == 916) && (i == 6)) || ((addr == 1057) && (i == 7))) {
-        b &= (addr == 1057) ? 0x0FU : 0xF0U; // remove checksum
+      if (((addr == 0x260) && (i == 7)) || ((addr == 0x394) && (i == 6)) || ((addr == 0x421) && (i == 7))) {
+        b &= (addr == 0x421) ? 0x0FU : 0xF0U; // remove checksum
       }
       chksum += (b % 16U) + (b / 16U);
     }
@@ -176,52 +168,52 @@ static int hyundai_rx_hook(CANPacket_t *to_push) {
   int addr = GET_ADDR(to_push);
 
   // SCC12 is on bus 2 for camera-based SCC cars, bus 0 on all others
-  if (valid && (addr == 1057) && (((bus == 0) && !hyundai_camera_scc) || ((bus == 2) && hyundai_camera_scc))) {
+  if (valid && (addr == 0x421) && (((bus == 0) && !hyundai_camera_scc) || ((bus == 2) && hyundai_camera_scc))) {
     // 2 bits: 13-14
     int cruise_engaged = (GET_BYTES(to_push, 0, 4) >> 13) & 0x3U;
     hyundai_common_cruise_state_check(cruise_engaged);
   }
 
   if (valid && (bus == 0)) {
-    if (addr == 593) {
-      int torque_driver_new = ((GET_BYTES(to_push, 0, 4) & 0x7ffU) * 0.79) - 808; // scale down new driver torque signal to match previous one
+    if (addr == 0x251) {
+      int torque_driver_new = (GET_BYTES(to_push, 0, 2) & 0x7ffU) - 1024U;
       // update array of samples
       update_sample(&torque_driver, torque_driver_new);
     }
 
     // ACC steering wheel buttons
-    if (addr == 1265) {
+    if (addr == 0x4F1) {
       int cruise_button = GET_BYTE(to_push, 0) & 0x7U;
       int main_button = GET_BIT(to_push, 3U);
       hyundai_common_cruise_buttons_check(cruise_button, main_button);
     }
 
     // gas press, different for EV, hybrid, and ICE models
-    if ((addr == 881) && hyundai_ev_gas_signal) {
+    if ((addr == 0x371) && hyundai_ev_gas_signal) {
       gas_pressed = (((GET_BYTE(to_push, 4) & 0x7FU) << 1) | GET_BYTE(to_push, 3) >> 7) != 0U;
-    } else if ((addr == 881) && hyundai_hybrid_gas_signal) {
+    } else if ((addr == 0x371) && hyundai_hybrid_gas_signal) {
       gas_pressed = GET_BYTE(to_push, 7) != 0U;
-    } else if ((addr == 608) && !hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) {
+    } else if ((addr == 0x260) && !hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) {
       gas_pressed = (GET_BYTE(to_push, 7) >> 6) != 0U;
     } else {
     }
 
     // sample wheel speed, averaging opposite corners
-    if (addr == 902) {
-      uint32_t hyundai_speed = (GET_BYTES(to_push, 0, 4) & 0x3FFFU) + ((GET_BYTES(to_push, 4, 4) >> 16) & 0x3FFFU);  // FL + RR
-      hyundai_speed /= 2;
-      vehicle_moving = hyundai_speed > HYUNDAI_STANDSTILL_THRSLD;
+    if (addr == 0x386) {
+      uint32_t front_left_speed = GET_BYTES(to_push, 0, 2) & 0x3FFFU;
+      uint32_t rear_right_speed = GET_BYTES(to_push, 6, 2) & 0x3FFFU;
+      vehicle_moving = (front_left_speed > HYUNDAI_STANDSTILL_THRSLD) || (rear_right_speed > HYUNDAI_STANDSTILL_THRSLD);
     }
 
-    if (addr == 916) {
-      brake_pressed = GET_BIT(to_push, 55U) != 0U;
+    if (addr == 0x394) {
+      brake_pressed = ((GET_BYTE(to_push, 5) >> 5U) & 0x2U) == 0x2U;
     }
 
-    bool stock_ecu_detected = (addr == 832);
+    bool stock_ecu_detected = (addr == 0x340);
 
     // If openpilot is controlling longitudinal we need to ensure the radar is turned off
     // Enforce by checking we don't see SCC12
-    if (hyundai_longitudinal && (addr == 1057)) {
+    if (hyundai_longitudinal && (addr == 0x421)) {
       stock_ecu_detected = true;
     }
     generic_rx_checks(stock_ecu_detected);
@@ -243,7 +235,7 @@ static int hyundai_tx_hook(CANPacket_t *to_send) {
   }
 
   // FCA11: Block any potential actuation
-  if (addr == 909) {
+  if (addr == 0x38D) {
     int CR_VSM_DecCmd = GET_BYTE(to_send, 1);
     int FCA_CmdAct = GET_BIT(to_send, 20U);
     int CF_VSM_DecCmdAct = GET_BIT(to_send, 31U);
@@ -254,7 +246,7 @@ static int hyundai_tx_hook(CANPacket_t *to_send) {
   }
 
   // ACCEL: safety check
-  if (addr == 1057) {
+  if (addr == 0x421) {
     int desired_accel_raw = (((GET_BYTE(to_send, 4) & 0x7U) << 8) | GET_BYTE(to_send, 3)) - 1023U;
     int desired_accel_val = ((GET_BYTE(to_send, 5) << 3) | (GET_BYTE(to_send, 4) >> 5)) - 1023U;
 
@@ -274,7 +266,7 @@ static int hyundai_tx_hook(CANPacket_t *to_send) {
   }
 
   // LKA STEER: safety check
-  if (addr == 832) {
+  if (addr == 0x340) {
     int desired_torque = ((GET_BYTES(to_send, 0, 4) >> 16) & 0x7ffU) - 1024U;
     bool steer_req = GET_BIT(to_send, 27U) != 0U;
 
@@ -285,14 +277,14 @@ static int hyundai_tx_hook(CANPacket_t *to_send) {
   }
 
   // UDS: Only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address
-  if (addr == 2000) {
+  if (addr == 0x7D0) {
     if ((GET_BYTES(to_send, 0, 4) != 0x00803E02U) || (GET_BYTES(to_send, 4, 4) != 0x0U)) {
       tx = 0;
     }
   }
 
   // BUTTONS: used for resume spamming and cruise cancellation
-  if ((addr == 1265) && !hyundai_longitudinal) {
+  if ((addr == 0x4F1) && !hyundai_longitudinal) {
     int button = GET_BYTE(to_send, 0) & 0x7U;
 
     bool allowed_resume = (button == 1) && controls_allowed;
@@ -313,7 +305,7 @@ static int hyundai_fwd_hook(int bus_num, int addr) {
   if (bus_num == 0) {
     bus_fwd = 2;
   }
-  if ((bus_num == 2) && (addr != 832) && (addr != 1157)) {
+  if ((bus_num == 2) && (addr != 0x340) && (addr != 0x485)) {
     bus_fwd = 0;
   }
 
@@ -329,11 +321,11 @@ static const addr_checks* hyundai_init(uint16_t param) {
   }
 
   if (hyundai_longitudinal) {
-    hyundai_rx_checks = (addr_checks){hyundai_long_addr_checks, HYUNDAI_LONG_ADDR_CHECK_LEN};
+    hyundai_rx_checks = SET_ADDR_CHECKS(hyundai_long_addr_checks);
   } else if (hyundai_camera_scc) {
-    hyundai_rx_checks = (addr_checks){hyundai_cam_scc_addr_checks, HYUNDAI_CAM_SCC_ADDR_CHECK_LEN};
+    hyundai_rx_checks = SET_ADDR_CHECKS(hyundai_cam_scc_addr_checks);
   } else {
-    hyundai_rx_checks = (addr_checks){hyundai_addr_checks, HYUNDAI_ADDR_CHECK_LEN};
+    hyundai_rx_checks = SET_ADDR_CHECKS(hyundai_addr_checks);
   }
   return &hyundai_rx_checks;
 }
@@ -344,7 +336,7 @@ static const addr_checks* hyundai_legacy_init(uint16_t param) {
   hyundai_longitudinal = false;
   hyundai_camera_scc = false;
 
-  hyundai_rx_checks = (addr_checks){hyundai_legacy_addr_checks, HYUNDAI_LEGACY_ADDR_CHECK_LEN};
+  hyundai_rx_checks = SET_ADDR_CHECKS(hyundai_legacy_addr_checks);
   return &hyundai_rx_checks;
 }
 
diff --git a/panda/board/safety/safety_hyundai_canfd.h b/panda/board/safety/safety_hyundai_canfd.h
index 2dc6e9bbff..f0af9060eb 100644
--- a/panda/board/safety/safety_hyundai_canfd.h
+++ b/panda/board/safety/safety_hyundai_canfd.h
@@ -24,6 +24,12 @@ const CanMsg HYUNDAI_CANFD_HDA2_TX_MSGS[] = {
   {0x2A4, 0, 24}, // CAM_0x2A4
 };
 
+const CanMsg HYUNDAI_CANFD_HDA2_ALT_STEERING_TX_MSGS[] = {
+  {0x110, 0, 32}, // LKAS_ALT
+  {0x1CF, 1, 8},  // CRUISE_BUTTON
+  {0x362, 0, 32}, // CAM_0x362
+};
+
 const CanMsg HYUNDAI_CANFD_HDA2_LONG_TX_MSGS[] = {
   {0x50, 0, 16},  // LKAS
   {0x1CF, 1, 8},  // CRUISE_BUTTON
@@ -47,79 +53,91 @@ const CanMsg HYUNDAI_CANFD_HDA1_TX_MSGS[] = {
   {0x1E0, 0, 16}, // LFAHDA_CLUSTER
 };
 
+
+// *** Addresses checked in rx hook ***
+// EV, ICE, HYBRID: ACCELERATOR (0x35), ACCELERATOR_BRAKE_ALT (0x100), ACCELERATOR_ALT (0x105)
+#define HYUNDAI_CANFD_COMMON_ADDR_CHECKS(pt_bus)                                                                              \
+  {.msg = {{0x35, (pt_bus), 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U},                   \
+           {0x100, (pt_bus), 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U},                  \
+           {0x105, (pt_bus), 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}}},                \
+  {.msg = {{0x175, (pt_bus), 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U}, { 0 }, { 0 }}},  \
+  {.msg = {{0xa0, (pt_bus), 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }, { 0 }}},   \
+  {.msg = {{0xea, (pt_bus), 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }, { 0 }}},   \
+
+#define HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(pt_bus)                                                                            \
+  {.msg = {{0x1cf, (pt_bus), 8, .check_checksum = false, .max_counter = 0xfU, .expected_timestep = 20000U}, { 0 }, { 0 }}}, \
+
+#define HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(pt_bus)                                                                            \
+  {.msg = {{0x1aa, (pt_bus), 16, .check_checksum = false, .max_counter = 0xffU, .expected_timestep = 20000U}, { 0 }, { 0 }}},   \
+
+// SCC_CONTROL (from ADAS unit or camera)
+#define HYUNDAI_CANFD_SCC_ADDR_CHECK(scc_bus)                                                                                 \
+  {.msg = {{0x1a0, (scc_bus), 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U}, { 0 }, { 0 }}}, \
+
+
+// *** Non-HDA2 checks ***
+// Camera sends SCC messages on HDA1.
+// Both button messages exist on some platforms, so we ensure we track the correct one using flag
 AddrCheckStruct hyundai_canfd_addr_checks[] = {
-  {.msg = {{0x35, 1, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U},
-           {0x35, 0, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U},
-           {0x105, 0, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}}},
-  {.msg = {{0x175, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U},
-           {0x175, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U}, { 0 }}},
-  {.msg = {{0xa0, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U},
-           {0xa0, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }}},
-  {.msg = {{0xea, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U},
-           {0xea, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }}},
-  {.msg = {{0x1a0, 1, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U},
-           {0x1a0, 2, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U}, { 0 }}},
-  {.msg = {{0x1cf, 1, 8, .check_checksum = false, .max_counter = 0xfU, .expected_timestep = 20000U},
-           {0x1cf, 0, 8, .check_checksum = false, .max_counter = 0xfU, .expected_timestep = 20000U},
-           {0x1aa, 0, 16, .check_checksum = false, .max_counter = 0xffU, .expected_timestep = 20000U}}},
+  HYUNDAI_CANFD_COMMON_ADDR_CHECKS(0)
+  HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(0)
+  HYUNDAI_CANFD_SCC_ADDR_CHECK(2)
 };
-#define HYUNDAI_CANFD_ADDR_CHECK_LEN (sizeof(hyundai_canfd_addr_checks) / sizeof(hyundai_canfd_addr_checks[0]))
-
-// 0x1a0 is on bus 0
-AddrCheckStruct hyundai_canfd_radar_scc_addr_checks[] = {
-  {.msg = {{0x35, 1, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U},
-           {0x35, 0, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U},
-           {0x105, 0, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}}},
-  {.msg = {{0x175, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U},
-           {0x175, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U}, { 0 }}},
-  {.msg = {{0xa0, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U},
-           {0xa0, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }}},
-  {.msg = {{0xea, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U},
-           {0xea, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }}},
-  {.msg = {{0x1a0, 0, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U}, { 0 }, { 0 }}},
-  {.msg = {{0x1cf, 1, 8, .check_checksum = false, .max_counter = 0xfU, .expected_timestep = 20000U},
-           {0x1cf, 0, 8, .check_checksum = false, .max_counter = 0xfU, .expected_timestep = 20000U},
-           {0x1aa, 0, 16, .check_checksum = false, .max_counter = 0xffU, .expected_timestep = 20000U}}},
+AddrCheckStruct hyundai_canfd_alt_buttons_addr_checks[] = {
+  HYUNDAI_CANFD_COMMON_ADDR_CHECKS(0)
+  HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(0)
+  HYUNDAI_CANFD_SCC_ADDR_CHECK(2)
 };
-#define HYUNDAI_CANFD_RADAR_SCC_ADDR_CHECK_LEN (sizeof(hyundai_canfd_radar_scc_addr_checks) / sizeof(hyundai_canfd_radar_scc_addr_checks[0]))
 
+// Longitudinal checks for HDA1
 AddrCheckStruct hyundai_canfd_long_addr_checks[] = {
-  {.msg = {{0x35, 1, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U},
-           {0x35, 0, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U},
-           {0x105, 0, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}}},
-  {.msg = {{0x175, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U},
-           {0x175, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U}, { 0 }}},
-  {.msg = {{0xa0, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U},
-           {0xa0, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }}},
-  {.msg = {{0xea, 1, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U},
-           {0xea, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }}},
-  {.msg = {{0x1cf, 1, 8, .check_checksum = false, .max_counter = 0xfU, .expected_timestep = 20000U},
-           {0x1cf, 0, 8, .check_checksum = false, .max_counter = 0xfU, .expected_timestep = 20000U},
-           {0x1aa, 0, 16, .check_checksum = false, .max_counter = 0xffU, .expected_timestep = 20000U}}},
+  HYUNDAI_CANFD_COMMON_ADDR_CHECKS(0)
+  HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(0)
+};
+AddrCheckStruct hyundai_canfd_long_alt_buttons_addr_checks[] = {
+  HYUNDAI_CANFD_COMMON_ADDR_CHECKS(0)
+  HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(0)
+};
+
+// Radar sends SCC messages on these cars instead of camera
+AddrCheckStruct hyundai_canfd_radar_scc_addr_checks[] = {
+  HYUNDAI_CANFD_COMMON_ADDR_CHECKS(0)
+  HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(0)
+  HYUNDAI_CANFD_SCC_ADDR_CHECK(0)
 };
-#define HYUNDAI_CANFD_LONG_ADDR_CHECK_LEN (sizeof(hyundai_canfd_long_addr_checks) / sizeof(hyundai_canfd_long_addr_checks[0]))
-
-AddrCheckStruct hyundai_canfd_ice_addr_checks[] = {
-  {.msg = {{0x100, 0, 32, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }, { 0 }}},
-  {.msg = {{0xa0, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }, { 0 }}},
-  {.msg = {{0xea, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 10000U}, { 0 }, { 0 }}},
-  {.msg = {{0x175, 0, 24, .check_checksum = true, .max_counter = 0xffU, .expected_timestep = 20000U}, { 0 }, { 0 }}},
-  {.msg = {{0x1aa, 0, 16, .check_checksum = false, .max_counter = 0xffU, .expected_timestep = 20000U}, { 0 }, { 0 }}},
+AddrCheckStruct hyundai_canfd_radar_scc_alt_buttons_addr_checks[] = {
+  HYUNDAI_CANFD_COMMON_ADDR_CHECKS(0)
+  HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(0)
+  HYUNDAI_CANFD_SCC_ADDR_CHECK(0)
 };
-#define HYUNDAI_CANFD_ICE_ADDR_CHECK_LEN (sizeof(hyundai_canfd_ice_addr_checks) / sizeof(hyundai_canfd_ice_addr_checks[0]))
 
-addr_checks hyundai_canfd_rx_checks = {hyundai_canfd_addr_checks, HYUNDAI_CANFD_ADDR_CHECK_LEN};
 
+// *** HDA2 checks ***
+// E-CAN is on bus 1, ADAS unit sends SCC messages on HDA2.
+// Does not use the alt buttons message
+AddrCheckStruct hyundai_canfd_hda2_addr_checks[] = {
+  HYUNDAI_CANFD_COMMON_ADDR_CHECKS(1)
+  HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(1)
+  HYUNDAI_CANFD_SCC_ADDR_CHECK(1)
+};
+AddrCheckStruct hyundai_canfd_hda2_long_addr_checks[] = {
+  HYUNDAI_CANFD_COMMON_ADDR_CHECKS(1)
+  HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(1)
+};
 
-uint16_t hyundai_canfd_crc_lut[256];
+addr_checks hyundai_canfd_rx_checks = SET_ADDR_CHECKS(hyundai_canfd_addr_checks);
 
 
-const int HYUNDAI_PARAM_CANFD_HDA2 = 16;
 const int HYUNDAI_PARAM_CANFD_ALT_BUTTONS = 32;
-bool hyundai_canfd_hda2 = false;
+const int HYUNDAI_PARAM_CANFD_HDA2_ALT_STEERING = 128;
 bool hyundai_canfd_alt_buttons = false;
+bool hyundai_canfd_hda2_alt_steering = false;
 
 
+int hyundai_canfd_hda2_get_lkas_addr(void) {
+  return hyundai_canfd_hda2_alt_steering ? 0x110 : 0x50;
+}
+
 static uint8_t hyundai_canfd_get_counter(CANPacket_t *to_push) {
   uint8_t ret = 0;
   if (GET_LEN(to_push) == 8U) {
@@ -135,39 +153,10 @@ static uint32_t hyundai_canfd_get_checksum(CANPacket_t *to_push) {
   return chksum;
 }
 
-static uint32_t hyundai_canfd_compute_checksum(CANPacket_t *to_push) {
-  int len = GET_LEN(to_push);
-  uint32_t address = GET_ADDR(to_push);
-
-  uint16_t crc = 0;
-
-  for (int i = 2; i < len; i++) {
-    crc = (crc << 8U) ^ hyundai_canfd_crc_lut[(crc >> 8U) ^ GET_BYTE(to_push, i)];
-  }
-
-  // Add address to crc
-  crc = (crc << 8U) ^ hyundai_canfd_crc_lut[(crc >> 8U) ^ ((address >> 0U) & 0xFFU)];
-  crc = (crc << 8U) ^ hyundai_canfd_crc_lut[(crc >> 8U) ^ ((address >> 8U) & 0xFFU)];
-
-  if (len == 8) {
-    crc ^= 0x5f29U;
-  } else if (len == 16) {
-    crc ^= 0x041dU;
-  } else if (len == 24) {
-    crc ^= 0x819dU;
-  } else if (len == 32) {
-    crc ^= 0x9f5bU;
-  } else {
-
-  }
-
-  return crc;
-}
-
 static int hyundai_canfd_rx_hook(CANPacket_t *to_push) {
 
   bool valid = addr_safety_check(to_push, &hyundai_canfd_rx_checks,
-                                 hyundai_canfd_get_checksum, hyundai_canfd_compute_checksum, hyundai_canfd_get_counter, NULL);
+                                 hyundai_canfd_get_checksum, hyundai_common_canfd_compute_checksum, hyundai_canfd_get_counter, NULL);
 
   int bus = GET_BUS(to_push);
   int addr = GET_ADDR(to_push);
@@ -215,23 +204,23 @@ static int hyundai_canfd_rx_hook(CANPacket_t *to_push) {
 
     // vehicle moving
     if (addr == 0xa0) {
-      uint32_t speed = 0;
-      for (int i = 8; i < 15; i+=2) {
-        speed += GET_BYTE(to_push, i) | (GET_BYTE(to_push, i + 1) << 8U);
-      }
-      vehicle_moving = (speed / 4U) > HYUNDAI_STANDSTILL_THRSLD;
+      uint32_t front_left_speed = GET_BYTES(to_push, 8, 2);
+      uint32_t rear_right_speed = GET_BYTES(to_push, 14, 2);
+      vehicle_moving = (front_left_speed > HYUNDAI_STANDSTILL_THRSLD) || (rear_right_speed > HYUNDAI_STANDSTILL_THRSLD);
     }
   }
 
   if (valid && (bus == scc_bus)) {
     // cruise state
     if ((addr == 0x1a0) && !hyundai_longitudinal) {
-      bool cruise_engaged = ((GET_BYTE(to_push, 8) >> 4) & 0x3U) != 0U;
+      // 1=enabled, 2=driver override
+      int cruise_status = ((GET_BYTE(to_push, 8) >> 4) & 0x7U);
+      bool cruise_engaged = (cruise_status == 1) || (cruise_status == 2);
       hyundai_common_cruise_state_check(cruise_engaged);
     }
   }
 
-  const int steer_addr = hyundai_canfd_hda2 ? 0x50 : 0x12a;
+  const int steer_addr = hyundai_canfd_hda2 ? hyundai_canfd_hda2_get_lkas_addr() : 0x12a;
   bool stock_ecu_detected = (addr == steer_addr) && (bus == 0);
   if (hyundai_longitudinal) {
     // on HDA2, ensure ADRV ECU is still knocked out
@@ -250,7 +239,11 @@ static int hyundai_canfd_tx_hook(CANPacket_t *to_send) {
   int addr = GET_ADDR(to_send);
 
   if (hyundai_canfd_hda2 && !hyundai_longitudinal) {
-    tx = msg_allowed(to_send, HYUNDAI_CANFD_HDA2_TX_MSGS, sizeof(HYUNDAI_CANFD_HDA2_TX_MSGS)/sizeof(HYUNDAI_CANFD_HDA2_TX_MSGS[0]));
+    if (hyundai_canfd_hda2_alt_steering) {
+      tx = msg_allowed(to_send, HYUNDAI_CANFD_HDA2_ALT_STEERING_TX_MSGS, sizeof(HYUNDAI_CANFD_HDA2_ALT_STEERING_TX_MSGS)/sizeof(HYUNDAI_CANFD_HDA2_ALT_STEERING_TX_MSGS[0]));
+    } else {
+      tx = msg_allowed(to_send, HYUNDAI_CANFD_HDA2_TX_MSGS, sizeof(HYUNDAI_CANFD_HDA2_TX_MSGS)/sizeof(HYUNDAI_CANFD_HDA2_TX_MSGS[0]));
+    }
   } else if (hyundai_canfd_hda2 && hyundai_longitudinal) {
     tx = msg_allowed(to_send, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS, sizeof(HYUNDAI_CANFD_HDA2_LONG_TX_MSGS)/sizeof(HYUNDAI_CANFD_HDA2_LONG_TX_MSGS[0]));
   } else {
@@ -258,7 +251,7 @@ static int hyundai_canfd_tx_hook(CANPacket_t *to_send) {
   }
 
   // steering
-  const int steer_addr = (hyundai_canfd_hda2 && !hyundai_longitudinal) ? 0x50 : 0x12a;
+  const int steer_addr = (hyundai_canfd_hda2 && !hyundai_longitudinal) ? hyundai_canfd_hda2_get_lkas_addr() : 0x12a;
   if (addr == steer_addr) {
     int desired_torque = (((GET_BYTE(to_send, 6) & 0xFU) << 7U) | (GET_BYTE(to_send, 5) >> 1U)) - 1024U;
     bool steer_req = GET_BIT(to_send, 52U) != 0U;
@@ -320,16 +313,17 @@ static int hyundai_canfd_fwd_hook(int bus_num, int addr) {
   }
   if (bus_num == 2) {
     // LKAS for HDA2, LFA for HDA1
-    int is_lkas_msg = (((addr == 0x50) || (addr == 0x2a4)) && hyundai_canfd_hda2);
-    int is_lfa_msg = ((addr == 0x12a) && !hyundai_canfd_hda2);
+    int hda2_lfa_block_addr = hyundai_canfd_hda2_alt_steering ? 0x362 : 0x2a4;
+    bool is_lkas_msg = ((addr == hyundai_canfd_hda2_get_lkas_addr()) || (addr == hda2_lfa_block_addr)) && hyundai_canfd_hda2;
+    bool is_lfa_msg = ((addr == 0x12a) && !hyundai_canfd_hda2);
 
     // HUD icons
-    int is_lfahda_msg = ((addr == 0x1e0) && !hyundai_canfd_hda2);
+    bool is_lfahda_msg = ((addr == 0x1e0) && !hyundai_canfd_hda2);
 
     // CRUISE_INFO for non-HDA2, we send our own longitudinal commands
-    int is_scc_msg = ((addr == 0x1a0) && hyundai_longitudinal && !hyundai_canfd_hda2);
+    bool is_scc_msg = ((addr == 0x1a0) && hyundai_longitudinal && !hyundai_canfd_hda2);
 
-    int block_msg = is_lkas_msg || is_lfa_msg || is_lfahda_msg || is_scc_msg;
+    bool block_msg = is_lkas_msg || is_lfa_msg || is_lfahda_msg || is_scc_msg;
     if (!block_msg) {
       bus_fwd = 0;
     }
@@ -342,8 +336,8 @@ static const addr_checks* hyundai_canfd_init(uint16_t param) {
   hyundai_common_init(param);
 
   gen_crc_lookup_table_16(0x1021, hyundai_canfd_crc_lut);
-  hyundai_canfd_hda2 = GET_FLAG(param, HYUNDAI_PARAM_CANFD_HDA2);
   hyundai_canfd_alt_buttons = GET_FLAG(param, HYUNDAI_PARAM_CANFD_ALT_BUTTONS);
+  hyundai_canfd_hda2_alt_steering = GET_FLAG(param, HYUNDAI_PARAM_CANFD_HDA2_ALT_STEERING);
 
   // no long for ICE yet
   if (!hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) {
@@ -351,14 +345,18 @@ static const addr_checks* hyundai_canfd_init(uint16_t param) {
   }
 
   if (hyundai_longitudinal) {
-    hyundai_canfd_rx_checks = (addr_checks){hyundai_canfd_long_addr_checks, HYUNDAI_CANFD_LONG_ADDR_CHECK_LEN};
+    if (hyundai_canfd_hda2) {
+      hyundai_canfd_rx_checks = SET_ADDR_CHECKS(hyundai_canfd_hda2_long_addr_checks);
+    } else {
+      hyundai_canfd_rx_checks = hyundai_canfd_alt_buttons ? SET_ADDR_CHECKS(hyundai_canfd_long_alt_buttons_addr_checks) : SET_ADDR_CHECKS(hyundai_canfd_long_addr_checks);
+    }
   } else {
-    if (!hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) {
-      hyundai_canfd_rx_checks = (addr_checks){hyundai_canfd_ice_addr_checks, HYUNDAI_CANFD_ICE_ADDR_CHECK_LEN};
-    } else if (!hyundai_camera_scc && !hyundai_canfd_hda2) {
-      hyundai_canfd_rx_checks = (addr_checks){hyundai_canfd_radar_scc_addr_checks, HYUNDAI_CANFD_RADAR_SCC_ADDR_CHECK_LEN};
+    if (hyundai_canfd_hda2) {
+      hyundai_canfd_rx_checks = SET_ADDR_CHECKS(hyundai_canfd_hda2_addr_checks);
+    } else if (!hyundai_camera_scc) {
+      hyundai_canfd_rx_checks = hyundai_canfd_alt_buttons ? SET_ADDR_CHECKS(hyundai_canfd_radar_scc_alt_buttons_addr_checks) : SET_ADDR_CHECKS(hyundai_canfd_radar_scc_addr_checks);
     } else {
-      hyundai_canfd_rx_checks = (addr_checks){hyundai_canfd_addr_checks, HYUNDAI_CANFD_ADDR_CHECK_LEN};
+      hyundai_canfd_rx_checks = hyundai_canfd_alt_buttons ? SET_ADDR_CHECKS(hyundai_canfd_alt_buttons_addr_checks) : SET_ADDR_CHECKS(hyundai_canfd_addr_checks);
     }
   }
 
diff --git a/panda/board/safety/safety_hyundai_common.h b/panda/board/safety/safety_hyundai_common.h
index 8bd84feacf..0b9116d147 100644
--- a/panda/board/safety/safety_hyundai_common.h
+++ b/panda/board/safety/safety_hyundai_common.h
@@ -5,10 +5,11 @@ const int HYUNDAI_PARAM_EV_GAS = 1;
 const int HYUNDAI_PARAM_HYBRID_GAS = 2;
 const int HYUNDAI_PARAM_LONGITUDINAL = 4;
 const int HYUNDAI_PARAM_CAMERA_SCC = 8;
+const int HYUNDAI_PARAM_CANFD_HDA2 = 16;
 const int HYUNDAI_PARAM_ALT_LIMITS = 64; // TODO: shift this down with the rest of the common flags
 
 const uint8_t HYUNDAI_PREV_BUTTON_SAMPLES = 8;  // roughly 160 ms
-const uint32_t HYUNDAI_STANDSTILL_THRSLD = 30;  // ~1kph
+const uint32_t HYUNDAI_STANDSTILL_THRSLD = 12;  // 0.375 kph
 
 enum {
   HYUNDAI_BTN_NONE = 0,
@@ -22,13 +23,17 @@ bool hyundai_ev_gas_signal = false;
 bool hyundai_hybrid_gas_signal = false;
 bool hyundai_longitudinal = false;
 bool hyundai_camera_scc = false;
+bool hyundai_canfd_hda2 = false;
 bool hyundai_alt_limits = false;
 uint8_t hyundai_last_button_interaction;  // button messages since the user pressed an enable button
 
+uint16_t hyundai_canfd_crc_lut[256];
+
 void hyundai_common_init(uint16_t param) {
   hyundai_ev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_EV_GAS);
   hyundai_hybrid_gas_signal = !hyundai_ev_gas_signal && GET_FLAG(param, HYUNDAI_PARAM_HYBRID_GAS);
   hyundai_camera_scc = GET_FLAG(param, HYUNDAI_PARAM_CAMERA_SCC);
+  hyundai_canfd_hda2 = GET_FLAG(param, HYUNDAI_PARAM_CANFD_HDA2);
   hyundai_alt_limits = GET_FLAG(param, HYUNDAI_PARAM_ALT_LIMITS);
 
   hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES;
@@ -47,11 +52,11 @@ void hyundai_common_cruise_state_check(const int cruise_engaged) {
   // enter controls on rising edge of ACC and recent user button press, exit controls when ACC off
   if (!hyundai_longitudinal) {
     if (cruise_engaged && !cruise_engaged_prev && (hyundai_last_button_interaction < HYUNDAI_PREV_BUTTON_SAMPLES)) {
-      controls_allowed = 1;
+      controls_allowed = true;
     }
 
     if (!cruise_engaged) {
-      controls_allowed = 0;
+      controls_allowed = false;
     }
     cruise_engaged_prev = cruise_engaged;
   }
@@ -70,16 +75,41 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const int main
     bool set = (cruise_button != HYUNDAI_BTN_SET) && (cruise_button_prev == HYUNDAI_BTN_SET);
     bool res = (cruise_button != HYUNDAI_BTN_RESUME) && (cruise_button_prev == HYUNDAI_BTN_RESUME);
     if (set || res) {
-      controls_allowed = 1;
+      controls_allowed = true;
     }
 
     // exit controls on cancel press
     if (cruise_button == HYUNDAI_BTN_CANCEL) {
-      controls_allowed = 0;
+      controls_allowed = false;
     }
 
     cruise_button_prev = cruise_button;
   }
 }
 
+uint32_t hyundai_common_canfd_compute_checksum(CANPacket_t *to_push) {
+  int len = GET_LEN(to_push);
+  uint32_t address = GET_ADDR(to_push);
+
+  uint16_t crc = 0;
+
+  for (int i = 2; i < len; i++) {
+    crc = (crc << 8U) ^ hyundai_canfd_crc_lut[(crc >> 8U) ^ GET_BYTE(to_push, i)];
+  }
+
+  // Add address to crc
+  crc = (crc << 8U) ^ hyundai_canfd_crc_lut[(crc >> 8U) ^ ((address >> 0U) & 0xFFU)];
+  crc = (crc << 8U) ^ hyundai_canfd_crc_lut[(crc >> 8U) ^ ((address >> 8U) & 0xFFU)];
+
+  if (len == 24) {
+    crc ^= 0x819dU;
+  } else if (len == 32) {
+    crc ^= 0x9f5bU;
+  } else {
+
+  }
+
+  return crc;
+}
+
 #endif
diff --git a/panda/board/safety/safety_nissan.h b/panda/board/safety/safety_nissan.h
index 1f44238e4e..ed925e7e83 100644
--- a/panda/board/safety/safety_nissan.h
+++ b/panda/board/safety/safety_nissan.h
@@ -38,6 +38,8 @@ AddrCheckStruct nissan_addr_checks[] = {
 addr_checks nissan_rx_checks = {nissan_addr_checks, NISSAN_ADDR_CHECK_LEN};
 
 // EPS Location. false = V-CAN, true = C-CAN
+const int NISSAN_PARAM_ALT_EPS_BUS = 1;
+
 bool nissan_alt_eps = false;
 
 static int nissan_rx_hook(CANPacket_t *to_push) {
@@ -48,13 +50,13 @@ static int nissan_rx_hook(CANPacket_t *to_push) {
     int bus = GET_BUS(to_push);
     int addr = GET_ADDR(to_push);
 
-    if (((bus == 0) && (!nissan_alt_eps)) || ((bus == 1) && (nissan_alt_eps))) {
+    if (bus == (nissan_alt_eps ? 1 : 0)) {
       if (addr == 0x2) {
         // Current steering angle
         // Factor -0.1, little endian
         int angle_meas_new = (GET_BYTES(to_push, 0, 4) & 0xFFFFU);
-        // Need to multiply by 10 here as LKAS and Steering wheel are different base unit
-        angle_meas_new = to_signed(angle_meas_new, 16) * 10;
+        // Multiply by -10 to match scale of LKAS angle
+        angle_meas_new = to_signed(angle_meas_new, 16) * -10;
 
         // update array of samples
         update_sample(&angle_meas, angle_meas_new);
@@ -88,7 +90,7 @@ static int nissan_rx_hook(CANPacket_t *to_push) {
     }
 
     // Handle cruise enabled
-    if ((addr == 0x30f) && (((bus == 2) && (!nissan_alt_eps)) || ((bus == 1) && (nissan_alt_eps)))) {
+    if ((addr == 0x30f) && (bus == (nissan_alt_eps ? 1 : 2))) {
       bool cruise_engaged = (GET_BYTE(to_push, 0) >> 3) & 1U;
       pcm_cruise_check(cruise_engaged);
     }
@@ -114,8 +116,8 @@ static int nissan_tx_hook(CANPacket_t *to_send) {
     int desired_angle = ((GET_BYTE(to_send, 0) << 10) | (GET_BYTE(to_send, 1) << 2) | ((GET_BYTE(to_send, 2) >> 6) & 0x3U));
     bool lka_active = (GET_BYTE(to_send, 6) >> 4) & 1U;
 
-    // offeset 1310 * NISSAN_STEERING_LIMITS.angle_deg_to_can
-    desired_angle =  desired_angle - 131000;
+    // Factor is -0.01, offset is 1310. Flip to correct sign, but keep units in CAN scale
+    desired_angle = -desired_angle + (1310 * NISSAN_STEERING_LIMITS.angle_deg_to_can);
 
     if (steer_angle_cmd_checks(desired_angle, lka_active, NISSAN_STEERING_LIMITS)) {
       violation = true;
@@ -158,7 +160,7 @@ static int nissan_fwd_hook(int bus_num, int addr) {
 }
 
 static const addr_checks* nissan_init(uint16_t param) {
-  nissan_alt_eps = param ? 1 : 0;
+  nissan_alt_eps = GET_FLAG(param, NISSAN_PARAM_ALT_EPS_BUS);
   return &nissan_rx_checks;
 }
 
diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h
index 8127b6d65e..bfe800142a 100644
--- a/panda/board/safety/safety_subaru.h
+++ b/panda/board/safety/safety_subaru.h
@@ -1,19 +1,36 @@
-#define SUBARU_STEERING_LIMITS_GENERATOR(steer_max, rate_up, rate_down)          \
-  {                                                                              \
-    .max_steer = (steer_max),                                                    \
-    .max_rt_delta = 940,                                                         \
-    .max_rt_interval = 250000,                                                   \
-    .max_rate_up = (rate_up),                                                    \
-    .max_rate_down = (rate_down),                                                \
-    .driver_torque_factor = 50,                                                  \
-    .driver_torque_allowance = 60,                                               \
-    .type = TorqueDriverLimited,                                                 \
-  }                                                                              \
-
-const SteeringLimits SUBARU_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70);
+#define SUBARU_STEERING_LIMITS_GENERATOR(steer_max, rate_up, rate_down)               \
+  {                                                                                   \
+    .max_steer = (steer_max),                                                         \
+    .max_rt_delta = 940,                                                              \
+    .max_rt_interval = 250000,                                                        \
+    .max_rate_up = (rate_up),                                                         \
+    .max_rate_down = (rate_down),                                                     \
+    .driver_torque_factor = 50,                                                       \
+    .driver_torque_allowance = 60,                                                    \
+    .type = TorqueDriverLimited,                                                      \
+    /* the EPS will temporary fault if the steering rate is too high, so we cut the   \
+       the steering torque every 7 frames for 1 frame if the steering rate is high */ \
+    .min_valid_request_frames = 7,                                                    \
+    .max_invalid_request_frames = 1,                                                  \
+    .min_valid_request_rt_interval = 144000,  /* 10% tolerance */                     \
+    .has_steer_req_tolerance = true,                                                  \
+  }
+
+
+const SteeringLimits SUBARU_STEERING_LIMITS      = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70);
 const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40);
 
 
+const LongitudinalLimits SUBARU_LONG_LIMITS = {
+  .min_gas = 808,       // appears to be engine braking
+  .max_gas = 3400,      // approx  2 m/s^2 when maxing cruise_rpm and cruise_throttle
+  .inactive_gas = 1818, // this is zero acceleration
+  .max_brake = 600,     // approx -3.5 m/s^2
+
+  .min_transmission_rpm = 0,
+  .max_transmission_rpm = 2400,
+};
+
 #define MSG_SUBARU_Brake_Status          0x13c
 #define MSG_SUBARU_CruiseControl         0x240
 #define MSG_SUBARU_Throttle              0x40
@@ -21,6 +38,7 @@ const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERA
 #define MSG_SUBARU_Wheel_Speeds          0x13a
 
 #define MSG_SUBARU_ES_LKAS               0x122
+#define MSG_SUBARU_ES_LKAS_ANGLE         0x124
 #define MSG_SUBARU_ES_Brake              0x220
 #define MSG_SUBARU_ES_Distance           0x221
 #define MSG_SUBARU_ES_Status             0x222
@@ -28,16 +46,32 @@ const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERA
 #define MSG_SUBARU_ES_LKAS_State         0x322
 #define MSG_SUBARU_ES_Infotainment       0x323
 
+#define MSG_SUBARU_ES_UDS_Request        0x787
+
+#define MSG_SUBARU_ES_HighBeamAssist     0x121
+#define MSG_SUBARU_ES_STATIC_1           0x22a
+#define MSG_SUBARU_ES_STATIC_2           0x325
+
 #define SUBARU_MAIN_BUS 0
 #define SUBARU_ALT_BUS  1
 #define SUBARU_CAM_BUS  2
 
-#define SUBARU_COMMON_TX_MSGS(alt_bus)              \
-  {MSG_SUBARU_ES_LKAS,         SUBARU_MAIN_BUS, 8}, \
-  {MSG_SUBARU_ES_Distance,     alt_bus,         8}, \
-  {MSG_SUBARU_ES_DashStatus,   SUBARU_MAIN_BUS, 8}, \
-  {MSG_SUBARU_ES_LKAS_State,   SUBARU_MAIN_BUS, 8}, \
-  {MSG_SUBARU_ES_Infotainment, SUBARU_MAIN_BUS, 8}, \
+#define SUBARU_COMMON_TX_MSGS(alt_bus, lkas_msg)      \
+  {lkas_msg,                     SUBARU_MAIN_BUS, 8}, \
+  {MSG_SUBARU_ES_Distance,       alt_bus,         8}, \
+  {MSG_SUBARU_ES_DashStatus,     SUBARU_MAIN_BUS, 8}, \
+  {MSG_SUBARU_ES_LKAS_State,     SUBARU_MAIN_BUS, 8}, \
+  {MSG_SUBARU_ES_Infotainment,   SUBARU_MAIN_BUS, 8}, \
+
+#define SUBARU_COMMON_LONG_TX_MSGS(alt_bus)           \
+  {MSG_SUBARU_ES_Brake,          alt_bus,         8}, \
+  {MSG_SUBARU_ES_Status,         alt_bus,         8}, \
+
+#define SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS()         \
+  {MSG_SUBARU_ES_UDS_Request,    SUBARU_CAM_BUS,  8}, \
+  {MSG_SUBARU_ES_HighBeamAssist, SUBARU_MAIN_BUS, 8}, \
+  {MSG_SUBARU_ES_STATIC_1,       SUBARU_MAIN_BUS, 8}, \
+  {MSG_SUBARU_ES_STATIC_2,       SUBARU_MAIN_BUS, 8}, \
 
 #define SUBARU_COMMON_ADDR_CHECKS(alt_bus)                                                                                                            \
   {.msg = {{MSG_SUBARU_Throttle,        SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}}, \
@@ -47,15 +81,28 @@ const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERA
   {.msg = {{MSG_SUBARU_CruiseControl,   alt_bus,         8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 50000U}, { 0 }, { 0 }}}, \
 
 const CanMsg SUBARU_TX_MSGS[] = {
-  SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS)
+  SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
 };
 #define SUBARU_TX_MSGS_LEN (sizeof(SUBARU_TX_MSGS) / sizeof(SUBARU_TX_MSGS[0]))
 
+const CanMsg SUBARU_LONG_TX_MSGS[] = {
+  SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
+  SUBARU_COMMON_LONG_TX_MSGS(SUBARU_MAIN_BUS)
+};
+#define SUBARU_LONG_TX_MSGS_LEN (sizeof(SUBARU_LONG_TX_MSGS) / sizeof(SUBARU_LONG_TX_MSGS[0]))
+
 const CanMsg SUBARU_GEN2_TX_MSGS[] = {
-  SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
+  SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
 };
 #define SUBARU_GEN2_TX_MSGS_LEN (sizeof(SUBARU_GEN2_TX_MSGS) / sizeof(SUBARU_GEN2_TX_MSGS[0]))
 
+const CanMsg SUBARU_GEN2_LONG_TX_MSGS[] = {
+  SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
+  SUBARU_COMMON_LONG_TX_MSGS(SUBARU_ALT_BUS)
+  SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS()
+};
+#define SUBARU_GEN2_LONG_TX_MSGS_LEN (sizeof(SUBARU_GEN2_LONG_TX_MSGS) / sizeof(SUBARU_GEN2_LONG_TX_MSGS[0]))
+
 AddrCheckStruct subaru_addr_checks[] = {
   SUBARU_COMMON_ADDR_CHECKS(SUBARU_MAIN_BUS)
 };
@@ -70,7 +117,10 @@ addr_checks subaru_gen2_rx_checks = {subaru_gen2_addr_checks, SUBARU_GEN2_ADDR_C
 
 
 const uint16_t SUBARU_PARAM_GEN2 = 1;
+const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
+
 bool subaru_gen2 = false;
+bool subaru_longitudinal = false;
 
 
 static uint32_t subaru_get_checksum(CANPacket_t *to_push) {
@@ -98,7 +148,7 @@ static int subaru_rx_hook(CANPacket_t *to_push) {
 
   if (valid) {
     const int bus = GET_BUS(to_push);
-    const int alt_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
+    const int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
 
     int addr = GET_ADDR(to_push);
     if ((addr == MSG_SUBARU_Steering_Torque) && (bus == SUBARU_MAIN_BUS)) {
@@ -106,21 +156,34 @@ static int subaru_rx_hook(CANPacket_t *to_push) {
       torque_driver_new = ((GET_BYTES(to_push, 0, 4) >> 16) & 0x7FFU);
       torque_driver_new = -1 * to_signed(torque_driver_new, 11);
       update_sample(&torque_driver, torque_driver_new);
+
+      int angle_meas_new = (GET_BYTES(to_push, 4, 2) & 0xFFFFU);
+      // convert Steering_Torque -> Steering_Angle to centidegrees, to match the ES_LKAS_ANGLE angle request units
+      angle_meas_new = ROUND(to_signed(angle_meas_new, 16) * -2.17);
+      update_sample(&angle_meas, angle_meas_new);
     }
 
     // enter controls on rising edge of ACC, exit controls on ACC off
-    if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_bus)) {
+    if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_main_bus)) {
       bool cruise_engaged = GET_BIT(to_push, 41U) != 0U;
       pcm_cruise_check(cruise_engaged);
     }
 
     // update vehicle moving with any non-zero wheel speed
-    if ((addr == MSG_SUBARU_Wheel_Speeds) && (bus == alt_bus)) {
-      vehicle_moving = ((GET_BYTES(to_push, 0, 4) >> 12) != 0U) || (GET_BYTES(to_push, 4, 4) != 0U);
+    if ((addr == MSG_SUBARU_Wheel_Speeds) && (bus == alt_main_bus)) {
+      uint32_t fr = (GET_BYTES(to_push, 1, 3) >> 4) & 0x1FFFU;
+      uint32_t rr = (GET_BYTES(to_push, 3, 3) >> 1) & 0x1FFFU;
+      uint32_t rl = (GET_BYTES(to_push, 4, 3) >> 6) & 0x1FFFU;
+      uint32_t fl = (GET_BYTES(to_push, 6, 2) >> 3) & 0x1FFFU;
+
+      vehicle_moving = (fr > 0U) || (rr > 0U) || (rl > 0U) || (fl > 0U);
+
+      float speed = (fr + rr + rl + fl) / 4U * 0.057;
+      update_sample(&vehicle_speed, ROUND(speed * VEHICLE_SPEED_FACTOR));
     }
 
-    if ((addr == MSG_SUBARU_Brake_Status) && (bus == alt_bus)) {
-      brake_pressed = ((GET_BYTE(to_push, 7) >> 6) & 1U);
+    if ((addr == MSG_SUBARU_Brake_Status) && (bus == alt_main_bus)) {
+      brake_pressed = GET_BIT(to_push, 62U) != 0U;
     }
 
     if ((addr == MSG_SUBARU_Throttle) && (bus == SUBARU_MAIN_BUS)) {
@@ -136,11 +199,16 @@ static int subaru_tx_hook(CANPacket_t *to_send) {
 
   int tx = 1;
   int addr = GET_ADDR(to_send);
-
-  if (subaru_gen2) {
-    tx = msg_allowed(to_send, SUBARU_GEN2_TX_MSGS, SUBARU_GEN2_TX_MSGS_LEN);
+  bool violation = false;
+
+  if (subaru_gen2 && subaru_longitudinal) {
+    tx = msg_allowed(to_send, SUBARU_GEN2_LONG_TX_MSGS, SUBARU_GEN2_LONG_TX_MSGS_LEN);
+  } else if (subaru_gen2) {
+    tx = msg_allowed(to_send, SUBARU_GEN2_TX_MSGS,      SUBARU_GEN2_TX_MSGS_LEN);
+  } else if (subaru_longitudinal) {
+    tx = msg_allowed(to_send, SUBARU_LONG_TX_MSGS,      SUBARU_LONG_TX_MSGS_LEN);
   } else {
-    tx = msg_allowed(to_send, SUBARU_TX_MSGS, SUBARU_TX_MSGS_LEN);
+    tx = msg_allowed(to_send, SUBARU_TX_MSGS,           SUBARU_TX_MSGS_LEN);
   }
 
   // steer cmd checks
@@ -148,11 +216,51 @@ static int subaru_tx_hook(CANPacket_t *to_send) {
     int desired_torque = ((GET_BYTES(to_send, 0, 4) >> 16) & 0x1FFFU);
     desired_torque = -1 * to_signed(desired_torque, 13);
 
+    bool steer_req = GET_BIT(to_send, 29U) != 0U;
+
     const SteeringLimits limits = subaru_gen2 ? SUBARU_GEN2_STEERING_LIMITS : SUBARU_STEERING_LIMITS;
-    if (steer_torque_cmd_checks(desired_torque, -1, limits)) {
-      tx = 0;
+    violation |= steer_torque_cmd_checks(desired_torque, steer_req, limits);
+  }
+
+  // check es_brake brake_pressure limits
+  if (addr == MSG_SUBARU_ES_Brake) {
+    int es_brake_pressure = GET_BYTES(to_send, 2, 2);
+    violation |= longitudinal_brake_checks(es_brake_pressure, SUBARU_LONG_LIMITS);
+  }
+
+  // check es_distance cruise_throttle limits
+  if (addr == MSG_SUBARU_ES_Distance) {
+    int cruise_throttle = (GET_BYTES(to_send, 2, 2) & 0xFFFU);
+    bool cruise_cancel = GET_BIT(to_send, 56U) != 0U;
+
+    if (subaru_longitudinal) {
+      violation |= longitudinal_gas_checks(cruise_throttle, SUBARU_LONG_LIMITS);
+    } else {
+      // If openpilot is not controlling long, only allow ES_Distance for cruise cancel requests,
+      // (when Cruise_Cancel is true, and Cruise_Throttle is inactive)
+      violation |= (cruise_throttle != SUBARU_LONG_LIMITS.inactive_gas);
+      violation |= (!cruise_cancel);
     }
+  }
 
+  // check es_status transmission_rpm limits
+  if (addr == MSG_SUBARU_ES_Status) {
+    int transmission_rpm = (GET_BYTES(to_send, 2, 2) & 0xFFFU);
+    violation |= longitudinal_transmission_rpm_checks(transmission_rpm, SUBARU_LONG_LIMITS);
+  }
+
+  if (addr == MSG_SUBARU_ES_UDS_Request) {
+    // tester present ('\x02\x3E\x80\x00\x00\x00\x00\x00') is allowed for gen2 longitudinal to keep eyesight disabled
+    bool is_tester_present = (GET_BYTES(to_send, 0, 4) == 0x00803E02U) && (GET_BYTES(to_send, 4, 4) == 0x0U);
+
+    // reading ES button data by identifier (b'\x03\x22\x11\x30\x00\x00\x00\x00') is also allowed (DID 0x1130)
+    bool is_button_rdbi = (GET_BYTES(to_send, 0, 4) == 0x30112203U) && (GET_BYTES(to_send, 4, 4) == 0x0U);
+
+    violation |= !(is_tester_present || is_button_rdbi);
+  }
+
+  if (violation){
+    tx = 0;
   }
   return tx;
 }
@@ -161,7 +269,7 @@ static int subaru_fwd_hook(int bus_num, int addr) {
   int bus_fwd = -1;
 
   if (bus_num == SUBARU_MAIN_BUS) {
-    bus_fwd = SUBARU_CAM_BUS;  // forward to camera
+    bus_fwd = SUBARU_CAM_BUS;  // to the eyesight camera
   }
 
   if (bus_num == SUBARU_CAM_BUS) {
@@ -170,7 +278,13 @@ static int subaru_fwd_hook(int bus_num, int addr) {
                        (addr == MSG_SUBARU_ES_DashStatus) ||
                        (addr == MSG_SUBARU_ES_LKAS_State) ||
                        (addr == MSG_SUBARU_ES_Infotainment));
-    if (!block_lkas) {
+
+    bool block_long = ((addr == MSG_SUBARU_ES_Brake) ||
+                       (addr == MSG_SUBARU_ES_Distance) ||
+                       (addr == MSG_SUBARU_ES_Status));
+
+    bool block_msg = block_lkas || (subaru_longitudinal && block_long);
+    if (!block_msg) {
       bus_fwd = SUBARU_MAIN_BUS;  // Main CAN
     }
   }
@@ -181,6 +295,10 @@ static int subaru_fwd_hook(int bus_num, int addr) {
 static const addr_checks* subaru_init(uint16_t param) {
   subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);
 
+#ifdef ALLOW_DEBUG
+  subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);
+#endif
+
   if (subaru_gen2) {
     subaru_rx_checks = (addr_checks){subaru_gen2_addr_checks, SUBARU_GEN2_ADDR_CHECK_LEN};
   } else {
diff --git a/panda/board/safety/safety_subaru_preglobal.h b/panda/board/safety/safety_subaru_preglobal.h
index b30af3d4aa..145a5cfa23 100644
--- a/panda/board/safety/safety_subaru_preglobal.h
+++ b/panda/board/safety/safety_subaru_preglobal.h
@@ -39,6 +39,11 @@ AddrCheckStruct subaru_preglobal_addr_checks[] = {
 #define SUBARU_PG_ADDR_CHECK_LEN (sizeof(subaru_preglobal_addr_checks) / sizeof(subaru_preglobal_addr_checks[0]))
 addr_checks subaru_preglobal_rx_checks = {subaru_preglobal_addr_checks, SUBARU_PG_ADDR_CHECK_LEN};
 
+
+const int SUBARU_PG_PARAM_REVERSED_DRIVER_TORQUE = 1;
+bool subaru_pg_reversed_driver_torque = false;
+
+
 static int subaru_preglobal_rx_hook(CANPacket_t *to_push) {
 
   bool valid = addr_safety_check(to_push, &subaru_preglobal_rx_checks, NULL, NULL, NULL, NULL);
@@ -51,6 +56,7 @@ static int subaru_preglobal_rx_hook(CANPacket_t *to_push) {
       int torque_driver_new;
       torque_driver_new = (GET_BYTE(to_push, 3) >> 5) + (GET_BYTE(to_push, 4) << 3);
       torque_driver_new = to_signed(torque_driver_new, 11);
+      torque_driver_new = subaru_pg_reversed_driver_torque ? -torque_driver_new : torque_driver_new;
       update_sample(&torque_driver, torque_driver_new);
     }
 
@@ -92,7 +98,9 @@ static int subaru_preglobal_tx_hook(CANPacket_t *to_send) {
     int desired_torque = ((GET_BYTES(to_send, 0, 4) >> 8) & 0x1FFFU);
     desired_torque = -1 * to_signed(desired_torque, 13);
 
-    if (steer_torque_cmd_checks(desired_torque, -1, SUBARU_PG_STEERING_LIMITS)) {
+    bool steer_req = (GET_BIT(to_send, 24U) != 0U);
+
+    if (steer_torque_cmd_checks(desired_torque, steer_req, SUBARU_PG_STEERING_LIMITS)) {
       tx = 0;
     }
 
@@ -118,7 +126,7 @@ static int subaru_preglobal_fwd_hook(int bus_num, int addr) {
 }
 
 static const addr_checks* subaru_preglobal_init(uint16_t param) {
-  UNUSED(param);
+  subaru_pg_reversed_driver_torque = GET_FLAG(param, SUBARU_PG_PARAM_REVERSED_DRIVER_TORQUE);
   return &subaru_preglobal_rx_checks;
 }
 
diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h
index 676670aaaf..168ad462dc 100644
--- a/panda/board/safety/safety_tesla.h
+++ b/panda/board/safety/safety_tesla.h
@@ -226,6 +226,8 @@ static const addr_checks* tesla_init(uint16_t param) {
   tesla_powertrain = GET_FLAG(param, TESLA_FLAG_POWERTRAIN);
   tesla_longitudinal = GET_FLAG(param, TESLA_FLAG_LONGITUDINAL_CONTROL);
 
+  tesla_stock_aeb = false;
+
   return tesla_powertrain ? (&tesla_pt_rx_checks) : (&tesla_rx_checks);
 }
 
diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h
index 0bc7f3c6df..721ce84445 100644
--- a/panda/board/safety/safety_toyota.h
+++ b/panda/board/safety/safety_toyota.h
@@ -222,7 +222,6 @@ static int toyota_tx_hook(CANPacket_t *to_send) {
 }
 
 static const addr_checks* toyota_init(uint16_t param) {
-  gas_interceptor_detected = 0;
   toyota_alt_brake = GET_FLAG(param, TOYOTA_PARAM_ALT_BRAKE);
   toyota_stock_longitudinal = GET_FLAG(param, TOYOTA_PARAM_STOCK_LONGITUDINAL);
   toyota_dbc_eps_torque_factor = param & TOYOTA_EPS_FACTOR;
diff --git a/panda/board/safety/safety_volkswagen_mqb.h b/panda/board/safety/safety_volkswagen_mqb.h
index 4cd9c2ec31..a88e4a9e81 100644
--- a/panda/board/safety/safety_volkswagen_mqb.h
+++ b/panda/board/safety/safety_volkswagen_mqb.h
@@ -76,21 +76,15 @@ static uint32_t volkswagen_mqb_compute_crc(CANPacket_t *to_push) {
   }
 
   uint8_t counter = volkswagen_mqb_get_counter(to_push);
-  switch(addr) {
-    case MSG_LH_EPS_03:
-      crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter];
-      break;
-    case MSG_ESP_05:
-      crc ^= (uint8_t[]){0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07}[counter];
-      break;
-    case MSG_TSK_06:
-      crc ^= (uint8_t[]){0xC4,0xE2,0x4F,0xE4,0xF8,0x2F,0x56,0x81,0x9F,0xE5,0x83,0x44,0x05,0x3F,0x97,0xDF}[counter];
-      break;
-    case MSG_MOTOR_20:
-      crc ^= (uint8_t[]){0xE9,0x65,0xAE,0x6B,0x7B,0x35,0xE5,0x5F,0x4E,0xC7,0x86,0xA2,0xBB,0xDD,0xEB,0xB4}[counter];
-      break;
-    default: // Undefined CAN message, CRC check expected to fail
-      break;
+  if (addr == MSG_LH_EPS_03) {
+    crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter];
+  } else if (addr == MSG_ESP_05) {
+    crc ^= (uint8_t[]){0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07}[counter];
+  } else if (addr == MSG_TSK_06) {
+    crc ^= (uint8_t[]){0xC4,0xE2,0x4F,0xE4,0xF8,0x2F,0x56,0x81,0x9F,0xE5,0x83,0x44,0x05,0x3F,0x97,0xDF}[counter];
+  } else if (addr == MSG_MOTOR_20) {
+    crc ^= (uint8_t[]){0xE9,0x65,0xAE,0x6B,0x7B,0x35,0xE5,0x5F,0x4E,0xC7,0x86,0xA2,0xBB,0xDD,0xEB,0xB4}[counter];
+  } else {  // Undefined CAN message, CRC check expected to fail
   }
   crc = volkswagen_crc8_lut_8h2f[crc];
 
@@ -223,7 +217,9 @@ static int volkswagen_mqb_tx_hook(CANPacket_t *to_send) {
       desired_torque *= -1;
     }
 
-    if (steer_torque_cmd_checks(desired_torque, -1, VOLKSWAGEN_MQB_STEERING_LIMITS)) {
+    bool steer_req = GET_BIT(to_send, 30U) != 0U;
+
+    if (steer_torque_cmd_checks(desired_torque, steer_req, VOLKSWAGEN_MQB_STEERING_LIMITS)) {
       tx = 0;
     }
   }
diff --git a/panda/board/safety/safety_volkswagen_pq.h b/panda/board/safety/safety_volkswagen_pq.h
index 7a909d4d5e..6232bdf569 100644
--- a/panda/board/safety/safety_volkswagen_pq.h
+++ b/panda/board/safety/safety_volkswagen_pq.h
@@ -63,7 +63,6 @@ static uint8_t volkswagen_pq_get_counter(CANPacket_t *to_push) {
   } else if (addr == MSG_GRA_NEU) {
     counter = (uint8_t)(GET_BYTE(to_push, 2) & 0xF0U) >> 4;
   } else {
-    counter = 0U;
   }
 
   return counter;
@@ -130,7 +129,7 @@ static int volkswagen_pq_rx_hook(CANPacket_t *to_push) {
         // Signal: Motor_5.GRA_Hauptschalter
         acc_main_on = GET_BIT(to_push, 50U);
         if (!acc_main_on) {
-          controls_allowed = 0;
+          controls_allowed = false;
         }
       }
 
@@ -148,7 +147,7 @@ static int volkswagen_pq_rx_hook(CANPacket_t *to_push) {
         // Exit controls on rising edge of Cancel, override Set/Resume if present simultaneously
         // Signal: GRA_ACC_01.GRA_Abbrechen
         if (GET_BIT(to_push, 9U) == 1U) {
-          controls_allowed = 0;
+          controls_allowed = false;
         }
       }
     } else {
@@ -197,7 +196,10 @@ static int volkswagen_pq_tx_hook(CANPacket_t *to_send) {
       desired_torque *= -1;
     }
 
-    if (steer_torque_cmd_checks(desired_torque, -1, VOLKSWAGEN_PQ_STEERING_LIMITS)) {
+    uint32_t hca_status = ((GET_BYTE(to_send, 1) >> 4) & 0xFU);
+    bool steer_req = (hca_status == 5U);
+
+    if (steer_torque_cmd_checks(desired_torque, steer_req, VOLKSWAGEN_PQ_STEERING_LIMITS)) {
       tx = 0;
     }
   }
diff --git a/panda/board/safety_declarations.h b/panda/board/safety_declarations.h
index b2f0ea9854..cbef3a25d4 100644
--- a/panda/board/safety_declarations.h
+++ b/panda/board/safety_declarations.h
@@ -4,6 +4,8 @@
 #define GET_BYTE(msg, b) ((msg)->data[(b)])
 #define GET_FLAG(value, mask) (((__typeof__(mask))(value) & (mask)) == (mask))
 
+#define SET_ADDR_CHECKS(name) ((addr_checks){(name), (sizeof((name)) / sizeof((name)[0]))})
+
 uint32_t GET_BYTES(const CANPacket_t *msg, int start, int len) {
   uint32_t ret = 0U;
   for (int i = 0; i < len; i++) {
@@ -16,12 +18,13 @@ uint32_t GET_BYTES(const CANPacket_t *msg, int start, int len) {
 const int MAX_WRONG_COUNTERS = 5;
 const uint8_t MAX_MISSED_MSGS = 10U;
 #define MAX_ADDR_CHECK_MSGS 3U
+#define MAX_SAMPLE_VALS 6
 // used to represent floating point vehicle speed in a sample_t
 #define VEHICLE_SPEED_FACTOR 100.0
 
 // sample struct that keeps 6 samples in memory
 struct sample_t {
-  int values[6];
+  int values[MAX_SAMPLE_VALS];
   int min;
   int max;
 } sample_t_default = {.values = {0}, .min = 0, .max = 0};
@@ -90,6 +93,11 @@ typedef struct {
   const int inactive_gas;
   const int max_brake;
 
+  // transmission rpm limits
+  const int max_transmission_rpm;
+  const int min_transmission_rpm;
+  const int inactive_transmission_rpm;
+
   // speed cmd limits
   const int inactive_speed;
 } LongitudinalLimits;
@@ -130,6 +138,7 @@ int safety_tx_lin_hook(int lin_num, uint8_t *data, int len);
 uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last);
 int to_signed(int d, int bits);
 void update_sample(struct sample_t *sample, int sample_new);
+void reset_sample(struct sample_t *sample);
 bool max_limit_check(int val, const int MAX, const int MIN);
 bool angle_dist_to_meas_check(int val, struct sample_t *val_meas,
   const int MAX_ERROR, const int MAX_VAL);
@@ -163,6 +172,7 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
 bool longitudinal_accel_checks(int desired_accel, const LongitudinalLimits limits);
 bool longitudinal_speed_checks(int desired_speed, const LongitudinalLimits limits);
 bool longitudinal_gas_checks(int desired_gas, const LongitudinalLimits limits);
+bool longitudinal_transmission_rpm_checks(int desired_transmission_rpm, const LongitudinalLimits limits);
 bool longitudinal_brake_checks(int desired_brake, const LongitudinalLimits limits);
 bool longitudinal_interceptor_checks(CANPacket_t *to_send);
 void pcm_cruise_check(bool cruise_engaged);
diff --git a/panda/board/stm32fx/llbxcan.h b/panda/board/stm32fx/llbxcan.h
index 64f9905146..fd344913f7 100644
--- a/panda/board/stm32fx/llbxcan.h
+++ b/panda/board/stm32fx/llbxcan.h
@@ -22,19 +22,19 @@ void print(const char *a);
 const uint32_t speeds[] = {100U, 200U, 500U, 1000U, 1250U, 2500U, 5000U, 10000U};
 const uint32_t data_speeds[] = {0U}; // No separate data speed, dummy
 
-bool llcan_set_speed(CAN_TypeDef *CAN_obj, uint32_t speed, bool loopback, bool silent) {
+bool llcan_set_speed(CAN_TypeDef *CANx, uint32_t speed, bool loopback, bool silent) {
   bool ret = true;
 
   // initialization mode
-  register_set(&(CAN_obj->MCR), CAN_MCR_TTCM | CAN_MCR_INRQ, 0x180FFU);
+  register_set(&(CANx->MCR), CAN_MCR_TTCM | CAN_MCR_INRQ, 0x180FFU);
   uint32_t timeout_counter = 0U;
-  while((CAN_obj->MSR & CAN_MSR_INAK) != CAN_MSR_INAK){
+  while((CANx->MSR & CAN_MSR_INAK) != CAN_MSR_INAK){
     // Delay for about 1ms
     delay(10000);
     timeout_counter++;
 
     if(timeout_counter >= CAN_INIT_TIMEOUT_MS){
-      print(CAN_NAME_FROM_CANIF(CAN_obj)); print(" set_speed timed out (1)!\n");
+      print(CAN_NAME_FROM_CANIF(CANx)); print(" set_speed timed out (1)!\n");
       ret = false;
       break;
     }
@@ -42,30 +42,30 @@ bool llcan_set_speed(CAN_TypeDef *CAN_obj, uint32_t speed, bool loopback, bool s
 
   if(ret){
     // set time quanta from defines
-    register_set(&(CAN_obj->BTR), ((CAN_BTR_TS1_0 * (CAN_SEQ1-1U)) |
+    register_set(&(CANx->BTR), ((CAN_BTR_TS1_0 * (CAN_SEQ1-1U)) |
                                    (CAN_BTR_TS2_0 * (CAN_SEQ2-1U)) |
                                    (CAN_BTR_SJW_0 * (CAN_SJW-1U)) |
                                    (can_speed_to_prescaler(speed) - 1U)), 0xC37F03FFU);
 
     // silent loopback mode for debugging
     if (loopback) {
-      register_set_bits(&(CAN_obj->BTR), CAN_BTR_SILM | CAN_BTR_LBKM);
+      register_set_bits(&(CANx->BTR), CAN_BTR_SILM | CAN_BTR_LBKM);
     }
     if (silent) {
-      register_set_bits(&(CAN_obj->BTR), CAN_BTR_SILM);
+      register_set_bits(&(CANx->BTR), CAN_BTR_SILM);
     }
 
     // reset
-    register_set(&(CAN_obj->MCR), CAN_MCR_TTCM | CAN_MCR_ABOM, 0x180FFU);
+    register_set(&(CANx->MCR), CAN_MCR_TTCM | CAN_MCR_ABOM, 0x180FFU);
 
     timeout_counter = 0U;
-    while(((CAN_obj->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)) {
+    while(((CANx->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)) {
       // Delay for about 1ms
       delay(10000);
       timeout_counter++;
 
       if(timeout_counter >= CAN_INIT_TIMEOUT_MS){
-        print(CAN_NAME_FROM_CANIF(CAN_obj)); print(" set_speed timed out (2)!\n");
+        print(CAN_NAME_FROM_CANIF(CANx)); print(" set_speed timed out (2)!\n");
         ret = false;
         break;
       }
@@ -75,17 +75,17 @@ bool llcan_set_speed(CAN_TypeDef *CAN_obj, uint32_t speed, bool loopback, bool s
   return ret;
 }
 
-void llcan_irq_disable(CAN_TypeDef *CAN_obj) {
-  if (CAN_obj == CAN1) {
+void llcan_irq_disable(CAN_TypeDef *CANx) {
+  if (CANx == CAN1) {
     NVIC_DisableIRQ(CAN1_TX_IRQn);
     NVIC_DisableIRQ(CAN1_RX0_IRQn);
     NVIC_DisableIRQ(CAN1_SCE_IRQn);
-  } else if (CAN_obj == CAN2) {
+  } else if (CANx == CAN2) {
     NVIC_DisableIRQ(CAN2_TX_IRQn);
     NVIC_DisableIRQ(CAN2_RX0_IRQn);
     NVIC_DisableIRQ(CAN2_SCE_IRQn);
   #ifdef CAN3
-    } else if (CAN_obj == CAN3) {
+    } else if (CANx == CAN3) {
       NVIC_DisableIRQ(CAN3_TX_IRQn);
       NVIC_DisableIRQ(CAN3_RX0_IRQn);
       NVIC_DisableIRQ(CAN3_SCE_IRQn);
@@ -94,17 +94,17 @@ void llcan_irq_disable(CAN_TypeDef *CAN_obj) {
   }
 }
 
-void llcan_irq_enable(CAN_TypeDef *CAN_obj) {
-  if (CAN_obj == CAN1) {
+void llcan_irq_enable(CAN_TypeDef *CANx) {
+  if (CANx == CAN1) {
     NVIC_EnableIRQ(CAN1_TX_IRQn);
     NVIC_EnableIRQ(CAN1_RX0_IRQn);
     NVIC_EnableIRQ(CAN1_SCE_IRQn);
-  } else if (CAN_obj == CAN2) {
+  } else if (CANx == CAN2) {
     NVIC_EnableIRQ(CAN2_TX_IRQn);
     NVIC_EnableIRQ(CAN2_RX0_IRQn);
     NVIC_EnableIRQ(CAN2_SCE_IRQn);
   #ifdef CAN3
-    } else if (CAN_obj == CAN3) {
+    } else if (CANx == CAN3) {
       NVIC_EnableIRQ(CAN3_TX_IRQn);
       NVIC_EnableIRQ(CAN3_RX0_IRQn);
       NVIC_EnableIRQ(CAN3_SCE_IRQn);
@@ -113,21 +113,21 @@ void llcan_irq_enable(CAN_TypeDef *CAN_obj) {
   }
 }
 
-bool llcan_init(CAN_TypeDef *CAN_obj) {
+bool llcan_init(CAN_TypeDef *CANx) {
   bool ret = true;
 
   // Enter init mode
-  register_set_bits(&(CAN_obj->FMR), CAN_FMR_FINIT);
+  register_set_bits(&(CANx->FMR), CAN_FMR_FINIT);
 
   // Wait for INAK bit to be set
   uint32_t timeout_counter = 0U;
-  while(((CAN_obj->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)) {
+  while(((CANx->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)) {
     // Delay for about 1ms
     delay(10000);
     timeout_counter++;
 
     if(timeout_counter >= CAN_INIT_TIMEOUT_MS){
-      print(CAN_NAME_FROM_CANIF(CAN_obj)); print(" initialization timed out!\n");
+      print(CAN_NAME_FROM_CANIF(CANx)); print(" initialization timed out!\n");
       ret = false;
       break;
     }
@@ -136,27 +136,27 @@ bool llcan_init(CAN_TypeDef *CAN_obj) {
   if(ret){
     // no mask
     // For some weird reason some of these registers do not want to set properly on CAN2 and CAN3. Probably something to do with the single/dual mode and their different filters.
-    CAN_obj->sFilterRegister[0].FR1 = 0U;
-    CAN_obj->sFilterRegister[0].FR2 = 0U;
-    CAN_obj->sFilterRegister[14].FR1 = 0U;
-    CAN_obj->sFilterRegister[14].FR2 = 0U;
-    CAN_obj->FA1R |= 1U | (1U << 14);
+    CANx->sFilterRegister[0].FR1 = 0U;
+    CANx->sFilterRegister[0].FR2 = 0U;
+    CANx->sFilterRegister[14].FR1 = 0U;
+    CANx->sFilterRegister[14].FR2 = 0U;
+    CANx->FA1R |= 1U | (1U << 14);
 
     // Exit init mode, do not wait
-    register_clear_bits(&(CAN_obj->FMR), CAN_FMR_FINIT);
+    register_clear_bits(&(CANx->FMR), CAN_FMR_FINIT);
 
     // enable certain CAN interrupts
-    register_set_bits(&(CAN_obj->IER), CAN_IER_TMEIE | CAN_IER_FMPIE0 | CAN_IER_ERRIE | CAN_IER_LECIE | CAN_IER_BOFIE | CAN_IER_EPVIE | CAN_IER_EWGIE | CAN_IER_FOVIE0 | CAN_IER_FFIE0);
+    register_set_bits(&(CANx->IER), CAN_IER_TMEIE | CAN_IER_FMPIE0 | CAN_IER_ERRIE | CAN_IER_LECIE | CAN_IER_BOFIE | CAN_IER_EPVIE | CAN_IER_EWGIE | CAN_IER_FOVIE0 | CAN_IER_FFIE0);
 
     // clear overrun flag on init
-    CAN_obj->RF0R &= ~(CAN_RF0R_FOVR0);
+    CANx->RF0R &= ~(CAN_RF0R_FOVR0);
 
-    llcan_irq_enable(CAN_obj);
+    llcan_irq_enable(CANx);
   }
   return ret;
 }
 
-void llcan_clear_send(CAN_TypeDef *CAN_obj) {
-  CAN_obj->TSR |= CAN_TSR_ABRQ0; // Abort message transmission on error interrupt
-  CAN_obj->MSR |= CAN_MSR_ERRI; // Clear error interrupt
+void llcan_clear_send(CAN_TypeDef *CANx) {
+  CANx->TSR |= CAN_TSR_ABRQ0; // Abort message transmission on error interrupt
+  CANx->MSR |= CAN_MSR_ERRI; // Clear error interrupt
 }
diff --git a/panda/board/stm32fx/llflash.h b/panda/board/stm32fx/llflash.h
index 52f628487f..61adcd4584 100644
--- a/panda/board/stm32fx/llflash.h
+++ b/panda/board/stm32fx/llflash.h
@@ -7,44 +7,22 @@ void flash_unlock(void) {
   FLASH->KEYR = 0xCDEF89AB;
 }
 
-void flash_lock(void) {
-  FLASH->CR |= FLASH_CR_LOCK;
-}
-
-bool flash_erase_sector(uint16_t sector) {
-#ifdef BOOTSTUB
+bool flash_erase_sector(uint8_t sector, bool unlocked) {
   // don't erase the bootloader(sector 0)
-  uint16_t min_sector = 1U;
-  uint16_t max_sector = 11U;
-#else
-  uint16_t min_sector = LOGGING_FLASH_SECTOR_A;
-  uint16_t max_sector = LOGGING_FLASH_SECTOR_B;
-#endif
-
-  bool ret = false;
-  if ((sector >= min_sector) && (sector <= max_sector) && (!flash_is_locked())) {
+  if (sector != 0 && sector < 12 && unlocked) {
     FLASH->CR = (sector << 3) | FLASH_CR_SER;
     FLASH->CR |= FLASH_CR_STRT;
-    while ((FLASH->SR & FLASH_SR_BSY) != 0U);
-    ret = true;
+    while (FLASH->SR & FLASH_SR_BSY);
+    return true;
   }
-  return ret;
+  return false;
 }
 
-void flash_write_word(uint32_t *prog_ptr, uint32_t data) {
-  #ifndef BOOTSTUB
-  // don't write to any region besides the logging region
-  if ((prog_ptr >= (uint32_t *)LOGGING_FLASH_BASE_A) && (prog_ptr < (uint32_t *)(LOGGING_FLASH_BASE_B + LOGGING_FLASH_SECTOR_SIZE))) {
-  #endif
-
-    uint32_t *pp = prog_ptr;
-    FLASH->CR = FLASH_CR_PSIZE_1 | FLASH_CR_PG;
-    *pp = data;
-    while ((FLASH->SR & FLASH_SR_BSY) != 0U);
-
-  #ifndef BOOTSTUB
-  }
-  #endif
+void flash_write_word(void *prog_ptr, uint32_t data) {
+  uint32_t *pp = prog_ptr;
+  FLASH->CR = FLASH_CR_PSIZE_1 | FLASH_CR_PG;
+  *pp = data;
+  while (FLASH->SR & FLASH_SR_BSY);
 }
 
 void flush_write_buffer(void) { }
diff --git a/panda/board/stm32fx/lluart.h b/panda/board/stm32fx/lluart.h
index 4cdd337210..91c24dafca 100644
--- a/panda/board/stm32fx/lluart.h
+++ b/panda/board/stm32fx/lluart.h
@@ -21,31 +21,28 @@ void uart_tx_ring(uart_ring *q){
 }
 
 void uart_rx_ring(uart_ring *q){
-  // Do not read out directly if DMA enabled
-  if (q->dma_rx == false) {
-    ENTER_CRITICAL();
+  ENTER_CRITICAL();
 
-    // Read out RX buffer
-    uint8_t c = q->uart->DR;  // This read after reading SR clears a bunch of interrupts
+  // Read out RX buffer
+  uint8_t c = q->uart->DR;  // This read after reading SR clears a bunch of interrupts
 
-    uint16_t next_w_ptr = (q->w_ptr_rx + 1U) % q->rx_fifo_size;
+  uint16_t next_w_ptr = (q->w_ptr_rx + 1U) % q->rx_fifo_size;
 
-    if ((next_w_ptr == q->r_ptr_rx) && q->overwrite) {
-      // overwrite mode: drop oldest byte
-      q->r_ptr_rx = (q->r_ptr_rx + 1U) % q->rx_fifo_size;
-    }
+  if ((next_w_ptr == q->r_ptr_rx) && q->overwrite) {
+    // overwrite mode: drop oldest byte
+    q->r_ptr_rx = (q->r_ptr_rx + 1U) % q->rx_fifo_size;
+  }
 
-    // Do not overwrite buffer data
-    if (next_w_ptr != q->r_ptr_rx) {
-      q->elems_rx[q->w_ptr_rx] = c;
-      q->w_ptr_rx = next_w_ptr;
-      if (q->callback != NULL) {
-        q->callback(q);
-      }
+  // Do not overwrite buffer data
+  if (next_w_ptr != q->r_ptr_rx) {
+    q->elems_rx[q->w_ptr_rx] = c;
+    q->w_ptr_rx = next_w_ptr;
+    if (q->callback != NULL) {
+      q->callback(q);
     }
-
-    EXIT_CRITICAL();
   }
+
+  EXIT_CRITICAL();
 }
 
 void uart_send_break(uart_ring *u) {
@@ -53,34 +50,6 @@ void uart_send_break(uart_ring *u) {
   u->uart->CR1 |= USART_CR1_SBK;
 }
 
-// This function should be called on:
-// * Half-transfer DMA interrupt
-// * Full-transfer DMA interrupt
-// * UART IDLE detection
-uint32_t prev_w_index = 0;
-void dma_pointer_handler(uart_ring *q, uint32_t dma_ndtr) {
-  ENTER_CRITICAL();
-  uint32_t w_index = (q->rx_fifo_size - dma_ndtr);
-
-  // Check for new data
-  if (w_index != prev_w_index){
-    // Check for overflow
-    if (
-      ((prev_w_index < q->r_ptr_rx) && (q->r_ptr_rx <= w_index)) ||                               // No rollover
-      ((w_index < prev_w_index) && ((q->r_ptr_rx <= w_index) || (prev_w_index < q->r_ptr_rx)))    // Rollover
-    ){
-      // We lost data. Set the new read pointer to the oldest byte still available
-      q->r_ptr_rx = (w_index + 1U) % q->rx_fifo_size;
-    }
-
-    // Set write pointer
-    q->w_ptr_rx = w_index;
-  }
-
-  prev_w_index = w_index;
-  EXIT_CRITICAL();
-}
-
 // This read after reading SR clears all error interrupts. We don't want compiler warnings, nor optimizations
 #define UART_READ_DR(uart) volatile uint8_t t = (uart)->DR; UNUSED(t);
 
@@ -106,103 +75,28 @@ void uart_interrupt_handler(uart_ring *q) {
   // Send if necessary
   uart_tx_ring(q);
 
-  // Run DMA pointer handler if the line is idle
-  if(q->dma_rx && (status & USART_SR_IDLE)){
-    // Reset IDLE flag
-    UART_READ_DR(q->uart)
-
-    if(q == &uart_ring_gps){
-      dma_pointer_handler(&uart_ring_gps, DMA2_Stream5->NDTR);
-    } else {
-      #ifdef DEBUG_UART
-        print("No IDLE dma_pointer_handler implemented for this UART.");
-      #endif
-    }
-  }
-
   EXIT_CRITICAL();
 }
 
-void USART1_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_gps); }
 void USART2_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_debug); }
 void USART3_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_lin2); }
 void UART5_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_lin1); }
 
-void DMA2_Stream5_IRQ_Handler(void) {
-  ENTER_CRITICAL();
-
-  // Handle errors
-  if((DMA2->HISR & DMA_HISR_TEIF5) || (DMA2->HISR & DMA_HISR_DMEIF5) || (DMA2->HISR & DMA_HISR_FEIF5)){
-    #ifdef DEBUG_UART
-      print("Encountered UART DMA error. Clearing and restarting DMA...\n");
-    #endif
-
-    // Clear flags
-    DMA2->HIFCR = DMA_HIFCR_CTEIF5 | DMA_HIFCR_CDMEIF5 | DMA_HIFCR_CFEIF5;
-
-    // Re-enable the DMA if necessary
-    DMA2_Stream5->CR |= DMA_SxCR_EN;
-  }
-
-  // Re-calculate write pointer and reset flags
-  dma_pointer_handler(&uart_ring_gps, DMA2_Stream5->NDTR);
-  DMA2->HIFCR = DMA_HIFCR_CTCIF5 | DMA_HIFCR_CHTIF5;
-
-  EXIT_CRITICAL();
-}
-
 // ***************************** Hardware setup *****************************
 
-void dma_rx_init(uart_ring *q) {
-  // Initialization is UART-dependent
-  if(q == &uart_ring_gps){
-    // DMA2, stream 5, channel 4
-
-    // Disable FIFO mode (enable direct)
-    DMA2_Stream5->FCR &= ~DMA_SxFCR_DMDIS;
-
-    // Setup addresses
-    DMA2_Stream5->PAR = (uint32_t)&(USART1->DR);    // Source
-    DMA2_Stream5->M0AR = (uint32_t)q->elems_rx;     // Destination
-    DMA2_Stream5->NDTR = q->rx_fifo_size;           // Number of bytes to copy
-
-    // Circular, Increment memory, byte size, periph -> memory, enable
-    // Transfer complete, half transfer, transfer error and direct mode error interrupt enable
-    DMA2_Stream5->CR = DMA_SxCR_CHSEL_2 | DMA_SxCR_MINC | DMA_SxCR_CIRC | DMA_SxCR_HTIE | DMA_SxCR_TCIE | DMA_SxCR_TEIE | DMA_SxCR_DMEIE | DMA_SxCR_EN;
-
-    // Enable DMA receiver in UART
-    q->uart->CR3 |= USART_CR3_DMAR;
-
-    // Enable UART IDLE interrupt
-    q->uart->CR1 |= USART_CR1_IDLEIE;
-
-    // Enable interrupt
-    NVIC_EnableIRQ(DMA2_Stream5_IRQn);
-  } else {
-    print("Tried to initialize RX DMA for an unsupported UART\n");
-  }
-}
-
 #define __DIV(_PCLK_, _BAUD_)                    (((_PCLK_) * 25U) / (4U * (_BAUD_)))
 #define __DIVMANT(_PCLK_, _BAUD_)                (__DIV((_PCLK_), (_BAUD_)) / 100U)
 #define __DIVFRAQ(_PCLK_, _BAUD_)                ((((__DIV((_PCLK_), (_BAUD_)) - (__DIVMANT((_PCLK_), (_BAUD_)) * 100U)) * 16U) + 50U) / 100U)
 #define __USART_BRR(_PCLK_, _BAUD_)              ((__DIVMANT((_PCLK_), (_BAUD_)) << 4) | (__DIVFRAQ((_PCLK_), (_BAUD_)) & 0x0FU))
 
 void uart_set_baud(USART_TypeDef *u, unsigned int baud) {
-  if (u == USART1) {
-    // USART1 is on APB2
-    u->BRR = __USART_BRR(48000000U, baud);
-  } else {
-    u->BRR = __USART_BRR(24000000U, baud);
-  }
+  u->BRR = __USART_BRR(APB1_FREQ*1000000U, baud);
 }
 
 void uart_init(uart_ring *q, int baud) {
   if(q->uart != NULL){
     // Register interrupts (max data rate: 115200 baud)
-    if(q->uart == USART1){
-      REGISTER_INTERRUPT(USART1_IRQn, USART1_IRQ_Handler, 150000U, FAULT_INTERRUPT_RATE_UART_1)
-    } else if (q->uart == USART2){
+    if (q->uart == USART2){
       REGISTER_INTERRUPT(USART2_IRQn, USART2_IRQ_Handler, 150000U, FAULT_INTERRUPT_RATE_UART_2)
     } else if (q->uart == USART3){
       REGISTER_INTERRUPT(USART3_IRQn, USART3_IRQ_Handler, 150000U, FAULT_INTERRUPT_RATE_UART_3)
@@ -211,9 +105,6 @@ void uart_init(uart_ring *q, int baud) {
     } else {
       // UART not used. Skip registering interrupts
     }
-    if(q->dma_rx){
-      REGISTER_INTERRUPT(DMA2_Stream5_IRQn, DMA2_Stream5_IRQ_Handler, 100U, FAULT_INTERRUPT_RATE_UART_DMA)   // Called twice per buffer
-    }
 
     // Set baud and enable peripheral with TX and RX mode
     uart_set_baud(q->uart, baud);
@@ -223,9 +114,7 @@ void uart_init(uart_ring *q, int baud) {
     }
 
     // Enable UART interrupts
-    if(q->uart == USART1){
-      NVIC_EnableIRQ(USART1_IRQn);
-    } else if (q->uart == USART2){
+    if (q->uart == USART2){
       NVIC_EnableIRQ(USART2_IRQn);
     } else if (q->uart == USART3){
       NVIC_EnableIRQ(USART3_IRQn);
@@ -234,10 +123,5 @@ void uart_init(uart_ring *q, int baud) {
     } else {
       // UART not used. Skip enabling interrupts
     }
-
-    // Initialise RX DMA if used
-    if(q->dma_rx){
-      dma_rx_init(q);
-    }
   }
 }
diff --git a/panda/board/stm32fx/peripherals.h b/panda/board/stm32fx/peripherals.h
index 3eeebf0eb6..5bdb8a056c 100644
--- a/panda/board/stm32fx/peripherals.h
+++ b/panda/board/stm32fx/peripherals.h
@@ -36,10 +36,6 @@ void common_init_gpio(void) {
 
   gpio_usb_init();
 
-  // A9,A10: USART 1 for talking to the GPS
-  set_gpio_alternate(GPIOA, 9, GPIO_AF7_USART1);
-  set_gpio_alternate(GPIOA, 10, GPIO_AF7_USART1);
-
    // B8,B9: CAN 1
   #ifdef STM32F4
     set_gpio_alternate(GPIOB, 8, GPIO_AF8_CAN1);
@@ -59,16 +55,23 @@ void flasher_peripherals_init(void) {
 
 // Peripheral initialization
 void peripherals_init(void) {
-  // enable GPIOB, UART2, CAN, USB clock
+  // enable GPIO(A,B,C,D)
   RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
   RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
   RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;
   RCC->AHB1ENR |= RCC_AHB1ENR_GPIODEN;
 
+  // Supplemental
   RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;
+  RCC->APB1ENR |= RCC_APB1ENR_PWREN;   // for RTC config
+  RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN;
+
+  // Connectivity
+  RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
+  RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN;
   RCC->APB1ENR |= RCC_APB1ENR_USART2EN;
   RCC->APB1ENR |= RCC_APB1ENR_USART3EN;
-  #ifdef PANDA
+  #ifndef PEDAL
     RCC->APB1ENR |= RCC_APB1ENR_UART5EN;
   #endif
   RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
@@ -76,21 +79,20 @@ void peripherals_init(void) {
   #ifdef CAN3
     RCC->APB1ENR |= RCC_APB1ENR_CAN3EN;
   #endif
+
+  // Analog
+  RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;
   RCC->APB1ENR |= RCC_APB1ENR_DACEN;
+
+  // Timers
+  RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;  // clock source timer
   RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;  // main counter
   RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;  // pedal and fan PWM
   RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;  // IR PWM
   RCC->APB1ENR |= RCC_APB1ENR_TIM5EN;  // k-line init
   RCC->APB1ENR |= RCC_APB1ENR_TIM6EN;  // interrupt timer
-  RCC->APB1ENR |= RCC_APB1ENR_TIM12EN; // gmlan_alt
-  RCC->APB1ENR |= RCC_APB1ENR_PWREN;   // for RTC config
-  RCC->APB2ENR |= RCC_APB2ENR_USART1EN;
-  RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN;
-  RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;  // clock source timer
-  RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;
-  RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
-  RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN;
   RCC->APB2ENR |= RCC_APB2ENR_TIM9EN;  // slow loop
+  RCC->APB1ENR |= RCC_APB1ENR_TIM12EN; // gmlan_alt
 }
 
 void enable_interrupt_timer(void) {
diff --git a/panda/board/stm32fx/stm32fx_config.h b/panda/board/stm32fx/stm32fx_config.h
index 1573426da8..4d382e0e74 100644
--- a/panda/board/stm32fx/stm32fx_config.h
+++ b/panda/board/stm32fx/stm32fx_config.h
@@ -28,6 +28,9 @@
 #define TICK_TIMER_IRQ TIM1_BRK_TIM9_IRQn
 #define TICK_TIMER TIM9
 
+#define GMLAN_BITBANG_TIMER_IRQ TIM8_BRK_TIM12_IRQn
+#define GMLAN_BITBANG_TIMER TIM12
+
 #define MICROSECOND_TIMER TIM2
 
 #define INTERRUPT_TIMER_IRQ TIM6_DAC_IRQn
@@ -38,20 +41,14 @@
 #define PROVISION_CHUNK_ADDRESS 0x1FFF79E0U
 #define DEVICE_SERIAL_NUMBER_ADDRESS 0x1FFF79C0U
 
-#define LOGGING_FLASH_SECTOR_A 10U
-#define LOGGING_FLASH_SECTOR_B 11U
-#define LOGGING_FLASH_BASE_A 0x080C0000U
-#define LOGGING_FLASH_BASE_B 0x080E0000U
-#define LOGGING_FLASH_SECTOR_SIZE 0x20000U
-
 #include "can_definitions.h"
 #include "comms_definitions.h"
 
 #ifndef BOOTSTUB
-  #ifdef PANDA
-    #include "main_declarations.h"
-  #else
+  #ifdef PEDAL
     #include "pedal/main_declarations.h"
+  #else
+    #include "main_declarations.h"
   #endif
 #else
   #include "bootstub_declarations.h"
@@ -71,27 +68,28 @@
 #include "stm32fx/board.h"
 #include "stm32fx/clock.h"
 #include "drivers/watchdog.h"
-#include "stm32fx/llflash.h"
 
-#if defined(PANDA) || defined(BOOTSTUB)
+#if !defined(PEDAL) || defined(BOOTSTUB)
   #include "drivers/spi.h"
   #include "stm32fx/llspi.h"
 #endif
 
-#if !defined(BOOTSTUB) && (defined(PANDA) || defined(PEDAL_USB))
+#if !defined(BOOTSTUB) && (!defined(PEDAL) || defined(PEDAL_USB))
   #include "drivers/uart.h"
   #include "stm32fx/lluart.h"
 #endif
 
-#if !defined(PEDAL_USB) && !defined(PEDAL) && !defined(BOOTSTUB)
+#if defined(PANDA) && !defined(BOOTSTUB)
   #include "stm32fx/llexti.h"
 #endif
 
-#ifndef BOOTSTUB
+#ifdef BOOTSTUB
+  #include "stm32fx/llflash.h"
+#else
   #include "stm32fx/llbxcan.h"
 #endif
 
-#if defined(PANDA) || defined(BOOTSTUB) || defined(PEDAL_USB)
+#if !defined(PEDAL) || defined(PEDAL_USB) || defined(BOOTSTUB)
   #include "stm32fx/llusb.h"
 #endif
 
diff --git a/panda/board/stm32h7/clock.h b/panda/board/stm32h7/clock.h
index b1846da261..c5f93cd02e 100644
--- a/panda/board/stm32h7/clock.h
+++ b/panda/board/stm32h7/clock.h
@@ -56,9 +56,6 @@ void clock_init(void) {
   // Set SysClock source to PLL
   register_set(&(RCC->CFGR), RCC_CFGR_SW_PLL1, 0x7U);
   while((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL1);
-
-  // SYSCFG peripheral clock enable
-  register_set_bits(&(RCC->AHB4ENR), RCC_APB4ENR_SYSCFGEN);
   //////////////END OTHER CLOCKS////////////////////
 
   // Configure clock source for USB (HSI48)
@@ -71,8 +68,4 @@ void clock_init(void) {
   register_set_bits(&(RCC->CR), RCC_CR_CSSHSEON);
   //Enable Vdd33usb supply level detector
   register_set_bits(&(PWR->CR3), PWR_CR3_USB33DEN);
-
-  // Enable CPU access to SRAM1 and SRAM2 (in domain D2)
-  register_set_bits(&(RCC->AHB2ENR), RCC_AHB2ENR_SRAM1EN);
-  register_set_bits(&(RCC->AHB2ENR), RCC_AHB2ENR_SRAM2EN);
 }
diff --git a/panda/board/stm32h7/lladc.h b/panda/board/stm32h7/lladc.h
index 29d5e19d69..0a52b78d37 100644
--- a/panda/board/stm32h7/lladc.h
+++ b/panda/board/stm32h7/lladc.h
@@ -18,12 +18,12 @@ void adc_init(void) {
 }
 
 uint16_t adc_get_raw(uint8_t channel) {
-
   ADC1->SQR1 &= ~(ADC_SQR1_L);
   ADC1->SQR1 = ((uint32_t) channel << 6U);
 
-  ADC1->SMPR1 = (0x7U << (channel * 3U) );
+  ADC1->SMPR1 = (0x2U << (channel * 3U));
   ADC1->PCSEL_RES0 = (0x1U << channel);
+  ADC1->CFGR2 = (127U << ADC_CFGR2_OVSR_Pos) | (0x7U << ADC_CFGR2_OVSS_Pos) | ADC_CFGR2_ROVSE;
 
   ADC1->CR |= ADC_CR_ADSTART;
   while (!(ADC1->ISR & ADC_ISR_EOC));
diff --git a/panda/board/stm32h7/llfdcan.h b/panda/board/stm32h7/llfdcan.h
index 0382e8ce2e..5f13dfaa9c 100644
--- a/panda/board/stm32h7/llfdcan.h
+++ b/panda/board/stm32h7/llfdcan.h
@@ -20,7 +20,7 @@
 // FDCAN_RX_FIFO_0_EL_CNT + FDCAN_TX_FIFO_EL_CNT can't exceed 47 elements (47 * 72 bytes = 3,384 bytes) per FDCAN module
 
 // RX FIFO 0
-#define FDCAN_RX_FIFO_0_EL_CNT 30UL
+#define FDCAN_RX_FIFO_0_EL_CNT 46UL
 #define FDCAN_RX_FIFO_0_HEAD_SIZE 8UL // bytes
 #define FDCAN_RX_FIFO_0_DATA_SIZE 64UL // bytes
 #define FDCAN_RX_FIFO_0_EL_SIZE (FDCAN_RX_FIFO_0_HEAD_SIZE + FDCAN_RX_FIFO_0_DATA_SIZE)
@@ -28,7 +28,7 @@
 #define FDCAN_RX_FIFO_0_OFFSET 0UL
 
 // TX FIFO
-#define FDCAN_TX_FIFO_EL_CNT 17UL
+#define FDCAN_TX_FIFO_EL_CNT 1UL
 #define FDCAN_TX_FIFO_HEAD_SIZE 8UL // bytes
 #define FDCAN_TX_FIFO_DATA_SIZE 64UL // bytes
 #define FDCAN_TX_FIFO_EL_SIZE (FDCAN_TX_FIFO_HEAD_SIZE + FDCAN_TX_FIFO_DATA_SIZE)
@@ -46,16 +46,16 @@ const uint32_t speeds[] = {100U, 200U, 500U, 1000U, 1250U, 2500U, 5000U, 10000U}
 const uint32_t data_speeds[] = {100U, 200U, 500U, 1000U, 1250U, 2500U, 5000U, 10000U, 20000U, 50000U};
 
 
-bool fdcan_request_init(FDCAN_GlobalTypeDef *CANx) {
+bool fdcan_request_init(FDCAN_GlobalTypeDef *FDCANx) {
   bool ret = true;
   // Exit from sleep mode
-  CANx->CCCR &= ~(FDCAN_CCCR_CSR);
-  while ((CANx->CCCR & FDCAN_CCCR_CSA) == FDCAN_CCCR_CSA);
+  FDCANx->CCCR &= ~(FDCAN_CCCR_CSR);
+  while ((FDCANx->CCCR & FDCAN_CCCR_CSA) == FDCAN_CCCR_CSA);
 
   // Request init
   uint32_t timeout_counter = 0U;
-  CANx->CCCR |= FDCAN_CCCR_INIT;
-  while ((CANx->CCCR & FDCAN_CCCR_INIT) == 0) {
+  FDCANx->CCCR |= FDCAN_CCCR_INIT;
+  while ((FDCANx->CCCR & FDCAN_CCCR_INIT) == 0) {
     // Delay for about 1ms
     delay(10000);
     timeout_counter++;
@@ -68,12 +68,12 @@ bool fdcan_request_init(FDCAN_GlobalTypeDef *CANx) {
   return ret;
 }
 
-bool fdcan_exit_init(FDCAN_GlobalTypeDef *CANx) {
+bool fdcan_exit_init(FDCAN_GlobalTypeDef *FDCANx) {
   bool ret = true;
 
-  CANx->CCCR &= ~(FDCAN_CCCR_INIT);
+  FDCANx->CCCR &= ~(FDCAN_CCCR_INIT);
   uint32_t timeout_counter = 0U;
-  while ((CANx->CCCR & FDCAN_CCCR_INIT) != 0) {
+  while ((FDCANx->CCCR & FDCAN_CCCR_INIT) != 0) {
     // Delay for about 1ms
     delay(10000);
     timeout_counter++;
@@ -86,24 +86,24 @@ bool fdcan_exit_init(FDCAN_GlobalTypeDef *CANx) {
   return ret;
 }
 
-bool llcan_set_speed(FDCAN_GlobalTypeDef *CANx, uint32_t speed, uint32_t data_speed, bool non_iso, bool loopback, bool silent) {
+bool llcan_set_speed(FDCAN_GlobalTypeDef *FDCANx, uint32_t speed, uint32_t data_speed, bool non_iso, bool loopback, bool silent) {
   UNUSED(speed);
-  bool ret = fdcan_request_init(CANx);
+  bool ret = fdcan_request_init(FDCANx);
 
   if (ret) {
     // Enable config change
-    CANx->CCCR |= FDCAN_CCCR_CCE;
+    FDCANx->CCCR |= FDCAN_CCCR_CCE;
 
     //Reset operation mode to Normal
-    CANx->CCCR &= ~(FDCAN_CCCR_TEST);
-    CANx->TEST &= ~(FDCAN_TEST_LBCK);
-    CANx->CCCR &= ~(FDCAN_CCCR_MON);
-    CANx->CCCR &= ~(FDCAN_CCCR_ASM);
-    CANx->CCCR &= ~(FDCAN_CCCR_NISO);
+    FDCANx->CCCR &= ~(FDCAN_CCCR_TEST);
+    FDCANx->TEST &= ~(FDCAN_TEST_LBCK);
+    FDCANx->CCCR &= ~(FDCAN_CCCR_MON);
+    FDCANx->CCCR &= ~(FDCAN_CCCR_ASM);
+    FDCANx->CCCR &= ~(FDCAN_CCCR_NISO);
 
     // TODO: add as a separate safety mode
     // Enable ASM restricted operation(for debug or automatic bitrate switching)
-    //CANx->CCCR |= FDCAN_CCCR_ASM;
+    //FDCANx->CCCR |= FDCAN_CCCR_ASM;
 
     uint8_t prescaler = BITRATE_PRESCALER;
     if (speed < 2500U) {
@@ -118,7 +118,7 @@ bool llcan_set_speed(FDCAN_GlobalTypeDef *CANx, uint32_t speed, uint32_t data_sp
     uint8_t seg2 = CAN_SEG2(tq, sp);
     uint8_t sjw = MIN(127U, seg2);
 
-    CANx->NBTP = (((sjw & 0x7FU)-1U)<<FDCAN_NBTP_NSJW_Pos) | (((seg1 & 0xFFU)-1U)<<FDCAN_NBTP_NTSEG1_Pos) | (((seg2 & 0x7FU)-1U)<<FDCAN_NBTP_NTSEG2_Pos) | (((prescaler & 0x1FFU)-1U)<<FDCAN_NBTP_NBRP_Pos);
+    FDCANx->NBTP = (((sjw & 0x7FU)-1U)<<FDCAN_NBTP_NSJW_Pos) | (((seg1 & 0xFFU)-1U)<<FDCAN_NBTP_NTSEG1_Pos) | (((seg2 & 0x7FU)-1U)<<FDCAN_NBTP_NTSEG2_Pos) | (((prescaler & 0x1FFU)-1U)<<FDCAN_NBTP_NBRP_Pos);
 
     // Set the data bit timing values
     if (data_speed == 50000U) {
@@ -131,103 +131,103 @@ bool llcan_set_speed(FDCAN_GlobalTypeDef *CANx, uint32_t speed, uint32_t data_sp
     seg2 = CAN_SEG2(tq, sp);
     sjw = MIN(15U, seg2);
 
-    CANx->DBTP = (((sjw & 0xFU)-1U)<<FDCAN_DBTP_DSJW_Pos) | (((seg1 & 0x1FU)-1U)<<FDCAN_DBTP_DTSEG1_Pos) | (((seg2 & 0xFU)-1U)<<FDCAN_DBTP_DTSEG2_Pos) | (((prescaler & 0x1FU)-1U)<<FDCAN_DBTP_DBRP_Pos);
+    FDCANx->DBTP = (((sjw & 0xFU)-1U)<<FDCAN_DBTP_DSJW_Pos) | (((seg1 & 0x1FU)-1U)<<FDCAN_DBTP_DTSEG1_Pos) | (((seg2 & 0xFU)-1U)<<FDCAN_DBTP_DTSEG2_Pos) | (((prescaler & 0x1FU)-1U)<<FDCAN_DBTP_DBRP_Pos);
 
     if (non_iso) {
       // FD non-ISO mode
-      CANx->CCCR |= FDCAN_CCCR_NISO;
+      FDCANx->CCCR |= FDCAN_CCCR_NISO;
     }
 
     // Silent loopback is known as internal loopback in the docs
     if (loopback) {
-      CANx->CCCR |= FDCAN_CCCR_TEST;
-      CANx->TEST |= FDCAN_TEST_LBCK;
-      CANx->CCCR |= FDCAN_CCCR_MON;
+      FDCANx->CCCR |= FDCAN_CCCR_TEST;
+      FDCANx->TEST |= FDCAN_TEST_LBCK;
+      FDCANx->CCCR |= FDCAN_CCCR_MON;
     }
     // Silent is known as bus monitoring in the docs
     if (silent) {
-      CANx->CCCR |= FDCAN_CCCR_MON;
+      FDCANx->CCCR |= FDCAN_CCCR_MON;
     }
-    ret = fdcan_exit_init(CANx);
+    ret = fdcan_exit_init(FDCANx);
     if (!ret) {
-      print(CAN_NAME_FROM_CANIF(CANx)); print(" set_speed timed out! (2)\n");
+      print(CAN_NAME_FROM_CANIF(FDCANx)); print(" set_speed timed out! (2)\n");
     }
   } else {
-    print(CAN_NAME_FROM_CANIF(CANx)); print(" set_speed timed out! (1)\n");
+    print(CAN_NAME_FROM_CANIF(FDCANx)); print(" set_speed timed out! (1)\n");
   }
   return ret;
 }
 
-void llcan_irq_disable(FDCAN_GlobalTypeDef *CANx) {
-  if (CANx == FDCAN1) {
+void llcan_irq_disable(FDCAN_GlobalTypeDef *FDCANx) {
+  if (FDCANx == FDCAN1) {
     NVIC_DisableIRQ(FDCAN1_IT0_IRQn);
     NVIC_DisableIRQ(FDCAN1_IT1_IRQn);
-  } else if (CANx == FDCAN2) {
+  } else if (FDCANx == FDCAN2) {
     NVIC_DisableIRQ(FDCAN2_IT0_IRQn);
     NVIC_DisableIRQ(FDCAN2_IT1_IRQn);
-  } else if (CANx == FDCAN3) {
+  } else if (FDCANx == FDCAN3) {
     NVIC_DisableIRQ(FDCAN3_IT0_IRQn);
     NVIC_DisableIRQ(FDCAN3_IT1_IRQn);
   } else {
   }
 }
 
-void llcan_irq_enable(FDCAN_GlobalTypeDef *CANx) {
-  if (CANx == FDCAN1) {
+void llcan_irq_enable(FDCAN_GlobalTypeDef *FDCANx) {
+  if (FDCANx == FDCAN1) {
     NVIC_EnableIRQ(FDCAN1_IT0_IRQn);
     NVIC_EnableIRQ(FDCAN1_IT1_IRQn);
-  } else if (CANx == FDCAN2) {
+  } else if (FDCANx == FDCAN2) {
     NVIC_EnableIRQ(FDCAN2_IT0_IRQn);
     NVIC_EnableIRQ(FDCAN2_IT1_IRQn);
-  } else if (CANx == FDCAN3) {
+  } else if (FDCANx == FDCAN3) {
     NVIC_EnableIRQ(FDCAN3_IT0_IRQn);
     NVIC_EnableIRQ(FDCAN3_IT1_IRQn);
   } else {
   }
 }
 
-bool llcan_init(FDCAN_GlobalTypeDef *CANx) {
-  uint32_t can_number = CAN_NUM_FROM_CANIF(CANx);
-  bool ret = fdcan_request_init(CANx);
+bool llcan_init(FDCAN_GlobalTypeDef *FDCANx) {
+  uint32_t can_number = CAN_NUM_FROM_CANIF(FDCANx);
+  bool ret = fdcan_request_init(FDCANx);
 
   if (ret) {
     // Enable config change
-    CANx->CCCR |= FDCAN_CCCR_CCE;
+    FDCANx->CCCR |= FDCAN_CCCR_CCE;
     // Enable automatic retransmission
-    CANx->CCCR &= ~(FDCAN_CCCR_DAR);
+    FDCANx->CCCR &= ~(FDCAN_CCCR_DAR);
     // Enable transmission pause feature
-    CANx->CCCR |= FDCAN_CCCR_TXP;
+    FDCANx->CCCR |= FDCAN_CCCR_TXP;
     // Disable protocol exception handling
-    CANx->CCCR |= FDCAN_CCCR_PXHD;
+    FDCANx->CCCR |= FDCAN_CCCR_PXHD;
     // FD with BRS
-    CANx->CCCR |= (FDCAN_CCCR_FDOE | FDCAN_CCCR_BRSE);
+    FDCANx->CCCR |= (FDCAN_CCCR_FDOE | FDCAN_CCCR_BRSE);
 
     // Set TX mode to FIFO
-    CANx->TXBC &= ~(FDCAN_TXBC_TFQM);
+    FDCANx->TXBC &= ~(FDCAN_TXBC_TFQM);
     // Configure TX element data size
-    CANx->TXESC |= 0x7U << FDCAN_TXESC_TBDS_Pos; // 64 bytes
+    FDCANx->TXESC |= 0x7U << FDCAN_TXESC_TBDS_Pos; // 64 bytes
     //Configure RX FIFO0 element data size
-    CANx->RXESC |= 0x7U << FDCAN_RXESC_F0DS_Pos;
+    FDCANx->RXESC |= 0x7U << FDCAN_RXESC_F0DS_Pos;
     // Disable filtering, accept all valid frames received
-    CANx->XIDFC &= ~(FDCAN_XIDFC_LSE); // No extended filters
-    CANx->SIDFC &= ~(FDCAN_SIDFC_LSS); // No standard filters
-    CANx->GFC &= ~(FDCAN_GFC_RRFE); // Accept extended remote frames
-    CANx->GFC &= ~(FDCAN_GFC_RRFS); // Accept standard remote frames
-    CANx->GFC &= ~(FDCAN_GFC_ANFE); // Accept extended frames to FIFO 0
-    CANx->GFC &= ~(FDCAN_GFC_ANFS); // Accept standard frames to FIFO 0
+    FDCANx->XIDFC &= ~(FDCAN_XIDFC_LSE); // No extended filters
+    FDCANx->SIDFC &= ~(FDCAN_SIDFC_LSS); // No standard filters
+    FDCANx->GFC &= ~(FDCAN_GFC_RRFE); // Accept extended remote frames
+    FDCANx->GFC &= ~(FDCAN_GFC_RRFS); // Accept standard remote frames
+    FDCANx->GFC &= ~(FDCAN_GFC_ANFE); // Accept extended frames to FIFO 0
+    FDCANx->GFC &= ~(FDCAN_GFC_ANFS); // Accept standard frames to FIFO 0
 
     uint32_t RxFIFO0SA = FDCAN_START_ADDRESS + (can_number * FDCAN_OFFSET);
     uint32_t TxFIFOSA = RxFIFO0SA + (FDCAN_RX_FIFO_0_EL_CNT * FDCAN_RX_FIFO_0_EL_SIZE);
 
     // RX FIFO 0
-    CANx->RXF0C |= (FDCAN_RX_FIFO_0_OFFSET + (can_number * FDCAN_OFFSET_W)) << FDCAN_RXF0C_F0SA_Pos;
-    CANx->RXF0C |= FDCAN_RX_FIFO_0_EL_CNT << FDCAN_RXF0C_F0S_Pos;
+    FDCANx->RXF0C |= (FDCAN_RX_FIFO_0_OFFSET + (can_number * FDCAN_OFFSET_W)) << FDCAN_RXF0C_F0SA_Pos;
+    FDCANx->RXF0C |= FDCAN_RX_FIFO_0_EL_CNT << FDCAN_RXF0C_F0S_Pos;
     // RX FIFO 0 switch to non-blocking (overwrite) mode
-    CANx->RXF0C |= FDCAN_RXF0C_F0OM;
+    FDCANx->RXF0C |= FDCAN_RXF0C_F0OM;
 
     // TX FIFO (mode set earlier)
-    CANx->TXBC |= (FDCAN_TX_FIFO_OFFSET + (can_number * FDCAN_OFFSET_W)) << FDCAN_TXBC_TBSA_Pos;
-    CANx->TXBC |= FDCAN_TX_FIFO_EL_CNT << FDCAN_TXBC_TFQS_Pos;
+    FDCANx->TXBC |= (FDCAN_TX_FIFO_OFFSET + (can_number * FDCAN_OFFSET_W)) << FDCAN_TXBC_TBSA_Pos;
+    FDCANx->TXBC |= FDCAN_TX_FIFO_EL_CNT << FDCAN_TXBC_TFQS_Pos;
 
     // Flush allocated RAM
     uint32_t EndAddress = TxFIFOSA + (FDCAN_TX_FIFO_EL_CNT * FDCAN_TX_FIFO_EL_SIZE);
@@ -236,34 +236,34 @@ bool llcan_init(FDCAN_GlobalTypeDef *CANx) {
     }
 
     // Enable both interrupts for each module
-    CANx->ILE = (FDCAN_ILE_EINT0 | FDCAN_ILE_EINT1);
+    FDCANx->ILE = (FDCAN_ILE_EINT0 | FDCAN_ILE_EINT1);
 
-    CANx->IE &= 0x0U; // Reset all interrupts
+    FDCANx->IE &= 0x0U; // Reset all interrupts
     // Messages for INT0
-    CANx->IE |= FDCAN_IE_RF0NE; // Rx FIFO 0 new message
-    CANx->IE |= FDCAN_IE_PEDE | FDCAN_IE_PEAE | FDCAN_IE_BOE | FDCAN_IE_EPE | FDCAN_IE_RF0LE;
+    FDCANx->IE |= FDCAN_IE_RF0NE; // Rx FIFO 0 new message
+    FDCANx->IE |= FDCAN_IE_PEDE | FDCAN_IE_PEAE | FDCAN_IE_BOE | FDCAN_IE_EPE | FDCAN_IE_RF0LE;
 
     // Messages for INT1 (Only TFE works??)
-    CANx->ILS |= FDCAN_ILS_TFEL;
-    CANx->IE |= FDCAN_IE_TFEE; // Tx FIFO empty
+    FDCANx->ILS |= FDCAN_ILS_TFEL;
+    FDCANx->IE |= FDCAN_IE_TFEE; // Tx FIFO empty
 
-    ret = fdcan_exit_init(CANx);
+    ret = fdcan_exit_init(FDCANx);
     if(!ret) {
-      print(CAN_NAME_FROM_CANIF(CANx)); print(" llcan_init timed out (2)!\n");
+      print(CAN_NAME_FROM_CANIF(FDCANx)); print(" llcan_init timed out (2)!\n");
     }
 
-    llcan_irq_enable(CANx);
+    llcan_irq_enable(FDCANx);
 
   } else {
-    print(CAN_NAME_FROM_CANIF(CANx)); print(" llcan_init timed out (1)!\n");
+    print(CAN_NAME_FROM_CANIF(FDCANx)); print(" llcan_init timed out (1)!\n");
   }
   return ret;
 }
 
-void llcan_clear_send(FDCAN_GlobalTypeDef *CANx) {
+void llcan_clear_send(FDCAN_GlobalTypeDef *FDCANx) {
   // from datasheet: "Transmit cancellation is not intended for Tx FIFO operation."
   // so we need to clear pending transmission manually by resetting FDCAN core
-  CANx->IR |= 0x3FCFFFFFU; // clear all interrupts
-  bool ret = llcan_init(CANx);
+  FDCANx->IR |= 0x3FCFFFFFU; // clear all interrupts
+  bool ret = llcan_init(FDCANx);
   UNUSED(ret);
 }
diff --git a/panda/board/stm32h7/llflash.h b/panda/board/stm32h7/llflash.h
index 800d9414b4..b95011a9ed 100644
--- a/panda/board/stm32h7/llflash.h
+++ b/panda/board/stm32h7/llflash.h
@@ -7,49 +7,27 @@ void flash_unlock(void) {
   FLASH->KEYR1 = 0xCDEF89AB;
 }
 
-void flash_lock(void) {
-  FLASH->CR1 |= FLASH_CR_LOCK;
-}
-
-bool flash_erase_sector(uint16_t sector) {
-  #ifdef BOOTSTUB
-    // don't erase the bootloader(sector 0)
-    uint16_t min_sector = 1U;
-    uint16_t max_sector = 7U;
-  #else
-    uint16_t min_sector = LOGGING_FLASH_SECTOR_A;
-    uint16_t max_sector = LOGGING_FLASH_SECTOR_B;
-  #endif
-
-  bool ret = false;
-  if ((sector >= min_sector) && (sector <= max_sector) && (!flash_is_locked())) {
+bool flash_erase_sector(uint8_t sector, bool unlocked) {
+  // don't erase the bootloader(sector 0)
+  if (sector != 0 && sector < 8 && unlocked) {
     FLASH->CR1 = (sector << 8) | FLASH_CR_SER;
     FLASH->CR1 |= FLASH_CR_START;
-    while ((FLASH->SR1 & FLASH_SR_QW) != 0U);
-    ret = true;
+    while (FLASH->SR1 & FLASH_SR_QW);
+    return true;
   }
-  return ret;
+  return false;
 }
 
-void flash_write_word(uint32_t *prog_ptr, uint32_t data) {
-  #ifndef BOOTSTUB
-  // don't write to any region besides the logging region
-  if ((prog_ptr >= (uint32_t *)LOGGING_FLASH_BASE_A) && (prog_ptr < (uint32_t *)(LOGGING_FLASH_BASE_B + LOGGING_FLASH_SECTOR_SIZE))) {
-  #endif
-
-    uint32_t *pp = prog_ptr;
-    FLASH->CR1 |= FLASH_CR_PG;
-    *pp = data;
-    while ((FLASH->SR1 & FLASH_SR_QW) != 0U);
-
-  #ifndef BOOTSTUB
-  }
-  #endif
+void flash_write_word(void *prog_ptr, uint32_t data) {
+  uint32_t *pp = prog_ptr;
+  FLASH->CR1 |= FLASH_CR_PG;
+  *pp = data;
+  while (FLASH->SR1 & FLASH_SR_QW);
 }
 
 void flush_write_buffer(void) {
-  if ((FLASH->SR1 & FLASH_SR_WBNE) != 0U) {
+  if (FLASH->SR1 & FLASH_SR_WBNE) {
     FLASH->CR1 |= FLASH_CR_FW;
-    while ((FLASH->SR1 & FLASH_CR_FW) != 0U);
+    while (FLASH->SR1 & FLASH_CR_FW);
   }
 }
diff --git a/panda/board/stm32h7/lluart.h b/panda/board/stm32h7/lluart.h
index 6d1a363e27..0ad7b6a867 100644
--- a/panda/board/stm32h7/lluart.h
+++ b/panda/board/stm32h7/lluart.h
@@ -1,38 +1,27 @@
-
-void dma_pointer_handler(uart_ring *q, uint32_t dma_ndtr) { UNUSED(q); UNUSED(dma_ndtr); }
-void dma_rx_init(uart_ring *q) { UNUSED(q); }
-
-#define __DIV(_PCLK_, _BAUD_)                    (((_PCLK_) * 25U) / (4U * (_BAUD_)))
-#define __DIVMANT(_PCLK_, _BAUD_)                (__DIV((_PCLK_), (_BAUD_)) / 100U)
-#define __DIVFRAQ(_PCLK_, _BAUD_)                ((((__DIV((_PCLK_), (_BAUD_)) - (__DIVMANT((_PCLK_), (_BAUD_)) * 100U)) * 16U) + 50U) / 100U)
-#define __USART_BRR(_PCLK_, _BAUD_)              ((__DIVMANT((_PCLK_), (_BAUD_)) << 4) | (__DIVFRAQ((_PCLK_), (_BAUD_)) & 0x0FU))
-
 void uart_rx_ring(uart_ring *q){
   // Do not read out directly if DMA enabled
-  if (q->dma_rx == false) {
-    ENTER_CRITICAL();
+  ENTER_CRITICAL();
 
-    // Read out RX buffer
-    uint8_t c = q->uart->RDR;  // This read after reading SR clears a bunch of interrupts
+  // Read out RX buffer
+  uint8_t c = q->uart->RDR;  // This read after reading SR clears a bunch of interrupts
 
-    uint16_t next_w_ptr = (q->w_ptr_rx + 1U) % q->rx_fifo_size;
+  uint16_t next_w_ptr = (q->w_ptr_rx + 1U) % q->rx_fifo_size;
 
-    if ((next_w_ptr == q->r_ptr_rx) && q->overwrite) {
-      // overwrite mode: drop oldest byte
-      q->r_ptr_rx = (q->r_ptr_rx + 1U) % q->rx_fifo_size;
-    }
+  if ((next_w_ptr == q->r_ptr_rx) && q->overwrite) {
+    // overwrite mode: drop oldest byte
+    q->r_ptr_rx = (q->r_ptr_rx + 1U) % q->rx_fifo_size;
+  }
 
-    // Do not overwrite buffer data
-    if (next_w_ptr != q->r_ptr_rx) {
-      q->elems_rx[q->w_ptr_rx] = c;
-      q->w_ptr_rx = next_w_ptr;
-      if (q->callback != NULL) {
-        q->callback(q);
-      }
+  // Do not overwrite buffer data
+  if (next_w_ptr != q->r_ptr_rx) {
+    q->elems_rx[q->w_ptr_rx] = c;
+    q->w_ptr_rx = next_w_ptr;
+    if (q->callback != NULL) {
+      q->callback(q);
     }
-
-    EXIT_CRITICAL();
   }
+
+  EXIT_CRITICAL();
 }
 
 void uart_tx_ring(uart_ring *q){
@@ -96,16 +85,6 @@ void uart_interrupt_handler(uart_ring *q) {
   // Send if necessary
   uart_tx_ring(q);
 
-  // Run DMA pointer handler if the line is idle
-  if(q->dma_rx && (status & USART_ISR_IDLE)){
-    // Reset IDLE flag
-    UART_READ_RDR(q->uart)
-
-    #ifdef DEBUG_UART
-      print("No IDLE dma_pointer_handler implemented for this UART.");
-    #endif
-  }
-
   EXIT_CRITICAL();
 }
 
@@ -115,10 +94,6 @@ void uart_init(uart_ring *q, int baud) {
   if (q->uart == UART7) {
     REGISTER_INTERRUPT(UART7_IRQn, UART7_IRQ_Handler, 150000U, FAULT_INTERRUPT_RATE_UART_7)
 
-    if (q->dma_rx) {
-      // TODO
-    }
-
     uart_set_baud(q->uart, baud);
     q->uart->CR1 = USART_CR1_UE | USART_CR1_TE | USART_CR1_RE;
 
@@ -127,10 +102,5 @@ void uart_init(uart_ring *q, int baud) {
 
     // Enable UART interrupts
     NVIC_EnableIRQ(UART7_IRQn);
-
-    // Initialise RX DMA if used
-    if (q->dma_rx) {
-      dma_rx_init(q);
-    }
   }
 }
diff --git a/panda/board/stm32h7/peripherals.h b/panda/board/stm32h7/peripherals.h
index 7e3b2e5b16..db8c0221e1 100644
--- a/panda/board/stm32h7/peripherals.h
+++ b/panda/board/stm32h7/peripherals.h
@@ -40,19 +40,6 @@ void common_init_gpio(void) {
   set_gpio_mode(GPIOE, 4, MODE_OUTPUT);
   set_gpio_output_type(GPIOE, 4, OUTPUT_TYPE_OPEN_DRAIN);
 
-  // F7,F8,F9,F10: BOARD ID
-  set_gpio_pullup(GPIOF, 7, PULL_NONE);
-  set_gpio_mode(GPIOF, 7, MODE_INPUT);
-
-  set_gpio_pullup(GPIOF, 8, PULL_NONE);
-  set_gpio_mode(GPIOF, 8, MODE_INPUT);
-
-  set_gpio_pullup(GPIOF, 9, PULL_NONE);
-  set_gpio_mode(GPIOF, 9, MODE_INPUT);
-
-  set_gpio_pullup(GPIOF, 10, PULL_NONE);
-  set_gpio_mode(GPIOF, 10, MODE_INPUT);
-
   //C4,A1: OBD_SBU1, OBD_SBU2
   set_gpio_pullup(GPIOC, 4, PULL_NONE);
   set_gpio_mode(GPIOC, 4, MODE_ANALOG);
@@ -110,29 +97,40 @@ void peripherals_init(void) {
   RCC->AHB4ENR |= RCC_AHB4ENR_GPIOFEN;
   RCC->AHB4ENR |= RCC_AHB4ENR_GPIOGEN;
 
-  RCC->APB2ENR |= RCC_APB2ENR_SPI4EN;  // SPI
+  // Enable CPU access to SRAM1 and SRAM2 (in domain D2) for DMA
+  RCC->AHB2ENR |= RCC_AHB2ENR_SRAM1EN | RCC_AHB2ENR_SRAM2EN;
+
+  // Supplemental
   RCC->AHB1ENR |= RCC_AHB1ENR_DMA1EN;  // DAC DMA
   RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;  // SPI DMA
+  RCC->APB4ENR |= RCC_APB4ENR_SYSCFGEN;
+
+  // Connectivity
+  RCC->APB2ENR |= RCC_APB2ENR_SPI4EN;  // SPI
+  RCC->APB1LENR |= RCC_APB1LENR_I2C5EN;  // codec I2C
+  RCC->AHB1ENR |= RCC_AHB1ENR_USB1OTGHSEN; // USB
+  RCC->AHB1LPENR |= RCC_AHB1LPENR_USB1OTGHSLPEN; // USB LP needed for CSleep state(__WFI())
+  RCC->AHB1LPENR &= ~(RCC_AHB1LPENR_USB1OTGHSULPILPEN); // disable USB ULPI
+  RCC->APB1LENR |= RCC_APB1LENR_UART7EN;  // SOM uart
+  RCC->APB1HENR |= RCC_APB1HENR_FDCANEN; // FDCAN core enable
+
+  // Analog
+  RCC->AHB1ENR |= RCC_AHB1ENR_ADC12EN; // Enable ADC12 clocks
+  RCC->APB1LENR |= RCC_APB1LENR_DAC12EN; // DAC
+
+  // Timers
+  RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;  // clock source timer
   RCC->APB1LENR |= RCC_APB1LENR_TIM2EN;  // main counter
   RCC->APB1LENR |= RCC_APB1LENR_TIM3EN;  // fan pwm
   RCC->APB1LENR |= RCC_APB1LENR_TIM6EN;  // interrupt timer
   RCC->APB1LENR |= RCC_APB1LENR_TIM7EN;  // DMA trigger timer
-  RCC->APB1LENR |= RCC_APB1LENR_UART7EN;  // SOM uart
-  RCC->APB1LENR |= RCC_APB1LENR_DAC12EN; // DAC
-  RCC->APB2ENR |= RCC_APB2ENR_TIM8EN;  // tick timer
-  RCC->APB1LENR |= RCC_APB1LENR_TIM12EN;  // slow loop
-  RCC->APB1LENR |= RCC_APB1LENR_I2C5EN;  // codec I2C
-  RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;  // clock source timer
-
-  RCC->APB1HENR |= RCC_APB1HENR_FDCANEN; // FDCAN core enable
-  RCC->AHB1ENR |= RCC_AHB1ENR_ADC12EN; // Enable ADC clocks
+  RCC->APB1LENR |= RCC_APB1LENR_TIM12EN;  // tick timer
+  RCC->APB1LENR |= RCC_APB1LENR_TIM13EN;  // gmlan bitbang timer
 
-  RCC->APB4ENR |= RCC_APB4ENR_SYSCFGEN;
-
-  // HS USB enable, also LP is needed for CSleep state(__WFI())
-  RCC->AHB1ENR |= RCC_AHB1ENR_USB1OTGHSEN;
-  RCC->AHB1LPENR |= RCC_AHB1LPENR_USB1OTGHSLPEN;
-  RCC->AHB1LPENR &= ~(RCC_AHB1LPENR_USB1OTGHSULPILPEN);
+#ifdef PANDA_JUNGLE
+  RCC->AHB3ENR |= RCC_AHB3ENR_SDMMC1EN; // SDMMC
+  RCC->AHB4ENR |= RCC_AHB4ENR_ADC3EN; // Enable ADC3 clocks
+#endif
 }
 
 void enable_interrupt_timer(void) {
diff --git a/panda/board/stm32h7/stm32h7_config.h b/panda/board/stm32h7/stm32h7_config.h
index 4e6f99bfa6..880ee4559d 100644
--- a/panda/board/stm32h7/stm32h7_config.h
+++ b/panda/board/stm32h7/stm32h7_config.h
@@ -36,6 +36,9 @@ separate IRQs for RX and TX.
 #define TICK_TIMER_IRQ TIM8_BRK_TIM12_IRQn
 #define TICK_TIMER TIM12
 
+#define GMLAN_BITBANG_TIMER_IRQ TIM8_UP_TIM13_IRQn
+#define GMLAN_BITBANG_TIMER TIM13
+
 #define MICROSECOND_TIMER TIM2
 
 #define INTERRUPT_TIMER_IRQ TIM6_DAC_IRQn
@@ -46,12 +49,6 @@ separate IRQs for RX and TX.
 #define PROVISION_CHUNK_ADDRESS 0x080FFFE0U
 #define DEVICE_SERIAL_NUMBER_ADDRESS 0x080FFFC0U
 
-#define LOGGING_FLASH_SECTOR_A 5U
-#define LOGGING_FLASH_SECTOR_B 6U
-#define LOGGING_FLASH_BASE_A 0x080A0000U
-#define LOGGING_FLASH_BASE_B 0x080C0000U
-#define LOGGING_FLASH_SECTOR_SIZE 0x20000U
-
 #include "can_definitions.h"
 #include "comms_definitions.h"
 
@@ -73,9 +70,8 @@ separate IRQs for RX and TX.
 #include "stm32h7/interrupt_handlers.h"
 #include "drivers/timers.h"
 #include "drivers/watchdog.h"
-#include "stm32h7/llflash.h"
 
-#if !defined(BOOTSTUB) && defined(PANDA)
+#if !defined(BOOTSTUB)
   #include "drivers/uart.h"
   #include "stm32h7/lluart.h"
 #endif
@@ -87,7 +83,9 @@ separate IRQs for RX and TX.
   #include "stm32h7/llexti.h"
 #endif
 
-#ifndef BOOTSTUB
+#ifdef BOOTSTUB
+  #include "stm32h7/llflash.h"
+#else
   #include "stm32h7/llfdcan.h"
 #endif
 
@@ -98,7 +96,7 @@ separate IRQs for RX and TX.
 
 void early_gpio_float(void) {
   RCC->AHB4ENR = RCC_AHB4ENR_GPIOAEN | RCC_AHB4ENR_GPIOBEN | RCC_AHB4ENR_GPIOCEN | RCC_AHB4ENR_GPIODEN | RCC_AHB4ENR_GPIOEEN | RCC_AHB4ENR_GPIOFEN | RCC_AHB4ENR_GPIOGEN | RCC_AHB4ENR_GPIOHEN;
-  GPIOA->MODER = 0; GPIOB->MODER = 0; GPIOC->MODER = 0; GPIOD->MODER = 0; GPIOE->MODER = 0; GPIOF->MODER = 0; GPIOG->MODER = 0; GPIOH->MODER = 0;
+  GPIOA->MODER = 0xAB000000; GPIOB->MODER = 0; GPIOC->MODER = 0; GPIOD->MODER = 0; GPIOE->MODER = 0; GPIOF->MODER = 0; GPIOG->MODER = 0; GPIOH->MODER = 0;
   GPIOA->ODR = 0; GPIOB->ODR = 0; GPIOC->ODR = 0; GPIOD->ODR = 0; GPIOE->ODR = 0; GPIOF->ODR = 0; GPIOG->ODR = 0; GPIOH->ODR = 0;
   GPIOA->PUPDR = 0; GPIOB->PUPDR = 0; GPIOC->PUPDR = 0; GPIOD->PUPDR = 0; GPIOE->PUPDR = 0; GPIOF->PUPDR = 0; GPIOG->PUPDR = 0; GPIOH->PUPDR = 0;
 }
diff --git a/panda/board/stm32h7/stm32h7x5_flash.ld b/panda/board/stm32h7/stm32h7x5_flash.ld
index 5aef663743..3b6eee5dd0 100644
--- a/panda/board/stm32h7/stm32h7x5_flash.ld
+++ b/panda/board/stm32h7/stm32h7x5_flash.ld
@@ -64,12 +64,17 @@ _Min_Stack_Size = 0x400; /* required amount of stack */
 /* Specify the memory areas */
 MEMORY
 {
+/* RAM */
+BACKUP_SRAM (xrw)  : ORIGIN = 0x38800000, LENGTH = 4K /* Backup SRAM(4kb) */
+SRAM4 (xrw)        : ORIGIN = 0x38000000, LENGTH = 16K /* SRAM4(16kb) best for BDMA and SDMMC1*/
+SRAM12 (xrw)       : ORIGIN = 0x30000000, LENGTH = 32K /* SRAM1(16kb) + SRAM2(16kb), not for BDMA or SDMMC1 */
+AXISRAM (xrw)      : ORIGIN = 0x24000000, LENGTH = 320K /* AXI SRAM */
 DTCMRAM (xrw)      : ORIGIN = 0x20000000, LENGTH = 128K /* DTCM */
-RAM_D1 (xrw)      : ORIGIN = 0x24000000, LENGTH = 320K /* AXI SRAM */
-RAM_D2 (xrw)      : ORIGIN = 0x30000000, LENGTH = 32K /* SRAM1(16kb) + SRAM2(16kb) */
-RAM_D3 (xrw)      : ORIGIN = 0x38000000, LENGTH = 16K /* SRAM4 */
+
+/* Code */
+SYSTEM (rx)        : ORIGIN = 0x1FF00000, LENGTH = 128K /* System memory */
+FLASH (rx)         : ORIGIN = 0x08000000, LENGTH = 1024K
 ITCMRAM (xrw)      : ORIGIN = 0x00000000, LENGTH = 64K
-FLASH (rx)      : ORIGIN = 0x8000000, LENGTH = 1024K
 }
 
 /* Define output sections */
@@ -141,7 +146,7 @@ SECTIONS
   _sidata = LOADADDR(.data);
 
   /* Initialized data sections goes into RAM, load LMA copy after code */
-  .data : 
+  .data :
   {
     . = ALIGN(4);
     _sdata = .;        /* create a global symbol at data start */
@@ -180,17 +185,35 @@ SECTIONS
     . = ALIGN(8);
   } >DTCMRAM
 
-  .ram_d1 (NOLOAD) :
+  .itcmram (NOLOAD) :
+  {
+    . = ALIGN(4);
+    *(.itcmram*)
+  } >ITCMRAM
+
+  .axisram (NOLOAD) :
+  {
+    . = ALIGN(4);
+    *(.axisram*)
+  } >AXISRAM
+
+  .sram12 (NOLOAD) :
+  {
+    . = ALIGN(4);
+    *(.sram12*)
+  } >SRAM12
+
+  .sram4 (NOLOAD) :
   {
     . = ALIGN(4);
-    *(.ram_d1*)
-  } >RAM_D1
+    *(.sram4*)
+  } >SRAM4
 
-  .ram_d2 (NOLOAD) :
+  .backup_sram (NOLOAD) :
   {
     . = ALIGN(4);
-    *(.ram_d2*)
-  } >RAM_D2
+    *(.backup_sram*)
+  } >BACKUP_SRAM
 
   .ARM.attributes 0 : { *(.ARM.attributes) }
 }
diff --git a/panda/python/__init__.py b/panda/python/__init__.py
index 5558a9ab33..ee5ac8d966 100644
--- a/panda/python/__init__.py
+++ b/panda/python/__init__.py
@@ -7,9 +7,8 @@ import struct
 import hashlib
 import binascii
 import datetime
-import warnings
 import logging
-from functools import wraps
+from functools import wraps, partial
 from typing import Optional
 from itertools import accumulate
 
@@ -91,53 +90,21 @@ def unpack_can_buffer(dat):
 
   return (ret, dat)
 
-def ensure_health_packet_version(fn):
-  @wraps(fn)
-  def wrapper(self, *args, **kwargs):
-    if self.health_version < self.HEALTH_PACKET_VERSION:
-      raise RuntimeError("Panda firmware has outdated health packet definition. Reflash panda firmware.")
-    elif self.health_version > self.HEALTH_PACKET_VERSION:
-      raise RuntimeError("Panda python library has outdated health packet definition. Update panda python library.")
-    return fn(self, *args, **kwargs)
-  return wrapper
 
-def ensure_can_packet_version(fn):
+def ensure_version(desc, lib_field, panda_field, fn):
   @wraps(fn)
   def wrapper(self, *args, **kwargs):
-    if self.can_version < self.CAN_PACKET_VERSION:
-      raise RuntimeError("Panda firmware has outdated CAN packet definition. Reflash panda firmware.")
-    elif self.can_version > self.CAN_PACKET_VERSION:
-      raise RuntimeError("Panda python library has outdated CAN packet definition. Update panda python library.")
+    lib_version = getattr(self, lib_field)
+    panda_version = getattr(self, panda_field)
+    if lib_version != panda_version:
+      raise RuntimeError(f"{desc} packet version mismatch: panda's firmware v{panda_version}, library v{lib_version}. Reflash panda.")
     return fn(self, *args, **kwargs)
   return wrapper
+ensure_can_packet_version = partial(ensure_version, "CAN", "CAN_PACKET_VERSION", "can_version")
+ensure_can_health_packet_version = partial(ensure_version, "CAN health", "CAN_HEALTH_PACKET_VERSION", "can_health_version")
+ensure_health_packet_version = partial(ensure_version, "health", "HEALTH_PACKET_VERSION", "health_version")
 
-def ensure_can_health_packet_version(fn):
-  @wraps(fn)
-  def wrapper(self, *args, **kwargs):
-    if self.can_health_version < self.CAN_HEALTH_PACKET_VERSION:
-      raise RuntimeError("Panda firmware has outdated CAN health packet definition. Reflash panda firmware.")
-    elif self.can_health_version > self.CAN_HEALTH_PACKET_VERSION:
-      raise RuntimeError("Panda python library has outdated CAN health packet definition. Update panda python library.")
-    return fn(self, *args, **kwargs)
-  return wrapper
-
-def parse_timestamp(dat):
-  a = struct.unpack("HBBBBBB", dat)
-  if a[0] == 0:
-    return None
 
-  try:
-    return datetime.datetime(a[0], a[1], a[2], a[4], a[5], a[6])
-  except ValueError:
-    return None
-
-def unpack_log(dat):
-  return {
-    'id': struct.unpack("H", dat[:2])[0],
-    'timestamp': parse_timestamp(dat[2:10]),
-    'uptime': struct.unpack("I", dat[10:14])[0],
-    'msg': bytes(dat[14:]).decode('utf-8', 'ignore').strip('\x00'),
-  }
 
 class ALTERNATIVE_EXPERIENCE:
   DEFAULT = 0
@@ -184,6 +151,7 @@ class Panda:
   GMLAN_CAN2 = 1
   GMLAN_CAN3 = 2
 
+  USB_PIDS = (0xddee, 0xddcc)
   REQUEST_IN = usb1.ENDPOINT_IN | usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE
   REQUEST_OUT = usb1.ENDPOINT_OUT | usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE
 
@@ -201,12 +169,12 @@ class Panda:
   CAN_PACKET_VERSION = 4
   HEALTH_PACKET_VERSION = 14
   CAN_HEALTH_PACKET_VERSION = 5
-  HEALTH_STRUCT = struct.Struct("<IIIIIIIIIBBBBBBHBBBHfBBHBHH")
+  HEALTH_STRUCT = struct.Struct("<IIIIIIIIIBBBBBBHBBBHfBBHBHHB")
   CAN_HEALTH_STRUCT = struct.Struct("<BIBBBBBBBBIIIIIIIHHBBBIIII")
 
-  F2_DEVICES = (HW_TYPE_PEDAL, )
-  F4_DEVICES = (HW_TYPE_WHITE_PANDA, HW_TYPE_GREY_PANDA, HW_TYPE_BLACK_PANDA, HW_TYPE_UNO, HW_TYPE_DOS)
-  H7_DEVICES = (HW_TYPE_RED_PANDA, HW_TYPE_RED_PANDA_V2, HW_TYPE_TRES)
+  F2_DEVICES = [HW_TYPE_PEDAL, ]
+  F4_DEVICES = [HW_TYPE_WHITE_PANDA, HW_TYPE_GREY_PANDA, HW_TYPE_BLACK_PANDA, HW_TYPE_UNO, HW_TYPE_DOS]
+  H7_DEVICES = [HW_TYPE_RED_PANDA, HW_TYPE_RED_PANDA_V2, HW_TYPE_TRES]
 
   INTERNAL_DEVICES = (HW_TYPE_UNO, HW_TYPE_DOS, HW_TYPE_TRES)
   HAS_OBD = (HW_TYPE_BLACK_PANDA, HW_TYPE_UNO, HW_TYPE_DOS, HW_TYPE_RED_PANDA, HW_TYPE_RED_PANDA_V2, HW_TYPE_TRES)
@@ -238,6 +206,7 @@ class Panda:
   FLAG_HYUNDAI_CANFD_HDA2 = 16
   FLAG_HYUNDAI_CANFD_ALT_BUTTONS = 32
   FLAG_HYUNDAI_ALT_LIMITS = 64
+  FLAG_HYUNDAI_CANFD_HDA2_ALT_STEERING = 128
 
   FLAG_TESLA_POWERTRAIN = 1
   FLAG_TESLA_LONG_CONTROL = 2
@@ -248,6 +217,11 @@ class Panda:
   FLAG_CHRYSLER_RAM_HD = 2
 
   FLAG_SUBARU_GEN2 = 1
+  FLAG_SUBARU_LONG = 2
+
+  FLAG_SUBARU_PREGLOBAL_REVERSED_DRIVER_TORQUE = 1
+
+  FLAG_NISSAN_ALT_EPS_BUS = 1
 
   FLAG_GM_HW_CAM = 1
   FLAG_GM_HW_CAM_LONG = 2
@@ -279,6 +253,8 @@ class Panda:
     if self._handle_open:
       self._handle.close()
       self._handle_open = False
+      if self._context is not None:
+        self._context.close()
 
   def connect(self, claim=True, wait=False):
     self.close()
@@ -286,9 +262,9 @@ class Panda:
     self._handle = None
     while self._handle is None:
       # try USB first, then SPI
-      self._handle, serial, self.bootstub, bcd = self.usb_connect(self._connect_serial, claim=claim)
+      self._context, self._handle, serial, self.bootstub, bcd = self.usb_connect(self._connect_serial, claim=claim)
       if self._handle is None:
-        self._handle, serial, self.bootstub, bcd = self.spi_connect(self._connect_serial)
+        self._context, self._handle, serial, self.bootstub, bcd = self.spi_connect(self._connect_serial)
       if not wait:
         break
 
@@ -324,8 +300,8 @@ class Panda:
       self.set_heartbeat_disabled()
       self.set_power_save(0)
 
-  @staticmethod
-  def spi_connect(serial, ignore_version=False):
+  @classmethod
+  def spi_connect(cls, serial, ignore_version=False):
     # get UID to confirm slave is present and up
     handle = None
     spi_serial = None
@@ -364,16 +340,16 @@ class Panda:
         err = f"panda protocol mismatch: expected {handle.PROTOCOL_VERSION}, got {spi_version}. reflash panda"
         raise PandaProtocolMismatch(err)
 
-    return handle, spi_serial, bootstub, None
+    return None, handle, spi_serial, bootstub, None
 
-  @staticmethod
-  def usb_connect(serial, claim=True):
+  @classmethod
+  def usb_connect(cls, serial, claim=True):
     handle, usb_serial, bootstub, bcd = None, None, None, None
     context = usb1.USBContext()
     context.open()
     try:
       for device in context.getDeviceList(skip_on_error=True):
-        if device.getVendorID() == 0xbbaa and device.getProductID() in (0xddcc, 0xddee):
+        if device.getVendorID() == 0xbbaa and device.getProductID() in cls.USB_PIDS:
           try:
             this_serial = device.getSerialNumber()
           except Exception:
@@ -383,7 +359,7 @@ class Panda:
             logging.debug("opening device %s %s", this_serial, hex(device.getProductID()))
 
             usb_serial = this_serial
-            bootstub = device.getProductID() == 0xddee
+            bootstub = (device.getProductID() & 0xF0) == 0xe0
             handle = device.open()
             if sys.platform not in ("win32", "cygwin", "msys", "darwin"):
               handle.setAutoDetachKernelDriver(True)
@@ -406,36 +382,36 @@ class Panda:
     else:
       context.close()
 
-    return usb_handle, usb_serial, bootstub, bcd
+    return context, usb_handle, usb_serial, bootstub, bcd
 
-  @staticmethod
-  def list():
-    ret = Panda.usb_list()
-    ret += Panda.spi_list()
+  @classmethod
+  def list(cls): # noqa: A003
+    ret = cls.usb_list()
+    ret += cls.spi_list()
     return list(set(ret))
 
-  @staticmethod
-  def usb_list():
+  @classmethod
+  def usb_list(cls):
     ret = []
     try:
       with usb1.USBContext() as context:
         for device in context.getDeviceList(skip_on_error=True):
-          if device.getVendorID() == 0xbbaa and device.getProductID() in (0xddcc, 0xddee):
+          if device.getVendorID() == 0xbbaa and device.getProductID() in cls.USB_PIDS:
             try:
               serial = device.getSerialNumber()
-              if len(serial) == 24:
+              if len(serial) == 24 or serial == "pedal":
                 ret.append(serial)
               else:
-                warnings.warn(f"found device with panda descriptors but invalid serial: {serial}", RuntimeWarning)
+                logging.warning(f"found device with panda descriptors but invalid serial: {serial}", RuntimeWarning)
             except Exception:
               continue
     except Exception:
       logging.exception("exception while listing pandas")
     return ret
 
-  @staticmethod
-  def spi_list():
-    _, serial, _, _ = Panda.spi_connect(None, ignore_version=True)
+  @classmethod
+  def spi_list(cls):
+    _, _, serial, _, _ = cls.spi_connect(None, ignore_version=True)
     if serial is not None:
       return [serial, ]
     return []
@@ -463,23 +439,17 @@ class Panda:
   def reconnect(self):
     if self._handle_open:
       self.close()
-      time.sleep(1.0)
 
     success = False
     # wait up to 15 seconds
-    for i in range(0, 15):
+    for _ in range(15*10):
       try:
         self.connect()
         success = True
         break
       except Exception:
-        logging.debug("reconnecting is taking %d seconds...", i + 1)
-        try:
-          dfu = PandaDFU(self.get_dfu_serial())
-          dfu.recover()
-        except Exception:
-          pass
-        time.sleep(1.0)
+        pass
+      time.sleep(0.1)
     if not success:
       raise Exception("reconnect failed")
 
@@ -524,6 +494,10 @@ class Panda:
       pass
 
   def flash(self, fn=None, code=None, reconnect=True):
+    if self.up_to_date(fn=fn):
+      logging.debug("flash: already up to date")
+      return
+
     if not fn:
       fn = os.path.join(FW_PATH, self._mcu_type.config.app_fn)
     assert os.path.isfile(fn)
@@ -588,9 +562,10 @@ class Panda:
       serials = Panda.list()
     return True
 
-  def up_to_date(self) -> bool:
+  def up_to_date(self, fn=None) -> bool:
     current = self.get_signature()
-    fn = os.path.join(FW_PATH, self.get_mcu_type().config.app_fn)
+    if fn is None:
+      fn = os.path.join(FW_PATH, self.get_mcu_type().config.app_fn)
     expected = Panda.get_signature_from_firmware(fn)
     return (current == expected)
 
@@ -631,6 +606,7 @@ class Panda:
       "fan_stall_count": a[24],
       "sbu1_voltage_mV": a[25],
       "sbu2_voltage_mV": a[26],
+      "som_reset_triggered": a[27],
     }
 
   @ensure_can_health_packet_version
@@ -772,13 +748,6 @@ class Panda:
   def enable_deepsleep(self):
     self._handle.controlWrite(Panda.REQUEST_OUT, 0xfb, 0, 0, b'')
 
-  def set_esp_power(self, on):
-    self._handle.controlWrite(Panda.REQUEST_OUT, 0xd9, int(on), 0, b'')
-
-  def esp_reset(self, bootmode=0):
-    self._handle.controlWrite(Panda.REQUEST_OUT, 0xda, int(bootmode), 0, b'')
-    time.sleep(0.2)
-
   def set_safety_mode(self, mode=SAFETY_SILENT, param=0):
     self._handle.controlWrite(Panda.REQUEST_OUT, 0xdc, mode, param, b'')
 
@@ -894,7 +863,7 @@ class Panda:
 
   def serial_write(self, port_number, ln):
     ret = 0
-    if type(ln) == str:
+    if isinstance(ln, str):
       ln = bytes(ln, 'utf-8')
     for i in range(0, len(ln), 0x20):
       ret += self._handle.bulkWrite(2, struct.pack("B", port_number) + ln[i:i + 0x20])
@@ -987,7 +956,8 @@ class Panda:
 
   def get_datetime(self):
     dat = self._handle.controlRead(Panda.REQUEST_IN, 0xa0, 0, 0, 8)
-    return parse_timestamp(dat)
+    a = struct.unpack("HBBBBBB", dat)
+    return datetime.datetime(a[0], a[1], a[2], a[4], a[5], a[6])
 
   # ****************** Timer *****************
   def get_microsecond_timer(self):
@@ -1019,14 +989,12 @@ class Panda:
   def set_green_led(self, enabled):
     self._handle.controlWrite(Panda.REQUEST_OUT, 0xf7, int(enabled), 0, b'')
 
-  # ****************** Logging *****************
-  def get_logs(self, last_id=None, get_all=False):
-    assert (last_id is None) or (0 <= last_id < 0xFFFF)
-
-    logs = []
-    dat = self._handle.controlRead(Panda.REQUEST_IN, 0xfd, 1 if get_all else 0, last_id if last_id is not None else 0xFFFF, 0x40)
-    while len(dat) > 0:
-      if len(dat) == 0x40:
-        logs.append(unpack_log(dat))
-      dat = self._handle.controlRead(Panda.REQUEST_IN, 0xfd, 0, 0xFFFF, 0x40)
-    return logs
+  def set_clock_source_period(self, period):
+    self._handle.controlWrite(Panda.REQUEST_OUT, 0xe6, period, 0, b'')
+
+  def force_relay_drive(self, intercept_relay_drive, ignition_relay_drive):
+    self._handle.controlWrite(Panda.REQUEST_OUT, 0xc5, (int(intercept_relay_drive) | int(ignition_relay_drive) << 1), 0, b'')
+
+  def read_som_gpio(self) -> bool:
+    r = self._handle.controlRead(Panda.REQUEST_IN, 0xc6, 0, 0, 1)
+    return r[0] == 1
diff --git a/panda/python/base.py b/panda/python/base.py
index d19a2b8612..ffe9353d62 100644
--- a/panda/python/base.py
+++ b/panda/python/base.py
@@ -15,7 +15,7 @@ class BaseHandle(ABC):
     ...
 
   @abstractmethod
-  def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = TIMEOUT) -> int:
+  def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = TIMEOUT, expect_disconnect: bool = False):
     ...
 
   @abstractmethod
@@ -53,14 +53,9 @@ class BaseSTBootloaderHandle(ABC):
     ...
 
   @abstractmethod
-  def erase_app(self) -> None:
-    ...
-
-  @abstractmethod
-  def erase_bootstub(self) -> None:
+  def erase_sector(self, sector: int) -> None:
     ...
 
   @abstractmethod
   def jump(self, address: int) -> None:
     ...
-
diff --git a/panda/python/canhandle.py b/panda/python/canhandle.py
new file mode 100644
index 0000000000..ff6e625552
--- /dev/null
+++ b/panda/python/canhandle.py
@@ -0,0 +1,53 @@
+import struct
+import signal
+
+from .base import BaseHandle
+
+
+class CanHandle(BaseHandle):
+  def __init__(self, p, bus):
+    self.p = p
+    self.bus = bus
+
+  def transact(self, dat):
+    def _handle_timeout(signum, frame):
+      # will happen on reset or can error
+      raise TimeoutError
+
+    signal.signal(signal.SIGALRM, _handle_timeout)
+    signal.alarm(1)
+
+    try:
+      self.p.isotp_send(1, dat, self.bus, recvaddr=2)
+    finally:
+      signal.alarm(0)
+
+    signal.signal(signal.SIGALRM, _handle_timeout)
+    signal.alarm(1)
+    try:
+      ret = self.p.isotp_recv(2, self.bus, sendaddr=1)
+    finally:
+      signal.alarm(0)
+
+    return ret
+
+  def close(self):
+    pass
+
+  def controlWrite(self, request_type, request, value, index, data, timeout=0, expect_disconnect=False):
+    # ignore data in reply, panda doesn't use it
+    return self.controlRead(request_type, request, value, index, 0, timeout)
+
+  def controlRead(self, request_type, request, value, index, length, timeout=0):
+    dat = struct.pack("HHBBHHH", 0, 0, request_type, request, value, index, length)
+    return self.transact(dat)
+
+  def bulkWrite(self, endpoint, data, timeout=0):
+    if len(data) > 0x10:
+      raise ValueError("Data must not be longer than 0x10")
+    dat = struct.pack("HH", endpoint, len(data)) + data
+    return self.transact(dat)
+
+  def bulkRead(self, endpoint, length, timeout=0):
+    dat = struct.pack("HH", endpoint, 0)
+    return self.transact(dat)
diff --git a/panda/python/ccp.py b/panda/python/ccp.py
index 9183120021..50a5f87a3d 100644
--- a/panda/python/ccp.py
+++ b/panda/python/ccp.py
@@ -1,4 +1,3 @@
-#!/usr/bin/env python3
 import sys
 import time
 import struct
diff --git a/panda/python/constants.py b/panda/python/constants.py
index 16409ac312..8078da3e62 100644
--- a/panda/python/constants.py
+++ b/panda/python/constants.py
@@ -10,35 +10,40 @@ USBPACKET_MAX_SIZE = 0x40
 class McuConfig(NamedTuple):
   mcu: str
   mcu_idcode: int
+  sector_sizes: List[int]
+  sector_count: int  # total sector count, used for MCU identification in DFU mode
   uid_address: int
   block_size: int
-  sector_sizes: List[int]
   serial_number_address: int
   app_address: int
   app_fn: str
   bootstub_address: int
   bootstub_fn: str
 
+  def sector_address(self, i):
+    # assume bootstub is in sector 0
+    return self.bootstub_address + sum(self.sector_sizes[:i])
+
 Fx = (
   0x1FFF7A10,
   0x800,
-  [0x4000 for _ in range(4)] + [0x10000] + [0x20000 for _ in range(11)],
   0x1FFF79C0,
   0x8004000,
   "panda.bin.signed",
   0x8000000,
   "bootstub.panda.bin",
 )
-F2Config = McuConfig("STM32F2", 0x411, *Fx)
-F4Config = McuConfig("STM32F4", 0x463, *Fx)
+F2Config = McuConfig("STM32F2", 0x411, [0x4000 for _ in range(4)] + [0x10000] + [0x20000 for _ in range(7)], 12, *Fx)
+F4Config = McuConfig("STM32F4", 0x463, [0x4000 for _ in range(4)] + [0x10000] + [0x20000 for _ in range(11)], 16, *Fx)
 
 H7Config = McuConfig(
   "STM32H7",
   0x483,
+  [0x20000 for _ in range(7)],
+  8,
   0x1FF1E800,
   0x400,
   # there is an 8th sector, but we use that for the provisioning chunk, so don't program over that!
-  [0x20000 for _ in range(7)],
   0x080FFFC0,
   0x8020000,
   "panda_h7.bin.signed",
diff --git a/panda/python/dfu.py b/panda/python/dfu.py
index bebc243ced..661f9e8f1d 100644
--- a/panda/python/dfu.py
+++ b/panda/python/dfu.py
@@ -14,9 +14,9 @@ class PandaDFU:
   def __init__(self, dfu_serial: Optional[str]):
     # try USB, then SPI
     handle: Optional[BaseSTBootloaderHandle]
-    handle = PandaDFU.usb_connect(dfu_serial)
+    self._context, handle = PandaDFU.usb_connect(dfu_serial)
     if handle is None:
-      handle = PandaDFU.spi_connect(dfu_serial)
+      self._context, handle = PandaDFU.spi_connect(dfu_serial)
 
     if handle is None:
       raise Exception(f"failed to open DFU device {dfu_serial}")
@@ -24,8 +24,21 @@ class PandaDFU:
     self._handle: BaseSTBootloaderHandle = handle
     self._mcu_type: McuType = self._handle.get_mcu_type()
 
+  def __enter__(self):
+    return self
+
+  def __exit__(self, *args):
+    self.close()
+
+  def close(self):
+    if self._handle is not None:
+      self._handle.close()
+      self._handle = None
+      if self._context is not None:
+        self._context.close()
+
   @staticmethod
-  def usb_connect(dfu_serial: Optional[str]) -> Optional[STBootloaderUSBHandle]:
+  def usb_connect(dfu_serial: Optional[str]):
     handle = None
     context = usb1.USBContext()
     context.open()
@@ -40,10 +53,10 @@ class PandaDFU:
           handle = STBootloaderUSBHandle(device, device.open())
           break
 
-    return handle
+    return context, handle
 
   @staticmethod
-  def spi_connect(dfu_serial: Optional[str]) -> Optional[STBootloaderSPIHandle]:
+  def spi_connect(dfu_serial: Optional[str]):
     handle = None
     this_dfu_serial = None
 
@@ -56,10 +69,10 @@ class PandaDFU:
     if dfu_serial is not None and dfu_serial != this_dfu_serial:
       handle = None
 
-    return handle
+    return None, handle
 
   @staticmethod
-  def list() -> List[str]:
+  def list() -> List[str]: # noqa: A003
     ret = PandaDFU.usb_list()
     ret += PandaDFU.spi_list()
     return list(set(ret))
@@ -82,7 +95,7 @@ class PandaDFU:
   @staticmethod
   def spi_list() -> List[str]:
     try:
-      h = PandaDFU.spi_connect(None)
+      _, h = PandaDFU.spi_connect(None)
       if h is not None:
         dfu_serial = PandaDFU.st_serial_to_dfu_serial(h.get_uid(), h.get_mcu_type())
         return [dfu_serial, ]
@@ -108,8 +121,11 @@ class PandaDFU:
 
   def program_bootstub(self, code_bootstub):
     self._handle.clear_status()
-    self._handle.erase_bootstub()
-    self._handle.erase_app()
+
+    # erase all sectors
+    for i in range(len(self._mcu_type.config.sector_sizes)):
+      self._handle.erase_sector(i)
+
     self._handle.program(self._mcu_type.config.bootstub_address, code_bootstub)
 
   def recover(self):
diff --git a/panda/python/isotp.py b/panda/python/isotp.py
index 45acb6abae..3334deb8ed 100644
--- a/panda/python/isotp.py
+++ b/panda/python/isotp.py
@@ -6,10 +6,8 @@ DEBUG = False
 def msg(x):
   if DEBUG:
     print("S:", binascii.hexlify(x))
-  if len(x) <= 7:
-    ret = bytes([len(x)]) + x
-  else:
-    assert False
+  assert len(x) <= 7
+  ret = bytes([len(x)]) + x
   return ret.ljust(8, b"\x00")
 
 kmsgs = []
@@ -56,7 +54,7 @@ def isotp_recv_subaddr(panda, addr, bus, sendaddr, subaddr):
     dat = msg[2:]
   else:
     print(binascii.hexlify(msg))
-    assert False
+    raise AssertionError
 
   return dat[0:tlen]
 
@@ -133,7 +131,7 @@ def isotp_recv(panda, addr, bus=0, sendaddr=None, subaddr=None):
       tlen = msg[0] & 0xf
       dat = msg[1:]
     else:
-      assert False
+      raise AssertionError
     dat = dat[0:tlen]
 
   if DEBUG:
diff --git a/panda/python/serial.py b/panda/python/serial.py
index 97fe8f1d90..9ac58862b5 100644
--- a/panda/python/serial.py
+++ b/panda/python/serial.py
@@ -8,7 +8,7 @@ class PandaSerial(object):
     self.panda.set_uart_baud(self.port, baud)
     self.buf = b""
 
-  def read(self, l=1):  # noqa: E741
+  def read(self, l=1):
     tt = self.panda.serial_read(self.port)
     if len(tt) > 0:
       self.buf += tt
diff --git a/panda/python/spi.py b/panda/python/spi.py
index ad0225b1e0..699431bf0c 100644
--- a/panda/python/spi.py
+++ b/panda/python/spi.py
@@ -17,10 +17,8 @@ from .utils import crc8_pedal
 
 try:
   import spidev
-  import spidev2
 except ImportError:
   spidev = None
-  spidev2 = None
 
 # Constants
 SYNC = 0x5A
@@ -194,6 +192,7 @@ class PandaSpiHandle(BaseHandle):
       return dat[3:-1]
 
   def _transfer_kernel_driver(self, spi, endpoint: int, data, timeout: int, max_rx_len: int = 1000, expect_disconnect: bool = False) -> bytes:
+    import spidev2
     self.tx_buf[:len(data)] = data
     self.ioctl_data.endpoint = endpoint
     self.ioctl_data.tx_length = len(data)
@@ -314,7 +313,7 @@ class STBootloaderSPIHandle(BaseSTBootloaderHandle):
 
       self._mcu_type = MCU_TYPE_BY_IDCODE[self.get_chip_id()]
     except PandaSpiException:
-      raise PandaSpiException("failed to connect to panda")  # pylint: disable=W0707
+      raise PandaSpiException("failed to connect to panda") from None
 
   def _get_ack(self, spi, timeout=1.0):
     data = 0x00
@@ -403,12 +402,6 @@ class STBootloaderSPIHandle(BaseSTBootloaderHandle):
 
   # *** PandaDFU API ***
 
-  def erase_app(self):
-    self.erase_sector(1)
-
-  def erase_bootstub(self):
-    self.erase_sector(0)
-
   def get_mcu_type(self):
     return self._mcu_type
 
@@ -421,7 +414,7 @@ class STBootloaderSPIHandle(BaseSTBootloaderHandle):
   def program(self, address, dat):
     bs = 256  # max block size for writing to flash over SPI
     dat += b"\xFF" * ((bs - len(dat)) % bs)
-    for i in range(0, len(dat) // bs):
+    for i in range(len(dat) // bs):
       block = dat[i * bs:(i + 1) * bs]
       self._cmd(0x31, data=[
         struct.pack('>I', address + i*bs),
diff --git a/panda/python/uds.py b/panda/python/uds.py
index 9b25dffe66..aaa0697f90 100644
--- a/panda/python/uds.py
+++ b/panda/python/uds.py
@@ -1,4 +1,3 @@
-#!/usr/bin/env python3
 import time
 import struct
 from collections import deque
diff --git a/panda/python/usb.py b/panda/python/usb.py
index 0f1bff7c16..0a012896be 100644
--- a/panda/python/usb.py
+++ b/panda/python/usb.py
@@ -34,9 +34,16 @@ class STBootloaderUSBHandle(BaseSTBootloaderHandle):
   def __init__(self, libusb_device, libusb_handle):
     self._libusb_handle = libusb_handle
 
-    # TODO: Find a way to detect F4 vs F2
-    # TODO: also check F4 BCD, don't assume in else
-    self._mcu_type = McuType.H7 if libusb_device.getbcdDevice() == 512 else McuType.F4
+    # example from F4: lsusb -v | grep Flash
+    # iInterface  4 @Internal Flash  /0x08000000/04*016Kg,01*064Kg,011*128Kg
+    for i in range(20):
+      desc = libusb_handle.getStringDescriptor(i, 0)
+      if desc is not None and desc.startswith("@Internal Flash"):
+        sector_count = sum([int(s.split('*')[0]) for s in desc.split('/')[-1].split(',')])
+        break
+    mcu_by_sector_count = {m.config.sector_count: m for m in McuType}
+    assert sector_count in mcu_by_sector_count, f"Unkown MCU: {sector_count=}"
+    self._mcu_type = mcu_by_sector_count[sector_count]
 
   def _status(self) -> None:
     while 1:
@@ -51,11 +58,8 @@ class STBootloaderUSBHandle(BaseSTBootloaderHandle):
   def get_mcu_type(self):
     return self._mcu_type
 
-  def erase_app(self):
-    self._erase_page_address(self._mcu_type.config.app_address)
-
-  def erase_bootstub(self):
-    self._erase_page_address(self._mcu_type.config.bootstub_address)
+  def erase_sector(self, sector: int):
+    self._erase_page_address(self._mcu_type.config.sector_address(sector))
 
   def clear_status(self):
     # Clear status
@@ -78,7 +82,7 @@ class STBootloaderUSBHandle(BaseSTBootloaderHandle):
     # Program
     bs = min(len(dat), self._mcu_type.config.block_size)
     dat += b"\xFF" * ((bs - len(dat)) % bs)
-    for i in range(0, len(dat) // bs):
+    for i in range(len(dat) // bs):
       ldat = dat[i * bs:(i + 1) * bs]
       print("programming %d with length %d" % (i, len(ldat)))
       self._libusb_handle.controlWrite(0x21, self.DFU_DNLOAD, 2 + i, 0, ldat)
diff --git a/pyproject.toml b/pyproject.toml
new file mode 100644
index 0000000000..38c3337c6f
--- /dev/null
+++ b/pyproject.toml
@@ -0,0 +1,199 @@
+[tool.pytest.ini_options]
+minversion = "6.0"
+addopts = "--ignore=openpilot/ --ignore=cereal/ --ignore=opendbc/ --ignore=panda/ --ignore=rednose_repo/ --ignore=tinygrad_repo/ -Werror --strict-config --strict-markers --durations=10 -n auto --dist=loadgroup"
+cpp_files = "test_*"
+python_files = "test_*.py"
+#timeout = "30"  # you get this long by default
+markers = [
+  "slow: tests that take awhile to run and can be skipped with -m 'not slow'",
+  "tici: tests that are only meant to run on the C3/C3X",
+]
+testpaths = [
+  "common",
+  "selfdrive/athena",
+  "selfdrive/boardd",
+  "selfdrive/car",
+  "selfdrive/controls",
+  "selfdrive/locationd",
+  "selfdrive/monitoring",
+  "selfdrive/thermald",
+  "selfdrive/test/longitudinal_maneuvers",
+  "system/hardware/tici",
+  "system/loggerd",
+  "system/proclogd",
+  "system/tests",
+  "system/ubloxd",
+  "tools/lib/tests",
+  "tools/replay",
+  "tools/cabana"
+]
+
+[tool.mypy]
+python_version = "3.11"
+plugins = [
+  "numpy.typing.mypy_plugin",
+]
+exclude = [
+  "body/",
+  "cereal/",
+  "opendbc/",
+  "panda/",
+  "rednose/",
+  "rednose_repo/",
+  "tinygrad/",
+  "tinygrad_repo/",
+  "third_party/",
+]
+
+# third-party packages
+ignore_missing_imports=true
+
+# helpful warnings
+warn_redundant_casts=true
+warn_unreachable=true
+warn_unused_ignores=true
+
+# restrict dynamic typing
+warn_return_any=true
+
+
+[tool.poetry]
+name = "openpilot"
+version = "0.1.0"
+description = "an open source driver assistance system"
+authors = ["Vehicle Researcher <user@comma.ai>"]
+license = "MIT"
+readme = "README.md"
+repository = "https://github.com/commaai/openpilot"
+documentation = "https://docs.comma.ai"
+
+
+[tool.poetry.dependencies]
+python = "~3.11"
+atomicwrites = "*"
+aiohttp = "*"
+aiortc = "*"
+casadi = "==3.6.3"
+cffi = "*"
+control = "*"
+crcmod = "*"
+cryptography = "*"
+Cython = "*"
+future-fstrings = "*"  # for acados
+hexdump = "*"
+Jinja2 = "*"
+json-rpc = "*"
+libusb1 = "*"
+numpy = "*"
+onnx = ">=1.14.0"
+onnxruntime = { version = ">=1.15.1", platform = "linux", markers = "platform_machine == 'aarch64'" }
+onnxruntime-gpu = { version = ">=1.15.1", platform = "linux", markers = "platform_machine == 'x86_64'" }
+psutil = "*"
+pyaudio = "*"
+pycapnp = "*"
+pycryptodome = "*"
+pycurl = "*"
+pydub = "*"
+PyJWT = "*"
+pyopencl = "*"
+pyserial = "*"
+PyYAML = "*"
+pyzmq = "*"
+requests = "*"
+scons = "*"
+sentry-sdk = "==1.28.1"  # needs to be updated with AGNOS
+setproctitle = "*"
+smbus2 = "*"
+sounddevice = "*"
+spidev = { version = "*", platform = "linux" }
+sympy = "*"
+timezonefinder = "*"
+tqdm = "*"
+websocket_client = "*"
+polyline = "*"
+sconscontrib = {git = "https://github.com/SCons/scons-contrib.git"}
+
+
+[tool.poetry.group.dev.dependencies]
+av = "*"
+azure-identity = "*"
+azure-storage-blob = "*"
+breathe = "*"
+coverage = "*"
+dictdiffer = "*"
+ft4222 = "*"
+hypothesis = "~6.47"
+inputs = "*"
+lru-dict = "*"
+markdown-it-py = "*"
+matplotlib = "*"
+metadrive-simulator = { git = "https://github.com/metadriverse/metadrive.git", rev ="72e842cd1d025bf676e4af8797a01e4aa282109f", markers = "platform_machine != 'aarch64'" } # no linux/aarch64 wheels for certain dependencies
+mpld3 = "*"
+mypy = "*"
+myst-parser = "*"
+natsort = "*"
+opencv-python-headless = "*"
+pandas = "*"
+parameterized = "^0.8"
+pprofile = "*"
+pre-commit = "*"
+pygame = "*"
+pympler = "*"
+pyprof2calltree = "*"
+pytest = "*"
+pytest-cov = "*"
+pytest-cpp = "*"
+pytest-subtests = "*"
+pytest-xdist = "*"
+pytest-timeout = "*"
+pytest-timeouts = "*"
+pytest-random-order = "*"
+ruff = "*"
+scipy = "*"
+sphinx = "*"
+sphinx-rtd-theme = "*"
+sphinx-sitemap = "*"
+tabulate = "*"
+tenacity = "*"
+types-atomicwrites = "*"
+types-pycurl = "*"
+types-PyYAML = "*"
+types-requests = "*"
+types-tabulate = "*"
+
+# this is only pinned since 5.15.11 is broken
+pyqt5 = { version = "==5.15.2", markers = "platform_machine == 'x86_64'" } # no aarch64 wheels for macOS/linux
+
+[tool.poetry.group.carla]
+optional = true
+
+[tool.poetry.group.carla.dependencies]
+carla = { url = "https://github.com/commaai/carla/releases/download/3.11.4/carla-0.9.14-cp311-cp311-linux_x86_64.whl", platform = "linux", markers = "platform_machine == 'x86_64'" }
+
+[build-system]
+requires = ["poetry-core"]
+build-backend = "poetry.core.masonry.api"
+
+# https://beta.ruff.rs/docs/configuration/#using-pyprojecttoml
+[tool.ruff]
+select = ["E", "F", "W", "PIE", "C4", "ISC", "RUF008", "RUF100", "A", "B", "TID251"]
+ignore = ["W292", "E741", "E402", "C408", "ISC003", "B027", "B024"]
+line-length = 160
+target-version="py311"
+exclude = [
+  "panda",
+  "opendbc",
+  "rednose_repo",
+  "tinygrad_repo",
+  "third_party",
+]
+flake8-implicit-str-concat.allow-multiline=false
+[tool.ruff.flake8-tidy-imports.banned-api]
+"selfdrive".msg = "Use openpilot.selfdrive"
+"common".msg = "Use openpilot.common"
+"system".msg = "Use openpilot.system"
+"third_party".msg = "Use openpilot.third_party"
+"tools".msg = "Use openpilot.tools"
+
+[tool.coverage.run]
+concurrency = ["multiprocessing", "thread"]
\ No newline at end of file
diff --git a/rednose/helpers/ekf_sym.cc b/rednose/helpers/ekf_sym.cc
index 8a2f2b88a9..cf3c88f12a 100644
--- a/rednose/helpers/ekf_sym.cc
+++ b/rednose/helpers/ekf_sym.cc
@@ -89,7 +89,7 @@ std::optional<Estimate> EKFSym::predict_and_update_batch(double t, int kind, std
   std::deque<Observation> rewound;
   if (!std::isnan(this->filter_time) && t < this->filter_time) {
     if (this->rewind_t.empty() || t < this->rewind_t.front() || t < this->rewind_t.back() - this->max_rewind_age) {
-      LOGD("observation too old at %d with filter at %d, ignoring!", t, this->filter_time);
+      LOGD("observation too old at %f with filter at %f, ignoring!", t, this->filter_time);
       return std::nullopt;
     }
     rewound = this->rewind(t);
diff --git a/rednose/helpers/ekf_sym.py b/rednose/helpers/ekf_sym.py
index b60c202a59..de68991a93 100644
--- a/rednose/helpers/ekf_sym.py
+++ b/rednose/helpers/ekf_sym.py
@@ -192,7 +192,7 @@ def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_p
   for group, kinds in func_lists.items():
     post_code += f"  .{group}s = {{\n"
     for kind in kinds:
-      str_kind = f"\"{kind}\"" if type(kind) == str else kind
+      str_kind = f"\"{kind}\"" if isinstance(kind, str) else kind
       post_code += f"    {{ {str_kind}, {name}_{group}_{kind} }},\n"
     post_code += "  },\n"
   post_code += "  .extra_routines = {\n"
diff --git a/rednose/helpers/kalmanfilter.py b/rednose/helpers/kalmanfilter.py
index 0c30e49164..d541faa34c 100644
--- a/rednose/helpers/kalmanfilter.py
+++ b/rednose/helpers/kalmanfilter.py
@@ -10,7 +10,8 @@ class KalmanFilter:
   Q = np.zeros((0, 0))
   obs_noise: Dict[int, Any] = {}
 
-  filter = None  # Should be initialized when initializating a KalmanFilter implementation
+  # Should be initialized when initializating a KalmanFilter implementation
+  filter = None # noqa: A003
 
   @property
   def x(self):
diff --git a/rednose/helpers/sympy_helpers.py b/rednose/helpers/sympy_helpers.py
index 716e94e018..71a12d03d3 100644
--- a/rednose/helpers/sympy_helpers.py
+++ b/rednose/helpers/sympy_helpers.py
@@ -1,4 +1,3 @@
-#!/usr/bin/env python3
 import sympy as sp
 import numpy as np
 
diff --git a/release/build_devel.sh b/release/build_devel.sh
index 668ac0de19..ca04c56f1e 100755
--- a/release/build_devel.sh
+++ b/release/build_devel.sh
@@ -44,9 +44,6 @@ git clean -xdff
 echo "[-] copying files T=$SECONDS"
 cd $SOURCE_DIR
 cp -pR --parents $(cat release/files_*) $TARGET_DIR/
-if [ ! -z "$EXTRA_FILES" ]; then
-  cp -pR --parents $EXTRA_FILES $TARGET_DIR/
-fi
 
 # in the directory
 cd $TARGET_DIR
diff --git a/release/build_release.sh b/release/build_release.sh
index f222fab26c..b713876cd6 100755
--- a/release/build_release.sh
+++ b/release/build_release.sh
@@ -52,17 +52,13 @@ echo "[-] committing version $VERSION T=$SECONDS"
 git add -f .
 git commit -a -m "openpilot v$VERSION release"
 
-# Build panda firmware
-pushd panda/
-CERT=/data/pandaextra/certs/release RELEASE=1 scons -u .
-mkdir /tmp/panda_obj/
-mv board/obj/panda.bin.signed board/obj/panda_h7.bin.signed board/obj/bootstub.panda.bin board/obj/bootstub.panda_h7.bin /tmp/panda_obj/
-popd
-
 # Build
 export PYTHONPATH="$BUILD_DIR"
 scons -j$(nproc)
 
+# release panda fw
+CERT=/data/pandaextra/certs/release RELEASE=1 scons -j$(nproc) panda/
+
 # Ensure no submodules in release
 if test "$(git submodule--helper list | wc -l)" -gt "0"; then
   echo "submodules found:"
@@ -78,14 +74,9 @@ find . -name '*.os' -delete
 find . -name '*.pyc' -delete
 find . -name 'moc_*' -delete
 find . -name '__pycache__' -delete
-rm -rf panda/board panda/certs panda/crypto
 rm -rf .sconsign.dblite Jenkinsfile release/
 rm selfdrive/modeld/models/supercombo.onnx
 
-# Move back signed panda fw
-mkdir -p panda/board/obj
-mv /tmp/panda_obj/* panda/board/obj/
-
 # Restore third_party
 git checkout third_party/
 
diff --git a/release/files_common b/release/files_common
index a3b3b92f6f..53720aa05a 100644
--- a/release/files_common
+++ b/release/files_common
@@ -6,6 +6,7 @@ launch_openpilot.sh
 
 Jenkinsfile
 SConstruct
+pyproject.toml
 
 README.md
 RELEASES.md
@@ -15,12 +16,14 @@ docs/INTEGRATION.md
 docs/LIMITATIONS.md
 site_scons/site_tools/cython.py
 
+openpilot/__init__.py
+openpilot/**
+
 common/.gitignore
 common/__init__.py
 common/conversions.py
 common/gpio.py
 common/realtime.py
-common/clock.pyx
 common/timeout.py
 common/ffi_wrapper.py
 common/file_helpers.py
@@ -60,6 +63,7 @@ release/*
 
 tools/__init__.py
 tools/lib/*
+tools/bodyteleop/*
 tools/joystick/*
 tools/replay/*.cc
 tools/replay/*.h
@@ -68,7 +72,6 @@ selfdrive/__init__.py
 selfdrive/sentry.py
 selfdrive/tombstoned.py
 selfdrive/updated.py
-selfdrive/rtshield.py
 selfdrive/statsd.py
 
 system/logmessaged.py
@@ -130,10 +133,6 @@ selfdrive/car/tesla/*.py
 selfdrive/car/toyota/*.py
 selfdrive/car/volkswagen/*.py
 
-system/clocksd/.gitignore
-system/clocksd/SConscript
-system/clocksd/clocksd.cc
-
 selfdrive/debug/can_printer.py
 selfdrive/debug/check_freq.py
 selfdrive/debug/dump.py
@@ -159,6 +158,8 @@ common/clutil.cc
 common/clutil.h
 common/params.h
 common/params.cc
+common/ratekeeper.cc
+common/ratekeeper.h
 common/watchdog.cc
 common/watchdog.h
 
@@ -196,14 +197,13 @@ selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore
 selfdrive/controls/lib/lateral_mpc_lib/*
 selfdrive/controls/lib/longitudinal_mpc_lib/*
 
-selfdrive/hardware
-
 system/__init__.py
 
 system/hardware/__init__.py
 system/hardware/base.h
 system/hardware/base.py
 system/hardware/hw.h
+system/hardware/hw.py
 system/hardware/tici/__init__.py
 system/hardware/tici/hardware.h
 system/hardware/tici/hardware.py
@@ -214,6 +214,7 @@ system/hardware/tici/agnos.json
 system/hardware/tici/amplifier.py
 system/hardware/tici/updater
 system/hardware/tici/iwlist.py
+system/hardware/tici/esim.nmconnection
 system/hardware/pc/__init__.py
 system/hardware/pc/hardware.h
 system/hardware/pc/hardware.py
@@ -227,7 +228,6 @@ system/ubloxd/*.cc
 selfdrive/locationd/__init__.py
 selfdrive/locationd/SConscript
 selfdrive/locationd/.gitignore
-selfdrive/locationd/laikad.py
 selfdrive/locationd/locationd.h
 selfdrive/locationd/locationd.cc
 selfdrive/locationd/paramsd.py
@@ -243,6 +243,7 @@ selfdrive/locationd/models/gnss_helpers.py
 
 selfdrive/locationd/torqued.py
 selfdrive/locationd/calibrationd.py
+selfdrive/locationd/helpers.py
 
 system/logcatd/.gitignore
 system/logcatd/SConscript
@@ -281,7 +282,6 @@ system/sensord/SConscript
 system/sensord/sensors_qcom2.cc
 system/sensord/sensors/*.cc
 system/sensord/sensors/*.h
-system/sensord/sensord
 system/sensord/pigeond.py
 
 selfdrive/thermald/thermald.py
@@ -294,6 +294,7 @@ selfdrive/test/helpers.py
 selfdrive/test/setup_device_ci.sh
 selfdrive/test/test_onroad.py
 selfdrive/test/test_time_to_onroad.py
+selfdrive/test/pytest-tici.ini
 
 selfdrive/ui/.gitignore
 selfdrive/ui/SConscript
@@ -313,6 +314,8 @@ selfdrive/ui/tests/test_translations.py
 
 selfdrive/ui/qt/*.cc
 selfdrive/ui/qt/*.h
+selfdrive/ui/qt/network/*.cc
+selfdrive/ui/qt/network/*.h
 selfdrive/ui/qt/offroad/*.cc
 selfdrive/ui/qt/offroad/*.h
 selfdrive/ui/qt/offroad/*.qml
@@ -349,29 +352,29 @@ selfdrive/manager/process.py
 selfdrive/manager/test/__init__.py
 selfdrive/manager/test/test_manager.py
 
+selfdrive/modeld/.gitignore
 selfdrive/modeld/__init__.py
 selfdrive/modeld/SConscript
-selfdrive/modeld/modeld.cc
-selfdrive/modeld/navmodeld.cc
-selfdrive/modeld/dmonitoringmodeld.cc
+selfdrive/modeld/modeld.py
+selfdrive/modeld/parse_model_outputs.py
+selfdrive/modeld/fill_model_msg.py
+selfdrive/modeld/get_model_metadata.py
+selfdrive/modeld/navmodeld.py
+selfdrive/modeld/dmonitoringmodeld.py
 selfdrive/modeld/constants.py
 selfdrive/modeld/modeld
-selfdrive/modeld/navmodeld
-selfdrive/modeld/dmonitoringmodeld
+
+selfdrive/modeld/models/__init__.py
+selfdrive/modeld/models/*.pxd
+selfdrive/modeld/models/*.pyx
 
 selfdrive/modeld/models/commonmodel.cc
 selfdrive/modeld/models/commonmodel.h
 
-selfdrive/modeld/models/driving.cc
-selfdrive/modeld/models/driving.h
 selfdrive/modeld/models/supercombo.onnx
 
-selfdrive/modeld/models/dmonitoring.cc
-selfdrive/modeld/models/dmonitoring.h
 selfdrive/modeld/models/dmonitoring_model_q.dlc
 
-selfdrive/modeld/models/nav.cc
-selfdrive/modeld/models/nav.h
 selfdrive/modeld/models/navmodel_q.dlc
 
 selfdrive/modeld/transforms/loadyuv.cc
@@ -387,12 +390,12 @@ selfdrive/modeld/thneed/thneed_common.cc
 selfdrive/modeld/thneed/thneed_qcom2.cc
 selfdrive/modeld/thneed/serialize.cc
 
-selfdrive/modeld/runners/snpemodel.cc
-selfdrive/modeld/runners/snpemodel.h
-selfdrive/modeld/runners/thneedmodel.cc
-selfdrive/modeld/runners/thneedmodel.h
-selfdrive/modeld/runners/runmodel.h
-selfdrive/modeld/runners/run.h
+selfdrive/modeld/runners/__init__.py
+selfdrive/modeld/runners/*.pxd
+selfdrive/modeld/runners/*.pyx
+selfdrive/modeld/runners/*.cc
+selfdrive/modeld/runners/*.h
+selfdrive/modeld/runners/*.py
 
 selfdrive/monitoring/dmonitoringd.py
 selfdrive/monitoring/driver_monitor.py
@@ -417,8 +420,6 @@ selfdrive/assets/navigation/*
 third_party/.gitignore
 third_party/SConscript
 
-third_party/cluster/*
-
 third_party/linux/**
 third_party/opencl/**
 
@@ -432,15 +433,11 @@ third_party/kaitai/*.h
 third_party/kaitai/*.cpp
 
 third_party/libyuv/include/**
-third_party/libyuv/lib/**
-third_party/libyuv/larch64/**
 
 third_party/snpe/include/**
 third_party/snpe/dsp**
 
 third_party/acados/.gitignore
-third_party/acados/x86_64/**
-third_party/acados/larch64/**
 third_party/acados/include/**
 third_party/acados/acados_template/**
 
@@ -452,7 +449,6 @@ scripts/stop_updater.sh
 
 rednose/.gitignore
 rednose/**
-laika/**
 
 body/.gitignore
 body/board/SConscript
@@ -570,6 +566,7 @@ opendbc/nissan_x_trail_2017_generated.dbc
 opendbc/nissan_leaf_2018_generated.dbc
 
 opendbc/subaru_global_2017_generated.dbc
+opendbc/subaru_global_2020_hybrid_generated.dbc
 opendbc/subaru_outback_2015_generated.dbc
 opendbc/subaru_outback_2019_generated.dbc
 opendbc/subaru_forester_2017_generated.dbc
@@ -587,14 +584,21 @@ opendbc/tesla_can.dbc
 opendbc/tesla_radar.dbc
 opendbc/tesla_powertrain.dbc
 
-tinygrad_repo/openpilot/compile.py
+tinygrad_repo/openpilot/compile2.py
 tinygrad_repo/extra/onnx.py
 tinygrad_repo/extra/onnx_ops.py
 tinygrad_repo/extra/thneed.py
 tinygrad_repo/extra/utils.py
-tinygrad_repo/tinygrad/codegen/ast.py
-tinygrad_repo/tinygrad/codegen/gpu.py
+tinygrad_repo/tinygrad/codegen/kernel.py
+tinygrad_repo/tinygrad/codegen/linearizer.py
+tinygrad_repo/tinygrad/features/image.py
+tinygrad_repo/tinygrad/features/search.py
 tinygrad_repo/tinygrad/nn/*
+tinygrad_repo/tinygrad/renderer/cstyle.py
+tinygrad_repo/tinygrad/renderer/opencl.py
+tinygrad_repo/tinygrad/runtime/lib.py
+tinygrad_repo/tinygrad/runtime/ops_cpu.py
+tinygrad_repo/tinygrad/runtime/ops_disk.py
 tinygrad_repo/tinygrad/runtime/ops_gpu.py
 tinygrad_repo/tinygrad/shape/*
 tinygrad_repo/tinygrad/*.py
diff --git a/release/files_pc b/release/files_pc
index 01ecae4327..13f1b52166 100644
--- a/release/files_pc
+++ b/release/files_pc
@@ -1,7 +1,6 @@
-selfdrive/modeld/runners/onnx*
-
 third_party/mapbox-gl-native-qt/x86_64/*.so
 
-third_party/libyuv/x64/**
+third_party/libyuv/x86_64/**
 third_party/snpe/x86_64/**
 third_party/snpe/x86_64-linux-clang/**
+third_party/acados/x86_64/**
diff --git a/release/files_tici b/release/files_tici
index 892b7cd2f4..ab49de34f6 100644
--- a/release/files_tici
+++ b/release/files_tici
@@ -1,6 +1,8 @@
+third_party/libyuv/larch64/**
 third_party/snpe/larch64**
 third_party/snpe/aarch64-ubuntu-gcc7.5/*
 third_party/mapbox-gl-native-qt/include/*
+third_party/acados/larch64/**
 
 system/timezoned.py
 
diff --git a/selfdrive/assets/.gitignore b/selfdrive/assets/.gitignore
index 283034ca8b..1f90a2a932 100644
--- a/selfdrive/assets/.gitignore
+++ b/selfdrive/assets/.gitignore
@@ -1 +1,2 @@
 *.cc
+translations_assets.qrc
diff --git a/selfdrive/assets/assets.qrc b/selfdrive/assets/assets.qrc
index 79a1a1e272..6d8d8df334 100644
--- a/selfdrive/assets/assets.qrc
+++ b/selfdrive/assets/assets.qrc
@@ -14,5 +14,7 @@
   <file>offroad/icon_wifi_strength_medium.svg</file>
   <file>offroad/icon_wifi_strength_high.svg</file>
   <file>offroad/icon_wifi_strength_full.svg</file>
+
+  <file alias="languages.json">../ui/translations/languages.json</file>
 </qresource>
 </RCC>
diff --git a/selfdrive/assets/navigation/direction_notificaiton_right.png b/selfdrive/assets/navigation/direction_notification_right.png
similarity index 100%
rename from selfdrive/assets/navigation/direction_notificaiton_right.png
rename to selfdrive/assets/navigation/direction_notification_right.png
diff --git a/selfdrive/assets/navigation/direction_notificaiton_sharp_right.png b/selfdrive/assets/navigation/direction_notification_sharp_right.png
similarity index 100%
rename from selfdrive/assets/navigation/direction_notificaiton_sharp_right.png
rename to selfdrive/assets/navigation/direction_notification_sharp_right.png
diff --git a/selfdrive/assets/navigation/direction_turn_slight_left_inactive.png b/selfdrive/assets/navigation/direction_turn_slight_left_inactive.png
new file mode 100644
index 0000000000..37f1f83627
Binary files /dev/null and b/selfdrive/assets/navigation/direction_turn_slight_left_inactive.png differ
diff --git a/selfdrive/assets/navigation/direction_turn_slight_right_inactive.png b/selfdrive/assets/navigation/direction_turn_slight_right_inactive.png
new file mode 100644
index 0000000000..8be2245811
Binary files /dev/null and b/selfdrive/assets/navigation/direction_turn_slight_right_inactive.png differ
diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py
index 35a9eaf192..c93b434677 100755
--- a/selfdrive/athena/athenad.py
+++ b/selfdrive/athena/athenad.py
@@ -20,7 +20,7 @@ from dataclasses import asdict, dataclass, replace
 from datetime import datetime
 from functools import partial
 from queue import Queue
-from typing import BinaryIO, Callable, Dict, List, Optional, Set, Union, cast
+from typing import Callable, Dict, List, Optional, Set, Union, cast
 
 import requests
 from jsonrpc import JSONRPCResponseManager, dispatcher
@@ -29,21 +29,21 @@ from websocket import (ABNF, WebSocket, WebSocketException, WebSocketTimeoutExce
 
 import cereal.messaging as messaging
 from cereal import log
-from cereal.services import service_list
-from common.api import Api
-from common.basedir import PERSIST
-from common.file_helpers import CallbackReader
-from common.params import Params
-from common.realtime import sec_since_boot, set_core_affinity
-from system.hardware import HARDWARE, PC, AGNOS
-from system.loggerd.config import ROOT
-from system.loggerd.xattr_cache import getxattr, setxattr
-from selfdrive.statsd import STATS_DIR
-from system.swaglog import SWAGLOG_DIR, cloudlog
-from system.version import get_commit, get_origin, get_short_branch, get_version
-
-
-# missing in pysocket
+from cereal.services import SERVICE_LIST
+from openpilot.common.api import Api
+from openpilot.common.basedir import PERSIST
+from openpilot.common.file_helpers import CallbackReader
+from openpilot.common.params import Params
+from openpilot.common.realtime import set_core_affinity
+from openpilot.system.hardware import HARDWARE, PC, AGNOS
+from openpilot.system.loggerd.xattr_cache import getxattr, setxattr
+from openpilot.selfdrive.statsd import STATS_DIR
+from openpilot.system.swaglog import cloudlog
+from openpilot.system.version import get_commit, get_origin, get_short_branch, get_version
+from openpilot.system.hardware.hw import Paths
+
+
+# TODO: use socket constant when mypy recognizes this as a valid attribute
 TCP_USER_TIMEOUT = 18
 
 ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai')
@@ -75,7 +75,7 @@ class UploadFile:
   allow_cellular: bool
 
   @classmethod
-  def from_dict(cls, d: Dict) -> UploadFile:
+  def from_dict(cls, d: dict) -> UploadFile:
     return cls(d.get("fn", ""), d.get("url", ""), d.get("headers", {}), d.get("allow_cellular", False))
 
 
@@ -85,14 +85,14 @@ class UploadItem:
   url: str
   headers: Dict[str, str]
   created_at: int
-  id: Optional[str]
+  id: Optional[str] # noqa: A003 (to match the response from the remote server)
   retry_count: int = 0
   current: bool = False
   progress: float = 0
   allow_cellular: bool = False
 
   @classmethod
-  def from_dict(cls, d: Dict) -> UploadItem:
+  def from_dict(cls, d: dict) -> UploadItem:
     return cls(d["path"], d["url"], d["headers"], d["created_at"], d["id"], d["retry_count"], d["current"],
                d["progress"], d["allow_cellular"])
 
@@ -119,12 +119,11 @@ class AbortTransferException(Exception):
 
 
 class UploadQueueCache:
-  params = Params()
 
   @staticmethod
   def initialize(upload_queue: Queue[UploadItem]) -> None:
     try:
-      upload_queue_json = UploadQueueCache.params.get("AthenadUploadQueue")
+      upload_queue_json = Params().get("AthenadUploadQueue")
       if upload_queue_json is not None:
         for item in json.loads(upload_queue_json):
           upload_queue.put(UploadItem.from_dict(item))
@@ -136,7 +135,7 @@ class UploadQueueCache:
     try:
       queue: List[Optional[UploadItem]] = list(upload_queue.queue)
       items = [asdict(i) for i in queue if i is not None and (i.id not in cancelled_uploads)]
-      UploadQueueCache.params.put("AthenadUploadQueue", json.dumps(items))
+      Params().put("AthenadUploadQueue", json.dumps(items))
     except Exception:
       cloudlog.exception("athena.UploadQueueCache.cache.exception")
 
@@ -213,6 +212,16 @@ def retry_upload(tid: int, end_event: threading.Event, increase_count: bool = Tr
         break
 
 
+def cb(sm, item, tid, sz: int, cur: int) -> None:
+  # Abort transfer if connection changed to metered after starting upload
+  sm.update(0)
+  metered = sm['deviceState'].networkMetered
+  if metered and (not item.allow_cellular):
+    raise AbortTransferException
+
+  cur_upload_items[tid] = replace(item, progress=cur / sz if sz else 1)
+
+
 def upload_handler(end_event: threading.Event) -> None:
   sm = messaging.SubMaster(['deviceState'])
   tid = threading.get_ident()
@@ -242,15 +251,6 @@ def upload_handler(end_event: threading.Event) -> None:
         continue
 
       try:
-        def cb(sz: int, cur: int) -> None:
-          # Abort transfer if connection changed to metered after starting upload
-          sm.update(0)
-          metered = sm['deviceState'].networkMetered
-          if metered and (not item.allow_cellular):
-            raise AbortTransferException
-
-          cur_upload_items[tid] = replace(item, progress=cur / sz if sz else 1)
-
         fn = item.path
         try:
           sz = os.path.getsize(fn)
@@ -258,7 +258,7 @@ def upload_handler(end_event: threading.Event) -> None:
           sz = -1
 
         cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type, metered=metered, retry_count=item.retry_count)
-        response = _do_upload(item, cb)
+        response = _do_upload(item, partial(cb, sm, item, tid))
 
         if response.status_code not in (200, 201, 401, 403, 412):
           cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type, metered=metered)
@@ -290,26 +290,22 @@ def _do_upload(upload_item: UploadItem, callback: Optional[Callable] = None) ->
     compress = True
 
   with open(path, "rb") as f:
-    data: BinaryIO
+    content = f.read()
     if compress:
       cloudlog.event("athena.upload_handler.compress", fn=path, fn_orig=upload_item.path)
-      compressed = bz2.compress(f.read())
-      size = len(compressed)
-      data = io.BytesIO(compressed)
-    else:
-      size = os.fstat(f.fileno()).st_size
-      data = f
+      content = bz2.compress(content)
 
+  with io.BytesIO(content) as data:
     return requests.put(upload_item.url,
-                        data=CallbackReader(data, callback, size) if callback else data,
-                        headers={**upload_item.headers, 'Content-Length': str(size)},
+                        data=CallbackReader(data, callback, len(content)) if callback else data,
+                        headers={**upload_item.headers, 'Content-Length': str(len(content))},
                         timeout=30)
 
 
 # security: user should be able to request any message from their car
 @dispatcher.add_method
-def getMessage(service: str, timeout: int = 1000) -> Dict:
-  if service is None or service not in service_list:
+def getMessage(service: str, timeout: int = 1000) -> dict:
+  if service is None or service not in SERVICE_LIST:
     raise Exception("invalid service")
 
   socket = messaging.sub_sock(service, timeout=timeout)
@@ -319,7 +315,7 @@ def getMessage(service: str, timeout: int = 1000) -> Dict:
     raise TimeoutError
 
   # this is because capnp._DynamicStructReader doesn't have typing information
-  return cast(Dict, ret.to_dict())
+  return cast(dict, ret.to_dict())
 
 
 @dispatcher.add_method
@@ -351,7 +347,7 @@ def scan_dir(path: str, prefix: str) -> List[str]:
   # (glob and friends traverse entire dir tree)
   with os.scandir(path) as i:
     for e in i:
-      rel_path = os.path.relpath(e.path, ROOT)
+      rel_path = os.path.relpath(e.path, Paths.log_root())
       if e.is_dir(follow_symlinks=False):
         # add trailing slash
         rel_path = os.path.join(rel_path, '')
@@ -366,7 +362,7 @@ def scan_dir(path: str, prefix: str) -> List[str]:
 
 @dispatcher.add_method
 def listDataDirectory(prefix='') -> List[str]:
-  return scan_dir(ROOT, prefix)
+  return scan_dir(Paths.log_root(), prefix)
 
 
 @dispatcher.add_method
@@ -407,7 +403,7 @@ def uploadFilesToUrls(files_data: List[UploadFileDict]) -> UploadFilesToUrlRespo
       failed.append(file.fn)
       continue
 
-    path = os.path.join(ROOT, file.fn)
+    path = os.path.join(Paths.log_root(), file.fn)
     if not os.path.exists(path) and not os.path.exists(strip_bz2_extension(path)):
       failed.append(file.fn)
       continue
@@ -551,7 +547,7 @@ def getNetworks():
 
 @dispatcher.add_method
 def takeSnapshot() -> Optional[Union[str, Dict[str, str]]]:
-  from system.camerad.snapshot.snapshot import jpeg_write, snapshot
+  from openpilot.system.camerad.snapshot.snapshot import jpeg_write, snapshot
   ret = snapshot()
   if ret is not None:
     def b64jpeg(x):
@@ -571,8 +567,8 @@ def get_logs_to_send_sorted() -> List[str]:
   # TODO: scan once then use inotify to detect file creation/deletion
   curr_time = int(time.time())
   logs = []
-  for log_entry in os.listdir(SWAGLOG_DIR):
-    log_path = os.path.join(SWAGLOG_DIR, log_entry)
+  for log_entry in os.listdir(Paths.swaglog_root()):
+    log_path = os.path.join(Paths.swaglog_root(), log_entry)
     time_sent = 0
     try:
       value = getxattr(log_path, LOG_ATTR_NAME)
@@ -592,10 +588,10 @@ def log_handler(end_event: threading.Event) -> None:
     return
 
   log_files = []
-  last_scan = 0
+  last_scan = 0.
   while not end_event.is_set():
     try:
-      curr_scan = sec_since_boot()
+      curr_scan = time.monotonic()
       if curr_scan - last_scan > 10:
         log_files = get_logs_to_send_sorted()
         last_scan = curr_scan
@@ -607,7 +603,7 @@ def log_handler(end_event: threading.Event) -> None:
         cloudlog.debug(f"athena.log_handler.forward_request {log_entry}")
         try:
           curr_time = int(time.time())
-          log_path = os.path.join(SWAGLOG_DIR, log_entry)
+          log_path = os.path.join(Paths.swaglog_root(), log_entry)
           setxattr(log_path, LOG_ATTR_NAME, int.to_bytes(curr_time, 4, sys.byteorder))
           with open(log_path) as f:
             jsonrpc = {
@@ -634,7 +630,7 @@ def log_handler(end_event: threading.Event) -> None:
           log_success = "result" in log_resp and log_resp["result"].get("success")
           cloudlog.debug(f"athena.log_handler.forward_response {log_entry} {log_success}")
           if log_entry and log_success:
-            log_path = os.path.join(SWAGLOG_DIR, log_entry)
+            log_path = os.path.join(Paths.swaglog_root(), log_entry)
             try:
               setxattr(log_path, LOG_ATTR_NAME, LOG_ATTR_VALUE_MAX_UNIX_TIME)
             except OSError:
@@ -651,8 +647,8 @@ def log_handler(end_event: threading.Event) -> None:
 
 def stat_handler(end_event: threading.Event) -> None:
   while not end_event.is_set():
-    last_scan = 0
-    curr_scan = sec_since_boot()
+    last_scan = 0.
+    curr_scan = time.monotonic()
     try:
       if curr_scan - last_scan > 10:
         stat_filenames = list(filter(lambda name: not name.startswith(tempfile.gettempprefix()), os.listdir(STATS_DIR)))
@@ -720,7 +716,7 @@ def ws_proxy_send(ws: WebSocket, local_sock: socket.socket, signal_sock: socket.
 
 
 def ws_recv(ws: WebSocket, end_event: threading.Event) -> None:
-  last_ping = int(sec_since_boot() * 1e9)
+  last_ping = int(time.monotonic() * 1e9)
   while not end_event.is_set():
     try:
       opcode, data = ws.recv_data(control_frame=True)
@@ -729,10 +725,10 @@ def ws_recv(ws: WebSocket, end_event: threading.Event) -> None:
           data = data.decode("utf-8")
         recv_queue.put_nowait(data)
       elif opcode == ABNF.OPCODE_PING:
-        last_ping = int(sec_since_boot() * 1e9)
+        last_ping = int(time.monotonic() * 1e9)
         Params().put("LastAthenaPingTime", str(last_ping))
     except WebSocketTimeoutException:
-      ns_since_last_ping = int(sec_since_boot() * 1e9) - last_ping
+      ns_since_last_ping = int(time.monotonic() * 1e9) - last_ping
       if ns_since_last_ping > RECONNECT_TIMEOUT_S * 1e9:
         cloudlog.exception("athenad.ws_recv.timeout")
         end_event.set()
@@ -821,10 +817,6 @@ def main(exit_event: Optional[threading.Event] = None):
     except (ConnectionError, TimeoutError, WebSocketException):
       conn_retries += 1
       params.remove("LastAthenaPingTime")
-    # TODO: socket.timeout and TimeoutError are now the same exception since python3.10
-    # Remove the socket.timeout case once we have fully moved to python3.11
-    except socket.timeout: # pylint: disable=duplicate-except
-      params.remove("LastAthenaPingTime")
     except Exception:
       cloudlog.exception("athenad.main.exception")
 
diff --git a/selfdrive/athena/manage_athenad.py b/selfdrive/athena/manage_athenad.py
index 59ca2430ce..877d8aca03 100755
--- a/selfdrive/athena/manage_athenad.py
+++ b/selfdrive/athena/manage_athenad.py
@@ -3,10 +3,10 @@
 import time
 from multiprocessing import Process
 
-from common.params import Params
-from selfdrive.manager.process import launcher
-from system.swaglog import cloudlog
-from system.version import get_version, is_dirty
+from openpilot.common.params import Params
+from openpilot.selfdrive.manager.process import launcher
+from openpilot.system.swaglog import cloudlog
+from openpilot.system.version import get_version, is_dirty
 
 ATHENA_MGR_PID_PARAM = "AthenadPid"
 
diff --git a/selfdrive/athena/registration.py b/selfdrive/athena/registration.py
index 32bc92059c..0ab69371c2 100755
--- a/selfdrive/athena/registration.py
+++ b/selfdrive/athena/registration.py
@@ -6,13 +6,13 @@ from pathlib import Path
 from typing import Optional
 
 from datetime import datetime, timedelta
-from common.api import api_get
-from common.params import Params
-from common.spinner import Spinner
-from common.basedir import PERSIST
-from selfdrive.controls.lib.alertmanager import set_offroad_alert
-from system.hardware import HARDWARE, PC
-from system.swaglog import cloudlog
+from openpilot.common.api import api_get
+from openpilot.common.params import Params
+from openpilot.common.spinner import Spinner
+from openpilot.common.basedir import PERSIST
+from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
+from openpilot.system.hardware import HARDWARE, PC
+from openpilot.system.swaglog import cloudlog
 
 
 UNREGISTERED_DONGLE_ID = "UnregisteredDevice"
diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript
index 2fe4591484..666763d9b0 100644
--- a/selfdrive/boardd/SConscript
+++ b/selfdrive/boardd/SConscript
@@ -7,5 +7,5 @@ env.Program('boardd', ['main.cc', 'boardd.cc'], LIBS=[panda] + libs)
 env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc'])
 
 envCython.Program('boardd_api_impl.so', 'boardd_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"])
-if GetOption('test'):
+if GetOption('extras'):
   env.Program('tests/test_boardd_usbprotocol', ['tests/test_boardd_usbprotocol.cc'], LIBS=[panda] + libs)
diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index 8f5375e6e1..0ec33c1a27 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -18,11 +18,13 @@
 #include <cstdio>
 #include <cstdlib>
 #include <future>
+#include <memory>
 #include <thread>
 
 #include "cereal/gen/cpp/car.capnp.h"
 #include "cereal/messaging/messaging.h"
 #include "common/params.h"
+#include "common/ratekeeper.h"
 #include "common/swaglog.h"
 #include "common/timing.h"
 #include "common/util.h"
@@ -155,7 +157,7 @@ bool safety_setter_thread(std::vector<Panda *> pandas) {
     }
     util::sleep_for(100);
   }
-  LOGW("got %d bytes CarParams", params.size());
+  LOGW("got %lu bytes CarParams", params.size());
 
   AlignedBuffer aligned_buf;
   capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params.data(), params.size()));
@@ -237,7 +239,7 @@ void can_send_thread(std::vector<Panda *> pandas, bool fake_send) {
         LOGT("sendcan sent to panda: %s", (panda->hw_serial()).c_str());
       }
     } else {
-      LOGE("sendcan too old to send: %llu, %llu", nanos_since_boot(), event.getLogMonoTime());
+      LOGE("sendcan too old to send: %" PRIu64 ", %" PRIu64, nanos_since_boot(), event.getLogMonoTime());
     }
   }
 }
@@ -248,8 +250,7 @@ void can_recv_thread(std::vector<Panda *> pandas) {
   PubMaster pm({"can"});
 
   // run at 100Hz
-  const uint64_t dt = 10000000ULL;
-  uint64_t next_frame_time = nanos_since_boot() + dt;
+  RateKeeper rk("boardd_can_recv", 100);
   std::vector<can_frame> raw_can_data;
 
   while (!do_exit && check_all_connected(pandas)) {
@@ -271,18 +272,7 @@ void can_recv_thread(std::vector<Panda *> pandas) {
     }
     pm.send("can", msg);
 
-    uint64_t cur_time = nanos_since_boot();
-    int64_t remaining = next_frame_time - cur_time;
-    if (remaining > 0) {
-      std::this_thread::sleep_for(std::chrono::nanoseconds(remaining));
-    } else {
-      if (ignition) {
-        LOGW("missed cycles (%d) %lld", (int)-1*remaining/dt, remaining);
-      }
-      next_frame_time = cur_time;
-    }
-
-    next_frame_time += dt;
+    rk.keepTime();
   }
 }
 
@@ -482,15 +472,20 @@ void panda_state_thread(std::vector<Panda *> pandas, bool spoofing_started) {
 
   LOGD("start panda state thread");
 
-  // run at 2hz
+  // run at 10hz
+  RateKeeper rk("panda_state_thread", 10);
+
   while (!do_exit && check_all_connected(pandas)) {
-    uint64_t start_time = nanos_since_boot();
+    // send out peripheralState at 2Hz
+    if (sm.frame % 5 == 0) {
+      send_peripheral_state(&pm, peripheral_panda);
+    }
 
-    // send out peripheralState
-    send_peripheral_state(&pm, peripheral_panda);
     auto ignition_opt = send_panda_states(&pm, pandas, spoofing_started);
 
     if (!ignition_opt) {
+      LOGE("Failed to get ignition_opt");
+      rk.keepTime();
       continue;
     }
 
@@ -543,8 +538,7 @@ void panda_state_thread(std::vector<Panda *> pandas, bool spoofing_started) {
       panda->send_heartbeat(engaged);
     }
 
-    uint64_t dt = nanos_since_boot() - start_time;
-    util::sleep_for(500 - dt / 1000000ULL);
+    rk.keepTime();
   }
 }
 
diff --git a/selfdrive/boardd/boardd.h b/selfdrive/boardd/boardd.h
index d3c9e1f94a..0646fc6189 100644
--- a/selfdrive/boardd/boardd.h
+++ b/selfdrive/boardd/boardd.h
@@ -1,5 +1,8 @@
 #pragma once
 
+#include <string>
+#include <vector>
+
 #include "selfdrive/boardd/panda.h"
 
 bool safety_setter_thread(std::vector<Panda *> pandas);
diff --git a/selfdrive/boardd/boardd.py b/selfdrive/boardd/boardd.py
index 527f1f4f52..0cdaf5e912 100644
--- a/selfdrive/boardd/boardd.py
+++ b/selfdrive/boardd/boardd.py
@@ -1,7 +1,5 @@
-# pylint: skip-file
-
 # Cython, now uses scons to build
-from selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp
+from openpilot.selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp
 assert can_list_to_can_capnp
 
 def can_capnp_to_can_list(can, src_filter=None):
diff --git a/selfdrive/boardd/can_list_to_can_capnp.cc b/selfdrive/boardd/can_list_to_can_capnp.cc
index c1778c51a2..72ca72688a 100644
--- a/selfdrive/boardd/can_list_to_can_capnp.cc
+++ b/selfdrive/boardd/can_list_to_can_capnp.cc
@@ -1,5 +1,5 @@
 #include "cereal/messaging/messaging.h"
-#include "panda.h"
+#include "selfdrive/boardd/panda.h"
 
 void can_list_to_can_capnp_cpp(const std::vector<can_frame> &can_list, std::string &out, bool sendCan, bool valid) {
   MessageBuilder msg;
diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc
index 8849a46bd8..e075887a4d 100644
--- a/selfdrive/boardd/panda.cc
+++ b/selfdrive/boardd/panda.cc
@@ -4,6 +4,7 @@
 
 #include <cassert>
 #include <stdexcept>
+#include <vector>
 
 #include "cereal/messaging/messaging.h"
 #include "common/swaglog.h"
diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h
index 5edca04419..9d4b1b2092 100644
--- a/selfdrive/boardd/panda.h
+++ b/selfdrive/boardd/panda.h
@@ -6,6 +6,7 @@
 #include <list>
 #include <memory>
 #include <optional>
+#include <string>
 #include <vector>
 
 #include "cereal/gen/cpp/car.capnp.h"
diff --git a/selfdrive/boardd/panda_comms.cc b/selfdrive/boardd/panda_comms.cc
index 120d2f67d5..bc4e5f5867 100644
--- a/selfdrive/boardd/panda_comms.cc
+++ b/selfdrive/boardd/panda_comms.cc
@@ -199,7 +199,7 @@ int PandaUsbHandle::bulk_write(unsigned char endpoint, unsigned char* data, int
     } else if (err != 0 || length != transferred) {
       handle_usb_issue(err, __func__);
     }
-  } while(err != 0 && connected);
+  } while (err != 0 && connected);
 
   return transferred;
 }
@@ -226,7 +226,7 @@ int PandaUsbHandle::bulk_read(unsigned char endpoint, unsigned char* data, int l
       handle_usb_issue(err, __func__);
     }
 
-  } while(err != 0 && connected);
+  } while (err != 0 && connected);
 
   return transferred;
 }
diff --git a/selfdrive/boardd/panda_comms.h b/selfdrive/boardd/panda_comms.h
index c102642e59..e61d25402f 100644
--- a/selfdrive/boardd/panda_comms.h
+++ b/selfdrive/boardd/panda_comms.h
@@ -20,8 +20,8 @@
 // comms base class
 class PandaCommsHandle {
 public:
-  PandaCommsHandle(std::string serial) {};
-  virtual ~PandaCommsHandle() {};
+  PandaCommsHandle(std::string serial) {}
+  virtual ~PandaCommsHandle() {}
   virtual void cleanup() = 0;
 
   std::string hw_serial;
diff --git a/selfdrive/boardd/pandad.py b/selfdrive/boardd/pandad.py
index 7cbac9b5d9..14d272965d 100755
--- a/selfdrive/boardd/pandad.py
+++ b/selfdrive/boardd/pandad.py
@@ -3,17 +3,16 @@
 import os
 import usb1
 import time
-import json
 import subprocess
 from typing import List, NoReturn
 from functools import cmp_to_key
 
 from panda import Panda, PandaDFU, PandaProtocolMismatch, FW_PATH
-from common.basedir import BASEDIR
-from common.params import Params
-from selfdrive.boardd.set_time import set_time
-from system.hardware import HARDWARE
-from system.swaglog import cloudlog
+from openpilot.common.basedir import BASEDIR
+from openpilot.common.params import Params
+from openpilot.selfdrive.boardd.set_time import set_time
+from openpilot.system.hardware import HARDWARE
+from openpilot.system.swaglog import cloudlog
 
 
 def get_expected_signature(panda: Panda) -> bytes:
@@ -24,49 +23,6 @@ def get_expected_signature(panda: Panda) -> bytes:
     cloudlog.exception("Error computing expected signature")
     return b""
 
-def read_panda_logs(panda: Panda) -> None:
-  """
-    Forward panda logs to the cloud
-  """
-
-  params = Params()
-  serial = panda.get_usb_serial()
-
-  log_state = {}
-  try:
-    l = json.loads(params.get("PandaLogState"))
-    for k, v in l.items():
-      if isinstance(k, str) and isinstance(v, int):
-        log_state[k] = v
-  except (TypeError, json.JSONDecodeError):
-    cloudlog.exception("failed to parse PandaLogState")
-
-  try:
-    if serial in log_state:
-      logs = panda.get_logs(last_id=log_state[serial])
-    else:
-      logs = panda.get_logs(get_all=True)
-
-    # truncate logs to 100 entries if needed
-    MAX_LOGS = 100
-    if len(logs) > MAX_LOGS:
-      cloudlog.warning(f"Panda {serial} has {len(logs)} logs, truncating to {MAX_LOGS}")
-      logs = logs[-MAX_LOGS:]
-
-    # update log state
-    if len(logs) > 0:
-      log_state[serial] = logs[-1]["id"]
-
-    for log in logs:
-      if log['timestamp'] is not None:
-        log['timestamp'] = log['timestamp'].isoformat()
-      cloudlog.event("panda_log", **log, serial=serial)
-
-    params.put("PandaLogState", json.dumps(log_state))
-  except Exception:
-    cloudlog.exception(f"Error getting logs for panda {serial}")
-
-
 def flash_panda(panda_serial: str) -> Panda:
   try:
     panda = Panda(panda_serial)
@@ -129,6 +85,7 @@ def main() -> NoReturn:
   count = 0
   first_run = True
   params = Params()
+  no_internal_panda_count = 0
 
   while True:
     try:
@@ -136,6 +93,16 @@ def main() -> NoReturn:
       cloudlog.event("pandad.flash_and_connect", count=count)
       params.remove("PandaSignatures")
 
+      # Handle missing internal panda
+      if no_internal_panda_count > 0:
+        if no_internal_panda_count == 3:
+          cloudlog.info("No pandas found, putting internal panda into DFU")
+          HARDWARE.recover_internal_panda()
+        else:
+          cloudlog.info("No pandas found, resetting internal panda")
+          HARDWARE.reset_internal_panda()
+        time.sleep(3)  # wait to come back up
+
       # Flash all Pandas in DFU mode
       dfu_serials = PandaDFU.list()
       if len(dfu_serials) > 0:
@@ -146,10 +113,7 @@ def main() -> NoReturn:
 
       panda_serials = Panda.list()
       if len(panda_serials) == 0:
-        if first_run:
-          cloudlog.info("No pandas found, resetting internal panda")
-          HARDWARE.reset_internal_panda()
-          time.sleep(2)  # wait to come back up
+        no_internal_panda_count += 1
         continue
 
       cloudlog.info(f"{len(panda_serials)} panda(s) found, connecting - {panda_serials}")
@@ -162,14 +126,14 @@ def main() -> NoReturn:
       # Ensure internal panda is present if expected
       internal_pandas = [panda for panda in pandas if panda.is_internal()]
       if HARDWARE.has_internal_panda() and len(internal_pandas) == 0:
-        cloudlog.error("Internal panda is missing, resetting")
-        HARDWARE.reset_internal_panda()
-        time.sleep(2)  # wait to come back up
+        cloudlog.error("Internal panda is missing, trying again")
+        no_internal_panda_count += 1
         continue
+      no_internal_panda_count = 0
 
       # sort pandas to have deterministic order
       pandas.sort(key=cmp_to_key(panda_sort_cmp))
-      panda_serials = list(map(lambda p: p.get_usb_serial(), pandas))
+      panda_serials = [p.get_usb_serial() for p in pandas]
 
       # log panda fw versions
       params.put("PandaSignatures", b','.join(p.get_signature() for p in pandas))
@@ -180,8 +144,9 @@ def main() -> NoReturn:
         if health["heartbeat_lost"]:
           params.put_bool("PandaHeartbeatLost", True)
           cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial())
-
-        read_panda_logs(panda)
+        if health["som_reset_triggered"]:
+          params.put_bool("PandaSomResetTriggered", True)
+          cloudlog.event("panda.som_reset_triggered", health=health, serial=panda.get_usb_serial())
 
         if first_run:
           if panda.is_internal():
diff --git a/selfdrive/boardd/set_time.py b/selfdrive/boardd/set_time.py
index 93453dcd97..fe17f64e82 100755
--- a/selfdrive/boardd/set_time.py
+++ b/selfdrive/boardd/set_time.py
@@ -3,7 +3,7 @@ import os
 import datetime
 from panda import Panda
 
-from common.time import MIN_DATE
+from openpilot.common.time import MIN_DATE
 
 def set_time(logger):
   sys_time = datetime.datetime.today()
diff --git a/selfdrive/boardd/spi.cc b/selfdrive/boardd/spi.cc
index 1732e902d5..d11e955c49 100644
--- a/selfdrive/boardd/spi.cc
+++ b/selfdrive/boardd/spi.cc
@@ -43,7 +43,7 @@ public:
   LockEx(int fd, std::recursive_mutex &m) : fd(fd), m(m) {
     m.lock();
     flock(fd, LOCK_EX);
-  };
+  }
 
   ~LockEx() {
     flock(fd, LOCK_UN);
diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py
index 593ccf595f..dfce0e3710 100755
--- a/selfdrive/boardd/tests/test_boardd_loopback.py
+++ b/selfdrive/boardd/tests/test_boardd_loopback.py
@@ -9,13 +9,12 @@ from pprint import pprint
 
 import cereal.messaging as messaging
 from cereal import car, log
-from common.params import Params
-from common.spinner import Spinner
-from common.timeout import Timeout
-from selfdrive.boardd.boardd import can_list_to_can_capnp
-from selfdrive.car import make_can_msg
-from system.hardware import TICI
-from selfdrive.test.helpers import phone_only, with_processes
+from openpilot.common.params import Params
+from openpilot.common.timeout import Timeout
+from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp
+from openpilot.selfdrive.car import make_can_msg
+from openpilot.system.hardware import TICI
+from openpilot.selfdrive.test.helpers import phone_only, with_processes
 
 
 class TestBoardd(unittest.TestCase):
@@ -24,11 +23,6 @@ class TestBoardd(unittest.TestCase):
   def setUpClass(cls):
     os.environ['STARTED'] = '1'
     os.environ['BOARDD_LOOPBACK'] = '1'
-    cls.spinner = Spinner()
-
-  @classmethod
-  def tearDownClass(cls):
-    cls.spinner.close()
 
   @phone_only
   @with_processes(['pandad'])
@@ -67,7 +61,6 @@ class TestBoardd(unittest.TestCase):
     n = 200
     for i in range(n):
       print(f"boardd loopback {i}/{n}")
-      self.spinner.update(f"boardd loopback {i}/{n}")
 
       sent_msgs = defaultdict(set)
       for _ in range(random.randrange(20, 100)):
diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py
index bb201bfe01..c90ae50ab9 100644
--- a/selfdrive/car/__init__.py
+++ b/selfdrive/car/__init__.py
@@ -1,11 +1,11 @@
 # functions common among cars
 from collections import namedtuple
-from typing import Dict, Optional
+from typing import Dict, List, Optional
 
 import capnp
 
 from cereal import car
-from common.numpy_fast import clip, interp
+from openpilot.common.numpy_fast import clip, interp
 
 
 # kg of standard extra cargo to count for drive, gas, etc...
@@ -24,25 +24,27 @@ def apply_hysteresis(val: float, val_steady: float, hyst_gap: float) -> float:
   return val_steady
 
 
-def create_button_event(cur_but: int, prev_but: int, buttons_dict: Dict[int, capnp.lib.capnp._EnumModule],
-                        unpressed: int = 0) -> capnp.lib.capnp._DynamicStructBuilder:
-  if cur_but != unpressed:
-    be = car.CarState.ButtonEvent(pressed=True)
-    but = cur_but
-  else:
-    be = car.CarState.ButtonEvent(pressed=False)
-    but = prev_but
-  be.type = buttons_dict.get(but, ButtonType.unknown)
-  return be
+def create_button_events(cur_btn: int, prev_btn: int, buttons_dict: Dict[int, capnp.lib.capnp._EnumModule],
+                         unpressed_btn: int = 0) -> List[capnp.lib.capnp._DynamicStructBuilder]:
+  events: List[capnp.lib.capnp._DynamicStructBuilder] = []
+
+  if cur_btn == prev_btn:
+    return events
+
+  # Add events for button presses, multiple when a button switches without going to unpressed
+  for pressed, btn in ((False, prev_btn), (True, cur_btn)):
+    if btn != unpressed_btn:
+      events.append(car.CarState.ButtonEvent(pressed=pressed,
+                                             type=buttons_dict.get(btn, ButtonType.unknown)))
+  return events
 
 
 def gen_empty_fingerprint():
-  return {i: {} for i in range(0, 8)}
+  return {i: {} for i in range(8)}
 
 
-# FIXME: hardcoding honda civic 2016 touring params so they can be used to
-# scale unknown params for other cars
-class CivicParams:
+# these params were derived for the Civic and used to calculate params for other cars
+class VehicleDynamicsParams:
   MASS = 1326. + STD_CARGO_KG
   WHEELBASE = 2.70
   CENTER_TO_FRONT = WHEELBASE * 0.4
@@ -55,18 +57,18 @@ class CivicParams:
 # TODO: get actual value, for now starting with reasonable value for
 # civic and scaling by mass and wheelbase
 def scale_rot_inertia(mass, wheelbase):
-  return CivicParams.ROTATIONAL_INERTIA * mass * wheelbase ** 2 / (CivicParams.MASS * CivicParams.WHEELBASE ** 2)
+  return VehicleDynamicsParams.ROTATIONAL_INERTIA * mass * wheelbase ** 2 / (VehicleDynamicsParams.MASS * VehicleDynamicsParams.WHEELBASE ** 2)
 
 
 # TODO: start from empirically derived lateral slip stiffness for the civic and scale by
 # mass and CG position, so all cars will have approximately similar dyn behaviors
-def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor=1.0):
+def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor):
   center_to_rear = wheelbase - center_to_front
-  tire_stiffness_front = (CivicParams.TIRE_STIFFNESS_FRONT * tire_stiffness_factor) * mass / CivicParams.MASS * \
-                         (center_to_rear / wheelbase) / (CivicParams.CENTER_TO_REAR / CivicParams.WHEELBASE)
+  tire_stiffness_front = (VehicleDynamicsParams.TIRE_STIFFNESS_FRONT * tire_stiffness_factor) * mass / VehicleDynamicsParams.MASS * \
+                         (center_to_rear / wheelbase) / (VehicleDynamicsParams.CENTER_TO_REAR / VehicleDynamicsParams.WHEELBASE)
 
-  tire_stiffness_rear = (CivicParams.TIRE_STIFFNESS_REAR * tire_stiffness_factor) * mass / CivicParams.MASS * \
-                        (center_to_front / wheelbase) / (CivicParams.CENTER_TO_FRONT / CivicParams.WHEELBASE)
+  tire_stiffness_rear = (VehicleDynamicsParams.TIRE_STIFFNESS_REAR * tire_stiffness_factor) * mass / VehicleDynamicsParams.MASS * \
+                        (center_to_front / wheelbase) / (VehicleDynamicsParams.CENTER_TO_FRONT / VehicleDynamicsParams.WHEELBASE)
 
   return tire_stiffness_front, tire_stiffness_rear
 
@@ -132,6 +134,30 @@ def apply_std_steer_angle_limits(apply_angle, apply_angle_last, v_ego, LIMITS):
   return clip(apply_angle, apply_angle_last - angle_rate_lim, apply_angle_last + angle_rate_lim)
 
 
+def common_fault_avoidance(fault_condition: bool, request: bool, above_limit_frames: int,
+                           max_above_limit_frames: int, max_mismatching_frames: int = 1):
+  """
+  Several cars have the ability to work around their EPS limits by cutting the
+  request bit of their LKAS message after a certain number of frames above the limit.
+  """
+
+  # Count up to max_above_limit_frames, at which point we need to cut the request for above_limit_frames to avoid a fault
+  if request and fault_condition:
+    above_limit_frames += 1
+  else:
+    above_limit_frames = 0
+
+  # Once we cut the request bit, count additionally to max_mismatching_frames before setting the request bit high again.
+  # Some brands do not respect our workaround without multiple messages on the bus, for example
+  if above_limit_frames > max_above_limit_frames:
+    request = False
+
+  if above_limit_frames >= max_above_limit_frames + max_mismatching_frames:
+    above_limit_frames = 0
+
+  return above_limit_frames, request
+
+
 def crc8_pedal(data):
   crc = 0xFF    # standard init value
   poly = 0xD5   # standard crc8: x8+x7+x6+x4+x2+1
@@ -189,3 +215,24 @@ class CanBusBase:
     else:
       num = len(CP.safetyConfigs)
     self.offset = 4 * (num - 1)
+
+
+class CanSignalRateCalculator:
+  """
+  Calculates the instantaneous rate of a CAN signal by using the counter
+  variable and the known frequency of the CAN message that contains it.
+  """
+  def __init__(self, frequency):
+    self.frequency = frequency
+    self.previous_counter = 0
+    self.previous_value = 0
+    self.rate = 0
+
+  def update(self, current_value, current_counter):
+    if current_counter != self.previous_counter:
+      self.rate = (current_value - self.previous_value) * self.frequency
+
+    self.previous_counter = current_counter
+    self.previous_value = current_value
+
+    return self.rate
diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py
index ee5d3f6886..1dad8e796a 100644
--- a/selfdrive/car/body/carcontroller.py
+++ b/selfdrive/car/body/carcontroller.py
@@ -1,11 +1,11 @@
 import numpy as np
 
-from common.params import Params
-from common.realtime import DT_CTRL
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_CTRL
 from opendbc.can.packer import CANPacker
-from selfdrive.car.body import bodycan
-from selfdrive.car.body.values import SPEED_FROM_RPM
-from selfdrive.controls.lib.pid import PIDController
+from openpilot.selfdrive.car.body import bodycan
+from openpilot.selfdrive.car.body.values import SPEED_FROM_RPM
+from openpilot.selfdrive.controls.lib.pid import PIDController
 
 
 MAX_TORQUE = 500
@@ -56,7 +56,7 @@ class CarController:
       speed_error = speed_desired - speed_measured
 
       if self.wheeled_body is None:
-        freeze_integrator = ((speed_error < 0 and self.speed_pid.error_integral <= -MAX_POS_INTEGRATOR) or 
+        freeze_integrator = ((speed_error < 0 and self.speed_pid.error_integral <= -MAX_POS_INTEGRATOR) or
                              (speed_error > 0 and self.speed_pid.error_integral >= MAX_POS_INTEGRATOR))
         angle_setpoint = self.speed_pid.update(speed_error, freeze_integrator=freeze_integrator)
 
diff --git a/selfdrive/car/body/carstate.py b/selfdrive/car/body/carstate.py
index dbbd85950d..fca9bcc627 100644
--- a/selfdrive/car/body/carstate.py
+++ b/selfdrive/car/body/carstate.py
@@ -1,7 +1,7 @@
 from cereal import car
 from opendbc.can.parser import CANParser
-from selfdrive.car.interfaces import CarStateBase
-from selfdrive.car.body.values import DBC
+from openpilot.selfdrive.car.interfaces import CarStateBase
+from openpilot.selfdrive.car.body.values import DBC
 
 STARTUP_TICKS = 100
 
@@ -32,29 +32,9 @@ class CarState(CarStateBase):
 
   @staticmethod
   def get_can_parser(CP):
-    signals = [
-      # sig_name, sig_address
-      ("SPEED_L", "MOTORS_DATA"),
-      ("SPEED_R", "MOTORS_DATA"),
-      ("ELEC_ANGLE_L", "MOTORS_DATA"),
-      ("ELEC_ANGLE_R", "MOTORS_DATA"),
-      ("COUNTER", "MOTORS_DATA"),
-      ("CHECKSUM", "MOTORS_DATA"),
-      ("IGNITION", "VAR_VALUES"),
-      ("ENABLE_MOTORS", "VAR_VALUES"),
-      ("FAULT", "VAR_VALUES"),
-      ("MOTOR_ERR_L", "VAR_VALUES"),
-      ("MOTOR_ERR_R", "VAR_VALUES"),
-      ("MCU_TEMP", "BODY_DATA"),
-      ("BATT_VOLTAGE", "BODY_DATA"),
-      ("BATT_PERCENTAGE", "BODY_DATA"),
-      ("CHARGER_CONNECTED", "BODY_DATA"),
-    ]
-
-    checks = [
+    messages = [
       ("MOTORS_DATA", 100),
       ("VAR_VALUES", 10),
       ("BODY_DATA", 1),
     ]
-
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py
index 4d583badae..12a2d5f304 100644
--- a/selfdrive/car/body/interface.py
+++ b/selfdrive/car/body/interface.py
@@ -1,10 +1,9 @@
-#!/usr/bin/env python3
 import math
 from cereal import car
-from common.realtime import DT_CTRL
-from selfdrive.car import get_safety_config
-from selfdrive.car.interfaces import CarInterfaceBase
-from selfdrive.car.body.values import SPEED_FROM_RPM
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.car import get_safety_config
+from openpilot.selfdrive.car.interfaces import CarInterfaceBase
+from openpilot.selfdrive.car.body.values import SPEED_FROM_RPM
 
 class CarInterface(CarInterfaceBase):
   @staticmethod
diff --git a/selfdrive/car/body/radar_interface.py b/selfdrive/car/body/radar_interface.py
index b2f7651136..e654bd61fd 100644
--- a/selfdrive/car/body/radar_interface.py
+++ b/selfdrive/car/body/radar_interface.py
@@ -1,5 +1,4 @@
-#!/usr/bin/env python3
-from selfdrive.car.interfaces import RadarInterfaceBase
+from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
 
 class RadarInterface(RadarInterfaceBase):
   pass
diff --git a/selfdrive/car/body/values.py b/selfdrive/car/body/values.py
index 4fef966374..ce811ad221 100644
--- a/selfdrive/car/body/values.py
+++ b/selfdrive/car/body/values.py
@@ -1,9 +1,10 @@
+from enum import StrEnum
 from typing import Dict
 
 from cereal import car
-from selfdrive.car import dbc_dict
-from selfdrive.car.docs_definitions import CarInfo
-from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
+from openpilot.selfdrive.car import dbc_dict
+from openpilot.selfdrive.car.docs_definitions import CarInfo
+from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
 
 Ecu = car.CarParams.Ecu
 
@@ -21,7 +22,7 @@ class CarControllerParams:
     pass
 
 
-class CAR:
+class CAR(StrEnum):
   BODY = "COMMA BODY"
 
 
diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py
index be3771d319..5c1234258e 100644
--- a/selfdrive/car/car_helpers.py
+++ b/selfdrive/car/car_helpers.py
@@ -1,17 +1,20 @@
 import os
-from typing import Dict, List
+import time
+from typing import Callable, Dict, List, Optional, Tuple
 
 from cereal import car
-from common.params import Params
-from common.basedir import BASEDIR
-from system.version import is_comma_remote, is_tested_branch
-from selfdrive.car.interfaces import get_interface_attr
-from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars
-from selfdrive.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN
-from selfdrive.car.fw_versions import get_fw_versions_ordered, get_present_ecus, match_fw_to_car, set_obd_multiplexing
-from system.swaglog import cloudlog
+from openpilot.common.params import Params
+from openpilot.common.basedir import BASEDIR
+from openpilot.system.version import is_comma_remote, is_tested_branch
+from openpilot.selfdrive.car.interfaces import get_interface_attr
+from openpilot.selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars
+from openpilot.selfdrive.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN
+from openpilot.selfdrive.car.fw_versions import get_fw_versions_ordered, get_present_ecus, match_fw_to_car, set_obd_multiplexing
+from openpilot.system.swaglog import cloudlog
 import cereal.messaging as messaging
-from selfdrive.car import gen_empty_fingerprint
+from openpilot.selfdrive.car import gen_empty_fingerprint
+
+FRAME_FINGERPRINT = 100  # 1s
 
 EventName = car.CarEvent.EventName
 
@@ -42,7 +45,7 @@ def get_one_can(logcan):
 def load_interfaces(brand_names):
   ret = {}
   for brand_name in brand_names:
-    path = f'selfdrive.car.{brand_name}'
+    path = f'openpilot.selfdrive.car.{brand_name}'
     CarInterface = __import__(path + '.interface', fromlist=['CarInterface']).CarInterface
 
     if os.path.exists(BASEDIR + '/' + path.replace('.', '/') + '/carstate.py'):
@@ -63,9 +66,8 @@ def load_interfaces(brand_names):
 def _get_interface_names() -> Dict[str, List[str]]:
   # returns a dict of brand name and its respective models
   brand_names = {}
-  for brand_name, model_names in get_interface_attr("CAR").items():
-    model_names = [getattr(model_names, c) for c in model_names.__dict__.keys() if not c.startswith("__")]
-    brand_names[brand_name] = model_names
+  for brand_name, brand_models in get_interface_attr("CAR").items():
+    brand_names[brand_name] = [model.value for model in brand_models]
 
   return brand_names
 
@@ -75,6 +77,46 @@ interface_names = _get_interface_names()
 interfaces = load_interfaces(interface_names)
 
 
+def can_fingerprint(next_can: Callable) -> Tuple[Optional[str], Dict[int, dict]]:
+  finger = gen_empty_fingerprint()
+  candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]}  # attempt fingerprint on both bus 0 and 1
+  frame = 0
+  car_fingerprint = None
+  done = False
+
+  while not done:
+    a = next_can()
+
+    for can in a.can:
+      # The fingerprint dict is generated for all buses, this way the car interface
+      # can use it to detect a (valid) multipanda setup and initialize accordingly
+      if can.src < 128:
+        if can.src not in finger:
+          finger[can.src] = {}
+        finger[can.src][can.address] = len(can.dat)
+
+      for b in candidate_cars:
+        # Ignore extended messages and VIN query response.
+        if can.src == b and can.address < 0x800 and can.address not in (0x7df, 0x7e0, 0x7e8):
+          candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b])
+
+    # if we only have one car choice and the time since we got our first
+    # message has elapsed, exit
+    for b in candidate_cars:
+      if len(candidate_cars[b]) == 1 and frame > FRAME_FINGERPRINT:
+        # fingerprint done
+        car_fingerprint = candidate_cars[b][0]
+
+    # bail if no cars left or we've been waiting for more than 2s
+    failed = (all(len(cc) == 0 for cc in candidate_cars.values()) and frame > FRAME_FINGERPRINT) or frame > 200
+    succeeded = car_fingerprint is not None
+    done = failed or succeeded
+
+    frame += 1
+
+  return car_fingerprint, finger
+
+
 # **** for use live only ****
 def fingerprint(logcan, sendcan, num_pandas):
   fixed_fingerprint = os.environ.get('FINGERPRINT', "")
@@ -83,6 +125,7 @@ def fingerprint(logcan, sendcan, num_pandas):
   ecu_rx_addrs = set()
   params = Params()
 
+  start_time = time.monotonic()
   if not skip_fw_query:
     # Vin query only reliably works through OBDII
     bus = 1
@@ -123,45 +166,12 @@ def fingerprint(logcan, sendcan, num_pandas):
   set_obd_multiplexing(params, False)
   params.put_bool("FirmwareQueryDone", True)
 
-  finger = gen_empty_fingerprint()
-  candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]}  # attempt fingerprint on both bus 0 and 1
-  frame = 0
-  frame_fingerprint = 100  # 1s
-  car_fingerprint = None
-  done = False
+  fw_query_time = time.monotonic() - start_time
 
-  # drain CAN socket so we always get the latest messages
+  # CAN fingerprint
+  # drain CAN socket so we get the latest messages
   messaging.drain_sock_raw(logcan)
-
-  while not done:
-    a = get_one_can(logcan)
-
-    for can in a.can:
-      # The fingerprint dict is generated for all buses, this way the car interface
-      # can use it to detect a (valid) multipanda setup and initialize accordingly
-      if can.src < 128:
-        if can.src not in finger:
-          finger[can.src] = {}
-        finger[can.src][can.address] = len(can.dat)
-
-      for b in candidate_cars:
-        # Ignore extended messages and VIN query response.
-        if can.src == b and can.address < 0x800 and can.address not in (0x7df, 0x7e0, 0x7e8):
-          candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b])
-
-    # if we only have one car choice and the time since we got our first
-    # message has elapsed, exit
-    for b in candidate_cars:
-      if len(candidate_cars[b]) == 1 and frame > frame_fingerprint:
-        # fingerprint done
-        car_fingerprint = candidate_cars[b][0]
-
-    # bail if no cars left or we've been waiting for more than 2s
-    failed = (all(len(cc) == 0 for cc in candidate_cars.values()) and frame > frame_fingerprint) or frame > 200
-    succeeded = car_fingerprint is not None
-    done = failed or succeeded
-
-    frame += 1
+  car_fingerprint, finger = can_fingerprint(lambda: get_one_can(logcan))
 
   exact_match = True
   source = car.CarParams.FingerprintSource.can
@@ -177,7 +187,8 @@ def fingerprint(logcan, sendcan, num_pandas):
     source = car.CarParams.FingerprintSource.fixed
 
   cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match, cached=cached,
-                 fw_count=len(car_fw), ecu_responses=list(ecu_rx_addrs), vin_rx_addr=vin_rx_addr, error=True)
+                 fw_count=len(car_fw), ecu_responses=list(ecu_rx_addrs), vin_rx_addr=vin_rx_addr, fingerprints=finger,
+                 fw_query_time=fw_query_time, error=True)
   return car_fingerprint, finger, vin, car_fw, source, exact_match
 
 
diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py
index b418179e0e..050eb41b1a 100644
--- a/selfdrive/car/chrysler/carcontroller.py
+++ b/selfdrive/car/chrysler/carcontroller.py
@@ -1,8 +1,8 @@
 from opendbc.can.packer import CANPacker
-from common.realtime import DT_CTRL
-from selfdrive.car import apply_meas_steer_torque_limits
-from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, create_cruise_buttons
-from selfdrive.car.chrysler.values import RAM_CARS, CarControllerParams, ChryslerFlags
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.car import apply_meas_steer_torque_limits
+from openpilot.selfdrive.car.chrysler import chryslercan
+from openpilot.selfdrive.car.chrysler.values import RAM_CARS, CarControllerParams, ChryslerFlags
 
 
 class CarController:
@@ -31,17 +31,18 @@ class CarController:
       # ACC cancellation
       if CC.cruiseControl.cancel:
         self.last_button_frame = self.frame
-        can_sends.append(create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, cancel=True))
+        can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, cancel=True))
 
       # ACC resume from standstill
       elif CC.cruiseControl.resume:
         self.last_button_frame = self.frame
-        can_sends.append(create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, resume=True))
+        can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, resume=True))
 
     # HUD alerts
     if self.frame % 25 == 0:
       if CS.lkas_car_model != -1:
-        can_sends.append(create_lkas_hud(self.packer, self.CP, lkas_active, CC.hudControl.visualAlert, self.hud_count, CS.lkas_car_model, CS.auto_high_beam))
+        can_sends.append(chryslercan.create_lkas_hud(self.packer, self.CP, lkas_active, CC.hudControl.visualAlert,
+                                                     self.hud_count, CS.lkas_car_model, CS.auto_high_beam))
         self.hud_count += 1
 
     # steering
@@ -72,7 +73,7 @@ class CarController:
         apply_steer = 0
       self.apply_steer_last = apply_steer
 
-      can_sends.append(create_lkas_command(self.packer, self.CP, int(apply_steer), lkas_control_bit))
+      can_sends.append(chryslercan.create_lkas_command(self.packer, self.CP, int(apply_steer), lkas_control_bit))
 
     self.frame += 1
 
diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py
index fdc5aa338a..eb1cf7e7d5 100644
--- a/selfdrive/car/chrysler/carstate.py
+++ b/selfdrive/car/chrysler/carstate.py
@@ -1,9 +1,9 @@
 from cereal import car
-from common.conversions import Conversions as CV
+from openpilot.common.conversions import Conversions as CV
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
-from selfdrive.car.interfaces import CarStateBase
-from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD, RAM_CARS
+from openpilot.selfdrive.car.interfaces import CarStateBase
+from openpilot.selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD, RAM_CARS
 
 
 class CarState(CarStateBase):
@@ -80,7 +80,8 @@ class CarState(CarStateBase):
     ret.accFaulted = cp_cruise.vl["DAS_3"]["ACC_FAULTED"] != 0
 
     if self.CP.carFingerprint in RAM_CARS:
-      self.auto_high_beam = cp_cam.vl["DAS_6"]['AUTO_HIGH_BEAM_ON']  # Auto High Beam isn't Located in this message on chrysler or jeep currently located in 729 message
+      # Auto High Beam isn't Located in this message on chrysler or jeep currently located in 729 message
+      self.auto_high_beam = cp_cam.vl["DAS_6"]['AUTO_HIGH_BEAM_ON']
       ret.steerFaultTemporary = cp.vl["EPS_3"]["DASM_FAULT"] == 1
     else:
       ret.steerFaultTemporary = cp.vl["EPS_2"]["LKAS_TEMPORARY_FAULT"] == 1
@@ -97,51 +98,16 @@ class CarState(CarStateBase):
     return ret
 
   @staticmethod
-  def get_cruise_signals():
-    signals = [
-      ("ACC_AVAILABLE", "DAS_3"),
-      ("ACC_ACTIVE", "DAS_3"),
-      ("ACC_FAULTED", "DAS_3"),
-      ("ACC_STANDSTILL", "DAS_3"),
-      ("COUNTER", "DAS_3"),
-      ("ACC_SET_SPEED_KPH", "DAS_4"),
-      ("ACC_STATE", "DAS_4"),
-    ]
-    checks = [
+  def get_cruise_messages():
+    messages = [
       ("DAS_3", 50),
       ("DAS_4", 50),
     ]
-    return signals, checks
+    return messages
 
   @staticmethod
   def get_can_parser(CP):
-    signals = [
-      # sig_name, sig_address
-      ("DOOR_OPEN_FL", "BCM_1"),
-      ("DOOR_OPEN_FR", "BCM_1"),
-      ("DOOR_OPEN_RL", "BCM_1"),
-      ("DOOR_OPEN_RR", "BCM_1"),
-      ("Brake_Pedal_State", "ESP_1"),
-      ("Accelerator_Position", "ECM_5"),
-      ("WHEEL_SPEED_FL", "ESP_6"),
-      ("WHEEL_SPEED_RR", "ESP_6"),
-      ("WHEEL_SPEED_RL", "ESP_6"),
-      ("WHEEL_SPEED_FR", "ESP_6"),
-      ("STEERING_ANGLE", "STEERING"),
-      ("STEERING_ANGLE_HP", "STEERING"),
-      ("STEERING_RATE", "STEERING"),
-      ("TURN_SIGNALS", "STEERING_LEVERS"),
-      ("HIGH_BEAM_PRESSED", "STEERING_LEVERS"),
-      ("SEATBELT_DRIVER_UNLATCHED", "ORC_1"),
-      ("COUNTER", "EPS_2",),
-      ("COLUMN_TORQUE", "EPS_2"),
-      ("EPS_TORQUE_MOTOR", "EPS_2"),
-      ("LKAS_TEMPORARY_FAULT", "EPS_2"),
-      ("LKAS_STATE", "EPS_2"),
-      ("COUNTER", "CRUISE_BUTTONS"),
-    ]
-
-    checks = [
+    messages = [
       # sig_address, frequency
       ("ESP_1", 50),
       ("EPS_2", 100),
@@ -155,53 +121,30 @@ class CarState(CarStateBase):
     ]
 
     if CP.enableBsm:
-      signals += [
-        ("RIGHT_STATUS", "BSM_1"),
-        ("LEFT_STATUS", "BSM_1"),
-      ]
-      checks.append(("BSM_1", 2))
+      messages.append(("BSM_1", 2))
 
     if CP.carFingerprint in RAM_CARS:
-      signals += [
-        ("DASM_FAULT", "EPS_3"),
-        ("Vehicle_Speed", "ESP_8"),
-        ("Gear_State", "Transmission_Status"),
-      ]
-      checks += [
+      messages += [
         ("ESP_8", 50),
         ("EPS_3", 50),
         ("Transmission_Status", 50),
       ]
     else:
-      signals += [
-        ("PRNDL", "GEAR"),
-        ("SPEED_LEFT", "SPEED_1"),
-        ("SPEED_RIGHT", "SPEED_1"),
-      ]
-      checks += [
+      messages += [
         ("GEAR", 50),
         ("SPEED_1", 100),
       ]
-      signals += CarState.get_cruise_signals()[0]
-      checks += CarState.get_cruise_signals()[1]
+      messages += CarState.get_cruise_messages()
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
 
   @staticmethod
   def get_cam_can_parser(CP):
-    signals = [
-      # sig_name, sig_address, default
-      ("CAR_MODEL", "DAS_6"),
-    ]
-    checks = [
+    messages = [
       ("DAS_6", 4),
     ]
 
     if CP.carFingerprint in RAM_CARS:
-      signals += [
-        ("AUTO_HIGH_BEAM_ON", "DAS_6"),
-      ]
-      signals += CarState.get_cruise_signals()[0]
-      checks += CarState.get_cruise_signals()[1]
+      messages += CarState.get_cruise_messages()
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py
index 10ed73e9f2..96439f35d8 100644
--- a/selfdrive/car/chrysler/chryslercan.py
+++ b/selfdrive/car/chrysler/chryslercan.py
@@ -1,5 +1,5 @@
 from cereal import car
-from selfdrive.car.chrysler.values import RAM_CARS
+from openpilot.selfdrive.car.chrysler.values import RAM_CARS
 
 GearShifter = car.CarState.GearShifter
 VisualAlert = car.CarControl.HUDControl.VisualAlert
diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py
index 22b2073883..fd030e3ec6 100755
--- a/selfdrive/car/chrysler/interface.py
+++ b/selfdrive/car/chrysler/interface.py
@@ -1,9 +1,9 @@
 #!/usr/bin/env python3
 from cereal import car
 from panda import Panda
-from selfdrive.car import STD_CARGO_KG, get_safety_config
-from selfdrive.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags
-from selfdrive.car.interfaces import CarInterfaceBase
+from openpilot.selfdrive.car import get_safety_config
+from openpilot.selfdrive.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags
+from openpilot.selfdrive.car.interfaces import CarInterfaceBase
 
 
 class CarInterface(CarInterfaceBase):
@@ -35,7 +35,7 @@ class CarInterface(CarInterfaceBase):
 
     # Chrysler
     if candidate in (CAR.PACIFICA_2017_HYBRID, CAR.PACIFICA_2018, CAR.PACIFICA_2018_HYBRID, CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020):
-      ret.mass = 2242. + STD_CARGO_KG
+      ret.mass = 2242.
       ret.wheelbase = 3.089
       ret.steerRatio = 16.2  # Pacifica Hybrid 2017
 
@@ -46,7 +46,7 @@ class CarInterface(CarInterfaceBase):
 
     # Jeep
     elif candidate in (CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019):
-      ret.mass = 1778 + STD_CARGO_KG
+      ret.mass = 1778
       ret.wheelbase = 2.71
       ret.steerRatio = 16.7
       ret.steerActuatorDelay = 0.2
@@ -61,17 +61,17 @@ class CarInterface(CarInterfaceBase):
       ret.steerActuatorDelay = 0.2
       ret.wheelbase = 3.88
       ret.steerRatio = 16.3
-      ret.mass = 2493. + STD_CARGO_KG
+      ret.mass = 2493.
       ret.minSteerSpeed = 14.5
       # Older EPS FW allow steer to zero
-      if any(fw.ecu == 'eps' and fw.fwVersion[:4] <= b"6831" for fw in car_fw):
+      if any(fw.ecu == 'eps' and b"68" < fw.fwVersion[:4] <= b"6831" for fw in car_fw):
         ret.minSteerSpeed = 0.
 
     elif candidate == CAR.RAM_HD:
       ret.steerActuatorDelay = 0.2
       ret.wheelbase = 3.785
       ret.steerRatio = 15.61
-      ret.mass = 3405. + STD_CARGO_KG
+      ret.mass = 3405.
       ret.minSteerSpeed = 16
       CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, 1.0, False)
 
diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py
index 0ab8c10b44..d982958422 100755
--- a/selfdrive/car/chrysler/radar_interface.py
+++ b/selfdrive/car/chrysler/radar_interface.py
@@ -1,8 +1,8 @@
 #!/usr/bin/env python3
 from opendbc.can.parser import CANParser
 from cereal import car
-from selfdrive.car.interfaces import RadarInterfaceBase
-from selfdrive.car.chrysler.values import DBC
+from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
+from openpilot.selfdrive.car.chrysler.values import DBC
 
 RADAR_MSGS_C = list(range(0x2c2, 0x2d4+2, 2))  # c_ messages 706,...,724
 RADAR_MSGS_D = list(range(0x2a2, 0x2b4+2, 2))  # d_ messages
@@ -22,18 +22,12 @@ def _create_radar_can_parser(car_fingerprint):
   #  ('LONG_DIST', 1074),
   #  ('LONG_DIST', 1075),
 
-  signals = list(zip(['LONG_DIST'] * msg_n +
-                     ['LAT_DIST'] * msg_n +
-                     ['REL_SPEED'] * msg_n,
-                     RADAR_MSGS_C * 2 +  # LONG_DIST, LAT_DIST
-                     RADAR_MSGS_D))  # REL_SPEED
+  messages = list(zip(RADAR_MSGS_C +
+                      RADAR_MSGS_D,
+                      [20] * msg_n +  # 20Hz (0.05s)
+                      [20] * msg_n, strict=True))  # 20Hz (0.05s)
 
-  checks = list(zip(RADAR_MSGS_C +
-                    RADAR_MSGS_D,
-                    [20] * msg_n +  # 20Hz (0.05s)
-                    [20] * msg_n))  # 20Hz (0.05s)
-
-  return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1)
+  return CANParser(DBC[car_fingerprint]['radar'], messages, 1)
 
 def _address_to_track(address):
   if address in RADAR_MSGS_C:
@@ -89,4 +83,4 @@ class RadarInterface(RadarInterfaceBase):
     ret.points = [x for x in self.pts.values() if x.dRel != 0]
 
     self.updated_messages.clear()
-    return ret
\ No newline at end of file
+    return ret
diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py
index b9ce596c93..af12c60a75 100644
--- a/selfdrive/car/chrysler/values.py
+++ b/selfdrive/car/chrysler/values.py
@@ -1,12 +1,13 @@
-from enum import IntFlag
+# ruff: noqa: E501
+from enum import IntFlag, StrEnum
 from dataclasses import dataclass, field
 from typing import Dict, List, Optional, Union
 
 from cereal import car
 from panda.python import uds
-from selfdrive.car import dbc_dict
-from selfdrive.car.docs_definitions import CarHarness, CarInfo, CarParts
-from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16
+from openpilot.selfdrive.car import dbc_dict
+from openpilot.selfdrive.car.docs_definitions import CarHarness, CarInfo, CarParts
+from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16
 
 Ecu = car.CarParams.Ecu
 
@@ -15,7 +16,7 @@ class ChryslerFlags(IntFlag):
   HIGHER_MIN_STEERING_SPEED = 1
 
 
-class CAR:
+class CAR(StrEnum):
   # Chrysler
   PACIFICA_2017_HYBRID = "CHRYSLER PACIFICA HYBRID 2017"
   PACIFICA_2018_HYBRID = "CHRYSLER PACIFICA HYBRID 2018"
@@ -131,7 +132,7 @@ FINGERPRINTS = {
     168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 450: 8, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 796: 8, 797: 8, 798: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 805: 8, 807: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 886: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1284: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 2018: 8, 2020: 8, 2026: 8, 2028: 8
   }],
   CAR.JEEP_CHEROKEE: [{
-    55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 874: 2, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 975: 8, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1543: 8, 1562: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
+    55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 874: 2, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 975: 8, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1543: 8, 1562: 8, 1576: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
   },
     # Based on c88f65eeaee4003a|2022-08-04--15-37-16
   {
@@ -271,6 +272,7 @@ FW_VERSIONS = {
     ],
     (Ecu.eps, 0x75A, None): [
       b'21590101AA',
+      b'21590101AB',
       b'68273275AF',
       b'68273275AG',
       b'68273275AH',
diff --git a/selfdrive/car/disable_ecu.py b/selfdrive/car/disable_ecu.py
index 36ebe12fa8..c11075342c 100755
--- a/selfdrive/car/disable_ecu.py
+++ b/selfdrive/car/disable_ecu.py
@@ -1,5 +1,6 @@
-from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
-from system.swaglog import cloudlog
+#!/usr/bin/env python3
+from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
+from openpilot.system.swaglog import cloudlog
 
 EXT_DIAG_REQUEST = b'\x10\x03'
 EXT_DIAG_RESPONSE = b'\x50\x03'
diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py
index 5db720b8f5..2e80dde010 100644
--- a/selfdrive/car/docs_definitions.py
+++ b/selfdrive/car/docs_definitions.py
@@ -6,7 +6,7 @@ from enum import Enum
 from typing import Dict, List, Optional, Tuple, Union
 
 from cereal import car
-from common.conversions import Conversions as CV
+from openpilot.common.conversions import Conversions as CV
 
 GOOD_TORQUE_THRESHOLD = 1.0  # m/s^2
 MODEL_YEARS_RE = r"(?<= )((\d{4}-\d{2})|(\d{4}))(,|$)"
@@ -50,7 +50,7 @@ class BasePart:
 
 class EnumBase(Enum):
   @property
-  def type(self):
+  def part_type(self):
     return PartType(self.__class__)
 
 
@@ -84,9 +84,12 @@ class CarHarness(EnumBase):
   nidec = BaseCarHarness("Honda Nidec connector")
   bosch_a = BaseCarHarness("Honda Bosch A connector")
   bosch_b = BaseCarHarness("Honda Bosch B connector")
-  toyota = BaseCarHarness("Toyota connector")
+  toyota_a = BaseCarHarness("Toyota A connector")
+  toyota_b = BaseCarHarness("Toyota B connector")
   subaru_a = BaseCarHarness("Subaru A connector")
   subaru_b = BaseCarHarness("Subaru B connector")
+  subaru_c = BaseCarHarness("Subaru C connector")
+  subaru_d = BaseCarHarness("Subaru D connector")
   fca = BaseCarHarness("FCA connector")
   ram = BaseCarHarness("Ram connector")
   vw = BaseCarHarness("VW connector")
@@ -108,9 +111,10 @@ class CarHarness(EnumBase):
   hyundai_o = BaseCarHarness("Hyundai O connector")
   hyundai_p = BaseCarHarness("Hyundai P connector")
   hyundai_q = BaseCarHarness("Hyundai Q connector")
+  hyundai_r = BaseCarHarness("Hyundai R connector")
   custom = BaseCarHarness("Developer connector")
   obd_ii = BaseCarHarness("OBD-II connector", parts=[Cable.long_obdc_cable, Cable.long_obdc_cable], has_connector=False)
-  gm = BaseCarHarness("GM connector")
+  gm = BaseCarHarness("GM connector", parts=[Accessory.harness_box])
   nissan_a = BaseCarHarness("Nissan A connector", parts=[Accessory.harness_box, Cable.rj45_cable_7ft, Cable.long_obdc_cable, Cable.usbc_coupler])
   nissan_b = BaseCarHarness("Nissan B connector", parts=[Accessory.harness_box, Cable.rj45_cable_7ft, Cable.long_obdc_cable, Cable.usbc_coupler])
   mazda = BaseCarHarness("Mazda connector")
@@ -119,14 +123,20 @@ class CarHarness(EnumBase):
 
 
 class Device(EnumBase):
-  three = BasePart("comma three", parts=[Mount.mount, Cable.right_angle_obd_c_cable_1_5ft])
-  # variant of comma three with angled mounts
-  three_angled_mount = BasePart("comma three", parts=[Mount.angled_mount_8_degrees, Cable.right_angle_obd_c_cable_1_5ft])
+  threex = BasePart("comma 3X", parts=[Mount.mount, Cable.right_angle_obd_c_cable_1_5ft])
+  # variant of comma 3X with angled mounts
+  threex_angled_mount = BasePart("comma 3X", parts=[Mount.angled_mount_8_degrees, Cable.right_angle_obd_c_cable_1_5ft])
   red_panda = BasePart("red panda")
 
 
 class Kit(EnumBase):
-  red_panda_kit = BasePart("CAN FD panda kit", parts=[Device.red_panda, Accessory.harness_box, Cable.usb_a_2_a_cable, Cable.usbc_otg_cable, Cable.obd_c_cable_1_5ft])
+  red_panda_kit = BasePart("CAN FD panda kit", parts=[Device.red_panda, Accessory.harness_box,
+                                                      Cable.usb_a_2_a_cable, Cable.usbc_otg_cable, Cable.obd_c_cable_1_5ft])
+
+
+class Tool(EnumBase):
+  socket_8mm_deep = BasePart("Socket Wrench 8mm or 5/16\" (deep)")
+  pry_tool = BasePart("Pry Tool")
 
 
 class PartType(Enum):
@@ -136,9 +146,10 @@ class PartType(Enum):
   device = Device
   kit = Kit
   mount = Mount
+  tool = Tool
 
 
-DEFAULT_CAR_PARTS: List[EnumBase] = [Device.three]
+DEFAULT_CAR_PARTS: List[EnumBase] = [Device.threex]
 
 
 @dataclass
@@ -165,10 +176,12 @@ CarFootnote = namedtuple("CarFootnote", ["text", "column", "docs_only", "shop_fo
 
 class CommonFootnote(Enum):
   EXP_LONG_AVAIL = CarFootnote(
-    "Experimental openpilot longitudinal control is available behind a toggle; the toggle is only available in non-release branches such as `devel` or `master-ci`. ",
+    "openpilot Longitudinal Control (Alpha) is available behind a toggle; " +
+    "the toggle is only available in non-release branches such as `devel` or `master-ci`.",
     Column.LONGITUDINAL, docs_only=True)
   EXP_LONG_DSU = CarFootnote(
-    "By default, this car will use the stock Adaptive Cruise Control (ACC) for longitudinal control. If the Driver Support Unit (DSU) is disconnected, openpilot ACC will replace " +
+    "By default, this car will use the stock Adaptive Cruise Control (ACC) for longitudinal control. " +
+    "If the Driver Support Unit (DSU) is disconnected, openpilot ACC will replace " +
     "stock ACC. <b><i>NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).</i></b>",
     Column.LONGITUDINAL)
 
@@ -265,10 +278,17 @@ class CarInfo:
     hardware_col = "None"
     if self.car_parts.parts:
       model_years = self.model + (' ' + self.years if self.years else '')
-      buy_link = f'<a href="https://comma.ai/shop/comma-three.html?make={self.make}&model={model_years}">Buy Here</a>'
-      car_parts_docs = self.car_parts.all_parts()
-      parts = '<br>'.join([f"- {car_parts_docs.count(part)} {part.value.name}" for part in sorted(set(car_parts_docs), key=lambda part: str(part.value.name))])
-      hardware_col = f'<details><summary>View</summary><sub>{parts}<br>{buy_link}</sub></details>'
+      buy_link = f'<a href="https://comma.ai/shop/comma-3x.html?make={self.make}&model={model_years}">Buy Here</a>'
+
+      tools_docs = [part for part in self.car_parts.all_parts() if isinstance(part, Tool)]
+      parts_docs = [part for part in self.car_parts.all_parts() if not isinstance(part, Tool)]
+
+      def display_func(parts):
+        return '<br>'.join([f"- {parts.count(part)} {part.value.name}" for part in sorted(set(parts), key=lambda part: str(part.value.name))])
+
+      hardware_col = f'<details><summary>Parts</summary><sub>{display_func(parts_docs)}<br>{buy_link}</sub></details>'
+      if len(tools_docs):
+        hardware_col += f'<details><summary>Tools</summary><sub>{display_func(tools_docs)}</sub></details>'
 
     self.row: Dict[Enum, Union[str, Star]] = {
       Column.MAKE: self.make,
@@ -320,8 +340,6 @@ class CarInfo:
       exp_link = "<a href='https://blog.comma.ai/090release/#experimental-mode' target='_blank' class='link-light-new-regular-text'>Experimental mode</a>"
       if CP.openpilotLongitudinalControl or CP.experimentalLongitudinalAvailable:
         sentence_builder += f" Traffic light and stop sign handling is also available in {exp_link}."
-      else:
-        sentence_builder += f" {exp_link}, with traffic light and stop sign handling, is not currently available for this car, but may be added in a future software update."
 
       return sentence_builder.format(car_model=f"{self.make} {self.model}", alc=alc, acc=acc)
 
diff --git a/selfdrive/car/ecu_addrs.py b/selfdrive/car/ecu_addrs.py
index 868f12cdb8..cff1df79a5 100755
--- a/selfdrive/car/ecu_addrs.py
+++ b/selfdrive/car/ecu_addrs.py
@@ -1,15 +1,14 @@
 #!/usr/bin/env python3
 import capnp
 import time
-from typing import Optional, Set, Tuple
+from typing import Optional, Set
 
 import cereal.messaging as messaging
 from panda.python.uds import SERVICE_TYPE
-from selfdrive.car import make_can_msg
-from selfdrive.boardd.boardd import can_list_to_can_capnp
-from system.swaglog import cloudlog
-
-EcuAddrBusType = Tuple[int, Optional[int], int]
+from openpilot.selfdrive.car import make_can_msg
+from openpilot.selfdrive.car.fw_query_definitions import EcuAddrBusType
+from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp
+from openpilot.system.swaglog import cloudlog
 
 
 def make_tester_present_msg(addr, bus, subaddr=None):
@@ -90,7 +89,7 @@ if __name__ == "__main__":
 
   print()
   print("Found ECUs on addresses:")
-  for addr, subaddr, bus in ecu_addrs:
+  for addr, subaddr, _ in ecu_addrs:
     msg = f"  0x{hex(addr)}"
     if subaddr is not None:
       msg += f" (sub-address: {hex(subaddr)})"
diff --git a/selfdrive/car/fingerprints.py b/selfdrive/car/fingerprints.py
index 1a9bb8c4e7..1d627e4b37 100644
--- a/selfdrive/car/fingerprints.py
+++ b/selfdrive/car/fingerprints.py
@@ -1,4 +1,4 @@
-from selfdrive.car.interfaces import get_interface_attr
+from openpilot.selfdrive.car.interfaces import get_interface_attr
 
 
 FW_VERSIONS = get_interface_attr('FW_VERSIONS', combine_brands=True, ignore_none=True)
@@ -29,9 +29,8 @@ def eliminate_incompatible_cars(msg, candidate_cars):
     car_fingerprints = _FINGERPRINTS[car_name]
 
     for fingerprint in car_fingerprints:
-      fingerprint.update(_DEBUG_ADDRESS)  # add alien debug address
-
-      if is_valid_for_fingerprint(msg, fingerprint):
+      # add alien debug address
+      if is_valid_for_fingerprint(msg, fingerprint | _DEBUG_ADDRESS):
         compatible_cars.append(car_name)
         break
 
diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py
index dd30bc57e1..fe9ee404a4 100644
--- a/selfdrive/car/ford/carcontroller.py
+++ b/selfdrive/car/ford/carcontroller.py
@@ -1,10 +1,9 @@
 from cereal import car
-from common.numpy_fast import clip
+from openpilot.common.numpy_fast import clip
 from opendbc.can.packer import CANPacker
-from selfdrive.car import apply_std_steer_angle_limits
-from selfdrive.car.ford.fordcan import CanBus, create_acc_msg, create_acc_ui_msg, create_button_msg, \
-                                       create_lat_ctl_msg, create_lat_ctl2_msg, create_lka_msg, create_lkas_ui_msg
-from selfdrive.car.ford.values import CANFD_CAR, CarControllerParams
+from openpilot.selfdrive.car import apply_std_steer_angle_limits
+from openpilot.selfdrive.car.ford import fordcan
+from openpilot.selfdrive.car.ford.values import CANFD_CAR, CarControllerParams
 
 LongCtrlState = car.CarControl.Actuators.LongControlState
 VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -27,7 +26,7 @@ class CarController:
     self.CP = CP
     self.VM = VM
     self.packer = CANPacker(dbc_name)
-    self.CAN = CanBus(CP)
+    self.CAN = fordcan.CanBus(CP)
     self.frame = 0
 
     self.apply_curvature_last = 0
@@ -47,15 +46,15 @@ class CarController:
 
     ### acc buttons ###
     if CC.cruiseControl.cancel:
-      can_sends.append(create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=True))
-      can_sends.append(create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, cancel=True))
+      can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=True))
+      can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, cancel=True))
     elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0:
-      can_sends.append(create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, resume=True))
-      can_sends.append(create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, resume=True))
+      can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, resume=True))
+      can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, resume=True))
     # if stock lane centering isn't off, send a button press to toggle it off
     # the stock system checks for steering pressed, and eventually disengages cruise control
     elif CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0 and (self.frame % CarControllerParams.ACC_UI_STEP) == 0:
-      can_sends.append(create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, tja_toggle=True))
+      can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, tja_toggle=True))
 
     ### lateral control ###
     # send steer msg at 20Hz
@@ -73,13 +72,13 @@ class CarController:
         # TODO: extended mode
         mode = 1 if CC.latActive else 0
         counter = (self.frame // CarControllerParams.STEER_STEP) % 0xF
-        can_sends.append(create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -apply_curvature, 0., counter))
+        can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -apply_curvature, 0., counter))
       else:
-        can_sends.append(create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -apply_curvature, 0.))
+        can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -apply_curvature, 0.))
 
     # send lka msg at 33Hz
     if (self.frame % CarControllerParams.LKA_STEP) == 0:
-      can_sends.append(create_lka_msg(self.packer, self.CAN))
+      can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN))
 
     ### longitudinal control ###
     # send acc msg at 50Hz
@@ -91,16 +90,16 @@ class CarController:
         gas = CarControllerParams.INACTIVE_GAS
 
       stopping = CC.actuators.longControlState == LongCtrlState.stopping
-      can_sends.append(create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping))
+      can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping))
 
     ### ui ###
     send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
     # send lkas ui msg at 1Hz or if ui state changes
     if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui:
-      can_sends.append(create_lkas_ui_msg(self.packer, self.CAN, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values))
+      can_sends.append(fordcan.create_lkas_ui_msg(self.packer, self.CAN, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values))
     # send acc ui msg at 5Hz or if ui state changes
     if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
-      can_sends.append(create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive,
+      can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive,
                                          fcw_alert, CS.out.cruiseState.standstill, hud_control,
                                          CS.acc_tja_status_stock_values))
 
diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py
index d9848096e7..5c787b787a 100644
--- a/selfdrive/car/ford/carstate.py
+++ b/selfdrive/car/ford/carstate.py
@@ -1,10 +1,10 @@
 from cereal import car
-from common.conversions import Conversions as CV
+from openpilot.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
-from selfdrive.car.ford.fordcan import CanBus
-from selfdrive.car.ford.values import CANFD_CAR, CarControllerParams, DBC
+from openpilot.selfdrive.car.interfaces import CarStateBase
+from openpilot.selfdrive.car.ford.fordcan import CanBus
+from openpilot.selfdrive.car.ford.values import CANFD_CAR, CarControllerParams, DBC
 
 GearShifter = car.CarState.GearShifter
 TransmissionType = car.CarParams.TransmissionType
@@ -111,70 +111,7 @@ class CarState(CarStateBase):
 
   @staticmethod
   def get_can_parser(CP):
-    signals = [
-      # sig_name, sig_address
-      ("TrnAinTq_D_Qf", "VehicleOperatingModes"),            # Used to detect hybrid or ICE platform variant
-
-      ("Veh_V_ActlBrk", "BrakeSysFeatures"),                 # ABS vehicle speed (kph)
-      ("VehYaw_W_Actl", "Yaw_Data_FD1"),                     # ABS vehicle yaw rate (rad/s)
-      ("VehStop_D_Stat", "DesiredTorqBrk"),                  # ABS vehicle stopped
-      ("PrkBrkStatus", "DesiredTorqBrk"),                    # ABS park brake status
-      ("ApedPos_Pc_ActlArb", "EngVehicleSpThrottle"),        # PCM throttle (pct)
-      ("BrkTot_Tq_Actl", "BrakeSnData_4"),                   # ABS brake torque (Nm)
-      ("BpedDrvAppl_D_Actl", "EngBrakeData"),                # PCM driver brake pedal pressed
-      ("Veh_V_DsplyCcSet", "EngBrakeData"),                  # PCM ACC set speed (mph)
-                                                             # The units might change with IPC settings?
-      ("CcStat_D_Actl", "EngBrakeData"),                     # PCM ACC status
-      ("AccStopMde_D_Rq", "EngBrakeData"),                   # PCM ACC standstill
-      ("AccEnbl_B_RqDrv", "Cluster_Info1_FD1"),              # PCM ACC enable
-      ("StePinComp_An_Est", "SteeringPinion_Data"),          # PSCM estimated steering angle (deg)
-      ("StePinCompAnEst_D_Qf", "SteeringPinion_Data"),       # PSCM estimated steering angle (quality flag)
-                                                             # Calculates steering angle (and offset) from pinion
-                                                             # angle and driving measurements.
-                                                             # StePinRelInit_An_Sns is the pinion angle, initialised
-                                                             # to zero at the beginning of the drive.
-      ("SteeringColumnTorque", "EPAS_INFO"),                 # PSCM steering column torque (Nm)
-      ("EPAS_Failure", "EPAS_INFO"),                         # PSCM EPAS status
-      ("TurnLghtSwtch_D_Stat", "Steering_Data_FD1"),         # SCCM Turn signal switch
-      ("TjaButtnOnOffPress", "Steering_Data_FD1"),           # SCCM ACC button, lane-centering/traffic jam assist toggle
-      ("DrStatDrv_B_Actl", "BodyInfo_3_FD1"),                # BCM Door open, driver
-      ("DrStatPsngr_B_Actl", "BodyInfo_3_FD1"),              # BCM Door open, passenger
-      ("DrStatRl_B_Actl", "BodyInfo_3_FD1"),                 # BCM Door open, rear left
-      ("DrStatRr_B_Actl", "BodyInfo_3_FD1"),                 # BCM Door open, rear right
-      ("FirstRowBuckleDriver", "RCMStatusMessage2_FD1"),     # RCM Seatbelt status, driver
-      ("HeadLghtHiFlash_D_Stat", "Steering_Data_FD1"),       # SCCM Passthrough the remaining buttons
-      ("WiprFront_D_Stat", "Steering_Data_FD1"),
-      ("LghtAmb_D_Sns", "Steering_Data_FD1"),
-      ("AccButtnGapDecPress", "Steering_Data_FD1"),
-      ("AccButtnGapIncPress", "Steering_Data_FD1"),
-      ("AslButtnOnOffCnclPress", "Steering_Data_FD1"),
-      ("AslButtnOnOffPress", "Steering_Data_FD1"),
-      ("LaSwtchPos_D_Stat", "Steering_Data_FD1"),
-      ("CcAslButtnCnclResPress", "Steering_Data_FD1"),
-      ("CcAslButtnDeny_B_Actl", "Steering_Data_FD1"),
-      ("CcAslButtnIndxDecPress", "Steering_Data_FD1"),
-      ("CcAslButtnIndxIncPress", "Steering_Data_FD1"),
-      ("CcAslButtnOffCnclPress", "Steering_Data_FD1"),
-      ("CcAslButtnOnOffCncl", "Steering_Data_FD1"),
-      ("CcAslButtnOnPress", "Steering_Data_FD1"),
-      ("CcAslButtnResDecPress", "Steering_Data_FD1"),
-      ("CcAslButtnResIncPress", "Steering_Data_FD1"),
-      ("CcAslButtnSetDecPress", "Steering_Data_FD1"),
-      ("CcAslButtnSetIncPress", "Steering_Data_FD1"),
-      ("CcAslButtnSetPress", "Steering_Data_FD1"),
-      ("CcButtnOffPress", "Steering_Data_FD1"),
-      ("CcButtnOnOffCnclPress", "Steering_Data_FD1"),
-      ("CcButtnOnOffPress", "Steering_Data_FD1"),
-      ("CcButtnOnPress", "Steering_Data_FD1"),
-      ("HeadLghtHiFlash_D_Actl", "Steering_Data_FD1"),
-      ("HeadLghtHiOn_B_StatAhb", "Steering_Data_FD1"),
-      ("AhbStat_B_Dsply", "Steering_Data_FD1"),
-      ("AccButtnGapTogglePress", "Steering_Data_FD1"),
-      ("WiprFrontSwtch_D_Stat", "Steering_Data_FD1"),
-      ("HeadLghtHiCtrl_D_RqAhb", "Steering_Data_FD1"),
-    ]
-
-    checks = [
+    messages = [
       # sig_address, frequency
       ("VehicleOperatingModes", 100),
       ("BrakeSysFeatures", 50),
@@ -192,93 +129,31 @@ class CarState(CarStateBase):
     ]
 
     if CP.carFingerprint in CANFD_CAR:
-      signals += [
-        ("LatCtlSte_D_Stat", "Lane_Assist_Data3_FD1"),       # PSCM lateral control status
-      ]
-      checks += [
+      messages += [
         ("Lane_Assist_Data3_FD1", 33),
       ]
 
     if CP.transmissionType == TransmissionType.automatic:
-      signals += [
-        ("TrnRng_D_RqGsm", "Gear_Shift_by_Wire_FD1"),        # GWM transmission gear position
-      ]
-      checks += [
+      messages += [
         ("Gear_Shift_by_Wire_FD1", 10),
       ]
     elif CP.transmissionType == TransmissionType.manual:
-      signals += [
-        ("CluPdlPos_Pc_Meas", "Engine_Clutch_Data"),         # PCM clutch (pct)
-        ("RvrseLghtOn_B_Stat", "BCM_Lamp_Stat_FD1"),         # BCM reverse light
-      ]
-      checks += [
+      messages += [
         ("Engine_Clutch_Data", 33),
         ("BCM_Lamp_Stat_FD1", 1),
       ]
 
     if CP.enableBsm and CP.carFingerprint not in CANFD_CAR:
-      signals += [
-        ("SodDetctLeft_D_Stat", "Side_Detect_L_Stat"),       # Blindspot sensor, left
-        ("SodDetctRight_D_Stat", "Side_Detect_R_Stat"),      # Blindspot sensor, right
-      ]
-      checks += [
+      messages += [
         ("Side_Detect_L_Stat", 5),
         ("Side_Detect_R_Stat", 5),
       ]
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus(CP).main)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).main)
 
   @staticmethod
   def get_cam_can_parser(CP):
-    signals = [
-      # sig_name, sig_address
-      ("CmbbDeny_B_Actl", "ACCDATA"),               # ACC/AEB unavailable/lockout
-
-      ("CmbbBrkDecel_B_Rq", "ACCDATA_2"),           # AEB actuation request bit
-
-      ("HaDsply_No_Cs", "ACCDATA_3"),
-      ("HaDsply_No_Cnt", "ACCDATA_3"),
-      ("AccStopStat_D_Dsply", "ACCDATA_3"),         # ACC stopped status message
-      ("AccTrgDist2_D_Dsply", "ACCDATA_3"),         # ACC target distance
-      ("AccStopRes_B_Dsply", "ACCDATA_3"),
-      ("TjaWarn_D_Rq", "ACCDATA_3"),                # TJA warning
-      ("Tja_D_Stat", "ACCDATA_3"),                  # TJA status
-      ("TjaMsgTxt_D_Dsply", "ACCDATA_3"),           # TJA text
-      ("IaccLamp_D_Rq", "ACCDATA_3"),               # iACC status icon
-      ("AccMsgTxt_D2_Rq", "ACCDATA_3"),             # ACC text
-      ("FcwDeny_B_Dsply", "ACCDATA_3"),             # FCW disabled
-      ("FcwMemStat_B_Actl", "ACCDATA_3"),           # FCW enabled setting
-      ("AccTGap_B_Dsply", "ACCDATA_3"),             # ACC time gap display setting
-      ("CadsAlignIncplt_B_Actl", "ACCDATA_3"),
-      ("AccFllwMde_B_Dsply", "ACCDATA_3"),          # ACC lead indicator
-      ("CadsRadrBlck_B_Actl", "ACCDATA_3"),
-      ("CmbbPostEvnt_B_Dsply", "ACCDATA_3"),        # AEB event status
-      ("AccStopMde_B_Dsply", "ACCDATA_3"),          # ACC stop mode display setting
-      ("FcwMemSens_D_Actl", "ACCDATA_3"),           # FCW sensitivity setting
-      ("FcwMsgTxt_D_Rq", "ACCDATA_3"),              # FCW text
-      ("AccWarn_D_Dsply", "ACCDATA_3"),             # ACC warning
-      ("FcwVisblWarn_B_Rq", "ACCDATA_3"),           # FCW visible alert
-      ("FcwAudioWarn_B_Rq", "ACCDATA_3"),           # FCW audio alert
-      ("AccTGap_D_Dsply", "ACCDATA_3"),             # ACC time gap
-      ("AccMemEnbl_B_RqDrv", "ACCDATA_3"),          # ACC adaptive/normal setting
-      ("FdaMem_B_Stat", "ACCDATA_3"),               # FDA enabled setting
-
-      ("FeatConfigIpmaActl", "IPMA_Data"),
-      ("FeatNoIpmaActl", "IPMA_Data"),
-      ("PersIndexIpma_D_Actl", "IPMA_Data"),
-      ("AhbcRampingV_D_Rq", "IPMA_Data"),           # AHB ramping
-      ("LaDenyStats_B_Dsply", "IPMA_Data"),         # LKAS error
-      ("CamraDefog_B_Req", "IPMA_Data"),            # Windshield heater?
-      ("CamraStats_D_Dsply", "IPMA_Data"),          # Camera status
-      ("DasAlrtLvl_D_Dsply", "IPMA_Data"),          # DAS alert level
-      ("DasStats_D_Dsply", "IPMA_Data"),            # DAS status
-      ("DasWarn_D_Dsply", "IPMA_Data"),             # DAS warning
-      ("AhbHiBeam_D_Rq", "IPMA_Data"),              # AHB status
-      ("Passthru_63", "IPMA_Data"),
-      ("Passthru_48", "IPMA_Data"),
-    ]
-
-    checks = [
+    messages = [
       # sig_address, frequency
       ("ACCDATA", 50),
       ("ACCDATA_2", 50),
@@ -287,13 +162,9 @@ class CarState(CarStateBase):
     ]
 
     if CP.enableBsm and CP.carFingerprint in CANFD_CAR:
-      signals += [
-        ("SodDetctLeft_D_Stat", "Side_Detect_L_Stat"),       # Blindspot sensor, left
-        ("SodDetctRight_D_Stat", "Side_Detect_R_Stat"),      # Blindspot sensor, right
-      ]
-      checks += [
+      messages += [
         ("Side_Detect_L_Stat", 5),
         ("Side_Detect_R_Stat", 5),
       ]
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus(CP).camera)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).camera)
diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py
index a49d7ad85d..e0086ecbb5 100644
--- a/selfdrive/car/ford/fordcan.py
+++ b/selfdrive/car/ford/fordcan.py
@@ -1,5 +1,5 @@
 from cereal import car
-from selfdrive.car import CanBusBase
+from openpilot.selfdrive.car import CanBusBase
 
 HUDControl = car.CarControl.HUDControl
 
diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py
index d74baa3ce4..3045c317ff 100644
--- a/selfdrive/car/ford/interface.py
+++ b/selfdrive/car/ford/interface.py
@@ -1,11 +1,10 @@
-#!/usr/bin/env python3
 from cereal import car
 from panda import Panda
-from common.conversions import Conversions as CV
-from selfdrive.car import STD_CARGO_KG, get_safety_config
-from selfdrive.car.ford.fordcan import CanBus
-from selfdrive.car.ford.values import CANFD_CAR, CAR, Ecu
-from selfdrive.car.interfaces import CarInterfaceBase
+from openpilot.common.conversions import Conversions as CV
+from openpilot.selfdrive.car import get_safety_config
+from openpilot.selfdrive.car.ford.fordcan import CanBus
+from openpilot.selfdrive.car.ford.values import CANFD_CAR, CAR, Ecu
+from openpilot.selfdrive.car.interfaces import CarInterfaceBase
 
 TransmissionType = car.CarParams.TransmissionType
 GearShifter = car.CarState.GearShifter
@@ -39,33 +38,33 @@ class CarInterface(CarInterfaceBase):
     if candidate == CAR.BRONCO_SPORT_MK1:
       ret.wheelbase = 2.67
       ret.steerRatio = 17.7
-      ret.mass = 1625 + STD_CARGO_KG
+      ret.mass = 1625
 
     elif candidate == CAR.ESCAPE_MK4:
       ret.wheelbase = 2.71
       ret.steerRatio = 16.7
-      ret.mass = 1750 + STD_CARGO_KG
+      ret.mass = 1750
 
     elif candidate == CAR.EXPLORER_MK6:
       ret.wheelbase = 3.025
       ret.steerRatio = 16.8
-      ret.mass = 2050 + STD_CARGO_KG
+      ret.mass = 2050
 
     elif candidate == CAR.F_150_MK14:
       # required trim only on SuperCrew
       ret.wheelbase = 3.69
       ret.steerRatio = 17.0
-      ret.mass = 2000 + STD_CARGO_KG
+      ret.mass = 2000
 
     elif candidate == CAR.FOCUS_MK4:
       ret.wheelbase = 2.7
       ret.steerRatio = 15.0
-      ret.mass = 1350 + STD_CARGO_KG
+      ret.mass = 1350
 
     elif candidate == CAR.MAVERICK_MK1:
       ret.wheelbase = 3.076
       ret.steerRatio = 17.0
-      ret.mass = 1650 + STD_CARGO_KG
+      ret.mass = 1650
 
     else:
       raise ValueError(f"Unsupported car: {candidate}")
diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py
index e44730ca4f..716c9b6e58 100644
--- a/selfdrive/car/ford/radar_interface.py
+++ b/selfdrive/car/ford/radar_interface.py
@@ -1,11 +1,10 @@
-#!/usr/bin/env python3
 from math import cos, sin
 from cereal import car
 from opendbc.can.parser import CANParser
-from common.conversions import Conversions as CV
-from selfdrive.car.ford.fordcan import CanBus
-from selfdrive.car.ford.values import DBC, RADAR
-from selfdrive.car.interfaces import RadarInterfaceBase
+from openpilot.common.conversions import Conversions as CV
+from openpilot.selfdrive.car.ford.fordcan import CanBus
+from openpilot.selfdrive.car.ford.values import DBC, RADAR
+from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
 
 DELPHI_ESR_RADAR_MSGS = list(range(0x500, 0x540))
 
@@ -15,30 +14,19 @@ DELPHI_MRR_RADAR_MSG_COUNT = 64
 
 def _create_delphi_esr_radar_can_parser(CP) -> CANParser:
   msg_n = len(DELPHI_ESR_RADAR_MSGS)
-  signals = list(zip(['X_Rel'] * msg_n + ['Angle'] * msg_n + ['V_Rel'] * msg_n,
-                     DELPHI_ESR_RADAR_MSGS * 3))
-  checks = list(zip(DELPHI_ESR_RADAR_MSGS, [20] * msg_n))
+  messages = list(zip(DELPHI_ESR_RADAR_MSGS, [20] * msg_n, strict=True))
 
-  return CANParser(RADAR.DELPHI_ESR, signals, checks, CanBus(CP).radar)
+  return CANParser(RADAR.DELPHI_ESR, messages, CanBus(CP).radar)
 
 
 def _create_delphi_mrr_radar_can_parser(CP) -> CANParser:
-  signals = []
-  checks = []
+  messages = []
 
   for i in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1):
     msg = f"MRR_Detection_{i:03d}"
-    signals += [
-      (f"CAN_DET_VALID_LEVEL_{i:02d}", msg),
-      (f"CAN_DET_AZIMUTH_{i:02d}", msg),
-      (f"CAN_DET_RANGE_{i:02d}", msg),
-      (f"CAN_DET_RANGE_RATE_{i:02d}", msg),
-      (f"CAN_DET_AMPLITUDE_{i:02d}", msg),
-      (f"CAN_SCAN_INDEX_2LSB_{i:02d}", msg),
-    ]
-    checks += [(msg, 20)]
-
-  return CANParser(RADAR.DELPHI_MRR, signals, checks, CanBus(CP).radar)
+    messages += [(msg, 20)]
+
+  return CANParser(RADAR.DELPHI_MRR, messages, CanBus(CP).radar)
 
 
 class RadarInterface(RadarInterfaceBase):
diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py
index 7ce0abb21f..d11ba4ccf0 100644
--- a/selfdrive/car/ford/values.py
+++ b/selfdrive/car/ford/values.py
@@ -1,13 +1,13 @@
 from collections import defaultdict
 from dataclasses import dataclass, field
-from enum import Enum
+from enum import Enum, StrEnum
 from typing import Dict, List, Union
 
 from cereal import car
-from selfdrive.car import AngleRateLimit, dbc_dict
-from selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column, \
+from openpilot.selfdrive.car import AngleRateLimit, dbc_dict
+from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column, \
                                            Device
-from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
+from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
 
 Ecu = car.CarParams.Ecu
 
@@ -40,7 +40,7 @@ class CarControllerParams:
     pass
 
 
-class CAR:
+class CAR(StrEnum):
   BRONCO_SPORT_MK1 = "FORD BRONCO SPORT 1ST GEN"
   ESCAPE_MK4 = "FORD ESCAPE 4TH GEN"
   EXPLORER_MK6 = "FORD EXPLORER 6TH GEN"
@@ -78,7 +78,7 @@ class FordCarInfo(CarInfo):
 
   def init_make(self, CP: car.CarParams):
     if CP.carFingerprint in (CAR.BRONCO_SPORT_MK1, CAR.MAVERICK_MK1):
-      self.car_parts = CarParts([Device.three_angled_mount, CarHarness.ford_q3])
+      self.car_parts = CarParts([Device.threex_angled_mount, CarHarness.ford_q3])
 
 
 CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
@@ -93,7 +93,10 @@ CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
   ],
   CAR.F_150_MK14: FordCarInfo("Ford F-150 2023", "Co-Pilot360 Active 2.0"),
   CAR.FOCUS_MK4: FordCarInfo("Ford Focus 2018", "Adaptive Cruise Control with Lane Centering", footnotes=[Footnote.FOCUS]),
-  CAR.MAVERICK_MK1: FordCarInfo("Ford Maverick 2022-23", "Co-Pilot360 Assist"),
+  CAR.MAVERICK_MK1: [
+    FordCarInfo("Ford Maverick 2022", "LARIAT Luxury"),
+    FordCarInfo("Ford Maverick 2023", "Co-Pilot360 Assist"),
+  ],
 }
 
 FW_QUERY_CONFIG = FwQueryConfig(
@@ -105,7 +108,6 @@ FW_QUERY_CONFIG = FwQueryConfig(
       [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
       [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
       # whitelist_ecus=[Ecu.engine],
-      auxiliary=True,
     ),
     Request(
       [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
@@ -153,6 +155,7 @@ FW_VERSIONS = {
       b'LX6C-2D053-NT\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'LX6C-2D053-NY\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'LX6C-2D053-SA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'LX6C-2D053-SD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x764, None): [
       b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -160,11 +163,13 @@ FW_VERSIONS = {
     (Ecu.fwdCamera, 0x706, None): [
       b'LJ6T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'LJ6T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'LV4T-14F397-GG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x7E0, None): [
       b'LX6A-14C204-BJV\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'LX6A-14C204-BJX\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'LX6A-14C204-CNG\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'LX6A-14C204-DPK\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'LX6A-14C204-ESG\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'MX6A-14C204-BEF\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'MX6A-14C204-BEJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -198,6 +203,7 @@ FW_VERSIONS = {
     ],
     (Ecu.engine, 0x7E0, None): [
       b'LB5A-14C204-ATJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'LB5A-14C204-AUJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'LB5A-14C204-AZL\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'LB5A-14C204-BUJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'LB5A-14C204-EAC\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -259,7 +265,9 @@ FW_VERSIONS = {
       b'NZ6A-14C204-AAA\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'NZ6A-14C204-PA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'NZ6A-14C204-ZA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'PZ6A-14C204-BE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'PZ6A-14C204-JC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'PZ6A-14C204-JE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
   },
 }
diff --git a/selfdrive/car/fw_query_definitions.py b/selfdrive/car/fw_query_definitions.py
index f9f8e30a68..1caedcd321 100755
--- a/selfdrive/car/fw_query_definitions.py
+++ b/selfdrive/car/fw_query_definitions.py
@@ -7,6 +7,9 @@ from typing import Callable, Dict, List, Optional, Set, Tuple
 
 import panda.python.uds as uds
 
+AddrType = Tuple[int, Optional[int]]
+EcuAddrBusType = Tuple[int, Optional[int], int]
+
 
 def p16(val):
   return struct.pack("!H", val)
@@ -76,7 +79,7 @@ class FwQueryConfig:
   extra_ecus: List[Tuple[capnp.lib.capnp._EnumModule, int, Optional[int]]] = field(default_factory=list)
   # Function a brand can implement to provide better fuzzy matching. Takes in FW versions,
   # returns set of candidates. Only will match if one candidate is returned
-  match_fw_to_car_fuzzy: Optional[Callable[[Dict[Tuple[int, Optional[int]], Set[bytes]]], Set[str]]] = None
+  match_fw_to_car_fuzzy: Optional[Callable[[Dict[AddrType, Set[bytes]]], Set[str]]] = None
 
   def __post_init__(self):
     for i in range(len(self.requests)):
diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py
index 5a7a2174c7..45c4967cb8 100755
--- a/selfdrive/car/fw_versions.py
+++ b/selfdrive/car/fw_versions.py
@@ -1,17 +1,18 @@
 #!/usr/bin/env python3
 from collections import defaultdict
-from typing import Any, DefaultDict, Dict, List, Optional, Set, Tuple
+from typing import Any, DefaultDict, Dict, List, Optional, Set
 from tqdm import tqdm
 import capnp
 
 import panda.python.uds as uds
 from cereal import car
-from common.params import Params
-from selfdrive.car.ecu_addrs import EcuAddrBusType, get_ecu_addrs
-from selfdrive.car.interfaces import get_interface_attr
-from selfdrive.car.fingerprints import FW_VERSIONS
-from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
-from system.swaglog import cloudlog
+from openpilot.common.params import Params
+from openpilot.selfdrive.car.ecu_addrs import get_ecu_addrs
+from openpilot.selfdrive.car.fw_query_definitions import AddrType, EcuAddrBusType
+from openpilot.selfdrive.car.interfaces import get_interface_attr
+from openpilot.selfdrive.car.fingerprints import FW_VERSIONS
+from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
+from openpilot.system.swaglog import cloudlog
 
 Ecu = car.CarParams.Ecu
 ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa]
@@ -35,8 +36,8 @@ def is_brand(brand: str, filter_brand: Optional[str]) -> bool:
 
 
 def build_fw_dict(fw_versions: List[capnp.lib.capnp._DynamicStructBuilder],
-                  filter_brand: Optional[str] = None) -> Dict[Tuple[int, Optional[int]], Set[bytes]]:
-  fw_versions_dict = defaultdict(set)
+                  filter_brand: Optional[str] = None) -> Dict[AddrType, Set[bytes]]:
+  fw_versions_dict: DefaultDict[AddrType, Set[bytes]] = defaultdict(set)
   for fw in fw_versions:
     if is_brand(fw.brand, filter_brand) and not fw.logging:
       sub_addr = fw.subAddress if fw.subAddress != 0 else None
@@ -44,8 +45,8 @@ def build_fw_dict(fw_versions: List[capnp.lib.capnp._DynamicStructBuilder],
   return dict(fw_versions_dict)
 
 
-def get_brand_addrs() -> Dict[str, Set[Tuple[int, Optional[int]]]]:
-  brand_addrs: DefaultDict[str, Set[Tuple[int, Optional[int]]]] = defaultdict(set)
+def get_brand_addrs() -> Dict[str, Set[AddrType]]:
+  brand_addrs: DefaultDict[str, Set[AddrType]] = defaultdict(set)
   for brand, cars in VERSIONS.items():
     # Add ecus in database + extra ecus to match against
     brand_addrs[brand] |= {(addr, sub_addr) for _, addr, sub_addr in FW_QUERY_CONFIGS[brand].extra_ecus}
@@ -54,7 +55,7 @@ def get_brand_addrs() -> Dict[str, Set[Tuple[int, Optional[int]]]]:
   return dict(brand_addrs)
 
 
-def match_fw_to_car_fuzzy(live_fw_versions, log=True, exclude=None):
+def match_fw_to_car_fuzzy(live_fw_versions, match_brand=None, log=True, exclude=None):
   """Do a fuzzy FW match. This function will return a match, and the number of firmware version
   that were matched uniquely to that specific car. If multiple ECUs uniquely match to different cars
   the match is rejected."""
@@ -62,6 +63,9 @@ def match_fw_to_car_fuzzy(live_fw_versions, log=True, exclude=None):
   # Build lookup table from (addr, sub_addr, fw) to list of candidate cars
   all_fw_versions = defaultdict(list)
   for candidate, fw_by_addr in FW_VERSIONS.items():
+    if not is_brand(MODEL_TO_BRAND[candidate], match_brand):
+      continue
+
     if candidate == exclude:
       continue
 
@@ -101,13 +105,14 @@ def match_fw_to_car_fuzzy(live_fw_versions, log=True, exclude=None):
     return set()
 
 
-def match_fw_to_car_exact(live_fw_versions, log=True) -> Set[str]:
+def match_fw_to_car_exact(live_fw_versions, match_brand=None, log=True) -> Set[str]:
   """Do an exact FW match. Returns all cars that match the given
   FW versions for a list of "essential" ECUs. If an ECU is not considered
   essential the FW version can be missing to get a fingerprint, but if it's present it
   needs to match the database."""
   invalid = set()
-  candidates = FW_VERSIONS
+  candidates = {c: f for c, f in FW_VERSIONS.items() if
+                is_brand(MODEL_TO_BRAND[c], match_brand)}
 
   for candidate, fws in candidates.items():
     config = FW_QUERY_CONFIGS[MODEL_TO_BRAND[candidate]]
@@ -129,7 +134,7 @@ def match_fw_to_car_exact(live_fw_versions, log=True) -> Set[str]:
       if ecu_type == Ecu.debug:
         continue
 
-      if not any([found_version in expected_versions for found_version in found_versions]):
+      if not any(found_version in expected_versions for found_version in found_versions):
         invalid.add(candidate)
         break
 
@@ -149,7 +154,7 @@ def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True, log=True):
     matches = set()
     for brand in VERSIONS.keys():
       fw_versions_dict = build_fw_dict(fw_versions, filter_brand=brand)
-      matches |= match_func(fw_versions_dict, log=log)
+      matches |= match_func(fw_versions_dict, match_brand=brand, log=log)
 
       # If specified and no matches so far, fall back to brand's fuzzy fingerprinting function
       config = FW_QUERY_CONFIGS[brand]
@@ -208,7 +213,7 @@ def get_brand_ecu_matches(ecu_rx_addrs):
   brand_addrs = get_brand_addrs()
   brand_matches = {brand: set() for brand, _, _ in REQUESTS}
 
-  brand_rx_offsets = set((brand, r.rx_offset) for brand, _, r in REQUESTS)
+  brand_rx_offsets = {(brand, r.rx_offset) for brand, _, r in REQUESTS}
   for addr, sub_addr, _ in ecu_rx_addrs:
     # Since we can't know what request an ecu responded to, add matches for all possible rx offsets
     for brand, rx_offset in brand_rx_offsets:
@@ -242,8 +247,9 @@ def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pand
 
     car_fw = get_fw_versions(logcan, sendcan, query_brand=brand, timeout=timeout, num_pandas=num_pandas, debug=debug, progress=progress)
     all_car_fw.extend(car_fw)
-    # Try to match using FW returned from this brand only
-    matches = match_fw_to_car_exact(build_fw_dict(car_fw))
+
+    # If there is a match using this brand's FW alone, finish querying early
+    _, matches = match_fw_to_car(car_fw, log=False)
     if len(matches) == 1:
       break
 
@@ -332,7 +338,7 @@ if __name__ == "__main__":
   import time
   import argparse
   import cereal.messaging as messaging
-  from selfdrive.car.vin import get_vin
+  from openpilot.selfdrive.car.vin import get_vin
 
   parser = argparse.ArgumentParser(description='Get firmware version of ECUs')
   parser.add_argument('--scan', action='store_true')
diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py
index f439eab486..f51cb75372 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.numpy_fast import interp
-from common.realtime import DT_CTRL
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import interp
+from openpilot.common.realtime import DT_CTRL
 from opendbc.can.packer import CANPacker
-from selfdrive.car import apply_driver_steer_torque_limits
-from selfdrive.car.gm import gmcan
-from selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons
+from openpilot.selfdrive.car import apply_driver_steer_torque_limits
+from openpilot.selfdrive.car.gm import gmcan
+from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 NetworkLocation = car.CarParams.NetworkLocation
@@ -111,7 +111,8 @@ class CarController:
 
         # GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation
         can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.enabled, at_full_stop))
-        can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, friction_brake_bus, self.apply_brake, idx, CC.enabled, near_stop, at_full_stop, self.CP))
+        can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, friction_brake_bus, self.apply_brake,
+                                                             idx, CC.enabled, near_stop, at_full_stop, self.CP))
 
         # Send dashboard UI commands (ACC status)
         send_fcw = hud_alert == VisualAlert.fcw
@@ -162,7 +163,8 @@ class CarController:
     lka_icon_status = (lka_active, lka_critical)
 
     # SW_GMLAN not yet on cam harness, no HUD alerts
-    if self.CP.networkLocation != NetworkLocation.fwdCamera and (self.frame % self.params.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last):
+    if self.CP.networkLocation != NetworkLocation.fwdCamera and \
+      (self.frame % self.params.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last):
       steer_alert = hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw)
       can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert))
       self.lka_icon_status_last = lka_icon_status
diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py
index 2f9c952876..89c1a3596a 100644
--- a/selfdrive/car/gm/carstate.py
+++ b/selfdrive/car/gm/carstate.py
@@ -1,11 +1,11 @@
 import copy
 from cereal import car
-from common.conversions import Conversions as CV
-from common.numpy_fast import mean
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import mean
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
-from selfdrive.car.interfaces import CarStateBase
-from selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD
+from openpilot.selfdrive.car.interfaces import CarStateBase
+from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD
 
 TransmissionType = car.CarParams.TransmissionType
 NetworkLocation = car.CarParams.NetworkLocation
@@ -98,7 +98,7 @@ class CarState(CarStateBase):
     ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1
     ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2
 
-    ret.parkingBrake = pt_cp.vl["VehicleIgnitionAlt"]["ParkBrake"] == 1
+    ret.parkingBrake = pt_cp.vl["BCMGeneralPlatformStatus"]["ParkBrakeSwActive"] == 1
     ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0
     ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
     ret.accFaulted = (pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED or
@@ -117,70 +117,25 @@ class CarState(CarStateBase):
 
   @staticmethod
   def get_cam_can_parser(CP):
-    signals = []
-    checks = []
+    messages = []
     if CP.networkLocation == NetworkLocation.fwdCamera:
-      signals += [
-        ("AEBCmdActive", "AEBCmd"),
-        ("RollingCounter", "ASCMLKASteeringCmd"),
-        ("ACCSpeedSetpoint", "ASCMActiveCruiseControlStatus"),
-        ("ACCCruiseState", "ASCMActiveCruiseControlStatus"),
-      ]
-      checks += [
+      messages += [
         ("AEBCmd", 10),
         ("ASCMLKASteeringCmd", 10),
         ("ASCMActiveCruiseControlStatus", 25),
       ]
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.CAMERA)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.CAMERA)
 
   @staticmethod
   def get_can_parser(CP):
-    signals = [
-      # sig_name, sig_address
-      ("BrakePedalPos", "ECMAcceleratorPos"),
-      ("FrontLeftDoor", "BCMDoorBeltStatus"),
-      ("FrontRightDoor", "BCMDoorBeltStatus"),
-      ("RearLeftDoor", "BCMDoorBeltStatus"),
-      ("RearRightDoor", "BCMDoorBeltStatus"),
-      ("LeftSeatBelt", "BCMDoorBeltStatus"),
-      ("RightSeatBelt", "BCMDoorBeltStatus"),
-      ("TurnSignals", "BCMTurnSignals"),
-      ("AcceleratorPedal2", "AcceleratorPedal2"),
-      ("CruiseState", "AcceleratorPedal2"),
-      ("ACCButtons", "ASCMSteeringButton"),
-      ("RollingCounter", "ASCMSteeringButton"),
-      ("SteeringWheelAngle", "PSCMSteeringAngle"),
-      ("SteeringWheelRate", "PSCMSteeringAngle"),
-      ("FLWheelSpd", "EBCMWheelSpdFront"),
-      ("FRWheelSpd", "EBCMWheelSpdFront"),
-      ("RLWheelSpd", "EBCMWheelSpdRear"),
-      ("RRWheelSpd", "EBCMWheelSpdRear"),
-      ("MovingBackward", "EBCMWheelSpdRear"),
-      ("FrictionBrakeUnavailable", "EBCMFrictionBrakeStatus"),
-      ("PRNDL2", "ECMPRDNL2"),
-      ("ManualMode", "ECMPRDNL2"),
-      ("LKADriverAppldTrq", "PSCMStatus"),
-      ("LKATorqueDelivered", "PSCMStatus"),
-      ("LKATorqueDeliveredStatus", "PSCMStatus"),
-      ("HandsOffSWlDetectionStatus", "PSCMStatus"),
-      ("HandsOffSWDetectionMode", "PSCMStatus"),
-      ("LKATotalTorqueDelivered", "PSCMStatus"),
-      ("PSCMStatusChecksum", "PSCMStatus"),
-      ("RollingCounter", "PSCMStatus"),
-      ("TractionControlOn", "ESPStatus"),
-      ("ParkBrake", "VehicleIgnitionAlt"),
-      ("CruiseMainOn", "ECMEngineStatus"),
-      ("BrakePressed", "ECMEngineStatus"),
-    ]
-
-    checks = [
+    messages = [
       ("BCMTurnSignals", 1),
       ("ECMPRDNL2", 10),
       ("PSCMStatus", 10),
       ("ESPStatus", 10),
       ("BCMDoorBeltStatus", 10),
-      ("VehicleIgnitionAlt", 10),
+      ("BCMGeneralPlatformStatus", 10),
       ("EBCMWheelSpdFront", 20),
       ("EBCMWheelSpdRear", 20),
       ("EBCMFrictionBrakeStatus", 20),
@@ -193,27 +148,19 @@ class CarState(CarStateBase):
 
     # Used to read back last counter sent to PT by camera
     if CP.networkLocation == NetworkLocation.fwdCamera:
-      signals += [
-        ("RollingCounter", "ASCMLKASteeringCmd"),
-      ]
-      checks += [
+      messages += [
         ("ASCMLKASteeringCmd", 0),
       ]
 
     if CP.transmissionType == TransmissionType.direct:
-      signals.append(("RegenPaddle", "EBCMRegenPaddle"))
-      checks.append(("EBCMRegenPaddle", 50))
+      messages.append(("EBCMRegenPaddle", 50))
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.POWERTRAIN)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.POWERTRAIN)
 
   @staticmethod
   def get_loopback_can_parser(CP):
-    signals = [
-      ("RollingCounter", "ASCMLKASteeringCmd"),
-    ]
-
-    checks = [
+    messages = [
       ("ASCMLKASteeringCmd", 0),
     ]
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.LOOPBACK, enforce_checks=False)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.LOOPBACK)
diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py
index 0de2066678..bd1e29ce3b 100644
--- a/selfdrive/car/gm/gmcan.py
+++ b/selfdrive/car/gm/gmcan.py
@@ -1,5 +1,5 @@
-from selfdrive.car import make_can_msg
-from selfdrive.car.gm.values import CAR
+from openpilot.selfdrive.car import make_can_msg
+from openpilot.selfdrive.car.gm.values import CAR
 
 
 def create_buttons(packer, bus, idx, button):
diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py
index e17346cfaf..a0defb7cfb 100755
--- a/selfdrive/car/gm/interface.py
+++ b/selfdrive/car/gm/interface.py
@@ -3,12 +3,12 @@ from cereal import car
 from math import fabs, exp
 from panda import Panda
 
-from common.conversions import Conversions as CV
-from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config
-from selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG
-from selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus
-from selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD
-from selfdrive.controls.lib.drive_helpers import get_friction
+from openpilot.common.conversions import Conversions as CV
+from openpilot.selfdrive.car import create_button_events, get_safety_config
+from openpilot.selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG
+from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus
+from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD
+from openpilot.selfdrive.controls.lib.drive_helpers import get_friction
 
 ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
@@ -21,7 +21,8 @@ BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.D
 
 NON_LINEAR_TORQUE_PARAMS = {
   CAR.BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178],
-  CAR.ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772]
+  CAR.ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772],
+  CAR.SILVERADO: [3.29974374, 1.0, 0.25571356, 0.0465122]
 }
 
 
@@ -128,17 +129,17 @@ class CarInterface(CarInterfaceBase):
     ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]]
     ret.lateralTuning.pid.kf = 0.00004   # full torque for 20 deg at 80mph means 0.00007818594
     ret.steerActuatorDelay = 0.1  # Default delay, not measured yet
-    tire_stiffness_factor = 0.444  # not optimized yet
+    ret.tireStiffnessFactor = 0.444  # not optimized yet
 
     ret.steerLimitTimer = 0.4
     ret.radarTimeStep = 0.0667  # GM radar runs at 15Hz instead of standard 20Hz
     ret.longitudinalActuatorDelayUpperBound = 0.5  # large delay to initially start braking
 
     if candidate == CAR.VOLT:
-      ret.mass = 1607. + STD_CARGO_KG
+      ret.mass = 1607.
       ret.wheelbase = 2.69
       ret.steerRatio = 17.7  # Stock 15.7, LiveParameters
-      tire_stiffness_factor = 0.469  # Stock Michelin Energy Saver A/S, LiveParameters
+      ret.tireStiffnessFactor = 0.469  # Stock Michelin Energy Saver A/S, LiveParameters
       ret.centerToFront = ret.wheelbase * 0.45  # Volt Gen 1, TODO corner weigh
 
       ret.lateralTuning.pid.kpBP = [0., 40.]
@@ -149,13 +150,13 @@ class CarInterface(CarInterfaceBase):
       ret.steerActuatorDelay = 0.2
 
     elif candidate == CAR.MALIBU:
-      ret.mass = 1496. + STD_CARGO_KG
+      ret.mass = 1496.
       ret.wheelbase = 2.83
       ret.steerRatio = 15.8
       ret.centerToFront = ret.wheelbase * 0.4  # wild guess
 
     elif candidate == CAR.HOLDEN_ASTRA:
-      ret.mass = 1363. + STD_CARGO_KG
+      ret.mass = 1363.
       ret.wheelbase = 2.662
       # Remaining parameters copied from Volt for now
       ret.centerToFront = ret.wheelbase * 0.4
@@ -163,7 +164,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate == CAR.ACADIA:
       ret.minEnableSpeed = -1.  # engage speed is decided by pcm
-      ret.mass = 4353. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 4353. * CV.LB_TO_KG
       ret.wheelbase = 2.86
       ret.steerRatio = 14.4  # end to end is 13.46
       ret.centerToFront = ret.wheelbase * 0.4
@@ -171,58 +172,63 @@ class CarInterface(CarInterfaceBase):
       CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
 
     elif candidate == CAR.BUICK_LACROSSE:
-      ret.mass = 1712. + STD_CARGO_KG
+      ret.mass = 1712.
       ret.wheelbase = 2.91
       ret.steerRatio = 15.8
       ret.centerToFront = ret.wheelbase * 0.4  # wild guess
       CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
 
     elif candidate == CAR.BUICK_REGAL:
-      ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG  # (3849+3708)/2
+      ret.mass = 3779. * CV.LB_TO_KG  # (3849+3708)/2
       ret.wheelbase = 2.83  # 111.4 inches in meters
       ret.steerRatio = 14.4  # guess for tourx
       ret.centerToFront = ret.wheelbase * 0.4  # guess for tourx
 
     elif candidate == CAR.CADILLAC_ATS:
-      ret.mass = 1601. + STD_CARGO_KG
+      ret.mass = 1601.
       ret.wheelbase = 2.78
       ret.steerRatio = 15.3
       ret.centerToFront = ret.wheelbase * 0.5
 
     elif candidate == CAR.ESCALADE:
       ret.minEnableSpeed = -1.  # engage speed is decided by pcm
-      ret.mass = 5653. * CV.LB_TO_KG + STD_CARGO_KG  # (5552+5815)/2
+      ret.mass = 5653. * CV.LB_TO_KG  # (5552+5815)/2
       ret.wheelbase = 2.95  # 116 inches in meters
       ret.steerRatio = 17.3
       ret.centerToFront = ret.wheelbase * 0.5
       CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
 
-    elif candidate == CAR.ESCALADE_ESV:
+    elif candidate in (CAR.ESCALADE_ESV, CAR.ESCALADE_ESV_2019):
       ret.minEnableSpeed = -1.  # engage speed is decided by pcm
-      ret.mass = 2739. + STD_CARGO_KG
+      ret.mass = 2739.
       ret.wheelbase = 3.302
       ret.steerRatio = 17.3
       ret.centerToFront = ret.wheelbase * 0.5
-      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]]
-      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]]
-      ret.lateralTuning.pid.kf = 0.000045
-      tire_stiffness_factor = 1.0
+      ret.tireStiffnessFactor = 1.0
+
+      if candidate == CAR.ESCALADE_ESV:
+        ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]]
+        ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]]
+        ret.lateralTuning.pid.kf = 0.000045
+      else:
+        ret.steerActuatorDelay = 0.2
+        CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
 
     elif candidate == CAR.BOLT_EUV:
-      ret.mass = 1669. + STD_CARGO_KG
+      ret.mass = 1669.
       ret.wheelbase = 2.63779
       ret.steerRatio = 16.8
       ret.centerToFront = ret.wheelbase * 0.4
-      tire_stiffness_factor = 1.0
+      ret.tireStiffnessFactor = 1.0
       ret.steerActuatorDelay = 0.2
       CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
 
     elif candidate == CAR.SILVERADO:
-      ret.mass = 2200. + STD_CARGO_KG
+      ret.mass = 2450.
       ret.wheelbase = 3.75
       ret.steerRatio = 16.3
       ret.centerToFront = ret.wheelbase * 0.5
-      tire_stiffness_factor = 1.0
+      ret.tireStiffnessFactor = 1.0
       # On the Bolt, the ECM and camera independently check that you are either above 5 kph or at a stop
       # with foot on brake to allow engagement, but this platform only has that check in the camera.
       # TODO: check if this is split by EV/ICE with more platforms in the future
@@ -231,39 +237,31 @@ class CarInterface(CarInterfaceBase):
       CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
 
     elif candidate == CAR.EQUINOX:
-      ret.mass = 3500. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 3500. * CV.LB_TO_KG
       ret.wheelbase = 2.72
       ret.steerRatio = 14.4
       ret.centerToFront = ret.wheelbase * 0.4
       CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
 
     elif candidate == CAR.TRAILBLAZER:
-      ret.mass = 1345. + STD_CARGO_KG
+      ret.mass = 1345.
       ret.wheelbase = 2.64
       ret.steerRatio = 16.8
       ret.centerToFront = ret.wheelbase * 0.4
-      tire_stiffness_factor = 1.0
+      ret.tireStiffnessFactor = 1.0
       ret.steerActuatorDelay = 0.2
       CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
 
-    # TODO: start from empirically derived lateral slip stiffness for the civic and scale by
-    # mass and CG position, so all cars will have approximately similar dyn behaviors
-    ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
-                                                                         tire_stiffness_factor=tire_stiffness_factor)
-
     return ret
 
   # returns a car.CarState
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback)
 
-    if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT:
-      buttonEvents = [create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS)]
-      # Handle ACCButtons changing buttons mid-press
-      if self.CS.cruise_buttons != CruiseButtons.UNPRESS and self.CS.prev_cruise_buttons != CruiseButtons.UNPRESS:
-        buttonEvents.append(create_button_event(CruiseButtons.UNPRESS, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS))
-
-      ret.buttonEvents = buttonEvents
+    # Don't add event if transitioning from INIT, unless it's to an actual button
+    if self.CS.cruise_buttons != CruiseButtons.UNPRESS or self.CS.prev_cruise_buttons != CruiseButtons.INIT:
+      ret.buttonEvents = create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT,
+                                              unpressed_btn=CruiseButtons.UNPRESS)
 
     # The ECM allows enabling on falling edge of set, but only rising edge of resume
     events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low,
diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py
index b86a85f915..b893babd31 100755
--- a/selfdrive/car/gm/radar_interface.py
+++ b/selfdrive/car/gm/radar_interface.py
@@ -1,10 +1,10 @@
 #!/usr/bin/env python3
 import math
 from cereal import car
-from common.conversions import Conversions as CV
+from openpilot.common.conversions import Conversions as CV
 from opendbc.can.parser import CANParser
-from selfdrive.car.gm.values import DBC, CanBus
-from selfdrive.car.interfaces import RadarInterfaceBase
+from openpilot.selfdrive.car.gm.values import DBC, CanBus
+from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
 
 RADAR_HEADER_MSG = 1120
 SLOT_1_MSG = RADAR_HEADER_MSG + 1
@@ -25,11 +25,11 @@ def create_radar_can_parser(car_fingerprint):
                      ['TrkRange'] * NUM_SLOTS + ['TrkRangeRate'] * NUM_SLOTS +
                      ['TrkRangeAccel'] * NUM_SLOTS + ['TrkAzimuth'] * NUM_SLOTS +
                      ['TrkWidth'] * NUM_SLOTS + ['TrkObjectID'] * NUM_SLOTS,
-                     [RADAR_HEADER_MSG] * 7 + radar_targets * 6))
+                     [RADAR_HEADER_MSG] * 7 + radar_targets * 6, strict=True))
 
-  checks = list({(s[1], 14) for s in signals})
+  messages = list({(s[1], 14) for s in signals})
 
-  return CANParser(DBC[car_fingerprint]['radar'], signals, checks, CanBus.OBSTACLE)
+  return CANParser(DBC[car_fingerprint]['radar'], messages, CanBus.OBSTACLE)
 
 
 class RadarInterface(RadarInterfaceBase):
diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py
index 4919a13288..e9ec8de9c5 100644
--- a/selfdrive/car/gm/values.py
+++ b/selfdrive/car/gm/values.py
@@ -1,11 +1,12 @@
+# ruff: noqa: E501
 from collections import defaultdict
 from dataclasses import dataclass
-from enum import Enum
+from enum import Enum, StrEnum
 from typing import Dict, List, Union
 
 from cereal import car
-from selfdrive.car import dbc_dict
-from selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column
+from openpilot.selfdrive.car import dbc_dict
+from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column
 Ecu = car.CarParams.Ecu
 
 
@@ -60,7 +61,7 @@ class CarControllerParams:
     self.BRAKE_LOOKUP_V = [self.MAX_BRAKE, 0.]
 
 
-class CAR:
+class CAR(StrEnum):
   HOLDEN_ASTRA = "HOLDEN ASTRA RS-V BK 2017"
   VOLT = "CHEVROLET VOLT PREMIER 2017"
   CADILLAC_ATS = "CADILLAC ATS Premium Performance 2018"
@@ -70,6 +71,7 @@ class CAR:
   BUICK_REGAL = "BUICK REGAL ESSENCE 2018"
   ESCALADE = "CADILLAC ESCALADE 2017"
   ESCALADE_ESV = "CADILLAC ESCALADE ESV 2016"
+  ESCALADE_ESV_2019 = "CADILLAC ESCALADE ESV 2019"
   BOLT_EUV = "CHEVROLET BOLT EUV 2022"
   SILVERADO = "CHEVROLET SILVERADO 1500 2020"
   EQUINOX = "CHEVROLET EQUINOX 2019"
@@ -105,6 +107,7 @@ CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = {
   CAR.BUICK_REGAL: GMCarInfo("Buick Regal Essence 2018"),
   CAR.ESCALADE: GMCarInfo("Cadillac Escalade 2017", "Driver Assist Package"),
   CAR.ESCALADE_ESV: GMCarInfo("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS"),
+  CAR.ESCALADE_ESV_2019: GMCarInfo("Cadillac Escalade ESV 2019", "Adaptive Cruise Control (ACC) & LKAS"),
   CAR.BOLT_EUV: [
     GMCarInfo("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", video_link="https://youtu.be/xvwzGMUA210"),
     GMCarInfo("Chevrolet Bolt EV 2022-23", "2LT Trim with Adaptive Cruise Control Package"),
@@ -197,6 +200,10 @@ FINGERPRINTS = {
   {
     309: 1, 848: 8, 849: 8, 850: 8, 851: 8, 852: 8, 853: 8, 854: 3, 1056: 6, 1057: 8, 1058: 8, 1059: 8, 1060: 8, 1061: 8, 1062: 8, 1063: 8, 1064: 8, 1065: 8, 1066: 8, 1067: 8, 1068: 8, 1120: 8, 1121: 8, 1122: 8, 1123: 8, 1124: 8, 1125: 8, 1126: 8, 1127: 8, 1128: 8, 1129: 8, 1130: 8, 1131: 8, 1132: 8, 1133: 8, 1134: 8, 1135: 8, 1136: 8, 1137: 8, 1138: 8, 1139: 8, 1140: 8, 1141: 8, 1142: 8, 1143: 8, 1146: 8, 1147: 8, 1148: 8, 1149: 8, 1150: 8, 1151: 8, 1216: 8, 1217: 8, 1218: 8, 1219: 8, 1220: 8, 1221: 8, 1222: 8, 1223: 8, 1224: 8, 1225: 8, 1226: 8, 1232: 8, 1233: 8, 1234: 8, 1235: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1240: 8, 1241: 8, 1242: 8, 1787: 8, 1788: 8
   }],
+  CAR.ESCALADE_ESV_2019: [
+  {
+    715: 8, 840: 5, 717: 5, 869: 4, 880: 6, 289: 8, 454: 8, 842: 5, 460: 5, 463: 3, 801: 8, 170: 8, 190: 6, 241: 6, 201: 8, 417: 7, 211: 2, 419: 1, 398: 8, 426: 7, 487: 8, 442: 8, 451: 8, 452: 8, 453: 6, 479: 3, 311: 8, 500: 6, 647: 6, 193: 8, 707: 8, 197: 8, 209: 7, 199: 4, 455: 7, 313: 8, 481: 7, 485: 8, 489: 8, 249: 8, 393: 7, 407: 7, 413: 8, 422: 4, 431: 8, 501: 8, 499: 3, 810: 8, 508: 8, 381: 8, 462: 4, 532: 6, 562: 8, 386: 8, 761: 7, 573: 1, 554: 3, 719: 5, 560: 8, 1279: 4, 388: 8, 288: 5, 1005: 6, 497: 8, 844: 8, 961: 8, 967: 4, 977: 8, 979: 8, 985: 5, 1001: 8, 1017: 8, 1019: 2, 1020: 8, 1217: 8, 510: 8, 866: 4, 304: 1, 969: 8, 384: 4, 1033: 7, 1009: 8, 1034: 7, 1296: 4, 1930: 7, 1105: 5, 1013: 5, 1225: 7, 1919: 7, 320: 3, 534: 2, 352: 5, 298: 8, 1223: 2, 1233: 8, 608: 8, 1265: 8, 609: 6, 1267: 1, 1417: 8, 610: 6, 1906: 7, 611: 6, 612: 8, 613: 8, 208: 8, 564: 5, 309: 8, 1221: 5, 1280: 4, 1249: 8, 1907: 7, 1257: 6, 1300: 8, 1920: 7, 563: 5, 1322: 6, 1323: 4, 1328: 4, 1917: 7, 328: 1, 1912: 7, 1914: 7, 804: 3, 1918: 7
+  }],
   CAR.BOLT_EUV: [
   {
     189: 7, 190: 7, 193: 8, 197: 8, 201: 8, 209: 7, 211: 3, 241: 6, 257: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 451: 8, 452: 8, 453: 6, 458: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 566: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 869: 4, 880: 6, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1280: 4, 1296: 4, 1300: 8, 1611: 8, 1930: 7
@@ -211,10 +218,10 @@ FINGERPRINTS = {
   }],
   # Trailblazer also matches as a Silverado, so comment out to avoid conflicts.
   # TODO: split with FW versions
-  CAR.TRAILBLAZER: [
-  {
+  # CAR.TRAILBLAZER: [
+  # {
   #   190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 587: 8, 707: 8, 715: 8, 717: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1609: 8, 1611: 8, 1613: 8, 1649: 8, 1792: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 2001: 8, 2017: 8, 2018: 8, 2020: 8
-  }],
+  # }],
 }
 
 GM_RX_OFFSET = 0x400
diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py
index 347c16c86f..056b47c4b3 100644
--- a/selfdrive/car/honda/carcontroller.py
+++ b/selfdrive/car/honda/carcontroller.py
@@ -1,13 +1,13 @@
 from collections import namedtuple
 
 from cereal import car
-from common.numpy_fast import clip, interp
-from common.realtime import DT_CTRL
+from openpilot.common.numpy_fast import clip, interp
+from openpilot.common.realtime import DT_CTRL
 from opendbc.can.packer import CANPacker
-from selfdrive.car import create_gas_interceptor_command
-from selfdrive.car.honda import hondacan
-from selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams
-from selfdrive.controls.lib.drive_helpers import rate_limit
+from openpilot.selfdrive.car import create_gas_interceptor_command
+from openpilot.selfdrive.car.honda import hondacan
+from openpilot.selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams
+from openpilot.selfdrive.controls.lib.drive_helpers import rate_limit
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 LongCtrlState = car.CarControl.Actuators.LongControlState
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index 35d6279902..03aedb31d2 100644
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@@ -1,50 +1,21 @@
 from collections import defaultdict
 
 from cereal import car
-from common.conversions import Conversions as CV
-from common.numpy_fast import interp
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import interp
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
-from selfdrive.car.honda.hondacan import get_cruise_speed_conversion, get_pt_bus
-from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL, HONDA_BOSCH_RADARLESS
-from selfdrive.car.interfaces import CarStateBase
+from openpilot.selfdrive.car.honda.hondacan import get_cruise_speed_conversion, get_pt_bus
+from openpilot.selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, \
+                                                 HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL, \
+                                                 HONDA_BOSCH_RADARLESS
+from openpilot.selfdrive.car.interfaces import CarStateBase
 
 TransmissionType = car.CarParams.TransmissionType
 
 
-def get_can_signals(CP, gearbox_msg, main_on_sig_msg):
-  signals = [
-    ("XMISSION_SPEED", "ENGINE_DATA"),
-    ("WHEEL_SPEED_FL", "WHEEL_SPEEDS"),
-    ("WHEEL_SPEED_FR", "WHEEL_SPEEDS"),
-    ("WHEEL_SPEED_RL", "WHEEL_SPEEDS"),
-    ("WHEEL_SPEED_RR", "WHEEL_SPEEDS"),
-    ("STEER_ANGLE", "STEERING_SENSORS"),
-    ("STEER_ANGLE_RATE", "STEERING_SENSORS"),
-    ("MOTOR_TORQUE", "STEER_MOTOR_TORQUE"),
-    ("STEER_TORQUE_SENSOR", "STEER_STATUS"),
-    ("IMPERIAL_UNIT", "CAR_SPEED"),
-    ("ROUGH_CAR_SPEED_2", "CAR_SPEED"),
-    ("LEFT_BLINKER", "SCM_FEEDBACK"),
-    ("RIGHT_BLINKER", "SCM_FEEDBACK"),
-    ("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS"),
-    ("SEATBELT_DRIVER_LATCHED", "SEATBELT_STATUS"),
-    ("BRAKE_PRESSED", "POWERTRAIN_DATA"),
-    ("BRAKE_SWITCH", "POWERTRAIN_DATA"),
-    ("CRUISE_BUTTONS", "SCM_BUTTONS"),
-    ("ESP_DISABLED", "VSA_STATUS"),
-    ("USER_BRAKE", "VSA_STATUS"),
-    ("BRAKE_HOLD_ACTIVE", "VSA_STATUS"),
-    ("STEER_STATUS", "STEER_STATUS"),
-    ("GEAR_SHIFTER", gearbox_msg),
-    ("GEAR", gearbox_msg),
-    ("PEDAL_GAS", "POWERTRAIN_DATA"),
-    ("CRUISE_SETTING", "SCM_BUTTONS"),
-    ("ACC_STATUS", "POWERTRAIN_DATA"),
-    ("MAIN_ON", main_on_sig_msg),
-  ]
-
-  checks = [
+def get_can_messages(CP, gearbox_msg):
+  messages = [
     ("ENGINE_DATA", 100),
     ("WHEEL_SPEEDS", 50),
     ("STEERING_SENSORS", 100),
@@ -58,79 +29,59 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg):
   ]
 
   if CP.carFingerprint == CAR.ODYSSEY_CHN:
-    checks += [
+    messages += [
       ("SCM_FEEDBACK", 25),
       ("SCM_BUTTONS", 50),
     ]
   else:
-    checks += [
+    messages += [
       ("SCM_FEEDBACK", 10),
       ("SCM_BUTTONS", 25),
     ]
 
   if CP.carFingerprint in (CAR.CRV_HYBRID, CAR.CIVIC_BOSCH_DIESEL, CAR.ACURA_RDX_3G, CAR.HONDA_E):
-    checks.append((gearbox_msg, 50))
+    messages.append((gearbox_msg, 50))
   else:
-    checks.append((gearbox_msg, 100))
+    messages.append((gearbox_msg, 100))
 
   if CP.carFingerprint in HONDA_BOSCH_ALT_BRAKE_SIGNAL:
-    signals.append(("BRAKE_PRESSED", "BRAKE_MODULE"))
-    checks.append(("BRAKE_MODULE", 50))
+    messages.append(("BRAKE_MODULE", 50))
 
   if CP.carFingerprint in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN}):
-    signals.append(("EPB_STATE", "EPB_STATUS"))
-    checks.append(("EPB_STATUS", 50))
+    messages.append(("EPB_STATUS", 50))
 
   if CP.carFingerprint in HONDA_BOSCH:
     # these messages are on camera bus on radarless cars
     if not CP.openpilotLongitudinalControl and CP.carFingerprint not in HONDA_BOSCH_RADARLESS:
-      signals += [
-        ("CRUISE_CONTROL_LABEL", "ACC_HUD"),
-        ("CRUISE_SPEED", "ACC_HUD"),
-        ("ACCEL_COMMAND", "ACC_CONTROL"),
-        ("AEB_STATUS", "ACC_CONTROL"),
-      ]
-      checks += [
+      messages += [
         ("ACC_HUD", 10),
         ("ACC_CONTROL", 50),
       ]
   else:  # Nidec signals
-    signals += [("CRUISE_SPEED_PCM", "CRUISE"),
-                ("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS")]
-
     if CP.carFingerprint == CAR.ODYSSEY_CHN:
-      checks.append(("CRUISE_PARAMS", 10))
+      messages.append(("CRUISE_PARAMS", 10))
     else:
-      checks.append(("CRUISE_PARAMS", 50))
+      messages.append(("CRUISE_PARAMS", 50))
 
-  if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G):
-    signals.append(("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK"))
+  # TODO: clean this up
+  if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT,
+                           CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G):
+    pass
   elif CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV):
-    signals.append(("DRIVERS_DOOR_OPEN", "SCM_BUTTONS"))
+    pass
   else:
-    signals += [("DOOR_OPEN_FL", "DOORS_STATUS"),
-                ("DOOR_OPEN_FR", "DOORS_STATUS"),
-                ("DOOR_OPEN_RL", "DOORS_STATUS"),
-                ("DOOR_OPEN_RR", "DOORS_STATUS")]
-    checks.append(("DOORS_STATUS", 3))
+    messages.append(("DOORS_STATUS", 3))
 
   # add gas interceptor reading if we are using it
   if CP.enableGasInterceptor:
-    signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR"))
-    signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR"))
-    checks.append(("GAS_SENSOR", 50))
+    messages.append(("GAS_SENSOR", 50))
 
   if CP.carFingerprint in HONDA_BOSCH_RADARLESS:
-    signals.append(("CRUISE_FAULT", "CRUISE_FAULT_STATUS"))
-    checks.append(("CRUISE_FAULT_STATUS", 50))
+    messages.append(("CRUISE_FAULT_STATUS", 50))
   elif CP.openpilotLongitudinalControl:
-    signals += [
-      ("BRAKE_ERROR_1", "STANDSTILL"),
-      ("BRAKE_ERROR_2", "STANDSTILL")
-    ]
-    checks.append(("STANDSTILL", 50))
+    messages.append(("STANDSTILL", 50))
 
-  return signals, checks
+  return messages
 
 
 class CarState(CarStateBase):
@@ -178,7 +129,8 @@ class CarState(CarStateBase):
     # panda checks if the signal is non-zero
     ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 1e-5
     # TODO: find a common signal across all cars
-    if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G):
+    if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT,
+                                  CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G):
       ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"])
     elif self.CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV):
       ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"])
@@ -314,55 +266,36 @@ class CarState(CarStateBase):
     return ret
 
   def get_can_parser(self, CP):
-    signals, checks = get_can_signals(CP, self.gearbox_msg, self.main_on_sig_msg)
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, get_pt_bus(CP.carFingerprint))
+    messages = get_can_messages(CP, self.gearbox_msg)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, get_pt_bus(CP.carFingerprint))
 
   @staticmethod
   def get_cam_can_parser(CP):
-    signals = []
-    checks = [
+    messages = [
       ("STEERING_CONTROL", 100),
     ]
 
     if CP.carFingerprint in HONDA_BOSCH_RADARLESS:
-      signals.append(("LKAS_PROBLEM", "LKAS_HUD"))
-      checks.append(("LKAS_HUD", 10))
+      messages.append(("LKAS_HUD", 10))
       if not CP.openpilotLongitudinalControl:
-        signals += [
-          ("CRUISE_SPEED", "ACC_HUD"),
-          ("CRUISE_CONTROL_LABEL", "ACC_HUD"),
-        ]
-        checks.append(("ACC_HUD", 10))
+        messages.append(("ACC_HUD", 10))
 
     elif CP.carFingerprint not in HONDA_BOSCH:
-      signals += [("COMPUTER_BRAKE", "BRAKE_COMMAND"),
-                  ("AEB_REQ_1", "BRAKE_COMMAND"),
-                  ("FCW", "BRAKE_COMMAND"),
-                  ("CHIME", "BRAKE_COMMAND"),
-                  ("LKAS_PROBLEM", "LKAS_HUD"),
-                  ("FCM_OFF", "ACC_HUD"),
-                  ("FCM_OFF_2", "ACC_HUD"),
-                  ("FCM_PROBLEM", "ACC_HUD"),
-                  ("ACC_PROBLEM", "ACC_HUD"),
-                  ("ICONS", "ACC_HUD")]
-      checks += [
+      messages += [
         ("ACC_HUD", 10),
         ("LKAS_HUD", 10),
         ("BRAKE_COMMAND", 50),
       ]
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
 
   @staticmethod
   def get_body_can_parser(CP):
     if CP.enableBsm:
-      signals = [("BSM_ALERT", "BSM_STATUS_RIGHT"),
-                 ("BSM_ALERT", "BSM_STATUS_LEFT")]
-
-      checks = [
+      messages = [
         ("BSM_STATUS_LEFT", 3),
         ("BSM_STATUS_RIGHT", 3),
       ]
       bus_body = 0 # B-CAN is forwarded to ACC-CAN radar side (CAN 0 on fake ethernet port)
-      return CANParser(DBC[CP.carFingerprint]["body"], signals, checks, bus_body)
+      return CANParser(DBC[CP.carFingerprint]["body"], messages, bus_body)
     return None
diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py
index 1fe0a13767..a8cbad78ce 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, HONDA_BOSCH_RADARLESS, CAR, CarControllerParams
+from openpilot.common.conversions import Conversions as CV
+from openpilot.selfdrive.car.honda.values import HondaFlags, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, CAR, CarControllerParams
 
 # 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 11eff61b33..d1a287a76a 100755
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@@ -1,12 +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 selfdrive.car.honda.values import CarControllerParams, CruiseButtons, HondaFlags, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL, HONDA_BOSCH_RADARLESS
-from selfdrive.car import STD_CARGO_KG, CivicParams, create_button_event, scale_tire_stiffness, get_safety_config
-from selfdrive.car.interfaces import CarInterfaceBase
-from selfdrive.car.disable_ecu import disable_ecu
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import interp
+from openpilot.selfdrive.car.honda.values import CarControllerParams, CruiseButtons, HondaFlags, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, \
+                                                                                            HONDA_BOSCH_ALT_BRAKE_SIGNAL, HONDA_BOSCH_RADARLESS
+from openpilot.selfdrive.car import create_button_events, get_safety_config
+from openpilot.selfdrive.car.interfaces import CarInterfaceBase
+from openpilot.selfdrive.car.disable_ecu import disable_ecu
 
 
 ButtonType = car.CarState.ButtonEvent.Type
@@ -88,9 +89,9 @@ class CarInterface(CarInterfaceBase):
         eps_modified = True
 
     if candidate == CAR.CIVIC:
-      ret.mass = CivicParams.MASS
-      ret.wheelbase = CivicParams.WHEELBASE
-      ret.centerToFront = CivicParams.CENTER_TO_FRONT
+      ret.mass = 1326.
+      ret.wheelbase = 2.70
+      ret.centerToFront = ret.wheelbase * 0.4
       ret.steerRatio = 15.38  # 10.93 is end-to-end spec
       if eps_modified:
         # stock request input values:     0x0000, 0x00DE, 0x014D, 0x01EF, 0x0290, 0x0377, 0x0454, 0x0610, 0x06EE
@@ -104,24 +105,22 @@ class CarInterface(CarInterfaceBase):
       else:
         ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
         ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]]
-      tire_stiffness_factor = 1.
 
     elif candidate in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CIVIC_2022):
-      ret.mass = CivicParams.MASS
-      ret.wheelbase = CivicParams.WHEELBASE
-      ret.centerToFront = CivicParams.CENTER_TO_FRONT
+      ret.mass = 1326.
+      ret.wheelbase = 2.70
+      ret.centerToFront = ret.wheelbase * 0.4
       ret.steerRatio = 15.38  # 10.93 is end-to-end spec
       ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]  # TODO: determine if there is a dead zone at the top end
-      tire_stiffness_factor = 1.
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
 
     elif candidate in (CAR.ACCORD, CAR.ACCORDH):
-      ret.mass = 3279. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 3279. * CV.LB_TO_KG
       ret.wheelbase = 2.83
       ret.centerToFront = ret.wheelbase * 0.39
       ret.steerRatio = 16.33  # 11.82 is spec end-to-end
       ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]  # TODO: determine if there is a dead zone at the top end
-      tire_stiffness_factor = 0.8467
+      ret.tireStiffnessFactor = 0.8467
 
       if eps_modified:
         ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]]
@@ -129,26 +128,26 @@ class CarInterface(CarInterfaceBase):
         ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
 
     elif candidate == CAR.ACURA_ILX:
-      ret.mass = 3095. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 3095. * CV.LB_TO_KG
       ret.wheelbase = 2.67
       ret.centerToFront = ret.wheelbase * 0.37
       ret.steerRatio = 18.61  # 15.3 is spec end-to-end
       ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]  # TODO: determine if there is a dead zone at the top end
-      tire_stiffness_factor = 0.72
+      ret.tireStiffnessFactor = 0.72
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
 
     elif candidate in (CAR.CRV, CAR.CRV_EU):
-      ret.mass = 3572. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 3572. * CV.LB_TO_KG
       ret.wheelbase = 2.62
       ret.centerToFront = ret.wheelbase * 0.41
       ret.steerRatio = 16.89  # as spec
       ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]]  # TODO: determine if there is a dead zone at the top end
-      tire_stiffness_factor = 0.444
+      ret.tireStiffnessFactor = 0.444
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
       ret.wheelSpeedFactor = 1.025
 
     elif candidate == CAR.CRV_5G:
-      ret.mass = 3410. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 3410. * CV.LB_TO_KG
       ret.wheelbase = 2.66
       ret.centerToFront = ret.wheelbase * 0.41
       ret.steerRatio = 16.0  # 12.3 is spec end-to-end
@@ -161,45 +160,45 @@ class CarInterface(CarInterfaceBase):
       else:
         ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
         ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]]
-      tire_stiffness_factor = 0.677
+      ret.tireStiffnessFactor = 0.677
       ret.wheelSpeedFactor = 1.025
 
     elif candidate == CAR.CRV_HYBRID:
-      ret.mass = 1667. + STD_CARGO_KG  # mean of 4 models in kg
+      ret.mass = 1667.  # mean of 4 models in kg
       ret.wheelbase = 2.66
       ret.centerToFront = ret.wheelbase * 0.41
       ret.steerRatio = 16.0  # 12.3 is spec end-to-end
       ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]  # TODO: determine if there is a dead zone at the top end
-      tire_stiffness_factor = 0.677
+      ret.tireStiffnessFactor = 0.677
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
       ret.wheelSpeedFactor = 1.025
 
     elif candidate == CAR.FIT:
-      ret.mass = 2644. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 2644. * CV.LB_TO_KG
       ret.wheelbase = 2.53
       ret.centerToFront = ret.wheelbase * 0.39
       ret.steerRatio = 13.06
       ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]  # TODO: determine if there is a dead zone at the top end
-      tire_stiffness_factor = 0.75
+      ret.tireStiffnessFactor = 0.75
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
 
     elif candidate == CAR.FREED:
-      ret.mass = 3086. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 3086. * CV.LB_TO_KG
       ret.wheelbase = 2.74
       # the remaining parameters were copied from FIT
       ret.centerToFront = ret.wheelbase * 0.39
       ret.steerRatio = 13.06
       ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
-      tire_stiffness_factor = 0.75
+      ret.tireStiffnessFactor = 0.75
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
 
     elif candidate in (CAR.HRV, CAR.HRV_3G):
-      ret.mass = 3125 * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 3125 * CV.LB_TO_KG
       ret.wheelbase = 2.61
       ret.centerToFront = ret.wheelbase * 0.41
       ret.steerRatio = 15.2
       ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
-      tire_stiffness_factor = 0.5
+      ret.tireStiffnessFactor = 0.5
       if candidate == CAR.HRV:
         ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]]
         ret.wheelSpeedFactor = 1.025
@@ -207,29 +206,29 @@ class CarInterface(CarInterfaceBase):
         ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]  # TODO: can probably use some tuning
 
     elif candidate == CAR.ACURA_RDX:
-      ret.mass = 3935. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 3935. * CV.LB_TO_KG
       ret.wheelbase = 2.68
       ret.centerToFront = ret.wheelbase * 0.38
       ret.steerRatio = 15.0  # as spec
-      tire_stiffness_factor = 0.444
+      ret.tireStiffnessFactor = 0.444
       ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]]  # TODO: determine if there is a dead zone at the top end
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
 
     elif candidate == CAR.ACURA_RDX_3G:
-      ret.mass = 4068. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 4068. * CV.LB_TO_KG
       ret.wheelbase = 2.75
       ret.centerToFront = ret.wheelbase * 0.41
       ret.steerRatio = 11.95  # as spec
       ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]]
-      tire_stiffness_factor = 0.677
+      ret.tireStiffnessFactor = 0.677
 
     elif candidate in (CAR.ODYSSEY, CAR.ODYSSEY_CHN):
-      ret.mass = 1900. + STD_CARGO_KG
+      ret.mass = 1900.
       ret.wheelbase = 3.00
       ret.centerToFront = ret.wheelbase * 0.41
       ret.steerRatio = 14.35  # as spec
-      tire_stiffness_factor = 0.82
+      ret.tireStiffnessFactor = 0.82
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]]
       if candidate == CAR.ODYSSEY_CHN:
         ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]]  # TODO: determine if there is a dead zone at the top end
@@ -237,39 +236,39 @@ class CarInterface(CarInterfaceBase):
         ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]  # TODO: determine if there is a dead zone at the top end
 
     elif candidate == CAR.PILOT:
-      ret.mass = 4278. * CV.LB_TO_KG + STD_CARGO_KG  # average weight
+      ret.mass = 4278. * CV.LB_TO_KG  # average weight
       ret.wheelbase = 2.86
       ret.centerToFront = ret.wheelbase * 0.428
       ret.steerRatio = 16.0  # as spec
       ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]  # TODO: determine if there is a dead zone at the top end
-      tire_stiffness_factor = 0.444
+      ret.tireStiffnessFactor = 0.444
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
 
     elif candidate == CAR.RIDGELINE:
-      ret.mass = 4515. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 4515. * CV.LB_TO_KG
       ret.wheelbase = 3.18
       ret.centerToFront = ret.wheelbase * 0.41
       ret.steerRatio = 15.59  # as spec
       ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]  # TODO: determine if there is a dead zone at the top end
-      tire_stiffness_factor = 0.444
+      ret.tireStiffnessFactor = 0.444
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
 
     elif candidate == CAR.INSIGHT:
-      ret.mass = 2987. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 2987. * CV.LB_TO_KG
       ret.wheelbase = 2.7
       ret.centerToFront = ret.wheelbase * 0.39
       ret.steerRatio = 15.0  # 12.58 is spec end-to-end
       ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]  # TODO: determine if there is a dead zone at the top end
-      tire_stiffness_factor = 0.82
+      ret.tireStiffnessFactor = 0.82
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
 
     elif candidate == CAR.HONDA_E:
-      ret.mass = 3338.8 * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 3338.8 * CV.LB_TO_KG
       ret.wheelbase = 2.5
       ret.centerToFront = ret.wheelbase * 0.5
       ret.steerRatio = 16.71
       ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]  # TODO: determine if there is a dead zone at the top end
-      tire_stiffness_factor = 0.82
+      ret.tireStiffnessFactor = 0.82
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] # TODO: can probably use some tuning
 
     else:
@@ -295,11 +294,6 @@ class CarInterface(CarInterfaceBase):
     ret.autoResumeSng = candidate in (HONDA_BOSCH | {CAR.CIVIC}) or ret.enableGasInterceptor
     ret.minEnableSpeed = -1. if ret.autoResumeSng else 25.5 * CV.MPH_TO_MS
 
-    # TODO: start from empirically derived lateral slip stiffness for the civic and scale by
-    # mass and CG position, so all cars will have approximately similar dyn behaviors
-    ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
-                                                                         tire_stiffness_factor=tire_stiffness_factor)
-
     ret.steerActuatorDelay = 0.1
     ret.steerLimitTimer = 0.8
 
@@ -314,15 +308,10 @@ class CarInterface(CarInterfaceBase):
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
 
-    buttonEvents = []
-
-    if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
-      buttonEvents.append(create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT))
-
-    if self.CS.cruise_setting != self.CS.prev_cruise_setting:
-      buttonEvents.append(create_button_event(self.CS.cruise_setting, self.CS.prev_cruise_setting, {1: ButtonType.altButton1}))
-
-    ret.buttonEvents = buttonEvents
+    ret.buttonEvents = [
+      *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT),
+      *create_button_events(self.CS.cruise_setting, self.CS.prev_cruise_setting, {1: ButtonType.altButton1}),
+    ]
 
     # events
     events = self.create_common_events(ret, pcm_enable=False)
diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py
index 660be4c449..8090f8e03e 100755
--- a/selfdrive/car/honda/radar_interface.py
+++ b/selfdrive/car/honda/radar_interface.py
@@ -1,18 +1,14 @@
 #!/usr/bin/env python3
 from cereal import car
 from opendbc.can.parser import CANParser
-from selfdrive.car.interfaces import RadarInterfaceBase
-from selfdrive.car.honda.values import DBC
+from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
+from openpilot.selfdrive.car.honda.values import DBC
 
 
 def _create_nidec_can_parser(car_fingerprint):
   radar_messages = [0x400] + list(range(0x430, 0x43A)) + list(range(0x440, 0x446))
-  signals = list(zip(['RADAR_STATE'] +
-                     ['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 +
-                     ['REL_SPEED'] * 16,
-                     [0x400] + radar_messages[1:] * 4))
-  checks = [(s[1], 20) for s in signals]
-  return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1)
+  messages = [(m, 20) for m in radar_messages]
+  return CANParser(DBC[car_fingerprint]['radar'], messages, 1)
 
 
 class RadarInterface(RadarInterfaceBase):
diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py
index ba7a42d2a1..38ba0f22fe 100644
--- a/selfdrive/car/honda/values.py
+++ b/selfdrive/car/honda/values.py
@@ -1,13 +1,13 @@
 from dataclasses import dataclass
-from enum import Enum, IntFlag
+from enum import Enum, IntFlag, StrEnum
 from typing import Dict, List, Optional, Union
 
 from cereal import car
-from common.conversions import Conversions as CV
+from openpilot.common.conversions import Conversions as CV
 from panda.python import uds
-from selfdrive.car import dbc_dict
-from selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column
-from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
+from openpilot.selfdrive.car import dbc_dict
+from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column
+from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
 
 Ecu = car.CarParams.Ecu
 VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -72,7 +72,7 @@ VISUAL_HUD = {
 }
 
 
-class CAR:
+class CAR(StrEnum):
   ACCORD = "HONDA ACCORD 2018"
   ACCORDH = "HONDA ACCORD HYBRID 2018"
   CIVIC = "HONDA CIVIC 2016"
@@ -123,13 +123,14 @@ CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = {
   CAR.ACCORDH: HondaCarInfo("Honda Accord Hybrid 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS),
   CAR.CIVIC: HondaCarInfo("Honda Civic 2016-18", min_steer_speed=12. * CV.MPH_TO_MS, video_link="https://youtu.be/-IkImTe1NYE"),
   CAR.CIVIC_BOSCH: [
-    HondaCarInfo("Honda Civic 2019-21", "All", video_link="https://www.youtube.com/watch?v=4Iz1Mz5LGF8", footnotes=[Footnote.CIVIC_DIESEL], min_steer_speed=2. * CV.MPH_TO_MS),
+    HondaCarInfo("Honda Civic 2019-21", "All", video_link="https://www.youtube.com/watch?v=4Iz1Mz5LGF8",
+                 footnotes=[Footnote.CIVIC_DIESEL], min_steer_speed=2. * CV.MPH_TO_MS),
     HondaCarInfo("Honda Civic Hatchback 2017-21", min_steer_speed=12. * CV.MPH_TO_MS),
   ],
   CAR.CIVIC_BOSCH_DIESEL: None,  # same platform
   CAR.CIVIC_2022: [
-    HondaCarInfo("Honda Civic 2022", "All", video_link="https://youtu.be/ytiOT5lcp6Q"),
-    HondaCarInfo("Honda Civic Hatchback 2022", "All", video_link="https://youtu.be/ytiOT5lcp6Q"),
+    HondaCarInfo("Honda Civic 2022-23", "All", video_link="https://youtu.be/ytiOT5lcp6Q"),
+    HondaCarInfo("Honda Civic Hatchback 2022-23", "All", video_link="https://youtu.be/ytiOT5lcp6Q"),
   ],
   CAR.ACURA_ILX: HondaCarInfo("Acura ILX 2016-19", "AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS),
   CAR.CRV: HondaCarInfo("Honda CR-V 2015-16", "Touring Trim", min_steer_speed=12. * CV.MPH_TO_MS),
@@ -1357,6 +1358,7 @@ FW_VERSIONS = {
       b'78109-T6Z-A910\x00\x00',
       b'78109-T6Z-AA10\x00\x00',
       b'78109-T6Z-C620\x00\x00',
+      b'78109-T6Z-C910\x00\x00',
       b'78109-TJZ-A510\x00\x00',
     ],
     (Ecu.srs, 0x18da53f1, None): [
@@ -1517,6 +1519,7 @@ FW_VERSIONS = {
       b'77959-T47-A940\x00\x00',
       b'77959-T47-A950\x00\x00',
       b'77959-T20-M820\x00\x00',
+      b'77959-T20-A980\x00\x00',
     ],
     (Ecu.combinationMeter, 0x18DA60F1, None): [
       b'78108-T21-A220\x00\x00',
@@ -1525,6 +1528,7 @@ FW_VERSIONS = {
       b'78108-T21-A230\x00\x00',
       b'78108-T22-A020\x00\x00',
       b'78108-T21-MB10\x00\x00',
+      b'78108-T21-A740\x00\x00',
     ],
     (Ecu.fwdRadar, 0x18dab0f1, None): [
       b'36161-T20-A070\x00\x00',
@@ -1552,6 +1556,7 @@ FW_VERSIONS = {
       b'37805-64A-A540\x00\x00',
       b'37805-64A-A620\x00\x00',
       b'37805-64D-P510\x00\x00',
+      b'37805-64S-AA10\x00\x00',
     ],
   },
 }
diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 11ff2fb6e4..0b5f724ab9 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -1,12 +1,12 @@
 from cereal import car
-from common.conversions import Conversions as CV
-from common.numpy_fast import clip
-from common.realtime import DT_CTRL
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import clip
+from openpilot.common.realtime import DT_CTRL
 from opendbc.can.packer import CANPacker
-from selfdrive.car import apply_driver_steer_torque_limits
-from selfdrive.car.hyundai import hyundaicanfd, hyundaican
-from selfdrive.car.hyundai.hyundaicanfd import CanBus
-from selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR
+from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance
+from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican
+from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
+from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 LongCtrlState = car.CarControl.Actuators.LongControlState
@@ -64,9 +64,17 @@ class CarController:
     new_steer = int(round(actuators.steer * self.params.STEER_MAX))
     apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
 
+    # >90 degree steering fault prevention
+    self.angle_limit_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringAngleDeg) >= MAX_ANGLE, CC.latActive,
+                                                                       self.angle_limit_counter, MAX_ANGLE_FRAMES,
+                                                                       MAX_ANGLE_CONSECUTIVE_FRAMES)
+
     if not CC.latActive:
       apply_steer = 0
 
+    # Hold torque with induced temporary fault when cutting the actuation bit
+    torque_fault = CC.latActive and not apply_steer_req
+
     self.apply_steer_last = apply_steer
 
     # accel + longitudinal
@@ -94,31 +102,18 @@ class CarController:
       if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
         can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", self.CAN.ECAN])
 
-    # >90 degree steering fault prevention
-    # Count up to MAX_ANGLE_FRAMES, at which point we need to cut torque to avoid a steering fault
-    if CC.latActive and abs(CS.out.steeringAngleDeg) >= MAX_ANGLE:
-      self.angle_limit_counter += 1
-    else:
-      self.angle_limit_counter = 0
-
-    # Cut steer actuation bit for two frames and hold torque with induced temporary fault
-    torque_fault = CC.latActive and self.angle_limit_counter > MAX_ANGLE_FRAMES
-    lat_active = CC.latActive and not torque_fault
-
-    if self.angle_limit_counter >= MAX_ANGLE_FRAMES + MAX_ANGLE_CONSECUTIVE_FRAMES:
-      self.angle_limit_counter = 0
-
     # CAN-FD platforms
     if self.CP.carFingerprint in CANFD_CAR:
       hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2
       hda2_long = hda2 and self.CP.openpilotLongitudinalControl
 
       # steering control
-      can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, lat_active, apply_steer))
+      can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_steer))
 
-      # disable LFA on HDA2
+      # prevent LFA from activating on HDA2 by sending "no lane lines detected" to ADAS ECU
       if self.frame % 5 == 0 and hda2:
-        can_sends.append(hyundaicanfd.create_cam_0x2a4(self.packer, self.CAN, CS.cam_0x2a4))
+        can_sends.append(hyundaicanfd.create_suppress_lfa(self.packer, self.CAN, CS.hda2_lfa_block_msg,
+                                                          self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING))
 
       # LFA and HDA icons
       if self.frame % 5 == 0 and (not hda2 or hda2_long):
@@ -137,48 +132,23 @@ class CarController:
           self.accel_last = accel
       else:
         # button presses
-        if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
-          # cruise cancel
-          if CC.cruiseControl.cancel:
-            if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
-              can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, self.CAN, CS.cruise_info))
-              self.last_button_frame = self.frame
-            else:
-              for _ in range(20):
-                can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL))
-              self.last_button_frame = self.frame
-
-          # cruise standstill resume
-          elif CC.cruiseControl.resume:
-            if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
-              # TODO: resume for alt button cars
-              pass
-            else:
-              for _ in range(20):
-                can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL))
-              self.last_button_frame = self.frame
+        can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False))
     else:
-      can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, lat_active,
+      can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, apply_steer_req,
                                                 torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
                                                 hud_control.leftLaneVisible, hud_control.rightLaneVisible,
                                                 left_lane_warning, right_lane_warning))
 
       if not self.CP.openpilotLongitudinalControl:
-        if CC.cruiseControl.cancel:
-          can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP.carFingerprint))
-        elif CC.cruiseControl.resume:
-          # send resume at a max freq of 10Hz
-          if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
-            # send 25 messages at a time to increases the likelihood of resume being accepted
-            can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL, self.CP.carFingerprint)] * 25)
-            if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15:
-              self.last_button_frame = self.frame
+        can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True))
 
       if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
         # TODO: unclear if this is needed
         jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
+        use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
         can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
-                                                        hud_control.leadVisible, set_speed_in_units, stopping, CC.cruiseControl.override))
+                                                        hud_control.leadVisible, set_speed_in_units, stopping,
+                                                        CC.cruiseControl.override, use_fca))
 
       # 20 Hz LFA MFA message
       if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
@@ -199,3 +169,39 @@ class CarController:
 
     self.frame += 1
     return new_actuators, can_sends
+
+  def create_button_messages(self, CC: car.CarControl, CS: car.CarState, use_clu11: bool):
+    can_sends = []
+    if use_clu11:
+      if CC.cruiseControl.cancel:
+        can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP.carFingerprint))
+      elif CC.cruiseControl.resume:
+        # send resume at a max freq of 10Hz
+        if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
+          # send 25 messages at a time to increases the likelihood of resume being accepted
+          can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL, self.CP.carFingerprint)] * 25)
+          if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15:
+            self.last_button_frame = self.frame
+    else:
+      if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
+        # cruise cancel
+        if CC.cruiseControl.cancel:
+          if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
+            can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, self.CAN, CS.cruise_info))
+            self.last_button_frame = self.frame
+          else:
+            for _ in range(20):
+              can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL))
+            self.last_button_frame = self.frame
+
+        # cruise standstill resume
+        elif CC.cruiseControl.resume:
+          if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
+            # TODO: resume for alt button cars
+            pass
+          else:
+            for _ in range(20):
+              can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL))
+            self.last_button_frame = self.frame
+
+    return can_sends
diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
index 32e2f86260..edc9e4855f 100644
--- a/selfdrive/car/hyundai/carstate.py
+++ b/selfdrive/car/hyundai/carstate.py
@@ -3,15 +3,17 @@ import copy
 import math
 
 from cereal import car
-from common.conversions import Conversions as CV
+from openpilot.common.conversions import Conversions as CV
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
-from selfdrive.car.hyundai.hyundaicanfd import CanBus
-from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_GEARS, CAMERA_SCC_CAR, CANFD_CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams
-from selfdrive.car.interfaces import CarStateBase
+from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
+from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_GEARS, CAMERA_SCC_CAR, \
+                                                   CANFD_CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams
+from openpilot.selfdrive.car.interfaces import CarStateBase
 
 PREV_BUTTON_SAMPLES = 8
 CLUSTER_SAMPLE_RATE = 20  # frames
+STANDSTILL_THRESHOLD = 12 * 0.03125 * CV.KPH_TO_MS
 
 
 class CarState(CarStateBase):
@@ -22,8 +24,8 @@ class CarState(CarStateBase):
     self.cruise_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
     self.main_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
 
-    self.gear_msg_canfd = "GEAR_ALT_2" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS_2 else \
-                          "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else \
+    self.gear_msg_canfd = "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else \
+                          "GEAR_ALT_2" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS_2 else \
                           "GEAR_SHIFTER"
     if CP.carFingerprint in CANFD_CAR:
       self.shifter_values = can_define.dv[self.gear_msg_canfd]["GEAR"]
@@ -34,6 +36,11 @@ class CarState(CarStateBase):
     else:  # preferred and elect gear methods use same definition
       self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]
 
+    self.accelerator_msg_canfd = "ACCELERATOR" if CP.carFingerprint in EV_CAR else \
+                                 "ACCELERATOR_ALT" if CP.carFingerprint in HYBRID_CAR else \
+                                 "ACCELERATOR_BRAKE_ALT"
+    self.cruise_btns_msg_canfd = "CRUISE_BUTTONS_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else \
+                                 "CRUISE_BUTTONS"
     self.is_metric = False
     self.buttons_counter = 0
 
@@ -67,7 +74,7 @@ class CarState(CarStateBase):
     )
     ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
     ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
-    ret.standstill = ret.vEgoRaw < 0.1
+    ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
 
     self.cluster_speed_counter += 1
     if self.cluster_speed_counter > CLUSTER_SAMPLE_RATE:
@@ -98,15 +105,17 @@ class CarState(CarStateBase):
       ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0
       ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1
       ret.cruiseState.standstill = False
+      ret.cruiseState.nonAdaptive = False
     else:
       ret.cruiseState.available = cp_cruise.vl["SCC11"]["MainMode_ACC"] == 1
       ret.cruiseState.enabled = cp_cruise.vl["SCC12"]["ACCMode"] != 0
       ret.cruiseState.standstill = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 4.
+      ret.cruiseState.nonAdaptive = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 2.  # Shows 'Cruise Control' on dash
       ret.cruiseState.speed = cp_cruise.vl["SCC11"]["VSetDis"] * speed_conv
 
     # TODO: Find brake pressure
     ret.brake = 0
-    ret.brakePressed = cp.vl["TCS13"]["DriverBraking"] != 0
+    ret.brakePressed = cp.vl["TCS13"]["DriverOverride"] == 2  # 2 includes regen braking by user on HEV/EV
     ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2  # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
     ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
     ret.accFaulted = cp.vl["TCS13"]["ACCEnable"] != 0  # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
@@ -123,12 +132,12 @@ class CarState(CarStateBase):
 
     # Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection,
     # as this seems to be standard over all cars, but is not the preferred method.
-    if self.CP.carFingerprint in CAN_GEARS["use_cluster_gears"]:
+    if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR):
+      gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
+    elif self.CP.carFingerprint in CAN_GEARS["use_cluster_gears"]:
       gear = cp.vl["CLU15"]["CF_Clu_Gear"]
     elif self.CP.carFingerprint in CAN_GEARS["use_tcu_gears"]:
       gear = cp.vl["TCU12"]["CUR_GR"]
-    elif self.CP.carFingerprint in CAN_GEARS["use_elect_gears"]:
-      gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
     else:
       gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
 
@@ -163,13 +172,11 @@ class CarState(CarStateBase):
     speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
 
     if self.CP.carFingerprint in (EV_CAR | HYBRID_CAR):
-      if self.CP.carFingerprint in EV_CAR:
-        ret.gas = cp.vl["ACCELERATOR"]["ACCELERATOR_PEDAL"] / 255.
-      else:
-        ret.gas = cp.vl["ACCELERATOR_ALT"]["ACCELERATOR_PEDAL"] / 1023.
+      offset = 255. if self.CP.carFingerprint in EV_CAR else 1023.
+      ret.gas = cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL"] / offset
       ret.gasPressed = ret.gas > 1e-5
     else:
-      ret.gasPressed = bool(cp.vl["ACCELERATOR_BRAKE_ALT"]["ACCELERATOR_PEDAL_PRESSED"])
+      ret.gasPressed = bool(cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL_PRESSED"])
 
     ret.brakePressed = cp.vl["TCS"]["DriverBraking"] == 1
 
@@ -188,7 +195,7 @@ class CarState(CarStateBase):
     )
     ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
     ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
-    ret.standstill = ret.vEgoRaw < 0.1
+    ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
 
     ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEERING_RATE"]
     ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1
@@ -197,8 +204,12 @@ class CarState(CarStateBase):
     ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
     ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0
 
-    ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"]["LEFT_LAMP"],
-                                                                      cp.vl["BLINKERS"]["RIGHT_LAMP"])
+    # TODO: alt signal usage may be described by cp.vl['BLINKERS']['USE_ALT_LAMP']
+    left_blinker_sig, right_blinker_sig = "LEFT_LAMP", "RIGHT_LAMP"
+    if self.CP.carFingerprint == CAR.KONA_EV_2ND_GEN:
+      left_blinker_sig, right_blinker_sig = "LEFT_LAMP_ALT", "RIGHT_LAMP_ALT"
+    ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"][left_blinker_sig],
+                                                                      cp.vl["BLINKERS"][right_blinker_sig])
     if self.CP.enableBsm:
       ret.leftBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FL_INDICATOR"] != 0
       ret.rightBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FR_INDICATOR"] != 0
@@ -217,79 +228,30 @@ class CarState(CarStateBase):
       ret.cruiseState.speed = cp_cruise_info.vl["SCC_CONTROL"]["VSetDis"] * speed_factor
       self.cruise_info = copy.copy(cp_cruise_info.vl["SCC_CONTROL"])
 
-    cruise_btn_msg = "CRUISE_BUTTONS_ALT" if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS"
+    # Manual Speed Limit Assist is a feature that replaces non-adaptive cruise control on EV CAN FD platforms.
+    # It limits the vehicle speed, overridable by pressing the accelerator past a certain point.
+    # The car will brake, but does not respect positive acceleration commands in this mode
+    # TODO: find this message on ICE & HYBRID cars + cruise control signals (if exists)
+    if self.CP.carFingerprint in EV_CAR:
+      ret.cruiseState.nonAdaptive = cp.vl["MANUAL_SPEED_LIMIT_ASSIST"]["MSLA_ENABLED"] == 1
+
     self.prev_cruise_buttons = self.cruise_buttons[-1]
-    self.cruise_buttons.extend(cp.vl_all[cruise_btn_msg]["CRUISE_BUTTONS"])
-    self.main_buttons.extend(cp.vl_all[cruise_btn_msg]["ADAPTIVE_CRUISE_MAIN_BTN"])
-    self.buttons_counter = cp.vl[cruise_btn_msg]["COUNTER"]
+    self.cruise_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"])
+    self.main_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"])
+    self.buttons_counter = cp.vl[self.cruise_btns_msg_canfd]["COUNTER"]
     ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0  # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
 
     if self.CP.flags & HyundaiFlags.CANFD_HDA2:
-      self.cam_0x2a4 = copy.copy(cp_cam.vl["CAM_0x2a4"])
+      self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING
+                                          else cp_cam.vl["CAM_0x2a4"])
 
     return ret
 
-  @staticmethod
-  def get_can_parser(CP):
+  def get_can_parser(self, CP):
     if CP.carFingerprint in CANFD_CAR:
-      return CarState.get_can_parser_canfd(CP)
-
-    signals = [
-      # signal_name, signal_address
-      ("WHL_SPD_FL", "WHL_SPD11"),
-      ("WHL_SPD_FR", "WHL_SPD11"),
-      ("WHL_SPD_RL", "WHL_SPD11"),
-      ("WHL_SPD_RR", "WHL_SPD11"),
-
-      ("YAW_RATE", "ESP12"),
-
-      ("CF_Gway_DrvSeatBeltInd", "CGW4"),
-
-      ("CF_Gway_DrvSeatBeltSw", "CGW1"),
-      ("CF_Gway_DrvDrSw", "CGW1"),       # Driver Door
-      ("CF_Gway_AstDrSw", "CGW1"),       # Passenger Door
-      ("CF_Gway_RLDrSw", "CGW2"),        # Rear left Door
-      ("CF_Gway_RRDrSw", "CGW2"),        # Rear right Door
-      ("CF_Gway_TurnSigLh", "CGW1"),
-      ("CF_Gway_TurnSigRh", "CGW1"),
-      ("CF_Gway_ParkBrakeSw", "CGW1"),
-
-      ("CYL_PRES", "ESP12"),
-
-      ("CF_Clu_CruiseSwState", "CLU11"),
-      ("CF_Clu_CruiseSwMain", "CLU11"),
-      ("CF_Clu_SldMainSW", "CLU11"),
-      ("CF_Clu_ParityBit1", "CLU11"),
-      ("CF_Clu_VanzDecimal" , "CLU11"),
-      ("CF_Clu_Vanz", "CLU11"),
-      ("CF_Clu_SPEED_UNIT", "CLU11"),
-      ("CF_Clu_DetentOut", "CLU11"),
-      ("CF_Clu_RheostatLevel", "CLU11"),
-      ("CF_Clu_CluInfo", "CLU11"),
-      ("CF_Clu_AmpInfo", "CLU11"),
-      ("CF_Clu_AliveCnt1", "CLU11"),
-
-      ("CF_Clu_VehicleSpeed", "CLU15"),
-
-      ("ACCEnable", "TCS13"),
-      ("ACC_REQ", "TCS13"),
-      ("DriverBraking", "TCS13"),
-      ("StandStill", "TCS13"),
-      ("PBRAKE_ACT", "TCS13"),
-
-      ("ESC_Off_Step", "TCS15"),
-      ("AVH_LAMP", "TCS15"),
-
-      ("CR_Mdps_StrColTq", "MDPS12"),
-      ("CF_Mdps_ToiActive", "MDPS12"),
-      ("CF_Mdps_ToiUnavail", "MDPS12"),
-      ("CF_Mdps_ToiFlt", "MDPS12"),
-      ("CR_Mdps_OutTq", "MDPS12"),
-
-      ("SAS_Angle", "SAS11"),
-      ("SAS_Speed", "SAS11"),
-    ]
-    checks = [
+      return self.get_can_parser_canfd(CP)
+
+    messages = [
       # address, frequency
       ("MDPS12", 50),
       ("TCS13", 50),
@@ -305,165 +267,60 @@ class CarState(CarStateBase):
     ]
 
     if not CP.openpilotLongitudinalControl and CP.carFingerprint not in CAMERA_SCC_CAR:
-      signals += [
-        ("MainMode_ACC", "SCC11"),
-        ("VSetDis", "SCC11"),
-        ("SCCInfoDisplay", "SCC11"),
-        ("ACC_ObjDist", "SCC11"),
-        ("ACCMode", "SCC12"),
-      ]
-      checks += [
+      messages += [
         ("SCC11", 50),
         ("SCC12", 50),
       ]
-
       if CP.flags & HyundaiFlags.USE_FCA.value:
-        signals += [
-          ("FCA_CmdAct", "FCA11"),
-          ("CF_VSM_Warn", "FCA11"),
-          ("CF_VSM_DecCmdAct", "FCA11"),
-        ]
-        checks.append(("FCA11", 50))
-      else:
-        signals += [
-          ("AEB_CmdAct", "SCC12"),
-          ("CF_VSM_Warn", "SCC12"),
-          ("CF_VSM_DecCmdAct", "SCC12"),
-        ]
+        messages.append(("FCA11", 50))
 
     if CP.enableBsm:
-      signals += [
-        ("CF_Lca_IndLeft", "LCA11"),
-        ("CF_Lca_IndRight", "LCA11"),
-      ]
-      checks.append(("LCA11", 50))
+      messages.append(("LCA11", 50))
 
     if CP.carFingerprint in (HYBRID_CAR | EV_CAR):
-      if CP.carFingerprint in HYBRID_CAR:
-        signals.append(("CR_Vcu_AccPedDep_Pos", "E_EMS11"))
-      else:
-        signals.append(("Accel_Pedal_Pos", "E_EMS11"))
-      checks.append(("E_EMS11", 50))
+      messages.append(("E_EMS11", 50))
     else:
-      signals += [
-        ("PV_AV_CAN", "EMS12"),
-        ("CF_Ems_AclAct", "EMS16"),
-      ]
-      checks += [
+      messages += [
         ("EMS12", 100),
         ("EMS16", 100),
       ]
 
-    if CP.carFingerprint in CAN_GEARS["use_cluster_gears"]:
-      signals.append(("CF_Clu_Gear", "CLU15"))
+    if CP.carFingerprint in (HYBRID_CAR | EV_CAR):
+      messages.append(("ELECT_GEAR", 20))
+    elif CP.carFingerprint in CAN_GEARS["use_cluster_gears"]:
+      pass
     elif CP.carFingerprint in CAN_GEARS["use_tcu_gears"]:
-      signals.append(("CUR_GR", "TCU12"))
-      checks.append(("TCU12", 100))
-    elif CP.carFingerprint in CAN_GEARS["use_elect_gears"]:
-      signals.append(("Elect_Gear_Shifter", "ELECT_GEAR"))
-      checks.append(("ELECT_GEAR", 20))
+      messages.append(("TCU12", 100))
     else:
-      signals.append(("CF_Lvr_Gear", "LVR12"))
-      checks.append(("LVR12", 100))
+      messages.append(("LVR12", 100))
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
 
   @staticmethod
   def get_cam_can_parser(CP):
     if CP.carFingerprint in CANFD_CAR:
       return CarState.get_cam_can_parser_canfd(CP)
 
-    signals = [
-      # signal_name, signal_address
-      ("CF_Lkas_LdwsActivemode", "LKAS11"),
-      ("CF_Lkas_LdwsSysState", "LKAS11"),
-      ("CF_Lkas_SysWarning", "LKAS11"),
-      ("CF_Lkas_LdwsLHWarning", "LKAS11"),
-      ("CF_Lkas_LdwsRHWarning", "LKAS11"),
-      ("CF_Lkas_HbaLamp", "LKAS11"),
-      ("CF_Lkas_FcwBasReq", "LKAS11"),
-      ("CF_Lkas_HbaSysState", "LKAS11"),
-      ("CF_Lkas_FcwOpt", "LKAS11"),
-      ("CF_Lkas_HbaOpt", "LKAS11"),
-      ("CF_Lkas_FcwSysState", "LKAS11"),
-      ("CF_Lkas_FcwCollisionWarning", "LKAS11"),
-      ("CF_Lkas_FusionState", "LKAS11"),
-      ("CF_Lkas_FcwOpt_USM", "LKAS11"),
-      ("CF_Lkas_LdwsOpt_USM", "LKAS11"),
-    ]
-    checks = [
+    messages = [
       ("LKAS11", 100)
     ]
 
     if not CP.openpilotLongitudinalControl and CP.carFingerprint in CAMERA_SCC_CAR:
-      signals += [
-        ("MainMode_ACC", "SCC11"),
-        ("VSetDis", "SCC11"),
-        ("SCCInfoDisplay", "SCC11"),
-        ("ACC_ObjDist", "SCC11"),
-        ("ACCMode", "SCC12"),
-      ]
-      checks += [
+      messages += [
         ("SCC11", 50),
         ("SCC12", 50),
       ]
 
       if CP.flags & HyundaiFlags.USE_FCA.value:
-        signals += [
-          ("FCA_CmdAct", "FCA11"),
-          ("CF_VSM_Warn", "FCA11"),
-          ("CF_VSM_DecCmdAct", "FCA11"),
-        ]
-        checks.append(("FCA11", 50))
-      else:
-        signals += [
-          ("AEB_CmdAct", "SCC12"),
-          ("CF_VSM_Warn", "SCC12"),
-          ("CF_VSM_DecCmdAct", "SCC12"),
-        ]
+        messages.append(("FCA11", 50))
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
 
-  @staticmethod
-  def get_can_parser_canfd(CP):
-
-    cruise_btn_msg = "CRUISE_BUTTONS_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS"
-    gear_msg = "GEAR_ALT_2" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS_2 else \
-               "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else \
-               "GEAR_SHIFTER"
-    signals = [
-      ("WHEEL_SPEED_1", "WHEEL_SPEEDS"),
-      ("WHEEL_SPEED_2", "WHEEL_SPEEDS"),
-      ("WHEEL_SPEED_3", "WHEEL_SPEEDS"),
-      ("WHEEL_SPEED_4", "WHEEL_SPEEDS"),
-
-      ("GEAR", gear_msg),
-
-      ("STEERING_RATE", "STEERING_SENSORS"),
-      ("STEERING_ANGLE", "STEERING_SENSORS"),
-      ("STEERING_COL_TORQUE", "MDPS"),
-      ("STEERING_OUT_TORQUE", "MDPS"),
-      ("LKA_FAULT", "MDPS"),
-
-      ("DriverBraking", "TCS"),
-      ("ACCEnable", "TCS"),
-      ("ACC_REQ", "TCS"),
-
-      ("COUNTER", cruise_btn_msg),
-      ("CRUISE_BUTTONS", cruise_btn_msg),
-      ("ADAPTIVE_CRUISE_MAIN_BTN", cruise_btn_msg),
-      ("DISTANCE_UNIT", "CRUISE_BUTTONS_ALT"),
-
-      ("LEFT_LAMP", "BLINKERS"),
-      ("RIGHT_LAMP", "BLINKERS"),
-
-      ("DRIVER_DOOR", "DOORS_SEATBELTS"),
-      ("DRIVER_SEATBELT", "DOORS_SEATBELTS"),
-    ]
-
-    checks = [
+  def get_can_parser_canfd(self, CP):
+    messages = [
+      (self.gear_msg_canfd, 100),
+      (self.accelerator_msg_canfd, 100),
       ("WHEEL_SPEEDS", 100),
-      (gear_msg, 100),
       ("STEERING_SENSORS", 100),
       ("MDPS", 100),
       ("TCS", 50),
@@ -472,77 +329,37 @@ class CarState(CarStateBase):
       ("DOORS_SEATBELTS", 4),
     ]
 
+    if CP.carFingerprint in EV_CAR:
+      messages += [
+        ("MANUAL_SPEED_LIMIT_ASSIST", 10),
+      ]
+
     if not (CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS):
-      checks.append(("CRUISE_BUTTONS", 50))
+      messages += [
+        ("CRUISE_BUTTONS", 50)
+      ]
 
     if CP.enableBsm:
-      signals += [
-        ("FL_INDICATOR", "BLINDSPOTS_REAR_CORNERS"),
-        ("FR_INDICATOR", "BLINDSPOTS_REAR_CORNERS"),
-      ]
-      checks += [
+      messages += [
         ("BLINDSPOTS_REAR_CORNERS", 20),
       ]
 
     if not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and not CP.openpilotLongitudinalControl:
-      signals += [
-        ("COUNTER", "SCC_CONTROL"),
-        ("CHECKSUM", "SCC_CONTROL"),
-        ("ACCMode", "SCC_CONTROL"),
-        ("VSetDis", "SCC_CONTROL"),
-        ("CRUISE_STANDSTILL", "SCC_CONTROL"),
-      ]
-      checks += [
+      messages += [
         ("SCC_CONTROL", 50),
       ]
 
-    if CP.carFingerprint in EV_CAR:
-      signals += [
-        ("ACCELERATOR_PEDAL", "ACCELERATOR"),
-      ]
-      checks += [
-        ("ACCELERATOR", 100),
-      ]
-    elif CP.carFingerprint in HYBRID_CAR:
-      signals += [
-        ("ACCELERATOR_PEDAL", "ACCELERATOR_ALT"),
-      ]
-      checks += [
-        ("ACCELERATOR_ALT", 100),
-      ]
-    else:
-      signals += [
-        ("ACCELERATOR_PEDAL_PRESSED", "ACCELERATOR_BRAKE_ALT"),
-      ]
-      checks += [
-        ("ACCELERATOR_BRAKE_ALT", 100),
-      ]
-
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus(CP).ECAN)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).ECAN)
 
   @staticmethod
   def get_cam_can_parser_canfd(CP):
-    signals = []
-    checks = []
+    messages = []
     if CP.flags & HyundaiFlags.CANFD_HDA2:
-      signals += [(f"BYTE{i}", "CAM_0x2a4") for i in range(3, 24)]
-      checks += [("CAM_0x2a4", 20)]
+      block_lfa_msg = "CAM_0x362" if CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING else "CAM_0x2a4"
+      messages += [(block_lfa_msg, 20)]
     elif CP.flags & HyundaiFlags.CANFD_CAMERA_SCC:
-      signals += [
-        ("COUNTER", "SCC_CONTROL"),
-        ("CHECKSUM", "SCC_CONTROL"),
-        ("NEW_SIGNAL_1", "SCC_CONTROL"),
-        ("MainMode_ACC", "SCC_CONTROL"),
-        ("ACCMode", "SCC_CONTROL"),
-        ("ZEROS_9", "SCC_CONTROL"),
-        ("CRUISE_STANDSTILL", "SCC_CONTROL"),
-        ("ZEROS_5", "SCC_CONTROL"),
-        ("DISTANCE_SETTING", "SCC_CONTROL"),
-        ("VSetDis", "SCC_CONTROL"),
-      ]
-
-      checks += [
+      messages += [
         ("SCC_CONTROL", 50),
       ]
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus(CP).CAM)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).CAM)
diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py
index dc5a5b6286..bc29aeb985 100644
--- a/selfdrive/car/hyundai/hyundaican.py
+++ b/selfdrive/car/hyundai/hyundaican.py
@@ -1,5 +1,5 @@
 import crcmod
-from selfdrive.car.hyundai.values import CAR, CHECKSUM, CAMERA_SCC_CAR
+from openpilot.selfdrive.car.hyundai.values import CAR, CHECKSUM, CAMERA_SCC_CAR
 
 hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf)
 
@@ -37,7 +37,8 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
                          CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.GENESIS_G70_2020,
                          CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022,
                          CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022,
-                         CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020, CAR.KIA_CEED):
+                         CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020, CAR.KIA_CEED,
+                         CAR.AZERA_6TH_GEN, CAR.AZERA_HEV_6TH_GEN, CAR.CUSTIN_1ST_GEN):
     values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1)
     values["CF_Lkas_LdwsOpt_USM"] = 2
 
@@ -125,7 +126,7 @@ def create_lfahda_mfc(packer, enabled, hda_set_speed=0):
   }
   return packer.make_can_msg("LFAHDA_MFC", 0, values)
 
-def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override):
+def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, use_fca):
   commands = []
 
   scc11_values = {
@@ -148,6 +149,13 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, s
     "aReqValue": accel,  # stock ramps up and down respecting jerk limit until it reaches aReqRaw
     "CR_VSM_Alive": idx % 0xF,
   }
+
+  # show AEB disabled indicator on dash with SCC12 if not sending FCA messages.
+  # these signals also prevent a TCS fault on non-FCA cars with alpha longitudinal
+  if not use_fca:
+    scc12_values["CF_VSM_ConfMode"] = 1
+    scc12_values["AEB_Status"] = 1  # AEB disabled
+
   scc12_dat = packer.make_can_msg("SCC12", 0, scc12_values)[2]
   scc12_values["CR_VSM_ChkSum"] = 0x10 - sum(sum(divmod(i, 16)) for i in scc12_dat) % 0x10
 
@@ -163,17 +171,19 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, s
   }
   commands.append(packer.make_can_msg("SCC14", 0, scc14_values))
 
-  # note that some vehicles most likely have an alternate checksum/counter definition
-  # https://github.com/commaai/opendbc/commit/9ddcdb22c4929baf310295e832668e6e7fcfa602
-  fca11_values = {
-    "CR_FCA_Alive": idx % 0xF,
-    "PAINT1_Status": 1,
-    "FCA_DrvSetStatus": 1,
-    "FCA_Status": 1, # AEB disabled
-  }
-  fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[2]
-  fca11_values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7])
-  commands.append(packer.make_can_msg("FCA11", 0, fca11_values))
+  # Only send FCA11 on cars where it exists on the bus
+  if use_fca:
+    # note that some vehicles most likely have an alternate checksum/counter definition
+    # https://github.com/commaai/opendbc/commit/9ddcdb22c4929baf310295e832668e6e7fcfa602
+    fca11_values = {
+      "CR_FCA_Alive": idx % 0xF,
+      "PAINT1_Status": 1,
+      "FCA_DrvSetStatus": 1,
+      "FCA_Status": 1,  # AEB disabled
+    }
+    fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[2]
+    fca11_values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7])
+    commands.append(packer.make_can_msg("FCA11", 0, fca11_values))
 
   return commands
 
@@ -187,6 +197,7 @@ def create_acc_opt(packer):
   }
   commands.append(packer.make_can_msg("SCC13", 0, scc13_values))
 
+  # TODO: this needs to be detected and conditionally sent on unsupported long cars
   fca12_values = {
     "FCA_DrvSetState": 2,
     "FCA_USM": 1, # AEB disabled
diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py
index e78e02ae50..a35fcb7779 100644
--- a/selfdrive/car/hyundai/hyundaicanfd.py
+++ b/selfdrive/car/hyundai/hyundaicanfd.py
@@ -1,6 +1,6 @@
-from common.numpy_fast import clip
-from selfdrive.car import CanBusBase
-from selfdrive.car.hyundai.values import HyundaiFlags
+from openpilot.common.numpy_fast import clip
+from openpilot.selfdrive.car import CanBusBase
+from openpilot.selfdrive.car.hyundai.values import HyundaiFlags
 
 
 class CanBus(CanBusBase):
@@ -46,25 +46,32 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer):
     "LKA_ASSIST": 0,
     "STEER_REQ": 1 if lat_active else 0,
     "STEER_MODE": 0,
-    "SET_ME_1": 0,
+    "HAS_LANE_SAFETY": 0,  # hide LKAS settings
     "NEW_SIGNAL_1": 0,
     "NEW_SIGNAL_2": 0,
   }
 
   if CP.flags & HyundaiFlags.CANFD_HDA2:
+    hda2_lkas_msg = "LKAS_ALT" if CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING else "LKAS"
     if CP.openpilotLongitudinalControl:
       ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
-    ret.append(packer.make_can_msg("LKAS", CAN.ACAN, values))
+    ret.append(packer.make_can_msg(hda2_lkas_msg, CAN.ACAN, values))
   else:
     ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
 
   return ret
 
-def create_cam_0x2a4(packer, CAN, cam_0x2a4):
-  values = {f"BYTE{i}": cam_0x2a4[f"BYTE{i}"] for i in range(3, 24)}
-  values['COUNTER'] = cam_0x2a4['COUNTER']
-  values["BYTE7"] = 0
-  return packer.make_can_msg("CAM_0x2a4", CAN.ACAN, values)
+def create_suppress_lfa(packer, CAN, hda2_lfa_block_msg, hda2_alt_steering):
+  suppress_msg = "CAM_0x362" if hda2_alt_steering else "CAM_0x2a4"
+  msg_bytes = 32 if hda2_alt_steering else 24
+
+  values = {f"BYTE{i}": hda2_lfa_block_msg[f"BYTE{i}"] for i in range(3, msg_bytes) if i != 7}
+  values["COUNTER"] = hda2_lfa_block_msg["COUNTER"]
+  values["SET_ME_0"] = 0
+  values["SET_ME_0_2"] = 0
+  values["LEFT_LANE_LINE"] = 0
+  values["RIGHT_LANE_LINE"] = 0
+  return packer.make_can_msg(suppress_msg, CAN.ACAN, values)
 
 def create_buttons(packer, CP, CAN, cnt, btn):
   values = {
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 66bf303def..26b3491358 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -1,13 +1,14 @@
-#!/usr/bin/env python3
 from cereal import car
 from panda import Panda
-from common.conversions import Conversions as CV
-from selfdrive.car.hyundai.hyundaicanfd import CanBus
-from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons
-from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
-from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config
-from selfdrive.car.interfaces import CarInterfaceBase
-from selfdrive.car.disable_ecu import disable_ecu
+from openpilot.common.conversions import Conversions as CV
+from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
+from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, \
+                                         CANFD_UNSUPPORTED_LONGITUDINAL_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, \
+                                         UNSUPPORTED_LONGITUDINAL_CAR, Buttons
+from openpilot.selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
+from openpilot.selfdrive.car import create_button_events, get_safety_config
+from openpilot.selfdrive.car.interfaces import CarInterfaceBase
+from openpilot.selfdrive.car.disable_ecu import disable_ecu
 
 Ecu = car.CarParams.Ecu
 ButtonType = car.CarState.ButtonEvent.Type
@@ -26,7 +27,8 @@ 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/car/tests/routes.py, we can remove it from this list.
-    ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, CAR.IONIQ_6}
+    # FIXME: the Optima Hybrid 2017 uses a different SCC12 checksum
+    ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, }
 
     hda2 = Ecu.adas in [fw.ecu for fw in car_fw]
     CAN = CanBus(None, hda2, fingerprint)
@@ -35,6 +37,8 @@ class CarInterface(CarInterfaceBase):
       # detect HDA2 with ADAS Driving ECU
       if hda2:
         ret.flags |= HyundaiFlags.CANFD_HDA2.value
+        if 0x110 in fingerprint[CAN.CAM]:
+          ret.flags |= HyundaiFlags.CANFD_HDA2_ALT_STEERING.value
       else:
         # non-HDA2
         if 0x1cf not in fingerprint[CAN.ECAN]:
@@ -58,183 +62,197 @@ class CarInterface(CarInterfaceBase):
 
     ret.steerActuatorDelay = 0.1  # Default delay
     ret.steerLimitTimer = 0.4
-    tire_stiffness_factor = 1.
     CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
 
-    if candidate in (CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022):
-      ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG
+    if candidate in (CAR.AZERA_6TH_GEN, CAR.AZERA_HEV_6TH_GEN):
+      ret.mass = 1600. if candidate == CAR.AZERA_6TH_GEN else 1675.  # ICE is ~average of 2.5L and 3.5L
+      ret.wheelbase = 2.885
+      ret.steerRatio = 14.5
+    elif candidate in (CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022):
+      ret.mass = 3982. * CV.LB_TO_KG
       ret.wheelbase = 2.766
       # Values from optimizer
       ret.steerRatio = 16.55  # 13.8 is spec end-to-end
-      tire_stiffness_factor = 0.82
+      ret.tireStiffnessFactor = 0.82
     elif candidate in (CAR.SONATA, CAR.SONATA_HYBRID):
-      ret.mass = 1513. + STD_CARGO_KG
+      ret.mass = 1513.
       ret.wheelbase = 2.84
       ret.steerRatio = 13.27 * 1.15   # 15% higher at the center seems reasonable
-      tire_stiffness_factor = 0.65
+      ret.tireStiffnessFactor = 0.65
     elif candidate == CAR.SONATA_LF:
-      ret.mass = 4497. * CV.LB_TO_KG
+      ret.mass = 1536.
       ret.wheelbase = 2.804
       ret.steerRatio = 13.27 * 1.15   # 15% higher at the center seems reasonable
     elif candidate == CAR.PALISADE:
-      ret.mass = 1999. + STD_CARGO_KG
+      ret.mass = 1999.
       ret.wheelbase = 2.90
       ret.steerRatio = 15.6 * 1.15
-      tire_stiffness_factor = 0.63
-    elif candidate == CAR.ELANTRA:
-      ret.mass = 1275. + STD_CARGO_KG
+      ret.tireStiffnessFactor = 0.63
+    elif candidate in (CAR.ELANTRA, CAR.ELANTRA_GT_I30):
+      ret.mass = 1275.
       ret.wheelbase = 2.7
       ret.steerRatio = 15.4            # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535
-      tire_stiffness_factor = 0.385    # stiffnessFactor settled on 1.0081302973865127
+      ret.tireStiffnessFactor = 0.385    # stiffnessFactor settled on 1.0081302973865127
       ret.minSteerSpeed = 32 * CV.MPH_TO_MS
     elif candidate == CAR.ELANTRA_2021:
-      ret.mass = (2800. * CV.LB_TO_KG) + STD_CARGO_KG
+      ret.mass = 2800. * CV.LB_TO_KG
       ret.wheelbase = 2.72
       ret.steerRatio = 12.9
-      tire_stiffness_factor = 0.65
+      ret.tireStiffnessFactor = 0.65
     elif candidate == CAR.ELANTRA_HEV_2021:
-      ret.mass = (3017. * CV.LB_TO_KG) + STD_CARGO_KG
+      ret.mass = 3017. * CV.LB_TO_KG
       ret.wheelbase = 2.72
       ret.steerRatio = 12.9
-      tire_stiffness_factor = 0.65
+      ret.tireStiffnessFactor = 0.65
     elif candidate == CAR.HYUNDAI_GENESIS:
-      ret.mass = 2060. + STD_CARGO_KG
+      ret.mass = 2060.
       ret.wheelbase = 3.01
       ret.steerRatio = 16.5
       ret.minSteerSpeed = 60 * CV.KPH_TO_MS
-    elif candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022):
-      ret.mass = {CAR.KONA_EV: 1685., CAR.KONA_HEV: 1425., CAR.KONA_EV_2022: 1743.}.get(candidate, 1275.) + STD_CARGO_KG
-      ret.wheelbase = 2.6
-      ret.steerRatio = 13.42  # Spec
-      tire_stiffness_factor = 0.385
+    elif candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022, CAR.KONA_EV_2ND_GEN):
+      ret.mass = {CAR.KONA_EV: 1685., CAR.KONA_HEV: 1425., CAR.KONA_EV_2022: 1743., CAR.KONA_EV_2ND_GEN: 1740.}.get(candidate, 1275.)
+      ret.wheelbase = {CAR.KONA_EV_2ND_GEN: 2.66, }.get(candidate, 2.6)
+      ret.steerRatio = {CAR.KONA_EV_2ND_GEN: 13.6, }.get(candidate, 13.42)  # Spec
+      ret.tireStiffnessFactor = 0.385
     elif candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV_2019, CAR.IONIQ_HEV_2022, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV):
-      ret.mass = 1490. + STD_CARGO_KG  # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
+      ret.mass = 1490.  # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
       ret.wheelbase = 2.7
       ret.steerRatio = 13.73  # Spec
-      tire_stiffness_factor = 0.385
+      ret.tireStiffnessFactor = 0.385
       if candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV_2019):
         ret.minSteerSpeed = 32 * CV.MPH_TO_MS
+    elif candidate in (CAR.IONIQ_5, CAR.IONIQ_6):
+      ret.mass = 1948
+      ret.wheelbase = 2.97
+      ret.steerRatio = 14.26
+      ret.tireStiffnessFactor = 0.65
     elif candidate == CAR.VELOSTER:
-      ret.mass = 3558. * CV.LB_TO_KG
+      ret.mass = 2917. * CV.LB_TO_KG
       ret.wheelbase = 2.80
       ret.steerRatio = 13.75 * 1.15
-      tire_stiffness_factor = 0.5
+      ret.tireStiffnessFactor = 0.5
     elif candidate == CAR.TUCSON:
       ret.mass = 3520. * CV.LB_TO_KG
       ret.wheelbase = 2.67
       ret.steerRatio = 14.00 * 1.15
-      tire_stiffness_factor = 0.385
+      ret.tireStiffnessFactor = 0.385
     elif candidate in (CAR.TUCSON_4TH_GEN, CAR.TUCSON_HYBRID_4TH_GEN):
-      ret.mass = 1630. + STD_CARGO_KG  # average
+      ret.mass = 1630.  # average
       ret.wheelbase = 2.756
       ret.steerRatio = 16.
-      tire_stiffness_factor = 0.385
+      ret.tireStiffnessFactor = 0.385
     elif candidate == CAR.SANTA_CRUZ_1ST_GEN:
-      ret.mass = 1870. + STD_CARGO_KG  # weight from Limited trim - the only supported trim
+      ret.mass = 1870.  # weight from Limited trim - the only supported trim
       ret.wheelbase = 3.000
-      ret.steerRatio = 14.2  # steering ratio according to Hyundai News https://www.hyundainews.com/assets/documents/original/48035-2022SantaCruzProductGuideSpecsv2081521.pdf
+      # steering ratio according to Hyundai News https://www.hyundainews.com/assets/documents/original/48035-2022SantaCruzProductGuideSpecsv2081521.pdf
+      ret.steerRatio = 14.2
+    elif candidate == CAR.CUSTIN_1ST_GEN:
+      ret.mass = 1690.  # from https://www.hyundai-motor.com.tw/clicktobuy/custin#spec_0
+      ret.wheelbase = 3.055
+      ret.steerRatio = 17.0  # from learner
 
     # Kia
     elif candidate == CAR.KIA_SORENTO:
-      ret.mass = 1985. + STD_CARGO_KG
+      ret.mass = 1985.
       ret.wheelbase = 2.78
       ret.steerRatio = 14.4 * 1.1   # 10% higher at the center seems reasonable
     elif candidate in (CAR.KIA_NIRO_EV, CAR.KIA_NIRO_EV_2ND_GEN, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_NIRO_HEV_2ND_GEN):
-      ret.mass = 3543. * CV.LB_TO_KG + STD_CARGO_KG  # average of all the cars
+      ret.mass = 3543. * CV.LB_TO_KG  # average of all the cars
       ret.wheelbase = 2.7
       ret.steerRatio = 13.6  # average of all the cars
-      tire_stiffness_factor = 0.385
+      ret.tireStiffnessFactor = 0.385
       if candidate == CAR.KIA_NIRO_PHEV:
         ret.minSteerSpeed = 32 * CV.MPH_TO_MS
     elif candidate == CAR.KIA_SELTOS:
-      ret.mass = 1337. + STD_CARGO_KG
+      ret.mass = 1337.
       ret.wheelbase = 2.63
       ret.steerRatio = 14.56
-      tire_stiffness_factor = 1
     elif candidate == CAR.KIA_SPORTAGE_5TH_GEN:
-      ret.mass = 1700. + STD_CARGO_KG  # weight from SX and above trims, average of FWD and AWD versions
+      ret.mass = 1700.  # weight from SX and above trims, average of FWD and AWD versions
       ret.wheelbase = 2.756
       ret.steerRatio = 13.6  # steering ratio according to Kia News https://www.kiamedia.com/us/en/models/sportage/2023/specifications
-    elif candidate in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.KIA_OPTIMA_H):
+    elif candidate in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA_H_G4_FL):
       ret.mass = 3558. * CV.LB_TO_KG
       ret.wheelbase = 2.80
       ret.steerRatio = 13.75
-      tire_stiffness_factor = 0.5
+      ret.tireStiffnessFactor = 0.5
       if candidate == CAR.KIA_OPTIMA_G4:
         ret.minSteerSpeed = 32 * CV.MPH_TO_MS
     elif candidate in (CAR.KIA_STINGER, CAR.KIA_STINGER_2022):
-      ret.mass = 1825. + STD_CARGO_KG
+      ret.mass = 1825.
       ret.wheelbase = 2.78
       ret.steerRatio = 14.4 * 1.15   # 15% higher at the center seems reasonable
     elif candidate == CAR.KIA_FORTE:
-      ret.mass = 3558. * CV.LB_TO_KG
+      ret.mass = 2878. * CV.LB_TO_KG
       ret.wheelbase = 2.80
       ret.steerRatio = 13.75
-      tire_stiffness_factor = 0.5
+      ret.tireStiffnessFactor = 0.5
     elif candidate == CAR.KIA_CEED:
-      ret.mass = 1450. + STD_CARGO_KG
+      ret.mass = 1450.
       ret.wheelbase = 2.65
       ret.steerRatio = 13.75
-      tire_stiffness_factor = 0.5
+      ret.tireStiffnessFactor = 0.5
     elif candidate in (CAR.KIA_K5_2021, CAR.KIA_K5_HEV_2020):
-      ret.mass = 3228. * CV.LB_TO_KG
+      ret.mass = 3381. * CV.LB_TO_KG
       ret.wheelbase = 2.85
       ret.steerRatio = 13.27  # 2021 Kia K5 Steering Ratio (all trims)
-      tire_stiffness_factor = 0.5
+      ret.tireStiffnessFactor = 0.5
     elif candidate == CAR.KIA_EV6:
-      ret.mass = 2055 + STD_CARGO_KG
+      ret.mass = 2055
       ret.wheelbase = 2.9
       ret.steerRatio = 16.
-      tire_stiffness_factor = 0.65
-    elif candidate in (CAR.IONIQ_5, CAR.IONIQ_6):
-      ret.mass = 1948 + STD_CARGO_KG
-      ret.wheelbase = 2.97
-      ret.steerRatio = 14.26
-      tire_stiffness_factor = 0.65
+      ret.tireStiffnessFactor = 0.65
     elif candidate == CAR.KIA_SPORTAGE_HYBRID_5TH_GEN:
-      ret.mass = 1767. + STD_CARGO_KG  # SX Prestige trim support only
+      ret.mass = 1767.  # SX Prestige trim support only
       ret.wheelbase = 2.756
       ret.steerRatio = 13.6
-    elif candidate in (CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN):
+    elif candidate in (CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN):
       ret.wheelbase = 2.81
       ret.steerRatio = 13.5  # average of the platforms
       if candidate == CAR.KIA_SORENTO_4TH_GEN:
-        ret.mass = 3957 * CV.LB_TO_KG + STD_CARGO_KG
+        ret.mass = 3957 * CV.LB_TO_KG
+      elif candidate == CAR.KIA_SORENTO_HEV_4TH_GEN:
+        ret.mass = 4255 * CV.LB_TO_KG
       else:
-        ret.mass = 4537 * CV.LB_TO_KG + STD_CARGO_KG
+        ret.mass = 4537 * CV.LB_TO_KG
     elif candidate == CAR.KIA_CARNIVAL_4TH_GEN:
-      ret.mass = 2087. + STD_CARGO_KG
+      ret.mass = 2087.
       ret.wheelbase = 3.09
       ret.steerRatio = 14.23
+    elif candidate == CAR.KIA_K8_HEV_1ST_GEN:
+      ret.mass = 1630.  # https://carprices.ae/brands/kia/2023/k8/1.6-turbo-hybrid
+      ret.wheelbase = 2.895
+      ret.steerRatio = 13.27  # guesstimate from K5 platform
 
     # Genesis
     elif candidate == CAR.GENESIS_GV60_EV_1ST_GEN:
-      ret.mass = 2205 + STD_CARGO_KG
+      ret.mass = 2205
       ret.wheelbase = 2.9
-      ret.steerRatio = 12.6 # https://www.motor1.com/reviews/586376/2023-genesis-gv60-first-drive/#:~:text=Relative%20to%20the%20related%20Ioniq,5%2FEV6%27s%2014.3%3A1.
+      # https://www.motor1.com/reviews/586376/2023-genesis-gv60-first-drive/#:~:text=Relative%20to%20the%20related%20Ioniq,5%2FEV6%27s%2014.3%3A1.
+      ret.steerRatio = 12.6
     elif candidate == CAR.GENESIS_G70:
       ret.steerActuatorDelay = 0.1
-      ret.mass = 1640.0 + STD_CARGO_KG
+      ret.mass = 1640.0
       ret.wheelbase = 2.84
       ret.steerRatio = 13.56
     elif candidate == CAR.GENESIS_G70_2020:
-      ret.mass = 3673.0 * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 3673.0 * CV.LB_TO_KG
       ret.wheelbase = 2.83
       ret.steerRatio = 12.9
     elif candidate == CAR.GENESIS_GV70_1ST_GEN:
-      ret.mass = 1950. + STD_CARGO_KG
+      ret.mass = 1950.
       ret.wheelbase = 2.87
       ret.steerRatio = 14.6
     elif candidate == CAR.GENESIS_G80:
-      ret.mass = 2060. + STD_CARGO_KG
+      ret.mass = 2060.
       ret.wheelbase = 3.01
       ret.steerRatio = 16.5
     elif candidate == CAR.GENESIS_G90:
-      ret.mass = 2200
+      ret.mass = 2200.
       ret.wheelbase = 3.15
       ret.steerRatio = 12.069
     elif candidate == CAR.GENESIS_GV80:
-      ret.mass = 2258. + STD_CARGO_KG
+      ret.mass = 2258.
       ret.wheelbase = 2.95
       ret.steerRatio = 14.14
 
@@ -242,11 +260,12 @@ class CarInterface(CarInterfaceBase):
     if candidate in CANFD_CAR:
       ret.longitudinalTuning.kpV = [0.1]
       ret.longitudinalTuning.kiV = [0.0]
-      ret.experimentalLongitudinalAvailable = candidate in (HYBRID_CAR | EV_CAR) and candidate not in CANFD_RADAR_SCC_CAR
+      ret.experimentalLongitudinalAvailable = (candidate in (HYBRID_CAR | EV_CAR) and candidate not in
+                                               (CANFD_UNSUPPORTED_LONGITUDINAL_CAR | CANFD_RADAR_SCC_CAR))
     else:
       ret.longitudinalTuning.kpV = [0.5]
       ret.longitudinalTuning.kiV = [0.0]
-      ret.experimentalLongitudinalAvailable = candidate not in (LEGACY_SAFETY_MODE_CAR | CAMERA_SCC_CAR)
+      ret.experimentalLongitudinalAvailable = candidate not in (UNSUPPORTED_LONGITUDINAL_CAR | CAMERA_SCC_CAR)
     ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
     ret.pcmCruise = not ret.openpilotLongitudinalControl
 
@@ -272,6 +291,8 @@ class CarInterface(CarInterfaceBase):
 
       if ret.flags & HyundaiFlags.CANFD_HDA2:
         ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2
+        if ret.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING:
+          ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2_ALT_STEERING
       if ret.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
         ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_ALT_BUTTONS
       if ret.flags & HyundaiFlags.CANFD_CAMERA_SCC:
@@ -299,10 +320,6 @@ class CarInterface(CarInterfaceBase):
 
     ret.centerToFront = ret.wheelbase * 0.4
 
-    # TODO: start from empirically derived lateral slip stiffness for the civic and scale by
-    # mass and CG position, so all cars will have approximately similar dyn behaviors
-    ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
-                                                                         tire_stiffness_factor=tire_stiffness_factor)
     return ret
 
   @staticmethod
@@ -320,13 +337,8 @@ class CarInterface(CarInterfaceBase):
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam)
 
-    if self.CS.CP.openpilotLongitudinalControl and self.CS.cruise_buttons[-1] != self.CS.prev_cruise_buttons:
-      buttonEvents = [create_button_event(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT)]
-      # Handle CF_Clu_CruiseSwState changing buttons mid-press
-      if self.CS.cruise_buttons[-1] != 0 and self.CS.prev_cruise_buttons != 0:
-        buttonEvents.append(create_button_event(0, self.CS.prev_cruise_buttons, BUTTONS_DICT))
-
-      ret.buttonEvents = buttonEvents
+    if self.CS.CP.openpilotLongitudinalControl:
+      ret.buttonEvents = create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT)
 
     # On some newer model years, the CANCEL button acts as a pause/resume button based on the PCM state
     # To avoid re-engaging when openpilot cancels, check user engagement intention via buttons
diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py
index 4ecca542b5..5260050986 100644
--- a/selfdrive/car/hyundai/radar_interface.py
+++ b/selfdrive/car/hyundai/radar_interface.py
@@ -1,33 +1,21 @@
-#!/usr/bin/env python3
 import math
 
 from cereal import car
 from opendbc.can.parser import CANParser
-from selfdrive.car.interfaces import RadarInterfaceBase
-from selfdrive.car.hyundai.values import DBC
+from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
+from openpilot.selfdrive.car.hyundai.values import DBC
 
 RADAR_START_ADDR = 0x500
 RADAR_MSG_COUNT = 32
 
+# POC for parsing corner radars: https://github.com/commaai/openpilot/pull/24221/
 
 def get_radar_can_parser(CP):
   if DBC[CP.carFingerprint]['radar'] is None:
     return None
 
-  signals = []
-  checks = []
-
-  for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT):
-    msg = f"RADAR_TRACK_{addr:x}"
-    signals += [
-      ("STATE", msg),
-      ("AZIMUTH", msg),
-      ("LONG_DIST", msg),
-      ("REL_ACCEL", msg),
-      ("REL_SPEED", msg),
-    ]
-    checks += [(msg, 50)]
-  return CANParser(DBC[CP.carFingerprint]['radar'], signals, checks, 1)
+  messages = [(f"RADAR_TRACK_{addr:x}", 50) for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT)]
+  return CANParser(DBC[CP.carFingerprint]['radar'], messages, 1)
 
 
 class RadarInterface(RadarInterfaceBase):
diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py
index 607c37f2f2..2d64d7a100 100644
--- a/selfdrive/car/hyundai/values.py
+++ b/selfdrive/car/hyundai/values.py
@@ -1,14 +1,15 @@
+# ruff: noqa: E501
 import re
 from dataclasses import dataclass
-from enum import Enum, IntFlag
+from enum import Enum, IntFlag, StrEnum
 from typing import Dict, List, Optional, Set, Tuple, Union
 
 from cereal import car
 from panda.python import uds
-from common.conversions import Conversions as CV
-from selfdrive.car import dbc_dict
-from selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column
-from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16
+from openpilot.common.conversions import Conversions as CV
+from openpilot.selfdrive.car import dbc_dict
+from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column
+from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16
 
 Ecu = car.CarParams.Ecu
 
@@ -36,9 +37,9 @@ 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.
-    elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.IONIQ,
+    elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.IONIQ,
                                CAR.IONIQ_EV_LTD, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_PHEV,
-                               CAR.KIA_OPTIMA_H, CAR.KIA_SORENTO):
+                               CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA_H_G4_FL, CAR.KIA_SORENTO):
       self.STEER_MAX = 255
 
     # these cars have significantly more torque than most HKG; limit to 70% of max
@@ -63,11 +64,15 @@ class HyundaiFlags(IntFlag):
   CANFD_ALT_GEARS_2 = 64
   SEND_LFA = 128
   USE_FCA = 256
+  CANFD_HDA2_ALT_STEERING = 512
 
 
-class CAR:
+class CAR(StrEnum):
   # Hyundai
+  AZERA_6TH_GEN = "HYUNDAI AZERA 6TH GEN"
+  AZERA_HEV_6TH_GEN = "HYUNDAI AZERA HYBRID 6TH GEN"
   ELANTRA = "HYUNDAI ELANTRA 2017"
+  ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT"
   ELANTRA_2021 = "HYUNDAI ELANTRA 2021"
   ELANTRA_HEV_2021 = "HYUNDAI ELANTRA HYBRID 2021"
   HYUNDAI_GENESIS = "HYUNDAI GENESIS 2015-2016"
@@ -80,6 +85,7 @@ class CAR:
   KONA = "HYUNDAI KONA 2020"
   KONA_EV = "HYUNDAI KONA ELECTRIC 2019"
   KONA_EV_2022 = "HYUNDAI KONA ELECTRIC 2022"
+  KONA_EV_2ND_GEN = "HYUNDAI KONA ELECTRIC 2ND GEN"
   KONA_HEV = "HYUNDAI KONA HYBRID 2020"
   SANTA_FE = "HYUNDAI SANTA FE 2019"
   SANTA_FE_2022 = "HYUNDAI SANTA FE 2022"
@@ -96,11 +102,13 @@ class CAR:
   TUCSON_4TH_GEN = "HYUNDAI TUCSON 4TH GEN"
   TUCSON_HYBRID_4TH_GEN = "HYUNDAI TUCSON HYBRID 4TH GEN"
   SANTA_CRUZ_1ST_GEN = "HYUNDAI SANTA CRUZ 1ST GEN"
+  CUSTIN_1ST_GEN = "HYUNDAI CUSTIN 1ST GEN"
 
   # Kia
   KIA_FORTE = "KIA FORTE E 2018 & GT 2021"
   KIA_K5_2021 = "KIA K5 2021"
   KIA_K5_HEV_2020 = "KIA K5 HYBRID 2020"
+  KIA_K8_HEV_1ST_GEN = "KIA K8 HYBRID 1ST GEN"
   KIA_NIRO_EV = "KIA NIRO EV 2020"
   KIA_NIRO_EV_2ND_GEN = "KIA NIRO EV 2ND GEN"
   KIA_NIRO_PHEV = "KIA NIRO HYBRID 2019"
@@ -109,10 +117,12 @@ class CAR:
   KIA_OPTIMA_G4 = "KIA OPTIMA 4TH GEN"
   KIA_OPTIMA_G4_FL = "KIA OPTIMA 4TH GEN FACELIFT"
   KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019"
+  KIA_OPTIMA_H_G4_FL = "KIA OPTIMA HYBRID 4TH GEN FACELIFT"
   KIA_SELTOS = "KIA SELTOS 2021"
   KIA_SPORTAGE_5TH_GEN = "KIA SPORTAGE 5TH GEN"
   KIA_SORENTO = "KIA SORENTO GT LINE 2018"
   KIA_SORENTO_4TH_GEN = "KIA SORENTO 4TH GEN"
+  KIA_SORENTO_HEV_4TH_GEN = "KIA SORENTO HYBRID 4TH GEN"
   KIA_SORENTO_PHEV_4TH_GEN = "KIA SORENTO PLUG-IN HYBRID 4TH GEN"
   KIA_SPORTAGE_HYBRID_5TH_GEN = "KIA SPORTAGE HYBRID 5TH GEN"
   KIA_STINGER = "KIA STINGER GT2 2018"
@@ -132,11 +142,10 @@ class CAR:
 
 
 class Footnote(Enum):
-  # footnotes which mention "red panda" will be replaced with the CAN FD panda kit on the shop page
   CANFD = CarFootnote(
-    "Requires a <a href=\"https://comma.ai/shop/panda\" target=\"_blank\">red panda</a> for this <a href=\"https://en.wikipedia.org/wiki/CAN_FD\" target=\"_blank\">CAN FD car</a>. " +
-    "All the hardware needed is sold in the <a href=\"https://comma.ai/shop/can-fd-panda-kit\" target=\"_blank\">CAN FD kit</a>.",
-    Column.MODEL, shop_footnote=True)
+    "Requires a <a href=\"https://comma.ai/shop/can-fd-panda-kit\" target=\"_blank\">CAN FD panda kit</a> if not using " +
+    "comma 3X for this <a href=\"https://en.wikipedia.org/wiki/CAN_FD\" target=\"_blank\">CAN FD car</a>.",
+    Column.MODEL, shop_footnote=False)
 
 
 @dataclass
@@ -149,15 +158,23 @@ class HyundaiCarInfo(CarInfo):
 
 
 CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
+  CAR.AZERA_6TH_GEN: HyundaiCarInfo("Hyundai Azera 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
+  CAR.AZERA_HEV_6TH_GEN: HyundaiCarInfo("Hyundai Azera Hybrid 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
   CAR.ELANTRA: [
-    HyundaiCarInfo("Hyundai Elantra 2017-19", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_b])),
+    # TODO: 2017-18 could be Hyundai G
+    HyundaiCarInfo("Hyundai Elantra 2017-18", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_b])),
+    HyundaiCarInfo("Hyundai Elantra 2019", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_g])),
+  ],
+  CAR.ELANTRA_GT_I30: [
     HyundaiCarInfo("Hyundai Elantra GT 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])),
     HyundaiCarInfo("Hyundai i30 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])),
   ],
   CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-23", video_link="https://youtu.be/_EdYQtV52-c", car_parts=CarParts.common([CarHarness.hyundai_k])),
-  CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c", car_parts=CarParts.common([CarHarness.hyundai_k])),
+  CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c",
+                                       car_parts=CarParts.common([CarHarness.hyundai_k])),
   CAR.HYUNDAI_GENESIS: [
-    HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_j])),  # TODO: check 2015 packages
+    # TODO: check 2015 packages
+    HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_j])),
     HyundaiCarInfo("Genesis G80 2017", "All", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_j])),
   ],
   CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", car_parts=CarParts.common([CarHarness.hyundai_c])),
@@ -168,13 +185,19 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
   CAR.IONIQ_PHEV: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2020-22", "All", car_parts=CarParts.common([CarHarness.hyundai_h])),
   CAR.KONA: HyundaiCarInfo("Hyundai Kona 2020", car_parts=CarParts.common([CarHarness.hyundai_b])),
   CAR.KONA_EV: HyundaiCarInfo("Hyundai Kona Electric 2018-21", car_parts=CarParts.common([CarHarness.hyundai_g])),
-  CAR.KONA_EV_2022: HyundaiCarInfo("Hyundai Kona Electric 2022", car_parts=CarParts.common([CarHarness.hyundai_o])),
-  CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020", video_link="https://youtu.be/0dwpAHiZgFo", car_parts=CarParts.common([CarHarness.hyundai_i])),  # TODO: check packages
+  CAR.KONA_EV_2022: HyundaiCarInfo("Hyundai Kona Electric 2022-23", car_parts=CarParts.common([CarHarness.hyundai_o])),
+  CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020", video_link="https://youtu.be/0dwpAHiZgFo",
+                               car_parts=CarParts.common([CarHarness.hyundai_i])),  # TODO: check packages
+  # TODO: this is the 2024 US MY, not yet released
+  CAR.KONA_EV_2ND_GEN: HyundaiCarInfo("Hyundai Kona Electric (with HDA II, Korea only) 2023", video_link="https://www.youtube.com/watch?v=U2fOCmcQ8hw",
+                                      car_parts=CarParts.common([CarHarness.hyundai_r])),
   CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All", car_parts=CarParts.common([CarHarness.hyundai_d])),
-  CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-23", "All", video_link="https://youtu.be/VnHzSTygTS4", car_parts=CarParts.common([CarHarness.hyundai_l])),
+  CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-23", "All", video_link="https://youtu.be/VnHzSTygTS4",
+                                    car_parts=CarParts.common([CarHarness.hyundai_l])),
   CAR.SANTA_FE_HEV_2022: HyundaiCarInfo("Hyundai Santa Fe Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])),
-  CAR.SANTA_FE_PHEV_2022: HyundaiCarInfo("Hyundai Santa Fe Plug-in Hybrid 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_l])),
-  CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-23", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw", car_parts=CarParts.common([CarHarness.hyundai_a])),
+  CAR.SANTA_FE_PHEV_2022: HyundaiCarInfo("Hyundai Santa Fe Plug-in Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])),
+  CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-23", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw",
+                             car_parts=CarParts.common([CarHarness.hyundai_a])),
   CAR.SONATA_LF: HyundaiCarInfo("Hyundai Sonata 2018-19", car_parts=CarParts.common([CarHarness.hyundai_e])),
   CAR.TUCSON: [
     HyundaiCarInfo("Hyundai Tucson 2021", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_l])),
@@ -192,23 +215,24 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
     HyundaiCarInfo("Hyundai Ioniq 5 (with HDA II) 2022-23", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q])),
   ],
   CAR.IONIQ_6: [
-    HyundaiCarInfo("Hyundai Ioniq 6 (without HDA II) 2023", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_k])),  # TODO: unknown
     HyundaiCarInfo("Hyundai Ioniq 6 (with HDA II) 2023", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p])),
   ],
   CAR.TUCSON_4TH_GEN: [
     HyundaiCarInfo("Hyundai Tucson 2022", car_parts=CarParts.common([CarHarness.hyundai_n])),
     HyundaiCarInfo("Hyundai Tucson 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_n])),
   ],
-  CAR.TUCSON_HYBRID_4TH_GEN: HyundaiCarInfo("Hyundai Tucson Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_n])),
+  CAR.TUCSON_HYBRID_4TH_GEN: HyundaiCarInfo("Hyundai Tucson Hybrid 2022-24", "All", car_parts=CarParts.common([CarHarness.hyundai_n])),
   CAR.SANTA_CRUZ_1ST_GEN: HyundaiCarInfo("Hyundai Santa Cruz 2022-23", car_parts=CarParts.common([CarHarness.hyundai_n])),
+  CAR.CUSTIN_1ST_GEN: HyundaiCarInfo("Hyundai Custin 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
 
   # Kia
   CAR.KIA_FORTE: [
     HyundaiCarInfo("Kia Forte 2019-21", car_parts=CarParts.common([CarHarness.hyundai_g])),
     HyundaiCarInfo("Kia Forte 2023", car_parts=CarParts.common([CarHarness.hyundai_e])),
   ],
-  CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-22", car_parts=CarParts.common([CarHarness.hyundai_a])),
-  CAR.KIA_K5_HEV_2020: HyundaiCarInfo("Kia K5 Hybrid 2020", car_parts=CarParts.common([CarHarness.hyundai_a])),
+  CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-24", car_parts=CarParts.common([CarHarness.hyundai_a])),
+  CAR.KIA_K5_HEV_2020: HyundaiCarInfo("Kia K5 Hybrid 2020-22", car_parts=CarParts.common([CarHarness.hyundai_a])),
+  CAR.KIA_K8_HEV_1ST_GEN: HyundaiCarInfo("Kia K8 Hybrid (with HDA II) 2023", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q])),
   CAR.KIA_NIRO_EV: [
     HyundaiCarInfo("Kia Niro EV 2019", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_h])),
     HyundaiCarInfo("Kia Niro EV 2020", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_f])),
@@ -224,23 +248,26 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
     HyundaiCarInfo("Kia Niro Hybrid 2021-22", car_parts=CarParts.common([CarHarness.hyundai_f])),  # TODO: 2021 could be hyundai_d, verify
   ],
   CAR.KIA_NIRO_HEV_2ND_GEN: HyundaiCarInfo("Kia Niro Hybrid 2023", car_parts=CarParts.common([CarHarness.hyundai_a])),
-  CAR.KIA_OPTIMA_G4: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control", car_parts=CarParts.common([CarHarness.hyundai_b])),  # TODO: may support 2016, 2018
+  CAR.KIA_OPTIMA_G4: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control",
+                                    car_parts=CarParts.common([CarHarness.hyundai_b])),  # TODO: may support 2016, 2018
   CAR.KIA_OPTIMA_G4_FL: HyundaiCarInfo("Kia Optima 2019-20", car_parts=CarParts.common([CarHarness.hyundai_g])),
-  CAR.KIA_OPTIMA_H: [
-    HyundaiCarInfo("Kia Optima Hybrid 2017", "Advanced Smart Cruise Control"),  # TODO: may support adjacent years
-    HyundaiCarInfo("Kia Optima Hybrid 2019"),
-  ],
+  # TODO: may support adjacent years. may have a non-zero minimum steering speed
+  CAR.KIA_OPTIMA_H: HyundaiCarInfo("Kia Optima Hybrid 2017", "Advanced Smart Cruise Control", car_parts=CarParts.common([CarHarness.hyundai_c])),
+  CAR.KIA_OPTIMA_H_G4_FL: HyundaiCarInfo("Kia Optima Hybrid 2019", car_parts=CarParts.common([CarHarness.hyundai_h])),
   CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021", car_parts=CarParts.common([CarHarness.hyundai_a])),
   CAR.KIA_SPORTAGE_5TH_GEN: HyundaiCarInfo("Kia Sportage 2023", car_parts=CarParts.common([CarHarness.hyundai_n])),
   CAR.KIA_SORENTO: [
-    HyundaiCarInfo("Kia Sorento 2018", "Advanced Smart Cruise Control", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", car_parts=CarParts.common([CarHarness.hyundai_c])),
+    HyundaiCarInfo("Kia Sorento 2018", "Advanced Smart Cruise Control & LKAS", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8",
+                   car_parts=CarParts.common([CarHarness.hyundai_e])),
     HyundaiCarInfo("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", car_parts=CarParts.common([CarHarness.hyundai_e])),
   ],
   CAR.KIA_SORENTO_4TH_GEN: HyundaiCarInfo("Kia Sorento 2021-23", car_parts=CarParts.common([CarHarness.hyundai_k])),
-  CAR.KIA_SORENTO_PHEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Plug-in Hybrid 2022-23", car_parts=CarParts.common([CarHarness.hyundai_a])),
+  CAR.KIA_SORENTO_HEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Hybrid 2021-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])),
+  CAR.KIA_SORENTO_PHEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Plug-in Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])),
   CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: HyundaiCarInfo("Kia Sportage Hybrid 2023", car_parts=CarParts.common([CarHarness.hyundai_n])),
-  CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0", car_parts=CarParts.common([CarHarness.hyundai_c])),
-  CAR.KIA_STINGER_2022: HyundaiCarInfo("Kia Stinger 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
+  CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0",
+                                  car_parts=CarParts.common([CarHarness.hyundai_c])),
+  CAR.KIA_STINGER_2022: HyundaiCarInfo("Kia Stinger 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
   CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019", car_parts=CarParts.common([CarHarness.hyundai_e])),
   CAR.KIA_EV6: [
     HyundaiCarInfo("Kia EV6 (Southeast Asia only) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_p])),
@@ -248,7 +275,7 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
     HyundaiCarInfo("Kia EV6 (with HDA II) 2022-23", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p]))
   ],
   CAR.KIA_CARNIVAL_4TH_GEN: [
-    HyundaiCarInfo("Kia Carnival 2023", car_parts=CarParts.common([CarHarness.hyundai_a])),
+    HyundaiCarInfo("Kia Carnival 2023-24", car_parts=CarParts.common([CarHarness.hyundai_a])),
     HyundaiCarInfo("Kia Carnival (China only) 2023", car_parts=CarParts.common([CarHarness.hyundai_k]))
   ],
 
@@ -276,9 +303,6 @@ class Buttons:
   CANCEL = 4  # on newer models, this is a pause/resume button
 
 FINGERPRINTS = {
-  CAR.ELANTRA: [{
-    66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 897: 8, 832: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1170: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1345: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2001: 8, 2003: 8, 2004: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
-  }],
   CAR.HYUNDAI_GENESIS: [{
     67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1342: 6, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4
   },
@@ -377,8 +401,9 @@ def match_fw_to_car_fuzzy(live_fw_versions) -> Set[str]:
   # Non-electric CAN FD platforms often do not have platform code specifiers needed
   # to distinguish between hybrid and ICE. All EVs so far are either exclusively
   # electric or specify electric in the platform code.
-  fuzzy_platform_blacklist = set(CANFD_CAR - EV_CAR)
-  candidates = set()
+  # TODO: whitelist platforms that we've seen hybrid and ICE versions of that have these specifiers
+  fuzzy_platform_blacklist = {str(c) for c in set(CANFD_CAR - EV_CAR)}
+  candidates: Set[str] = set()
 
   for candidate, fws in FW_VERSIONS.items():
     # Keep track of ECUs which pass all checks (platform codes, within date range)
@@ -446,6 +471,7 @@ PART_NUMBER_FW_PATTERN = re.compile(b'(?<=[0-9][.,][0-9]{2} )([0-9]{5}[-/]?[A-Z]
 # TODO: use abs, it has the platform code and part number on many platforms
 PLATFORM_CODE_ECUS = [Ecu.fwdRadar, Ecu.fwdCamera, Ecu.eps]
 # So far we've only seen dates in fwdCamera
+# TODO: there are date codes in the ABS firmware versions in hex
 DATE_FW_ECUS = [Ecu.fwdCamera]
 
 FW_QUERY_CONFIG = FwQueryConfig(
@@ -509,6 +535,40 @@ FW_QUERY_CONFIG = FwQueryConfig(
 )
 
 FW_VERSIONS = {
+  CAR.AZERA_6TH_GEN: {
+    (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\x00IG__ SCC F-CU-      1.00 1.00 99110-G8100         ',
+    ],
+    (Ecu.eps, 0x7d4, None): [
+      b'\xf1\x00IG  MDPS C 1.00 1.02 56310G8510\x00 4IGSC103',
+    ],
+    (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00IG  MFC  AT MES LHD 1.00 1.04 99211-G8100 200511',
+    ],
+    (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\x00bcsh8p54  U912\x00\x00\x00\x00\x00\x00SIG0M35MH0\xa4 |.',
+    ],
+    (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x81641KA051\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
+  },
+  CAR.AZERA_HEV_6TH_GEN: {
+    (Ecu.fwdCamera, 0x7C4, None): [
+      b'\xf1\x00IGH MFC  AT KOR LHD 1.00 1.02 99211-G8100 191029',
+    ],
+    (Ecu.eps, 0x7d4, None): [
+      b'\xf1\x00IG  MDPS C 1.00 1.00 56310M9600\x00 4IHSC100',
+    ],
+    (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\x00IGhe SCC FHCUP      1.00 1.00 99110-M9100         ',
+    ],
+    (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\x006T7N0_C2\x00\x006T7VA051\x00\x00TIGSH24KA1\xc7\x85\xe2`',
+    ],
+    (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x816H590051\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
+  },
   CAR.HYUNDAI_GENESIS: {
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\x00DH LKAS 1.1 -150210',
@@ -563,11 +623,13 @@ FW_VERSIONS = {
       b'\xf1\x00AE  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',
+      b'\xf1\x00AE  MDPS C 1.00 1.01 56310/G2310 4APHC101',
     ],
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\x00AEP 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',
+      b'\xf1\x00AEP MFC  AT EUR LHD 1.00 1.01 95740-G2600 190819',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x816H6F6051\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -580,6 +642,7 @@ FW_VERSIONS = {
       b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL2\x00\x00\x00\x00',
       b'\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL0\x00\x00\x00\x00',
       b'\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL2\xad\xeb\xabt',
+      b'\xf1\x006U3H1_C2\x00\x006U3J8051\x00\x00PAETG16UL0\x00\x00\x00\x00',
     ],
   },
   CAR.IONIQ_EV_2020: {
@@ -595,6 +658,7 @@ FW_VERSIONS = {
       b'\xf1\x00AEE MFC  AT EUR LHD 1.00 1.01 95740-G2600 190819',
       b'\xf1\x00AEE MFC  AT EUR LHD 1.00 1.03 95740-G2500 190516',
       b'\xf1\x00AEE MFC  AT EUR RHD 1.00 1.01 95740-G2600 190819',
+      b'\xf1\x00AEE MFC  AT EUR LHD 1.00 1.00 95740-G2600 190730',
     ],
   },
   CAR.IONIQ_EV_LTD: {
@@ -902,6 +966,7 @@ FW_VERSIONS = {
       b'\xf1\x00TM ESC \x04 102!\x04\x05 58910-S2GA0',
       b'\xf1\x00TM ESC \x04 101 \x08\x04 58910-S2GA0',
       b'\xf1\x00TM ESC \x02 103"\x07\x08 58910-S2GA0',
+      b'\xf1\x00TM ESC   103!\x030 58910-S1MA0',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M1_0a0_L50',
@@ -915,12 +980,15 @@ FW_VERSIONS = {
       b'\xf1\x81HM6M2_0a0_G00',
       b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M1_0a0_J10',
       b'\xf1\x8739101-2STN8\xf1\x81HM6M1_0a0_M00',
+      b'\xf1\x87           \xf1\x81           ',
     ],
     (Ecu.eps, 0x7d4, None): [
       b'\xf1\x00TM  MDPS C 1.00 1.02 56370-S2AA0 0B19',
       b'\xf1\x00TM  MDPS C 1.00 1.01 56310-S1AB0 4TSDC101',
+      b'\xf1\x00TM  MDPS C 1.00 1.01 56310-S1EB0 4TSDC101',
     ],
     (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00TM  MFC  AT MES LHD 1.00 1.05 99211-S1500 220126',
       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',
@@ -974,20 +1042,45 @@ FW_VERSIONS = {
   CAR.SANTA_FE_PHEV_2022: {
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\x8799110CL500\xf1\x00TMhe SCC FHCUP      1.00 1.00 99110-CL500         ',
+      b'\xf1\x00TMhe SCC FHCUP      1.00 1.01 99110-CL500         ',
     ],
     (Ecu.eps, 0x7d4, None): [
       b'\xf1\x00TM  MDPS C 1.00 1.02 56310-CLAC0 4TSHC102',
       b'\xf1\x00TM  MDPS C 1.00 1.02 56310-CLEC0 4TSHC102',
+      b'\xf1\x00TM  MDPS C 1.00 1.02 56310CLEC0\x00 4TSHC102',
     ],
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\x00TMP MFC  AT USA LHD 1.00 1.03 99211-S1500 210224',
+      b'\xf1\x00TMP MFC  AT USA LHD 1.00 1.06 99211-S1500 220727',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x8795441-3D121\x00\xf1\x81E16\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333  E16\x00\x00\x00\x00\x00\x00\x00TTM2P16SA0o\x88^\xbe',
       b'\xf1\x8795441-3D121\x00\xf1\x81E16\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333  E16\x00\x00\x00\x00\x00\x00\x00TTM2P16SA1\x0b\xc5\x0f\xea',
+      b'\xf1\x00PSBG2333  E16\x00\x00\x00\x00\x00\x00\x00TTM2P16SA1\x0b\xc5\x0f\xea',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x87391312MTF0',
+      b'\xf1\x87391312MTF1',
+    ],
+  },
+  CAR.CUSTIN_1ST_GEN: {
+    (Ecu.abs, 0x7d1, None): [
+      b'\xf1\x00KU ESC \x01 101!\x02\x03 58910-O3200',
+    ],
+    (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\x00KU__ SCC F-CUP      1.00 1.01 99110-O3000         ',
+    ],
+    (Ecu.eps, 0x7d4, None): [
+      b'\xf1\x00KU  MDPS C 1.00 1.01 56310/O3100 4KUCC101',
+    ],
+    (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00KU2 MFC  AT CHN LHD 1.00 1.02 99211-O3000 220923',
+    ],
+    (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x87391212MEC0',
+    ],
+    (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\x00bcsh8p54  U928\x00\x00\x00\x00\x00\x00SKU0T15KB2\x92U\xf9M',
     ],
   },
   CAR.KIA_STINGER: {
@@ -1037,21 +1130,26 @@ FW_VERSIONS = {
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\x00CK__ SCC F-CUP      1.00 1.00 99110-J5500         ',
       b'\xf1\x00CK__ SCC FHCUP      1.00 1.00 99110-J5500         ',
+      b'\xf1\x00CK__ SCC FHCUP      1.00 1.00 99110-J5600         ',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x81640R0051\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\xf1\x81640N2051\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\xf1\x81HM6M1_0a0_H00',
     ],
     (Ecu.eps, 0x7d4, None): [
       b'\xf1\x00CK  MDPS R 1.00 5.03 57700-J5380 4C2VR503',
       b'\xf1\x00CK  MDPS R 1.00 5.03 57700-J5300 4C2CL503',
+      b'\xf1\x00CK  MDPS R 1.00 5.04 57700-J5520 4C4VL504',
     ],
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\x00CK  MFC  AT AUS RHD 1.00 1.00 99211-J5500 210622',
       b'\xf1\x00CK  MFC  AT KOR LHD 1.00 1.00 99211-J5500 210622',
+      b'\xf1\x00CK  MFC  AT USA LHD 1.00 1.00 99211-J5500 210622',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x87VCNLF11383972DK1vffV\x99\x99\x89\x98\x86eUU\x88wg\x89vfff\x97fff\x99\x87o\xff"\xc1\xf1\x81E30\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54  E30\x00\x00\x00\x00\x00\x00\x00SCK0T33GH0\xbe`\xfb\xc6',
+      b'\xf1\x00bcsh8p54  E31\x00\x00\x00\x00\x00\x00\x00SCK0T33NH07\xdf\xf0\xc1',
       b'\xf1\x00bcsh8p54  E31\x00\x00\x00\x00\x00\x00\x00SCK0T25KH2B\xfbI\xe2',
     ],
   },
@@ -1234,20 +1332,24 @@ FW_VERSIONS = {
   CAR.GENESIS_G80: {
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\x00DH__ SCC F-CUP      1.00 1.01 96400-B1120         ',
+      b'\xf1\x00DH__ SCC FHCUP      1.00 1.01 96400-B1110         ',
     ],
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\x00DH  LKAS AT USA LHD 1.01 1.03 95895-B1500 180713',
       b'\xf1\x00DH  LKAS AT USA LHD 1.01 1.02 95895-B1500 170810',
       b'\xf1\x00DH  LKAS AT USA LHD 1.01 1.01 95895-B1500 161014',
+      b'\xf1\x00DH  LKAS AT KOR LHD 1.01 1.02 95895-B1500 170810',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x00bcsh8p54  E21\x00\x00\x00\x00\x00\x00\x00SDH0T33NH4\xd7O\x9e\xc9',
       b'\xf1\x00bcsh8p54  E18\x00\x00\x00\x00\x00\x00\x00TDH0G38NH3:-\xa9n',
       b'\xf1\x00bcsh8p54  E18\x00\x00\x00\x00\x00\x00\x00SDH0G38NH2j\x9dA\x1c',
       b'\xf1\x00bcsh8p54  E18\x00\x00\x00\x00\x00\x00\x00SDH0T33NH3\x97\xe6\xbc\xb8',
+      b'\xf1\x00bcsh8p54  E18\x00\x00\x00\x00\x00\x00\x00SDH0G33KH2\xae\xde\xd5!',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\xf1\x81640A4051\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
   },
   CAR.GENESIS_G90: {
@@ -1315,16 +1417,22 @@ FW_VERSIONS = {
       b'\xf1\x8799110L2100\xf1\x00DL3_ SCC F-CUP      1.00 1.03 99110-L2100         ',
       b'\xf1\x8799110L2100\xf1\x00DL3_ SCC FHCUP      1.00 1.03 99110-L2100         ',
       b'\xf1\x00DL3_ SCC F-CUP      1.00 1.03 99110-L2100         ',
+      b'\xf1\x00DL3_ SCC FHCUP      1.00 1.03 99110-L2100         ',
+      b'\xf1\x00DL3_ SCC FHCUP      1.00 1.04 99110-L2100         ',
     ],
     (Ecu.eps, 0x7D4, None): [
       b'\xf1\x8756310-L3110\xf1\000DL3 MDPS C 1.00 1.01 56310-L3110 4DLAC101',
       b'\xf1\x8756310-L3220\xf1\x00DL3 MDPS C 1.00 1.01 56310-L3220 4DLAC101',
       b'\xf1\x8757700-L3000\xf1\x00DL3 MDPS R 1.00 1.02 57700-L3000 4DLAP102',
       b'\xf1\x00DL3 MDPS C 1.00 1.01 56310-L3220 4DLAC101',
+      b'\xf1\x00DL3 MDPS C 1.00 1.02 56310-L2220 4DLDC102',
+      b'\xf1\x00DL3 MDPS C 1.00 1.02 56310L3220\x00 4DLAC102',
     ],
     (Ecu.fwdCamera, 0x7C4, None): [
       b'\xf1\x00DL3 MFC  AT USA LHD 1.00 1.03 99210-L3000 200915',
       b'\xf1\x00DL3 MFC  AT USA LHD 1.00 1.04 99210-L3000 210208',
+      b'\xf1\x00DL3 MFC  AT KOR LHD 1.00 1.04 99210-L2000 210527',
+      b'\xf1\x00DL3 MFC  AT USA LHD 1.00 1.05 99210-L3000 211222',
     ],
     (Ecu.abs, 0x7D1, None): [
       b'\xf1\000DL ESC \006 101 \004\002 58910-L3200',
@@ -1332,11 +1440,15 @@ FW_VERSIONS = {
       b'\xf1\x8758910-L3800\xf1\x00DL ESC \t 101 \x07\x02 58910-L3800',
       b'\xf1\x8758910-L3600\xf1\x00DL ESC \x03 100 \x08\x02 58910-L3600',
       b'\xf1\x00DL ESC \t 100 \x06\x02 58910-L3800',
+      b'\xf1\x00DL ESC \x01 104 \x07\x12 58910-L2200',
+      b'\xf1\x00DL ESC \x06 103"\x08\x06 58910-L3200',
     ],
     (Ecu.engine, 0x7E0, None): [
       b'\xf1\x87391212MKT0',
       b'\xf1\x87391212MKV0',
       b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x82DLDWN5TMDCXXXJ1B',
+      b'\xf1\x81HM6M2_0a0_DQ0',
+      b'\xf1\x87391212MKT3',
     ],
     (Ecu.transmission, 0x7E1, None): [
       b'\xf1\000bcsh8p54  U913\000\000\000\000\000\000TDL2T16NB1ia\v\xb8',
@@ -1345,6 +1457,8 @@ FW_VERSIONS = {
       b'\xf1\x87SCMSAA8572454GK1\x87x\x87\x88Vf\x86hgwvwvwwgvwwgT?\xfb\xff\x97fo\xffH\xb8\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54  U913\x00\x00\x00\x00\x00\x00TDL4T16NB05\x94t\x18',
       b'\xf1\x87954A02N300\x00\x00\x00\x00\x00\xf1\x81T02730A1  \xf1\x00T02601BL  T02730A1  WDL3T25XXX730NS2b\x1f\xb8%',
       b'\xf1\x00bcsh8p54  U913\x00\x00\x00\x00\x00\x00TDL4T16NB05\x94t\x18',
+      b'\xf1\x00HT6TA261BLHT6TAB00A1SDL0C20KS0\x00\x00\x00\x00\x00\x00\\\x9f\xa5\x15',
+      b'\xf1\x00bcsh8p54  U913\x00\x00\x00\x00\x00\x00TDL2T16NB2.\x13\xf6\xed',
     ],
   },
   CAR.KIA_K5_HEV_2020: {
@@ -1353,15 +1467,18 @@ FW_VERSIONS = {
     ],
     (Ecu.eps, 0x7D4, None): [
       b'\xf1\x00DL3 MDPS C 1.00 1.02 56310-L7000 4DLHC102',
+      b'\xf1\x00DL3 MDPS C 1.00 1.02 56310-L7220 4DLHC102',
     ],
     (Ecu.fwdCamera, 0x7C4, None): [
       b'\xf1\x00DL3HMFC  AT KOR LHD 1.00 1.02 99210-L2000 200309',
+      b'\xf1\x00DL3HMFC  AT KOR LHD 1.00 1.04 99210-L2000 210527',
     ],
     (Ecu.engine, 0x7E0, None): [
       b'\xf1\x87391162JLA0',
     ],
     (Ecu.transmission, 0x7E1, None): [
       b'\xf1\x00PSBG2323  E08\x00\x00\x00\x00\x00\x00\x00TDL2H20KA2\xe3\xc6cz',
+      b'\xf1\x00PSBG2333  E16\x00\x00\x00\x00\x00\x00\x00TDL2H20KA5T\xf2\xc9\xc2',
     ],
   },
   CAR.KONA_EV: {
@@ -1397,26 +1514,37 @@ FW_VERSIONS = {
       b'\xf1\x8758520-K4010\xf1\x00OS IEB \x04 101 \x11\x13 58520-K4010',
       b'\xf1\x8758520-K4010\xf1\x00OS IEB \x03 101 \x11\x13 58520-K4010',
       b'\xf1\x00OS IEB \r 102"\x05\x16 58520-K4010',
-      # TODO: these return from the MULTI request, above return from LONG
-      b'\x01\x04\x7f\xff\xff\xf8\xff\xff\x00\x00\x01\xd3\x00\x00\x00\x00\xff\xb7\xff\xee\xff\xe0\x00\xc0\xc0\xfc\xd5\xfc\x00\x00U\x10\xffP\xf5\xff\xfd\x00\x00\x00\x00\xfc\x00\x01',
-      b'\x01\x04\x7f\xff\xff\xf8\xff\xff\x00\x00\x01\xdb\x00\x00\x00\x00\xff\xb1\xff\xd9\xff\xd2\x00\xc0\xc0\xfc\xd5\xfc\x00\x00U\x10\xff\xd6\xf5\x00\x06\x00\x00\x00\x14\xfd\x00\x04',
-      b'\x01\x04\x7f\xff\xff\xf8\xff\xff\x00\x00\x01\xd3\x00\x00\x00\x00\xff\xb7\xff\xf4\xff\xd9\x00\xc0',
+      b'\xf1\x00OS IEB \x02 102"\x05\x16 58520-K4010',
+      b'\xf1\x00OS IEB \x03 102"\x05\x16 58520-K4010',
     ],
     (Ecu.fwdCamera, 0x7C4, None): [
       b'\xf1\x00OSP LKA  AT CND LHD 1.00 1.02 99211-J9110 802',
       b'\xf1\x00OSP LKA  AT EUR RHD 1.00 1.02 99211-J9110 802',
       b'\xf1\x00OSP LKA  AT AUS RHD 1.00 1.04 99211-J9200 904',
       b'\xf1\x00OSP LKA  AT EUR LHD 1.00 1.04 99211-J9200 904',
+      b'\xf1\x00OSP LKA  AT EUR RHD 1.00 1.04 99211-J9200 904',
+      b'\xf1\x00OSP LKA  AT USA LHD 1.00 1.04 99211-J9200 904',
     ],
     (Ecu.eps, 0x7D4, None): [
       b'\xf1\x00OSP MDPS C 1.00 1.02 56310K4260\x00 4OEPC102',
       b'\xf1\x00OSP MDPS C 1.00 1.02 56310/K4970 4OEPC102',
       b'\xf1\x00OSP MDPS C 1.00 1.02 56310/K4271 4OEPC102',
+      b'\xf1\x00OSP MDPS C 1.00 1.02 56310-K4271 4OEPC102',
+      b'\xf1\x00OSP MDPS C 1.00 1.02 56310K4971\x00 4OEPC102',
+      b'\xf1\x00OSP MDPS C 1.00 1.02 56310K4261\x00 4OEPC102',
     ],
     (Ecu.fwdRadar, 0x7D0, None): [
       b'\xf1\x00YB__ FCA -----      1.00 1.01 99110-K4500      \x00\x00\x00',
     ],
   },
+  CAR.KONA_EV_2ND_GEN: {
+    (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\x00SXev RDR -----      1.00 1.00 99110-BF000         ',
+    ],
+    (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00SX2EMFC  AT KOR LHD 1.00 1.00 99211-BF000 230410',
+    ],
+  },
   CAR.KIA_NIRO_EV: {
     (Ecu.fwdRadar, 0x7D0, None): [
       b'\xf1\x00DEev SCC F-CUP      1.00 1.00 99110-Q4000         ',
@@ -1565,7 +1693,53 @@ FW_VERSIONS = {
       b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6B8051\x00\x00\xf1\x006T6H0_C2\x00\x006T6B8051\x00\x00TJFSG24NH27\xa7\xc2\xb4',
     ],
   },
+  CAR.KIA_OPTIMA_H: {
+    (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\x00JFhe SCC FNCUP      1.00 1.00 96400-A8000         ',
+    ],
+    (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00JFP LKAS AT EUR LHD 1.00 1.03 95895-A8100 160711',
+    ],
+  },
+  CAR.KIA_OPTIMA_H_G4_FL: {
+    (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\x00JFhe SCC FHCUP      1.00 1.01 99110-A8500         ',
+    ],
+    (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00JFH MFC  AT KOR LHD 1.00 1.01 95895-A8200 180323',
+    ],
+    (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x816H6D1051\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
+  },
   CAR.ELANTRA: {
+    (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00ADP LKAS AT USA LHD 1.00 1.03 99211-F2000 X31',
+      b'\xf1\x00AD  LKAS AT USA LHD 1.01 1.01 95895-F2000 251',
+    ],
+    (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\x006T6K0_C2\x00\x006T6S2051\x00\x00TAD0N20NSD(\xfcA\x9d',
+      b'\xf1\x006T6K0_C2\x00\x006T6S2051\x00\x00TAD0N20NSD\x00\x00\x00\x00',
+      b'\xf1\x006T6J0_C2\x00\x006T6F0051\x00\x00TAD0N20NS2\x00\x00\x00\x00',
+      b'\xf1\x006T6J0_C2\x00\x006T6F0051\x00\x00TAD0N20NS2\xc5\x92\x9e\x8a',
+      b'\xf1\x006T6J0_C2\x00\x006T6F0051\x00\x00TAD0N20SS2.~\x90\x87',
+    ],
+    (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x8161698051\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\xf1\x8161657051\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\xf1\x816165D051\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\xf1\x816165E051\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.abs, 0x7d1, None): [
+      b'\xf1\x00AD ESC \x11 11 \x18\x05\x06 58910-F2840',
+      b'\xf1\x00AD ESC \x11 12 \x15\t\t 58920-F2810',
+    ],
+    (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\x00AD__ SCC H-CUP      1.00 1.00 99110-F2100         ',
+      b'\xf1\x00AD__ SCC H-CUP      1.00 1.01 96400-F2100         ',
+    ],
+  },
+  CAR.ELANTRA_GT_I30: {
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\x00PD  LKAS AT USA LHD 1.01 1.01 95740-G3100 A54',
       b'\xf1\x00PD  LKAS AT KOR LHD 1.00 1.02 95740-G3000 A51',
@@ -1769,6 +1943,7 @@ FW_VERSIONS = {
       b'\xf1\x00NE1 MFC  AT USA LHD 1.00 1.01 99211-GI010 211007',
       b'\xf1\x00NE1 MFC  AT EUR RHD 1.00 1.02 99211-GI010 211206',
       b'\xf1\x00NE1 MFC  AT USA LHD 1.00 1.03 99211-GI010 220401',
+      b'\xf1\x00NE1 MFC  AT USA LHD 1.00 1.06 99211-GI010 230110',
     ],
   },
   CAR.IONIQ_6: {
@@ -1776,6 +1951,7 @@ FW_VERSIONS = {
       b'\xf1\x00CE__ RDR -----      1.00 1.01 99110-KL000         ',
     ],
     (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00CE  MFC  AT EUR LHD 1.00 1.03 99211-KL000 221011',
       b'\xf1\x00CE  MFC  AT USA LHD 1.00 1.04 99211-KL000 221213',
     ],
   },
@@ -1795,10 +1971,13 @@ FW_VERSIONS = {
       b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-N9220 14K',
       b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.01 99211-N9100 14A',
       b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-N9250 14W',
+      b'\xf1\x00NX4 FR_CMR AT EUR LHD 1.00 2.02 99211-N9000 14E',
+      b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-N9260 14Y',
     ],
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\x00NX4__               1.00 1.00 99110-N9100         ',
       b'\xf1\x00NX4__               1.01 1.00 99110-N9100         ',
+      b'\xf1\x00NX4__               1.00 1.01 99110-N9000         ',
     ],
   },
   CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: {
@@ -1824,6 +2003,7 @@ FW_VERSIONS = {
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1030 662',
       b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1040 663',
+      b'\xf1\x00NQ5 FR_CMR AT AUS RHD 1.00 1.00 99211-P1040 663',
     ],
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\x00NQ5__               1.00 1.02 99110-P1000         ',
@@ -1857,10 +2037,12 @@ FW_VERSIONS = {
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\x00MQ4 MFC  AT USA LHD 1.00 1.05 99210-R5000 210623',
       b'\xf1\x00MQ4 MFC  AT USA LHD 1.00 1.03 99210-R5000 200903',
+      b'\xf1\x00MQ4 MFC  AT USA LHD 1.00 1.00 99210-R5100 221019',
     ],
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\x00MQ4_ SCC FHCUP      1.00 1.06 99110-P2000         ',
       b'\xf1\x00MQ4_ SCC F-CUP      1.00 1.06 99110-P2000         ',
+      b'\xf1\x00MQ4_ SCC FHCUP      1.00 1.08 99110-P2000         ',
     ],
   },
   CAR.KIA_NIRO_HEV_2ND_GEN: {
@@ -1884,45 +2066,90 @@ FW_VERSIONS = {
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\x00KA4 MFC  AT USA LHD 1.00 1.06 99210-R0000 220221',
       b'\xf1\x00KA4CMFC  AT CHN LHD 1.00 1.01 99211-I4000 210525',
+      b'\xf1\x00KA4 MFC  AT USA LHD 1.00 1.00 99210-R0100 230105',
     ],
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\x00KA4_ SCC FHCUP      1.00 1.03 99110-R0000         ',
       b'\xf1\x00KA4c SCC FHCUP      1.00 1.01 99110-I4000         ',
+      b'\xf1\x00KA4_ SCC FHCUP      1.00 1.00 99110-R0100         ',
+    ],
+  },
+  CAR.KIA_SORENTO_HEV_4TH_GEN: {
+    (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00MQ4HMFC  AT KOR LHD 1.00 1.12 99210-P2000 230331',
+      b'\xf1\x00MQ4HMFC  AT USA LHD 1.00 1.11 99210-P2000 211217',
+      b'\xf1\x00MQ4HMFC  AT KOR LHD 1.00 1.04 99210-P2000 200330',
+    ],
+    (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\x00MQhe SCC FHCUP      1.00 1.07 99110-P4000         ',
+      b'\xf1\x00MQhe SCC FHCUP      1.00 1.04 99110-P4000         ',
+    ],
+  },
+  CAR.KIA_K8_HEV_1ST_GEN: {
+    (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00GL3HMFC  AT KOR LHD 1.00 1.03 99211-L8000 210907',
+    ],
+    (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\x00GL3_ RDR -----      1.00 1.02 99110-L8000         ',
     ],
   },
 }
 
 CHECKSUM = {
-  "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.KIA_K5_HEV_2020],
+  "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021,
+           CAR.SONATA_HYBRID, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022,
+           CAR.KIA_K5_HEV_2020, CAR.CUSTIN_1ST_GEN],
   "6B": [CAR.KIA_SORENTO, CAR.HYUNDAI_GENESIS],
 }
 
 CAN_GEARS = {
-  # which message has the gear
-  "use_cluster_gears": {CAR.ELANTRA, CAR.KONA},
+  # which message has the gear. hybrid and EV use ELECT_GEAR
+  "use_cluster_gears": {CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA},
   "use_tcu_gears": {CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON},
-  "use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.KONA_EV_2022, CAR.KIA_K5_HEV_2020},
 }
 
-CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.IONIQ_6, CAR.TUCSON_4TH_GEN, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.SANTA_CRUZ_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_NIRO_HEV_2ND_GEN, CAR.KIA_NIRO_EV_2ND_GEN, CAR.GENESIS_GV80, CAR.KIA_CARNIVAL_4TH_GEN}
+CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.IONIQ_6, CAR.TUCSON_4TH_GEN, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN,
+             CAR.SANTA_CRUZ_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN,
+             CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_NIRO_HEV_2ND_GEN, CAR.KIA_NIRO_EV_2ND_GEN,
+             CAR.GENESIS_GV80, CAR.KIA_CARNIVAL_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN, CAR.KONA_EV_2ND_GEN, CAR.KIA_K8_HEV_1ST_GEN}
 
 # The radar does SCC on these cars when HDA I, rather than the camera
-CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.GENESIS_GV80, CAR.KIA_CARNIVAL_4TH_GEN}
+CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.GENESIS_GV80,
+                       CAR.KIA_CARNIVAL_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN}
+
+# These CAN FD cars do not accept communication control to disable the ADAS ECU,
+# responds with 0x7F2822 - 'conditions not correct'
+CANFD_UNSUPPORTED_LONGITUDINAL_CAR = {CAR.IONIQ_6, CAR.KONA_EV_2ND_GEN}
 
 # The camera does SCC on these cars, rather than the radar
 CAMERA_SCC_CAR = {CAR.KONA_EV_2022, }
 
-HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_K5_HEV_2020, CAR.KIA_NIRO_HEV_2ND_GEN}  # these cars use a different gas signal
-EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_EV_2ND_GEN, CAR.KONA_EV_2022, CAR.KIA_EV6, CAR.IONIQ_5, CAR.IONIQ_6, CAR.GENESIS_GV60_EV_1ST_GEN}
+# these cars use a different gas signal
+HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ,
+              CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.TUCSON_HYBRID_4TH_GEN,
+              CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_K5_HEV_2020, CAR.KIA_NIRO_HEV_2ND_GEN,
+              CAR.KIA_SORENTO_HEV_4TH_GEN, CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA_H_G4_FL, CAR.KIA_K8_HEV_1ST_GEN,
+              CAR.AZERA_HEV_6TH_GEN}
+
+EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_EV_2ND_GEN, CAR.KONA_EV_2022,
+          CAR.KIA_EV6, CAR.IONIQ_5, CAR.IONIQ_6, CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KONA_EV_2ND_GEN}
 
 # these cars require a special panda safety mode due to missing counters and checksums in the messages
-LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.VELOSTER,
-                          CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022}
+LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_LTD, CAR.KIA_OPTIMA_G4,
+                          CAR.VELOSTER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022,
+                          CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30}
+
+# these cars have not been verified to work with longitudinal yet - radar disable, sending correct messages, etc.
+UNSUPPORTED_LONGITUDINAL_CAR = LEGACY_SAFETY_MODE_CAR | {CAR.KIA_NIRO_PHEV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA_G4_FL,
+                                                         CAR.KIA_OPTIMA_H_G4_FL}
 
 # If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points.
 # If no points are outputted by default it might be possible to turn it on using  selfdrive/debug/hyundai_enable_radar_points.py
 DBC = {
+  CAR.AZERA_6TH_GEN: dbc_dict('hyundai_kia_generic', None),
+  CAR.AZERA_HEV_6TH_GEN: dbc_dict('hyundai_kia_generic', None),
   CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None),
+  CAR.ELANTRA_GT_I30: dbc_dict('hyundai_kia_generic', None),
   CAR.ELANTRA_2021: dbc_dict('hyundai_kia_generic', None),
   CAR.ELANTRA_HEV_2021: dbc_dict('hyundai_kia_generic', None),
   CAR.GENESIS_G70: dbc_dict('hyundai_kia_generic', None),
@@ -1945,6 +2172,7 @@ DBC = {
   CAR.KIA_OPTIMA_G4: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_OPTIMA_G4_FL: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None),
+  CAR.KIA_OPTIMA_H_G4_FL: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_SELTOS: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format
   CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None),
@@ -1980,4 +2208,8 @@ DBC = {
   CAR.KIA_NIRO_EV_2ND_GEN: dbc_dict('hyundai_canfd', None),
   CAR.GENESIS_GV80: dbc_dict('hyundai_canfd', None),
   CAR.KIA_CARNIVAL_4TH_GEN: dbc_dict('hyundai_canfd', None),
+  CAR.KIA_SORENTO_HEV_4TH_GEN: dbc_dict('hyundai_canfd', None),
+  CAR.KONA_EV_2ND_GEN: dbc_dict('hyundai_canfd', None),
+  CAR.KIA_K8_HEV_1ST_GEN: dbc_dict('hyundai_canfd', None),
+  CAR.CUSTIN_1ST_GEN: dbc_dict('hyundai_kia_generic', None),
 }
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index ec9f4a0f06..1b68b1dbcf 100644
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@@ -1,19 +1,20 @@
 import yaml
 import os
 import time
+import numpy as np
 from abc import abstractmethod, ABC
 from typing import Any, Dict, Optional, Tuple, List, Callable
 
 from cereal import car
-from common.basedir import BASEDIR
-from common.conversions import Conversions as CV
-from common.kalman.simple_kalman import KF1D
-from common.numpy_fast import clip
-from common.realtime import DT_CTRL
-from selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness
-from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, get_friction
-from selfdrive.controls.lib.events import Events
-from selfdrive.controls.lib.vehicle_model import VehicleModel
+from openpilot.common.basedir import BASEDIR
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.kalman.simple_kalman import KF1D, get_kalman_gain
+from openpilot.common.numpy_fast import clip
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG
+from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, get_friction
+from openpilot.selfdrive.controls.lib.events import Events
+from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
 
 ButtonType = car.CarState.ButtonEvent.Type
 GearShifter = car.CarState.GearShifter
@@ -100,22 +101,20 @@ class CarInterfaceBase(ABC):
     ret = CarInterfaceBase.get_std_params(candidate)
     ret = cls._get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs)
 
-    # Set common params using fields set by the car interface
-    # TODO: get actual value, for now starting with reasonable value for
-    # civic and scaling by mass and wheelbase
-    ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
+    # Vehicle mass is published curb weight plus assumed payload such as a human driver; notCars have no assumed payload
+    if not ret.notCar:
+      ret.mass = ret.mass + STD_CARGO_KG
 
-    # TODO: some car interfaces set stiffness factor
-    if ret.tireStiffnessFront == 0 or ret.tireStiffnessRear == 0:
-      # TODO: start from empirically derived lateral slip stiffness for the civic and scale by
-      # mass and CG position, so all cars will have approximately similar dyn behaviors
-      ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront)
+    # Set params dependent on values set by the car interface
+    ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
+    ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, ret.tireStiffnessFactor)
 
     return ret
 
   @staticmethod
   @abstractmethod
-  def _get_params(ret: car.CarParams, candidate: str, fingerprint: Dict[int, Dict[int, int]], car_fw: List[car.CarParams.CarFw], experimental_long: bool, docs: bool):
+  def _get_params(ret: car.CarParams, candidate: str, fingerprint: Dict[int, Dict[int, int]],
+                  car_fw: List[car.CarParams.CarFw], experimental_long: bool, docs: bool):
     raise NotImplementedError
 
   @staticmethod
@@ -151,6 +150,7 @@ class CarInterfaceBase(ABC):
     ret.autoResumeSng = True  # describes whether car can resume from a stop automatically
 
     # standard ALC params
+    ret.tireStiffnessFactor = 1.0
     ret.steerControlType = car.CarParams.SteerControlType.torque
     ret.minSteerSpeed = 0.
     ret.wheelSpeedFactor = 1.0
@@ -336,12 +336,13 @@ class CarStateBase(ABC):
     self.cluster_speed_hyst_gap = 0.0
     self.cluster_min_speed = 0.0  # min speed before dropping to 0
 
-    # Q = np.matrix([[0.0, 0.0], [0.0, 100.0]])
-    # R = 0.3
-    self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
-                         A=[[1.0, DT_CTRL], [0.0, 1.0]],
-                         C=[1.0, 0.0],
-                         K=[[0.17406039], [1.65925647]])
+    Q = [[0.0, 0.0], [0.0, 100.0]]
+    R = 0.3
+    A = [[1.0, DT_CTRL], [0.0, 1.0]]
+    C = [[1.0, 0.0]]
+    x0=[[0.0], [0.0]]
+    K = get_kalman_gain(DT_CTRL, np.array(A), np.array(C), np.array(Q), R)
+    self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K)
 
   def update_speed_kf(self, v_ego_raw):
     if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0:  # Prevent large accelerations when car starts at non zero speed
@@ -442,7 +443,7 @@ def get_interface_attr(attr: str, combine_brands: bool = False, ignore_none: boo
   for car_folder in sorted([x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]):
     try:
       brand_name = car_folder.split('/')[-1]
-      brand_values = __import__(f'selfdrive.car.{brand_name}.values', fromlist=[attr])
+      brand_values = __import__(f'openpilot.selfdrive.car.{brand_name}.values', fromlist=[attr])
       if hasattr(brand_values, attr) or not ignore_none:
         attr_data = getattr(brand_values, attr, None)
       else:
diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py
index 93033126a0..adbc598ea6 100644
--- a/selfdrive/car/isotp_parallel_query.py
+++ b/selfdrive/car/isotp_parallel_query.py
@@ -3,8 +3,8 @@ from collections import defaultdict
 from functools import partial
 
 import cereal.messaging as messaging
-from system.swaglog import cloudlog
-from selfdrive.boardd.boardd import can_list_to_can_capnp
+from openpilot.system.swaglog import cloudlog
+from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp
 from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_addr_for_tx_addr
 
 
diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py
index cb401a8abd..320ad19bb4 100644
--- a/selfdrive/car/mazda/carcontroller.py
+++ b/selfdrive/car/mazda/carcontroller.py
@@ -1,8 +1,8 @@
 from cereal import car
 from opendbc.can.packer import CANPacker
-from selfdrive.car import apply_driver_steer_torque_limits
-from selfdrive.car.mazda import mazdacan
-from selfdrive.car.mazda.values import CarControllerParams, Buttons
+from openpilot.selfdrive.car import apply_driver_steer_torque_limits
+from openpilot.selfdrive.car.mazda import mazdacan
+from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py
index af88308954..1f7846ca06 100644
--- a/selfdrive/car/mazda/carstate.py
+++ b/selfdrive/car/mazda/carstate.py
@@ -1,9 +1,9 @@
 from cereal import car
-from common.conversions import Conversions as CV
+from openpilot.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
-from selfdrive.car.mazda.values import DBC, LKAS_LIMITS, GEN1
+from openpilot.selfdrive.car.interfaces import CarStateBase
+from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, GEN1
 
 class CarState(CarStateBase):
   def __init__(self, CP):
@@ -107,22 +107,7 @@ class CarState(CarStateBase):
 
   @staticmethod
   def get_can_parser(CP):
-    signals = [
-      # sig_name, sig_address
-      ("LEFT_BLINK", "BLINK_INFO"),
-      ("RIGHT_BLINK", "BLINK_INFO"),
-      ("HIGH_BEAMS", "BLINK_INFO"),
-      ("STEER_ANGLE", "STEER"),
-      ("STEER_ANGLE_RATE", "STEER_RATE"),
-      ("STEER_TORQUE_SENSOR", "STEER_TORQUE"),
-      ("STEER_TORQUE_MOTOR", "STEER_TORQUE"),
-      ("FL", "WHEEL_SPEEDS"),
-      ("FR", "WHEEL_SPEEDS"),
-      ("RL", "WHEEL_SPEEDS"),
-      ("RR", "WHEEL_SPEEDS"),
-    ]
-
-    checks = [
+    messages = [
       # sig_address, frequency
       ("BLINK_INFO", 10),
       ("STEER", 67),
@@ -132,30 +117,7 @@ class CarState(CarStateBase):
     ]
 
     if CP.carFingerprint in GEN1:
-      signals += [
-        ("LKAS_BLOCK", "STEER_RATE"),
-        ("LKAS_TRACK_STATE", "STEER_RATE"),
-        ("HANDS_OFF_5_SECONDS", "STEER_RATE"),
-        ("CRZ_ACTIVE", "CRZ_CTRL"),
-        ("CRZ_AVAILABLE", "CRZ_CTRL"),
-        ("CRZ_SPEED", "CRZ_EVENTS"),
-        ("STANDSTILL", "PEDALS"),
-        ("BRAKE_ON", "PEDALS"),
-        ("BRAKE_PRESSURE", "BRAKE"),
-        ("GEAR", "GEAR"),
-        ("DRIVER_SEATBELT", "SEATBELT"),
-        ("FL", "DOORS"),
-        ("FR", "DOORS"),
-        ("BL", "DOORS"),
-        ("BR", "DOORS"),
-        ("PEDAL_GAS", "ENGINE_DATA"),
-        ("SPEED", "ENGINE_DATA"),
-        ("CTR", "CRZ_BTNS"),
-        ("LEFT_BS_STATUS", "BSM"),
-        ("RIGHT_BS_STATUS", "BSM"),
-      ]
-
-      checks += [
+      messages += [
         ("ENGINE_DATA", 100),
         ("CRZ_CTRL", 50),
         ("CRZ_EVENTS", 50),
@@ -168,41 +130,17 @@ class CarState(CarStateBase):
         ("BSM", 10),
       ]
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
 
   @staticmethod
   def get_cam_can_parser(CP):
-    signals = []
-    checks = []
+    messages = []
 
     if CP.carFingerprint in GEN1:
-      signals += [
-        # sig_name, sig_address
-        ("LKAS_REQUEST", "CAM_LKAS"),
-        ("CTR", "CAM_LKAS"),
-        ("ERR_BIT_1", "CAM_LKAS"),
-        ("LINE_NOT_VISIBLE", "CAM_LKAS"),
-        ("BIT_1", "CAM_LKAS"),
-        ("ERR_BIT_2", "CAM_LKAS"),
-        ("STEERING_ANGLE", "CAM_LKAS"),
-        ("ANGLE_ENABLED", "CAM_LKAS"),
-        ("CHKSUM", "CAM_LKAS"),
-
-        ("LINE_VISIBLE", "CAM_LANEINFO"),
-        ("LINE_NOT_VISIBLE", "CAM_LANEINFO"),
-        ("LANE_LINES", "CAM_LANEINFO"),
-        ("BIT1", "CAM_LANEINFO"),
-        ("BIT2", "CAM_LANEINFO"),
-        ("BIT3", "CAM_LANEINFO"),
-        ("NO_ERR_BIT", "CAM_LANEINFO"),
-        ("S1", "CAM_LANEINFO"),
-        ("S1_HBEAM", "CAM_LANEINFO"),
-      ]
-
-      checks += [
+      messages += [
         # sig_address, frequency
         ("CAM_LANEINFO", 2),
         ("CAM_LKAS", 16),
       ]
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py
index 443116bc18..7ac93d9dee 100755
--- a/selfdrive/car/mazda/interface.py
+++ b/selfdrive/car/mazda/interface.py
@@ -1,9 +1,9 @@
 #!/usr/bin/env python3
 from cereal import car
-from common.conversions import Conversions as CV
-from selfdrive.car.mazda.values import CAR, LKAS_LIMITS
-from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config
-from selfdrive.car.interfaces import CarInterfaceBase
+from openpilot.common.conversions import Conversions as CV
+from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS
+from openpilot.selfdrive.car import get_safety_config
+from openpilot.selfdrive.car.interfaces import CarInterfaceBase
 
 ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
@@ -20,24 +20,24 @@ class CarInterface(CarInterfaceBase):
 
     ret.steerActuatorDelay = 0.1
     ret.steerLimitTimer = 0.8
-    tire_stiffness_factor = 0.70   # not optimized yet
+    ret.tireStiffnessFactor = 0.70   # not optimized yet
 
     CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
 
     if candidate in (CAR.CX5, CAR.CX5_2022):
-      ret.mass = 3655 * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 3655 * CV.LB_TO_KG
       ret.wheelbase = 2.7
       ret.steerRatio = 15.5
     elif candidate in (CAR.CX9, CAR.CX9_2021):
-      ret.mass = 4217 * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 4217 * CV.LB_TO_KG
       ret.wheelbase = 3.1
       ret.steerRatio = 17.6
     elif candidate == CAR.MAZDA3:
-      ret.mass = 2875 * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 2875 * CV.LB_TO_KG
       ret.wheelbase = 2.7
       ret.steerRatio = 14.0
     elif candidate == CAR.MAZDA6:
-      ret.mass = 3443 * CV.LB_TO_KG + STD_CARGO_KG
+      ret.mass = 3443 * CV.LB_TO_KG
       ret.wheelbase = 2.83
       ret.steerRatio = 15.5
 
@@ -46,11 +46,6 @@ class CarInterface(CarInterfaceBase):
 
     ret.centerToFront = ret.wheelbase * 0.41
 
-    # TODO: start from empirically derived lateral slip stiffness for the civic and scale by
-    # mass and CG position, so all cars will have approximately similar dyn behaviors
-    ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
-                                                                         tire_stiffness_factor=tire_stiffness_factor)
-
     return ret
 
   # returns a car.CarState
diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py
index 58a505f917..e350c5587f 100644
--- a/selfdrive/car/mazda/mazdacan.py
+++ b/selfdrive/car/mazda/mazdacan.py
@@ -1,4 +1,4 @@
-from selfdrive.car.mazda.values import GEN1, Buttons
+from openpilot.selfdrive.car.mazda.values import GEN1, Buttons
 
 
 def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
diff --git a/selfdrive/car/mazda/radar_interface.py b/selfdrive/car/mazda/radar_interface.py
index b2f7651136..b461fcd5f8 100755
--- a/selfdrive/car/mazda/radar_interface.py
+++ b/selfdrive/car/mazda/radar_interface.py
@@ -1,5 +1,5 @@
 #!/usr/bin/env python3
-from selfdrive.car.interfaces import RadarInterfaceBase
+from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
 
 class RadarInterface(RadarInterfaceBase):
   pass
diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py
index 71fd9f186c..1547f69b04 100644
--- a/selfdrive/car/mazda/values.py
+++ b/selfdrive/car/mazda/values.py
@@ -1,10 +1,11 @@
 from dataclasses import dataclass, field
+from enum import StrEnum
 from typing import Dict, List, Union
 
 from cereal import car
-from selfdrive.car import dbc_dict
-from selfdrive.car.docs_definitions import CarHarness, CarInfo, CarParts
-from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
+from openpilot.selfdrive.car import dbc_dict
+from openpilot.selfdrive.car.docs_definitions import CarHarness, CarInfo, CarParts
+from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
 
 Ecu = car.CarParams.Ecu
 
@@ -25,7 +26,7 @@ class CarControllerParams:
     pass
 
 
-class CAR:
+class CAR(StrEnum):
   CX5 = "MAZDA CX-5"
   CX9 = "MAZDA CX-9"
   MAZDA3 = "MAZDA 3"
@@ -46,7 +47,7 @@ CAR_INFO: Dict[str, Union[MazdaCarInfo, List[MazdaCarInfo]]] = {
   CAR.MAZDA3: MazdaCarInfo("Mazda 3 2017-18"),
   CAR.MAZDA6: MazdaCarInfo("Mazda 6 2017-20"),
   CAR.CX9_2021: MazdaCarInfo("Mazda CX-9 2021-23", video_link="https://youtu.be/dA3duO4a0O4"),
-  CAR.CX5_2022: MazdaCarInfo("Mazda CX-5 2022-23"),
+  CAR.CX5_2022: MazdaCarInfo("Mazda CX-5 2022-24"),
 }
 
 
@@ -88,14 +89,17 @@ FW_VERSIONS = {
     (Ecu.engine, 0x7e0, None): [
       b'PX2G-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'PX2H-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'PX2H-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'PX85-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'SH54-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'PXFG-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'PEW5-188K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x764, None): [
       b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.abs, 0x760, None): [
+      b'KGWD-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'KSD5-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.fwdCamera, 0x706, None): [
@@ -105,9 +109,12 @@ FW_VERSIONS = {
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'PYB2-21PS1-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'PYB2-21PS1-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'SH51-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'PXDL-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'PXFG-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'PXFG-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'PG69-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
   },
   CAR.CX5: {
diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py
index 1c74aef1fa..c7821bdb97 100755
--- a/selfdrive/car/mock/interface.py
+++ b/selfdrive/car/mock/interface.py
@@ -1,63 +1,35 @@
 #!/usr/bin/env python3
 from cereal import car
-from system.swaglog import cloudlog
 import cereal.messaging as messaging
-from selfdrive.car import get_safety_config
-from selfdrive.car.interfaces import CarInterfaceBase
+from openpilot.selfdrive.car.interfaces import CarInterfaceBase
 
-
-# mocked car interface to work with chffrplus
+# mocked car interface for dashcam mode
 class CarInterface(CarInterfaceBase):
   def __init__(self, CP, CarController, CarState):
     super().__init__(CP, CarController, CarState)
 
-    cloudlog.debug("Using Mock Car Interface")
-
-    self.sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal'])
-
     self.speed = 0.
-    self.prev_speed = 0.
+    self.sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal'])
 
   @staticmethod
   def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
     ret.carName = "mock"
-    ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput)]
     ret.mass = 1700.
     ret.wheelbase = 2.70
     ret.centerToFront = ret.wheelbase * 0.5
-    ret.steerRatio = 13.  # reasonable
-    ret.tireStiffnessFront = 1e6    # very stiff to neglect slip
-    ret.tireStiffnessRear = 1e6     # very stiff to neglect slip
-
+    ret.steerRatio = 13.
     return ret
 
-  # returns a car.CarState
   def _update(self, c):
     self.sm.update(0)
     gps_sock = 'gpsLocationExternal' if self.sm.rcv_frame['gpsLocationExternal'] > 1 else 'gpsLocation'
-    if self.sm.updated[gps_sock]:
-      self.prev_speed = self.speed
-      self.speed = self.sm[gps_sock].speed
 
-    # create message
     ret = car.CarState.new_message()
-
-    # speeds
-    ret.vEgo = self.speed
-    ret.vEgoRaw = self.speed
-
-    ret.aEgo = self.speed - self.prev_speed
-    ret.brakePressed = ret.aEgo < -0.5
-
-    ret.standstill = self.speed < 0.01
-    ret.wheelSpeeds.fl = self.speed
-    ret.wheelSpeeds.fr = self.speed
-    ret.wheelSpeeds.rl = self.speed
-    ret.wheelSpeeds.rr = self.speed
+    ret.vEgo = self.sm[gps_sock].speed
+    ret.vEgoRaw = self.sm[gps_sock].speed
 
     return ret
 
   def apply(self, c, now_nanos):
-    # in mock no carcontrols
     actuators = car.CarControl.Actuators.new_message()
     return actuators, []
diff --git a/selfdrive/car/mock/radar_interface.py b/selfdrive/car/mock/radar_interface.py
old mode 100755
new mode 100644
index b2f7651136..e654bd61fd
--- a/selfdrive/car/mock/radar_interface.py
+++ b/selfdrive/car/mock/radar_interface.py
@@ -1,5 +1,4 @@
-#!/usr/bin/env python3
-from selfdrive.car.interfaces import RadarInterfaceBase
+from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
 
 class RadarInterface(RadarInterfaceBase):
   pass
diff --git a/selfdrive/car/mock/values.py b/selfdrive/car/mock/values.py
index dfc7902e41..c6c96579b4 100644
--- a/selfdrive/car/mock/values.py
+++ b/selfdrive/car/mock/values.py
@@ -1,9 +1,10 @@
+from enum import StrEnum
 from typing import Dict, List, Optional, Union
 
-from selfdrive.car.docs_definitions import CarInfo
+from openpilot.selfdrive.car.docs_definitions import CarInfo
 
 
-class CAR:
+class CAR(StrEnum):
   MOCK = 'mock'
 
 
diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py
index 4e99d24903..2da518bbf1 100644
--- a/selfdrive/car/nissan/carcontroller.py
+++ b/selfdrive/car/nissan/carcontroller.py
@@ -1,8 +1,8 @@
 from cereal import car
 from opendbc.can.packer import CANPacker
-from selfdrive.car import apply_std_steer_angle_limits
-from selfdrive.car.nissan import nissancan
-from selfdrive.car.nissan.values import CAR, CarControllerParams
+from openpilot.selfdrive.car import apply_std_steer_angle_limits
+from openpilot.selfdrive.car.nissan import nissancan
+from openpilot.selfdrive.car.nissan.values import CAR, CarControllerParams
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
@@ -66,8 +66,8 @@ class CarController:
     # Below are the HUD messages. We copy the stock message and modify
     if self.CP.carFingerprint != CAR.ALTIMA:
       if self.frame % 2 == 0:
-        can_sends.append(nissancan.create_lkas_hud_msg(
-          self.packer, CS.lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart))
+        can_sends.append(nissancan.create_lkas_hud_msg(self.packer, CS.lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible,
+                                                       hud_control.leftLaneDepart, hud_control.rightLaneDepart))
 
       if self.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 7fbc807665..b2ba9ce290 100644
--- a/selfdrive/car/nissan/carstate.py
+++ b/selfdrive/car/nissan/carstate.py
@@ -2,10 +2,10 @@ import copy
 from collections import deque
 from cereal import car
 from opendbc.can.can_define import CANDefine
-from selfdrive.car.interfaces import CarStateBase
-from common.conversions import Conversions as CV
+from openpilot.selfdrive.car.interfaces import CarStateBase
+from openpilot.common.conversions import Conversions as CV
 from opendbc.can.parser import CANParser
-from selfdrive.car.nissan.values import CAR, DBC, CarControllerParams
+from openpilot.selfdrive.car.nissan.values import CAR, DBC, CarControllerParams
 
 TORQUE_SAMPLES = 12
 
@@ -119,29 +119,7 @@ class CarState(CarStateBase):
 
   @staticmethod
   def get_can_parser(CP):
-    signals = [
-      # sig_name, sig_address
-      ("WHEEL_SPEED_FL", "WHEEL_SPEEDS_FRONT"),
-      ("WHEEL_SPEED_FR", "WHEEL_SPEEDS_FRONT"),
-      ("WHEEL_SPEED_RL", "WHEEL_SPEEDS_REAR"),
-      ("WHEEL_SPEED_RR", "WHEEL_SPEEDS_REAR"),
-
-      ("STEER_ANGLE", "STEER_ANGLE_SENSOR"),
-
-      ("DOOR_OPEN_FR", "DOORS_LIGHTS"),
-      ("DOOR_OPEN_FL", "DOORS_LIGHTS"),
-      ("DOOR_OPEN_RR", "DOORS_LIGHTS"),
-      ("DOOR_OPEN_RL", "DOORS_LIGHTS"),
-
-      ("RIGHT_BLINKER", "LIGHTS"),
-      ("LEFT_BLINKER", "LIGHTS"),
-
-      ("ESP_DISABLED", "ESP"),
-
-      ("GEAR_SHIFTER", "GEARBOX"),
-    ]
-
-    checks = [
+    messages = [
       # sig_address, frequency
       ("STEER_ANGLE_SENSOR", 100),
       ("WHEEL_SPEEDS_REAR", 50),
@@ -153,51 +131,14 @@ class CarState(CarStateBase):
     ]
 
     if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA):
-      signals += [
-        ("USER_BRAKE_PRESSED", "DOORS_LIGHTS"),
-
-        ("GAS_PEDAL", "GAS_PEDAL"),
-        ("SEATBELT_DRIVER_LATCHED", "HUD"),
-        ("SPEED_MPH", "HUD"),
-
-        ("PROPILOT_BUTTON", "CRUISE_THROTTLE"),
-        ("CANCEL_BUTTON", "CRUISE_THROTTLE"),
-        ("GAS_PEDAL_INVERTED", "CRUISE_THROTTLE"),
-        ("SET_BUTTON", "CRUISE_THROTTLE"),
-        ("RES_BUTTON", "CRUISE_THROTTLE"),
-        ("FOLLOW_DISTANCE_BUTTON", "CRUISE_THROTTLE"),
-        ("NO_BUTTON_PRESSED", "CRUISE_THROTTLE"),
-        ("GAS_PEDAL", "CRUISE_THROTTLE"),
-        ("USER_BRAKE_PRESSED", "CRUISE_THROTTLE"),
-        ("NEW_SIGNAL_2", "CRUISE_THROTTLE"),
-        ("GAS_PRESSED_INVERTED", "CRUISE_THROTTLE"),
-        ("COUNTER", "CRUISE_THROTTLE"),
-        ("unsure1", "CRUISE_THROTTLE"),
-        ("unsure2", "CRUISE_THROTTLE"),
-        ("unsure3", "CRUISE_THROTTLE"),
-      ]
-
-      checks += [
+      messages += [
         ("GAS_PEDAL", 100),
         ("CRUISE_THROTTLE", 50),
         ("HUD", 25),
       ]
 
     elif CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC):
-      signals += [
-        ("USER_BRAKE_PRESSED", "CRUISE_THROTTLE"),
-        ("GAS_PEDAL", "CRUISE_THROTTLE"),
-        ("CRUISE_AVAILABLE", "CRUISE_THROTTLE"),
-        ("SPEED_MPH", "HUD_SETTINGS"),
-        ("SEATBELT_DRIVER_LATCHED", "SEATBELT"),
-
-        # Copy other values, we use this to cancel
-        ("CANCEL_SEATBELT", "CANCEL_MSG"),
-        ("NEW_SIGNAL_1", "CANCEL_MSG"),
-        ("NEW_SIGNAL_2", "CANCEL_MSG"),
-        ("NEW_SIGNAL_3", "CANCEL_MSG"),
-      ]
-      checks += [
+      messages += [
         ("BRAKE_PEDAL", 100),
         ("CRUISE_THROTTLE", 50),
         ("CANCEL_MSG", 50),
@@ -206,126 +147,28 @@ class CarState(CarStateBase):
       ]
 
     if CP.carFingerprint == CAR.ALTIMA:
-      signals += [
-        ("LKAS_ENABLED", "LKAS_SETTINGS"),
-        ("CRUISE_ENABLED", "CRUISE_STATE"),
-        ("SET_SPEED", "PROPILOT_HUD"),
-      ]
-      checks += [
+      messages += [
         ("CRUISE_STATE", 10),
         ("LKAS_SETTINGS", 10),
         ("PROPILOT_HUD", 50),
       ]
-      return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1)
+      return CANParser(DBC[CP.carFingerprint]["pt"], messages, 1)
 
-    signals.append(("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR"))
-    checks.append(("STEER_TORQUE_SENSOR", 100))
+    messages.append(("STEER_TORQUE_SENSOR", 100))
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
 
   @staticmethod
   def get_adas_can_parser(CP):
     # this function generates lists for signal, messages and initial values
 
     if CP.carFingerprint == CAR.ALTIMA:
-      signals = [
-        ("DESIRED_ANGLE", "LKAS"),
-        ("SET_0x80_2", "LKAS"),
-        ("MAX_TORQUE", "LKAS"),
-        ("SET_0x80", "LKAS"),
-        ("COUNTER", "LKAS"),
-        ("LKA_ACTIVE", "LKAS"),
-
-        ("CRUISE_ON", "PRO_PILOT"),
-      ]
-      checks = [
+      messages = [
         ("LKAS", 100),
         ("PRO_PILOT", 100),
       ]
     else:
-      signals = [
-        # sig_name, sig_address
-        ("LKAS_ENABLED", "LKAS_SETTINGS"),
-
-        ("CRUISE_ENABLED", "CRUISE_STATE"),
-
-        ("DESIRED_ANGLE", "LKAS"),
-        ("SET_0x80_2", "LKAS"),
-        ("MAX_TORQUE", "LKAS"),
-        ("SET_0x80", "LKAS"),
-        ("COUNTER", "LKAS"),
-        ("LKA_ACTIVE", "LKAS"),
-
-        # Below are the HUD messages. We copy the stock message and modify
-        ("LARGE_WARNING_FLASHING", "PROPILOT_HUD"),
-        ("SIDE_RADAR_ERROR_FLASHING1", "PROPILOT_HUD"),
-        ("SIDE_RADAR_ERROR_FLASHING2", "PROPILOT_HUD"),
-        ("LEAD_CAR", "PROPILOT_HUD"),
-        ("LEAD_CAR_ERROR", "PROPILOT_HUD"),
-        ("FRONT_RADAR_ERROR", "PROPILOT_HUD"),
-        ("FRONT_RADAR_ERROR_FLASHING", "PROPILOT_HUD"),
-        ("SIDE_RADAR_ERROR_FLASHING3", "PROPILOT_HUD"),
-        ("LKAS_ERROR_FLASHING", "PROPILOT_HUD"),
-        ("SAFETY_SHIELD_ACTIVE", "PROPILOT_HUD"),
-        ("RIGHT_LANE_GREEN_FLASH", "PROPILOT_HUD"),
-        ("LEFT_LANE_GREEN_FLASH", "PROPILOT_HUD"),
-        ("FOLLOW_DISTANCE", "PROPILOT_HUD"),
-        ("AUDIBLE_TONE", "PROPILOT_HUD"),
-        ("SPEED_SET_ICON", "PROPILOT_HUD"),
-        ("SMALL_STEERING_WHEEL_ICON", "PROPILOT_HUD"),
-        ("unknown59", "PROPILOT_HUD"),
-        ("unknown55", "PROPILOT_HUD"),
-        ("unknown26", "PROPILOT_HUD"),
-        ("unknown28", "PROPILOT_HUD"),
-        ("unknown31", "PROPILOT_HUD"),
-        ("SET_SPEED", "PROPILOT_HUD"),
-        ("unknown43", "PROPILOT_HUD"),
-        ("unknown08", "PROPILOT_HUD"),
-        ("unknown05", "PROPILOT_HUD"),
-        ("unknown02", "PROPILOT_HUD"),
-
-        ("NA_HIGH_ACCEL_TEMP", "PROPILOT_HUD_INFO_MSG"),
-        ("SIDE_RADAR_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"),
-        ("SIDE_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"),
-        ("LKAS_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"),
-        ("FRONT_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"),
-        ("SIDE_RADAR_NA_CLEAN_REAR_CAMERA", "PROPILOT_HUD_INFO_MSG"),
-        ("NA_POOR_ROAD_CONDITIONS", "PROPILOT_HUD_INFO_MSG"),
-        ("CURRENTLY_UNAVAILABLE", "PROPILOT_HUD_INFO_MSG"),
-        ("SAFETY_SHIELD_OFF", "PROPILOT_HUD_INFO_MSG"),
-        ("FRONT_COLLISION_NA_FRONT_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG"),
-        ("PEDAL_MISSAPPLICATION_SYSTEM_ACTIVATED", "PROPILOT_HUD_INFO_MSG"),
-        ("SIDE_IMPACT_NA_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG"),
-        ("WARNING_DO_NOT_ENTER", "PROPILOT_HUD_INFO_MSG"),
-        ("SIDE_IMPACT_SYSTEM_OFF", "PROPILOT_HUD_INFO_MSG"),
-        ("SIDE_IMPACT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"),
-        ("FRONT_COLLISION_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"),
-        ("SIDE_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"),
-        ("LKAS_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"),
-        ("FRONT_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"),
-        ("PROPILOT_NA_MSGS", "PROPILOT_HUD_INFO_MSG"),
-        ("BOTTOM_MSG", "PROPILOT_HUD_INFO_MSG"),
-        ("HANDS_ON_WHEEL_WARNING", "PROPILOT_HUD_INFO_MSG"),
-        ("WARNING_STEP_ON_BRAKE_NOW", "PROPILOT_HUD_INFO_MSG"),
-        ("PROPILOT_NA_FRONT_CAMERA_OBSTRUCTED", "PROPILOT_HUD_INFO_MSG"),
-        ("PROPILOT_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"),
-        ("WARNING_PROPILOT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"),
-        ("ACC_UNAVAILABLE_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"),
-        ("ACC_NA_FRONT_CAMERA_IMPARED", "PROPILOT_HUD_INFO_MSG"),
-        ("unknown07", "PROPILOT_HUD_INFO_MSG"),
-        ("unknown10", "PROPILOT_HUD_INFO_MSG"),
-        ("unknown15", "PROPILOT_HUD_INFO_MSG"),
-        ("unknown23", "PROPILOT_HUD_INFO_MSG"),
-        ("unknown19", "PROPILOT_HUD_INFO_MSG"),
-        ("unknown31", "PROPILOT_HUD_INFO_MSG"),
-        ("unknown32", "PROPILOT_HUD_INFO_MSG"),
-        ("unknown46", "PROPILOT_HUD_INFO_MSG"),
-        ("unknown61", "PROPILOT_HUD_INFO_MSG"),
-        ("unknown55", "PROPILOT_HUD_INFO_MSG"),
-        ("unknown50", "PROPILOT_HUD_INFO_MSG"),
-      ]
-
-      checks = [
+      messages = [
         ("PROPILOT_HUD_INFO_MSG", 2),
         ("LKAS_SETTINGS", 10),
         ("CRUISE_STATE", 50),
@@ -333,19 +176,16 @@ class CarState(CarStateBase):
         ("LKAS", 100),
       ]
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
 
   @staticmethod
   def get_cam_can_parser(CP):
-    signals = []
-    checks = []
+    messages = []
 
     if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL):
-      signals.append(("CRUISE_ON", "PRO_PILOT"))
-      checks.append(("PRO_PILOT", 100))
+      messages.append(("PRO_PILOT", 100))
     elif CP.carFingerprint == CAR.ALTIMA:
-      signals.append(("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR"))
-      checks.append(("STEER_TORQUE_SENSOR", 100))
-      return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
+      messages.append(("STEER_TORQUE_SENSOR", 100))
+      return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, 1)
diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py
index 573dff9f05..aedcaa1887 100644
--- a/selfdrive/car/nissan/interface.py
+++ b/selfdrive/car/nissan/interface.py
@@ -1,8 +1,8 @@
-#!/usr/bin/env python3
 from cereal import car
-from selfdrive.car import STD_CARGO_KG, get_safety_config
-from selfdrive.car.interfaces import CarInterfaceBase
-from selfdrive.car.nissan.values import CAR
+from panda import Panda
+from openpilot.selfdrive.car import get_safety_config
+from openpilot.selfdrive.car.interfaces import CarInterfaceBase
+from openpilot.selfdrive.car.nissan.values import CAR
 
 
 class CarInterface(CarInterfaceBase):
@@ -22,17 +22,17 @@ class CarInterface(CarInterfaceBase):
     ret.radarUnavailable = True
 
     if candidate in (CAR.ROGUE, CAR.XTRAIL):
-      ret.mass = 1610 + STD_CARGO_KG
+      ret.mass = 1610
       ret.wheelbase = 2.705
       ret.centerToFront = ret.wheelbase * 0.44
     elif candidate in (CAR.LEAF, CAR.LEAF_IC):
-      ret.mass = 1610 + STD_CARGO_KG
+      ret.mass = 1610
       ret.wheelbase = 2.705
       ret.centerToFront = ret.wheelbase * 0.44
     elif candidate == CAR.ALTIMA:
       # Altima has EPS on C-CAN unlike the others that have it on V-CAN
-      ret.safetyConfigs[0].safetyParam = 1 # EPS is on alternate bus
-      ret.mass = 1492 + STD_CARGO_KG
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_NISSAN_ALT_EPS_BUS
+      ret.mass = 1492
       ret.wheelbase = 2.824
       ret.centerToFront = ret.wheelbase * 0.44
 
@@ -47,7 +47,7 @@ class CarInterface(CarInterfaceBase):
     be.type = car.CarState.ButtonEvent.Type.accelCruise
     buttonEvents.append(be)
 
-    events = self.create_common_events(ret)
+    events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.brake])
 
     if self.CS.lkas_enabled:
       events.add(car.CarEvent.EventName.invalidLkasSetting)
diff --git a/selfdrive/car/nissan/nissancan.py b/selfdrive/car/nissan/nissancan.py
index 89754775b1..49dcd6fe93 100644
--- a/selfdrive/car/nissan/nissancan.py
+++ b/selfdrive/car/nissan/nissancan.py
@@ -1,5 +1,5 @@
 import crcmod
-from selfdrive.car.nissan.values import CAR
+from openpilot.selfdrive.car.nissan.values import CAR
 
 # TODO: add this checksum to the CANPacker
 nissan_checksum = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff)
diff --git a/selfdrive/car/nissan/radar_interface.py b/selfdrive/car/nissan/radar_interface.py
index b2f7651136..e654bd61fd 100644
--- a/selfdrive/car/nissan/radar_interface.py
+++ b/selfdrive/car/nissan/radar_interface.py
@@ -1,5 +1,4 @@
-#!/usr/bin/env python3
-from selfdrive.car.interfaces import RadarInterfaceBase
+from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
 
 class RadarInterface(RadarInterfaceBase):
   pass
diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py
index d4e10e11ca..474cb15e7f 100644
--- a/selfdrive/car/nissan/values.py
+++ b/selfdrive/car/nissan/values.py
@@ -1,11 +1,13 @@
+# ruff: noqa: E501
 from dataclasses import dataclass, field
+from enum import StrEnum
 from typing import Dict, List, Optional, Union
 
 from cereal import car
 from panda.python import uds
-from selfdrive.car import AngleRateLimit, dbc_dict
-from selfdrive.car.docs_definitions import CarInfo, CarHarness, CarParts
-from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
+from openpilot.selfdrive.car import AngleRateLimit, dbc_dict
+from openpilot.selfdrive.car.docs_definitions import CarInfo, CarHarness, CarParts
+from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
 
 Ecu = car.CarParams.Ecu
 
@@ -20,7 +22,7 @@ class CarControllerParams:
     pass
 
 
-class CAR:
+class CAR(StrEnum):
   XTRAIL = "NISSAN X-TRAIL 2017"
   LEAF = "NISSAN LEAF 2018"
   # Leaf with ADAS ECU found behind instrument cluster instead of glovebox
@@ -79,9 +81,14 @@ FINGERPRINTS = {
   ]
 }
 
+# Default diagnostic session
 NISSAN_DIAGNOSTIC_REQUEST_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, 0x81])
 NISSAN_DIAGNOSTIC_RESPONSE_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, 0x81])
 
+# Manufacturer specific
+NISSAN_DIAGNOSTIC_REQUEST_KWP_2 = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, 0xda])
+NISSAN_DIAGNOSTIC_RESPONSE_KWP_2 = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, 0xda])
+
 NISSAN_VERSION_REQUEST_KWP = b'\x21\x83'
 NISSAN_VERSION_RESPONSE_KWP = b'\x61\x83'
 
@@ -98,6 +105,11 @@ FW_QUERY_CONFIG = FwQueryConfig(
       [NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP],
       rx_offset=NISSAN_RX_OFFSET,
     ),
+    # Rogue's engine solely responds to this
+    Request(
+      [NISSAN_DIAGNOSTIC_REQUEST_KWP_2, NISSAN_VERSION_REQUEST_KWP],
+      [NISSAN_DIAGNOSTIC_RESPONSE_KWP_2, NISSAN_VERSION_RESPONSE_KWP],
+    ),
     Request(
       [StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
       [StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
@@ -120,6 +132,20 @@ FW_VERSIONS = {
     (Ecu.gateway, 0x18dad0f1, None): [
       b'284U29HE0A',
     ],
+  },
+    CAR.LEAF: {
+    (Ecu.abs, 0x740, None): [
+      b'476606WK9B',
+    ],
+    (Ecu.eps, 0x742, None): [
+      b'5SN2A\xb7A\x05\x02N126F\x15\xb2\x00\x00\x00\x00\x00\x00\x00\x80',
+    ],
+    (Ecu.fwdCamera, 0x707, None): [
+      b'6WK2CDB\x04\x18\x00\x00\x00\x00\x00R=1\x18\x99\x10\x00\x00\x00\x80',
+    ],
+    (Ecu.gateway, 0x18dad0f1, None): [
+      b'284U26WK0C',
+    ],
   },
   CAR.LEAF_IC: {
     (Ecu.fwdCamera, 0x707, None): [
diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py
index b37c88797a..c277f012ab 100644
--- a/selfdrive/car/subaru/carcontroller.py
+++ b/selfdrive/car/subaru/carcontroller.py
@@ -1,7 +1,14 @@
+from openpilot.common.numpy_fast import clip, interp
 from opendbc.can.packer import CANPacker
-from selfdrive.car import apply_driver_steer_torque_limits
-from selfdrive.car.subaru import subarucan
-from selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, CanBus, CarControllerParams, SubaruFlags
+from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance
+from openpilot.selfdrive.car.subaru import subarucan
+from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_ES_ADDR, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, STEER_RATE_LIMITED, \
+                                                  CanBus, CarControllerParams, SubaruFlags
+
+# FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and
+# involves the total steering angle change rather than rate, but these limits work well for now
+MAX_STEER_RATE = 25  # deg/s
+MAX_STEER_RATE_FRAMES = 7  # tx control frames needed before torque can be cut
 
 
 class CarController:
@@ -11,7 +18,7 @@ class CarController:
     self.frame = 0
 
     self.cruise_button_prev = 0
-    self.last_cancel_frame = 0
+    self.steer_rate_counter = 0
 
     self.p = CarControllerParams(CP)
     self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
@@ -36,12 +43,35 @@ class CarController:
         apply_steer = 0
 
       if self.CP.carFingerprint in PREGLOBAL_CARS:
-        can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, CC.latActive))
+        can_sends.append(subarucan.create_preglobal_steering_control(self.packer, self.frame // self.p.STEER_STEP, apply_steer, CC.latActive))
       else:
-        can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, CC.latActive))
+        apply_steer_req = CC.latActive
+
+        if self.CP.carFingerprint in STEER_RATE_LIMITED:
+          # Steering rate fault prevention
+          self.steer_rate_counter, apply_steer_req = \
+            common_fault_avoidance(abs(CS.out.steeringRateDeg) > MAX_STEER_RATE, apply_steer_req,
+                                  self.steer_rate_counter, MAX_STEER_RATE_FRAMES)
+
+        can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, apply_steer_req))
 
       self.apply_steer_last = apply_steer
 
+    # *** longitudinal ***
+
+    if CC.longActive:
+      apply_throttle = int(round(interp(actuators.accel, CarControllerParams.THROTTLE_LOOKUP_BP, CarControllerParams.THROTTLE_LOOKUP_V)))
+      apply_rpm = int(round(interp(actuators.accel, CarControllerParams.RPM_LOOKUP_BP, CarControllerParams.RPM_LOOKUP_V)))
+      apply_brake = int(round(interp(actuators.accel, CarControllerParams.BRAKE_LOOKUP_BP, CarControllerParams.BRAKE_LOOKUP_V)))
+
+      # limit min and max values
+      cruise_throttle = clip(apply_throttle, CarControllerParams.THROTTLE_MIN, CarControllerParams.THROTTLE_MAX)
+      cruise_rpm = clip(apply_rpm, CarControllerParams.RPM_MIN, CarControllerParams.RPM_MAX)
+      cruise_brake = clip(apply_brake, CarControllerParams.BRAKE_MIN, CarControllerParams.BRAKE_MAX)
+    else:
+      cruise_throttle = CarControllerParams.THROTTLE_INACTIVE
+      cruise_rpm = CarControllerParams.RPM_MIN
+      cruise_brake = CarControllerParams.BRAKE_MIN
 
     # *** alerts and pcm cancel ***
     if self.CP.carFingerprint in PREGLOBAL_CARS:
@@ -64,20 +94,46 @@ class CarController:
         can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg))
 
     else:
-      if pcm_cancel_cmd and (self.frame - self.last_cancel_frame) > 0.2:
-        bus = CanBus.alt if self.CP.carFingerprint in GLOBAL_GEN2 else CanBus.main
-        can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, bus, pcm_cancel_cmd))
-        self.last_cancel_frame = self.frame
-
       if self.frame % 10 == 0:
-        can_sends.append(subarucan.create_es_dashstatus(self.packer, CS.es_dashstatus_msg))
+        can_sends.append(subarucan.create_es_dashstatus(self.packer, self.frame // 10, CS.es_dashstatus_msg, CC.enabled, self.CP.openpilotLongitudinalControl,
+                                                        CC.longActive, hud_control.leadVisible))
 
-        can_sends.append(subarucan.create_es_lkas_state(self.packer, CS.es_lkas_state_msg, CC.enabled, hud_control.visualAlert,
+        can_sends.append(subarucan.create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, CC.enabled, hud_control.visualAlert,
                                                         hud_control.leftLaneVisible, hud_control.rightLaneVisible,
                                                         hud_control.leftLaneDepart, hud_control.rightLaneDepart))
 
         if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
-          can_sends.append(subarucan.create_es_infotainment(self.packer, CS.es_infotainment_msg, hud_control.visualAlert))
+          can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg, hud_control.visualAlert))
+
+      if self.CP.openpilotLongitudinalControl:
+        if self.frame % 5 == 0:
+          can_sends.append(subarucan.create_es_status(self.packer, self.frame // 5, CS.es_status_msg,
+                                                      self.CP.openpilotLongitudinalControl, CC.longActive, cruise_rpm))
+
+          can_sends.append(subarucan.create_es_brake(self.packer, self.frame // 5, CS.es_brake_msg, CC.enabled, cruise_brake))
+
+          can_sends.append(subarucan.create_es_distance(self.packer, self.frame // 5, CS.es_distance_msg, 0, pcm_cancel_cmd,
+                                                        self.CP.openpilotLongitudinalControl, cruise_brake > 0, cruise_throttle))
+      else:
+        if pcm_cancel_cmd:
+          if self.CP.carFingerprint not in HYBRID_CARS:
+            bus = CanBus.alt if self.CP.carFingerprint in GLOBAL_GEN2 else CanBus.main
+            can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg["COUNTER"] + 1, CS.es_distance_msg, bus, pcm_cancel_cmd))
+
+      if self.CP.flags & SubaruFlags.DISABLE_EYESIGHT:
+        # Tester present (keeps eyesight disabled)
+        if self.frame % 100 == 0:
+          can_sends.append([GLOBAL_ES_ADDR, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", CanBus.camera])
+
+        # Create all of the other eyesight messages to keep the rest of the car happy when eyesight is disabled
+        if self.frame % 5 == 0:
+          can_sends.append(subarucan.create_es_highbeamassist(self.packer))
+
+        if self.frame % 10 == 0:
+          can_sends.append(subarucan.create_es_static_1(self.packer))
+
+        if self.frame % 2 == 0:
+          can_sends.append(subarucan.create_es_static_2(self.packer))
 
     new_actuators = actuators.copy()
     new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py
index 189c244ca8..c8a6dfe1e6 100644
--- a/selfdrive/car/subaru/carstate.py
+++ b/selfdrive/car/subaru/carstate.py
@@ -1,10 +1,11 @@
 import copy
 from cereal import car
 from opendbc.can.can_define import CANDefine
-from common.conversions import Conversions as CV
-from selfdrive.car.interfaces import CarStateBase
+from openpilot.common.conversions import Conversions as CV
+from openpilot.selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
-from selfdrive.car.subaru.values import DBC, CAR, GLOBAL_GEN2, PREGLOBAL_CARS, CanBus, SubaruFlags
+from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, CanBus, SubaruFlags
+from openpilot.selfdrive.car import CanSignalRateCalculator
 
 
 class CarState(CarStateBase):
@@ -13,13 +14,17 @@ class CarState(CarStateBase):
     can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
     self.shifter_values = can_define.dv["Transmission"]["Gear"]
 
+    self.angle_rate_calulator = CanSignalRateCalculator(50)
+
   def update(self, cp, cp_cam, cp_body):
     ret = car.CarState.new_message()
 
-    ret.gas = cp.vl["Throttle"]["Throttle_Pedal"] / 255.
+    throttle_msg = cp.vl["Throttle"] if self.car_fingerprint not in HYBRID_CARS else cp_body.vl["Throttle_Hybrid"]
+    ret.gas = throttle_msg["Throttle_Pedal"] / 255.
+
     ret.gasPressed = ret.gas > 1e-5
     if self.car_fingerprint in PREGLOBAL_CARS:
-      ret.brakePressed = cp.vl["Brake_Pedal"]["Brake_Pedal"] > 2
+      ret.brakePressed = cp.vl["Brake_Pedal"]["Brake_Pedal"] > 0
     else:
       cp_brakes = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp
       ret.brakePressed = cp_brakes.vl["Brake_Status"]["Brake"] == 1
@@ -43,10 +48,16 @@ class CarState(CarStateBase):
       ret.leftBlindspot = (cp.vl["BSD_RCTA"]["L_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["L_APPROACHING"] == 1)
       ret.rightBlindspot = (cp.vl["BSD_RCTA"]["R_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["R_APPROACHING"] == 1)
 
-    can_gear = int(cp.vl["Transmission"]["Gear"])
+    cp_transmission = cp_body if self.car_fingerprint in HYBRID_CARS else cp
+    can_gear = int(cp_transmission.vl["Transmission"]["Gear"])
     ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
 
     ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"]
+
+    if self.car_fingerprint not in PREGLOBAL_CARS:
+      # ideally we get this from the car, but unclear if it exists. diagnostic software doesn't even have it
+      ret.steeringRateDeg = self.angle_rate_calulator.update(ret.steeringAngleDeg, cp.vl["Steering_Torque"]["COUNTER"])
+
     ret.steeringTorque = cp.vl["Steering_Torque"]["Steer_Torque_Sensor"]
     ret.steeringTorqueEps = cp.vl["Steering_Torque"]["Steer_Torque_Output"]
 
@@ -54,8 +65,12 @@ class CarState(CarStateBase):
     ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold
 
     cp_cruise = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp
-    ret.cruiseState.enabled = cp_cruise.vl["CruiseControl"]["Cruise_Activated"] != 0
-    ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0
+    if self.car_fingerprint in HYBRID_CARS:
+      ret.cruiseState.enabled = cp_cam.vl["ES_DashStatus"]['Cruise_Activated'] != 0
+      ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0
+    else:
+      ret.cruiseState.enabled = cp_cruise.vl["CruiseControl"]["Cruise_Activated"] != 0
+      ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0
     ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.KPH_TO_MS
 
     if (self.car_fingerprint in PREGLOBAL_CARS and cp.vl["Dash_State2"]["UNITS"] == 1) or \
@@ -69,7 +84,7 @@ class CarState(CarStateBase):
                         cp.vl["BodyInfo"]["DOOR_OPEN_FL"]])
     ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1
 
-    cp_es_distance = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam
+    cp_es_distance = cp_body if self.car_fingerprint in (GLOBAL_GEN2 | HYBRID_CARS) else cp_cam
     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"]
@@ -79,12 +94,24 @@ class CarState(CarStateBase):
       ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3
       ret.stockFcw = (cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 1) or \
                      (cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 2)
-      # 8 is known AEB, there are a few other values related to AEB we ignore
-      ret.stockAeb = (cp_es_distance.vl["ES_Brake"]["AEB_Status"] == 8) and \
-                     (cp_es_distance.vl["ES_Brake"]["Brake_Pressure"] != 0)
+
       self.es_lkas_state_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
+      cp_es_brake = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam
+      self.es_brake_msg = copy.copy(cp_es_brake.vl["ES_Brake"])
+      cp_es_status = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam
+
+      # TODO: Hybrid cars don't have ES_Distance, need a replacement
+      if self.car_fingerprint not in HYBRID_CARS:
+        # 8 is known AEB, there are a few other values related to AEB we ignore
+        ret.stockAeb = (cp_es_distance.vl["ES_Brake"]["AEB_Status"] == 8) and \
+                       (cp_es_distance.vl["ES_Brake"]["Brake_Pressure"] != 0)
+
+        self.es_status_msg = copy.copy(cp_es_status.vl["ES_Status"])
+        self.cruise_control_msg = copy.copy(cp_cruise.vl["CruiseControl"])
+
+    if self.car_fingerprint not in HYBRID_CARS:
+      self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"])
 
-    self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"])
     self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"])
     if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
       self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"])
@@ -92,247 +119,102 @@ class CarState(CarStateBase):
     return ret
 
   @staticmethod
-  def get_common_global_body_signals():
-    signals = [
-      ("Cruise_On", "CruiseControl"),
-      ("Cruise_Activated", "CruiseControl"),
-      ("FL", "Wheel_Speeds"),
-      ("FR", "Wheel_Speeds"),
-      ("RL", "Wheel_Speeds"),
-      ("RR", "Wheel_Speeds"),
-      ("Brake", "Brake_Status"),
-    ]
-    checks = [
-      ("CruiseControl", 20),
+  def get_common_global_body_messages(CP):
+    messages = [
       ("Wheel_Speeds", 50),
       ("Brake_Status", 50),
     ]
 
-    return signals, checks
+    if CP.carFingerprint not in HYBRID_CARS:
+      messages.append(("CruiseControl", 20))
 
-  @staticmethod
-  def get_common_global_es_signals():
-    signals = [
-      ("AEB_Status", "ES_Brake"),
-      ("Brake_Pressure", "ES_Brake"),
-      ("COUNTER", "ES_Distance"),
-      ("CHECKSUM", "ES_Distance"),
-      ("Signal1", "ES_Distance"),
-      ("Cruise_Fault", "ES_Distance"),
-      ("Cruise_Throttle", "ES_Distance"),
-      ("Signal2", "ES_Distance"),
-      ("Car_Follow", "ES_Distance"),
-      ("Signal3", "ES_Distance"),
-      ("Cruise_Soft_Disable", "ES_Distance"),
-      ("Signal7", "ES_Distance"),
-      ("Cruise_Brake_Active", "ES_Distance"),
-      ("Distance_Swap", "ES_Distance"),
-      ("Cruise_EPB", "ES_Distance"),
-      ("Signal4", "ES_Distance"),
-      ("Close_Distance", "ES_Distance"),
-      ("Signal5", "ES_Distance"),
-      ("Cruise_Cancel", "ES_Distance"),
-      ("Cruise_Set", "ES_Distance"),
-      ("Cruise_Resume", "ES_Distance"),
-      ("Signal6", "ES_Distance"),
-    ]
+    return messages
 
-    checks = [
+  @staticmethod
+  def get_common_global_es_messages(CP):
+    messages = [
       ("ES_Brake", 20),
-      ("ES_Distance", 20),
     ]
 
-    return signals, checks
+    if CP.carFingerprint not in HYBRID_CARS:
+      messages += [
+        ("ES_Distance", 20),
+        ("ES_Status", 20)
+      ]
+
+    return messages
 
   @staticmethod
-  def get_can_parser(CP):
-    signals = [
-      # sig_name, sig_address
-      ("Steer_Torque_Sensor", "Steering_Torque"),
-      ("Steer_Torque_Output", "Steering_Torque"),
-      ("Steering_Angle", "Steering_Torque"),
-      ("Steer_Error_1", "Steering_Torque"),
-      ("Brake_Pedal", "Brake_Pedal"),
-      ("Throttle_Pedal", "Throttle"),
-      ("LEFT_BLINKER", "Dashlights"),
-      ("RIGHT_BLINKER", "Dashlights"),
-      ("SEATBELT_FL", "Dashlights"),
-      ("DOOR_OPEN_FR", "BodyInfo"),
-      ("DOOR_OPEN_FL", "BodyInfo"),
-      ("DOOR_OPEN_RR", "BodyInfo"),
-      ("DOOR_OPEN_RL", "BodyInfo"),
-      ("Gear", "Transmission"),
+  def get_common_preglobal_body_messages():
+    messages = [
+      ("CruiseControl", 50),
+      ("Wheel_Speeds", 50),
+      ("Dash_State2", 1),
     ]
 
-    checks = [
+    return messages
+
+  @staticmethod
+  def get_can_parser(CP):
+    messages = [
       # sig_address, frequency
-      ("Throttle", 100),
       ("Dashlights", 10),
-      ("Brake_Pedal", 50),
-      ("Transmission", 100),
       ("Steering_Torque", 50),
       ("BodyInfo", 1),
+      ("Brake_Pedal", 50),
     ]
 
-    if CP.enableBsm:
-      signals += [
-        ("L_ADJACENT", "BSD_RCTA"),
-        ("R_ADJACENT", "BSD_RCTA"),
-        ("L_APPROACHING", "BSD_RCTA"),
-        ("R_APPROACHING", "BSD_RCTA"),
+    if CP.carFingerprint not in HYBRID_CARS:
+      messages += [
+        ("Throttle", 100),
+        ("Transmission", 100)
       ]
-      checks.append(("BSD_RCTA", 17))
+
+    if CP.enableBsm:
+      messages.append(("BSD_RCTA", 17))
 
     if CP.carFingerprint not in PREGLOBAL_CARS:
       if CP.carFingerprint not in GLOBAL_GEN2:
-        signals += CarState.get_common_global_body_signals()[0]
-        checks += CarState.get_common_global_body_signals()[1]
-
-      signals += [
-        ("Steer_Warning", "Steering_Torque"),
-        ("UNITS", "Dashlights"),
-      ]
-
-      checks += [
-        ("Dashlights", 10),
-        ("BodyInfo", 10),
-      ]
+        messages += CarState.get_common_global_body_messages(CP)
     else:
-      signals += [
-        ("FL", "Wheel_Speeds"),
-        ("FR", "Wheel_Speeds"),
-        ("RL", "Wheel_Speeds"),
-        ("RR", "Wheel_Speeds"),
-        ("UNITS", "Dash_State2"),
-        ("Cruise_On", "CruiseControl"),
-        ("Cruise_Activated", "CruiseControl"),
-      ]
-      checks += [
-        ("Wheel_Speeds", 50),
-        ("Dash_State2", 1),
-      ]
-
-      if CP.carFingerprint == CAR.FORESTER_PREGLOBAL:
-        checks += [
-          ("Dashlights", 20),
-          ("BodyInfo", 1),
-          ("CruiseControl", 50),
-        ]
+      messages += CarState.get_common_preglobal_body_messages()
 
-      if CP.carFingerprint in (CAR.LEGACY_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018):
-        checks += [
-          ("Dashlights", 10),
-          ("CruiseControl", 50),
-        ]
-
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.main)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.main)
 
   @staticmethod
   def get_cam_can_parser(CP):
     if CP.carFingerprint in PREGLOBAL_CARS:
-      signals = [
-        ("Cruise_Set_Speed", "ES_DashStatus"),
-        ("Not_Ready_Startup", "ES_DashStatus"),
-
-        ("Cruise_Throttle", "ES_Distance"),
-        ("Signal1", "ES_Distance"),
-        ("Car_Follow", "ES_Distance"),
-        ("Signal2", "ES_Distance"),
-        ("Brake_On", "ES_Distance"),
-        ("Distance_Swap", "ES_Distance"),
-        ("Standstill", "ES_Distance"),
-        ("Signal3", "ES_Distance"),
-        ("Close_Distance", "ES_Distance"),
-        ("Signal4", "ES_Distance"),
-        ("Standstill_2", "ES_Distance"),
-        ("Cruise_Fault", "ES_Distance"),
-        ("Signal5", "ES_Distance"),
-        ("COUNTER", "ES_Distance"),
-        ("Signal6", "ES_Distance"),
-        ("Cruise_Button", "ES_Distance"),
-        ("Signal7", "ES_Distance"),
-      ]
-
-      checks = [
+      messages = [
         ("ES_DashStatus", 20),
         ("ES_Distance", 20),
       ]
     else:
-      signals = [
-        ("COUNTER", "ES_DashStatus"),
-        ("CHECKSUM", "ES_DashStatus"),
-        ("PCB_Off", "ES_DashStatus"),
-        ("LDW_Off", "ES_DashStatus"),
-        ("Signal1", "ES_DashStatus"),
-        ("Cruise_State_Msg", "ES_DashStatus"),
-        ("LKAS_State_Msg", "ES_DashStatus"),
-        ("Signal2", "ES_DashStatus"),
-        ("Cruise_Soft_Disable", "ES_DashStatus"),
-        ("Cruise_Status_Msg", "ES_DashStatus"),
-        ("Signal3", "ES_DashStatus"),
-        ("Cruise_Distance", "ES_DashStatus"),
-        ("Signal4", "ES_DashStatus"),
-        ("Conventional_Cruise", "ES_DashStatus"),
-        ("Signal5", "ES_DashStatus"),
-        ("Cruise_Disengaged", "ES_DashStatus"),
-        ("Cruise_Activated", "ES_DashStatus"),
-        ("Signal6", "ES_DashStatus"),
-        ("Cruise_Set_Speed", "ES_DashStatus"),
-        ("Cruise_Fault", "ES_DashStatus"),
-        ("Cruise_On", "ES_DashStatus"),
-        ("Display_Own_Car", "ES_DashStatus"),
-        ("Brake_Lights", "ES_DashStatus"),
-        ("Car_Follow", "ES_DashStatus"),
-        ("Signal7", "ES_DashStatus"),
-        ("Far_Distance", "ES_DashStatus"),
-        ("Cruise_State", "ES_DashStatus"),
-
-        ("COUNTER", "ES_LKAS_State"),
-        ("CHECKSUM", "ES_LKAS_State"),
-        ("LKAS_Alert_Msg", "ES_LKAS_State"),
-        ("Signal1", "ES_LKAS_State"),
-        ("LKAS_ACTIVE", "ES_LKAS_State"),
-        ("LKAS_Dash_State", "ES_LKAS_State"),
-        ("Signal2", "ES_LKAS_State"),
-        ("Backward_Speed_Limit_Menu", "ES_LKAS_State"),
-        ("LKAS_Left_Line_Enable", "ES_LKAS_State"),
-        ("LKAS_Left_Line_Light_Blink", "ES_LKAS_State"),
-        ("LKAS_Right_Line_Enable", "ES_LKAS_State"),
-        ("LKAS_Right_Line_Light_Blink", "ES_LKAS_State"),
-        ("LKAS_Left_Line_Visible", "ES_LKAS_State"),
-        ("LKAS_Right_Line_Visible", "ES_LKAS_State"),
-        ("LKAS_Alert", "ES_LKAS_State"),
-        ("Signal3", "ES_LKAS_State"),
-      ]
-
-      checks = [
+      messages = [
         ("ES_DashStatus", 10),
         ("ES_LKAS_State", 10),
       ]
 
       if CP.carFingerprint not in GLOBAL_GEN2:
-        signals += CarState.get_common_global_es_signals()[0]
-        checks += CarState.get_common_global_es_signals()[1]
+        messages += CarState.get_common_global_es_messages(CP)
 
       if CP.flags & SubaruFlags.SEND_INFOTAINMENT:
-        signals += [
-          ("COUNTER", "ES_Infotainment"),
-          ("CHECKSUM", "ES_Infotainment"),
-          ("LKAS_State_Infotainment", "ES_Infotainment"),
-          ("LKAS_Blue_Lines", "ES_Infotainment"),
-          ("Signal1", "ES_Infotainment"),
-          ("Signal2", "ES_Infotainment"),
-        ]
-        checks.append(("ES_Infotainment", 10))
-
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.camera)
+        messages.append(("ES_Infotainment", 10))
+
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.camera)
 
   @staticmethod
   def get_body_can_parser(CP):
+    messages = []
+
     if CP.carFingerprint in GLOBAL_GEN2:
-      signals, checks = CarState.get_common_global_body_signals()
-      signals += CarState.get_common_global_es_signals()[0]
-      checks += CarState.get_common_global_es_signals()[1]
-      return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.alt)
+      messages += CarState.get_common_global_body_messages(CP)
+      messages += CarState.get_common_global_es_messages(CP)
+
+    if CP.carFingerprint in HYBRID_CARS:
+      messages += [
+        ("Throttle_Hybrid", 40),
+        ("Transmission", 100)
+      ]
+
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.alt)
 
-    return None
diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py
index f7698dbe7c..79576f6433 100644
--- a/selfdrive/car/subaru/interface.py
+++ b/selfdrive/car/subaru/interface.py
@@ -1,9 +1,9 @@
-#!/usr/bin/env python3
 from cereal import car
 from panda import Panda
-from selfdrive.car import STD_CARGO_KG, get_safety_config
-from selfdrive.car.interfaces import CarInterfaceBase
-from selfdrive.car.subaru.values import CAR, GLOBAL_GEN2, PREGLOBAL_CARS, SubaruFlags
+from openpilot.selfdrive.car import get_safety_config
+from openpilot.selfdrive.car.disable_ecu import disable_ecu
+from openpilot.selfdrive.car.interfaces import CarInterfaceBase
+from openpilot.selfdrive.car.subaru.values import CAR, GLOBAL_ES_ADDR, LKAS_ANGLE, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, SubaruFlags
 
 
 class CarInterface(CarInterfaceBase):
@@ -12,7 +12,11 @@ class CarInterface(CarInterfaceBase):
   def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
     ret.carName = "subaru"
     ret.radarUnavailable = True
-    ret.dashcamOnly = candidate in PREGLOBAL_CARS
+    # for HYBRID CARS to be upstreamed, we need:
+    # - replacement for ES_Distance so we can cancel the cruise control
+    # - to find the Cruise_Activated bit from the car
+    # - proper panda safety setup (use the correct cruise_activated bit, throttle from Throttle_Hybrid, etc)
+    ret.dashcamOnly = candidate in (PREGLOBAL_CARS | LKAS_ANGLE | HYBRID_CARS)
     ret.autoResumeSng = False
 
     # Detect infotainment message sent from the camera
@@ -30,10 +34,14 @@ class CarInterface(CarInterfaceBase):
 
     ret.steerLimitTimer = 0.4
     ret.steerActuatorDelay = 0.1
-    CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
 
-    if candidate == CAR.ASCENT:
-      ret.mass = 2031. + STD_CARGO_KG
+    if candidate in LKAS_ANGLE:
+      ret.steerControlType = car.CarParams.SteerControlType.angle
+    else:
+      CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
+
+    if candidate in (CAR.ASCENT, CAR.ASCENT_2023):
+      ret.mass = 2031.
       ret.wheelbase = 2.89
       ret.centerToFront = ret.wheelbase * 0.5
       ret.steerRatio = 13.5
@@ -44,7 +52,7 @@ class CarInterface(CarInterfaceBase):
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.0025, 0.1], [0.00025, 0.01]]
 
     elif candidate == CAR.IMPREZA:
-      ret.mass = 1568. + STD_CARGO_KG
+      ret.mass = 1568.
       ret.wheelbase = 2.67
       ret.centerToFront = ret.wheelbase * 0.5
       ret.steerRatio = 15
@@ -55,7 +63,7 @@ class CarInterface(CarInterfaceBase):
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.3], [0.02, 0.03]]
 
     elif candidate == CAR.IMPREZA_2020:
-      ret.mass = 1480. + STD_CARGO_KG
+      ret.mass = 1480.
       ret.wheelbase = 2.67
       ret.centerToFront = ret.wheelbase * 0.5
       ret.steerRatio = 17           # learned, 14 stock
@@ -64,8 +72,15 @@ class CarInterface(CarInterfaceBase):
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]]
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.045, 0.042, 0.20], [0.04, 0.035, 0.045]]
 
-    elif candidate == CAR.FORESTER:
-      ret.mass = 1568. + STD_CARGO_KG
+    elif candidate == CAR.CROSSTREK_HYBRID:
+      ret.mass = 1668.
+      ret.wheelbase = 2.67
+      ret.centerToFront = ret.wheelbase * 0.5
+      ret.steerRatio = 17
+      ret.steerActuatorDelay = 0.1
+
+    elif candidate in (CAR.FORESTER, CAR.FORESTER_2022, CAR.FORESTER_HYBRID):
+      ret.mass = 1568.
       ret.wheelbase = 2.67
       ret.centerToFront = ret.wheelbase * 0.5
       ret.steerRatio = 17           # learned, 14 stock
@@ -74,37 +89,50 @@ class CarInterface(CarInterfaceBase):
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]]
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]]
 
-    elif candidate in (CAR.OUTBACK, CAR.LEGACY):
-      ret.mass = 1568. + STD_CARGO_KG
+    elif candidate in (CAR.OUTBACK, CAR.LEGACY, CAR.OUTBACK_2023):
+      ret.mass = 1568.
       ret.wheelbase = 2.67
       ret.centerToFront = ret.wheelbase * 0.5
       ret.steerRatio = 17
       ret.steerActuatorDelay = 0.1
-      CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
 
     elif candidate in (CAR.FORESTER_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018):
-      ret.safetyConfigs[0].safetyParam = 1  # Outback 2018-2019 and Forester have reversed driver torque signal
-      ret.mass = 1568 + STD_CARGO_KG
+      ret.safetyConfigs[0].safetyParam = Panda.FLAG_SUBARU_PREGLOBAL_REVERSED_DRIVER_TORQUE  # Outback 2018-2019 and Forester have reversed driver torque signal
+      ret.mass = 1568
       ret.wheelbase = 2.67
       ret.centerToFront = ret.wheelbase * 0.5
       ret.steerRatio = 20           # learned, 14 stock
 
     elif candidate == CAR.LEGACY_PREGLOBAL:
-      ret.mass = 1568 + STD_CARGO_KG
+      ret.mass = 1568
       ret.wheelbase = 2.67
       ret.centerToFront = ret.wheelbase * 0.5
       ret.steerRatio = 12.5   # 14.5 stock
       ret.steerActuatorDelay = 0.15
 
     elif candidate == CAR.OUTBACK_PREGLOBAL:
-      ret.mass = 1568 + STD_CARGO_KG
+      ret.mass = 1568
       ret.wheelbase = 2.67
       ret.centerToFront = ret.wheelbase * 0.5
       ret.steerRatio = 20           # learned, 14 stock
-
     else:
       raise ValueError(f"unknown car: {candidate}")
 
+    #ret.experimentalLongitudinalAvailable = candidate not in (GLOBAL_GEN2 | PREGLOBAL_CARS | LKAS_ANGLE | HYBRID_CARS)
+    ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
+
+    if candidate in GLOBAL_GEN2 and ret.openpilotLongitudinalControl:
+      ret.flags |= SubaruFlags.DISABLE_EYESIGHT
+
+    if ret.openpilotLongitudinalControl:
+      ret.longitudinalTuning.kpBP = [0., 5., 35.]
+      ret.longitudinalTuning.kpV = [0.8, 1.0, 1.5]
+      ret.longitudinalTuning.kiBP = [0., 35.]
+      ret.longitudinalTuning.kiV = [0.54, 0.36]
+
+      ret.stoppingControl = True
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_LONG
+
     return ret
 
   # returns a car.CarState
@@ -116,5 +144,10 @@ class CarInterface(CarInterfaceBase):
 
     return ret
 
+  @staticmethod
+  def init(CP, logcan, sendcan):
+    if CP.flags & SubaruFlags.DISABLE_EYESIGHT:
+      disable_ecu(logcan, sendcan, bus=2, addr=GLOBAL_ES_ADDR, com_cont_req=b'\x28\x03\x01')
+
   def apply(self, c, now_nanos):
     return self.CC.update(c, self.CS, now_nanos)
diff --git a/selfdrive/car/subaru/radar_interface.py b/selfdrive/car/subaru/radar_interface.py
index b2f7651136..e654bd61fd 100644
--- a/selfdrive/car/subaru/radar_interface.py
+++ b/selfdrive/car/subaru/radar_interface.py
@@ -1,5 +1,4 @@
-#!/usr/bin/env python3
-from selfdrive.car.interfaces import RadarInterfaceBase
+from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
 
 class RadarInterface(RadarInterfaceBase):
   pass
diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py
index 0c32a150d8..7d1939a497 100644
--- a/selfdrive/car/subaru/subarucan.py
+++ b/selfdrive/car/subaru/subarucan.py
@@ -1,5 +1,5 @@
 from cereal import car
-from selfdrive.car.subaru.values import CanBus
+from openpilot.selfdrive.car.subaru.values import CanBus
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
@@ -16,17 +16,15 @@ def create_steering_control(packer, apply_steer, steer_req):
 def create_steering_status(packer):
   return packer.make_can_msg("ES_LKAS_State", 0, {})
 
-
-def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd):
+def create_es_distance(packer, frame, es_distance_msg, bus, pcm_cancel_cmd, long_enabled = False, brake_cmd = False, cruise_throttle = 0):
   values = {s: es_distance_msg[s] for s in [
     "CHECKSUM",
-    "COUNTER",
     "Signal1",
     "Cruise_Fault",
     "Cruise_Throttle",
     "Signal2",
     "Car_Follow",
-    "Signal3",
+    "Low_Speed_Follow",
     "Cruise_Soft_Disable",
     "Signal7",
     "Cruise_Brake_Active",
@@ -40,16 +38,28 @@ def create_es_distance(packer, es_distance_msg, bus, pcm_cancel_cmd):
     "Cruise_Resume",
     "Signal6",
   ]}
-  values["COUNTER"] = (values["COUNTER"] + 1) % 0x10
+
+  values["COUNTER"] = frame % 0x10
+
+  if long_enabled:
+    values["Cruise_Throttle"] = cruise_throttle
+
+    # Do not disable openpilot on Eyesight Soft Disable, if openpilot is controlling long
+    values["Cruise_Soft_Disable"] = 0
+
+    if brake_cmd:
+      values["Cruise_Brake_Active"] = 1
+
   if pcm_cancel_cmd:
     values["Cruise_Cancel"] = 1
+    values["Cruise_Throttle"] = 1818 # inactive throttle
+
   return packer.make_can_msg("ES_Distance", bus, values)
 
 
-def create_es_lkas_state(packer, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
+def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
   values = {s: es_lkas_state_msg[s] for s in [
     "CHECKSUM",
-    "COUNTER",
     "LKAS_Alert_Msg",
     "Signal1",
     "LKAS_ACTIVE",
@@ -66,6 +76,8 @@ def create_es_lkas_state(packer, es_lkas_state_msg, enabled, visual_alert, left_
     "Signal3",
   ]}
 
+  values["COUNTER"] = frame % 0x10
+
   # Filter the stock LKAS "Keep hands on wheel" alert
   if values["LKAS_Alert_Msg"] == 1:
     values["LKAS_Alert_Msg"] = 0
@@ -97,19 +109,20 @@ def create_es_lkas_state(packer, es_lkas_state_msg, enabled, visual_alert, left_
     elif right_lane_depart:
       values["LKAS_Alert"] = 11  # Right lane departure dash alert
 
-  values["LKAS_ACTIVE"] = 1  # Show LKAS lane lines
-  values["LKAS_Dash_State"] = 2 if enabled else 0  # Green enabled indicator
+  if enabled:
+    values["LKAS_ACTIVE"] = 1  # Show LKAS lane lines
+    values["LKAS_Dash_State"] = 2  # Green enabled indicator
+  else:
+    values["LKAS_Dash_State"] = 0  # LKAS Not enabled
 
   values["LKAS_Left_Line_Visible"] = int(left_line)
   values["LKAS_Right_Line_Visible"] = int(right_line)
 
   return packer.make_can_msg("ES_LKAS_State", CanBus.main, values)
 
-
-def create_es_dashstatus(packer, dashstatus_msg):
+def create_es_dashstatus(packer, frame, dashstatus_msg, enabled, long_enabled, long_active, lead_visible):
   values = {s: dashstatus_msg[s] for s in [
     "CHECKSUM",
-    "COUNTER",
     "PCB_Off",
     "LDW_Off",
     "Signal1",
@@ -137,23 +150,85 @@ def create_es_dashstatus(packer, dashstatus_msg):
     "Cruise_State",
   ]}
 
+  values["COUNTER"] = frame % 0x10
+
+  if enabled and long_active:
+    values["Cruise_State"] = 0
+    values["Cruise_Activated"] = 1
+    values["Cruise_Disengaged"] = 0
+    values["Car_Follow"] = int(lead_visible)
+
+  if long_enabled:
+    values["PCB_Off"] = 1 # AEB is not presevered, so show the PCB_Off on dash
+
   # Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts
   if values["LKAS_State_Msg"] in (2, 3):
     values["LKAS_State_Msg"] = 0
 
   return packer.make_can_msg("ES_DashStatus", CanBus.main, values)
 
+def create_es_brake(packer, frame, es_brake_msg, enabled, brake_value):
+  values = {s: es_brake_msg[s] for s in [
+    "CHECKSUM",
+    "Signal1",
+    "Brake_Pressure",
+    "AEB_Status",
+    "Cruise_Brake_Lights",
+    "Cruise_Brake_Fault",
+    "Cruise_Brake_Active",
+    "Cruise_Activated",
+    "Signal3",
+  ]}
+
+  values["COUNTER"] = frame % 0x10
+
+  if enabled:
+    values["Cruise_Activated"] = 1
+
+  values["Brake_Pressure"] = brake_value
+
+  if brake_value > 0:
+    values["Cruise_Brake_Active"] = 1
+    values["Cruise_Brake_Lights"] = 1 if brake_value >= 70 else 0
+
+  return packer.make_can_msg("ES_Brake", CanBus.main, values)
+
+def create_es_status(packer, frame, es_status_msg, long_enabled, long_active, cruise_rpm):
+  values = {s: es_status_msg[s] for s in [
+    "CHECKSUM",
+    "Signal1",
+    "Cruise_Fault",
+    "Cruise_RPM",
+    "Signal2",
+    "Cruise_Activated",
+    "Brake_Lights",
+    "Cruise_Hold",
+    "Signal3",
+  ]}
+
+  values["COUNTER"] = frame % 0x10
+
+  if long_enabled:
+    values["Cruise_RPM"] = cruise_rpm
 
-def create_es_infotainment(packer, es_infotainment_msg, visual_alert):
+    if long_active:
+      values["Cruise_Activated"] = 1
+
+  return packer.make_can_msg("ES_Status", CanBus.main, values)
+
+
+def create_es_infotainment(packer, frame, es_infotainment_msg, visual_alert):
   # Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts
   values = {s: es_infotainment_msg[s] for s in [
     "CHECKSUM",
-    "COUNTER",
     "LKAS_State_Infotainment",
     "LKAS_Blue_Lines",
     "Signal1",
     "Signal2",
   ]}
+
+  values["COUNTER"] = frame % 0x10
+
   if values["LKAS_State_Infotainment"] in (3, 4):
     values["LKAS_State_Infotainment"] = 0
 
@@ -168,15 +243,40 @@ def create_es_infotainment(packer, es_infotainment_msg, visual_alert):
   return packer.make_can_msg("ES_Infotainment", CanBus.main, values)
 
 
+def create_es_highbeamassist(packer):
+  values = {
+    "HBA_Available": False,
+  }
+
+  return packer.make_can_msg("ES_HighBeamAssist", CanBus.main, values)
+
+
+def create_es_static_1(packer):
+  values = {
+    "SET_3": 3,
+  }
+
+  return packer.make_can_msg("ES_STATIC_1", CanBus.main, values)
+
+
+def create_es_static_2(packer):
+  values = {
+    "SET_3": 3,
+  }
+
+  return packer.make_can_msg("ES_STATIC_2", CanBus.main, values)
+
+
 # *** Subaru Pre-global ***
 
-def subaru_preglobal_checksum(packer, values, addr):
+def subaru_preglobal_checksum(packer, values, addr, checksum_byte=7):
   dat = packer.make_can_msg(addr, 0, values)[2]
-  return (sum(dat[:7])) % 256
+  return (sum(dat[:checksum_byte]) + sum(dat[checksum_byte+1:])) % 256
 
 
-def create_preglobal_steering_control(packer, apply_steer, steer_req):
+def create_preglobal_steering_control(packer, frame, apply_steer, steer_req):
   values = {
+    "COUNTER": frame % 0x08,
     "LKAS_Command": apply_steer,
     "LKAS_Active": steer_req,
   }
@@ -191,7 +291,7 @@ def create_preglobal_es_distance(packer, cruise_button, es_distance_msg):
     "Signal1",
     "Car_Follow",
     "Signal2",
-    "Brake_On",
+    "Cruise_Brake_Active",
     "Distance_Swap",
     "Standstill",
     "Signal3",
diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py
index 0e3f2e8d05..882d46b7c3 100644
--- a/selfdrive/car/subaru/values.py
+++ b/selfdrive/car/subaru/values.py
@@ -1,12 +1,12 @@
 from dataclasses import dataclass, field
-from enum import Enum, IntFlag
+from enum import Enum, IntFlag, StrEnum
 from typing import Dict, List, Union
 
 from cereal import car
 from panda.python import uds
-from selfdrive.car import dbc_dict
-from selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column
-from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
+from openpilot.selfdrive.car import dbc_dict
+from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Tool, Column
+from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
 
 Ecu = car.CarParams.Ecu
 
@@ -29,9 +29,37 @@ class CarControllerParams:
     else:
       self.STEER_MAX = 2047
 
+  THROTTLE_MIN = 808
+  THROTTLE_MAX = 3400
+
+  THROTTLE_INACTIVE     = 1818   # corresponds to zero acceleration
+  THROTTLE_ENGINE_BRAKE = 808    # while braking, eyesight sets throttle to this, probably for engine braking
+
+  BRAKE_MIN = 0
+  BRAKE_MAX = 600                # about -3.5m/s2 from testing
+
+  RPM_MIN = 0
+  RPM_MAX = 2400
+
+  RPM_INACTIVE = 600             # a good base rpm for zero acceleration
+
+  THROTTLE_LOOKUP_BP = [0, 2]
+  THROTTLE_LOOKUP_V = [THROTTLE_INACTIVE, THROTTLE_MAX]
+
+  RPM_LOOKUP_BP = [0, 2]
+  RPM_LOOKUP_V = [RPM_INACTIVE, RPM_MAX]
+
+  BRAKE_LOOKUP_BP = [-3.5, 0]
+  BRAKE_LOOKUP_V = [BRAKE_MAX, BRAKE_MIN]
+
 
 class SubaruFlags(IntFlag):
   SEND_INFOTAINMENT = 1
+  DISABLE_EYESIGHT = 2
+
+
+GLOBAL_ES_ADDR = 0x787
+GEN2_ES_BUTTONS_DID = b'\x11\x30'
 
 
 class CanBus:
@@ -40,14 +68,19 @@ class CanBus:
   camera = 2
 
 
-class CAR:
+class CAR(StrEnum):
   # Global platform
   ASCENT = "SUBARU ASCENT LIMITED 2019"
+  ASCENT_2023 = "SUBARU ASCENT 2023"
   IMPREZA = "SUBARU IMPREZA LIMITED 2019"
   IMPREZA_2020 = "SUBARU IMPREZA SPORT 2020"
   FORESTER = "SUBARU FORESTER 2019"
   OUTBACK = "SUBARU OUTBACK 6TH GEN"
+  CROSSTREK_HYBRID = "SUBARU CROSSTREK HYBRID 2020"
+  FORESTER_HYBRID = "SUBARU FORESTER HYBRID 2020"
   LEGACY = "SUBARU LEGACY 7TH GEN"
+  FORESTER_2022 = "SUBARU FORESTER 2022"
+  OUTBACK_2023 = "SUBARU OUTBACK 7TH GEN"
 
   # Pre-global
   FORESTER_PREGLOBAL = "SUBARU FORESTER 2017 - 2018"
@@ -68,6 +101,8 @@ class SubaruCarInfo(CarInfo):
   car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.subaru_a]))
   footnotes: List[Enum] = field(default_factory=lambda: [Footnote.GLOBAL])
 
+  def init_make(self, CP: car.CarParams):
+    self.car_parts.parts.extend([Tool.socket_8mm_deep, Tool.pry_tool])
 
 CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = {
   CAR.ASCENT: SubaruCarInfo("Subaru Ascent 2019-21", "All"),
@@ -83,11 +118,17 @@ CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = {
     SubaruCarInfo("Subaru Crosstrek 2020-23"),
     SubaruCarInfo("Subaru XV 2020-21"),
   ],
+  # TODO: is there an XV and Impreza too?
+  CAR.CROSSTREK_HYBRID: SubaruCarInfo("Subaru Crosstrek Hybrid 2020", car_parts=CarParts.common([CarHarness.subaru_b])),
+  CAR.FORESTER_HYBRID: SubaruCarInfo("Subaru Forester Hybrid 2020"),
   CAR.FORESTER: SubaruCarInfo("Subaru Forester 2019-21", "All"),
   CAR.FORESTER_PREGLOBAL: SubaruCarInfo("Subaru Forester 2017-18"),
   CAR.LEGACY_PREGLOBAL: SubaruCarInfo("Subaru Legacy 2015-18"),
   CAR.OUTBACK_PREGLOBAL: SubaruCarInfo("Subaru Outback 2015-17"),
   CAR.OUTBACK_PREGLOBAL_2018: SubaruCarInfo("Subaru Outback 2018-19"),
+  CAR.FORESTER_2022: SubaruCarInfo("Subaru Forester 2022", "All", car_parts=CarParts.common([CarHarness.subaru_c])),
+  CAR.OUTBACK_2023: SubaruCarInfo("Subaru Outback 2023", "All", car_parts=CarParts.common([CarHarness.subaru_d])),
+  CAR.ASCENT_2023: SubaruCarInfo("Subaru Ascent 2023", "All", car_parts=CarParts.common([CarHarness.subaru_d])),
 }
 
 SUBARU_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
@@ -101,6 +142,12 @@ FW_QUERY_CONFIG = FwQueryConfig(
       [StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST],
       [StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE],
     ),
+    # Some Eyesight modules fail on TESTER_PRESENT_REQUEST
+    # TODO: check if this resolves the fingerprinting issue for the 2023 Ascent and other new Subaru cars
+    Request(
+      [SUBARU_VERSION_REQUEST],
+      [SUBARU_VERSION_RESPONSE],
+    ),
   ],
 )
 
@@ -138,11 +185,29 @@ FW_VERSIONS = {
       b'\x01\xfe\xfa\x00\x00',
     ],
   },
+  CAR.ASCENT_2023: {
+    (Ecu.abs, 0x7b0, None): [
+      b'\xa5 #\x03\x00',
+    ],
+    (Ecu.eps, 0x746, None): [
+      b'%\xc0\xd0\x11',
+    ],
+    (Ecu.fwdCamera, 0x787, None): [
+      b'\x05!\x08\x1dK\x05!\x08\x01/',
+    ],
+    (Ecu.engine, 0x7a2, None): [
+      b'\xe5,\xa0P\x07',
+    ],
+    (Ecu.transmission, 0x7a3, None): [
+      b'\x04\xfe\xf3\x00\x00',
+    ],
+  },
   CAR.LEGACY: {
     (Ecu.abs, 0x7b0, None): [
       b'\xa1\\  x04\x01',
       b'\xa1  \x03\x03',
       b'\xa1  \x02\x01',
+      b'\xa1  \x02\x02',
     ],
     (Ecu.eps, 0x746, None): [
       b'\x9b\xc0\x11\x00',
@@ -156,11 +221,13 @@ FW_VERSIONS = {
       b'\xde\"a0\x07',
       b'\xe2"aq\x07',
       b'\xde,\xa0@\x07',
+      b'\xe2,\xa0@\x07',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xa5\xf6\x05@\x00',
       b'\xa7\xf6\x04@\x00',
       b'\xa5\xfe\xc7@\x00',
+      b'\xa7\xfe\xc4@\x00',
     ],
   },
   CAR.IMPREZA: {
@@ -251,6 +318,7 @@ FW_VERSIONS = {
       b'\xa2 !`\000',
       b'\xf1\x00\xb2\x06\x04',
       b'\xa2  `\x00',
+      b'\xa2 !3\x00',
     ],
     (Ecu.eps, 0x746, None): [
       b'\x9a\xc0\000\000',
@@ -264,6 +332,8 @@ FW_VERSIONS = {
       b'\x00\x00eq\x1f@ "',
       b'\x00\x00eq\x00\x00\x00\x00',
       b'\x00\x00e\x8f\x00\x00\x00\x00',
+      b'\x00\x00e\x92\x00\x00\x00\x00',
+      b'\x00\x00e\xa4\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\xca!ap\a',
@@ -271,21 +341,46 @@ FW_VERSIONS = {
       b'\xca!`0\a',
       b'\xcc\"f0\a',
       b'\xcc!fp\a',
+      b'\xcc!`p\x07',
       b'\xca!f@\x07',
       b'\xca!fp\x07',
       b'\xf3"f@\x07',
       b'\xe6!fp\x07',
       b'\xf3"fp\x07',
+      b'\xe6"f0\x07',
+      b'\xe6"fp\x07',
+      b'\xe6!`@\x07',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xe6\xf5\004\000\000',
       b'\xe6\xf5$\000\000',
+      b'\xe7\xf5\x04\x00\x00',
       b'\xe7\xf6B0\000',
       b'\xe7\xf5D0\000',
       b'\xf1\x00\xd7\x10@',
       b'\xe6\xf5D0\x00',
       b'\xe9\xf6F0\x00',
       b'\xe9\xf5B0\x00',
+      b'\xe9\xf6B0\x00',
+      b'\xe9\xf5"\x00\x00',
+    ],
+  },
+  CAR.CROSSTREK_HYBRID: {
+    (Ecu.abs, 0x7b0, None): [
+      b'\xa2 \x19e\x01',
+      b'\xa2 !e\x01',
+    ],
+    (Ecu.eps, 0x746, None): [
+      b'\x9a\xc2\x01\x00',
+      b'\n\xc2\x01\x00',
+    ],
+    (Ecu.fwdCamera, 0x787, None): [
+      b'\x00\x00el\x1f@ #',
+    ],
+    (Ecu.engine, 0x7e0, None): [
+      b'\xd7!`@\x07',
+      b'\xd7!`p\a',
+      b'\xf4!`0\x07',
     ],
   },
   CAR.FORESTER: {
@@ -331,10 +426,28 @@ FW_VERSIONS = {
       b'\x1a\xe6F1\x00',
     ],
   },
+  CAR.FORESTER_HYBRID: {
+    (Ecu.abs, 0x7b0, None): [
+      b'\xa3 \x19T\x00',
+    ],
+    (Ecu.eps, 0x746, None): [
+      b'\x8d\xc2\x00\x00',
+    ],
+    (Ecu.fwdCamera, 0x787, None): [
+      b'\x00\x00eY\x1f@ !',
+    ],
+    (Ecu.engine, 0x7e0, None): [
+      b'\xd2\xa1`r\x07',
+    ],
+    (Ecu.transmission, 0x7e1, None): [
+       b'\x1b\xa7@a\x00',
+    ],
+  },
   CAR.FORESTER_PREGLOBAL: {
     (Ecu.abs, 0x7b0, None): [
       b'\x7d\x97\x14\x40',
       b'\xf1\x00\xbb\x0c\x04',
+      b'm\x97\x14@',
     ],
     (Ecu.eps, 0x746, None): [
       b'}\xc0\x10\x00',
@@ -343,13 +456,15 @@ FW_VERSIONS = {
     (Ecu.fwdCamera, 0x787, None): [
       b'\x00\x00\x64\x35\x1f\x40\x20\x09',
       b'\x00\x00c\xe9\x1f@ \x03',
-      b'\x00\x00d\xd3\x1f@ \t'
+      b'\x00\x00d\xd3\x1f@ \t',
+      b'\x00\x00c\xe9\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\xba"@p\a',
       b'\xa7)\xa0q\a',
       b'\xf1\x82\xa7)\xa0q\a',
       b'\xba"@@\a',
+      b'\xa7"@p\x07',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xdc\xf2\x60\x60\x00',
@@ -358,6 +473,7 @@ FW_VERSIONS = {
       b'\xdc\xf2`\x81\000',
       b'\xdc\xf2`\x80\x00',
       b'\x1a\xf6F`\x00',
+      b'\xda\xf2`\x80\x00',
     ],
   },
   CAR.LEGACY_PREGLOBAL: {
@@ -366,6 +482,7 @@ FW_VERSIONS = {
       b'[\xba\xc4\x03',
       b'{\x97D\x00',
       b'[\x97D\000',
+      b'k\x9aD\x00',
     ],
     (Ecu.eps, 0x746, None): [
       b'[\xb0\x00\x01',
@@ -382,12 +499,14 @@ FW_VERSIONS = {
       b'\xa0+@p\x07',
       b'\xb4"@0\x07',
       b'\xa0"@q\a',
+      b'\xab+@p\x07',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xbe\xf2\x00p\x00',
       b'\xbf\xfb\xc0\x80\x00',
       b'\xbd\xf2\x00`\x00',
       b'\xbf\xf2\000\x80\000',
+      b'\xbe\xfb\xc0p\x00',
     ],
   },
   CAR.OUTBACK_PREGLOBAL: {
@@ -431,6 +550,7 @@ FW_VERSIONS = {
       b'\xb4"@r\a',
       b'\xa0+@@\x07',
       b'\xa0\"@\x80\a',
+      b'\xa0*@u\x07',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xbd\xfb\xe0\x80\x00',
@@ -524,6 +644,7 @@ FW_VERSIONS = {
       b'\xbc"`q\x07',
       b'\xe3,\xa0@\x07',
       b'\xbc,\xa0u\x07',
+      b'\xde,\xa0@\x07',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xa5\xfe\xf7@\x00',
@@ -536,14 +657,69 @@ FW_VERSIONS = {
       b'\xa5\xfe\xf8@\x00',
     ],
   },
+  CAR.FORESTER_2022: {
+    (Ecu.abs, 0x7b0, None): [
+      b'\xa3 !x\x00',
+      b'\xa3 !v\x00',
+      b'\xa3 "v\x00',
+      b'\xa3 "x\x00',
+    ],
+    (Ecu.eps, 0x746, None): [
+      b'-\xc0%0',
+      b'-\xc0\x040',
+      b'=\xc0%\x02',
+      b'=\xc04\x02',
+    ],
+    (Ecu.fwdCamera, 0x787, None): [
+      b'\x04!\x01\x1eD\x07!\x00\x04,'
+    ],
+    (Ecu.engine, 0x7e0, None): [
+      b'\xd5"a0\x07',
+      b'\xd5"`0\x07',
+      b'\xf1"aq\x07',
+      b'\xf1"`q\x07',
+    ],
+    (Ecu.transmission, 0x7e1, None): [
+      b'\x1d\x86B0\x00',
+      b'\x1d\xf6B0\x00',
+      b'\x1e\x86B0\x00',
+      b'\x1e\xf6D0\x00',
+    ],
+  },
+  CAR.OUTBACK_2023: {
+    (Ecu.abs, 0x7b0, None): [
+      b'\xa1 #\x14\x00',
+      b'\xa1 #\x17\x00',
+    ],
+    (Ecu.eps, 0x746, None): [
+      b'+\xc0\x10\x11\x00',
+      b'+\xc0\x12\x11\x00',
+    ],
+    (Ecu.fwdCamera, 0x787, None): [
+      b'\t!\x08\x046\x05!\x08\x01/',
+    ],
+    (Ecu.engine, 0x7a2, None): [
+      b'\xed,\xa0q\x07',
+      b'\xed,\xa2q\x07',
+    ],
+    (Ecu.transmission, 0x7a3, None): [
+      b'\xa8\x8e\xf41\x00',
+      b'\xa8\xfe\xf41\x00',
+    ]
+  }
 }
 
 DBC = {
   CAR.ASCENT: dbc_dict('subaru_global_2017_generated', None),
+  CAR.ASCENT_2023: dbc_dict('subaru_global_2017_generated', None),
   CAR.IMPREZA: dbc_dict('subaru_global_2017_generated', None),
   CAR.IMPREZA_2020: dbc_dict('subaru_global_2017_generated', None),
   CAR.FORESTER: dbc_dict('subaru_global_2017_generated', None),
+  CAR.FORESTER_2022: dbc_dict('subaru_global_2017_generated', None),
   CAR.OUTBACK: dbc_dict('subaru_global_2017_generated', None),
+  CAR.FORESTER_HYBRID: dbc_dict('subaru_global_2020_hybrid_generated', None),
+  CAR.CROSSTREK_HYBRID: dbc_dict('subaru_global_2020_hybrid_generated', None),
+  CAR.OUTBACK_2023: dbc_dict('subaru_global_2017_generated', None),
   CAR.LEGACY: dbc_dict('subaru_global_2017_generated', None),
   CAR.FORESTER_PREGLOBAL: dbc_dict('subaru_forester_2017_generated', None),
   CAR.LEGACY_PREGLOBAL: dbc_dict('subaru_outback_2015_generated', None),
@@ -551,5 +727,11 @@ DBC = {
   CAR.OUTBACK_PREGLOBAL_2018: dbc_dict('subaru_outback_2019_generated', None),
 }
 
-GLOBAL_GEN2 = (CAR.OUTBACK, CAR.LEGACY)
-PREGLOBAL_CARS = (CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018)
+LKAS_ANGLE = {CAR.FORESTER_2022, CAR.OUTBACK_2023, CAR.ASCENT_2023}
+GLOBAL_GEN2 = {CAR.OUTBACK, CAR.LEGACY, CAR.OUTBACK_2023, CAR.ASCENT_2023}
+PREGLOBAL_CARS = {CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018}
+HYBRID_CARS = {CAR.CROSSTREK_HYBRID, CAR.FORESTER_HYBRID}
+
+# Cars that temporarily fault when steering angle rate is greater than some threshold.
+# Appears to be all torque-based cars produced around 2019 - present
+STEER_RATE_LIMITED = GLOBAL_GEN2 | {CAR.IMPREZA_2020, CAR.FORESTER}
diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py
index aeaaba88e7..95a248a614 100644
--- a/selfdrive/car/tesla/carcontroller.py
+++ b/selfdrive/car/tesla/carcontroller.py
@@ -1,8 +1,8 @@
-from common.numpy_fast import clip
+from openpilot.common.numpy_fast import clip
 from opendbc.can.packer import CANPacker
-from selfdrive.car import apply_std_steer_angle_limits
-from selfdrive.car.tesla.teslacan import TeslaCAN
-from selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams
+from openpilot.selfdrive.car import apply_std_steer_angle_limits
+from openpilot.selfdrive.car.tesla.teslacan import TeslaCAN
+from openpilot.selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams
 
 
 class CarController:
diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py
index 0f373842f2..2cb4f09d79 100644
--- a/selfdrive/car/tesla/carstate.py
+++ b/selfdrive/car/tesla/carstate.py
@@ -1,9 +1,9 @@
 import copy
 from collections import deque
 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 openpilot.common.conversions import Conversions as CV
+from openpilot.selfdrive.car.tesla.values import DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS
+from openpilot.selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
 
@@ -78,7 +78,7 @@ class CarState(CarStateBase):
     ret.buttonEvents = buttonEvents
 
     # Doors
-    ret.doorOpen = any([(self.can_define.dv["GTW_carState"][door].get(int(cp.vl["GTW_carState"][door]), "OPEN") == "OPEN") for door in DOORS])
+    ret.doorOpen = any((self.can_define.dv["GTW_carState"][door].get(int(cp.vl["GTW_carState"][door]), "OPEN") == "OPEN") for door in DOORS)
 
     # Blinkers
     ret.leftBlinker = (cp.vl["GTW_carState"]["BC_indicatorLStatus"] == 1)
@@ -101,68 +101,7 @@ class CarState(CarStateBase):
 
   @staticmethod
   def get_can_parser(CP):
-    signals = [
-      # sig_name, sig_address
-      ("ESP_vehicleSpeed", "ESP_B"),
-      ("DI_pedalPos", "DI_torque1"),
-      ("DI_brakePedal", "DI_torque2"),
-      ("StW_AnglHP", "STW_ANGLHP_STAT"),
-      ("StW_AnglHP_Spd", "STW_ANGLHP_STAT"),
-      ("EPAS_handsOnLevel", "EPAS_sysStatus"),
-      ("EPAS_torsionBarTorque", "EPAS_sysStatus"),
-      ("EPAS_internalSAS", "EPAS_sysStatus"),
-      ("EPAS_eacStatus", "EPAS_sysStatus"),
-      ("EPAS_eacErrorCode", "EPAS_sysStatus"),
-      ("DI_cruiseState", "DI_state"),
-      ("DI_digitalSpeed", "DI_state"),
-      ("DI_speedUnits", "DI_state"),
-      ("DI_gear", "DI_torque2"),
-      ("DOOR_STATE_FL", "GTW_carState"),
-      ("DOOR_STATE_FR", "GTW_carState"),
-      ("DOOR_STATE_RL", "GTW_carState"),
-      ("DOOR_STATE_RR", "GTW_carState"),
-      ("DOOR_STATE_FrontTrunk", "GTW_carState"),
-      ("BOOT_STATE", "GTW_carState"),
-      ("BC_indicatorLStatus", "GTW_carState"),
-      ("BC_indicatorRStatus", "GTW_carState"),
-      ("SDM_bcklDrivStatus", "SDM1"),
-      ("driverBrakeStatus", "BrakeMessage"),
-
-      # We copy this whole message when spamming cancel
-      ("SpdCtrlLvr_Stat", "STW_ACTN_RQ"),
-      ("VSL_Enbl_Rq", "STW_ACTN_RQ"),
-      ("SpdCtrlLvrStat_Inv", "STW_ACTN_RQ"),
-      ("DTR_Dist_Rq", "STW_ACTN_RQ"),
-      ("TurnIndLvr_Stat", "STW_ACTN_RQ"),
-      ("HiBmLvr_Stat", "STW_ACTN_RQ"),
-      ("WprWashSw_Psd", "STW_ACTN_RQ"),
-      ("WprWash_R_Sw_Posn_V2", "STW_ACTN_RQ"),
-      ("StW_Lvr_Stat", "STW_ACTN_RQ"),
-      ("StW_Cond_Flt", "STW_ACTN_RQ"),
-      ("StW_Cond_Psd", "STW_ACTN_RQ"),
-      ("HrnSw_Psd", "STW_ACTN_RQ"),
-      ("StW_Sw00_Psd", "STW_ACTN_RQ"),
-      ("StW_Sw01_Psd", "STW_ACTN_RQ"),
-      ("StW_Sw02_Psd", "STW_ACTN_RQ"),
-      ("StW_Sw03_Psd", "STW_ACTN_RQ"),
-      ("StW_Sw04_Psd", "STW_ACTN_RQ"),
-      ("StW_Sw05_Psd", "STW_ACTN_RQ"),
-      ("StW_Sw06_Psd", "STW_ACTN_RQ"),
-      ("StW_Sw07_Psd", "STW_ACTN_RQ"),
-      ("StW_Sw08_Psd", "STW_ACTN_RQ"),
-      ("StW_Sw09_Psd", "STW_ACTN_RQ"),
-      ("StW_Sw10_Psd", "STW_ACTN_RQ"),
-      ("StW_Sw11_Psd", "STW_ACTN_RQ"),
-      ("StW_Sw12_Psd", "STW_ACTN_RQ"),
-      ("StW_Sw13_Psd", "STW_ACTN_RQ"),
-      ("StW_Sw14_Psd", "STW_ACTN_RQ"),
-      ("StW_Sw15_Psd", "STW_ACTN_RQ"),
-      ("WprSw6Posn", "STW_ACTN_RQ"),
-      ("MC_STW_ACTN_RQ", "STW_ACTN_RQ"),
-      ("CRC_STW_ACTN_RQ", "STW_ACTN_RQ"),
-    ]
-
-    checks = [
+    messages = [
       # sig_address, frequency
       ("ESP_B", 50),
       ("DI_torque1", 100),
@@ -175,19 +114,12 @@ class CarState(CarStateBase):
       ("SDM1", 10),
       ("BrakeMessage", 50),
     ]
-
-    return CANParser(DBC[CP.carFingerprint]['chassis'], signals, checks, CANBUS.chassis)
+    return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.chassis)
 
   @staticmethod
   def get_cam_can_parser(CP):
-    signals = [
-      # sig_name, sig_address
-      ("DAS_accState", "DAS_control"),
-      ("DAS_aebEvent", "DAS_control"),
-      ("DAS_controlCounter", "DAS_control"),
-    ]
-    checks = [
+    messages = [
       # sig_address, frequency
       ("DAS_control", 40),
     ]
-    return CANParser(DBC[CP.carFingerprint]['chassis'], signals, checks, CANBUS.autopilot_chassis)
+    return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.autopilot_chassis)
diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py
index afd3fb3be4..e06139729c 100755
--- a/selfdrive/car/tesla/interface.py
+++ b/selfdrive/car/tesla/interface.py
@@ -1,9 +1,9 @@
 #!/usr/bin/env python3
 from cereal import car
 from panda import Panda
-from selfdrive.car.tesla.values import CANBUS, CAR
-from selfdrive.car import STD_CARGO_KG, get_safety_config
-from selfdrive.car.interfaces import CarInterfaceBase
+from openpilot.selfdrive.car.tesla.values import CANBUS, CAR
+from openpilot.selfdrive.car import get_safety_config
+from openpilot.selfdrive.car.interfaces import CarInterfaceBase
 
 
 class CarInterface(CarInterfaceBase):
@@ -42,7 +42,7 @@ class CarInterface(CarInterfaceBase):
     ret.steerActuatorDelay = 0.25
 
     if candidate in (CAR.AP2_MODELS, CAR.AP1_MODELS):
-      ret.mass = 2100. + STD_CARGO_KG
+      ret.mass = 2100.
       ret.wheelbase = 2.959
       ret.centerToFront = ret.wheelbase * 0.5
       ret.steerRatio = 15.0
diff --git a/selfdrive/car/tesla/radar_interface.py b/selfdrive/car/tesla/radar_interface.py
index a09f53e758..b3e7c7fcb1 100755
--- a/selfdrive/car/tesla/radar_interface.py
+++ b/selfdrive/car/tesla/radar_interface.py
@@ -1,8 +1,8 @@
 #!/usr/bin/env python3
 from cereal import car
 from opendbc.can.parser import CANParser
-from selfdrive.car.tesla.values import DBC, CANBUS
-from selfdrive.car.interfaces import RadarInterfaceBase
+from openpilot.selfdrive.car.tesla.values import DBC, CANBUS
+from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
 
 RADAR_MSGS_A = list(range(0x310, 0x36E, 3))
 RADAR_MSGS_B = list(range(0x311, 0x36F, 3))
@@ -10,13 +10,7 @@ NUM_POINTS = len(RADAR_MSGS_A)
 
 def get_radar_can_parser(CP):
   # Status messages
-  signals = [
-    ('RADC_HWFail', 'TeslaRadarSguInfo'),
-    ('RADC_SGUFail', 'TeslaRadarSguInfo'),
-    ('RADC_SensorDirty', 'TeslaRadarSguInfo'),
-  ]
-
-  checks = [
+  messages = [
     ('TeslaRadarSguInfo', 10),
   ]
 
@@ -25,28 +19,12 @@ def get_radar_can_parser(CP):
   for i in range(NUM_POINTS):
     msg_id_a = RADAR_MSGS_A[i]
     msg_id_b = RADAR_MSGS_B[i]
-
-    # There is a bunch more info in the messages,
-    # but these are the only things actually used in openpilot
-    signals.extend([
-      ('LongDist', msg_id_a),
-      ('LongSpeed', msg_id_a),
-      ('LatDist', msg_id_a),
-      ('LongAccel', msg_id_a),
-      ('Meas', msg_id_a),
-      ('Tracked', msg_id_a),
-      ('Index', msg_id_a),
-
-      ('LatSpeed', msg_id_b),
-      ('Index2', msg_id_b),
-    ])
-
-    checks.extend([
+    messages.extend([
       (msg_id_a, 8),
       (msg_id_b, 8),
     ])
 
-  return CANParser(DBC[CP.carFingerprint]['radar'], signals, checks, CANBUS.radar)
+  return CANParser(DBC[CP.carFingerprint]['radar'], messages, CANBUS.radar)
 
 class RadarInterface(RadarInterfaceBase):
   def __init__(self, CP):
diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py
index a491c030f8..6bb27b995f 100644
--- a/selfdrive/car/tesla/teslacan.py
+++ b/selfdrive/car/tesla/teslacan.py
@@ -1,7 +1,7 @@
 import crcmod
 
-from common.conversions import Conversions as CV
-from selfdrive.car.tesla.values import CANBUS, CarControllerParams
+from openpilot.common.conversions import Conversions as CV
+from openpilot.selfdrive.car.tesla.values import CANBUS, CarControllerParams
 
 
 class TeslaCAN:
diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py
index f79e5f0097..548809af1b 100644
--- a/selfdrive/car/tesla/values.py
+++ b/selfdrive/car/tesla/values.py
@@ -1,17 +1,19 @@
+# ruff: noqa: E501
 from collections import namedtuple
+from enum import StrEnum
 from typing import Dict, List, Union
 
 from cereal import car
-from selfdrive.car import AngleRateLimit, dbc_dict
-from selfdrive.car.docs_definitions import CarInfo
-from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
+from openpilot.selfdrive.car import AngleRateLimit, dbc_dict
+from openpilot.selfdrive.car.docs_definitions import CarInfo
+from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
 
 Ecu = car.CarParams.Ecu
 
 Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])
 
 
-class CAR:
+class CAR(StrEnum):
   AP1_MODELS = 'TESLA AP1 MODEL S'
   AP2_MODELS = 'TESLA AP2 MODEL S'
 
diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py
index 005ea114fd..a920bd45da 100755
--- a/selfdrive/car/tests/test_car_interfaces.py
+++ b/selfdrive/car/tests/test_car_interfaces.py
@@ -1,17 +1,24 @@
 #!/usr/bin/env python3
+import os
 import math
 import unittest
 import hypothesis.strategies as st
-from hypothesis import given, settings
+from hypothesis import Phase, given, settings
 import importlib
 from parameterized import parameterized
 
-from cereal import car
-from common.realtime import DT_CTRL
-from selfdrive.car import gen_empty_fingerprint
-from selfdrive.car.car_helpers import interfaces
-from selfdrive.car.fingerprints import all_known_cars
-from selfdrive.test.fuzzy_generation import DrawType, FuzzyGenerator
+from cereal import car, messaging
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.car import gen_empty_fingerprint
+from openpilot.selfdrive.car.car_helpers import interfaces
+from openpilot.selfdrive.car.fingerprints import all_known_cars
+from openpilot.selfdrive.car.fw_versions import FW_VERSIONS
+from openpilot.selfdrive.car.interfaces import get_interface_attr
+from openpilot.selfdrive.test.fuzzy_generation import DrawType, FuzzyGenerator
+
+ALL_ECUS = list({ecu for ecus in FW_VERSIONS.values() for ecu in ecus.keys()})
+
+MAX_EXAMPLES = int(os.environ.get('MAX_EXAMPLES', '20'))
 
 
 def get_fuzzy_car_interface_args(draw: DrawType) -> dict:
@@ -20,12 +27,8 @@ def get_fuzzy_car_interface_args(draw: DrawType) -> dict:
                                                                      st.integers(min_value=0, max_value=64)) for key in
                                                 gen_empty_fingerprint()})
 
-  # just the most important fields
-  car_fw_strategy = st.lists(st.fixed_dictionaries({
-    'ecu': st.sampled_from(list(car.CarParams.Ecu.schema.enumerants.keys())),
-    # TODO: only use reasonable addrs for the paired ecu and brand/platform
-    'address': st.integers(min_value=0, max_value=0x800),
-  }))
+  # only pick from possible ecus to reduce search space
+  car_fw_strategy = st.lists(st.sampled_from(ALL_ECUS))
 
   params_strategy = st.fixed_dictionaries({
     'fingerprints': fingerprint_strategy,
@@ -34,14 +37,21 @@ def get_fuzzy_car_interface_args(draw: DrawType) -> dict:
   })
 
   params: dict = draw(params_strategy)
-  params['car_fw'] = [car.CarParams.CarFw(**fw) for fw in params['car_fw']]
+  params['car_fw'] = [car.CarParams.CarFw(ecu=fw[0], address=fw[1], subAddress=fw[2] or 0) for fw in params['car_fw']]
   return params
 
 
 class TestCarInterfaces(unittest.TestCase):
 
+  @classmethod
+  def setUpClass(cls):
+    os.environ['NO_RADAR_SLEEP'] = '1'
+
+  # FIXME: Due to the lists used in carParams, Phase.target is very slow and will cause
+  #  many generated examples to overrun when max_examples > ~20, don't use it
   @parameterized.expand([(car,) for car in sorted(all_known_cars())])
-  @settings(max_examples=5)
+  @settings(max_examples=MAX_EXAMPLES, deadline=None,
+            phases=(Phase.reuse, Phase.generate, Phase.shrink))
   @given(data=st.data())
   def test_car_interfaces(self, car_name, data):
     CarInterface, CarController, CarState = interfaces[car_name]
@@ -77,9 +87,6 @@ class TestCarInterfaces(unittest.TestCase):
         self.assertTrue(not math.isnan(tune.torque.kf) and tune.torque.kf > 0)
         self.assertTrue(not math.isnan(tune.torque.friction) and tune.torque.friction > 0)
 
-      elif tune.which() == 'indi':
-        self.assertTrue(len(tune.indi.outerLoopGainV))
-
     cc_msg = FuzzyGenerator.get_random_msg(data.draw, car.CarControl, real_floats=True)
     # Run car interface
     now_nanos = 0
@@ -109,5 +116,37 @@ class TestCarInterfaces(unittest.TestCase):
        hasattr(radar_interface, '_update') and hasattr(radar_interface, 'trigger_msg'):
       radar_interface._update([radar_interface.trigger_msg])
 
+    # Test radar fault
+    if not car_params.radarUnavailable and radar_interface.rcp is not None:
+      cans = [messaging.new_message('can', 1).to_bytes() for _ in range(5)]
+      rr = radar_interface.update(cans)
+      self.assertTrue(rr is None or len(rr.errors) > 0)
+
+  def test_interface_attrs(self):
+    """Asserts basic behavior of interface attribute getter"""
+    num_brands = len(get_interface_attr('CAR'))
+    self.assertGreaterEqual(num_brands, 13)
+
+    # Should return value for all brands when not combining, even if attribute doesn't exist
+    ret = get_interface_attr('FAKE_ATTR')
+    self.assertEqual(len(ret), num_brands)
+
+    # Make sure we can combine dicts
+    ret = get_interface_attr('DBC', combine_brands=True)
+    self.assertGreaterEqual(len(ret), 160)
+
+    # We don't support combining non-dicts
+    ret = get_interface_attr('CAR', combine_brands=True)
+    self.assertEqual(len(ret), 0)
+
+    # If brand has None value, it shouldn't return when ignore_none=True is specified
+    none_brands = {b for b, v in get_interface_attr('FINGERPRINTS').items() if v is None}
+    self.assertGreaterEqual(len(none_brands), 1)
+
+    ret = get_interface_attr('FINGERPRINTS', ignore_none=True)
+    none_brands_in_ret = none_brands.intersection(ret)
+    self.assertEqual(len(none_brands_in_ret), 0, f'Brands with None values in ignore_none=True result: {none_brands_in_ret}')
+
+
 if __name__ == "__main__":
   unittest.main()
diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml
index 62d39171f9..12b483cfa7 100644
--- a/selfdrive/car/torque_data/override.yaml
+++ b/selfdrive/car/torque_data/override.yaml
@@ -7,9 +7,13 @@ NISSAN LEAF 2018 Instrument Cluster: [.nan, 1.5, .nan]
 NISSAN LEAF 2018: [.nan, 1.5, .nan]
 NISSAN ROGUE 2019: [.nan, 1.5, .nan]
 
+# New subarus angle based controllers
+SUBARU FORESTER 2022: [.nan, 3.0, .nan]
+SUBARU OUTBACK 7TH GEN: [.nan, 3.0, .nan]
+SUBARU ASCENT 2023: [.nan, 3.0, .nan]
+
 # Toyota LTA also has torque
 TOYOTA RAV4 2023: [.nan, 3.0, .nan]
-TOYOTA RAV4 HYBRID 2023: [.nan, 3.0, .nan]
 
 # Tesla has high torque
 TESLA AP1 MODEL S: [.nan, 2.5, .nan]
@@ -32,6 +36,7 @@ RAM 1500 5TH GEN: [2.0, 2.0, 0.05]
 RAM HD 5TH GEN: [1.4, 1.4, 0.05]
 SUBARU OUTBACK 6TH GEN: [2.0, 2.0, 0.2]
 CADILLAC ESCALADE 2017: [1.899999976158142, 1.842270016670227, 0.1120000034570694]
+CADILLAC ESCALADE ESV 2019: [1.15, 1.3, 0.2]
 CHEVROLET BOLT EUV 2022: [2.0, 2.0, 0.05]
 CHEVROLET SILVERADO 1500 2020: [1.9, 1.9, 0.112]
 CHEVROLET TRAILBLAZER 2021: [1.33, 1.9, 0.16]
@@ -50,6 +55,15 @@ KIA NIRO EV 2ND GEN: [2.05, 2.5, 0.14]
 GENESIS GV80 2023: [2.5, 2.5, 0.1]
 KIA CARNIVAL 4TH GEN: [1.75, 1.75, 0.15]
 GMC ACADIA DENALI 2018: [1.6, 1.6, 0.2]
+LEXUS IS 2023: [2.0, 2.0, 0.1]
+KIA SORENTO HYBRID 4TH GEN: [2.5, 2.5, 0.1]
+HYUNDAI KONA ELECTRIC 2ND GEN: [2.5, 2.5, 0.1]
+HYUNDAI IONIQ 6 2023: [2.5, 2.5, 0.1]
+HYUNDAI AZERA 6TH GEN: [1.8, 1.8, 0.1]
+HYUNDAI AZERA HYBRID 6TH GEN: [1.8, 1.8, 0.1]
+KIA K8 HYBRID 1ST GEN: [2.5, 2.5, 0.1]
+HYUNDAI CUSTIN 1ST GEN: [2.5, 2.5, 0.1]
+LEXUS GS F 2016: [2.5, 2.5, 0.08]
 
 # Dashcam or fallback configured as ideal car
 mock: [10.0, 10, 0.0]
diff --git a/selfdrive/car/torque_data/params.yaml b/selfdrive/car/torque_data/params.yaml
index 800507d91d..2d2dbc3f0b 100644
--- a/selfdrive/car/torque_data/params.yaml
+++ b/selfdrive/car/torque_data/params.yaml
@@ -45,15 +45,12 @@ KIA K5 2021: [2.405339728085138, 1.460032270828705, 0.11650989850813716]
 KIA NIRO EV 2020: [2.9215954981365337, 2.1500583840260044, 0.09236802474810267]
 KIA SORENTO GT LINE 2018: [2.464854685101844, 1.5335274218367956, 0.12056170567599558]
 KIA STINGER GT2 2018: [2.7499043387418967, 1.849652021986449, 0.12048334239559202]
-LEXUS ES 2019: [1.935835, 2.134803912579666, 0.093439]
-LEXUS ES HYBRID 2019: [2.135678, 1.863360677810788, 0.109627]
-LEXUS NX 2018: [2.302625600642627, 2.1382378491466625, 0.14986840878892838]
+LEXUS ES 2019: [2.0357564999999997, 1.999082295195227, 0.101533]
+LEXUS NX 2018: [2.3525924753753613, 1.9731412277641067, 0.15168101064205927]
 LEXUS NX 2020: [2.4331999786982936, 2.1045680431705414, 0.14099899317761067]
-LEXUS NX HYBRID 2018: [2.4025593501080955, 1.8080446063815507, 0.15349361249519017]
 LEXUS RX 2016: [1.5876816543130423, 1.0427699298523752, 0.21334066732397142]
-LEXUS RX 2020: [1.5228812994274734, 1.431102486563665, 0.164117]
+LEXUS RX 2020: [1.5375561442049257, 1.343166476215164, 0.1931062001527557]
 LEXUS RX HYBRID 2017: [1.6984261557042386, 1.3211501880159107, 0.1820354534928893]
-LEXUS RX HYBRID 2020: [1.5522309889823778, 1.255230465866663, 0.2220954003055114]
 MAZDA CX-9 2021: [1.7601682915983443, 1.0889677335154337, 0.17713792194297195]
 SKODA SUPERB 3RD GEN: [1.166437404652981, 1.1686163012668165, 0.12194533036948708]
 SUBARU FORESTER 2019: [3.6617001649776793, 2.342197172531713, 0.11075960785398745]
@@ -64,30 +61,23 @@ TOYOTA AVALON 2019: [1.7036141952825095, 1.239619084240008, 0.08459830394899492]
 TOYOTA AVALON 2022: [2.3154403649717357, 2.7777922854327124, 0.11453999639164605]
 TOYOTA C-HR 2018: [1.5591084333664578, 1.271271459066948, 0.20259087058453193]
 TOYOTA C-HR 2021: [1.7678810166088303, 1.3742176337919942, 0.2319674583741509]
-TOYOTA CAMRY 2018: [2.1172995371905015, 1.7156177222420887, 0.105192506]
-TOYOTA CAMRY 2021: [2.446083, 2.3476510120007434, 0.121615]
-TOYOTA CAMRY HYBRID 2018: [1.996333, 1.7996193116697359, 0.112565]
-TOYOTA CAMRY HYBRID 2021: [2.263582, 2.3901492458927986, 0.115257]
+TOYOTA CAMRY 2018: [2.0568162685952505, 1.7576185169559122, 0.108878753]
+TOYOTA CAMRY 2021: [2.3548324999999997, 2.368900128946771, 0.118436]
 TOYOTA COROLLA 2017: [3.117154369115421, 1.8438132575043773, 0.12289685869250652]
-TOYOTA COROLLA HYBRID TSS2 2019: [1.9079729107361805, 1.8118712531729109, 0.22251440891543514]
-TOYOTA COROLLA TSS2 2019: [2.0742917676766712, 1.9258612322678952, 0.16888685704519352]
+TOYOTA COROLLA TSS2 2019: [1.991132339206426, 1.868866242720403, 0.19570063298031432]
 TOYOTA HIGHLANDER 2017: [1.8696367437248915, 1.626293990451463, 0.17485372210240796]
-TOYOTA HIGHLANDER 2020: [2.022340166827233, 1.6183134804881791, 0.14592306380054457]
+TOYOTA HIGHLANDER 2020: [1.9617570834136164, 1.8611643317268927, 0.14519673256119725]
 TOYOTA HIGHLANDER HYBRID 2018: [1.752033, 1.6433903296845025, 0.144600]
-TOYOTA HIGHLANDER HYBRID 2020: [1.901174, 2.104015182965606, 0.14447040132184993]
 TOYOTA MIRAI 2021: [2.506899832157829, 1.7417213930750164, 0.20182618449440565]
 TOYOTA PRIUS 2017: [1.60, 1.5023147650693636, 0.151515]
 TOYOTA PRIUS TSS2 2021: [1.972600, 1.9104337425537743, 0.170968]
 TOYOTA RAV4 2017: [2.085695074355425, 2.2142832316984733, 0.13339165270103975]
-TOYOTA RAV4 2019: [2.331293, 2.0993589721530252, 0.129822]
-TOYOTA RAV4 2019 8965: [2.5084506298290377, 2.4216520504763475, 0.11992835265067918]
-TOYOTA RAV4 2019 x02: [2.7209621987605024, 2.2148637653781593, 0.10862567142268198]
+TOYOTA RAV4 2019: [2.279239424615458, 2.087101966779332, 0.13682208413446817]
+TOYOTA RAV4 2019 8965: [2.3080951748210854, 2.1189367835820603, 0.12942102328134028]
+TOYOTA RAV4 2019 x02: [2.762293266024922, 2.243615865975329, 0.11113568178327986]
 TOYOTA RAV4 HYBRID 2017: [1.9796257271652042, 1.7503987331707576, 0.14628860048885406]
-TOYOTA RAV4 HYBRID 2019: [2.2271858492309153, 2.074844961405639, 0.14382216826893632]
-TOYOTA RAV4 HYBRID 2019 8965: [2.1077397198131336, 1.8162215166877735, 0.13891369391200137]
-TOYOTA RAV4 HYBRID 2019 x02: [2.803624333289342, 2.272367966572498, 0.11364569214387774]
-TOYOTA RAV4 HYBRID 2022: [2.241883248393209, 1.9304407208090029, 0.112174]
-TOYOTA RAV4 HYBRID 2022 x02: [3.044930631831037, 2.3979189796380918, 0.14023209146703736]
+TOYOTA RAV4 2022: [2.241883248393209, 1.9304407208090029, 0.112174]
+TOYOTA RAV4 2022 x02: [3.044930631831037, 2.3979189796380918, 0.14023209146703736]
 TOYOTA SIENNA 2018: [1.689726, 1.3208264576110418, 0.140456]
 VOLKSWAGEN ARTEON 1ST GEN: [1.45136518053819, 1.3639364049316804, 0.23806361745695032]
 VOLKSWAGEN ATLAS 1ST GEN: [1.4677006726964945, 1.6733266634075656, 0.12959584092073367]
diff --git a/selfdrive/car/torque_data/substitute.yaml b/selfdrive/car/torque_data/substitute.yaml
index d79dbe8573..c7a1566b32 100644
--- a/selfdrive/car/torque_data/substitute.yaml
+++ b/selfdrive/car/torque_data/substitute.yaml
@@ -4,24 +4,18 @@ MAZDA CX-5: MAZDA CX-9 2021
 MAZDA CX-5 2022: MAZDA CX-9 2021
 MAZDA CX-9: MAZDA CX-9 2021
 
-TOYOTA ALPHARD HYBRID 2021 : TOYOTA SIENNA 2018
 TOYOTA ALPHARD 2020: TOYOTA SIENNA 2018
 TOYOTA PRIUS v 2017 : TOYOTA PRIUS 2017
-TOYOTA RAV4 2022: TOYOTA RAV4 HYBRID 2022
-TOYOTA C-HR HYBRID 2018: TOYOTA C-HR 2018
-TOYOTA C-HR HYBRID 2022: TOYOTA C-HR 2021
 LEXUS IS 2018: LEXUS NX 2018
 LEXUS CT HYBRID 2018 : LEXUS NX 2018
-LEXUS ES 2018: TOYOTA CAMRY HYBRID 2018
-LEXUS ES HYBRID 2018: TOYOTA CAMRY HYBRID 2018
-LEXUS NX HYBRID 2020: LEXUS NX 2020
+LEXUS ES 2018: TOYOTA CAMRY 2018
+LEXUS ES HYBRID 2018: TOYOTA CAMRY 2018
 LEXUS RC 2020: LEXUS NX 2020
-TOYOTA AVALON HYBRID 2019: TOYOTA AVALON 2019
-TOYOTA AVALON HYBRID 2022: TOYOTA AVALON 2022
 
 KIA OPTIMA 4TH GEN: HYUNDAI SONATA 2020
 KIA OPTIMA 4TH GEN FACELIFT: HYUNDAI SONATA 2020
 KIA OPTIMA HYBRID 2017 & SPORTS 2019: HYUNDAI SONATA 2020
+KIA OPTIMA HYBRID 4TH GEN FACELIFT: HYUNDAI SONATA 2020
 KIA FORTE E 2018 & GT 2021: HYUNDAI SONATA 2020
 KIA CEED INTRO ED 2019: HYUNDAI SONATA 2020
 KIA SELTOS 2021: HYUNDAI SONATA 2020
@@ -34,8 +28,8 @@ HYUNDAI KONA ELECTRIC 2022: HYUNDAI KONA ELECTRIC 2019
 HYUNDAI IONIQ HYBRID 2017-2019: HYUNDAI IONIQ PLUG-IN HYBRID 2019
 HYUNDAI IONIQ HYBRID 2020-2022: HYUNDAI IONIQ PLUG-IN HYBRID 2019
 HYUNDAI IONIQ ELECTRIC 2020: HYUNDAI IONIQ PLUG-IN HYBRID 2019
-HYUNDAI IONIQ 6 2023: HYUNDAI IONIQ 5 2022
 HYUNDAI ELANTRA 2017: HYUNDAI SONATA 2019
+HYUNDAI I30 N LINE 2019 & GT 2018 DCT: HYUNDAI SONATA 2019
 HYUNDAI ELANTRA HYBRID 2021: HYUNDAI SONATA 2020
 HYUNDAI TUCSON 2019: HYUNDAI SANTA FE 2019
 HYUNDAI TUCSON 4TH GEN: HYUNDAI TUCSON HYBRID 4TH GEN
@@ -76,6 +70,8 @@ VOLKSWAGEN POLO 6TH GEN: VOLKSWAGEN GOLF 7TH GEN
 SEAT LEON 3RD GEN: VOLKSWAGEN GOLF 7TH GEN
 SEAT ATECA 1ST GEN: VOLKSWAGEN GOLF 7TH GEN
 
+SUBARU CROSSTREK HYBRID 2020: SUBARU IMPREZA SPORT 2020
+SUBARU FORESTER HYBRID 2020: SUBARU IMPREZA SPORT 2020
 SUBARU LEGACY 7TH GEN: SUBARU OUTBACK 6TH GEN
 
 # Old subarus don't have much data guessing it's like low torque impreza
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 905251b532..2a02a57d86 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -1,12 +1,10 @@
 from cereal import car
-from common.numpy_fast import clip, interp
-from selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, \
+from openpilot.common.numpy_fast import clip, interp
+from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \
                           create_gas_interceptor_command, make_can_msg
-from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
-                                           create_accel_command, create_acc_cancel_command, \
-                                           create_fcw_command, create_lta_steer_command
-from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
-                                        MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, \
+from openpilot.selfdrive.car.toyota import toyotacan
+from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
+                                        MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, \
                                         UNSUPPORTED_DSU_CAR
 from opendbc.can.packer import CANPacker
 
@@ -56,25 +54,18 @@ class CarController:
     new_steer = int(round(actuators.steer * self.params.STEER_MAX))
     apply_steer = apply_meas_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.params)
 
-    # Count up to MAX_STEER_RATE_FRAMES, at which point we need to cut torque to avoid a steering fault
-    if lat_active and abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE:
-      self.steer_rate_counter += 1
-    else:
-      self.steer_rate_counter = 0
+    # >100 degree/sec steering fault prevention
+    self.steer_rate_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE, CC.latActive,
+                                                                      self.steer_rate_counter, MAX_STEER_RATE_FRAMES)
 
-    apply_steer_req = 1
-    if not lat_active:
+    if not CC.latActive:
       apply_steer = 0
-      apply_steer_req = 0
-    elif self.steer_rate_counter > MAX_STEER_RATE_FRAMES:
-      apply_steer_req = 0
-      self.steer_rate_counter = 0
 
     # *** steer angle ***
     if self.CP.steerControlType == SteerControlType.angle:
       # If using LTA control, disable LKA and set steering angle command
       apply_steer = 0
-      apply_steer_req = 0
+      apply_steer_req = False
       if self.frame % 2 == 0:
         # EPS uses the torque sensor angle to control with, offset to compensate
         apply_angle = actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg
@@ -92,13 +83,13 @@ class CarController:
     # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
     # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
     # on consecutive messages
-    can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req))
+    can_sends.append(toyotacan.create_steer_command(self.packer, apply_steer, apply_steer_req))
     if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR:
       lta_active = lat_active and self.CP.steerControlType == SteerControlType.angle
       full_torque_condition = (abs(CS.out.steeringTorqueEps) < self.params.STEER_MAX and
                                abs(CS.out.steeringTorque) < MAX_DRIVER_TORQUE_ALLOWANCE)
       setme_x64 = 100 if lta_active and full_torque_condition else 0
-      can_sends.append(create_lta_steer_command(self.packer, self.last_angle, lta_active, self.frame // 2, setme_x64))
+      can_sends.append(toyotacan.create_lta_steer_command(self.packer, self.last_angle, lta_active, self.frame // 2, setme_x64))
 
     # *** gas and brake ***
     if self.CP.enableGasInterceptor and CC.longActive:
@@ -132,18 +123,22 @@ class CarController:
 
     self.last_standstill = CS.out.standstill
 
+    # handle UI messages
+    fcw_alert = hud_control.visualAlert == VisualAlert.fcw
+    steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
+
     # we can spam can to cancel the system even if we are using lat only control
     if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
       lead = hud_control.leadVisible or CS.out.vEgo < 12.  # at low speed we always assume the lead is present so ACC can be engaged
 
       # Lexus IS uses a different cancellation message
       if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
-        can_sends.append(create_acc_cancel_command(self.packer))
+        can_sends.append(toyotacan.create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type))
+        can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type))
+        can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False))
 
     if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
@@ -156,9 +151,6 @@ class CarController:
       # ui mesg is at 1Hz but we send asap if:
       # - there is something to display
       # - there is something to stop displaying
-      fcw_alert = hud_control.visualAlert == VisualAlert.fcw
-      steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
-
       send_ui = False
       if ((fcw_alert or steer_alert) and not self.alert_active) or \
          (not (fcw_alert or steer_alert) and self.alert_active):
@@ -169,18 +161,22 @@ class CarController:
         send_ui = True
 
       if self.frame % 20 == 0 or send_ui:
-        can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible,
-                                           hud_control.rightLaneVisible, hud_control.leftLaneDepart,
-                                           hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud))
+        can_sends.append(toyotacan.create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible,
+                                                     hud_control.rightLaneVisible, hud_control.leftLaneDepart,
+                                                     hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud))
 
-      if (self.frame % 100 == 0 or send_ui) and self.CP.enableDsu:
-        can_sends.append(create_fcw_command(self.packer, fcw_alert))
+      if (self.frame % 100 == 0 or send_ui) and (self.CP.enableDsu or self.CP.flags & ToyotaFlags.DISABLE_RADAR.value):
+        can_sends.append(toyotacan.create_fcw_command(self.packer, fcw_alert))
 
     # *** static msgs ***
     for addr, cars, bus, fr_step, vl in STATIC_DSU_MSGS:
       if self.frame % fr_step == 0 and self.CP.enableDsu and self.CP.carFingerprint in cars:
         can_sends.append(make_can_msg(addr, vl, bus))
 
+    # keep radar disabled
+    if self.frame % 20 == 0 and self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
+      can_sends.append([0x750, 0, b"\x0F\x02\x3E\x00\x00\x00\x00\x00", 0])
+
     new_actuators = actuators.copy()
     new_actuators.steer = apply_steer / self.params.STEER_MAX
     new_actuators.steerOutputCan = apply_steer
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index b6ecbe5e5f..3a89d655da 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -1,14 +1,15 @@
 import copy
 
 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 openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import mean
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.realtime import DT_CTRL
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
-from selfdrive.car.interfaces import CarStateBase
-from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
+from openpilot.selfdrive.car.interfaces import CarStateBase
+from openpilot.selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
+                                                  TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
 
 SteerControlType = car.CarParams.SteerControlType
 
@@ -57,9 +58,7 @@ class CarState(CarStateBase):
       ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2
       ret.gasPressed = ret.gas > 805
     else:
-      # TODO: find a new, common signal
-      msg = "GAS_PEDAL_HYBRID" if (self.CP.flags & ToyotaFlags.HYBRID) else "GAS_PEDAL"
-      ret.gas = cp.vl[msg]["GAS_PEDAL"]
+      # TODO: find a common gas pedal percentage signal
       ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0
 
     ret.wheelSpeeds = self.get_wheel_speeds(
@@ -75,6 +74,7 @@ class CarState(CarStateBase):
     ret.standstill = ret.vEgoRaw == 0
 
     ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
+    ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]
     torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
 
     # On some cars, the angle measurement is non-zero while initializing
@@ -82,16 +82,14 @@ class CarState(CarStateBase):
       self.accurate_steer_angle_seen = True
 
     if self.accurate_steer_angle_seen:
-      # Offset seems to be invalid for large steering angles
-      if abs(ret.steeringAngleDeg) < 90 and cp.can_valid:
+      # Offset seems to be invalid for large steering angles and high angle rates
+      if abs(ret.steeringAngleDeg) < 90 and abs(ret.steeringRateDeg) < 100 and cp.can_valid:
         self.angle_offset.update(torque_sensor_angle_deg - ret.steeringAngleDeg)
 
       if self.angle_offset.initialized:
         ret.steeringAngleOffsetDeg = self.angle_offset.x
         ret.steeringAngleDeg = torque_sensor_angle_deg - self.angle_offset.x
 
-    ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]
-
     can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
     ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
     ret.leftBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1
@@ -129,10 +127,10 @@ class CarState(CarStateBase):
 
     cp_acc = cp_cam if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) else cp
 
-    if self.CP.carFingerprint in (TSS2_CAR | RADAR_ACC_CAR):
+    if self.CP.carFingerprint in TSS2_CAR and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
       if not (self.CP.flags & ToyotaFlags.SMART_DSU.value):
         self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"]
-      ret.stockFcw = bool(cp_acc.vl["ACC_HUD"]["FCW"])
+      ret.stockFcw = bool(cp_acc.vl["PCS_HUD"]["FCW"])
 
     # some TSS2 cars have low speed lockout permanently set, so ignore on those cars
     # these cars are identified by an ACC_TYPE value of 2.
@@ -152,7 +150,7 @@ class CarState(CarStateBase):
     ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"])
     ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0
 
-    if not self.CP.enableDsu:
+    if not self.CP.enableDsu and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
       ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
 
     if self.CP.enableBsm:
@@ -166,44 +164,7 @@ class CarState(CarStateBase):
 
   @staticmethod
   def get_can_parser(CP):
-    signals = [
-      # sig_name, sig_address
-      ("STEER_ANGLE", "STEER_ANGLE_SENSOR"),
-      ("GEAR", "GEAR_PACKET"),
-      ("BRAKE_PRESSED", "BRAKE_MODULE"),
-      ("WHEEL_SPEED_FL", "WHEEL_SPEEDS"),
-      ("WHEEL_SPEED_FR", "WHEEL_SPEEDS"),
-      ("WHEEL_SPEED_RL", "WHEEL_SPEEDS"),
-      ("WHEEL_SPEED_RR", "WHEEL_SPEEDS"),
-      ("DOOR_OPEN_FL", "BODY_CONTROL_STATE"),
-      ("DOOR_OPEN_FR", "BODY_CONTROL_STATE"),
-      ("DOOR_OPEN_RL", "BODY_CONTROL_STATE"),
-      ("DOOR_OPEN_RR", "BODY_CONTROL_STATE"),
-      ("SEATBELT_DRIVER_UNLATCHED", "BODY_CONTROL_STATE"),
-      ("PARKING_BRAKE", "BODY_CONTROL_STATE"),
-      ("UNITS", "BODY_CONTROL_STATE_2"),
-      ("TC_DISABLED", "ESP_CONTROL"),
-      ("BRAKE_HOLD_ACTIVE", "ESP_CONTROL"),
-      ("STEER_FRACTION", "STEER_ANGLE_SENSOR"),
-      ("STEER_RATE", "STEER_ANGLE_SENSOR"),
-      ("CRUISE_ACTIVE", "PCM_CRUISE"),
-      ("CRUISE_STATE", "PCM_CRUISE"),
-      ("GAS_RELEASED", "PCM_CRUISE"),
-      ("UI_SET_SPEED", "PCM_CRUISE_SM"),
-      ("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"),
-    ]
-
-    # Check LTA state if using LTA angle control
-    if CP.steerControlType == SteerControlType.angle:
-      signals.append(("LTA_STATE", "EPS_STATUS"))
-
-    checks = [
+    messages = [
       ("GEAR_PACKET", 1),
       ("LIGHT_STALK", 1),
       ("BLINKERS_STATE", 0.15),
@@ -219,95 +180,49 @@ class CarState(CarStateBase):
       ("STEER_TORQUE_SENSOR", 50),
     ]
 
-    if CP.flags & ToyotaFlags.HYBRID:
-      signals.append(("GAS_PEDAL", "GAS_PEDAL_HYBRID"))
-      checks.append(("GAS_PEDAL_HYBRID", 33))
-    else:
-      signals.append(("GAS_PEDAL", "GAS_PEDAL"))
-      checks.append(("GAS_PEDAL", 33))
-
     if CP.carFingerprint in UNSUPPORTED_DSU_CAR:
-      signals.append(("MAIN_ON", "DSU_CRUISE"))
-      signals.append(("SET_SPEED", "DSU_CRUISE"))
-      signals.append(("UI_SET_SPEED", "PCM_CRUISE_ALT"))
-      checks.append(("DSU_CRUISE", 5))
-      checks.append(("PCM_CRUISE_ALT", 1))
+      messages.append(("DSU_CRUISE", 5))
+      messages.append(("PCM_CRUISE_ALT", 1))
     else:
-      signals.append(("MAIN_ON", "PCM_CRUISE_2"))
-      signals.append(("SET_SPEED", "PCM_CRUISE_2"))
-      signals.append(("ACC_FAULTED", "PCM_CRUISE_2"))
-      signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2"))
-      checks.append(("PCM_CRUISE_2", 33))
+      messages.append(("PCM_CRUISE_2", 33))
 
     # add gas interceptor reading if we are using it
     if CP.enableGasInterceptor:
-      signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR"))
-      signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR"))
-      checks.append(("GAS_SENSOR", 50))
+      messages.append(("GAS_SENSOR", 50))
 
     if CP.enableBsm:
-      signals += [
-        ("L_ADJACENT", "BSM"),
-        ("L_APPROACHING", "BSM"),
-        ("R_ADJACENT", "BSM"),
-        ("R_APPROACHING", "BSM"),
-      ]
-      checks.append(("BSM", 1))
+      messages.append(("BSM", 1))
 
-    if CP.carFingerprint in RADAR_ACC_CAR:
+    if CP.carFingerprint in RADAR_ACC_CAR and not CP.flags & ToyotaFlags.DISABLE_RADAR.value:
       if not CP.flags & ToyotaFlags.SMART_DSU.value:
-        signals += [
-          ("ACC_TYPE", "ACC_CONTROL"),
-        ]
-        checks += [
+        messages += [
           ("ACC_CONTROL", 33),
         ]
-      signals += [
-        ("FCW", "ACC_HUD"),
-      ]
-      checks += [
-        ("ACC_HUD", 1),
+      messages += [
+        ("PCS_HUD", 1),
       ]
 
-    if CP.carFingerprint not in (TSS2_CAR - RADAR_ACC_CAR) and not CP.enableDsu:
-      signals += [
-        ("FORCE", "PRE_COLLISION"),
-        ("PRECOLLISION_ACTIVE", "PRE_COLLISION"),
-      ]
-      checks += [
+    if CP.carFingerprint not in (TSS2_CAR - RADAR_ACC_CAR) and not CP.enableDsu and not CP.flags & ToyotaFlags.DISABLE_RADAR.value:
+      messages += [
         ("PRE_COLLISION", 33),
       ]
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
 
   @staticmethod
   def get_cam_can_parser(CP):
-    signals = []
-    checks = []
+    messages = []
 
     if CP.carFingerprint != CAR.PRIUS_V:
-      signals += [
-        ("LANE_SWAY_FLD", "LKAS_HUD"),
-        ("LANE_SWAY_BUZZER", "LKAS_HUD"),
-        ("LANE_SWAY_WARNING", "LKAS_HUD"),
-        ("LANE_SWAY_SENSITIVITY", "LKAS_HUD"),
-        ("LANE_SWAY_TOGGLE", "LKAS_HUD"),
-      ]
-      checks += [
+      messages += [
         ("LKAS_HUD", 1),
       ]
 
     if CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR):
-      signals += [
-        ("PRECOLLISION_ACTIVE", "PRE_COLLISION"),
-        ("FORCE", "PRE_COLLISION"),
-        ("ACC_TYPE", "ACC_CONTROL"),
-        ("FCW", "ACC_HUD"),
-      ]
-      checks += [
+      messages += [
         ("PRE_COLLISION", 33),
         ("ACC_CONTROL", 33),
-        ("ACC_HUD", 1),
+        ("PCS_HUD", 1),
       ]
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 75f61609db..9cf86d69ab 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -1,11 +1,12 @@
-#!/usr/bin/env python3
 from cereal import car
-from common.conversions import Conversions as CV
+from openpilot.common.conversions import Conversions as CV
 from panda import Panda
-from selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
-                                        MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR
-from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config
-from selfdrive.car.interfaces import CarInterfaceBase
+from panda.python import uds
+from openpilot.selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
+                                        MIN_ACC_SPEED, EPS_SCALE, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR
+from openpilot.selfdrive.car import get_safety_config
+from openpilot.selfdrive.car.disable_ecu import disable_ecu
+from openpilot.selfdrive.car.interfaces import CarInterfaceBase
 
 EventName = car.CarEvent.EventName
 SteerControlType = car.CarParams.SteerControlType
@@ -42,14 +43,14 @@ class CarInterface(CarInterfaceBase):
 
     ret.stoppingControl = False  # Toyota starts braking more when it thinks you want to stop
 
-    stop_and_go = False
+    stop_and_go = candidate in TSS2_CAR
 
     if candidate == CAR.PRIUS:
       stop_and_go = True
       ret.wheelbase = 2.70
       ret.steerRatio = 15.74   # unknown end-to-end spec
-      tire_stiffness_factor = 0.6371   # hand-tune
-      ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.tireStiffnessFactor = 0.6371   # hand-tune
+      ret.mass = 3045. * CV.LB_TO_KG
       # Only give steer angle deadzone to for bad angle sensor prius
       for fw in car_fw:
         if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00':
@@ -60,67 +61,65 @@ class CarInterface(CarInterfaceBase):
       stop_and_go = True
       ret.wheelbase = 2.78
       ret.steerRatio = 17.4
-      tire_stiffness_factor = 0.5533
-      ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.tireStiffnessFactor = 0.5533
+      ret.mass = 3340. * CV.LB_TO_KG
 
     elif candidate in (CAR.RAV4, CAR.RAV4H):
       stop_and_go = True if (candidate in CAR.RAV4H) else False
       ret.wheelbase = 2.65
       ret.steerRatio = 16.88   # 14.5 is spec end-to-end
-      tire_stiffness_factor = 0.5533
-      ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
+      ret.tireStiffnessFactor = 0.5533
+      ret.mass = 3650. * CV.LB_TO_KG  # mean between normal and hybrid
 
     elif candidate == CAR.COROLLA:
       ret.wheelbase = 2.70
       ret.steerRatio = 18.27
-      tire_stiffness_factor = 0.444  # not optimized yet
-      ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
+      ret.tireStiffnessFactor = 0.444  # not optimized yet
+      ret.mass = 2860. * CV.LB_TO_KG  # mean between normal and hybrid
 
-    elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RXH, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2):
+    elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RXH, CAR.LEXUS_RX_TSS2):
       stop_and_go = True
       ret.wheelbase = 2.79
       ret.steerRatio = 16.  # 14.8 is spec end-to-end
       ret.wheelSpeedFactor = 1.035
-      tire_stiffness_factor = 0.5533
-      ret.mass = 4481. * CV.LB_TO_KG + STD_CARGO_KG  # mean between min and max
+      ret.tireStiffnessFactor = 0.5533
+      ret.mass = 4481. * CV.LB_TO_KG  # mean between min and max
 
-    elif candidate in (CAR.CHR, CAR.CHRH, CAR.CHR_TSS2, CAR.CHRH_TSS2):
+    elif candidate in (CAR.CHR, CAR.CHR_TSS2):
       stop_and_go = True
       ret.wheelbase = 2.63906
       ret.steerRatio = 13.6
-      tire_stiffness_factor = 0.7933
-      ret.mass = 3300. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.tireStiffnessFactor = 0.7933
+      ret.mass = 3300. * CV.LB_TO_KG
 
-    elif candidate in (CAR.CAMRY, CAR.CAMRYH, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2):
+    elif candidate in (CAR.CAMRY, CAR.CAMRY_TSS2):
       stop_and_go = True
       ret.wheelbase = 2.82448
       ret.steerRatio = 13.7
-      tire_stiffness_factor = 0.7933
-      ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
+      ret.tireStiffnessFactor = 0.7933
+      ret.mass = 3400. * CV.LB_TO_KG  # mean between normal and hybrid
 
-    elif candidate in (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2):
+    elif candidate in (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.HIGHLANDER_TSS2):
       stop_and_go = True
       ret.wheelbase = 2.8194  # average of 109.8 and 112.2 in
       ret.steerRatio = 16.0
-      tire_stiffness_factor = 0.8
-      ret.mass = 4516. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
+      ret.tireStiffnessFactor = 0.8
+      ret.mass = 4516. * CV.LB_TO_KG  # mean between normal and hybrid
 
-    elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019, CAR.AVALON_TSS2, CAR.AVALONH_TSS2):
+    elif candidate in (CAR.AVALON, CAR.AVALON_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
-      ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
+      ret.tireStiffnessFactor = 0.7983
+      ret.mass = 3505. * CV.LB_TO_KG  # mean between normal and hybrid
 
-    elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022,
-                       CAR.RAV4_TSS2_2023, CAR.RAV4H_TSS2_2023):
-      stop_and_go = True
+    elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023):
       ret.wheelbase = 2.68986
       ret.steerRatio = 14.3
-      tire_stiffness_factor = 0.7933
-      ret.mass = 3585. * CV.LB_TO_KG + STD_CARGO_KG  # Average between ICE and Hybrid
+      ret.tireStiffnessFactor = 0.7933
+      ret.mass = 3585. * CV.LB_TO_KG  # Average between ICE and Hybrid
       ret.lateralTuning.init('pid')
       ret.lateralTuning.pid.kiBP = [0.0]
       ret.lateralTuning.pid.kpBP = [0.0]
@@ -137,80 +136,81 @@ class CarInterface(CarInterfaceBase):
           ret.lateralTuning.pid.kf = 0.00004
           break
 
-    elif candidate in (CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2):
-      stop_and_go = True
+    elif candidate == CAR.COROLLA_TSS2:
       ret.wheelbase = 2.67  # Average between 2.70 for sedan and 2.64 for hatchback
       ret.steerRatio = 13.9
-      tire_stiffness_factor = 0.444  # not optimized yet
-      ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.tireStiffnessFactor = 0.444  # not optimized yet
+      ret.mass = 3060. * CV.LB_TO_KG
 
-    elif candidate in (CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2):
+    elif candidate in (CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_ES_TSS2):
       if candidate not in (CAR.LEXUS_ES,):  # TODO: LEXUS_ES may have sng
         stop_and_go = True
       ret.wheelbase = 2.8702
       ret.steerRatio = 16.0  # not optimized
-      tire_stiffness_factor = 0.444  # not optimized yet
-      ret.mass = 3677. * CV.LB_TO_KG + STD_CARGO_KG  # mean between min and max
+      ret.tireStiffnessFactor = 0.444  # not optimized yet
+      ret.mass = 3677. * CV.LB_TO_KG  # mean between min and max
 
     elif candidate == CAR.SIENNA:
       stop_and_go = True
       ret.wheelbase = 3.03
       ret.steerRatio = 15.5
-      tire_stiffness_factor = 0.444
-      ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.tireStiffnessFactor = 0.444
+      ret.mass = 4590. * CV.LB_TO_KG
 
-    elif candidate in (CAR.LEXUS_IS, CAR.LEXUS_RC):
+    elif candidate in (CAR.LEXUS_IS, CAR.LEXUS_IS_TSS2, CAR.LEXUS_RC):
       ret.wheelbase = 2.79908
       ret.steerRatio = 13.3
-      tire_stiffness_factor = 0.444
-      ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG
+      ret.tireStiffnessFactor = 0.444
+      ret.mass = 3736.8 * CV.LB_TO_KG
+
+    elif candidate == CAR.LEXUS_GS_F:
+      ret.wheelbase = 2.84988
+      ret.steerRatio = 13.3
+      ret.tireStiffnessFactor = 0.444
+      ret.mass = 4034. * CV.LB_TO_KG
 
     elif candidate == CAR.LEXUS_CTH:
       stop_and_go = True
       ret.wheelbase = 2.60
       ret.steerRatio = 18.6
-      tire_stiffness_factor = 0.517
-      ret.mass = 3108 * CV.LB_TO_KG + STD_CARGO_KG  # mean between min and max
+      ret.tireStiffnessFactor = 0.517
+      ret.mass = 3108 * CV.LB_TO_KG  # mean between min and max
 
-    elif candidate in (CAR.LEXUS_NX, CAR.LEXUS_NXH, CAR.LEXUS_NX_TSS2, CAR.LEXUS_NXH_TSS2):
+    elif candidate in (CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2):
       stop_and_go = True
       ret.wheelbase = 2.66
       ret.steerRatio = 14.7
-      tire_stiffness_factor = 0.444  # not optimized yet
-      ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG
+      ret.tireStiffnessFactor = 0.444  # not optimized yet
+      ret.mass = 4070 * CV.LB_TO_KG
 
     elif candidate == CAR.PRIUS_TSS2:
-      stop_and_go = True
       ret.wheelbase = 2.70002  # from toyota online sepc.
       ret.steerRatio = 13.4   # True steerRatio from older prius
-      tire_stiffness_factor = 0.6371   # hand-tune
-      ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.tireStiffnessFactor = 0.6371   # hand-tune
+      ret.mass = 3115. * CV.LB_TO_KG
 
     elif candidate == CAR.MIRAI:
       stop_and_go = True
       ret.wheelbase = 2.91
       ret.steerRatio = 14.8
-      tire_stiffness_factor = 0.8
-      ret.mass = 4300. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.tireStiffnessFactor = 0.8
+      ret.mass = 4300. * CV.LB_TO_KG
 
-    elif candidate in (CAR.ALPHARD_TSS2, CAR.ALPHARDH_TSS2):
-      stop_and_go = True
+    elif candidate == CAR.ALPHARD_TSS2:
       ret.wheelbase = 3.00
       ret.steerRatio = 14.2
-      tire_stiffness_factor = 0.444
-      ret.mass = 4305. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.tireStiffnessFactor = 0.444
+      ret.mass = 4305. * CV.LB_TO_KG
 
     ret.centerToFront = ret.wheelbase * 0.44
 
-    # TODO: start from empirically derived lateral slip stiffness for the civic and scale by
-    # mass and CG position, so all cars will have approximately similar dyn behaviors
-    ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
-                                                                         tire_stiffness_factor=tire_stiffness_factor)
-
+    # TODO: Some TSS-P platforms have BSM, but are flipped based on region or driving direction.
+    # Detect flipped signals and enable for C-HR and others
     ret.enableBsm = 0x3F6 in fingerprint[0] and candidate in TSS2_CAR
 
     # Detect smartDSU, which intercepts ACC_CMD from the DSU (or radar) allowing openpilot to send it
-    if 0x2FF in fingerprint[0]:
+    # 0x2AA is sent by a similar device which intercepts the radar instead of DSU on NO_DSU_CARs
+    if 0x2FF in fingerprint[0] or (0x2AA in fingerprint[0] and candidate in NO_DSU_CAR):
       ret.flags |= ToyotaFlags.SMART_DSU.value
 
     # No radar dbc for cars without DSU which are not TSS 2.0
@@ -219,15 +219,22 @@ class CarInterface(CarInterfaceBase):
 
     # In TSS2 cars, the camera does long control
     found_ecus = [fw.ecu for fw in car_fw]
-    ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) and not (ret.flags & ToyotaFlags.SMART_DSU)
+    ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) \
+                                        and not (ret.flags & ToyotaFlags.SMART_DSU)
     ret.enableGasInterceptor = 0x201 in fingerprint[0]
 
     # if the smartDSU is detected, openpilot can send ACC_CONTROL and the smartDSU will block it from the DSU or radar.
-    # since we don't yet parse radar on TSS2 radar-based ACC cars, gate longitudinal behind experimental toggle
+    # since we don't yet parse radar on TSS2/TSS-P radar-based ACC cars, gate longitudinal behind experimental toggle
     use_sdsu = bool(ret.flags & ToyotaFlags.SMART_DSU)
-    if candidate in RADAR_ACC_CAR:
+    if candidate in (RADAR_ACC_CAR | NO_DSU_CAR):
       ret.experimentalLongitudinalAvailable = use_sdsu
-      use_sdsu = use_sdsu and experimental_long
+
+      if not use_sdsu:
+        # Disabling radar is only supported on TSS2 radar-ACC cars
+        if experimental_long and candidate in RADAR_ACC_CAR and False:  # TODO: disabling radar isn't supported yet
+          ret.flags |= ToyotaFlags.DISABLE_RADAR.value
+      else:
+        use_sdsu = use_sdsu and experimental_long
 
     # openpilot longitudinal enabled by default:
     #  - non-(TSS2 radar ACC cars) w/ smartDSU installed
@@ -235,17 +242,14 @@ class CarInterface(CarInterfaceBase):
     #  - TSS2 cars with camera sending ACC_CONTROL where we can block it
     # openpilot longitudinal behind experimental long toggle:
     #  - TSS2 radar ACC cars w/ smartDSU installed
-    ret.openpilotLongitudinalControl = use_sdsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR)
+    #  - TSS2 radar ACC cars w/o smartDSU installed (disables radar)
+    #  - TSS-P DSU-less cars w/ CAN filter installed (no radar parser yet)
+    ret.openpilotLongitudinalControl = use_sdsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) or bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value)
     ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR
 
     if not ret.openpilotLongitudinalControl:
       ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL
 
-    # we can't use the fingerprint to detect this reliably, since
-    # the EV gas pedal signal can take a couple seconds to appear
-    if candidate in EV_HYBRID_CAR:
-      ret.flags |= ToyotaFlags.HYBRID.value
-
     # min speed to enable ACC. if car can do stop and go, then set enabling speed
     # to a negative value, so it won't matter.
     ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED
@@ -270,6 +274,13 @@ class CarInterface(CarInterfaceBase):
 
     return ret
 
+  @staticmethod
+  def init(CP, logcan, sendcan):
+    # disable radar if alpha longitudinal toggled on radar-ACC car without CAN filter/smartDSU
+    if CP.flags & ToyotaFlags.DISABLE_RADAR.value:
+      communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX, uds.MESSAGE_TYPE.NORMAL])
+      disable_ecu(logcan, sendcan, bus=0, addr=0x750, sub_addr=0xf, com_cont_req=communication_control)
+
   # returns a car.CarState
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam)
diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py
index 64906b34be..fae6eecaf6 100755
--- a/selfdrive/car/toyota/radar_interface.py
+++ b/selfdrive/car/toyota/radar_interface.py
@@ -1,8 +1,8 @@
 #!/usr/bin/env python3
 from opendbc.can.parser import CANParser
 from cereal import car
-from selfdrive.car.toyota.values import DBC, TSS2_CAR
-from selfdrive.car.interfaces import RadarInterfaceBase
+from openpilot.selfdrive.car.toyota.values import DBC, TSS2_CAR
+from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
 
 
 def _create_radar_can_parser(car_fingerprint):
@@ -15,14 +15,9 @@ def _create_radar_can_parser(car_fingerprint):
 
   msg_a_n = len(RADAR_A_MSGS)
   msg_b_n = len(RADAR_B_MSGS)
+  messages = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [20] * (msg_a_n + msg_b_n), strict=True))
 
-  signals = list(zip(['LONG_DIST'] * msg_a_n + ['NEW_TRACK'] * msg_a_n + ['LAT_DIST'] * msg_a_n +
-                     ['REL_SPEED'] * msg_a_n + ['VALID'] * msg_a_n + ['SCORE'] * msg_b_n,
-                     RADAR_A_MSGS * 5 + RADAR_B_MSGS))
-
-  checks = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [20] * (msg_a_n + msg_b_n)))
-
-  return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1)
+  return CANParser(DBC[car_fingerprint]['radar'], messages, 1)
 
 class RadarInterface(RadarInterfaceBase):
   def __init__(self, CP):
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index 01861c534a..ed0237c1be 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -27,7 +27,7 @@ def create_lta_steer_command(packer, steer_angle, steer_req, frame, setme_x64):
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,
@@ -38,6 +38,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_ty
     "RELEASE_STANDSTILL": not standstill_req,
     "CANCEL_REQ": pcm_cancel,
     "ALLOW_LONG_PRESS": 1,
+    "ACC_CUT_IN": fcw_alert,  # only shown when ACC enabled
   }
   return packer.make_can_msg("ACC_CONTROL", 0, values)
 
@@ -56,14 +57,14 @@ def create_acc_cancel_command(packer):
 
 def create_fcw_command(packer, fcw):
   values = {
-    "PCS_INDICATOR": 1,
+    "PCS_INDICATOR": 1,  # PCS turned off
     "FCW": fcw,
     "SET_ME_X20": 0x20,
     "SET_ME_X10": 0x10,
     "PCS_OFF": 1,
     "PCS_SENSITIVITY": 0,
   }
-  return packer.make_can_msg("ACC_HUD", 0, values)
+  return packer.make_can_msg("PCS_HUD", 0, values)
 
 
 def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled, stock_lkas_hud):
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index 58e3464e3c..4a1011982c 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -1,13 +1,14 @@
+import re
 from collections import defaultdict
 from dataclasses import dataclass, field
-from enum import Enum, IntFlag
-from typing import Dict, List, Union
+from enum import Enum, IntFlag, StrEnum
+from typing import Dict, List, Set, Union
 
 from cereal import car
-from common.conversions import Conversions as CV
-from selfdrive.car import AngleRateLimit, dbc_dict
-from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, CarParts, CarHarness
-from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
+from openpilot.common.conversions import Conversions as CV
+from openpilot.selfdrive.car import AngleRateLimit, dbc_dict
+from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, CarParts, CarHarness
+from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
 
 Ecu = car.CarParams.Ecu
 MIN_ACC_SPEED = 19. * CV.MPH_TO_MS
@@ -42,33 +43,25 @@ class CarControllerParams:
 class ToyotaFlags(IntFlag):
   HYBRID = 1
   SMART_DSU = 2
+  DISABLE_RADAR = 4
 
 
-class CAR:
+class CAR(StrEnum):
   # Toyota
   ALPHARD_TSS2 = "TOYOTA ALPHARD 2020"
-  ALPHARDH_TSS2 = "TOYOTA ALPHARD HYBRID 2021"
   AVALON = "TOYOTA AVALON 2016"
   AVALON_2019 = "TOYOTA AVALON 2019"
-  AVALONH_2019 = "TOYOTA AVALON HYBRID 2019"
   AVALON_TSS2 = "TOYOTA AVALON 2022"  # TSS 2.5
-  AVALONH_TSS2 = "TOYOTA AVALON HYBRID 2022"
   CAMRY = "TOYOTA CAMRY 2018"
-  CAMRYH = "TOYOTA CAMRY HYBRID 2018"
   CAMRY_TSS2 = "TOYOTA CAMRY 2021"  # TSS 2.5
-  CAMRYH_TSS2 = "TOYOTA CAMRY HYBRID 2021"
   CHR = "TOYOTA C-HR 2018"
   CHR_TSS2 = "TOYOTA C-HR 2021"
-  CHRH = "TOYOTA C-HR HYBRID 2018"
-  CHRH_TSS2 = "TOYOTA C-HR HYBRID 2022"
   COROLLA = "TOYOTA COROLLA 2017"
-  COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019"
   # LSS2 Lexus UX Hybrid is same as a TSS2 Corolla Hybrid
-  COROLLAH_TSS2 = "TOYOTA COROLLA HYBRID TSS2 2019"
+  COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019"
   HIGHLANDER = "TOYOTA HIGHLANDER 2017"
   HIGHLANDER_TSS2 = "TOYOTA HIGHLANDER 2020"
   HIGHLANDERH = "TOYOTA HIGHLANDER HYBRID 2018"
-  HIGHLANDERH_TSS2 = "TOYOTA HIGHLANDER HYBRID 2020"
   PRIUS = "TOYOTA PRIUS 2017"
   PRIUS_V = "TOYOTA PRIUS v 2017"
   PRIUS_TSS2 = "TOYOTA PRIUS TSS2 2021"
@@ -77,9 +70,6 @@ class CAR:
   RAV4_TSS2 = "TOYOTA RAV4 2019"
   RAV4_TSS2_2022 = "TOYOTA RAV4 2022"
   RAV4_TSS2_2023 = "TOYOTA RAV4 2023"
-  RAV4H_TSS2 = "TOYOTA RAV4 HYBRID 2019"
-  RAV4H_TSS2_2022 = "TOYOTA RAV4 HYBRID 2022"
-  RAV4H_TSS2_2023 = "TOYOTA RAV4 HYBRID 2023"
   MIRAI = "TOYOTA MIRAI 2021"  # TSS 2.5
   SIENNA = "TOYOTA SIENNA 2018"
 
@@ -88,17 +78,15 @@ class CAR:
   LEXUS_ES = "LEXUS ES 2018"
   LEXUS_ESH = "LEXUS ES HYBRID 2018"
   LEXUS_ES_TSS2 = "LEXUS ES 2019"
-  LEXUS_ESH_TSS2 = "LEXUS ES HYBRID 2019"
   LEXUS_IS = "LEXUS IS 2018"
+  LEXUS_IS_TSS2 = "LEXUS IS 2023"
   LEXUS_NX = "LEXUS NX 2018"
-  LEXUS_NXH = "LEXUS NX HYBRID 2018"
   LEXUS_NX_TSS2 = "LEXUS NX 2020"
-  LEXUS_NXH_TSS2 = "LEXUS NX HYBRID 2020"
   LEXUS_RC = "LEXUS RC 2020"
   LEXUS_RX = "LEXUS RX 2016"
   LEXUS_RXH = "LEXUS RX HYBRID 2017"
   LEXUS_RX_TSS2 = "LEXUS RX 2020"
-  LEXUS_RXH_TSS2 = "LEXUS RX HYBRID 2020"
+  LEXUS_GS_F = "LEXUS GS F 2016"
 
 
 class Footnote(Enum):
@@ -110,45 +98,60 @@ class Footnote(Enum):
 @dataclass
 class ToyotaCarInfo(CarInfo):
   package: str = "All"
-  car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.toyota]))
+  car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.toyota_a]))
 
 
 CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
   # Toyota
-  CAR.ALPHARD_TSS2: ToyotaCarInfo("Toyota Alphard 2019-20"),
-  CAR.ALPHARDH_TSS2: ToyotaCarInfo("Toyota Alphard Hybrid 2021"),
+  CAR.ALPHARD_TSS2: [
+    ToyotaCarInfo("Toyota Alphard 2019-20"),
+    ToyotaCarInfo("Toyota Alphard Hybrid 2021"),
+  ],
   CAR.AVALON: [
     ToyotaCarInfo("Toyota Avalon 2016", "Toyota Safety Sense P"),
     ToyotaCarInfo("Toyota Avalon 2017-18"),
   ],
-  CAR.AVALON_2019: ToyotaCarInfo("Toyota Avalon 2019-21"),
-  CAR.AVALONH_2019: ToyotaCarInfo("Toyota Avalon Hybrid 2019-21"),
-  CAR.AVALON_TSS2: ToyotaCarInfo("Toyota Avalon 2022"),
-  CAR.AVALONH_TSS2: ToyotaCarInfo("Toyota Avalon Hybrid 2022"),
-  CAR.CAMRY: ToyotaCarInfo("Toyota Camry 2018-20", video_link="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]),
-  CAR.CAMRYH: ToyotaCarInfo("Toyota Camry Hybrid 2018-20", video_link="https://www.youtube.com/watch?v=Q2DYY0AWKgk"),
-  CAR.CAMRY_TSS2: ToyotaCarInfo("Toyota Camry 2021-23", footnotes=[Footnote.CAMRY]),
-  CAR.CAMRYH_TSS2: ToyotaCarInfo("Toyota Camry Hybrid 2021-23"),
-  CAR.CHR: ToyotaCarInfo("Toyota C-HR 2017-20"),
-  CAR.CHR_TSS2: ToyotaCarInfo("Toyota C-HR 2021"),
-  CAR.CHRH: ToyotaCarInfo("Toyota C-HR Hybrid 2017-20"),
-  CAR.CHRH_TSS2: ToyotaCarInfo("Toyota C-HR Hybrid 2021-22"),
+  CAR.AVALON_2019: [
+    ToyotaCarInfo("Toyota Avalon 2019-21"),
+    ToyotaCarInfo("Toyota Avalon Hybrid 2019-21"),
+  ],
+  CAR.AVALON_TSS2: [
+    ToyotaCarInfo("Toyota Avalon 2022"),
+    ToyotaCarInfo("Toyota Avalon Hybrid 2022"),
+  ],
+  CAR.CAMRY: [
+    ToyotaCarInfo("Toyota Camry 2018-20", video_link="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]),
+    ToyotaCarInfo("Toyota Camry Hybrid 2018-20", video_link="https://www.youtube.com/watch?v=Q2DYY0AWKgk"),
+  ],
+  CAR.CAMRY_TSS2: [
+    ToyotaCarInfo("Toyota Camry 2021-24", footnotes=[Footnote.CAMRY]),
+    ToyotaCarInfo("Toyota Camry Hybrid 2021-24"),
+  ],
+  CAR.CHR: [
+    ToyotaCarInfo("Toyota C-HR 2017-20"),
+    ToyotaCarInfo("Toyota C-HR Hybrid 2017-20"),
+  ],
+  CAR.CHR_TSS2: [
+    ToyotaCarInfo("Toyota C-HR 2021"),
+    ToyotaCarInfo("Toyota C-HR Hybrid 2021-22"),
+  ],
   CAR.COROLLA: ToyotaCarInfo("Toyota Corolla 2017-19"),
   CAR.COROLLA_TSS2: [
     ToyotaCarInfo("Toyota Corolla 2020-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"),
     ToyotaCarInfo("Toyota Corolla Cross (Non-US only) 2020-23", min_enable_speed=7.5),
     ToyotaCarInfo("Toyota Corolla Hatchback 2019-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"),
-  ],
-  CAR.COROLLAH_TSS2: [
+    # Hybrid platforms
     ToyotaCarInfo("Toyota Corolla Hybrid 2020-22"),
     ToyotaCarInfo("Toyota Corolla Hybrid (Non-US only) 2020-23", min_enable_speed=7.5),
     ToyotaCarInfo("Toyota Corolla Cross Hybrid (Non-US only) 2020-22", min_enable_speed=7.5),
     ToyotaCarInfo("Lexus UX Hybrid 2019-23"),
   ],
   CAR.HIGHLANDER: ToyotaCarInfo("Toyota Highlander 2017-19", video_link="https://www.youtube.com/watch?v=0wS0wXSLzoo"),
-  CAR.HIGHLANDER_TSS2: ToyotaCarInfo("Toyota Highlander 2020-23"),
+  CAR.HIGHLANDER_TSS2: [
+    ToyotaCarInfo("Toyota Highlander 2020-23"),
+    ToyotaCarInfo("Toyota Highlander Hybrid 2020-23"),
+  ],
   CAR.HIGHLANDERH: ToyotaCarInfo("Toyota Highlander Hybrid 2017-19"),
-  CAR.HIGHLANDERH_TSS2: ToyotaCarInfo("Toyota Highlander Hybrid 2020-23"),
   CAR.PRIUS: [
     ToyotaCarInfo("Toyota Prius 2016", "Toyota Safety Sense P", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"),
     ToyotaCarInfo("Toyota Prius 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"),
@@ -167,12 +170,18 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
     ToyotaCarInfo("Toyota RAV4 Hybrid 2016", "Toyota Safety Sense P", video_link="https://youtu.be/LhT5VzJVfNI?t=26"),
     ToyotaCarInfo("Toyota RAV4 Hybrid 2017-18", video_link="https://youtu.be/LhT5VzJVfNI?t=26")
   ],
-  CAR.RAV4_TSS2: ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"),
-  CAR.RAV4_TSS2_2022: ToyotaCarInfo("Toyota RAV4 2022"),
-  CAR.RAV4_TSS2_2023: ToyotaCarInfo("Toyota RAV4 2023"),
-  CAR.RAV4H_TSS2: ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"),
-  CAR.RAV4H_TSS2_2022: ToyotaCarInfo("Toyota RAV4 Hybrid 2022", video_link="https://youtu.be/U0nH9cnrFB0"),
-  CAR.RAV4H_TSS2_2023: ToyotaCarInfo("Toyota RAV4 Hybrid 2023"),
+  CAR.RAV4_TSS2: [
+    ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"),
+    ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"),
+  ],
+  CAR.RAV4_TSS2_2022: [
+    ToyotaCarInfo("Toyota RAV4 2022"),
+    ToyotaCarInfo("Toyota RAV4 Hybrid 2022", video_link="https://youtu.be/U0nH9cnrFB0"),
+  ],
+  CAR.RAV4_TSS2_2023: [
+    ToyotaCarInfo("Toyota RAV4 2023"),
+    ToyotaCarInfo("Toyota RAV4 Hybrid 2023"),
+  ],
   CAR.MIRAI: ToyotaCarInfo("Toyota Mirai 2021"),
   CAR.SIENNA: ToyotaCarInfo("Toyota Sienna 2018-20", video_link="https://www.youtube.com/watch?v=q1UPOo4Sh68", min_enable_speed=MIN_ACC_SPEED),
 
@@ -180,13 +189,21 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
   CAR.LEXUS_CTH: ToyotaCarInfo("Lexus CT Hybrid 2017-18", "Lexus Safety System+"),
   CAR.LEXUS_ES: ToyotaCarInfo("Lexus ES 2017-18"),
   CAR.LEXUS_ESH: ToyotaCarInfo("Lexus ES Hybrid 2017-18"),
-  CAR.LEXUS_ES_TSS2: ToyotaCarInfo("Lexus ES 2019-22"),
-  CAR.LEXUS_ESH_TSS2: ToyotaCarInfo("Lexus ES Hybrid 2019-23", video_link="https://youtu.be/BZ29osRVJeg?t=12"),
+  CAR.LEXUS_ES_TSS2: [
+    ToyotaCarInfo("Lexus ES 2019-24"),
+    ToyotaCarInfo("Lexus ES Hybrid 2019-23", video_link="https://youtu.be/BZ29osRVJeg?t=12"),
+  ],
   CAR.LEXUS_IS: ToyotaCarInfo("Lexus IS 2017-19"),
-  CAR.LEXUS_NX: ToyotaCarInfo("Lexus NX 2018-19"),
-  CAR.LEXUS_NXH: ToyotaCarInfo("Lexus NX Hybrid 2018-19"),
-  CAR.LEXUS_NX_TSS2: ToyotaCarInfo("Lexus NX 2020-21"),
-  CAR.LEXUS_NXH_TSS2: ToyotaCarInfo("Lexus NX Hybrid 2020-21"),
+  CAR.LEXUS_IS_TSS2: ToyotaCarInfo("Lexus IS 2022-23"),
+  CAR.LEXUS_GS_F: ToyotaCarInfo("Lexus GS F 2016"),
+  CAR.LEXUS_NX: [
+    ToyotaCarInfo("Lexus NX 2018-19"),
+    ToyotaCarInfo("Lexus NX Hybrid 2018-19"),
+  ],
+  CAR.LEXUS_NX_TSS2: [
+    ToyotaCarInfo("Lexus NX 2020-21"),
+    ToyotaCarInfo("Lexus NX Hybrid 2020-21"),
+  ],
   CAR.LEXUS_RC: ToyotaCarInfo("Lexus RC 2018-20"),
   CAR.LEXUS_RX: [
     ToyotaCarInfo("Lexus RX 2016", "Lexus Safety System+"),
@@ -196,33 +213,152 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
     ToyotaCarInfo("Lexus RX Hybrid 2016", "Lexus Safety System+"),
     ToyotaCarInfo("Lexus RX Hybrid 2017-19"),
   ],
-  CAR.LEXUS_RX_TSS2: ToyotaCarInfo("Lexus RX 2020-22"),
-  CAR.LEXUS_RXH_TSS2: ToyotaCarInfo("Lexus RX Hybrid 2020-22"),
+  CAR.LEXUS_RX_TSS2: [
+    ToyotaCarInfo("Lexus RX 2020-22"),
+    ToyotaCarInfo("Lexus RX Hybrid 2020-22"),
+  ],
 }
 
 # (addr, cars, bus, 1/freq*100, vl)
 STATIC_DSU_MSGS = [
-  (0x128, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1,   3, b'\xf4\x01\x90\x83\x00\x37'),
+  (0x128, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1,   3, b'\xf4\x01\x90\x83\x00\x37'),
   (0x128, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH), 1,   3, b'\x03\x00\x20\x00\x00\x52'),
-  (0x141, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 1,   2, b'\x00\x00\x00\x46'),
-  (0x160, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 1,   7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'),
-  (0x161, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX, CAR.PRIUS_V, CAR.LEXUS_ES), 1,   7, b'\x00\x1e\x00\x00\x00\x80\x07'),
+  (0x141, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON,
+           CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 1,   2, b'\x00\x00\x00\x46'),
+  (0x160, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON,
+           CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 1,   7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'),
+  (0x161, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX, CAR.PRIUS_V, CAR.LEXUS_ES),
+                                                                                               1,   7, b'\x00\x1e\x00\x00\x00\x80\x07'),
   (0X161, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1,  7, b'\x00\x1e\x00\xd4\x00\x00\x5b'),
-  (0x283, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0,   3, b'\x00\x00\x00\x00\x00\x00\x8c'),
+  (0x283, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON,
+           CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0,   3, b'\x00\x00\x00\x00\x00\x00\x8c'),
   (0x2E6, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0,   3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
   (0x2E7, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0,   3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
   (0x33E, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0,  20, b'\x0f\xff\x26\x40\x00\x1f\x00'),
-  (0x344, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0,   5, b'\x00\x00\x01\x00\x00\x00\x00\x50'),
-  (0x365, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0,  20, b'\x00\x00\x00\x80\x03\x00\x08'),
-  (0x365, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0,  20, b'\x00\x00\x00\x80\xfc\x00\x08'),
-  (0x366, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0,  20, b'\x00\x00\x4d\x82\x40\x02\x00'),
-  (0x366, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0,  20, b'\x00\x72\x07\xff\x09\xfe\x00'),
+  (0x344, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON,
+           CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0,   5, b'\x00\x00\x01\x00\x00\x00\x00\x50'),
+  (0x365, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0,  20, b'\x00\x00\x00\x80\x03\x00\x08'),
+  (0x365, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX,
+           CAR.PRIUS_V), 0,  20, b'\x00\x00\x00\x80\xfc\x00\x08'),
+  (0x366, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0,  20, b'\x00\x00\x4d\x82\x40\x02\x00'),
+  (0x366, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V),
+          0,  20, b'\x00\x72\x07\xff\x09\xfe\x00'),
   (0x366, (CAR.LEXUS_ES,), 0,  20, b'\x00\x95\x07\xfe\x08\x05\x00'),
   (0x470, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'),
   (0x470, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.PRIUS_V), 1,  100, b'\x00\x00\x01\x79'),
-  (0x4CB, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'),
+  (0x4CB, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON,
+           CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'),
 ]
 
+
+def get_platform_codes(fw_versions: List[bytes]) -> Dict[bytes, Set[bytes]]:
+  # Returns sub versions in a dict so comparisons can be made within part-platform-major_version combos
+  codes = defaultdict(set)  # Optional[part]-platform-major_version: set of sub_version
+  for fw in fw_versions:
+    # FW versions returned from UDS queries can return multiple fields/chunks of data (different ECU calibrations, different data?)
+    #  and are prefixed with a byte that describes how many chunks of data there are.
+    # But FW returned from KWP requires querying of each sub-data id and does not have a length prefix.
+
+    length_code = 1
+    length_code_match = FW_LEN_CODE.search(fw)
+    if length_code_match is not None:
+      length_code = length_code_match.group()[0]
+      fw = fw[1:]
+
+    # fw length should be multiple of 16 bytes (per chunk, even if no length code), skip parsing if unexpected length
+    if length_code * FW_CHUNK_LEN != len(fw):
+      continue
+
+    chunks = [fw[FW_CHUNK_LEN * i:FW_CHUNK_LEN * i + FW_CHUNK_LEN].strip(b'\x00 ') for i in range(length_code)]
+
+    # only first is considered for now since second is commonly shared (TODO: understand that)
+    first_chunk = chunks[0]
+    if len(first_chunk) == 8:
+      # TODO: no part number, but some short chunks have it in subsequent chunks
+      fw_match = SHORT_FW_PATTERN.search(first_chunk)
+      if fw_match is not None:
+        platform, major_version, sub_version = fw_match.groups()
+        codes[b'-'.join((platform, major_version))].add(sub_version)
+
+    elif len(first_chunk) == 10:
+      fw_match = MEDIUM_FW_PATTERN.search(first_chunk)
+      if fw_match is not None:
+        part, platform, major_version, sub_version = fw_match.groups()
+        codes[b'-'.join((part, platform, major_version))].add(sub_version)
+
+    elif len(first_chunk) == 12:
+      fw_match = LONG_FW_PATTERN.search(first_chunk)
+      if fw_match is not None:
+        part, platform, major_version, sub_version = fw_match.groups()
+        codes[b'-'.join((part, platform, major_version))].add(sub_version)
+
+  return dict(codes)
+
+
+def match_fw_to_car_fuzzy(live_fw_versions) -> Set[str]:
+  candidates = set()
+
+  for candidate, fws in FW_VERSIONS.items():
+    # Keep track of ECUs which pass all checks (platform codes, within sub-version range)
+    valid_found_ecus = set()
+    valid_expected_ecus = {ecu[1:] for ecu in fws if ecu[0] in PLATFORM_CODE_ECUS}
+    for ecu, expected_versions in fws.items():
+      addr = ecu[1:]
+      # Only check ECUs expected to have platform codes
+      if ecu[0] not in PLATFORM_CODE_ECUS:
+        continue
+
+      # Expected platform codes & versions
+      expected_platform_codes = get_platform_codes(expected_versions)
+
+      # Found platform codes & versions
+      found_platform_codes = get_platform_codes(live_fw_versions.get(addr, set()))
+
+      # Check part number + platform code + major version matches for any found versions
+      # Platform codes and major versions change for different physical parts, generation, API, etc.
+      # Sub-versions are incremented for minor recalls, do not need to be checked.
+      if not any(found_platform_code in expected_platform_codes for found_platform_code in found_platform_codes):
+        break
+
+      valid_found_ecus.add(addr)
+
+    # If all live ECUs pass all checks for candidate, add it as a match
+    if valid_expected_ecus.issubset(valid_found_ecus):
+      candidates.add(candidate)
+
+  return {str(c) for c in (candidates - FUZZY_EXCLUDED_PLATFORMS)}
+
+
+# Regex patterns for parsing more general platform-specific identifiers from FW versions.
+# - Part number: Toyota part number (usually last character needs to be ignored to find a match).
+#    Each ECU address has just one part number.
+# - Platform: usually multiple codes per an openpilot platform, however this is the least variable and
+#    is usually shared across ECUs and model years signifying this describes something about the specific platform.
+#    This describes more generational changes (TSS-P vs TSS2), or manufacture region.
+# - Major version: second least variable part of the FW version. Seen splitting cars by model year/API such as
+#    RAV4 2022/2023 and Avalon. Used to differentiate cars where API has changed slightly, but is not a generational change.
+#    It is important to note that these aren't always consecutive, for example:
+#    Avalon 2016-18's fwdCamera has these major versions: 01, 03 while 2019 has: 02
+# - Sub version: exclusive to major version, but shared with other cars. Should only be used for further filtering.
+#    Seen bumped in TSB FW updates, and describes other minor differences.
+SHORT_FW_PATTERN = re.compile(b'[A-Z0-9](?P<platform>[A-Z0-9]{2})(?P<major_version>[A-Z0-9]{2})(?P<sub_version>[A-Z0-9]{3})')
+MEDIUM_FW_PATTERN = re.compile(b'(?P<part>[A-Z0-9]{5})(?P<platform>[A-Z0-9]{2})(?P<major_version>[A-Z0-9]{1})(?P<sub_version>[A-Z0-9]{2})')
+LONG_FW_PATTERN = re.compile(b'(?P<part>[A-Z0-9]{5})(?P<platform>[A-Z0-9]{2})(?P<major_version>[A-Z0-9]{2})(?P<sub_version>[A-Z0-9]{3})')
+FW_LEN_CODE = re.compile(b'^[\x01-\x03]')  # highest seen is 3 chunks, 16 bytes each
+FW_CHUNK_LEN = 16
+
+# List of ECUs that are most unique across openpilot platforms
+# TODO: use hybrid ECU, splits similar ICE and hybrid variants
+# - fwdCamera: describes actual features related to ADAS. For example, on the Avalon it describes
+#    when TSS-P became standard, whether the car supports stop and go, and whether it's TSS2.
+#    On the RAV4, it describes the move to the radar doing ACC, and the use of LTA for lane keeping.
+# - abs: differentiates hybrid/ICE on most cars (Corolla TSS2 is an exception)
+# - eps: describes lateral API changes for the EPS, such as using LTA for lane keeping and rejecting LKA messages
+PLATFORM_CODE_ECUS = [Ecu.fwdCamera, Ecu.abs, Ecu.eps]
+
+# These platforms have at least one platform code for all ECUs shared with another platform.
+FUZZY_EXCLUDED_PLATFORMS: set[CAR] = set()
+
 # Some ECUs that use KWP2000 have their FW versions on non-standard data identifiers.
 # Toyota diagnostic software first gets the supported data ids, then queries them one by one.
 # For example, sends: 0x1a8800, receives: 0x1a8800010203, queries: 0x1a8801, 0x1a8802, 0x1a8803
@@ -236,7 +372,7 @@ FW_QUERY_CONFIG = FwQueryConfig(
       [StdQueries.SHORT_TESTER_PRESENT_REQUEST, TOYOTA_VERSION_REQUEST_KWP],
       [StdQueries.SHORT_TESTER_PRESENT_RESPONSE, TOYOTA_VERSION_RESPONSE_KWP],
       whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.dsu, Ecu.abs, Ecu.eps, Ecu.epb, Ecu.telematics,
-                      Ecu.hybrid, Ecu.srs, Ecu.combinationMeter, Ecu.transmission, Ecu.gateway, Ecu.hvac],
+                      Ecu.srs, Ecu.combinationMeter, Ecu.transmission, Ecu.gateway, Ecu.hvac],
       bus=0,
     ),
     Request(
@@ -256,9 +392,10 @@ FW_QUERY_CONFIG = FwQueryConfig(
   ],
   non_essential_ecus={
     # FIXME: On some models, abs can sometimes be missing
-    Ecu.abs: [CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_IS],
+    Ecu.abs: [CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_IS, CAR.ALPHARD_TSS2],
     # On some models, the engine can show on two different addresses
-    Ecu.engine: [CAR.CAMRY, CAR.COROLLA_TSS2, CAR.CHR, CAR.CHR_TSS2, CAR.LEXUS_IS, CAR.LEXUS_RC],
+    Ecu.engine: [CAR.CAMRY, CAR.COROLLA_TSS2, CAR.CHR, CAR.CHR_TSS2, CAR.LEXUS_IS, CAR.LEXUS_RC,
+                 CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2, CAR.LEXUS_RX_TSS2],
   },
   extra_ecus=[
     # All known ECUs on a late-model Toyota vehicle not queried here:
@@ -292,6 +429,7 @@ FW_QUERY_CONFIG = FwQueryConfig(
     (Ecu.combinationMeter, 0x7c0, None),
     (Ecu.hvac, 0x7c4, None),
   ],
+  match_fw_to_car_fuzzy=match_fw_to_car_fuzzy,
 )
 
 FW_VERSIONS = {
@@ -326,44 +464,26 @@ FW_VERSIONS = {
       b'F152607171\x00\x00\x00\x00\x00\x00',
       b'F152607110\x00\x00\x00\x00\x00\x00',
       b'F152607180\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.dsu, 0x791, None): [
-      b'881510703200\x00\x00\x00\x00',
-    ],
-    (Ecu.eps, 0x7a1, None): [
-      b'8965B41080\x00\x00\x00\x00\x00\x00',
-      b'8965B07010\x00\x00\x00\x00\x00\x00',
-      b'8965B41090\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.engine, 0x700, None): [
-      b'\x01896630725200\x00\x00\x00\x00',
-      b'\x01896630725300\x00\x00\x00\x00',
-      b'\x01896630735100\x00\x00\x00\x00',
-      b'\x01896630738000\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdRadar, 0x750, 0xf): [
-      b'8821F4702300\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdCamera, 0x750, 0x6d): [
-      b'8646F0702100\x00\x00\x00\x00',
-    ],
-  },
-  CAR.AVALONH_2019: {
-    (Ecu.abs, 0x7b0, None): [
       b'F152641040\x00\x00\x00\x00\x00\x00',
       b'F152641061\x00\x00\x00\x00\x00\x00',
       b'F152641050\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.dsu, 0x791, None): [
+      b'881510703200\x00\x00\x00\x00',
       b'881510704200\x00\x00\x00\x00',
       b'881514107100\x00\x00\x00\x00',
     ],
     (Ecu.eps, 0x7a1, None): [
+      b'8965B41080\x00\x00\x00\x00\x00\x00',
       b'8965B07010\x00\x00\x00\x00\x00\x00',
       b'8965B41090\x00\x00\x00\x00\x00\x00',
       b'8965B41070\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x700, None): [
+      b'\x01896630725200\x00\x00\x00\x00',
+      b'\x01896630725300\x00\x00\x00\x00',
+      b'\x01896630735100\x00\x00\x00\x00',
+      b'\x01896630738000\x00\x00\x00\x00',
       b'\x02896630724000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
       b'\x02896630737000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
       b'\x02896630728000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
@@ -380,6 +500,7 @@ FW_VERSIONS = {
       b'\x01F152607240\x00\x00\x00\x00\x00\x00',
       b'\x01F152607250\x00\x00\x00\x00\x00\x00',
       b'\x01F152607280\x00\x00\x00\x00\x00\x00',
+      b'F152641080\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.eps, 0x7a1, None): [
       b'8965B41110\x00\x00\x00\x00\x00\x00',
@@ -387,24 +508,6 @@ FW_VERSIONS = {
     (Ecu.engine, 0x700, None): [
       b'\x01896630742000\x00\x00\x00\x00',
       b'\x01896630743000\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdRadar, 0x750, 0xf): [
-      b'\x018821F6201200\x00\x00\x00\x00',
-      b'\x018821F6201300\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdCamera, 0x750, 0x6d): [
-      b'\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
-      b'\x028646F4104100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
-    ],
-  },
-  CAR.AVALONH_TSS2: {
-    (Ecu.abs, 0x7b0, None): [
-      b'F152641080\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.eps, 0x7a1, None): [
-      b'8965B41110\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.engine, 0x700, None): [
       b'\x018966306Q6000\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x750, 0xf): [
@@ -440,6 +543,27 @@ FW_VERSIONS = {
       b'\x018966333Q6300\x00\x00\x00\x00',
       b'\x018966333Q6500\x00\x00\x00\x00',
       b'\x018966333W6000\x00\x00\x00\x00',
+      b'\x018966306Q6000\x00\x00\x00\x00',
+      b'\x018966333N1100\x00\x00\x00\x00',
+      b'\x018966333N4300\x00\x00\x00\x00',
+      b'\x018966333X0000\x00\x00\x00\x00',
+      b'\x018966333X4000\x00\x00\x00\x00',
+      b'\x01896633T16000\x00\x00\x00\x00',
+      b'\x018966306L9000\x00\x00\x00\x00',
+      b'\x028966306B2100\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
+      b'\x028966306B2300\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
+      b'\x028966306B2500\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
+      b'\x028966306N8100\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
+      b'\x028966306N8200\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
+      b'\x028966306N8300\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
+      b'\x028966306N8400\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
+      b'\x028966306R5000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
+      b'\x028966306R5000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
+      b'\x028966306R6000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
+      b'\x028966306R6000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
+      b'\x028966306S0000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
+      b'\x028966306S0100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
+      b'\x028966306S1100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\x02333P1100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -455,6 +579,11 @@ FW_VERSIONS = {
       b'8821F0608000    ',
       b'8821F0608200    ',
       b'8821F0609100    ',
+      b'8821F0603400    ',
+      b'8821F0604000    ',
+      b'8821F0604200    ',
+      b'8821F0606200    ',
+      b'8821F0609000    ',
     ],
     (Ecu.abs, 0x7b0, None): [
       b'F152606210\x00\x00\x00\x00\x00\x00',
@@ -465,6 +594,12 @@ FW_VERSIONS = {
       b'F152633540\x00\x00\x00\x00\x00\x00',
       b'F152633A10\x00\x00\x00\x00\x00\x00',
       b'F152633A20\x00\x00\x00\x00\x00\x00',
+      b'F152633214\x00\x00\x00\x00\x00\x00',
+      b'F152633660\x00\x00\x00\x00\x00\x00',
+      b'F152633712\x00\x00\x00\x00\x00\x00',
+      b'F152633713\x00\x00\x00\x00\x00\x00',
+      b'F152633B51\x00\x00\x00\x00\x00\x00',
+      b'F152633B60\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.eps, 0x7a1, None): [
       b'8965B33540\x00\x00\x00\x00\x00\x00',
@@ -472,6 +607,9 @@ FW_VERSIONS = {
       b'8965B33580\x00\x00\x00\x00\x00\x00',
       b'8965B33581\x00\x00\x00\x00\x00\x00',
       b'8965B33621\x00\x00\x00\x00\x00\x00',
+      b'8965B33550\x00\x00\x00\x00\x00\x00',
+      b'8965B33551\x00\x00\x00\x00\x00\x00',
+      b'8965B33611\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x750, 0xf): [  # Same as 0x791
       b'8821F0601200    ',
@@ -484,114 +622,40 @@ FW_VERSIONS = {
       b'8821F0608000    ',
       b'8821F0608200    ',
       b'8821F0609100    ',
-    ],
-    (Ecu.fwdCamera, 0x750, 0x6d): [
-      b'8646F0601200    ',
-      b'8646F0601300    ',
-      b'8646F0601400    ',
-      b'8646F0603400    ',
-      b'8646F0604100    ',
-      b'8646F0605000    ',
-      b'8646F0606000    ',
-      b'8646F0606100    ',
-      b'8646F0607100    ',
-    ],
-  },
-  CAR.CAMRYH: {
-    (Ecu.engine, 0x700, None): [
-      b'\x018966306Q6000\x00\x00\x00\x00',
-      b'\x018966333N1100\x00\x00\x00\x00',
-      b'\x018966333N4300\x00\x00\x00\x00',
-      b'\x018966333X0000\x00\x00\x00\x00',
-      b'\x018966333X4000\x00\x00\x00\x00',
-      b'\x01896633T16000\x00\x00\x00\x00',
-      b'\x028966306B2100\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
-      b'\x028966306B2300\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
-      b'\x028966306B2500\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
-      b'\x028966306N8100\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
-      b'\x028966306N8200\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
-      b'\x028966306N8300\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
-      b'\x028966306N8400\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
-      b'\x028966306R5000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
-      b'\x028966306R5000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
-      b'\x028966306R6000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
-      b'\x028966306R6000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
-      b'\x028966306S0000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
-      b'\x028966306S0100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
-      b'\x028966306S1100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
-    ],
-    (Ecu.abs, 0x7b0, None): [
-      b'F152633214\x00\x00\x00\x00\x00\x00',
-      b'F152633660\x00\x00\x00\x00\x00\x00',
-      b'F152633712\x00\x00\x00\x00\x00\x00',
-      b'F152633713\x00\x00\x00\x00\x00\x00',
-      b'F152633B51\x00\x00\x00\x00\x00\x00',
-      b'F152633B60\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.dsu, 0x791, None): [
-      b'8821F0601200    ',
-      b'8821F0601300    ',
-      b'8821F0603400    ',
-      b'8821F0604000    ',
-      b'8821F0604100    ',
-      b'8821F0604200    ',
-      b'8821F0605200    ',
-      b'8821F0606200    ',
-      b'8821F0607200    ',
-      b'8821F0608000    ',
-      b'8821F0608200    ',
-      b'8821F0609000    ',
-      b'8821F0609100    ',
-    ],
-    (Ecu.eps, 0x7a1, None): [
-      b'8965B33540\x00\x00\x00\x00\x00\x00',
-      b'8965B33542\x00\x00\x00\x00\x00\x00',
-      b'8965B33550\x00\x00\x00\x00\x00\x00',
-      b'8965B33551\x00\x00\x00\x00\x00\x00',
-      b'8965B33580\x00\x00\x00\x00\x00\x00',
-      b'8965B33581\x00\x00\x00\x00\x00\x00',
-      b'8965B33611\x00\x00\x00\x00\x00\x00',
-      b'8965B33621\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdRadar, 0x750, 0xf): [  # Same as 0x791
-      b'8821F0601200    ',
-      b'8821F0601300    ',
       b'8821F0603400    ',
       b'8821F0604000    ',
-      b'8821F0604100    ',
       b'8821F0604200    ',
-      b'8821F0605200    ',
       b'8821F0606200    ',
-      b'8821F0607200    ',
-      b'8821F0608000    ',
-      b'8821F0608200    ',
       b'8821F0609000    ',
-      b'8821F0609100    ',
     ],
     (Ecu.fwdCamera, 0x750, 0x6d): [
       b'8646F0601200    ',
       b'8646F0601300    ',
       b'8646F0601400    ',
       b'8646F0603400    ',
-      b'8646F0603500    ',
       b'8646F0604100    ',
       b'8646F0605000    ',
       b'8646F0606000    ',
       b'8646F0606100    ',
-      b'8646F0607000    ',
       b'8646F0607100    ',
+      b'8646F0603500    ',
+      b'8646F0607000    ',
     ],
   },
   CAR.CAMRY_TSS2: {
     (Ecu.eps, 0x7a1, None): [
       b'8965B33630\x00\x00\x00\x00\x00\x00',
       b'8965B33640\x00\x00\x00\x00\x00\x00',
+      b'8965B33650\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.abs, 0x7b0, None): [
       b'\x01F152606370\x00\x00\x00\x00\x00\x00',
       b'\x01F152606390\x00\x00\x00\x00\x00\x00',
       b'\x01F152606400\x00\x00\x00\x00\x00\x00',
       b'\x01F152606431\x00\x00\x00\x00\x00\x00',
+      b'F152633D00\x00\x00\x00\x00\x00\x00',
+      b'F152633D60\x00\x00\x00\x00\x00\x00',
+      b'F152633310\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x700, None): [
       b'\x018966306Q5000\x00\x00\x00\x00',
@@ -602,10 +666,16 @@ FW_VERSIONS = {
       b'\x018966306T3200\x00\x00\x00\x00',
       b'\x018966306T4000\x00\x00\x00\x00',
       b'\x018966306T4100\x00\x00\x00\x00',
+      b'\x018966306Q6000\x00\x00\x00\x00',
+      b'\x018966306Q7000\x00\x00\x00\x00',
+      b'\x018966306T0000\x00\x00\x00\x00',
+      b'\x018966306V1000\x00\x00\x00\x00',
+      b'\x01896633T20000\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x750, 0xf): [
       b'\x018821F6201200\x00\x00\x00\x00',
       b'\x018821F6201300\x00\x00\x00\x00',
+      b'\x018821F6201400\x00\x00\x00\x00',
     ],
     (Ecu.fwdCamera, 0x750, 0x6d): [
       b'\x028646F0602100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
@@ -615,33 +685,7 @@ FW_VERSIONS = {
       b'\x028646F3305200\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
       b'\x028646F3305300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
       b'\x028646F3305500\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
-    ],
-  },
-  CAR.CAMRYH_TSS2: {
-    (Ecu.eps, 0x7a1, None): [
-      b'8965B33630\x00\x00\x00\x00\x00\x00',
-      b'8965B33650\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.abs, 0x7b0, None): [
-      b'F152633D00\x00\x00\x00\x00\x00\x00',
-      b'F152633D60\x00\x00\x00\x00\x00\x00',
-      b'F152633310\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.engine, 0x700, None): [
-      b'\x018966306Q6000\x00\x00\x00\x00',
-      b'\x018966306Q7000\x00\x00\x00\x00',
-      b'\x018966306V1000\x00\x00\x00\x00',
-      b'\x01896633T20000\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdRadar, 0x750, 15): [
-      b'\x018821F6201200\x00\x00\x00\x00',
-      b'\x018821F6201300\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdCamera, 0x750, 109): [
-      b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
-      b'\x028646F3305300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
       b'\x028646F3305300\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
-      b'\x028646F3305500\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
     ],
   },
   CAR.CHR: {
@@ -651,6 +695,13 @@ FW_VERSIONS = {
       b'\x01896631017200\x00\x00\x00\x00',
       b'\x0189663F413100\x00\x00\x00\x00',
       b'\x0189663F414100\x00\x00\x00\x00',
+      b'\x0289663F405100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x02896631013200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x0289663F405000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x0289663F418000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x0289663F423000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x0289663F431000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x0189663F438000\x00\x00\x00\x00',
     ],
     (Ecu.dsu, 0x791, None): [
       b'8821F0W01000    ',
@@ -661,6 +712,9 @@ FW_VERSIONS = {
       b'8821FF405100    ',
       b'8821FF406000    ',
       b'8821FF407100    ',
+      b'8821FF402300    ',
+      b'8821FF402400    ',
+      b'8821FF405000    ',
     ],
     (Ecu.abs, 0x7b0, None): [
       b'F152610020\x00\x00\x00\x00\x00\x00',
@@ -671,11 +725,21 @@ FW_VERSIONS = {
       b'F1526F4073\x00\x00\x00\x00\x00\x00',
       b'F1526F4121\x00\x00\x00\x00\x00\x00',
       b'F1526F4122\x00\x00\x00\x00\x00\x00',
+      b'F152610012\x00\x00\x00\x00\x00\x00',
+      b'F152610013\x00\x00\x00\x00\x00\x00',
+      b'F152610014\x00\x00\x00\x00\x00\x00',
+      b'F152610040\x00\x00\x00\x00\x00\x00',
+      b'F152610190\x00\x00\x00\x00\x00\x00',
+      b'F152610200\x00\x00\x00\x00\x00\x00',
+      b'F152610220\x00\x00\x00\x00\x00\x00',
+      b'F152610230\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.eps, 0x7a1, None): [
       b'8965B10011\x00\x00\x00\x00\x00\x00',
       b'8965B10040\x00\x00\x00\x00\x00\x00',
       b'8965B10070\x00\x00\x00\x00\x00\x00',
+      b'8965B10020\x00\x00\x00\x00\x00\x00',
+      b'8965B10050\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\x0331024000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00',
@@ -695,26 +759,37 @@ FW_VERSIONS = {
       b'8821FF406000    ',
       b'8821FF407100    ',
       b'8821F0W01100    ',
-    ],
-    (Ecu.fwdCamera, 0x750, 0x6d): [
+      b'8821FF402300    ',
+      b'8821FF402400    ',
+      b'8821FF405000    ',
+    ],
+    (Ecu.fwdCamera, 0x750, 0x6d): [
       b'8646FF401700    ',
       b'8646FF401800    ',
       b'8646FF404000    ',
       b'8646FF406000    ',
       b'8646FF407000    ',
+      b'8646FF402100    ',
+      b'8646FF407100    ',
     ],
   },
   CAR.CHR_TSS2: {
     (Ecu.abs, 0x7b0, None): [
       b'F152610260\x00\x00\x00\x00\x00\x00',
       b'F1526F4270\x00\x00\x00\x00\x00\x00',
+      b'F152610041\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.eps, 0x7a1, None): [
       b'8965B10091\x00\x00\x00\x00\x00\x00',
       b'8965B10110\x00\x00\x00\x00\x00\x00',
+      b'8965B10092\x00\x00\x00\x00\x00\x00',
+      b'8965B10111\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x700, None): [
       b'\x0189663F459000\x00\x00\x00\x00',
+      b'\x0189663F438000\x00\x00\x00\x00',
+      b'\x02896631025000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x0289663F453000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\x0331014000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00',
@@ -722,87 +797,12 @@ FW_VERSIONS = {
     (Ecu.fwdRadar, 0x750, 0xf): [
       b'\x018821FF410200\x00\x00\x00\x00',
       b'\x018821FF410300\x00\x00\x00\x00',
+      b'\x018821FF410500\x00\x00\x00\x00',
     ],
     (Ecu.fwdCamera, 0x750, 0x6d): [
       b'\x028646FF410200\x00\x00\x00\x008646GF408200\x00\x00\x00\x00',
       b'\x028646FF411100\x00\x00\x00\x008646GF409000\x00\x00\x00\x00',
-    ],
-  },
-  CAR.CHRH: {
-    (Ecu.engine, 0x700, None): [
-      b'\x0289663F405100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x02896631013200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x0289663F405000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x0289663F418000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x0289663F423000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x0289663F431000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x0189663F438000\x00\x00\x00\x00',
-    ],
-    (Ecu.abs, 0x7b0, None): [
-      b'F152610012\x00\x00\x00\x00\x00\x00',
-      b'F152610013\x00\x00\x00\x00\x00\x00',
-      b'F152610014\x00\x00\x00\x00\x00\x00',
-      b'F152610040\x00\x00\x00\x00\x00\x00',
-      b'F152610190\x00\x00\x00\x00\x00\x00',
-      b'F152610200\x00\x00\x00\x00\x00\x00',
-      b'F152610230\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.dsu, 0x791, None): [
-      b'8821F0W01000    ',
-      b'8821FF402300    ',
-      b'8821FF402400    ',
-      b'8821FF404000    ',
-      b'8821FF404100    ',
-      b'8821FF405000    ',
-      b'8821FF406000    ',
-      b'8821FF407100    ',
-    ],
-    (Ecu.eps, 0x7a1, None): [
-      b'8965B10011\x00\x00\x00\x00\x00\x00',
-      b'8965B10020\x00\x00\x00\x00\x00\x00',
-      b'8965B10040\x00\x00\x00\x00\x00\x00',
-      b'8965B10050\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdRadar, 0x750, 0xf): [
-      b'8821F0W01000    ',
-      b'8821FF402300    ',
-      b'8821FF402400    ',
-      b'8821FF404000    ',
-      b'8821FF404100    ',
-      b'8821FF405000    ',
-      b'8821FF406000    ',
-      b'8821FF407100    ',
-    ],
-    (Ecu.fwdCamera, 0x750, 0x6d): [
-      b'8646FF401700    ',
-      b'8646FF402100    ',
-      b'8646FF404000    ',
-      b'8646FF406000    ',
-      b'8646FF407000    ',
-      b'8646FF407100    ',
-    ],
-  },
-  CAR.CHRH_TSS2: {
-    (Ecu.eps, 0x7a1, None): [
-      b'8965B10092\x00\x00\x00\x00\x00\x00',
-      b'8965B10091\x00\x00\x00\x00\x00\x00',
-      b'8965B10111\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.abs, 0x7b0, None): [
-      b'F152610041\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.engine, 0x700, None): [
-      b'\x0189663F438000\x00\x00\x00\x00',
-      b'\x02896631025000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x0289663F453000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdRadar, 0x750, 15): [
-      b'\x018821FF410500\x00\x00\x00\x00',
-      b'\x018821FF410300\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdCamera, 0x750, 109): [
       b'\x028646FF413100\x00\x00\x00\x008646GF411100\x00\x00\x00\x00',
-      b'\x028646FF411100\x00\x00\x00\x008646GF409000\x00\x00\x00\x00',
     ],
   },
   CAR.COROLLA: {
@@ -872,11 +872,39 @@ FW_VERSIONS = {
       b'\x018966312W3000\x00\x00\x00\x00',
       b'\x018966312W9000\x00\x00\x00\x00',
       b'\x01896637644000\x00\x00\x00\x00',
+      b'\x01896630ZJ1000\x00\x00\x00\x00',
+      b'\x01896630ZU8000\x00\x00\x00\x00',
+      b'\x01896637621000\x00\x00\x00\x00',
+      b'\x01896637623000\x00\x00\x00\x00',
+      b'\x01896637624000\x00\x00\x00\x00',
+      b'\x01896637626000\x00\x00\x00\x00',
+      b'\x01896637639000\x00\x00\x00\x00',
+      b'\x01896637648000\x00\x00\x00\x00',
+      b'\x01896637643000\x00\x00\x00\x00',
+      b'\x02896630A07000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x02896630A21000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x02896630ZJ5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x02896630ZK8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x02896630ZN8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x02896630ZQ3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x02896630ZR2000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x02896630ZT8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x02896630ZT9000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x02896630ZZ0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x028966312K6000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x028966312L0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x028966312Q3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x028966312Q3100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x028966312Q4000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x038966312L7000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1205001\x00\x00\x00\x00',
+      b'\x038966312N1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
+      b'\x038966312T3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1205001\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\x0230A10000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\x0230A11000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\x0230ZN4000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\x0230ZN5000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\x03312K7000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00',
       b'\x03312M3000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00',
       b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00',
@@ -903,13 +931,18 @@ FW_VERSIONS = {
       b'8965B76012\x00\x00\x00\x00\x00\x00',
       b'\x018965B12510\x00\x00\x00\x00\x00\x00',
       b'\x018965B1256000\x00\x00\x00\x00',
+      b'8965B12451\x00\x00\x00\x00\x00\x00',
+      b'8965B16101\x00\x00\x00\x00\x00\x00',
+      b'8965B16170\x00\x00\x00\x00\x00\x00',
+      b'8965B76050\x00\x00\x00\x00\x00\x00',
+      b'8965B76091\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.abs, 0x7b0, None): [
       b'\x01F152602280\x00\x00\x00\x00\x00\x00',
       b'\x01F152602560\x00\x00\x00\x00\x00\x00',
       b'\x01F152602590\x00\x00\x00\x00\x00\x00',
       b'\x01F152602650\x00\x00\x00\x00\x00\x00',
-      b"\x01F15260A010\x00\x00\x00\x00\x00\x00",
+      b'\x01F15260A010\x00\x00\x00\x00\x00\x00',
       b'\x01F15260A050\x00\x00\x00\x00\x00\x00',
       b'\x01F152612641\x00\x00\x00\x00\x00\x00',
       b'\x01F152612651\x00\x00\x00\x00\x00\x00',
@@ -927,74 +960,7 @@ FW_VERSIONS = {
       b'\x01F152612B91\x00\x00\x00\x00\x00\x00',
       b'\x01F15260A070\x00\x00\x00\x00\x00\x00',
       b'\x01F152676250\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdRadar, 0x750, 0xf): [
-      b'\x018821F3301100\x00\x00\x00\x00',
-      b'\x018821F3301200\x00\x00\x00\x00',
-      b'\x018821F3301300\x00\x00\x00\x00',
-      b'\x018821F3301400\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdCamera, 0x750, 0x6d): [
-      b'\x028646F12010D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
-      b'\x028646F1201100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
-      b'\x028646F1201200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
-      b'\x028646F1201300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
-      b'\x028646F1201400\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
-      b'\x028646F1202000\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
-      b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
-      b'\x028646F1202200\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
-      b'\x028646F1601100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
-      b'\x028646F1601300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
-    ],
-  },
-  CAR.COROLLAH_TSS2: {
-    (Ecu.engine, 0x700, None): [
-      b'\x01896630ZJ1000\x00\x00\x00\x00',
-      b'\x01896630ZU8000\x00\x00\x00\x00',
-      b'\x01896637621000\x00\x00\x00\x00',
-      b'\x01896637623000\x00\x00\x00\x00',
-      b'\x01896637624000\x00\x00\x00\x00',
-      b'\x01896637626000\x00\x00\x00\x00',
-      b'\x01896637639000\x00\x00\x00\x00',
-      b'\x01896637648000\x00\x00\x00\x00',
-      b'\x01896637643000\x00\x00\x00\x00',
-      b'\x02896630A07000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x02896630A21000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x02896630ZJ5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x02896630ZK8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x02896630ZN8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x02896630ZQ3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x02896630ZR2000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x02896630ZT8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x02896630ZT9000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x02896630ZZ0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x028966312K6000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x028966312L0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x028966312Q3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x028966312Q3100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x028966312Q4000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
-      b'\x038966312L7000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1205001\x00\x00\x00\x00',
-      b'\x038966312N1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
-      b'\x038966312T3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1205001\x00\x00\x00\x00',
-    ],
-    (Ecu.eps, 0x7a1, None): [
-      b'8965B12361\x00\x00\x00\x00\x00\x00',
-      b'8965B12451\x00\x00\x00\x00\x00\x00',
-      b'8965B16011\x00\x00\x00\x00\x00\x00',
-      b'8965B16101\x00\x00\x00\x00\x00\x00',
-      b'8965B16170\x00\x00\x00\x00\x00\x00',
-      b'8965B76012\x00\x00\x00\x00\x00\x00',
-      b'8965B76050\x00\x00\x00\x00\x00\x00',
-      b'8965B76091\x00\x00\x00\x00\x00\x00',
-      b'\x018965B12350\x00\x00\x00\x00\x00\x00',
-      b'\x018965B12470\x00\x00\x00\x00\x00\x00',
-      b'\x018965B12490\x00\x00\x00\x00\x00\x00',
-      b'\x018965B12500\x00\x00\x00\x00\x00\x00',
-      b'\x018965B12510\x00\x00\x00\x00\x00\x00',
-      b'\x018965B12520\x00\x00\x00\x00\x00\x00',
-      b'\x018965B12530\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.abs, 0x7b0, None): [
+      b'\x01F152602470\x00\x00\x00\x00\x00\x00',
       b'F152612590\x00\x00\x00\x00\x00\x00',
       b'F152612691\x00\x00\x00\x00\x00\x00',
       b'F152612692\x00\x00\x00\x00\x00\x00',
@@ -1028,14 +994,15 @@ FW_VERSIONS = {
     (Ecu.fwdCamera, 0x750, 0x6d): [
       b'\x028646F12010D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
       b'\x028646F1201100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
+      b'\x028646F1201200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
       b'\x028646F1201300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
       b'\x028646F1201400\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
       b'\x028646F1202000\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
       b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
       b'\x028646F1202200\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
       b'\x028646F1601100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
+      b'\x028646F1601300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
       b'\x028646F1601200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
-      b"\x028646F1601300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00",
       b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
       b'\x028646F76020C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
       b'\x028646F7603100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
@@ -1121,6 +1088,18 @@ FW_VERSIONS = {
       b'\x01F15260E110\x00\x00\x00\x00\x00\x00',
       b'\x01F15260E170\x00\x00\x00\x00\x00\x00',
       b'\x01F15260E05300\x00\x00\x00\x00',
+      b'\x01F15264872300\x00\x00\x00\x00',
+      b'\x01F15264872400\x00\x00\x00\x00',
+      b'\x01F15264872500\x00\x00\x00\x00',
+      b'\x01F15264872600\x00\x00\x00\x00',
+      b'\x01F15264872700\x00\x00\x00\x00',
+      b'\x01F15264873500\x00\x00\x00\x00',
+      b'\x01F152648C6300\x00\x00\x00\x00',
+      b'\x01F152648J4000\x00\x00\x00\x00',
+      b'\x01F152648J5000\x00\x00\x00\x00',
+      b'\x01F152648J6000\x00\x00\x00\x00',
+      b'\x01F152648J7000\x00\x00\x00\x00',
+      b'\x01F152648L5000\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x700, None): [
       b'\x01896630E62100\x00\x00\x00\x00',
@@ -1141,44 +1120,13 @@ FW_VERSIONS = {
       b'\x01896630EE1100\x00\x00\x00\x00',
       b'\x01896630EG3000\x00\x00\x00\x00',
       b'\x01896630EG5000\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdRadar, 0x750, 0xf): [
-      b'\x018821F3301400\x00\x00\x00\x00',
-      b'\x018821F6201200\x00\x00\x00\x00',
-      b'\x018821F6201300\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdCamera, 0x750, 0x6d): [
-      b'\x028646F0E02100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
-      b'\x028646F4803000\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
-      b'\x028646F4803000\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
-      b'\x028646F4803200\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
-    ],
-  },
-  CAR.HIGHLANDERH_TSS2: {
-    (Ecu.eps, 0x7a1, None): [
-      b'8965B48241\x00\x00\x00\x00\x00\x00',
-      b'8965B48310\x00\x00\x00\x00\x00\x00',
-      b'8965B48400\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.abs, 0x7b0, None): [
-      b'\x01F15264872300\x00\x00\x00\x00',
-      b'\x01F15264872400\x00\x00\x00\x00',
-      b'\x01F15264872500\x00\x00\x00\x00',
-      b'\x01F15264872600\x00\x00\x00\x00',
-      b'\x01F15264873500\x00\x00\x00\x00',
-      b'\x01F152648C6300\x00\x00\x00\x00',
-      b'\x01F152648J4000\x00\x00\x00\x00',
-      b'\x01F152648J5000\x00\x00\x00\x00',
-      b'\x01F152648J6000\x00\x00\x00\x00',
-      b'\x01F15264872700\x00\x00\x00\x00',
-    ],
-    (Ecu.engine, 0x700, None): [
       b'\x01896630E67000\x00\x00\x00\x00',
       b'\x01896630EA1000\x00\x00\x00\x00',
       b'\x01896630EE4000\x00\x00\x00\x00',
       b'\x01896630EE4100\x00\x00\x00\x00',
       b'\x01896630EE5000\x00\x00\x00\x00',
       b'\x01896630EE6000\x00\x00\x00\x00',
+      b'\x01896630EE7000\x00\x00\x00\x00',
       b'\x01896630EF8000\x00\x00\x00\x00',
       b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
       b'\x02896630E66100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
@@ -1243,6 +1191,27 @@ FW_VERSIONS = {
       b'8646F5301400\x00\x00\x00\x00',
     ],
   },
+  CAR.LEXUS_IS_TSS2: {
+    (Ecu.engine, 0x700, None): [
+      b'\x018966353S1000\x00\x00\x00\x00',
+      b'\x018966353S2000\x00\x00\x00\x00',
+    ],
+    (Ecu.abs, 0x7b0, None): [
+      b'\x01F15265337200\x00\x00\x00\x00',
+      b'\x01F15265342000\x00\x00\x00\x00',
+    ],
+    (Ecu.eps, 0x7a1, None): [
+      b'8965B53450\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.fwdRadar, 0x750, 0xf): [
+      b'\x018821F6201200\x00\x00\x00\x00',
+      b'\x018821F6201300\x00\x00\x00\x00',
+    ],
+    (Ecu.fwdCamera, 0x750, 0x6d): [
+      b'\x028646F5303300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
+      b'\x028646F5303400\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
+    ],
+  },
   CAR.PRIUS: {
     (Ecu.engine, 0x700, None): [
       b'\x02896634761000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
@@ -1410,150 +1379,65 @@ FW_VERSIONS = {
     ],
     (Ecu.abs, 0x7b0, None): [
       b'F152642090\x00\x00\x00\x00\x00\x00',
-      b'F152642110\x00\x00\x00\x00\x00\x00',
-      b'F152642120\x00\x00\x00\x00\x00\x00',
-      b'F152642400\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.dsu, 0x791, None): [
-      b'881514202200\x00\x00\x00\x00',
-      b'881514202300\x00\x00\x00\x00',
-      b'881514202400\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdRadar, 0x750, 0xf): [
-      b'8821F4702000\x00\x00\x00\x00',
-      b'8821F4702100\x00\x00\x00\x00',
-      b'8821F4702300\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdCamera, 0x750, 0x6d): [
-      b'8646F4201100\x00\x00\x00\x00',
-      b'8646F4201200\x00\x00\x00\x00',
-      b'8646F4202001\x00\x00\x00\x00',
-      b'8646F4202100\x00\x00\x00\x00',
-      b'8646F4204000\x00\x00\x00\x00',
-    ],
-  },
-  CAR.RAV4_TSS2: {
-    (Ecu.engine, 0x700, None): [
-      b'\x01896630R58000\x00\x00\x00\x00',
-      b'\x01896630R58100\x00\x00\x00\x00',
-      b'\x018966342E2000\x00\x00\x00\x00',
-      b'\x018966342M8000\x00\x00\x00\x00',
-      b'\x018966342S9000\x00\x00\x00\x00',
-      b'\x018966342T1000\x00\x00\x00\x00',
-      b'\x018966342T6000\x00\x00\x00\x00',
-      b'\x018966342T9000\x00\x00\x00\x00',
-      b'\x018966342U4000\x00\x00\x00\x00',
-      b'\x018966342U4100\x00\x00\x00\x00',
-      b'\x018966342U5100\x00\x00\x00\x00',
-      b'\x018966342V0000\x00\x00\x00\x00',
-      b'\x018966342V3000\x00\x00\x00\x00',
-      b'\x018966342V3100\x00\x00\x00\x00',
-      b'\x018966342V3200\x00\x00\x00\x00',
-      b'\x01896634A05000\x00\x00\x00\x00',
-      b'\x01896634A19000\x00\x00\x00\x00',
-      b'\x01896634A19100\x00\x00\x00\x00',
-      b'\x01896634A20000\x00\x00\x00\x00',
-      b'\x01896634A20100\x00\x00\x00\x00',
-      b'\x01896634A22000\x00\x00\x00\x00',
-      b'\x01896634A22100\x00\x00\x00\x00',
-      b'\x01896634A30000\x00\x00\x00\x00',
-      b'\x01896634A44000\x00\x00\x00\x00',
-      b'\x01896634A45000\x00\x00\x00\x00',
-      b'\x01896634A46000\x00\x00\x00\x00',
-      b'\x028966342M7000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
-      b'\x028966342T0000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
-      b'\x028966342V1000\x00\x00\x00\x00897CF1202001\x00\x00\x00\x00',
-      b'\x028966342Y8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
-      b'\x02896634A18000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
-      b'\x02896634A18100\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
-      b'\x02896634A43000\x00\x00\x00\x00897CF4201001\x00\x00\x00\x00',
-      b'\x02896634A47000\x00\x00\x00\x00897CF4201001\x00\x00\x00\x00',
-      b'\x028966342Z8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
-    ],
-    (Ecu.abs, 0x7b0, None): [
-      b'\x01F15260R210\x00\x00\x00\x00\x00\x00',
-      b'\x01F15260R220\x00\x00\x00\x00\x00\x00',
-      b'\x01F15260R290\x00\x00\x00\x00\x00\x00',
-      b'\x01F15260R300\x00\x00\x00\x00\x00\x00',
-      b'\x01F15260R302\x00\x00\x00\x00\x00\x00',
-      b'\x01F152642551\x00\x00\x00\x00\x00\x00',
-      b'\x01F152642561\x00\x00\x00\x00\x00\x00',
-      b'\x01F152642601\x00\x00\x00\x00\x00\x00',
-      b'\x01F152642700\x00\x00\x00\x00\x00\x00',
-      b'\x01F152642701\x00\x00\x00\x00\x00\x00',
-      b'\x01F152642710\x00\x00\x00\x00\x00\x00',
-      b'\x01F152642711\x00\x00\x00\x00\x00\x00',
-      b'\x01F152642750\x00\x00\x00\x00\x00\x00',
-      b'\x01F152642751\x00\x00\x00\x00\x00\x00',
-      b'\x01F15260R292\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.eps, 0x7a1, None): [
-      b'8965B42170\x00\x00\x00\x00\x00\x00',
-      b'8965B42171\x00\x00\x00\x00\x00\x00',
-      b'8965B42180\x00\x00\x00\x00\x00\x00',
-      b'8965B42181\x00\x00\x00\x00\x00\x00',
-      b'\x028965B0R01200\x00\x00\x00\x008965B0R02200\x00\x00\x00\x00',
-      b'\x028965B0R01300\x00\x00\x00\x008965B0R02300\x00\x00\x00\x00',
-      b'\x028965B0R01400\x00\x00\x00\x008965B0R02400\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdRadar, 0x750, 0xf): [
-      b'\x018821F3301100\x00\x00\x00\x00',
-      b'\x018821F3301200\x00\x00\x00\x00',
-      b'\x018821F3301300\x00\x00\x00\x00',
-      b'\x018821F3301400\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdCamera, 0x750, 0x6d): [
-      b'\x028646F4203200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
-      b'\x028646F4203300\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
-      b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
-      b'\x028646F4203500\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
-      b'\x028646F4203700\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
-      b'\x028646F4203800\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
-    ],
-  },
-  CAR.RAV4_TSS2_2022: {
-    (Ecu.abs, 0x7b0, None): [
-      b'\x01F15260R350\x00\x00\x00\x00\x00\x00',
-      b'\x01F15260R361\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.eps, 0x7a1, None): [
-      b'\x028965B0R01500\x00\x00\x00\x008965B0R02500\x00\x00\x00\x00',
-    ],
-    (Ecu.engine, 0x700, None): [
-      b'\x01896634AA0000\x00\x00\x00\x00',
-      b'\x01896634AA0100\x00\x00\x00\x00',
-      b'\x01896634AA1000\x00\x00\x00\x00',
-      b'\x01896634A88000\x00\x00\x00\x00',
-      b'\x01896634A89000\x00\x00\x00\x00',
-      b'\x01896634A89100\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdRadar, 0x750, 0xf): [
-      b'\x018821F0R01100\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdCamera, 0x750, 0x6d): [
-      b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00',
-    ],
-  },
-  CAR.RAV4_TSS2_2023: {
-    (Ecu.abs, 0x7b0, None): [
-      b'\x01F15260R450\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.eps, 0x7a1, None): [
-      b'\x028965B0R11000\x00\x00\x00\x008965B0R12000\x00\x00\x00\x00',
+      b'F152642110\x00\x00\x00\x00\x00\x00',
+      b'F152642120\x00\x00\x00\x00\x00\x00',
+      b'F152642400\x00\x00\x00\x00\x00\x00',
     ],
-    (Ecu.engine, 0x700, None): [
-      b'\x01896634A88100\x00\x00\x00\x00',
-      b'\x01896634AJ2000\x00\x00\x00\x00',
+    (Ecu.dsu, 0x791, None): [
+      b'881514202200\x00\x00\x00\x00',
+      b'881514202300\x00\x00\x00\x00',
+      b'881514202400\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x750, 0xf): [
-      b'\x018821F0R03100\x00\x00\x00\x00',
+      b'8821F4702000\x00\x00\x00\x00',
+      b'8821F4702100\x00\x00\x00\x00',
+      b'8821F4702300\x00\x00\x00\x00',
     ],
     (Ecu.fwdCamera, 0x750, 0x6d): [
-      b'\x028646F0R05100\x00\x00\x00\x008646G0R02100\x00\x00\x00\x00',
+      b'8646F4201100\x00\x00\x00\x00',
+      b'8646F4201200\x00\x00\x00\x00',
+      b'8646F4202001\x00\x00\x00\x00',
+      b'8646F4202100\x00\x00\x00\x00',
+      b'8646F4204000\x00\x00\x00\x00',
     ],
   },
-  CAR.RAV4H_TSS2: {
+  CAR.RAV4_TSS2: {
     (Ecu.engine, 0x700, None): [
+      b'\x01896630R58000\x00\x00\x00\x00',
+      b'\x01896630R58100\x00\x00\x00\x00',
+      b'\x018966342E2000\x00\x00\x00\x00',
+      b'\x018966342M8000\x00\x00\x00\x00',
+      b'\x018966342S9000\x00\x00\x00\x00',
+      b'\x018966342T1000\x00\x00\x00\x00',
+      b'\x018966342T6000\x00\x00\x00\x00',
+      b'\x018966342T9000\x00\x00\x00\x00',
+      b'\x018966342U4000\x00\x00\x00\x00',
+      b'\x018966342U4100\x00\x00\x00\x00',
+      b'\x018966342U5100\x00\x00\x00\x00',
+      b'\x018966342V0000\x00\x00\x00\x00',
+      b'\x018966342V3000\x00\x00\x00\x00',
+      b'\x018966342V3100\x00\x00\x00\x00',
+      b'\x018966342V3200\x00\x00\x00\x00',
+      b'\x01896634A05000\x00\x00\x00\x00',
+      b'\x01896634A19000\x00\x00\x00\x00',
+      b'\x01896634A19100\x00\x00\x00\x00',
+      b'\x01896634A20000\x00\x00\x00\x00',
+      b'\x01896634A20100\x00\x00\x00\x00',
+      b'\x01896634A22000\x00\x00\x00\x00',
+      b'\x01896634A22100\x00\x00\x00\x00',
+      b'\x01896634A30000\x00\x00\x00\x00',
+      b'\x01896634A44000\x00\x00\x00\x00',
+      b'\x01896634A45000\x00\x00\x00\x00',
+      b'\x01896634A46000\x00\x00\x00\x00',
+      b'\x028966342M7000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
+      b'\x028966342T0000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
+      b'\x028966342V1000\x00\x00\x00\x00897CF1202001\x00\x00\x00\x00',
+      b'\x028966342Y8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
+      b'\x02896634A18000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
+      b'\x02896634A18100\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
+      b'\x02896634A43000\x00\x00\x00\x00897CF4201001\x00\x00\x00\x00',
+      b'\x02896634A47000\x00\x00\x00\x00897CF4201001\x00\x00\x00\x00',
+      b'\x028966342Z8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
       b'\x01896634A15000\x00\x00\x00\x00',
       b'\x018966342M5000\x00\x00\x00\x00',
       b'\x018966342W8000\x00\x00\x00\x00',
@@ -1574,6 +1458,21 @@ FW_VERSIONS = {
       b'\x02896634A14101\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
     ],
     (Ecu.abs, 0x7b0, None): [
+      b'\x01F15260R210\x00\x00\x00\x00\x00\x00',
+      b'\x01F15260R220\x00\x00\x00\x00\x00\x00',
+      b'\x01F15260R290\x00\x00\x00\x00\x00\x00',
+      b'\x01F15260R300\x00\x00\x00\x00\x00\x00',
+      b'\x01F15260R302\x00\x00\x00\x00\x00\x00',
+      b'\x01F152642551\x00\x00\x00\x00\x00\x00',
+      b'\x01F152642561\x00\x00\x00\x00\x00\x00',
+      b'\x01F152642601\x00\x00\x00\x00\x00\x00',
+      b'\x01F152642700\x00\x00\x00\x00\x00\x00',
+      b'\x01F152642701\x00\x00\x00\x00\x00\x00',
+      b'\x01F152642710\x00\x00\x00\x00\x00\x00',
+      b'\x01F152642711\x00\x00\x00\x00\x00\x00',
+      b'\x01F152642750\x00\x00\x00\x00\x00\x00',
+      b'\x01F152642751\x00\x00\x00\x00\x00\x00',
+      b'\x01F15260R292\x00\x00\x00\x00\x00\x00',
       b'F152642291\x00\x00\x00\x00\x00\x00',
       b'F152642290\x00\x00\x00\x00\x00\x00',
       b'F152642322\x00\x00\x00\x00\x00\x00',
@@ -1611,8 +1510,10 @@ FW_VERSIONS = {
       b'\x028646F4203800\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
     ],
   },
-  CAR.RAV4H_TSS2_2022: {
+  CAR.RAV4_TSS2_2022: {
     (Ecu.abs, 0x7b0, None): [
+      b'\x01F15260R350\x00\x00\x00\x00\x00\x00',
+      b'\x01F15260R361\x00\x00\x00\x00\x00\x00',
       b'\x01F15264283100\x00\x00\x00\x00',
       b'\x01F15264286200\x00\x00\x00\x00',
       b'\x01F15264286100\x00\x00\x00\x00',
@@ -1624,6 +1525,12 @@ FW_VERSIONS = {
       b'8965B42172\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x700, None): [
+      b'\x01896634AA0000\x00\x00\x00\x00',
+      b'\x01896634AA0100\x00\x00\x00\x00',
+      b'\x01896634AA1000\x00\x00\x00\x00',
+      b'\x01896634A88000\x00\x00\x00\x00',
+      b'\x01896634A89000\x00\x00\x00\x00',
+      b'\x01896634A89100\x00\x00\x00\x00',
       b'\x01896634A02001\x00\x00\x00\x00',
       b'\x01896634A03000\x00\x00\x00\x00',
       b'\x01896634A08000\x00\x00\x00\x00',
@@ -1639,29 +1546,30 @@ FW_VERSIONS = {
       b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00',
     ],
   },
-  CAR.RAV4H_TSS2_2023: {
+  CAR.RAV4_TSS2_2023: {
     (Ecu.abs, 0x7b0, None): [
+      b'\x01F15260R450\x00\x00\x00\x00\x00\x00',
       b'\x01F15264283200\x00\x00\x00\x00',
       b'\x01F15264283300\x00\x00\x00\x00',
+      b'\x01F152642F1000\x00\x00\x00\x00',
     ],
     (Ecu.eps, 0x7a1, None): [
       b'\x028965B0R11000\x00\x00\x00\x008965B0R12000\x00\x00\x00\x00',
       b'8965B42371\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x700, None): [
+      b'\x01896634A88100\x00\x00\x00\x00',
+      b'\x01896634AJ2000\x00\x00\x00\x00',
+      b'\x01896634A89100\x00\x00\x00\x00',
       b'\x01896634AE1001\x00\x00\x00\x00',
       b'\x01896634AF0000\x00\x00\x00\x00',
     ],
-    (Ecu.hybrid, 0x7d2, None): [
-      b'\x02899830R41000\x00\x00\x00\x00899850R20000\x00\x00\x00\x00',
-      b'\x028998342C0000\x00\x00\x00\x00899854224000\x00\x00\x00\x00',
-      b'\x02899830R39000\x00\x00\x00\x00899850R20000\x00\x00\x00\x00',
-    ],
     (Ecu.fwdRadar, 0x750, 0xf): [
       b'\x018821F0R03100\x00\x00\x00\x00',
     ],
     (Ecu.fwdCamera, 0x750, 0x6d): [
       b'\x028646F0R05100\x00\x00\x00\x008646G0R02100\x00\x00\x00\x00',
+      b'\x028646F0R05200\x00\x00\x00\x008646G0R02200\x00\x00\x00\x00',
     ],
   },
   CAR.SIENNA: {
@@ -1720,42 +1628,10 @@ FW_VERSIONS = {
   CAR.LEXUS_ES_TSS2: {
     (Ecu.engine, 0x700, None): [
       b'\x018966306U6000\x00\x00\x00\x00',
-      b'\x01896630EC9100\x00\x00\x00\x00',
       b'\x018966333T5000\x00\x00\x00\x00',
       b'\x018966333T5100\x00\x00\x00\x00',
       b'\x018966333X6000\x00\x00\x00\x00',
       b'\x01896633T07000\x00\x00\x00\x00',
-    ],
-    (Ecu.abs, 0x7b0, None): [
-      b'\x01F152606281\x00\x00\x00\x00\x00\x00',
-      b'\x01F152606340\x00\x00\x00\x00\x00\x00',
-      b'\x01F152606461\x00\x00\x00\x00\x00\x00',
-      b'\x01F15260E031\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.eps, 0x7a1, None): [
-      b'8965B33252\x00\x00\x00\x00\x00\x00',
-      b'8965B33590\x00\x00\x00\x00\x00\x00',
-      b'8965B33690\x00\x00\x00\x00\x00\x00',
-      b'8965B33721\x00\x00\x00\x00\x00\x00',
-      b'8965B48271\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdRadar, 0x750, 0xf): [
-      b'\x018821F3301100\x00\x00\x00\x00',
-      b'\x018821F3301200\x00\x00\x00\x00',
-      b'\x018821F3301400\x00\x00\x00\x00',
-      b'\x018821F6201300\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdCamera, 0x750, 0x6d): [
-      b'\x028646F33030D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
-      b'\x028646F3303200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
-      b'\x028646F3304100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
-      b'\x028646F3304300\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
-      b'\x028646F3309100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
-      b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
-    ],
-  },
-  CAR.LEXUS_ESH_TSS2: {
-    (Ecu.engine, 0x700, None): [
       b'\x028966333S8000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
       b'\x028966333S8000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
       b'\x028966333T0100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
@@ -1766,6 +1642,10 @@ FW_VERSIONS = {
       b'\x01896633T58000\x00\x00\x00\x00',
     ],
     (Ecu.abs, 0x7b0, None): [
+      b'\x01F152606281\x00\x00\x00\x00\x00\x00',
+      b'\x01F152606340\x00\x00\x00\x00\x00\x00',
+      b'\x01F152606461\x00\x00\x00\x00\x00\x00',
+      b'\x01F15260646200\x00\x00\x00\x00',
       b'F152633423\x00\x00\x00\x00\x00\x00',
       b'F152633680\x00\x00\x00\x00\x00\x00',
       b'F152633681\x00\x00\x00\x00\x00\x00',
@@ -1781,19 +1661,20 @@ FW_VERSIONS = {
     (Ecu.fwdRadar, 0x750, 0xf): [
       b'\x018821F3301100\x00\x00\x00\x00',
       b'\x018821F3301200\x00\x00\x00\x00',
-      b'\x018821F3301300\x00\x00\x00\x00',
       b'\x018821F3301400\x00\x00\x00\x00',
       b'\x018821F6201300\x00\x00\x00\x00',
+      b'\x018821F3301300\x00\x00\x00\x00',
+      b'\x018821F6201400\x00\x00\x00\x00',
     ],
     (Ecu.fwdCamera, 0x750, 0x6d): [
-      b'\x028646F0610000\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
       b'\x028646F33030D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
-      b'\x028646F3303100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
       b'\x028646F3303200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
       b'\x028646F3304100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
-      b'\x028646F3304200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
       b'\x028646F3304300\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
       b'\x028646F3309100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
+      b'\x028646F0610000\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
+      b'\x028646F3303100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
+      b'\x028646F3304200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
     ],
   },
   CAR.LEXUS_ES: {
@@ -1838,6 +1719,26 @@ FW_VERSIONS = {
         b'8646F3302200\x00\x00\x00\x00',
       ],
   },
+  CAR.LEXUS_GS_F: {
+    (Ecu.engine, 0x7E0, None): [
+      b'\x0233075200\x00\x00\x00\x00\x00\x00\x00\x00530B9000\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.abs, 0x7b0, None): [
+      b'F152630700\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.dsu, 0x791, None): [
+      b'881513016200\x00\x00\x00\x00',
+    ],
+    (Ecu.eps, 0x7a1, None): [
+      b'8965B30551\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.fwdRadar, 0x750, 0xf): [
+      b'8821F4702000\x00\x00\x00\x00',
+    ],
+    (Ecu.fwdCamera, 0x750, 0x6d): [
+      b'8646F3002100\x00\x00\x00\x00',
+    ],
+  },
   CAR.LEXUS_NX: {
     (Ecu.engine, 0x700, None): [
       b'\x01896637850000\x00\x00\x00\x00',
@@ -1846,17 +1747,30 @@ FW_VERSIONS = {
       b'\x01896637854000\x00\x00\x00\x00',
       b'\x01896637878000\x00\x00\x00\x00',
     ],
+    (Ecu.engine, 0x7e0, None): [
+      b'\x0237841000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\x0237842000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\x0237880000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\x0237882000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\x0237886000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
     (Ecu.abs, 0x7b0, None): [
       b'F152678130\x00\x00\x00\x00\x00\x00',
       b'F152678140\x00\x00\x00\x00\x00\x00',
+      b'F152678160\x00\x00\x00\x00\x00\x00',
+      b'F152678170\x00\x00\x00\x00\x00\x00',
+      b'F152678171\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.dsu, 0x791, None): [
       b'881517803100\x00\x00\x00\x00',
       b'881517803300\x00\x00\x00\x00',
+      b'881517804300\x00\x00\x00\x00',
+      b'881517804100\x00\x00\x00\x00',
     ],
     (Ecu.eps, 0x7a1, None): [
       b'8965B78060\x00\x00\x00\x00\x00\x00',
       b'8965B78080\x00\x00\x00\x00\x00\x00',
+      b'8965B78100\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x750, 0xf): [
       b'8821F4702100\x00\x00\x00\x00',
@@ -1875,30 +1789,15 @@ FW_VERSIONS = {
       b'\x018966378G2000\x00\x00\x00\x00',
       b'\x018966378G3000\x00\x00\x00\x00',
       b'\x018966378B2000\x00\x00\x00\x00',
+      b'\x018966378B3100\x00\x00\x00\x00',
     ],
-    (Ecu.abs, 0x7b0, None): [
-      b'\x01F152678221\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.eps, 0x7a1, None): [
-      b'8965B78120\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdRadar, 0x750, 0xf): [
-      b"\x018821F3301400\x00\x00\x00\x00",
-      b'\x018821F3301200\x00\x00\x00\x00',
-      b'\x018821F3301300\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdCamera, 0x750, 0x6d): [
-      b'\x028646F78030A0\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
-      b'\x028646F7803100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
-    ],
-  },
-  CAR.LEXUS_NXH_TSS2: {
     (Ecu.engine, 0x7e0, None): [
       b'\x0237887000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\x02378A0000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\x02378F4000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.abs, 0x7b0, None): [
+      b'\x01F152678221\x00\x00\x00\x00\x00\x00',
       b'F152678210\x00\x00\x00\x00\x00\x00',
       b'F152678211\x00\x00\x00\x00\x00\x00',
     ],
@@ -1906,7 +1805,8 @@ FW_VERSIONS = {
       b'8965B78120\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x750, 0xf): [
-      b'\x018821F3301400\x00\x00\x00\x00',
+      b"\x018821F3301400\x00\x00\x00\x00",
+      b'\x018821F3301200\x00\x00\x00\x00',
       b'\x018821F3301300\x00\x00\x00\x00',
     ],
     (Ecu.fwdCamera, 0x750, 0x6d): [
@@ -1914,39 +1814,9 @@ FW_VERSIONS = {
       b'\x028646F7803100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
     ],
   },
-  CAR.LEXUS_NXH: {
-    (Ecu.engine, 0x7e0, None): [
-      b'\x0237841000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
-      b'\x0237842000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
-      b'\x0237880000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
-      b'\x0237882000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
-      b'\x0237886000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.abs, 0x7b0, None): [
-      b'F152678160\x00\x00\x00\x00\x00\x00',
-      b'F152678170\x00\x00\x00\x00\x00\x00',
-      b'F152678171\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.dsu, 0x791, None): [
-      b'881517804300\x00\x00\x00\x00',
-      b'881517804100\x00\x00\x00\x00',
-    ],
-    (Ecu.eps, 0x7a1, None): [
-      b'8965B78060\x00\x00\x00\x00\x00\x00',
-      b'8965B78080\x00\x00\x00\x00\x00\x00',
-      b'8965B78100\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdRadar, 0x750, 0xf): [
-      b'8821F4702300\x00\x00\x00\x00',
-      b'8821F4702100\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdCamera, 0x750, 0x6d): [
-      b'8646F7801300\x00\x00\x00\x00',
-      b'8646F7801100\x00\x00\x00\x00',
-    ],
-  },
   CAR.LEXUS_RC: {
     (Ecu.engine, 0x700, None): [
+      b'\x01896632461100\x00\x00\x00\x00',
       b'\x01896632478200\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x7e0, None): [
@@ -1957,17 +1827,20 @@ FW_VERSIONS = {
       b'F152624221\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.dsu, 0x791, None): [
+      b'881512404100\x00\x00\x00\x00',
       b'881512407000\x00\x00\x00\x00',
       b'881512409100\x00\x00\x00\x00',
     ],
     (Ecu.eps, 0x7a1, None): [
       b'8965B24081\x00\x00\x00\x00\x00\x00',
+      b'8965B24240\x00\x00\x00\x00\x00\x00',
       b'8965B24320\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x750, 0xf): [
       b'8821F4702300\x00\x00\x00\x00',
     ],
     (Ecu.fwdCamera, 0x750, 0x6d): [
+      b'8646F2401100\x00\x00\x00\x00',
       b'8646F2401200\x00\x00\x00\x00',
       b'8646F2402200\x00\x00\x00\x00',
     ],
@@ -2077,6 +1950,7 @@ FW_VERSIONS = {
       b'\x01896630EA9000\x00\x00\x00\x00',
       b'\x01896630EB0000\x00\x00\x00\x00',
       b'\x01896630EC9000\x00\x00\x00\x00',
+      b'\x01896630EC9100\x00\x00\x00\x00',
       b'\x01896630ED0000\x00\x00\x00\x00',
       b'\x01896630ED0100\x00\x00\x00\x00',
       b'\x01896630ED6000\x00\x00\x00\x00',
@@ -2089,30 +1963,8 @@ FW_VERSIONS = {
       b'\x01896634D44000\x00\x00\x00\x00',
       b'\x018966348X0000\x00\x00\x00\x00',
       b'\x01896630ED5000\x00\x00\x00\x00',
+      b'\x018966348R9200\x00\x00\x00\x00',
     ],
-    (Ecu.abs, 0x7b0, None): [
-      b'\x01F15260E031\x00\x00\x00\x00\x00\x00',
-      b'\x01F15260E041\x00\x00\x00\x00\x00\x00',
-      b'\x01F152648781\x00\x00\x00\x00\x00\x00',
-      b'\x01F152648801\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.eps, 0x7a1, None): [
-      b'8965B48261\x00\x00\x00\x00\x00\x00',
-      b'8965B48271\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdRadar, 0x750, 0xf): [
-      b'\x018821F3301100\x00\x00\x00\x00',
-      b'\x018821F3301300\x00\x00\x00\x00',
-      b'\x018821F3301400\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdCamera, 0x750, 0x6d): [
-      b'\x028646F4810100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
-      b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
-      b'\x028646F4810300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
-      b'\x028646F4810400\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
-    ],
-  },
-  CAR.LEXUS_RXH_TSS2: {
     (Ecu.engine, 0x7e0, None): [
       b'\x02348X4000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\x02348X5000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -2120,25 +1972,35 @@ FW_VERSIONS = {
       b'\x02348Y3000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\x0234D14000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\x0234D16000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\x02348U2000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.abs, 0x7b0, None): [
+      b'\x01F15260E031\x00\x00\x00\x00\x00\x00',
+      b'\x01F15260E041\x00\x00\x00\x00\x00\x00',
+      b'\x01F152648781\x00\x00\x00\x00\x00\x00',
+      b'\x01F152648801\x00\x00\x00\x00\x00\x00',
       b'F152648831\x00\x00\x00\x00\x00\x00',
       b'F152648891\x00\x00\x00\x00\x00\x00',
       b'F152648D00\x00\x00\x00\x00\x00\x00',
       b'F152648D60\x00\x00\x00\x00\x00\x00',
       b'F152648811\x00\x00\x00\x00\x00\x00',
+      b'F152648C80\x00\x00\x00\x00\x00\x00',
+      b'F152648493\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.eps, 0x7a1, None): [
       b'8965B48261\x00\x00\x00\x00\x00\x00',
       b'8965B48271\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x750, 0xf): [
+      b'\x018821F3301100\x00\x00\x00\x00',
+      b'\x018821F3301300\x00\x00\x00\x00',
       b'\x018821F3301400\x00\x00\x00\x00',
     ],
     (Ecu.fwdCamera, 0x750, 0x6d): [
       b'\x028646F4810100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
       b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
       b'\x028646F4810300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
+      b'\x028646F4810400\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
     ],
   },
   CAR.PRIUS_TSS2: {
@@ -2186,34 +2048,22 @@ FW_VERSIONS = {
     (Ecu.engine, 0x7e0, None): [
       b'\x0235870000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\x0235883000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.eps, 0x7a1, None): [
-      b'8965B58040\x00\x00\x00\x00\x00\x00',
-      b'8965B58052\x00\x00\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdRadar, 0x750, 0xf): [
-      b'\x018821F3301200\x00\x00\x00\x00',
-      b'\x018821F3301400\x00\x00\x00\x00',
-    ],
-    (Ecu.fwdCamera, 0x750, 0x6d): [
-      b'\x028646F58010C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
-      b'\x028646F5803200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
-    ],
-  },
-  CAR.ALPHARDH_TSS2: {
-    (Ecu.engine, 0x7e0, None): [
       b'\x0235879000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.eps, 0x7a1, None): [
       b'8965B58040\x00\x00\x00\x00\x00\x00',
+      b'8965B58052\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.abs, 0x7b0, None): [
       b'F152658341\x00\x00\x00\x00\x00\x00'
     ],
     (Ecu.fwdRadar, 0x750, 0xf): [
+      b'\x018821F3301200\x00\x00\x00\x00',
       b'\x018821F3301400\x00\x00\x00\x00',
     ],
     (Ecu.fwdCamera, 0x750, 0x6d): [
+      b'\x028646F58010C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
+      b'\x028646F5803200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
       b'\x028646FV201000\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
     ],
   },
@@ -2231,71 +2081,53 @@ DBC = {
   CAR.LEXUS_RX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
   CAR.LEXUS_RXH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
   CAR.LEXUS_RX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
-  CAR.LEXUS_RXH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.CHR: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
   CAR.CHR_TSS2: dbc_dict('toyota_nodsu_pt_generated', None),
-  CAR.CHRH: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
-  CAR.CHRH_TSS2: dbc_dict('toyota_nodsu_pt_generated', None),
   CAR.CAMRY: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
-  CAR.CAMRYH: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
   CAR.CAMRY_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
-  CAR.CAMRYH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.HIGHLANDER: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
   CAR.HIGHLANDER_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.HIGHLANDERH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
-  CAR.HIGHLANDERH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.AVALON: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
   CAR.AVALON_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
-  CAR.AVALONH_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
   CAR.AVALON_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
-  CAR.AVALONH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.RAV4_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None),
   CAR.RAV4_TSS2_2023: dbc_dict('toyota_nodsu_pt_generated', None),
   CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
-  CAR.COROLLAH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.LEXUS_ES: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
   CAR.LEXUS_ES_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
-  CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.LEXUS_ESH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
   CAR.SIENNA: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
   CAR.LEXUS_IS: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
+  CAR.LEXUS_IS_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.LEXUS_CTH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
-  CAR.RAV4H_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
-  CAR.RAV4H_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None),
-  CAR.RAV4H_TSS2_2023: dbc_dict('toyota_nodsu_pt_generated', None),
-  CAR.LEXUS_NXH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
   CAR.LEXUS_NX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
   CAR.LEXUS_NX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
-  CAR.LEXUS_NXH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.MIRAI: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.ALPHARD_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
-  CAR.ALPHARDH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
+  CAR.LEXUS_GS_F: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
 }
 
 # These cars have non-standard EPS torque scale factors. All others are 73
 EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.PRIUS_V: 100})
 
 # Toyota/Lexus Safety Sense 2.0 and 2.5
-TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022,
-            CAR.RAV4H_TSS2_2023, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2,
-            CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.LEXUS_NXH_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.AVALONH_TSS2, CAR.ALPHARDH_TSS2, CAR.CHR_TSS2, CAR.CHRH_TSS2}
+TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023, CAR.COROLLA_TSS2, CAR.LEXUS_ES_TSS2,
+            CAR.LEXUS_RX_TSS2, CAR.HIGHLANDER_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.LEXUS_IS_TSS2,
+            CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.CHR_TSS2}
 
-NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH}
+NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CAMRY}
 
 # the DSU uses the AEB message for longitudinal on these cars
-UNSUPPORTED_DSU_CAR = {CAR.LEXUS_IS, CAR.LEXUS_RC}
+UNSUPPORTED_DSU_CAR = {CAR.LEXUS_IS, CAR.LEXUS_RC, CAR.LEXUS_GS_F}
 
 # these cars have a radar which sends ACC messages instead of the camera
-RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2_2023, CAR.RAV4_TSS2_2023, CAR.CHR_TSS2, CAR.CHRH_TSS2}
+RADAR_ACC_CAR = {CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023, CAR.CHR_TSS2}
 
 # these cars use the Lane Tracing Assist (LTA) message for lateral control
-ANGLE_CONTROL_CAR = {CAR.RAV4H_TSS2_2023, CAR.RAV4_TSS2_2023}
-
-EV_HYBRID_CAR = {CAR.AVALONH_2019, CAR.AVALONH_TSS2, CAR.CAMRYH, CAR.CAMRYH_TSS2, CAR.CHRH, CAR.CHRH_TSS2, CAR.COROLLAH_TSS2, CAR.HIGHLANDERH, CAR.HIGHLANDERH_TSS2, CAR.PRIUS,
-                 CAR.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.RAV4H_TSS2_2023, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH,
-                 CAR.LEXUS_RXH_TSS2, CAR.LEXUS_NXH_TSS2, CAR.PRIUS_TSS2, CAR.ALPHARDH_TSS2}
+ANGLE_CONTROL_CAR = {CAR.RAV4_TSS2_2023}
 
 # no resume button press required
 NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.PRIUS_V, CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_ESH}
diff --git a/selfdrive/car/vin.py b/selfdrive/car/vin.py
index cf1c25e851..28edf157ae 100755
--- a/selfdrive/car/vin.py
+++ b/selfdrive/car/vin.py
@@ -3,9 +3,9 @@ import re
 
 import cereal.messaging as messaging
 from panda.python.uds import get_rx_addr_for_tx_addr, FUNCTIONAL_ADDRS
-from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
-from selfdrive.car.fw_query_definitions import StdQueries
-from system.swaglog import cloudlog
+from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
+from openpilot.selfdrive.car.fw_query_definitions import StdQueries
+from openpilot.system.swaglog import cloudlog
 
 VIN_UNKNOWN = "0" * 17
 VIN_RE = "[A-HJ-NPR-Z0-9]{17}"
diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py
index 4db5606217..e7a7f2a998 100644
--- a/selfdrive/car/volkswagen/carcontroller.py
+++ b/selfdrive/car/volkswagen/carcontroller.py
@@ -1,11 +1,11 @@
 from cereal import car
 from opendbc.can.packer import CANPacker
-from common.numpy_fast import clip
-from common.conversions import Conversions as CV
-from common.realtime import DT_CTRL
-from selfdrive.car import apply_driver_steer_torque_limits
-from selfdrive.car.volkswagen import mqbcan, pqcan
-from selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams
+from openpilot.common.numpy_fast import clip
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.car import apply_driver_steer_torque_limits
+from openpilot.selfdrive.car.volkswagen import mqbcan, pqcan
+from openpilot.selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 LongCtrlState = car.CarControl.Actuators.LongControlState
@@ -89,7 +89,8 @@ class CarController:
       if hud_control.leadVisible and self.frame * DT_CTRL > 1.0:  # Don't display lead until we know the scaling factor
         lead_distance = 512 if CS.upscale_lead_car_signal else 8
       acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive)
-      set_speed = hud_control.setSpeed * CV.MS_TO_KPH  # FIXME: follow the recent displayed-speed updates, also use mph_kmh toggle to fix display rounding problem?
+      # FIXME: follow the recent displayed-speed updates, also use mph_kmh toggle to fix display rounding problem?
+      set_speed = hud_control.setSpeed * CV.MS_TO_KPH
       can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, CANBUS.pt, acc_hud_status, set_speed,
                                                        lead_distance))
 
diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py
index f3cd2808a8..cbe8918d48 100644
--- a/selfdrive/car/volkswagen/carstate.py
+++ b/selfdrive/car/volkswagen/carstate.py
@@ -1,9 +1,9 @@
 import numpy as np
 from cereal import car
-from common.conversions import Conversions as CV
-from selfdrive.car.interfaces import CarStateBase
+from openpilot.common.conversions import Conversions as CV
+from openpilot.selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
-from selfdrive.car.volkswagen.values import DBC, CANBUS, PQ_CARS, NetworkLocation, TransmissionType, GearShifter, \
+from openpilot.selfdrive.car.volkswagen.values import DBC, CANBUS, PQ_CARS, NetworkLocation, TransmissionType, GearShifter, \
                                             CarControllerParams
 
 
@@ -254,54 +254,7 @@ class CarState(CarStateBase):
     if CP.carFingerprint in PQ_CARS:
       return CarState.get_can_parser_pq(CP)
 
-    signals = [
-      # sig_name, sig_address
-      ("LWI_Lenkradwinkel", "LWI_01"),           # Absolute steering angle
-      ("LWI_VZ_Lenkradwinkel", "LWI_01"),        # Steering angle sign
-      ("LWI_Lenkradw_Geschw", "LWI_01"),         # Absolute steering rate
-      ("LWI_VZ_Lenkradw_Geschw", "LWI_01"),      # Steering rate sign
-      ("ESP_VL_Radgeschw_02", "ESP_19"),         # ABS wheel speed, front left
-      ("ESP_VR_Radgeschw_02", "ESP_19"),         # ABS wheel speed, front right
-      ("ESP_HL_Radgeschw_02", "ESP_19"),         # ABS wheel speed, rear left
-      ("ESP_HR_Radgeschw_02", "ESP_19"),         # ABS wheel speed, rear right
-      ("ESP_Gierrate", "ESP_02"),                # Absolute yaw rate
-      ("ESP_VZ_Gierrate", "ESP_02"),             # Yaw rate sign
-      ("ZV_FT_offen", "Gateway_72"),             # Door open, driver
-      ("ZV_BT_offen", "Gateway_72"),             # Door open, passenger
-      ("ZV_HFS_offen", "Gateway_72"),            # Door open, rear left
-      ("ZV_HBFS_offen", "Gateway_72"),           # Door open, rear right
-      ("ZV_HD_offen", "Gateway_72"),             # Trunk or hatch open
-      ("Comfort_Signal_Left", "Blinkmodi_02"),   # Left turn signal including comfort blink interval
-      ("Comfort_Signal_Right", "Blinkmodi_02"),  # Right turn signal including comfort blink interval
-      ("AB_Gurtschloss_FA", "Airbag_02"),        # Seatbelt status, driver
-      ("AB_Gurtschloss_BF", "Airbag_02"),        # Seatbelt status, passenger
-      ("ESP_Fahrer_bremst", "ESP_05"),           # Driver applied brake pressure over threshold
-      ("MO_Fahrer_bremst", "Motor_14"),          # Brake pedal switch
-      ("ESP_Bremsdruck", "ESP_05"),              # Brake pressure
-      ("MO_Fahrpedalrohwert_01", "Motor_20"),    # Accelerator pedal value
-      ("EPS_Lenkmoment", "LH_EPS_03"),           # Absolute driver torque input
-      ("EPS_VZ_Lenkmoment", "LH_EPS_03"),        # Driver torque input sign
-      ("EPS_HCA_Status", "LH_EPS_03"),           # EPS HCA control status
-      ("ESP_Tastung_passiv", "ESP_21"),          # Stability control disabled
-      ("ESP_Haltebestaetigung", "ESP_21"),       # ESP hold confirmation
-      ("KBI_Handbremse", "Kombi_01"),            # Manual handbrake applied
-      ("KBI_Variante", "Kombi_03"),              # Digital/full-screen instrument cluster installed
-      ("TSK_Status", "TSK_06"),                  # ACC engagement status from drivetrain coordinator
-      ("GRA_Hauptschalter", "GRA_ACC_01"),       # ACC button, on/off
-      ("GRA_Abbrechen", "GRA_ACC_01"),           # ACC button, cancel
-      ("GRA_Tip_Setzen", "GRA_ACC_01"),          # ACC button, set
-      ("GRA_Tip_Hoch", "GRA_ACC_01"),            # ACC button, increase or accel
-      ("GRA_Tip_Runter", "GRA_ACC_01"),          # ACC button, decrease or decel
-      ("GRA_Tip_Wiederaufnahme", "GRA_ACC_01"),  # ACC button, resume
-      ("GRA_Verstellung_Zeitluecke", "GRA_ACC_01"),  # ACC button, time gap adj
-      ("GRA_Typ_Hauptschalter", "GRA_ACC_01"),   # ACC main button type
-      ("GRA_Codierung", "GRA_ACC_01"),           # ACC button configuration/coding
-      ("GRA_Tip_Stufe_2", "GRA_ACC_01"),         # unknown related to stalk type
-      ("GRA_ButtonTypeInfo", "GRA_ACC_01"),      # unknown related to stalk type
-      ("COUNTER", "GRA_ACC_01"),                 # GRA_ACC_01 CAN message counter
-    ]
-
-    checks = [
+    messages = [
       # sig_address, frequency
       ("LWI_01", 100),      # From J500 Steering Assist with integrated sensors
       ("LH_EPS_03", 100),   # From J500 Steering Assist with integrated sensors
@@ -321,108 +274,41 @@ class CarState(CarStateBase):
     ]
 
     if CP.transmissionType == TransmissionType.automatic:
-      signals.append(("GE_Fahrstufe", "Getriebe_11"))  # Auto trans gear selector position
-      checks.append(("Getriebe_11", 20))  # From J743 Auto transmission control module
+      messages.append(("Getriebe_11", 20))  # From J743 Auto transmission control module
     elif CP.transmissionType == TransmissionType.direct:
-      signals.append(("GearPosition", "EV_Gearshift"))  # EV gear selector position
-      checks.append(("EV_Gearshift", 10))  # From J??? unknown EV control module
-    elif CP.transmissionType == TransmissionType.manual:
-      signals += [("MO_Kuppl_schalter", "Motor_14"),  # Clutch switch
-                  ("BCM1_Rueckfahrlicht_Schalter", "Gateway_72")]  # Reverse light from BCM
+      messages.append(("EV_Gearshift", 10))  # From J??? unknown EV control module
 
     if CP.networkLocation == NetworkLocation.fwdCamera:
       # Radars are here on CANBUS.pt
-      signals += MqbExtraSignals.fwd_radar_signals
-      checks += MqbExtraSignals.fwd_radar_checks
+      messages += MqbExtraSignals.fwd_radar_messages
       if CP.enableBsm:
-        signals += MqbExtraSignals.bsm_radar_signals
-        checks += MqbExtraSignals.bsm_radar_checks
+        messages += MqbExtraSignals.bsm_radar_messages
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.pt)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, CANBUS.pt)
 
   @staticmethod
   def get_cam_can_parser(CP):
     if CP.carFingerprint in PQ_CARS:
       return CarState.get_cam_can_parser_pq(CP)
 
-    signals = []
-    checks = []
+    messages = []
 
     if CP.networkLocation == NetworkLocation.fwdCamera:
-      signals += [
-        # sig_name, sig_address
-        ("LDW_SW_Warnung_links", "LDW_02"),      # Blind spot in warning mode on left side due to lane departure
-        ("LDW_SW_Warnung_rechts", "LDW_02"),     # Blind spot in warning mode on right side due to lane departure
-        ("LDW_Seite_DLCTLC", "LDW_02"),          # Direction of most likely lane departure (left or right)
-        ("LDW_DLC", "LDW_02"),                   # Lane departure, distance to line crossing
-        ("LDW_TLC", "LDW_02"),                   # Lane departure, time to line crossing
-      ]
-      checks += [
+      messages += [
         # sig_address, frequency
         ("LDW_02", 10)      # From R242 Driver assistance camera
       ]
     else:
       # Radars are here on CANBUS.cam
-      signals += MqbExtraSignals.fwd_radar_signals
-      checks += MqbExtraSignals.fwd_radar_checks
+      messages += MqbExtraSignals.fwd_radar_messages
       if CP.enableBsm:
-        signals += MqbExtraSignals.bsm_radar_signals
-        checks += MqbExtraSignals.bsm_radar_checks
+        messages += MqbExtraSignals.bsm_radar_messages
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.cam)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, CANBUS.cam)
 
   @staticmethod
   def get_can_parser_pq(CP):
-    signals = [
-      # sig_name, sig_address, default
-      ("LH3_BLW", "Lenkhilfe_3"),                # Absolute steering angle
-      ("LH3_BLWSign", "Lenkhilfe_3"),            # Steering angle sign
-      ("LH3_LM", "Lenkhilfe_3"),                 # Absolute driver torque input
-      ("LH3_LMSign", "Lenkhilfe_3"),             # Driver torque input sign
-      ("LH2_Sta_HCA", "Lenkhilfe_2"),            # Steering rack HCA status
-      ("Lenkradwinkel_Geschwindigkeit", "Lenkwinkel_1"),  # Absolute steering rate
-      ("Lenkradwinkel_Geschwindigkeit_S", "Lenkwinkel_1"),  # Steering rate sign
-      ("Geschwindigkeit_neu__Bremse_1_", "Bremse_1"),  # Vehicle speed from ABS
-      ("Radgeschw__VL_4_1", "Bremse_3"),         # ABS wheel speed, front left
-      ("Radgeschw__VR_4_1", "Bremse_3"),         # ABS wheel speed, front right
-      ("Radgeschw__HL_4_1", "Bremse_3"),         # ABS wheel speed, rear left
-      ("Radgeschw__HR_4_1", "Bremse_3"),         # ABS wheel speed, rear right
-      ("Giergeschwindigkeit", "Bremse_5"),       # Absolute yaw rate
-      ("Vorzeichen_der_Giergeschwindigk", "Bremse_5"),  # Yaw rate sign
-      ("Gurtschalter_Fahrer", "Airbag_1"),       # Seatbelt status, driver
-      ("Gurtschalter_Beifahrer", "Airbag_1"),    # Seatbelt status, passenger
-      ("Bremstestschalter", "Motor_2"),          # Brake pedal pressed (brake light test switch)
-      ("Bremslichtschalter", "Motor_2"),         # Brakes applied (brake light switch)
-      ("Bremsdruck", "Bremse_5"),                # Brake pressure applied
-      ("Vorzeichen_Bremsdruck", "Bremse_5"),     # Brake pressure applied sign (???)
-      ("Fahrpedal_Rohsignal", "Motor_3"),        # Accelerator pedal value
-      ("ESP_Passiv_getastet", "Bremse_1"),       # Stability control disabled
-      ("GRA_Hauptschalter", "Motor_5"),          # ACC main switch
-      ("GRA_Status", "Motor_2"),                 # ACC engagement status
-      ("GK1_Fa_Tuerkont", "Gate_Komf_1"),        # Door open, driver
-      ("BSK_BT_geoeffnet", "Gate_Komf_1"),       # Door open, passenger
-      ("BSK_HL_geoeffnet", "Gate_Komf_1"),       # Door open, rear left
-      ("BSK_HR_geoeffnet", "Gate_Komf_1"),       # Door open, rear right
-      ("BSK_HD_Hauptraste", "Gate_Komf_1"),      # Trunk or hatch open
-      ("GK1_Blinker_li", "Gate_Komf_1"),         # Left turn signal on
-      ("GK1_Blinker_re", "Gate_Komf_1"),         # Right turn signal on
-      ("Bremsinfo", "Kombi_1"),                  # Manual handbrake applied
-      ("GRA_Hauptschalt", "GRA_Neu"),            # ACC button, on/off
-      ("GRA_Typ_Hauptschalt", "GRA_Neu"),        # ACC button, momentary vs latching
-      ("GRA_Kodierinfo", "GRA_Neu"),             # ACC button, configuration
-      ("GRA_Abbrechen", "GRA_Neu"),              # ACC button, cancel
-      ("GRA_Neu_Setzen", "GRA_Neu"),             # ACC button, set
-      ("GRA_Up_lang", "GRA_Neu"),                # ACC button, increase or accel, long press
-      ("GRA_Down_lang", "GRA_Neu"),              # ACC button, decrease or decel, long press
-      ("GRA_Up_kurz", "GRA_Neu"),                # ACC button, increase or accel, short press
-      ("GRA_Down_kurz", "GRA_Neu"),              # ACC button, decrease or decel, short press
-      ("GRA_Recall", "GRA_Neu"),                 # ACC button, resume
-      ("GRA_Zeitluecke", "GRA_Neu"),             # ACC button, time gap adj
-      ("COUNTER", "GRA_Neu"),                    # ACC button, message counter
-      ("GRA_Sender", "GRA_Neu"),                 # ACC button, CAN message originator
-    ]
-
-    checks = [
+    messages = [
       # sig_address, frequency
       ("Bremse_1", 100),    # From J104 ABS/ESP controller
       ("Bremse_3", 100),    # From J104 ABS/ESP controller
@@ -440,95 +326,55 @@ class CarState(CarStateBase):
     ]
 
     if CP.transmissionType == TransmissionType.automatic:
-      signals += [("Waehlhebelposition__Getriebe_1_", "Getriebe_1", 0)]  # Auto trans gear selector position
-      checks += [("Getriebe_1", 100)]  # From J743 Auto transmission control module
+      messages += [("Getriebe_1", 100)]  # From J743 Auto transmission control module
     elif CP.transmissionType == TransmissionType.manual:
-      signals += [("Kupplungsschalter", "Motor_1", 0),  # Clutch switch
-                  ("GK1_Rueckfahr", "Gate_Komf_1", 0)]  # Reverse light from BCM
-      checks += [("Motor_1", 100)]  # From J623 Engine control module
+      messages += [("Motor_1", 100)]  # From J623 Engine control module
 
     if CP.networkLocation == NetworkLocation.fwdCamera:
       # Extended CAN devices other than the camera are here on CANBUS.pt
-      signals += PqExtraSignals.fwd_radar_signals
-      checks += PqExtraSignals.fwd_radar_checks
+      messages += PqExtraSignals.fwd_radar_messages
       if CP.enableBsm:
-        signals += PqExtraSignals.bsm_radar_signals
-        checks += PqExtraSignals.bsm_radar_checks
+        messages += PqExtraSignals.bsm_radar_messages
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.pt)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, CANBUS.pt)
 
   @staticmethod
   def get_cam_can_parser_pq(CP):
 
-    signals = []
-    checks = []
+    messages = []
 
     if CP.networkLocation == NetworkLocation.fwdCamera:
-      signals += [
-        # sig_name, sig_address
-        ("LDW_SW_Warnung_links", "LDW_Status"),      # Blind spot in warning mode on left side due to lane departure
-        ("LDW_SW_Warnung_rechts", "LDW_Status"),     # Blind spot in warning mode on right side due to lane departure
-        ("LDW_Seite_DLCTLC", "LDW_Status"),          # Direction of most likely lane departure (left or right)
-        ("LDW_DLC", "LDW_Status"),                   # Lane departure, distance to line crossing
-        ("LDW_TLC", "LDW_Status"),                   # Lane departure, time to line crossing
-      ]
-      checks += [
+      messages += [
         # sig_address, frequency
         ("LDW_Status", 10)      # From R242 Driver assistance camera
       ]
 
     if CP.networkLocation == NetworkLocation.gateway:
       # Radars are here on CANBUS.cam
-      signals += PqExtraSignals.fwd_radar_signals
-      checks += PqExtraSignals.fwd_radar_checks
+      messages += PqExtraSignals.fwd_radar_messages
       if CP.enableBsm:
-        signals += PqExtraSignals.bsm_radar_signals
-        checks += PqExtraSignals.bsm_radar_checks
+        messages += PqExtraSignals.bsm_radar_messages
 
-    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.cam)
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, CANBUS.cam)
 
 
 class MqbExtraSignals:
   # Additional signal and message lists for optional or bus-portable controllers
-  fwd_radar_signals = [
-    ("ACC_Wunschgeschw_02", "ACC_02"),           # ACC set speed
-    ("ACC_Typ", "ACC_06"),                       # Basic vs FtS vs SnG
-    ("AWV2_Freigabe", "ACC_10"),                 # FCW brake jerk release
-    ("ANB_Teilbremsung_Freigabe", "ACC_10"),     # AEB partial braking release
-    ("ANB_Zielbremsung_Freigabe", "ACC_10"),     # AEB target braking release
-  ]
-  fwd_radar_checks = [
+  fwd_radar_messages = [
     ("ACC_06", 50),                              # From J428 ACC radar control module
     ("ACC_10", 50),                              # From J428 ACC radar control module
     ("ACC_02", 17),                              # From J428 ACC radar control module
   ]
-  bsm_radar_signals = [
-    ("SWA_Infostufe_SWA_li", "SWA_01"),          # Blind spot object info, left
-    ("SWA_Warnung_SWA_li", "SWA_01"),            # Blind spot object warning, left
-    ("SWA_Infostufe_SWA_re", "SWA_01"),          # Blind spot object info, right
-    ("SWA_Warnung_SWA_re", "SWA_01"),            # Blind spot object warning, right
-  ]
-  bsm_radar_checks = [
+  bsm_radar_messages = [
     ("SWA_01", 20),                              # From J1086 Lane Change Assist
   ]
 
 class PqExtraSignals:
   # Additional signal and message lists for optional or bus-portable controllers
-  fwd_radar_signals = [
-    ("ACS_Typ_ACC", "ACC_System"),               # Basic vs FtS (no SnG support on PQ)
-    ("ACA_StaACC", "ACC_GRA_Anzeige"),           # ACC drivetrain coordinator status
-    ("ACA_V_Wunsch", "ACC_GRA_Anzeige"),         # ACC set speed
-  ]
-  fwd_radar_checks = [
+  fwd_radar_messages = [
     ("ACC_System", 50),                          # From J428 ACC radar control module
     ("ACC_GRA_Anzeige", 25),                     # From J428 ACC radar control module
   ]
-  bsm_radar_signals = [
-    ("SWA_Infostufe_SWA_li", "SWA_1"),           # Blind spot object info, left
-    ("SWA_Warnung_SWA_li", "SWA_1"),             # Blind spot object warning, left
-    ("SWA_Infostufe_SWA_re", "SWA_1"),           # Blind spot object info, right
-    ("SWA_Warnung_SWA_re", "SWA_1"),             # Blind spot object warning, right
-  ]
-  bsm_radar_checks = [
+  bsm_radar_messages = [
     ("SWA_1", 20),                               # From J1086 Lane Change Assist
   ]
diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py
index 2b0603f162..d5377453c1 100644
--- a/selfdrive/car/volkswagen/interface.py
+++ b/selfdrive/car/volkswagen/interface.py
@@ -1,9 +1,9 @@
 from cereal import car
 from panda import Panda
-from common.conversions import Conversions as CV
-from selfdrive.car import STD_CARGO_KG, get_safety_config
-from selfdrive.car.interfaces import CarInterfaceBase
-from selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter
+from openpilot.common.conversions import Conversions as CV
+from openpilot.selfdrive.car import get_safety_config
+from openpilot.selfdrive.car.interfaces import CarInterfaceBase
+from openpilot.selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter
 
 ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
@@ -101,32 +101,32 @@ class CarInterface(CarInterfaceBase):
     # Per-chassis tuning values, override tuning defaults here if desired
 
     if candidate == CAR.ARTEON_MK1:
-      ret.mass = 1733 + STD_CARGO_KG
+      ret.mass = 1733
       ret.wheelbase = 2.84
 
     elif candidate == CAR.ATLAS_MK1:
-      ret.mass = 2011 + STD_CARGO_KG
+      ret.mass = 2011
       ret.wheelbase = 2.98
 
     elif candidate == CAR.CRAFTER_MK2:
-      ret.mass = 2100 + STD_CARGO_KG
+      ret.mass = 2100
       ret.wheelbase = 3.64  # SWB, LWB is 4.49, TBD how to detect difference
       ret.minSteerSpeed = 50 * CV.KPH_TO_MS
 
     elif candidate == CAR.GOLF_MK7:
-      ret.mass = 1397 + STD_CARGO_KG
+      ret.mass = 1397
       ret.wheelbase = 2.62
 
     elif candidate == CAR.JETTA_MK7:
-      ret.mass = 1328 + STD_CARGO_KG
+      ret.mass = 1328
       ret.wheelbase = 2.71
 
     elif candidate == CAR.PASSAT_MK8:
-      ret.mass = 1551 + STD_CARGO_KG
+      ret.mass = 1551
       ret.wheelbase = 2.79
 
     elif candidate == CAR.PASSAT_NMS:
-      ret.mass = 1503 + STD_CARGO_KG
+      ret.mass = 1503
       ret.wheelbase = 2.80
       ret.minEnableSpeed = 20 * CV.KPH_TO_MS  # ACC "basic", no FtS
       ret.minSteerSpeed = 50 * CV.KPH_TO_MS
@@ -134,86 +134,86 @@ class CarInterface(CarInterfaceBase):
       CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
 
     elif candidate == CAR.POLO_MK6:
-      ret.mass = 1230 + STD_CARGO_KG
+      ret.mass = 1230
       ret.wheelbase = 2.55
 
     elif candidate == CAR.SHARAN_MK2:
-      ret.mass = 1639 + STD_CARGO_KG
+      ret.mass = 1639
       ret.wheelbase = 2.92
       ret.minSteerSpeed = 50 * CV.KPH_TO_MS
       ret.steerActuatorDelay = 0.2
 
     elif candidate == CAR.TAOS_MK1:
-      ret.mass = 1498 + STD_CARGO_KG
+      ret.mass = 1498
       ret.wheelbase = 2.69
 
     elif candidate == CAR.TCROSS_MK1:
-      ret.mass = 1150 + STD_CARGO_KG
+      ret.mass = 1150
       ret.wheelbase = 2.60
 
     elif candidate == CAR.TIGUAN_MK2:
-      ret.mass = 1715 + STD_CARGO_KG
+      ret.mass = 1715
       ret.wheelbase = 2.74
 
     elif candidate == CAR.TOURAN_MK2:
-      ret.mass = 1516 + STD_CARGO_KG
+      ret.mass = 1516
       ret.wheelbase = 2.79
 
     elif candidate == CAR.TRANSPORTER_T61:
-      ret.mass = 1926 + STD_CARGO_KG
+      ret.mass = 1926
       ret.wheelbase = 3.00  # SWB, LWB is 3.40, TBD how to detect difference
       ret.minSteerSpeed = 14.0
 
     elif candidate == CAR.TROC_MK1:
-      ret.mass = 1413 + STD_CARGO_KG
+      ret.mass = 1413
       ret.wheelbase = 2.63
 
     elif candidate == CAR.AUDI_A3_MK3:
-      ret.mass = 1335 + STD_CARGO_KG
+      ret.mass = 1335
       ret.wheelbase = 2.61
 
     elif candidate == CAR.AUDI_Q2_MK1:
-      ret.mass = 1205 + STD_CARGO_KG
+      ret.mass = 1205
       ret.wheelbase = 2.61
 
     elif candidate == CAR.AUDI_Q3_MK2:
-      ret.mass = 1623 + STD_CARGO_KG
+      ret.mass = 1623
       ret.wheelbase = 2.68
 
     elif candidate == CAR.SEAT_ATECA_MK1:
-      ret.mass = 1900 + STD_CARGO_KG
+      ret.mass = 1900
       ret.wheelbase = 2.64
 
     elif candidate == CAR.SEAT_LEON_MK3:
-      ret.mass = 1227 + STD_CARGO_KG
+      ret.mass = 1227
       ret.wheelbase = 2.64
 
     elif candidate == CAR.SKODA_FABIA_MK4:
-      ret.mass = 1266 + STD_CARGO_KG
+      ret.mass = 1266
       ret.wheelbase = 2.56
 
     elif candidate == CAR.SKODA_KAMIQ_MK1:
-      ret.mass = 1265 + STD_CARGO_KG
+      ret.mass = 1265
       ret.wheelbase = 2.66
 
     elif candidate == CAR.SKODA_KAROQ_MK1:
-      ret.mass = 1278 + STD_CARGO_KG
+      ret.mass = 1278
       ret.wheelbase = 2.66
 
     elif candidate == CAR.SKODA_KODIAQ_MK1:
-      ret.mass = 1569 + STD_CARGO_KG
+      ret.mass = 1569
       ret.wheelbase = 2.79
 
     elif candidate == CAR.SKODA_OCTAVIA_MK3:
-      ret.mass = 1388 + STD_CARGO_KG
+      ret.mass = 1388
       ret.wheelbase = 2.68
 
     elif candidate == CAR.SKODA_SCALA_MK1:
-      ret.mass = 1192 + STD_CARGO_KG
+      ret.mass = 1192
       ret.wheelbase = 2.65
 
     elif candidate == CAR.SKODA_SUPERB_MK3:
-      ret.mass = 1505 + STD_CARGO_KG
+      ret.mass = 1505
       ret.wheelbase = 2.84
 
     else:
diff --git a/selfdrive/car/volkswagen/radar_interface.py b/selfdrive/car/volkswagen/radar_interface.py
index b2f7651136..e654bd61fd 100644
--- a/selfdrive/car/volkswagen/radar_interface.py
+++ b/selfdrive/car/volkswagen/radar_interface.py
@@ -1,5 +1,4 @@
-#!/usr/bin/env python3
-from selfdrive.car.interfaces import RadarInterfaceBase
+from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
 
 class RadarInterface(RadarInterfaceBase):
   pass
diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py
old mode 100755
new mode 100644
index ac80055300..c8e6daaef8
--- a/selfdrive/car/volkswagen/values.py
+++ b/selfdrive/car/volkswagen/values.py
@@ -1,15 +1,15 @@
 from collections import defaultdict, namedtuple
 from dataclasses import dataclass, field
-from enum import Enum
+from enum import Enum, StrEnum
 from typing import Dict, List, Union
 
 from cereal import car
 from panda.python import uds
 from opendbc.can.can_define import CANDefine
-from selfdrive.car import dbc_dict
-from selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column, \
+from openpilot.selfdrive.car import dbc_dict
+from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column, \
                                            Device
-from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16
+from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16
 
 Ecu = car.CarParams.Ecu
 NetworkLocation = car.CarParams.NetworkLocation
@@ -114,7 +114,7 @@ class CANBUS:
 # FW_VERSIONS for that existing CAR.
 # Exception: SEAT Leon and SEAT Ateca share a chassis code
 
-class CAR:
+class CAR(StrEnum):
   ARTEON_MK1 = "VOLKSWAGEN ARTEON 1ST GEN"          # Chassis AN, Mk1 VW Arteon and variants
   ATLAS_MK1 = "VOLKSWAGEN ATLAS 1ST GEN"            # Chassis CA, Mk1 VW Atlas and Atlas Cross Sport
   CRAFTER_MK2 = "VOLKSWAGEN CRAFTER 2ND GEN"        # Chassis SY/SZ, Mk2 VW Crafter, VW Grand California, MAN TGE
@@ -161,7 +161,7 @@ class Footnote(Enum):
     Column.MODEL)
   SKODA_HEATED_WINDSHIELD = CarFootnote(
     "Some Škoda vehicles are equipped with heated windshields, which are known " +
-    "to block GPS signal needed for some comma three functionality.",
+    "to block GPS signal needed for some comma 3X functionality.",
     Column.MODEL)
   VW_EXP_LONG = CarFootnote(
     "Only available for vehicles using a gateway (J533) harness. At this time, vehicles using a camera harness " +
@@ -184,7 +184,7 @@ class VWCarInfo(CarInfo):
       self.footnotes.append(Footnote.SKODA_HEATED_WINDSHIELD)
 
     if CP.carFingerprint in (CAR.CRAFTER_MK2, CAR.TRANSPORTER_T61):
-      self.car_parts = CarParts([Device.three_angled_mount, CarHarness.j533])
+      self.car_parts = CarParts([Device.threex_angled_mount, CarHarness.j533])
 
 
 CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = {
@@ -196,7 +196,7 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = {
   ],
   CAR.ATLAS_MK1: [
     VWCarInfo("Volkswagen Atlas 2018-23"),
-    VWCarInfo("Volkswagen Atlas Cross Sport 2021-22"),
+    VWCarInfo("Volkswagen Atlas Cross Sport 2020-22"),
     VWCarInfo("Volkswagen Teramont 2018-22"),
     VWCarInfo("Volkswagen Teramont Cross Sport 2021-22"),
     VWCarInfo("Volkswagen Teramont X 2021-22"),
@@ -245,9 +245,9 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = {
   CAR.TOURAN_MK2: VWCarInfo("Volkswagen Touran 2016-23"),
   CAR.TRANSPORTER_T61: [
     VWCarInfo("Volkswagen Caravelle 2020"),
-    VWCarInfo("Volkswagen California 2021"),
+    VWCarInfo("Volkswagen California 2021-23"),
   ],
-  CAR.TROC_MK1: VWCarInfo("Volkswagen T-Roc 2021", footnotes=[Footnote.VW_MQB_A0]),
+  CAR.TROC_MK1: VWCarInfo("Volkswagen T-Roc 2018-22", footnotes=[Footnote.VW_MQB_A0]),
   CAR.AUDI_A3_MK3: [
     VWCarInfo("Audi A3 2014-19"),
     VWCarInfo("Audi A3 Sportback e-tron 2017-18"),
@@ -259,13 +259,13 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = {
   CAR.SEAT_ATECA_MK1: VWCarInfo("SEAT Ateca 2018"),
   CAR.SEAT_LEON_MK3: VWCarInfo("SEAT Leon 2014-20"),
   CAR.SKODA_FABIA_MK4: VWCarInfo("Škoda Fabia 2022-23", footnotes=[Footnote.VW_MQB_A0]),
-  CAR.SKODA_KAMIQ_MK1: VWCarInfo("Škoda Kamiq 2021", footnotes=[Footnote.VW_MQB_A0, Footnote.KAMIQ]),
-  CAR.SKODA_KAROQ_MK1: VWCarInfo("Škoda Karoq 2019-21"),
+  CAR.SKODA_KAMIQ_MK1: VWCarInfo("Škoda Kamiq 2021-23", footnotes=[Footnote.VW_MQB_A0, Footnote.KAMIQ]),
+  CAR.SKODA_KAROQ_MK1: VWCarInfo("Škoda Karoq 2019-23"),
   CAR.SKODA_KODIAQ_MK1: VWCarInfo("Škoda Kodiaq 2017-23"),
   CAR.SKODA_SCALA_MK1: VWCarInfo("Škoda Scala 2020-23", footnotes=[Footnote.VW_MQB_A0]),
   CAR.SKODA_SUPERB_MK3: VWCarInfo("Škoda Superb 2015-22"),
   CAR.SKODA_OCTAVIA_MK3: [
-    VWCarInfo("Škoda Octavia 2015, 2018-19"),
+    VWCarInfo("Škoda Octavia 2015-19"),
     VWCarInfo("Škoda Octavia RS 2016"),
   ],
 }
@@ -350,6 +350,7 @@ FW_VERSIONS = {
   CAR.ATLAS_MK1: {
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x8703H906026AA\xf1\x899970',
+      b'\xf1\x8703H906026AG\xf1\x899973',
       b'\xf1\x8703H906026AJ\xf1\x890638',
       b'\xf1\x8703H906026AJ\xf1\x891017',
       b'\xf1\x8703H906026AT\xf1\x891922',
@@ -375,6 +376,7 @@ FW_VERSIONS = {
     (Ecu.srs, 0x715, None): [
       b'\xf1\x873Q0959655BC\xf1\x890503\xf1\x82\0161914151912001103111122031200',
       b'\xf1\x873Q0959655BN\xf1\x890713\xf1\x82\0162214152212001105141122052900',
+      b'\xf1\x873Q0959655DB\xf1\x890720\xf1\x82\x0e1114151112001105111122052900',
       b'\xf1\x873Q0959655DB\xf1\x890720\xf1\x82\0162214152212001105141122052900',
       b'\xf1\x873Q0959655DM\xf1\x890732\xf1\x82\x0e1114151112001105161122052J00',
       b'\xf1\x873Q0959655DM\xf1\x890732\xf1\x82\x0e1115151112001105171122052J00',
@@ -382,6 +384,7 @@ FW_VERSIONS = {
     (Ecu.eps, 0x712, None): [
       b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\00571B60924A1',
       b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\x0571B6G920A1',
+      b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\x0571B6M921A1',
       b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820528B6080105',
       b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820528B6090105',
     ],
@@ -396,6 +399,7 @@ FW_VERSIONS = {
   },
   CAR.CRAFTER_MK2: {
     (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x8704L906056BP\xf1\x894729',
       b'\xf1\x8704L906056EK\xf1\x896391',
       b'\xf1\x8705L906023BC\xf1\x892688',
     ],
@@ -403,15 +407,18 @@ FW_VERSIONS = {
     #(Ecu.transmission, 0x7e1, None): [
     #],
     (Ecu.srs, 0x715, None): [
+      b'\xf1\x873Q0959655AL\xf1\x890505\xf1\x82\x0e1411001413001203151311031100',
       b'\xf1\x873Q0959655BG\xf1\x890703\xf1\x82\x0e16120016130012051G1313052900',
       b'\xf1\x875QF959655AS\xf1\x890755\xf1\x82\x1315140015150011111100050200--1311120749',
     ],
     (Ecu.eps, 0x712, None): [
+      b'\xf1\x872N0909143D\x00\xf1\x897010\xf1\x82\x05183AZ306A2',
       b'\xf1\x872N0909143E \xf1\x897021\xf1\x82\x05163AZ306A2',
       b'\xf1\x872N0909144K \xf1\x897045\xf1\x82\x05233AZ810A2',
     ],
     (Ecu.fwdRadar, 0x757, None): [
       b'\xf1\x872Q0907572AA\xf1\x890396',
+      b'\xf1\x872Q0907572J \xf1\x890156',
       b'\xf1\x872Q0907572M \xf1\x890233',
     ],
   },
@@ -458,8 +465,10 @@ FW_VERSIONS = {
       b'\xf1\x878V0906259H \xf1\x890002',
       b'\xf1\x878V0906259J \xf1\x890003',
       b'\xf1\x878V0906259K \xf1\x890001',
+      b'\xf1\x878V0906259K \xf1\x890003',
       b'\xf1\x878V0906259P \xf1\x890001',
       b'\xf1\x878V0906259Q \xf1\x890002',
+      b'\xf1\x878V0906259R \xf1\x890002',
       b'\xf1\x878V0906264F \xf1\x890003',
       b'\xf1\x878V0906264L \xf1\x890002',
       b'\xf1\x878V0906264M \xf1\x890001',
@@ -482,10 +491,12 @@ FW_VERSIONS = {
       b'\xf1\x870CW300047E \xf1\x895261',
       b'\xf1\x870CW300048J \xf1\x890611',
       b'\xf1\x870CW300049H \xf1\x890905',
+      b'\xf1\x870CW300050G \xf1\x891905',
       b'\xf1\x870D9300012  \xf1\x894904',
       b'\xf1\x870D9300012  \xf1\x894913',
       b'\xf1\x870D9300012  \xf1\x894937',
       b'\xf1\x870D9300012  \xf1\x895045',
+      b'\xf1\x870D9300012  \xf1\x895046',
       b'\xf1\x870D9300014M \xf1\x895004',
       b'\xf1\x870D9300014Q \xf1\x895006',
       b'\xf1\x870D9300020J \xf1\x894902',
@@ -500,6 +511,7 @@ FW_VERSIONS = {
       b'\xf1\x870GC300012A \xf1\x891401',
       b'\xf1\x870GC300012A \xf1\x891403',
       b'\xf1\x870GC300014B \xf1\x892401',
+      b'\xf1\x870GC300014B \xf1\x892403',
       b'\xf1\x870GC300014B \xf1\x892405',
       b'\xf1\x870GC300020G \xf1\x892401',
       b'\xf1\x870GC300020G \xf1\x892403',
@@ -519,7 +531,9 @@ FW_VERSIONS = {
       b'\xf1\x875Q0959655BS\xf1\x890403\xf1\x82\x1314160011123300314240012250229333463100',
       b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x13141600111233003142404A2251229333463100',
       b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x13141600111233003142404A2252229333463100',
+      b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x13141600111233003142405A2251229333463100',
       b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x13141600111233003142405A2252229333463100',
+      b'\xf1\x875Q0959655CA\xf1\x890403\xf1\x82\x1314160011123300314240012250229333463100',
       b'\xf1\x875Q0959655C \xf1\x890361\xf1\x82\x111413001112120004110415121610169112',
       b'\xf1\x875Q0959655D \xf1\x890388\xf1\x82\x111413001113120006110417121A101A9113',
       b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\x13271112111312--071104171825102591131211',
@@ -545,6 +559,7 @@ FW_VERSIONS = {
       b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571A0J714A1',
       b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571A0JA15A1',
       b'\xf1\x873Q0909144M \xf1\x895082\xf1\x82\x0571A01A18A1',
+      b'\xf1\x873Q0909144M \xf1\x895082\xf1\x82\x0571A02A16A1',
       b'\xf1\x873Q0909144M \xf1\x895082\xf1\x82\x0571A0JA16A1',
       b'\xf1\x873QM909144  \xf1\x895072\xf1\x82\x0571A01714A1',
       b'\xf1\x875Q0909143K \xf1\x892033\xf1\x820519A9040203',
@@ -645,6 +660,7 @@ FW_VERSIONS = {
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x8703N906026E \xf1\x892114',
       b'\xf1\x8704E906023AH\xf1\x893379',
+      b'\xf1\x8704E906023BM\xf1\x894522',
       b'\xf1\x8704L906026DP\xf1\x891538',
       b'\xf1\x8704L906026ET\xf1\x891990',
       b'\xf1\x8704L906026FP\xf1\x892012',
@@ -660,6 +676,7 @@ FW_VERSIONS = {
       b'\xf1\x870D9300014L \xf1\x895002',
       b'\xf1\x870D9300041A \xf1\x894801',
       b'\xf1\x870DD300045T \xf1\x891601',
+      b'\xf1\x870DD300046H \xf1\x891601',
       b'\xf1\x870DL300011H \xf1\x895201',
       b'\xf1\x870CW300042H \xf1\x891601',
       b'\xf1\x870CW300042H \xf1\x891607',
@@ -707,12 +724,14 @@ FW_VERSIONS = {
   CAR.PASSAT_NMS: {
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x8706K906016C \xf1\x899609',
+      b'\xf1\x8706K906016E \xf1\x899830',
       b'\xf1\x8706K906016G \xf1\x891124',
       b'\xf1\x8706K906071BJ\xf1\x894891',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x8709G927158AB\xf1\x893318',
       b'\xf1\x8709G927158BD\xf1\x893121',
+      b'\xf1\x8709G927158DK\xf1\x893594',
       b'\xf1\x8709G927158FQ\xf1\x893745',
     ],
     (Ecu.srs, 0x715, None): [
@@ -764,15 +783,18 @@ FW_VERSIONS = {
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x8704E906027NJ\xf1\x891445',
       b'\xf1\x8704E906027NP\xf1\x891286',
+      b'\xf1\x8705E906013BD\xf1\x892496',
       b'\xf1\x8705E906013E \xf1\x891624',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x8709G927158EM\xf1\x893812',
       b'\xf1\x8709S927158BL\xf1\x893791',
+      b'\xf1\x8709S927158DN\xf1\x893946',
       b'\xf1\x8709S927158FF\xf1\x893876',
     ],
     (Ecu.srs, 0x715, None): [
       b'\xf1\x875Q0959655CB\xf1\x890421\xf1\x82\x1311111111333500314646021450149333613100',
+      b'\xf1\x875Q0959655CB\xf1\x890421\xf1\x82\x1312111111333500314646021550159333613100',
       b'\xf1\x875Q0959655CE\xf1\x890421\xf1\x82\x1311110011333300314240021350139333613100',
     ],
     (Ecu.eps, 0x712, None): [
@@ -808,15 +830,19 @@ FW_VERSIONS = {
       b'\xf1\x8704E906027NB\xf1\x899504',
       b'\xf1\x8704L906026EJ\xf1\x893661',
       b'\xf1\x8704L906027G \xf1\x899893',
+      b'\xf1\x8705E906018BS\xf1\x890914',
       b'\xf1\x875N0906259  \xf1\x890002',
       b'\xf1\x875NA906259H \xf1\x890002',
       b'\xf1\x875NA907115E \xf1\x890003',
       b'\xf1\x875NA907115E \xf1\x890005',
+      b'\xf1\x875NA907115J \xf1\x890002',
       b'\xf1\x8783A907115B \xf1\x890005',
       b'\xf1\x8783A907115F \xf1\x890002',
       b'\xf1\x8783A907115G \xf1\x890001',
       b'\xf1\x8783A907115K \xf1\x890001',
+      b'\xf1\x8783A907115K \xf1\x890002',
       b'\xf1\x8704E906024AP\xf1\x891461',
+      b'\xf1\x8783A907115  \xf1\x890007',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x8709G927158DT\xf1\x893698',
@@ -824,6 +850,7 @@ FW_VERSIONS = {
       b'\xf1\x8709G927158GC\xf1\x893821',
       b'\xf1\x8709G927158GD\xf1\x893820',
       b'\xf1\x8709G927158GM\xf1\x893936',
+      b'\xf1\x8709G927158GN\xf1\x893938',
       b'\xf1\x870D9300043  \xf1\x895202',
       b'\xf1\x870DL300011N \xf1\x892001',
       b'\xf1\x870DL300011N \xf1\x892012',
@@ -834,12 +861,15 @@ FW_VERSIONS = {
       b'\xf1\x870DL300013G \xf1\x892120',
       b'\xf1\x870DL300014C \xf1\x893703',
       b'\xf1\x870DD300046K \xf1\x892302',
+      b'\xf1\x870GC300013P \xf1\x892401',
+      b'\xf1\x870GC300046Q \xf1\x892802',
     ],
     (Ecu.srs, 0x715, None): [
       b'\xf1\x875Q0959655AR\xf1\x890317\xf1\x82\02331310031333334313132573732379333313100',
       b'\xf1\x875Q0959655BJ\xf1\x890336\xf1\x82\x1312110031333300314232583732379333423100',
       b'\xf1\x875Q0959655BJ\xf1\x890339\xf1\x82\x1331310031333334313132013730379333423100',
       b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02316143231313500314641011750179333423100',
+      b'\xf1\x875Q0959655BS\xf1\x890403\xf1\x82\x1312110031333300314240013750379333423100',
       b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02312110031333300314240583752379333423100',
       b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02331310031333336313140013950399333423100',
       b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x1331310031333334313140013750379333423100',
@@ -847,6 +877,7 @@ FW_VERSIONS = {
       b'\xf1\x875Q0959655CB\xf1\x890421\xf1\x82\x1316143231313500314647021750179333613100',
       b'\xf1\x875Q0959655CG\xf1\x890421\xf1\x82\x1331310031333300314240024050409333613100',
       b'\xf1\x875Q0959655CD\xf1\x890421\xf1\x82\x13123112313333003145406F6154619333613100',
+      b'\xf1\x875Q0959655AG\xf1\x890338\xf1\x82\x1316143231313500314617011730179333423100',
     ],
     (Ecu.eps, 0x712, None): [
       b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820529A6060603',
@@ -859,11 +890,14 @@ FW_VERSIONS = {
       b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\x0521A60604A1',
       b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\x0521A60604A1',
       b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\00521A60804A1',
+      b'\xf1\x875QM907144D \xf1\x891063\xf1\x82\x002RA60A2ROM',
       b'\xf1\x875QM907144D \xf1\x891063\xf1\x82\x002SA6092SOM',
       b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567A6017A00',
+      b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\x0521A60804A1',
     ],
     (Ecu.fwdRadar, 0x757, None): [
       b'\xf1\x872Q0907572AA\xf1\x890396',
+      b'\xf1\x872Q0907572AB\xf1\x890397',
       b'\xf1\x872Q0907572J \xf1\x890156',
       b'\xf1\x872Q0907572M \xf1\x890233',
       b'\xf1\x872Q0907572Q \xf1\x890342',
@@ -900,12 +934,14 @@ FW_VERSIONS = {
       b'\xf1\x8704L906057AP\xf1\x891186',
       b'\xf1\x8704L906057N \xf1\x890413',
       b'\xf1\x8705L906023E \xf1\x891352',
+      b'\xf1\x8705L906023MR\xf1\x892582',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x870BT300012G \xf1\x893102',
       b'\xf1\x870BT300012E \xf1\x893105',
       b'\xf1\x870BT300046R \xf1\x893102',
       b'\xf1\x870DV300012B \xf1\x893701',
+      b'\xf1\x870DV300012B \xf1\x893702',
     ],
     (Ecu.srs, 0x715, None): [
       b'\xf1\x872Q0959655AE\xf1\x890506\xf1\x82\x1316170411110411--04041704161611152S1411',
@@ -926,20 +962,29 @@ FW_VERSIONS = {
   CAR.TROC_MK1: {
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x8705E906018AT\xf1\x899640',
+      b'\xf1\x8705E906018CK\xf1\x890863',
+      b'\xf1\x8705E906018P \xf1\x896020',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x870CW300050J \xf1\x891911',
       b'\xf1\x870CW300051M \xf1\x891925',
+      b'\xf1\x870CW300051M \xf1\x891928',
+      b'\xf1\x870CW300041S \xf1\x891615',
     ],
     (Ecu.srs, 0x715, None): [
       b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x1311110012333300314240681152119333463100',
       b'\xf1\x875Q0959655CG\xf1\x890421\xf1\x82\x13111100123333003142404M1152119333613100',
+      b'\xf1\x875Q0959655CF\xf1\x890421\xf1\x82\x1311110012333300314240021150119333613100',
+      b'\xf1\x873Q0959655BH\xf1\x890712\xf1\x82\x0e1111001111001105111111052900',
     ],
     (Ecu.eps, 0x712, None): [
       b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521060405A1',
+      b'\xf1\x875WA907144M \xf1\x891051\xf1\x82\x001T06081T7N',
+      b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\x0521060403A1',
     ],
     (Ecu.fwdRadar, 0x757, None): [
       b'\xf1\x872Q0907572T \xf1\x890383',
+      b'\xf1\x872Q0907572M \xf1\x890233',
     ],
   },
   CAR.AUDI_A3_MK3: {
@@ -953,6 +998,7 @@ FW_VERSIONS = {
       b'\xf1\x875G0906259D \xf1\x890002',
       b'\xf1\x875G0906259L \xf1\x890002',
       b'\xf1\x875G0906259Q \xf1\x890002',
+      b'\xf1\x878V0906259E \xf1\x890001',
       b'\xf1\x878V0906259F \xf1\x890002',
       b'\xf1\x878V0906259H \xf1\x890002',
       b'\xf1\x878V0906259J \xf1\x890002',
@@ -967,6 +1013,7 @@ FW_VERSIONS = {
       b'\xf1\x870CW300048  \xf1\x895201',
       b'\xf1\x870D9300012  \xf1\x894912',
       b'\xf1\x870D9300012  \xf1\x894931',
+      b'\xf1\x870D9300012L \xf1\x894521',
       b'\xf1\x870D9300012K \xf1\x894513',
       b'\xf1\x870D9300013B \xf1\x894902',
       b'\xf1\x870D9300013B \xf1\x894931',
@@ -997,6 +1044,7 @@ FW_VERSIONS = {
       b'\xf1\x875Q0959655N \xf1\x890361\xf1\x82\0211212001112111104110411111521159114',
     ],
     (Ecu.eps, 0x712, None): [
+      b'\xf1\x873Q0909144F \xf1\x895043\xf1\x82\x0561G01A13A0',
       b'\xf1\x873Q0909144H \xf1\x895061\xf1\x82\00566G0HA14A1',
       b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566G0HA14A1',
       b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571G01A16A1',
@@ -1012,6 +1060,7 @@ FW_VERSIONS = {
       b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521G00807A1',
     ],
     (Ecu.fwdRadar, 0x757, None): [
+      b'\xf1\x875Q0907567M \xf1\x890398\xf1\x82\x0101',
       b'\xf1\x875Q0907567N \xf1\x890400\xf1\x82\x0101',
       b'\xf1\x875Q0907572F \xf1\x890400\xf1\x82\x0101',
       b'\xf1\x875Q0907572D \xf1\x890304\xf1\x82\x0101',
@@ -1043,10 +1092,14 @@ FW_VERSIONS = {
       b'\xf1\x8705L906022M \xf1\x890901',
       b'\xf1\x8783A906259  \xf1\x890001',
       b'\xf1\x8783A906259  \xf1\x890005',
+      b'\xf1\x8783A906259C \xf1\x890002',
+      b'\xf1\x8783A906259D \xf1\x890001',
       b'\xf1\x8783A906259F \xf1\x890001',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x8709G927158CN\xf1\x893608',
+      b'\xf1\x8709G927158FL\xf1\x893758',
+      b'\xf1\x8709G927158GG\xf1\x893825',
       b'\xf1\x8709G927158GP\xf1\x893937',
       b'\xf1\x870GC300045D \xf1\x892802',
       b'\xf1\x870GC300046F \xf1\x892701',
@@ -1056,11 +1109,13 @@ FW_VERSIONS = {
       b'\xf1\x875Q0959655BQ\xf1\x890421\xf1\x82\x132121111121120031112124218C219321532111',
       b'\xf1\x875Q0959655CC\xf1\x890421\xf1\x82\x131111111111120031111224118A119321532111',
       b'\xf1\x875Q0959655CC\xf1\x890421\xf1\x82\x131111111111120031111237116A119321532111',
+      b'\xf1\x875Q0959655BQ\xf1\x890421\xf1\x82\x132121111121120031112124218A219321532111',
     ],
     (Ecu.eps, 0x712, None): [
       b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567G6000300',
       b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567G6000800',
       b'\xf1\x875QF909144B \xf1\x895582\xf1\x82\x0571G60533A1',
+      b'\xf1\x875QF909144B \xf1\x895582\xf1\x82\x0571G60733A1',
       b'\xf1\x875TA907145D \xf1\x891051\xf1\x82\x001PG60A1P7N',
     ],
     (Ecu.fwdRadar, 0x757, None): [
@@ -1147,41 +1202,50 @@ FW_VERSIONS = {
   CAR.SKODA_KAMIQ_MK1: {
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x8705C906032M \xf1\x891333',
+      b'\xf1\x8705E906013CK\xf1\x892540',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x870CW300020  \xf1\x891906',
+      b'\xf1\x870CW300020T \xf1\x892204',
     ],
     (Ecu.srs, 0x715, None): [
       b'\xf1\x872Q0959655AM\xf1\x890351\xf1\x82\0222221042111042121040404042E2711152H14',
+      b'\xf1\x872Q0959655BJ\xf1\x890412\xf1\x82\x132223042111042121040404042B251215391423',
     ],
     (Ecu.eps, 0x712, None): [
       b'\xf1\x872Q1909144M \xf1\x896041',
+      b'\xf1\x872Q1909144AB\xf1\x896050',
     ],
     (Ecu.fwdRadar, 0x757, None): [
       b'\xf1\x872Q0907572T \xf1\x890383',
+      b'\xf1\x872Q0907572AA\xf1\x890396',
     ],
   },
   CAR.SKODA_KAROQ_MK1: {
     (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x8705E906013H \xf1\x892407',
       b'\xf1\x8705E906018P \xf1\x895472',
       b'\xf1\x8705E906018P \xf1\x896020',
       b'\xf1\x8705L906022BS\xf1\x890913',
     ],
     (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\x870CW300020T \xf1\x892202',
       b'\xf1\x870CW300041S \xf1\x891615',
       b'\xf1\x870GC300014L \xf1\x892802',
     ],
     (Ecu.srs, 0x715, None): [
+      b'\xf1\x875QF959655AT\xf1\x890755\xf1\x82\x1312110012120011111100010200--2521210749',
       b'\xf1\x873Q0959655BH\xf1\x890703\xf1\x82\x0e1213001211001101131112012100',
       b'\xf1\x873Q0959655BH\xf1\x890712\xf1\x82\0161213001211001101131122012100',
       b'\xf1\x873Q0959655DE\xf1\x890731\xf1\x82\x0e1213001211001101131121012J00',
     ],
     (Ecu.eps, 0x712, None): [
       b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\x0563T6090500',
-      b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\00567T6100500',
+      b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T6100500',
       b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T6100700',
     ],
     (Ecu.fwdRadar, 0x757, None): [
+      b'\xf1\x872Q0907572AA\xf1\x890396',
       b'\xf1\x872Q0907572AB\xf1\x890397',
       b'\xf1\x872Q0907572M \xf1\x890233',
       b'\xf1\x872Q0907572T \xf1\x890383',
@@ -1190,6 +1254,7 @@ FW_VERSIONS = {
   CAR.SKODA_KODIAQ_MK1: {
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x8704E906027DD\xf1\x893123',
+      b'\xf1\x8704E906027NB\xf1\x899504',
       b'\xf1\x8704E906027NB\xf1\x896517',
       b'\xf1\x8704L906026DE\xf1\x895418',
       b'\xf1\x8704L906026EJ\xf1\x893661',
@@ -1243,6 +1308,7 @@ FW_VERSIONS = {
       b'\xf1\x8704E906027HD\xf1\x893742',
       b'\xf1\x8704E906027MH\xf1\x894786',
       b'\xf1\x8704L906021DT\xf1\x898127',
+      b'\xf1\x8704L906026BP\xf1\x897608',
       b'\xf1\x8704L906026BS\xf1\x891541',
       b'\xf1\x875G0906259C \xf1\x890002',
     ],
@@ -1252,11 +1318,13 @@ FW_VERSIONS = {
       b'\xf1\x870CW300043B \xf1\x891601',
       b'\xf1\x870CW300043P \xf1\x891605',
       b'\xf1\x870D9300041C \xf1\x894936',
+      b'\xf1\x870D9300041H \xf1\x895220',
       b'\xf1\x870D9300041J \xf1\x894902',
       b'\xf1\x870D9300041P \xf1\x894507',
     ],
     (Ecu.srs, 0x715, None): [
       b'\xf1\x873Q0959655AC\xf1\x890200\xf1\x82\r11120011100010022212110200',
+      b'\xf1\x873Q0959655AP\xf1\x890305\xf1\x82\r11110011110011213331312131',
       b'\xf1\x873Q0959655AQ\xf1\x890200\xf1\x82\r11120011100010312212113100',
       b'\xf1\x873Q0959655AS\xf1\x890200\xf1\x82\r11120011100010022212110200',
       b'\xf1\x873Q0959655BH\xf1\x890703\xf1\x82\0163221003221002105755331052100',
@@ -1268,13 +1336,15 @@ FW_VERSIONS = {
       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',
+      b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\x0521T00601A1',
+      b'\xf1\x875QD909144E \xf1\x891081\xf1\x82\x0521T00503A1',
     ],
     (Ecu.fwdRadar, 0x757, None): [
       b'\xf1\x875Q0907572D \xf1\x890304\xf1\x82\x0101',
       b'\xf1\x875Q0907572F \xf1\x890400\xf1\x82\00101',
       b'\xf1\x875Q0907572J \xf1\x890654',
+      b'\xf1\x875Q0907572K \xf1\x890402\xf1\x82\x0101',
       b'\xf1\x875Q0907572P \xf1\x890682',
       b'\xf1\x875Q0907572R \xf1\x890771',
     ],
@@ -1304,8 +1374,12 @@ FW_VERSIONS = {
   },
   CAR.SKODA_SUPERB_MK3: {
     (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x8704E906027BS\xf1\x892887',
+      b'\xf1\x8704E906027BT\xf1\x899042',
       b'\xf1\x8704L906026ET\xf1\x891343',
+      b'\xf1\x8704L906026ET\xf1\x891990',
       b'\xf1\x8704L906026FP\xf1\x891196',
+      b'\xf1\x8704L906026KA\xf1\x896014',
       b'\xf1\x8704L906026KB\xf1\x894071',
       b'\xf1\x8704L906026KD\xf1\x894798',
       b'\xf1\x8704L906026MT\xf1\x893076',
@@ -1316,12 +1390,16 @@ FW_VERSIONS = {
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x870CW300042H \xf1\x891601',
+      b'\xf1\x870CW300043B \xf1\x891603',
+      b'\xf1\x870CW300049Q \xf1\x890906',
       b'\xf1\x870D9300011T \xf1\x894801',
       b'\xf1\x870D9300012  \xf1\x894940',
       b'\xf1\x870D9300013A \xf1\x894905',
       b'\xf1\x870D9300014K \xf1\x895006',
       b'\xf1\x870D9300041H \xf1\x894905',
+      b'\xf1\x870D9300042M \xf1\x895013',
       b'\xf1\x870D9300043F \xf1\x895202',
+      b'\xf1\x870GC300013K \xf1\x892403',
       b'\xf1\x870GC300014M \xf1\x892801',
       b'\xf1\x870GC300019G \xf1\x892803',
       b'\xf1\x870GC300043  \xf1\x892301',
@@ -1331,17 +1409,23 @@ FW_VERSIONS = {
       b'\xf1\x875Q0959655AE\xf1\x890130\xf1\x82\022111200111121001121118112231292221111',
       b'\xf1\x875Q0959655AK\xf1\x890130\xf1\x82\022111200111121001121110012211292221111',
       b'\xf1\x875Q0959655AS\xf1\x890317\xf1\x82\x1331310031313100313131823133319331313100',
+      b'\xf1\x875Q0959655AT\xf1\x890317\xf1\x82\x1331310031313100313131013131319331313100',
       b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\02331310031313100313131013141319331413100',
+      b'\xf1\x875Q0959655BK\xf1\x890336\xf1\x82\x1331310031313100313131013141319331413100',
       b'\xf1\x875Q0959655CA\xf1\x890403\xf1\x82\x1331310031313100313151013141319331423100',
+      b'\xf1\x875Q0959655CA\xf1\x890403\xf1\x82\x1331310031313100313151823143319331423100',
       b'\xf1\x875Q0959655CH\xf1\x890421\xf1\x82\x1333310031313100313152025350539331463100',
       b'\xf1\x875Q0959655CH\xf1\x890421\xf1\x82\x1333310031313100313152855372539331463100',
     ],
     (Ecu.eps, 0x712, None): [
       b'\xf1\x875Q0909143K \xf1\x892033\xf1\x820514UZ070203',
+      b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522UZ050303',
       b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522UZ070303',
       b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820526UZ070505',
+      b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820526UZ060505',
       b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\00563UZ060700',
       b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\x0563UZ060600',
+      b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567UZ070500',
       b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567UZ070600',
       b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567UZ070700',
     ],
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index fe5bb36714..19ce407932 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -1,32 +1,33 @@
 #!/usr/bin/env python3
 import os
 import math
+import time
 from typing import SupportsFloat
 
 from cereal import car, log
-from common.numpy_fast import clip
-from common.realtime import sec_since_boot, config_realtime_process, Priority, Ratekeeper, DT_CTRL
-from common.profiler import Profiler
-from common.params import Params, put_nonblocking, put_bool_nonblocking
+from openpilot.common.numpy_fast import clip
+from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL
+from openpilot.common.profiler import Profiler
+from openpilot.common.params import Params, put_nonblocking, put_bool_nonblocking
 import cereal.messaging as messaging
 from cereal.visionipc import VisionIpcClient, VisionStreamType
-from common.conversions import Conversions as CV
+from openpilot.common.conversions import Conversions as CV
 from panda import ALTERNATIVE_EXPERIENCE
-from system.swaglog import cloudlog
-from system.version import is_release_branch, get_short_branch
-from selfdrive.boardd.boardd import can_list_to_can_capnp
-from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can
-from selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET
-from selfdrive.controls.lib.drive_helpers import VCruiseHelper, get_lag_adjusted_curvature
-from selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
-from selfdrive.controls.lib.longcontrol import LongControl
-from selfdrive.controls.lib.latcontrol_pid import LatControlPID
-from selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
-from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
-from selfdrive.controls.lib.events import Events, ET
-from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
-from selfdrive.controls.lib.vehicle_model import VehicleModel
-from system.hardware import HARDWARE
+from openpilot.system.swaglog import cloudlog
+from openpilot.system.version import is_release_branch, get_short_branch
+from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp
+from openpilot.selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can
+from openpilot.selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET
+from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, get_lag_adjusted_curvature
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
+from openpilot.selfdrive.controls.lib.longcontrol import LongControl
+from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
+from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
+from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from openpilot.selfdrive.controls.lib.events import Events, ET
+from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
+from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
+from openpilot.system.hardware import HARDWARE
 
 SOFT_DISABLE_TIME = 3  # seconds
 LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
@@ -35,7 +36,6 @@ LANE_DEPARTURE_THRESHOLD = 0.1
 REPLAY = "REPLAY" in os.environ
 SIMULATION = "SIMULATION" in os.environ
 TESTING_CLOSET = "TESTING_CLOSET" in os.environ
-NOSENSOR = "NOSENSOR" in os.environ
 IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"}
 
 ThermalStatus = log.DeviceState.ThermalStatus
@@ -56,37 +56,33 @@ ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES)
 
 
 class Controls:
-  def __init__(self, sm=None, pm=None, can_sock=None, CI=None):
+  def __init__(self, CI=None):
     config_realtime_process(4, Priority.CTRL_HIGH)
 
     # Ensure the current branch is cached, otherwise the first iteration of controlsd lags
     self.branch = get_short_branch("")
 
     # Setup sockets
-    self.pm = pm
-    if self.pm is None:
-      self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState',
-                                     'carControl', 'carEvents', 'carParams'])
+    self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState',
+                                   'carControl', 'carEvents', 'carParams'])
 
+    self.sensor_packets = ["accelerometer", "gyroscope"]
     self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"]
 
-    self.can_sock = can_sock
-    if can_sock is None:
-      can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 20
-      self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
+    can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 20
+    self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
 
     self.log_sock = messaging.sub_sock('androidLog')
 
     self.params = Params()
-    self.sm = sm
-    if self.sm is None:
-      ignore = ['testJoystick']
-      if SIMULATION:
-        ignore += ['driverCameraState', 'managerState']
-      self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
-                                     'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
-                                     'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'testJoystick'] + self.camera_packets,
-                                    ignore_alive=ignore, ignore_avg_freq=['radarState', 'testJoystick'])
+    ignore = self.sensor_packets + ['testJoystick']
+    if SIMULATION:
+      ignore += ['driverCameraState', 'managerState']
+    self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
+                                   'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
+                                   'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
+                                   'testJoystick'] + self.camera_packets + self.sensor_packets,
+                                  ignore_alive=ignore, ignore_avg_freq=['radarState', 'testJoystick'])
 
     if CI is None:
       # wait for one pandaState and one CAN packet
@@ -125,6 +121,11 @@ class Controls:
       safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
       self.CP.safetyConfigs = [safety_config]
 
+    # Write previous route's CarParams
+    prev_cp = self.params.get("CarParamsPersistent")
+    if prev_cp is not None:
+      self.params.put("CarParamsPrevRoute", prev_cp)
+
     # Write CarParams for radard
     cp_bytes = self.CP.to_bytes()
     self.params.put("CarParams", cp_bytes)
@@ -351,7 +352,7 @@ class Controls:
 
     # generic catch-all. ideally, a more specific event should be added above instead
     can_rcv_timeout = self.can_rcv_timeout_counter >= 5
-    has_disable_events = self.events.any(ET.NO_ENTRY) and (self.events.any(ET.SOFT_DISABLE) or self.events.any(ET.IMMEDIATE_DISABLE))
+    has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE))
     no_system_errors = (not has_disable_events) or (len(self.events) == num_events)
     if (not self.sm.all_checks() or can_rcv_timeout) and no_system_errors:
       if not self.sm.all_alive():
@@ -373,17 +374,20 @@ class Controls:
     else:
       self.logged_comm_issue = None
 
-    if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY):
-      self.events.add(EventName.vehicleModelInvalid)
     if not self.sm['lateralPlan'].mpcSolutionValid:
       self.events.add(EventName.plannerError)
-    if not (self.sm['liveParameters'].sensorValid or self.sm['liveLocationKalman'].sensorsOK) and not NOSENSOR:
-      if self.sm.frame > 5 / DT_CTRL:  # Give locationd some time to receive all the inputs
-        self.events.add(EventName.sensorDataInvalid)
     if not self.sm['liveLocationKalman'].posenetOK:
       self.events.add(EventName.posenetInvalid)
     if not self.sm['liveLocationKalman'].deviceStable:
       self.events.add(EventName.deviceFalling)
+    if not self.sm['liveLocationKalman'].inputsOK:
+      self.events.add(EventName.locationdTemporaryError)
+    if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY):
+      self.events.add(EventName.paramsdTemporaryError)
+
+    # conservative HW alert. if the data or frequency are off, locationd will throw an error
+    if any((self.sm.frame - self.sm.rcv_frame[s])*DT_CTRL > 10. for s in self.sensor_packets):
+      self.events.add(EventName.sensorDataInvalid)
 
     if not REPLAY:
       # Check for mismatch between openpilot and car's PCM
@@ -412,15 +416,12 @@ class Controls:
 
     # TODO: fix simulator
     if not SIMULATION or REPLAY:
-      if not NOSENSOR:
-        if not self.sm['liveLocationKalman'].gpsOK and self.sm['liveLocationKalman'].inputsOK and (self.distance_traveled > 1000):
-          # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
-          self.events.add(EventName.noGps)
+      if not self.sm['liveLocationKalman'].gpsOK and self.sm['liveLocationKalman'].inputsOK and (self.distance_traveled > 1000):
+        # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
+        self.events.add(EventName.noGps)
 
       if self.sm['modelV2'].frameDropPerc > 20:
         self.events.add(EventName.modeldLagging)
-      if self.sm['liveLocationKalman'].excessiveResets:
-        self.events.add(EventName.localizerMalfunction)
 
   def data_sample(self):
     """Receive data from sockets and update carState"""
@@ -487,29 +488,29 @@ class Controls:
     # ENABLED, SOFT DISABLING, PRE ENABLING, OVERRIDING
     if self.state != State.disabled:
       # user and immediate disable always have priority in a non-disabled state
-      if self.events.any(ET.USER_DISABLE):
+      if self.events.contains(ET.USER_DISABLE):
         self.state = State.disabled
         self.current_alert_types.append(ET.USER_DISABLE)
 
-      elif self.events.any(ET.IMMEDIATE_DISABLE):
+      elif self.events.contains(ET.IMMEDIATE_DISABLE):
         self.state = State.disabled
         self.current_alert_types.append(ET.IMMEDIATE_DISABLE)
 
       else:
         # ENABLED
         if self.state == State.enabled:
-          if self.events.any(ET.SOFT_DISABLE):
+          if self.events.contains(ET.SOFT_DISABLE):
             self.state = State.softDisabling
             self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
             self.current_alert_types.append(ET.SOFT_DISABLE)
 
-          elif self.events.any(ET.OVERRIDE_LATERAL) or self.events.any(ET.OVERRIDE_LONGITUDINAL):
+          elif self.events.contains(ET.OVERRIDE_LATERAL) or self.events.contains(ET.OVERRIDE_LONGITUDINAL):
             self.state = State.overriding
             self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL]
 
         # SOFT DISABLING
         elif self.state == State.softDisabling:
-          if not self.events.any(ET.SOFT_DISABLE):
+          if not self.events.contains(ET.SOFT_DISABLE):
             # no more soft disabling condition, so go back to ENABLED
             self.state = State.enabled
 
@@ -521,32 +522,32 @@ class Controls:
 
         # PRE ENABLING
         elif self.state == State.preEnabled:
-          if not self.events.any(ET.PRE_ENABLE):
+          if not self.events.contains(ET.PRE_ENABLE):
             self.state = State.enabled
           else:
             self.current_alert_types.append(ET.PRE_ENABLE)
 
         # OVERRIDING
         elif self.state == State.overriding:
-          if self.events.any(ET.SOFT_DISABLE):
+          if self.events.contains(ET.SOFT_DISABLE):
             self.state = State.softDisabling
             self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
             self.current_alert_types.append(ET.SOFT_DISABLE)
-          elif not (self.events.any(ET.OVERRIDE_LATERAL) or self.events.any(ET.OVERRIDE_LONGITUDINAL)):
+          elif not (self.events.contains(ET.OVERRIDE_LATERAL) or self.events.contains(ET.OVERRIDE_LONGITUDINAL)):
             self.state = State.enabled
           else:
             self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL]
 
     # DISABLED
     elif self.state == State.disabled:
-      if self.events.any(ET.ENABLE):
-        if self.events.any(ET.NO_ENTRY):
+      if self.events.contains(ET.ENABLE):
+        if self.events.contains(ET.NO_ENTRY):
           self.current_alert_types.append(ET.NO_ENTRY)
 
         else:
-          if self.events.any(ET.PRE_ENABLE):
+          if self.events.contains(ET.PRE_ENABLE):
             self.state = State.preEnabled
-          elif self.events.any(ET.OVERRIDE_LATERAL) or self.events.any(ET.OVERRIDE_LONGITUDINAL):
+          elif self.events.contains(ET.OVERRIDE_LATERAL) or self.events.contains(ET.OVERRIDE_LONGITUDINAL):
             self.state = State.overriding
           else:
             self.state = State.enabled
@@ -572,7 +573,8 @@ class Controls:
     if self.CP.lateralTuning.which() == 'torque':
       torque_params = self.sm['liveTorqueParameters']
       if self.sm.all_checks(['liveTorqueParameters']) and torque_params.useParams:
-        self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered, torque_params.frictionCoefficientFiltered)
+        self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
+                                           torque_params.frictionCoefficientFiltered)
 
     lat_plan = self.sm['lateralPlan']
     long_plan = self.sm['longitudinalPlan']
@@ -584,7 +586,7 @@ class Controls:
     standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
     CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
                    (not standstill or self.joystick_mode)
-    CC.longActive = self.enabled and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
+    CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
 
     actuators = CC.actuators
     actuators.longControlState = self.LoC.long_control_state
@@ -622,13 +624,20 @@ class Controls:
     else:
       lac_log = log.ControlsState.LateralDebugState.new_message()
       if self.sm.rcv_frame['testJoystick'] > 0:
+        # reset joystick if it hasn't been received in a while
+        should_reset_joystick = (self.sm.frame - self.sm.rcv_frame['testJoystick'])*DT_CTRL > 0.2
+        if not should_reset_joystick:
+          joystick_axes = self.sm['testJoystick'].axes
+        else:
+          joystick_axes = [0.0, 0.0]
+
         if CC.longActive:
-          actuators.accel = 4.0*clip(self.sm['testJoystick'].axes[0], -1, 1)
+          actuators.accel = 4.0*clip(joystick_axes[0], -1, 1)
 
         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.
+          steer = clip(joystick_axes[1], -1, 1)
+          # max angle is 45 for angle-based cars, max curvature is 0.02
+          actuators.steer, actuators.steeringAngleDeg, actuators.curvature = steer, steer * 45., steer * -0.02
 
         lac_log.active = self.active
         lac_log.steeringAngleDeg = CS.steeringAngleDeg
@@ -742,7 +751,7 @@ class Controls:
 
     if not self.read_only and self.initialized:
       # send car controls over can
-      now_nanos = self.can_log_mono_time if REPLAY else int(sec_since_boot() * 1e9)
+      now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9)
       self.last_actuators, can_sends = self.CI.apply(CC, now_nanos)
       self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
       CC.actuatorsOutput = self.last_actuators
@@ -782,7 +791,7 @@ class Controls:
     controlsState.desiredCurvature = self.desired_curvature
     controlsState.desiredCurvatureRate = self.desired_curvature_rate
     controlsState.state = self.state
-    controlsState.engageable = not self.events.any(ET.NO_ENTRY)
+    controlsState.engageable = not self.events.contains(ET.NO_ENTRY)
     controlsState.longControlState = self.LoC.long_control_state
     controlsState.vPid = float(self.LoC.v_pid)
     controlsState.vCruise = float(self.v_cruise_helper.v_cruise_kph)
@@ -805,8 +814,6 @@ class Controls:
       controlsState.lateralControlState.pidState = lac_log
     elif lat_tuning == 'torque':
       controlsState.lateralControlState.torqueState = lac_log
-    elif lat_tuning == 'indi':
-      controlsState.lateralControlState.indiState = lac_log
 
     self.pm.send('controlsState', dat)
 
@@ -841,7 +848,7 @@ class Controls:
     self.CC = CC
 
   def step(self):
-    start_time = sec_since_boot()
+    start_time = time.monotonic()
     self.prof.checkpoint("Ratekeeper", ignore=True)
 
     self.is_metric = self.params.get_bool("IsMetric")
@@ -878,8 +885,8 @@ class Controls:
       self.prof.display()
 
 
-def main(sm=None, pm=None, logcan=None):
-  controls = Controls(sm, pm, logcan)
+def main():
+  controls = Controls()
   controls.controlsd_thread()
 
 
diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py
index f32e838333..6abcf4cbba 100644
--- a/selfdrive/controls/lib/alertmanager.py
+++ b/selfdrive/controls/lib/alertmanager.py
@@ -5,9 +5,9 @@ from collections import defaultdict
 from dataclasses import dataclass
 from typing import List, Dict, Optional
 
-from common.basedir import BASEDIR
-from common.params import Params
-from selfdrive.controls.lib.events import Alert
+from openpilot.common.basedir import BASEDIR
+from openpilot.common.params import Params
+from openpilot.selfdrive.controls.lib.events import Alert
 
 
 with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f:
diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py
index 4652b41c1c..d538035070 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 openpilot.common.conversions import Conversions as CV
+from openpilot.common.realtime import DT_MDL
 
 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 7cbcbc3d49..b728f1114c 100644
--- a/selfdrive/controls/lib/drive_helpers.py
+++ b/selfdrive/controls/lib/drive_helpers.py
@@ -1,10 +1,10 @@
 import math
 
 from cereal import car, log
-from common.conversions import Conversions as CV
-from common.numpy_fast import clip, interp
-from common.realtime import DT_MDL
-from selfdrive.modeld.constants import T_IDXS
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import clip, interp
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.modeld.constants import ModelConstants
 
 # WARNING: this value was determined based on the model's training distribution,
 #          model predictions above this speed can be unpredictable
@@ -177,7 +177,7 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
   # in high delay cases some corrections never even get commanded. So just use
   # psi to calculate a simple linearization of desired curvature
   current_curvature_desired = curvatures[0]
-  psi = interp(delay, T_IDXS[:CONTROL_N], psis)
+  psi = interp(delay, ModelConstants.T_IDXS[:CONTROL_N], psis)
   average_curvature_desired = psi / (v_ego * delay)
   desired_curvature = 2 * average_curvature_desired - current_curvature_desired
 
@@ -194,7 +194,8 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
   return safe_desired_curvature, safe_desired_curvature_rate
 
 
-def get_friction(lateral_accel_error: float, lateral_accel_deadzone: float, friction_threshold: float, torque_params: car.CarParams.LateralTorqueTuning, friction_compensation: bool) -> float:
+def get_friction(lateral_accel_error: float, lateral_accel_deadzone: float, friction_threshold: float,
+                 torque_params: car.CarParams.LateralTorqueTuning, friction_compensation: bool) -> float:
   friction_interp = interp(
     apply_center_deadzone(lateral_accel_error, lateral_accel_deadzone),
     [-friction_threshold, friction_threshold],
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index 19540f06fc..79bead3ac8 100755
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -1,3 +1,4 @@
+#!/usr/bin/env python3
 import math
 import os
 from enum import IntEnum
@@ -5,10 +6,10 @@ 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.locationd.calibrationd import MIN_SPEED_FILTER
-from system.version import get_short_branch
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER
+from openpilot.system.version import get_short_branch
 
 AlertSize = log.ControlsState.AlertSize
 AlertStatus = log.ControlsState.AlertStatus
@@ -67,7 +68,7 @@ class Events:
     self.events_prev = {k: (v + 1 if k in self.events else 0) for k, v in self.events_prev.items()}
     self.events = self.static_events.copy()
 
-  def any(self, event_type: str) -> bool:
+  def contains(self, event_type: str) -> bool:
     return any(event_type in EVENTS.get(e, {}) for e in self.events)
 
   def create_alerts(self, event_types: List[str], callback_args=None):
@@ -238,7 +239,7 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S
     f"Steer Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}",
     "",
     AlertStatus.userPrompt, AlertSize.small,
-    Priority.MID, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4)
+    Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4)
 
 
 def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
@@ -423,19 +424,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
 
   # ********** events only containing alerts that display while engaged **********
 
-  # openpilot tries to learn certain parameters about your car by observing
-  # how the car behaves to steering inputs from both human and openpilot driving.
-  # This includes:
-  # - steer ratio: gear ratio of the steering rack. Steering angle divided by tire angle
-  # - tire stiffness: how much grip your tires have
-  # - angle offset: most steering angle sensors are offset and measure a non zero angle when driving straight
-  # This alert is thrown when any of these values exceed a sanity check. This can be caused by
-  # bad alignment or bad sensor data. If this happens consistently consider creating an issue on GitHub
-  EventName.vehicleModelInvalid: {
-    ET.NO_ENTRY: NoEntryAlert("Vehicle Parameter Identification Failed"),
-    ET.SOFT_DISABLE: soft_disable_alert("Vehicle Parameter Identification Failed"),
-  },
-
   EventName.steerTempUnavailableSilent: {
     ET.WARNING: Alert(
       "Steering Temporarily Unavailable",
@@ -505,7 +493,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
       "Press Resume to Exit Standstill",
       "",
       AlertStatus.userPrompt, AlertSize.small,
-      Priority.LOW, VisualAlert.none, AudibleAlert.none, .2),
+      Priority.MID, VisualAlert.none, AudibleAlert.none, .2),
   },
 
   EventName.belowSteerSpeed: {
@@ -549,7 +537,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
       "Take Control",
       "Turn Exceeds Steering Limit",
       AlertStatus.userPrompt, AlertSize.mid,
-      Priority.LOW, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 1.),
+      Priority.LOW, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 2.),
   },
 
   # Thrown when the fan is driven at >50% but is not rotating
@@ -575,11 +563,34 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
     ET.PERMANENT: NormalPermanentAlert("GPS Malfunction", "Likely Hardware Issue"),
   },
 
-  # When the GPS position and localizer diverge the localizer is reset to the
-  # current GPS position. This alert is thrown when the localizer is reset
-  # more often than expected.
-  EventName.localizerMalfunction: {
-    # ET.PERMANENT: NormalPermanentAlert("Sensor Malfunction", "Hardware Malfunction"),
+  EventName.locationdTemporaryError: {
+    ET.NO_ENTRY: NoEntryAlert("locationd Temporary Error"),
+    ET.SOFT_DISABLE: soft_disable_alert("locationd Temporary Error"),
+  },
+
+  EventName.locationdPermanentError: {
+    ET.NO_ENTRY: NoEntryAlert("locationd Permanent Error"),
+    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("locationd Permanent Error"),
+    ET.PERMANENT: NormalPermanentAlert("locationd Permanent Error"),
+  },
+
+  # openpilot tries to learn certain parameters about your car by observing
+  # how the car behaves to steering inputs from both human and openpilot driving.
+  # This includes:
+  # - steer ratio: gear ratio of the steering rack. Steering angle divided by tire angle
+  # - tire stiffness: how much grip your tires have
+  # - angle offset: most steering angle sensors are offset and measure a non zero angle when driving straight
+  # This alert is thrown when any of these values exceed a sanity check. This can be caused by
+  # bad alignment or bad sensor data. If this happens consistently consider creating an issue on GitHub
+  EventName.paramsdTemporaryError: {
+    ET.NO_ENTRY: NoEntryAlert("paramsd Temporary Error"),
+    ET.SOFT_DISABLE: soft_disable_alert("paramsd Temporary Error"),
+  },
+
+  EventName.paramsdPermanentError: {
+    ET.NO_ENTRY: NoEntryAlert("paramsd Permanent Error"),
+    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("paramsd Permanent Error"),
+    ET.PERMANENT: NormalPermanentAlert("paramsd Permanent Error"),
   },
 
   # ********** events that affect controls state transitions **********
@@ -677,7 +688,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
   EventName.sensorDataInvalid: {
     ET.PERMANENT: Alert(
       "Sensor Data Invalid",
-      "Ensure device is mounted securely",
+      "Possible Hardware Issue",
       AlertStatus.normal, AlertSize.mid,
       Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=1.),
     ET.NO_ENTRY: NoEntryAlert("Sensor Data Invalid"),
@@ -724,7 +735,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
     ET.SOFT_DISABLE: soft_disable_alert("Calibration Incomplete"),
     ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"),
   },
-  
+
   EventName.calibrationRecalibrating: {
     ET.PERMANENT: calibration_incomplete_alert,
     ET.SOFT_DISABLE: soft_disable_alert("Device Remount Detected: Recalibrating"),
@@ -958,32 +969,28 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
 
 if __name__ == '__main__':
   # print all alerts by type and priority
-  from cereal.services import service_list
-  from collections import defaultdict, OrderedDict
+  from cereal.services import SERVICE_LIST
+  from collections import defaultdict
 
   event_names = {v: k for k, v in EventName.schema.enumerants.items()}
-  alerts_by_type: Dict[str, Dict[int, List[str]]] = defaultdict(lambda: defaultdict(list))
+  alerts_by_type: Dict[str, Dict[Priority, List[str]]] = defaultdict(lambda: defaultdict(list))
 
   CP = car.CarParams.new_message()
   CS = car.CarState.new_message()
-  sm = messaging.SubMaster(list(service_list.keys()))
+  sm = messaging.SubMaster(list(SERVICE_LIST.keys()))
 
   for i, alerts in EVENTS.items():
     for et, alert in alerts.items():
       if callable(alert):
         alert = alert(CP, CS, sm, False, 1)
-      priority = alert.priority
-      alerts_by_type[et][priority].append(event_names[i])
+      alerts_by_type[et][alert.priority].append(event_names[i])
 
-  all_alerts = {}
+  all_alerts: Dict[str, List[tuple[Priority, List[str]]]] = {}
   for et, priority_alerts in alerts_by_type.items():
-    all_alerts[et] = OrderedDict([
-      (str(priority), l)
-      for priority, l in sorted(priority_alerts.items(), key=lambda x: -int(x[0]))
-    ])
+    all_alerts[et] = sorted(priority_alerts.items(), key=lambda x: x[0], reverse=True)
 
   for status, evs in sorted(all_alerts.items(), key=lambda x: x[0]):
     print(f"**** {status} ****")
-    for p, alert_list in evs.items():
-      print(f"  {p}:")
+    for p, alert_list in evs:
+      print(f"  {repr(p)}:")
       print("   ", ', '.join(alert_list), "\n")
diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py
index d38959c560..30e1918442 100644
--- a/selfdrive/controls/lib/latcontrol.py
+++ b/selfdrive/controls/lib/latcontrol.py
@@ -1,7 +1,7 @@
 from abc import abstractmethod, ABC
 
-from common.numpy_fast import clip
-from common.realtime import DT_CTRL
+from openpilot.common.numpy_fast import clip
+from openpilot.common.realtime import DT_CTRL
 
 MIN_LATERAL_CONTROL_SPEED = 0.3  # m/s
 
diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py
index 9ed140d38e..b13d41e51c 100644
--- a/selfdrive/controls/lib/latcontrol_angle.py
+++ b/selfdrive/controls/lib/latcontrol_angle.py
@@ -1,7 +1,7 @@
 import math
 
 from cereal import log
-from selfdrive.controls.lib.latcontrol import LatControl
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 
 STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
 
diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py
index 6696d2e304..c41130af95 100644
--- a/selfdrive/controls/lib/latcontrol_pid.py
+++ b/selfdrive/controls/lib/latcontrol_pid.py
@@ -1,8 +1,8 @@
 import math
 
 from cereal import log
-from selfdrive.controls.lib.latcontrol import LatControl
-from selfdrive.controls.lib.pid import PIDController
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+from openpilot.selfdrive.controls.lib.pid import PIDController
 
 
 class LatControlPID(LatControl):
diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py
index 6550b19227..2c77630632 100644
--- a/selfdrive/controls/lib/latcontrol_torque.py
+++ b/selfdrive/controls/lib/latcontrol_torque.py
@@ -1,10 +1,10 @@
 import math
 
 from cereal import log
-from common.numpy_fast import interp
-from selfdrive.controls.lib.latcontrol import LatControl
-from selfdrive.controls.lib.pid import PIDController
-from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.common.numpy_fast import interp
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+from openpilot.selfdrive.controls.lib.pid import PIDController
+from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
 
 # At higher speeds (25+mph) we can assume:
 # Lateral acceleration achieved by a specific car correlates to
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/SConscript b/selfdrive/controls/lib/lateral_mpc_lib/SConscript
index af9283f073..49666abe69 100644
--- a/selfdrive/controls/lib/lateral_mpc_lib/SConscript
+++ b/selfdrive/controls/lib/lateral_mpc_lib/SConscript
@@ -1,4 +1,4 @@
-Import('env', 'envCython', 'arch', 'common')
+Import('env', 'envCython', 'arch')
 
 gen = "c_generated_code"
 
@@ -32,25 +32,25 @@ generated_files = [
   f'{gen}/Makefile',
 
   f'{gen}/main_lat.c',
+  f'{gen}/main_sim_lat.c',
   f'{gen}/acados_solver_lat.h',
+  f'{gen}/acados_sim_solver_lat.h',
+  f'{gen}/acados_sim_solver_lat.c',
   f'{gen}/acados_solver.pxd',
 
   f'{gen}/lat_model/lat_expl_vde_adj.c',
 
   f'{gen}/lat_model/lat_model.h',
-  f'{gen}/lat_cost/lat_cost_y_fun.h',
-  f'{gen}/lat_cost/lat_cost_y_e_fun.h',
-  f'{gen}/lat_cost/lat_cost_y_0_fun.h',
+  f'{gen}/lat_constraints/lat_constraints.h',
+  f'{gen}/lat_cost/lat_cost.h',
 ] + build_files
 
 acados_dir = '#third_party/acados'
 acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera'
 
 source_list = ['lat_mpc.py',
-  '#/selfdrive/modeld/constants.py',
+  '#selfdrive/modeld/constants.py',
   f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
-  f'{acados_dir}/x86_64/lib/libacados.so',
-  f'{acados_dir}/larch64/lib/libacados.so',
   f'{acados_templates_dir}/acados_solver.in.c',
 ]
 
@@ -60,7 +60,6 @@ lenv.Clean(generated_files, Dir(gen))
 generated_lat = lenv.Command(generated_files,
                              source_list,
                              f"cd {Dir('.').abspath} && python3 lat_mpc.py")
-lenv.Depends(generated_lat, "#common")
 
 lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
 lenv["CXXFLAGS"].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 ca7b991e69..83ec3b3a13 100755
--- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
+++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
@@ -1,16 +1,16 @@
 #!/usr/bin/env python3
 import os
+import time
 import numpy as np
 
 from casadi import SX, vertcat, sin, cos
-from common.realtime import sec_since_boot
 # WARNING: imports outside of constants will not trigger a rebuild
-from selfdrive.modeld.constants import T_IDXS
+from openpilot.selfdrive.modeld.constants import ModelConstants
 
 if __name__ == '__main__':  # generating code
-  from third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+  from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
-  from selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython  # pylint: disable=no-name-in-module, import-error
+  from openpilot.selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
 
 LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
 EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
@@ -66,7 +66,7 @@ def gen_lat_ocp():
   ocp = AcadosOcp()
   ocp.model = gen_lat_model()
 
-  Tf = np.array(T_IDXS)[N]
+  Tf = np.array(ModelConstants.T_IDXS)[N]
 
   # set dimensions
   ocp.dims.N = N
@@ -122,18 +122,22 @@ def gen_lat_ocp():
 
   # set prediction horizon
   ocp.solver_options.tf = Tf
-  ocp.solver_options.shooting_nodes = np.array(T_IDXS)[:N+1]
+  ocp.solver_options.shooting_nodes = np.array(ModelConstants.T_IDXS)[:N+1]
 
   ocp.code_export_directory = EXPORT_DIR
   return ocp
 
 
 class LateralMpc():
-  def __init__(self, x0=np.zeros(X_DIM)):
+  def __init__(self, x0=None):
+    if x0 is None:
+      x0 = np.zeros(X_DIM)
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.reset(x0)
 
-  def reset(self, x0=np.zeros(X_DIM)):
+  def reset(self, x0=None):
+    if x0 is None:
+      x0 = np.zeros(X_DIM)
     self.x_sol = np.zeros((N+1, X_DIM))
     self.u_sol = np.zeros((N, 1))
     self.yref = np.zeros((N+1, COST_DIM))
@@ -178,9 +182,9 @@ class LateralMpc():
     self.solver.set(N, "p", p_cp[N])
     self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
 
-    t = sec_since_boot()
+    t = time.monotonic()
     self.solution_status = self.solver.solve()
-    self.solve_time = sec_since_boot() - t
+    self.solve_time = time.monotonic() - t
 
     for i in range(N+1):
       self.x_sol[i] = self.solver.get(i, 'x')
diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py
index 5914312420..2417eb3c68 100644
--- a/selfdrive/controls/lib/lateral_planner.py
+++ b/selfdrive/controls/lib/lateral_planner.py
@@ -1,29 +1,12 @@
 import numpy as np
-from common.realtime import sec_since_boot, DT_MDL
-from common.numpy_fast import interp
-from system.swaglog import cloudlog
-from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
-from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
-from selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, get_speed_error
-from selfdrive.controls.lib.desire_helper import DesireHelper
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, get_speed_error
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 import cereal.messaging as messaging
 from cereal import log
 
 TRAJECTORY_SIZE = 33
 CAMERA_OFFSET = 0.04
 
-
-PATH_COST = 1.0
-LATERAL_MOTION_COST = 0.11
-LATERAL_ACCEL_COST = 0.0
-LATERAL_JERK_COST = 0.04
-# Extreme steering rate is unpleasant, even
-# when it does not cause bad jerk.
-# TODO this cost should be lowered when low
-# speed lateral control is stable on all cars
-STEERING_RATE_COST = 700.0
-
-
 class LateralPlanner:
   def __init__(self, CP, debug=False):
     self.DH = DesireHelper()
@@ -36,40 +19,26 @@ class LateralPlanner:
 
     self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
     self.velocity_xyz = np.zeros((TRAJECTORY_SIZE, 3))
-    self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
-    self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,))
-    self.t_idxs = np.arange(TRAJECTORY_SIZE)
-    self.y_pts = np.zeros((TRAJECTORY_SIZE,))
     self.v_plan = np.zeros((TRAJECTORY_SIZE,))
-    self.v_ego = 0.0
+    self.x_sol = np.zeros((TRAJECTORY_SIZE, 4), dtype=np.float32)
+    self.v_ego = MIN_SPEED
     self.l_lane_change_prob = 0.0
     self.r_lane_change_prob = 0.0
 
     self.debug_mode = debug
 
-    self.lat_mpc = LateralMpc()
-    self.reset_mpc(np.zeros(4))
-
-  def reset_mpc(self, x0=np.zeros(4)):
-    self.x0 = x0
-    self.lat_mpc.reset(x0=self.x0)
-
   def update(self, sm):
-    # clip speed , lateral planning is not possible at 0 speed
-    measured_curvature = sm['controlsState'].curvature
     v_ego_car = sm['carState'].vEgo
 
     # Parse model predictions
     md = sm['modelV2']
-    if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE:
+    if len(md.position.x) == TRAJECTORY_SIZE and len(md.velocity.x) == TRAJECTORY_SIZE and len(md.lateralPlannerSolution.x) == TRAJECTORY_SIZE:
       self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
-      self.t_idxs = np.array(md.position.t)
-      self.plan_yaw = np.array(md.orientation.z)
-      self.plan_yaw_rate = np.array(md.orientationRate.z)
       self.velocity_xyz = np.column_stack([md.velocity.x, md.velocity.y, md.velocity.z])
       car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car)
       self.v_plan = np.clip(car_speed, MIN_SPEED, np.inf)
       self.v_ego = self.v_plan[0]
+      self.x_sol = np.column_stack([md.lateralPlannerSolution.x, md.lateralPlannerSolution.y, md.lateralPlannerSolution.yaw, md.lateralPlannerSolution.yawRate])
 
     # Lane change logic
     desire_state = md.meta.desireState
@@ -79,66 +48,23 @@ class LateralPlanner:
     lane_change_prob = self.l_lane_change_prob + self.r_lane_change_prob
     self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
 
-    self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST,
-                             LATERAL_ACCEL_COST, LATERAL_JERK_COST,
-                             STEERING_RATE_COST)
-
-    y_pts = self.path_xyz[:LAT_MPC_N+1, 1]
-    heading_pts = self.plan_yaw[:LAT_MPC_N+1]
-    yaw_rate_pts = self.plan_yaw_rate[:LAT_MPC_N+1]
-    self.y_pts = y_pts
-
-    assert len(y_pts) == LAT_MPC_N + 1
-    assert len(heading_pts) == LAT_MPC_N + 1
-    assert len(yaw_rate_pts) == LAT_MPC_N + 1
-    lateral_factor = np.clip(self.factor1 - (self.factor2 * self.v_plan**2), 0.0, np.inf)
-    p = np.column_stack([self.v_plan, lateral_factor])
-    self.lat_mpc.run(self.x0,
-                     p,
-                     y_pts,
-                     heading_pts,
-                     yaw_rate_pts)
-    # init state for next iteration
-    # mpc.u_sol is the desired second derivative of psi given x0 curv state.
-    # with x0[3] = measured_yaw_rate, this would be the actual desired yaw rate.
-    # instead, interpolate x_sol so that x0[3] is the desired yaw rate for lat_control.
-    self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3])
-
-    #  Check for infeasible MPC solution
-    mpc_nans = np.isnan(self.lat_mpc.x_sol[:, 3]).any()
-    t = sec_since_boot()
-    if mpc_nans or self.lat_mpc.solution_status != 0:
-      self.reset_mpc()
-      self.x0[3] = measured_curvature * self.v_ego
-      if t > self.last_cloudlog_t + 5.0:
-        self.last_cloudlog_t = t
-        cloudlog.warning("Lateral mpc - nan: True")
-
-    if self.lat_mpc.cost > 1e6 or mpc_nans:
-      self.solution_invalid_cnt += 1
-    else:
-      self.solution_invalid_cnt = 0
-
   def publish(self, sm, pm):
-    plan_solution_valid = self.solution_invalid_cnt < 2
     plan_send = messaging.new_message('lateralPlan')
     plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2'])
 
     lateralPlan = plan_send.lateralPlan
     lateralPlan.modelMonoTime = sm.logMonoTime['modelV2']
-    lateralPlan.dPathPoints = self.y_pts.tolist()
-    lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
+    lateralPlan.dPathPoints = self.path_xyz[:,1].tolist()
+    lateralPlan.psis = self.x_sol[0:CONTROL_N, 2].tolist()
 
-    lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/self.v_ego).tolist()
-    lateralPlan.curvatureRates = [float(x/self.v_ego) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
+    lateralPlan.curvatures = (self.x_sol[0:CONTROL_N, 3]/self.v_ego).tolist()
+    lateralPlan.curvatureRates = [float(0) for _ in range(CONTROL_N-1)] # TODO: unused
 
-    lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
-    lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
+    lateralPlan.mpcSolutionValid = bool(1)
+    lateralPlan.solverExecutionTime = 0.0
     if self.debug_mode:
-      lateralPlan.solverCost = self.lat_mpc.cost
       lateralPlan.solverState = log.LateralPlan.SolverState.new_message()
-      lateralPlan.solverState.x = self.lat_mpc.x_sol.tolist()
-      lateralPlan.solverState.u = self.lat_mpc.u_sol.flatten().tolist()
+      lateralPlan.solverState.x = self.x_sol.tolist()
 
     lateralPlan.desire = self.DH.desire
     lateralPlan.useLaneLines = False
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index e8095813f2..ee65c4a69e 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -1,9 +1,9 @@
 from cereal import car
-from common.numpy_fast import clip, interp
-from common.realtime import DT_CTRL
-from selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone
-from selfdrive.controls.lib.pid import PIDController
-from selfdrive.modeld.constants import T_IDXS
+from openpilot.common.numpy_fast import clip, interp
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone
+from openpilot.selfdrive.controls.lib.pid import PIDController
+from openpilot.selfdrive.modeld.constants import ModelConstants
 
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
@@ -70,19 +70,19 @@ class LongControl:
     # Interp control trajectory
     speeds = long_plan.speeds
     if len(speeds) == CONTROL_N:
-      v_target_now = interp(t_since_plan, T_IDXS[:CONTROL_N], speeds)
-      a_target_now = interp(t_since_plan, T_IDXS[:CONTROL_N], long_plan.accels)
+      v_target_now = interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds)
+      a_target_now = interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], long_plan.accels)
 
-      v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, T_IDXS[:CONTROL_N], speeds)
+      v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds)
       a_target_lower = 2 * (v_target_lower - v_target_now) / self.CP.longitudinalActuatorDelayLowerBound - a_target_now
 
-      v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, T_IDXS[:CONTROL_N], speeds)
+      v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds)
       a_target_upper = 2 * (v_target_upper - v_target_now) / self.CP.longitudinalActuatorDelayUpperBound - a_target_now
 
       v_target = min(v_target_lower, v_target_upper)
       a_target = min(a_target_lower, a_target_upper)
 
-      v_target_1sec = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan + 1.0, T_IDXS[:CONTROL_N], speeds)
+      v_target_1sec = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan + 1.0, ModelConstants.T_IDXS[:CONTROL_N], speeds)
     else:
       v_target = 0.0
       v_target_now = 0.0
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
index 64d5aa963a..79afa1d918 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
@@ -1,4 +1,4 @@
-Import('env', 'envCython', 'arch', 'common')
+Import('env', 'envCython', 'arch', 'messaging_python', 'common_python')
 
 gen = "c_generated_code"
 
@@ -38,26 +38,25 @@ generated_files = [
   f'{gen}/Makefile',
 
   f'{gen}/main_long.c',
+  f'{gen}/main_sim_long.c',
   f'{gen}/acados_solver_long.h',
+  f'{gen}/acados_sim_solver_long.h',
+  f'{gen}/acados_sim_solver_long.c',
   f'{gen}/acados_solver.pxd',
 
   f'{gen}/long_model/long_expl_vde_adj.c',
 
   f'{gen}/long_model/long_model.h',
-  f'{gen}/long_constraints/long_h_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',
+  f'{gen}/long_constraints/long_constraints.h',
+  f'{gen}/long_cost/long_cost.h',
 ] + build_files
 
 acados_dir = '#third_party/acados'
 acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera'
 
 source_list = ['long_mpc.py',
-  '#/selfdrive/modeld/constants.py',
+  '#selfdrive/modeld/constants.py',
   f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
-  f'{acados_dir}/x86_64/lib/libacados.so',
-  f'{acados_dir}/larch64/lib/libacados.so',
   f'{acados_templates_dir}/acados_solver.in.c',
 ]
 
@@ -67,7 +66,7 @@ lenv.Clean(generated_files, Dir(gen))
 generated_long = lenv.Command(generated_files,
                               source_list,
                               f"cd {Dir('.').abspath} && python3 long_mpc.py")
-lenv.Depends(generated_long, "#cereal")
+lenv.Depends(generated_long, [messaging_python, common_python])
 
 lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
 lenv["CXXFLAGS"].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
old mode 100644
new mode 100755
index 46d39b34fb..a23ba1eaf6
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -1,18 +1,19 @@
 #!/usr/bin/env python3
 import os
+import time
 import numpy as np
 from cereal import log
-from common.realtime import sec_since_boot
-from common.numpy_fast import clip
-from system.swaglog import cloudlog
+from openpilot.common.numpy_fast import clip
+from openpilot.system.swaglog import cloudlog
 # WARNING: imports outside of constants will not trigger a rebuild
-from selfdrive.modeld.constants import index_function
-from selfdrive.controls.radard import _LEAD_ACCEL_TAU
+from openpilot.selfdrive.modeld.constants import index_function
+from openpilot.selfdrive.car.interfaces import ACCEL_MIN
+from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 
 if __name__ == '__main__':  # generating code
-  from third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+  from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
-  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 openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
 
 from casadi import SX, vertcat
 
@@ -52,8 +53,6 @@ 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)
 FCW_IDXS = T_IDXS < 5.0
 T_DIFFS = np.diff(T_IDXS, prepend=[0.])
-MIN_ACCEL = -3.5
-MAX_ACCEL = 2.0
 COMFORT_BRAKE = 2.5
 STOP_DISTANCE = 6.0
 
@@ -84,7 +83,9 @@ def get_stopped_equivalence_factor(v_lead):
 def get_safe_obstacle_distance(v_ego, t_follow):
   return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE
 
-def desired_follow_distance(v_ego, v_lead, t_follow=get_T_FOLLOW()):
+def desired_follow_distance(v_ego, v_lead, t_follow=None):
+  if t_follow is None:
+    t_follow = get_T_FOLLOW()
   return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead)
 
 
@@ -289,7 +290,7 @@ class LongitudinalMpc:
     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):
+      for i in range(N+1):
         self.solver.set(i, 'x', self.x0)
 
   @staticmethod
@@ -316,7 +317,7 @@ class LongitudinalMpc:
 
     # MPC will not converge if immediate crash is expected
     # Clip lead distance to what is still possible to brake for
-    min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-MIN_ACCEL * 2)
+    min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-ACCEL_MIN * 2)
     x_lead = clip(x_lead, min_x_lead, 1e8)
     v_lead = clip(v_lead, 0.0, 1e8)
     a_lead = clip(a_lead, -10., 5.)
@@ -343,7 +344,7 @@ class LongitudinalMpc:
     lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
     lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
 
-    self.params[:,0] = MIN_ACCEL
+    self.params[:,0] = ACCEL_MIN
     self.params[:,1] = self.max_a
 
     # Update in ACC mode or ACC/e2e blend
@@ -410,7 +411,7 @@ class LongitudinalMpc:
         self.source = 'lead1'
 
   def run(self):
-    # t0 = sec_since_boot()
+    # t0 = time.monotonic()
     # reset = 0
     for i in range(N+1):
       self.solver.set(i, 'p', self.params[i])
@@ -424,7 +425,8 @@ class LongitudinalMpc:
     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}")
+    # 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()
@@ -440,14 +442,15 @@ class LongitudinalMpc:
 
     self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution)
 
-    t = sec_since_boot()
+    t = time.monotonic()
     if self.solution_status != 0:
       if t > self.last_cloudlog_t + 5.0:
         self.last_cloudlog_t = t
         cloudlog.warning(f"Long mpc reset, solution_status: {self.solution_status}")
       self.reset()
       # reset = 1
-    # print(f"long_mpc timings: total internal {self.solve_time:.2e}, external: {(sec_since_boot() - t0):.2e} qp {self.time_qp_solution:.2e}, lin {self.time_linearization:.2e} qp_iter {qp_iter}, reset {reset}")
+    # print(f"long_mpc timings: total internal {self.solve_time:.2e}, external: {(time.monotonic() - t0):.2e} qp {self.time_qp_solution:.2e}, \
+    # lin {self.time_linearization:.2e} qp_iter {qp_iter}, reset {reset}")
 
 
 if __name__ == "__main__":
diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index a9c3cc7804..b1eff5302f 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -1,20 +1,21 @@
 #!/usr/bin/env python3
 import math
 import numpy as np
-from common.numpy_fast import clip, interp
-from common.params import Params
+from openpilot.common.numpy_fast import clip, interp
+from openpilot.common.params import Params
 from cereal import log
 
 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.controls.lib.longcontrol import LongCtrlState
-from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, MIN_ACCEL, MAX_ACCEL
-from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
-from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error
-from system.swaglog import cloudlog
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
+from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error
+from openpilot.system.swaglog import cloudlog
 
 LON_MPC_STEP = 0.2  # first step is 0.2s
 A_CRUISE_MIN = -1.2
@@ -75,9 +76,9 @@ class LongitudinalPlanner:
     if (len(model_msg.position.x) == 33 and
        len(model_msg.velocity.x) == 33 and
        len(model_msg.acceleration.x) == 33):
-      x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC
-      v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x) - model_error
-      a = np.interp(T_IDXS_MPC, T_IDXS, model_msg.acceleration.x)
+      x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC
+      v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - model_error
+      a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x)
       j = np.zeros(len(T_IDXS_MPC))
     else:
       x = np.zeros(len(T_IDXS_MPC))
@@ -93,8 +94,7 @@ class LongitudinalPlanner:
     self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
 
     v_ego = sm['carState'].vEgo
-    v_cruise_kph = sm['controlsState'].vCruise
-    v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
+    v_cruise_kph = min(sm['controlsState'].vCruise, V_CRUISE_MAX)
     v_cruise = v_cruise_kph * CV.KPH_TO_MS
 
     long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
@@ -110,8 +110,8 @@ class LongitudinalPlanner:
       accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
       accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
     else:
-      accel_limits = [MIN_ACCEL, MAX_ACCEL]
-      accel_limits_turns = [MIN_ACCEL, MAX_ACCEL]
+      accel_limits = [ACCEL_MIN, ACCEL_MAX]
+      accel_limits_turns = [ACCEL_MIN, ACCEL_MAX]
 
     if reset_state:
       self.v_desired_filter.x = v_ego
@@ -135,11 +135,11 @@ class LongitudinalPlanner:
     x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
     self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=self.personality)
 
-    self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
-    self.a_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.a_solution)
+    self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
+    self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution)
     self.v_desired_trajectory = self.v_desired_trajectory_full[:CONTROL_N]
     self.a_desired_trajectory = self.a_desired_trajectory_full[:CONTROL_N]
-    self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
+    self.j_desired_trajectory = np.interp(ModelConstants.T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
 
     # TODO counter is only needed because radar is glitchy, remove once radar is gone
     self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill
@@ -148,7 +148,7 @@ class LongitudinalPlanner:
 
     # Interpolate 0.05 seconds and save as starting point for next iteration
     a_prev = self.a_desired
-    self.a_desired = float(interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory))
+    self.a_desired = float(interp(DT_MDL, ModelConstants.T_IDXS[:CONTROL_N], self.a_desired_trajectory))
     self.v_desired_filter.x = self.v_desired_filter.x + DT_MDL * (self.a_desired + a_prev) / 2.0
 
   def publish(self, sm, pm):
diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py
index 965158131b..f4ec7e5996 100644
--- a/selfdrive/controls/lib/pid.py
+++ b/selfdrive/controls/lib/pid.py
@@ -1,7 +1,7 @@
 import numpy as np
 from numbers import Number
 
-from common.numpy_fast import clip, interp
+from openpilot.common.numpy_fast import clip, interp
 
 
 class PIDController():
diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py
index 7e61efcf45..15dbd4c5e2 100755
--- a/selfdrive/controls/plannerd.py
+++ b/selfdrive/controls/plannerd.py
@@ -2,32 +2,32 @@
 import os
 import numpy as np
 from cereal import car
-from common.params import Params
-from common.realtime import Priority, config_realtime_process
-from system.swaglog import cloudlog
-from selfdrive.modeld.constants import T_IDXS
-from selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
-from selfdrive.controls.lib.lateral_planner import LateralPlanner
+from openpilot.common.params import Params
+from openpilot.common.realtime import Priority, config_realtime_process
+from openpilot.system.swaglog import cloudlog
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+from openpilot.selfdrive.controls.lib.lateral_planner import LateralPlanner
 import cereal.messaging as messaging
 
 def cumtrapz(x, t):
   return np.concatenate([[0], np.cumsum(((x[0:-1] + x[1:])/2) * np.diff(t))])
 
 def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner):
-  plan_odo = cumtrapz(longitudinal_planner.v_desired_trajectory_full, T_IDXS)
-  model_odo = cumtrapz(lateral_planner.v_plan, T_IDXS)
+  plan_odo = cumtrapz(longitudinal_planner.v_desired_trajectory_full, ModelConstants.T_IDXS)
+  model_odo = cumtrapz(lateral_planner.v_plan, ModelConstants.T_IDXS)
 
   ui_send = messaging.new_message('uiPlan')
   ui_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2'])
   uiPlan = ui_send.uiPlan
   uiPlan.frameId = sm['modelV2'].frameId
-  uiPlan.position.x = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,0]).tolist()
-  uiPlan.position.y = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,1]).tolist()
+  uiPlan.position.x = np.interp(plan_odo, model_odo, lateral_planner.x_sol[:,0]).tolist()
+  uiPlan.position.y = np.interp(plan_odo, model_odo, lateral_planner.x_sol[:,1]).tolist()
   uiPlan.position.z = np.interp(plan_odo, model_odo, lateral_planner.path_xyz[:,2]).tolist()
   uiPlan.accel = longitudinal_planner.a_desired_trajectory_full.tolist()
   pm.send('uiPlan', ui_send)
 
-def plannerd_thread(sm=None, pm=None):
+def plannerd_thread():
   config_realtime_process(5, Priority.CTRL_LOW)
 
   cloudlog.info("plannerd is waiting for CarParams")
@@ -41,12 +41,9 @@ def plannerd_thread(sm=None, pm=None):
   longitudinal_planner = LongitudinalPlanner(CP)
   lateral_planner = LateralPlanner(CP, debug=debug_mode)
 
-  if sm is None:
-    sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'],
-                             poll=['radarState', 'modelV2'], ignore_avg_freq=['radarState'])
-
-  if pm is None:
-    pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan', 'uiPlan'])
+  pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan', 'uiPlan'])
+  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'],
+                           poll=['radarState', 'modelV2'], ignore_avg_freq=['radarState'])
 
   while True:
     sm.update()
@@ -58,8 +55,8 @@ def plannerd_thread(sm=None, pm=None):
       longitudinal_planner.publish(sm, pm)
       publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner)
 
-def main(sm=None, pm=None):
-  plannerd_thread(sm, pm)
+def main():
+  plannerd_thread()
 
 
 if __name__ == "__main__":
diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py
index bba3ad7cff..ec42ae66f2 100755
--- a/selfdrive/controls/radard.py
+++ b/selfdrive/controls/radard.py
@@ -6,12 +6,12 @@ from typing import Optional, Dict, Any
 
 import capnp
 from cereal import messaging, log, car
-from common.numpy_fast import interp
-from common.params import Params
-from common.realtime import Ratekeeper, Priority, config_realtime_process
-from system.swaglog import cloudlog
+from openpilot.common.numpy_fast import interp
+from openpilot.common.params import Params
+from openpilot.common.realtime import Ratekeeper, Priority, config_realtime_process
+from openpilot.system.swaglog import cloudlog
 
-from common.kalman.simple_kalman import KF1D
+from openpilot.common.kalman.simple_kalman import KF1D
 
 
 # Default lead acceleration decay set to 50% at 1s
@@ -50,7 +50,8 @@ class KalmanParams:
 
 
 class Track:
-  def __init__(self, v_lead: float, kalman_params: KalmanParams):
+  def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams):
+    self.identifier = identifier
     self.cnt = 0
     self.aLeadTau = _LEAD_ACCEL_TAU
     self.K_A = kalman_params.A
@@ -98,11 +99,12 @@ class Track:
       "vLead": float(self.vLead),
       "vLeadK": float(self.vLeadK),
       "aLeadK": float(self.aLeadK),
+      "aLeadTau": float(self.aLeadTau),
       "status": True,
       "fcw": self.is_potential_fcw(model_prob),
       "modelProb": model_prob,
       "radar": True,
-      "aLeadTau": float(self.aLeadTau)
+      "radarTrackId": self.identifier,
     }
 
   def potential_low_speed_lead(self, v_ego: float):
@@ -158,12 +160,14 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa
     "aLeadTau": 0.3,
     "fcw": False,
     "modelProb": float(lead_msg.prob),
+    "status": True,
     "radar": False,
-    "status": True
+    "radarTrackId": -1,
   }
 
 
-def get_lead(v_ego: float, ready: bool, tracks: Dict[int, Track], lead_msg: capnp._DynamicStructReader, model_v_ego: float, low_speed_override: bool = True) -> Dict[str, Any]:
+def get_lead(v_ego: float, ready: bool, tracks: Dict[int, Track], lead_msg: capnp._DynamicStructReader,
+             model_v_ego: float, low_speed_override: bool = True) -> Dict[str, Any]:
   # Determine leads, this is where the essential logic happens
   if len(tracks) > 0 and ready and lead_msg.prob > .5:
     track = match_vision_to_track(v_ego, lead_msg, tracks)
@@ -236,7 +240,7 @@ class RadarD:
 
       # create the track if it doesn't exist or it's a new track
       if ids not in self.tracks:
-        self.tracks[ids] = Track(v_lead, self.kalman_params)
+        self.tracks[ids] = Track(ids, v_lead, self.kalman_params)
       self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3])
 
     # *** publish radarState ***
diff --git a/selfdrive/debug/can_printer.py b/selfdrive/debug/can_printer.py
index 3f991d4e6c..220008979d 100755
--- a/selfdrive/debug/can_printer.py
+++ b/selfdrive/debug/can_printer.py
@@ -1,17 +1,17 @@
 #!/usr/bin/env python3
 import argparse
 import binascii
+import time
 from collections import defaultdict
 
 import cereal.messaging as messaging
-from common.realtime import sec_since_boot
 
 
 def can_printer(bus, max_msg, addr, ascii_decode):
   logcan = messaging.sub_sock('can', addr=addr)
 
-  start = sec_since_boot()
-  lp = sec_since_boot()
+  start = time.monotonic()
+  lp = time.monotonic()
   msgs = defaultdict(list)
   while 1:
     can_recv = messaging.drain_sock(logcan, wait_for_one=True)
@@ -20,17 +20,17 @@ def can_printer(bus, max_msg, addr, ascii_decode):
         if y.src == bus:
           msgs[y.address].append(y.dat)
 
-    if sec_since_boot() - lp > 0.1:
+    if time.monotonic() - lp > 0.1:
       dd = chr(27) + "[2J"
-      dd += f"{sec_since_boot() - start:5.2f}\n"
+      dd += f"{time.monotonic() - start:5.2f}\n"
       for addr in sorted(msgs.keys()):
         a = f"\"{msgs[addr][-1].decode('ascii', 'backslashreplace')}\"" if ascii_decode else ""
         x = binascii.hexlify(msgs[addr][-1]).decode('ascii')
-        freq = len(msgs[addr]) / (sec_since_boot() - start)
+        freq = len(msgs[addr]) / (time.monotonic() - start)
         if max_msg is None or addr < max_msg:
           dd += "%04X(%4d)(%6d)(%3dHz) %s %s\n" % (addr, addr, len(msgs[addr]), freq, x.ljust(20), a)
       print(dd)
-      lp = sec_since_boot()
+      lp = time.monotonic()
 
 if __name__ == "__main__":
   parser = argparse.ArgumentParser(description="simple CAN data viewer",
diff --git a/selfdrive/debug/check_freq.py b/selfdrive/debug/check_freq.py
index 6436abb4f1..7e7b05e950 100755
--- a/selfdrive/debug/check_freq.py
+++ b/selfdrive/debug/check_freq.py
@@ -1,10 +1,10 @@
 #!/usr/bin/env python3
 import argparse
 import numpy as np
+import time
 from collections import defaultdict, deque
 from typing import DefaultDict, Deque, MutableSequence
 
-from common.realtime import sec_since_boot
 import cereal.messaging as messaging
 
 
@@ -22,7 +22,7 @@ if __name__ == "__main__":
   rcv_times: DefaultDict[str, MutableSequence[float]] = defaultdict(lambda: deque(maxlen=100))
   valids: DefaultDict[str, Deque[bool]] = defaultdict(lambda: deque(maxlen=100))
 
-  t = sec_since_boot()
+  t = time.monotonic()
   for name in socket_names:
     sock = messaging.sub_sock(name, poller=poller)
     sockets[sock] = name
@@ -36,7 +36,7 @@ if __name__ == "__main__":
 
       name = msg.which()
 
-      t = sec_since_boot()
+      t = time.monotonic()
       rcv_times[name].append(msg.logMonoTime / 1e9)
       valids[name].append(msg.valid)
 
diff --git a/selfdrive/debug/dump.py b/selfdrive/debug/dump.py
index 722cdc6406..db18f4c622 100755
--- a/selfdrive/debug/dump.py
+++ b/selfdrive/debug/dump.py
@@ -3,13 +3,14 @@ import os
 import sys
 import argparse
 import json
-from hexdump import hexdump
 import codecs
-codecs.register_error("strict", codecs.backslashreplace_errors)
+import cereal.messaging as messaging
 
+from hexdump import hexdump
 from cereal import log
-import cereal.messaging as messaging
-from cereal.services import service_list
+from cereal.services import SERVICE_LIST
+
+codecs.register_error("strict", codecs.backslashreplace_errors)
 
 if __name__ == "__main__":
 
@@ -30,7 +31,7 @@ if __name__ == "__main__":
 
   poller = messaging.Poller()
 
-  for m in args.socket if len(args.socket) > 0 else service_list:
+  for m in args.socket if len(args.socket) > 0 else SERVICE_LIST:
     messaging.sub_sock(m, poller, addr=args.addr)
 
   values = None
diff --git a/selfdrive/debug/filter_log_message.py b/selfdrive/debug/filter_log_message.py
index af52953936..20cef0fcc0 100755
--- a/selfdrive/debug/filter_log_message.py
+++ b/selfdrive/debug/filter_log_message.py
@@ -4,8 +4,8 @@ import argparse
 import json
 
 import cereal.messaging as messaging
-from tools.lib.logreader import LogReader
-from tools.lib.route import Route
+from openpilot.tools.lib.logreader import LogReader
+from openpilot.tools.lib.route import Route
 
 LEVELS = {
   "DEBUG": 10,
@@ -59,7 +59,7 @@ if __name__ == "__main__":
       logs = [args.route[0]]
     else:
       r = Route(args.route[0])
-      logs = [q_log if r_log is None else r_log for (q_log, r_log) in zip(r.qlog_paths(), r.log_paths())]
+      logs = [q_log if r_log is None else r_log for (q_log, r_log) in zip(r.qlog_paths(), r.log_paths(), strict=True)]
 
   if len(args.route) == 2 and logs:
     n = int(args.route[1])
diff --git a/selfdrive/debug/hyundai_enable_radar_points.py b/selfdrive/debug/hyundai_enable_radar_points.py
index ac3651bf2f..7735391f4f 100755
--- a/selfdrive/debug/hyundai_enable_radar_points.py
+++ b/selfdrive/debug/hyundai_enable_radar_points.py
@@ -62,6 +62,10 @@ SUPPORTED_FW_VERSIONS = {
   b"TM__ SCC F-CUP      1.00 1.02 99110-S2000\x18\x07\x08\x18W    ": ConfigValues(
     default_config=b"\x00\x00\x00\x01\x00\x00",
     tracks_enabled=b"\x00\x00\x00\x01\x00\x01"),
+  # 2021 K5 HEV
+  b"DLhe SCC FHCUP      1.00 1.02 99110-L7000 \x01 \x102    ": ConfigValues(
+    default_config=b"\x00\x00\x00\x01\x00\x00",
+    tracks_enabled=b"\x00\x00\x00\x01\x00\x01"),
 }
 
 if __name__ == "__main__":
diff --git a/selfdrive/debug/uiview.py b/selfdrive/debug/uiview.py
index 93d901f7c9..f4440a912c 100755
--- a/selfdrive/debug/uiview.py
+++ b/selfdrive/debug/uiview.py
@@ -2,8 +2,8 @@
 import time
 
 from cereal import car, log, messaging
-from common.params import Params
-from selfdrive.manager.process_config import managed_processes
+from openpilot.common.params import Params
+from openpilot.selfdrive.manager.process_config import managed_processes
 
 if __name__ == "__main__":
   CP = car.CarParams(notCar=True)
diff --git a/selfdrive/debug/vw_mqb_config.py b/selfdrive/debug/vw_mqb_config.py
index 6b5ec36935..75409e3f87 100755
--- a/selfdrive/debug/vw_mqb_config.py
+++ b/selfdrive/debug/vw_mqb_config.py
@@ -144,6 +144,7 @@ if __name__ == "__main__":
       uds_client.write_data_by_identifier(VOLKSWAGEN_DATA_IDENTIFIER_TYPE.CODING, new_coding)  # type: ignore
     except (NegativeResponseError, MessageTimeoutError):
       print("Writing new configuration failed!")
+      print("Make sure the comma processes are stopped: tmux kill-session -t comma")
       quit()
 
     try:
diff --git a/selfdrive/hardware b/selfdrive/hardware
deleted file mode 120000
index 02a42c502f..0000000000
--- a/selfdrive/hardware
+++ /dev/null
@@ -1 +0,0 @@
-../system/hardware/
\ No newline at end of file
diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript
index 740f827a49..a6febe0170 100644
--- a/selfdrive/locationd/SConscript
+++ b/selfdrive/locationd/SConscript
@@ -7,8 +7,4 @@ locationd_sources = ["locationd.cc", "models/live_kf.cc", ekf_sym_cc]
 lenv = env.Clone()
 lenv["_LIBFLAGS"] += f' {libkf[0].get_labspath()}'
 locationd = lenv.Program("locationd", locationd_sources, LIBS=loc_libs + transformations)
-lenv.Depends(locationd, libkf)
-
-if File("liblocationd.cc").exists():
-  liblocationd = lenv.SharedLibrary("liblocationd", ["liblocationd.cc"] + locationd_sources, LIBS=loc_libs + transformations)
-  lenv.Depends(liblocationd, libkf)
\ No newline at end of file
+lenv.Depends(locationd, libkf)
\ No newline at end of file
diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py
index e990a46f12..566373d7b8 100755
--- a/selfdrive/locationd/calibrationd.py
+++ b/selfdrive/locationd/calibrationd.py
@@ -14,11 +14,11 @@ from typing import List, NoReturn, Optional
 
 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.orientation import rot_from_euler, euler_from_rot
-from system.swaglog import cloudlog
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.params import Params, put_nonblocking
+from openpilot.common.realtime import set_realtime_priority
+from openpilot.common.transformations.orientation import rot_from_euler, euler_from_rot
+from openpilot.system.swaglog import cloudlog
 
 MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS
 MAX_VEL_ANGLE_STD = np.radians(0.25)
@@ -31,7 +31,8 @@ SMOOTH_CYCLES = 10
 BLOCK_SIZE = 100
 INPUTS_NEEDED = 5   # Minimum blocks needed for valid calibration
 INPUTS_WANTED = 50   # We want a little bit more than we need for stability
-MAX_ALLOWED_SPREAD = np.radians(2)
+MAX_ALLOWED_YAW_SPREAD = np.radians(2)
+MAX_ALLOWED_PITCH_SPREAD = np.radians(4)
 RPY_INIT = np.array([0.0,0.0,0.0])
 WIDE_FROM_DEVICE_EULER_INIT = np.array([0.0, 0.0, 0.0])
 HEIGHT_INIT = np.array([1.22])
@@ -156,7 +157,8 @@ class Calibrator:
     # If spread is too high, assume mounting was changed and reset to last block.
     # Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model.
     # TODO: add height spread check with smooth transition too
-    if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == log.LiveCalibrationData.Status.calibrated:
+    spread_too_high = self.calib_spread[1] > MAX_ALLOWED_PITCH_SPREAD or self.calib_spread[2] > MAX_ALLOWED_YAW_SPREAD
+    if spread_too_high and self.cal_status == log.LiveCalibrationData.Status.calibrated:
       self.reset(self.rpys[self.block_idx - 1], valid_blocks=1, smooth_from=self.rpy)
       self.cal_status = log.LiveCalibrationData.Status.recalibrating
 
@@ -211,7 +213,8 @@ class Calibrator:
       new_height = HEIGHT_INIT
 
     self.rpys[self.block_idx] = moving_avg_with_linear_decay(self.rpys[self.block_idx], new_rpy, self.idx, float(BLOCK_SIZE))
-    self.wide_from_device_eulers[self.block_idx] = moving_avg_with_linear_decay(self.wide_from_device_eulers[self.block_idx], new_wide_from_device_euler, self.idx, float(BLOCK_SIZE))
+    self.wide_from_device_eulers[self.block_idx] = moving_avg_with_linear_decay(self.wide_from_device_eulers[self.block_idx],
+                                                                                new_wide_from_device_euler, self.idx, float(BLOCK_SIZE))
     self.heights[self.block_idx] = moving_avg_with_linear_decay(self.heights[self.block_idx], new_height, self.idx, float(BLOCK_SIZE))
 
     self.idx = (self.idx + 1) % BLOCK_SIZE
diff --git a/selfdrive/locationd/helpers.py b/selfdrive/locationd/helpers.py
new file mode 100644
index 0000000000..bde21f7879
--- /dev/null
+++ b/selfdrive/locationd/helpers.py
@@ -0,0 +1,77 @@
+import numpy as np
+import signal
+import sys
+from typing import List, Optional, Tuple, Any
+
+from cereal import log
+from openpilot.common.params import Params
+from openpilot.system.swaglog import cloudlog
+
+
+class NPQueue:
+  def __init__(self, maxlen: int, rowsize: int) -> None:
+    self.maxlen = maxlen
+    self.arr = np.empty((0, rowsize))
+
+  def __len__(self) -> int:
+    return len(self.arr)
+
+  def append(self, pt: List[float]) -> None:
+    if len(self.arr) < self.maxlen:
+      self.arr = np.append(self.arr, [pt], axis=0)
+    else:
+      self.arr[:-1] = self.arr[1:]
+      self.arr[-1] = pt
+
+
+class PointBuckets:
+  def __init__(self, x_bounds: List[Tuple[float, float]], min_points: List[float], min_points_total: int, points_per_bucket: int, rowsize: int) -> None:
+    self.x_bounds = x_bounds
+    self.buckets = {bounds: NPQueue(maxlen=points_per_bucket, rowsize=rowsize) for bounds in x_bounds}
+    self.buckets_min_points = dict(zip(x_bounds, min_points, strict=True))
+    self.min_points_total = min_points_total
+
+  def bucket_lengths(self) -> List[int]:
+    return [len(v) for v in self.buckets.values()]
+
+  def __len__(self) -> int:
+    return sum(self.bucket_lengths())
+
+  def is_valid(self) -> bool:
+    individual_buckets_valid = all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values(), strict=True))
+    total_points_valid = self.__len__() >= self.min_points_total
+    return individual_buckets_valid and total_points_valid
+
+  def add_point(self, x: float, y: float, bucket_val: float) -> None:
+    raise NotImplementedError
+
+  def get_points(self, num_points: Optional[int] = None) -> Any:
+    points = np.vstack([x.arr for x in self.buckets.values()])
+    if num_points is None:
+      return points
+    return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)]
+
+  def load_points(self, points: List[List[float]]) -> None:
+    for point in points:
+      self.add_point(*point)
+
+
+class ParameterEstimator:
+  """ Base class for parameter estimators """
+  def reset(self) -> None:
+    raise NotImplementedError
+
+  def handle_log(self, t: int, which: str, msg: log.Event) -> None:
+    raise NotImplementedError
+
+  def get_msg(self, valid: bool, with_points: bool) -> log.Event:
+    raise NotImplementedError
+
+
+def cache_points_onexit(param_name, estimator, sig, frame):
+  signal.signal(sig, signal.SIG_DFL)
+  cloudlog.warning(f"Caching {param_name} param")
+  params = Params()
+  msg = estimator.get_msg(valid=True, with_points=True)
+  params.put(param_name, msg.to_bytes())
+  sys.exit(0)
diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py
deleted file mode 100755
index 5d8641a91b..0000000000
--- a/selfdrive/locationd/laikad.py
+++ /dev/null
@@ -1,474 +0,0 @@
-#!/usr/bin/env python3
-import math
-import os
-import time
-import shutil
-from collections import defaultdict
-from concurrent.futures import Future, ProcessPoolExecutor
-from enum import IntEnum
-from typing import List, Optional, Dict, Any
-
-import numpy as np
-
-from cereal import log, messaging
-from common.params import Params, put_nonblocking
-from laika import AstroDog
-from laika.constants import SECS_IN_HR, SECS_IN_MIN
-from laika.downloader import DownloadFailed
-from laika.ephemeris import EphemerisType, GPSEphemeris, GLONASSEphemeris, ephemeris_structs, parse_qcom_ephem
-from laika.gps_time import GPSTime
-from laika.helpers import ConstellationId, get_sv_id
-from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom
-from laika.opt import calc_pos_fix, get_posfix_sympy_fun, calc_vel_fix, get_velfix_sympy_func
-from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind
-from selfdrive.locationd.models.gnss_kf import GNSSKalman
-from selfdrive.locationd.models.gnss_kf import States as GStates
-from system.swaglog import cloudlog
-
-MAX_TIME_GAP = 10
-EPHEMERIS_CACHE = 'LaikadEphemerisV3'
-DOWNLOADS_CACHE_FOLDER = "/tmp/comma_download_cache/"
-CACHE_VERSION = 0.2
-POS_FIX_RESIDUAL_THRESHOLD = 100.0
-
-
-class LogEphemerisType(IntEnum):
-  nav = 0
-  nasaUltraRapid = 1
-  glonassIacUltraRapid = 2
-  qcom = 3
-
-class EphemerisSource(IntEnum):
-  gnssChip = 0
-  internet = 1
-  cache = 2
-  unknown = 3
-
-def get_log_eph_type(ephem):
-  if ephem.eph_type == EphemerisType.NAV:
-    source_type = LogEphemerisType.nav
-  elif ephem.eph_type == EphemerisType.QCOM_POLY:
-    source_type = LogEphemerisType.qcom
-  else:
-    assert ephem.file_epoch is not None
-    file_src = ephem.file_source
-    if file_src == 'igu':  # example nasa: '2214/igu22144_00.sp3.Z'
-      source_type = LogEphemerisType.nasaUltraRapid
-    elif file_src == 'Sta':  # example nasa: '22166/ultra/Stark_1D_22061518.sp3'
-      source_type = LogEphemerisType.glonassIacUltraRapid
-    else:
-      raise Exception(f"Didn't expect file source {file_src}")
-  return source_type
-
-def get_log_eph_source(ephem):
-  if ephem.file_name == 'qcom' or ephem.file_name == 'ublox':
-    source = EphemerisSource.gnssChip
-  elif ephem.file_name == EPHEMERIS_CACHE:
-    source = EphemerisSource.cache
-  else:
-    source = EphemerisSource.internet
-  return source
-
-
-class Laikad:
-  def __init__(self, valid_const=(ConstellationId.GPS, ConstellationId.GLONASS), auto_fetch_navs=True, auto_update=False,
-               valid_ephem_types=(EphemerisType.NAV, EphemerisType.QCOM_POLY),
-               save_ephemeris=False, use_qcom=False):
-    """
-    valid_const: GNSS constellation which can be used
-    auto_fetch_navs: If true fetch navs from internet when needed
-    auto_update: If true download AstroDog will download all files needed. This can be ephemeris or correction data like ionosphere.
-    valid_ephem_types: Valid ephemeris types to be used by AstroDog
-    save_ephemeris: If true saves and loads nav and orbit ephemeris to cache.
-    """
-    self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, clear_old_ephemeris=True, cache_dir=DOWNLOADS_CACHE_FOLDER)
-    self.gnss_kf = GNSSKalman(GENERATED_DIR, cython=True, erratic_clock=use_qcom)
-
-    self.auto_fetch_navs = auto_fetch_navs
-    self.orbit_fetch_executor: Optional[ProcessPoolExecutor] = None
-    self.orbit_fetch_future: Optional[Future] = None
-
-    self.last_report_time = GPSTime(0, 0)
-    self.last_fetch_navs_t = GPSTime(0, 0)
-    self.last_cached_t = GPSTime(0, 0)
-    self.save_ephemeris = save_ephemeris
-    self.load_cache()
-
-    self.posfix_functions = {constellation: get_posfix_sympy_fun(constellation) for constellation in (ConstellationId.GPS, ConstellationId.GLONASS)}
-    self.velfix_function = get_velfix_sympy_func()
-    self.last_fix_pos = None
-    self.last_fix_t = None
-    self.use_qcom = use_qcom
-    self.first_log_time = None
-    self.ttff = -1
-
-    # qcom specific stuff
-    self.qcom_reports_received = 1
-    self.qcom_reports = []
-
-  def load_cache(self):
-    if not self.save_ephemeris:
-      return
-
-    cache_bytes = Params().get(EPHEMERIS_CACHE)
-    if not cache_bytes:
-      return
-
-    nav_dict = {}
-    try:
-      with ephemeris_structs.EphemerisCache.from_bytes(cache_bytes) as ephem_cache:
-        glonass_navs = [GLONASSEphemeris(data_struct, file_name=EPHEMERIS_CACHE) for data_struct in ephem_cache.glonassEphemerides]
-        gps_navs = [GPSEphemeris(data_struct, file_name=EPHEMERIS_CACHE) for data_struct in ephem_cache.gpsEphemerides]
-      for e in sum([glonass_navs, gps_navs], []):
-        if e.prn not in nav_dict:
-          nav_dict[e.prn] = []
-        nav_dict[e.prn].append(e)
-      self.astro_dog.add_navs(nav_dict)
-    except Exception:
-      cloudlog.exception("Error parsing cache")
-    cloudlog.debug(
-      f"Loaded navs ({sum([len(nav_dict[prn]) for prn in nav_dict.keys()])}). Unique orbit and nav sats: {list(nav_dict.keys())} ")
-
-  def cache_ephemeris(self):
-
-    if self.save_ephemeris and (self.last_report_time - self.last_cached_t > SECS_IN_MIN):
-      nav_list: List = sum([v for k,v in self.astro_dog.navs.items()], [])
-      #TODO this only saves currently valid ephems, when we download future ephems we should save them too
-      valid_navs = [e for e in nav_list if e.valid(self.last_report_time)]
-      if len(valid_navs) > 0:
-        ephem_cache = ephemeris_structs.EphemerisCache(**{'glonassEphemerides': [e.data for e in valid_navs if e.prn[0]=='R'],
-                                                          'gpsEphemerides': [e.data for e in valid_navs if e.prn[0]=='G']})
-        put_nonblocking(EPHEMERIS_CACHE, ephem_cache.to_bytes())
-        cloudlog.debug("Cache saved")
-      self.last_cached_t = self.last_report_time
-
-  def create_ephem_statuses(self):
-    ephemeris_statuses = []
-    eph_list: List = sum([v for k,v in self.astro_dog.navs.items()], []) + sum([v for k,v in self.astro_dog.qcom_polys.items()], [])
-    for eph in eph_list:
-      status = log.GnssMeasurements.EphemerisStatus.new_message()
-      status.constellationId = ConstellationId.from_rinex_char(eph.prn[0]).value
-      status.svId = get_sv_id(eph.prn)
-      status.type = get_log_eph_type(eph).value
-      status.source = get_log_eph_source(eph).value
-      status.tow = eph.epoch.tow
-      status.gpsWeek = eph.epoch.week
-      ephemeris_statuses.append(status)
-    return ephemeris_statuses
-
-
-  def get_lsq_fix(self, t, measurements):
-    if self.last_fix_t is None or abs(self.last_fix_t - t) > 0:
-      min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in measurements) else 4
-      position_solution, pr_residuals, pos_std = calc_pos_fix(measurements, self.posfix_functions, min_measurements=min_measurements)
-      if len(position_solution) < 3:
-        return None
-      position_estimate = position_solution[:3]
-
-      position_std_residual = np.median(np.abs(pr_residuals))
-      position_std = np.median(np.abs(pos_std))/10
-      position_std = max(position_std_residual, position_std) * np.ones(3)
-
-      velocity_solution, prr_residuals, vel_std = calc_vel_fix(measurements, position_estimate, self.velfix_function, min_measurements=min_measurements)
-      if len(velocity_solution) < 3:
-        return None
-      velocity_estimate = velocity_solution[:3]
-
-      velocity_std_residual = np.median(np.abs(prr_residuals))
-      velocity_std = np.median(np.abs(vel_std))/10
-      velocity_std = max(velocity_std, velocity_std_residual) * np.ones(3)
-
-      return position_estimate, position_std, velocity_estimate, velocity_std
-
-  def gps_time_from_qcom_report(self, gnss_msg):
-    report = gnss_msg.drMeasurementReport
-    if report.source == log.QcomGnss.MeasurementSource.gps:
-      report_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0)
-    elif report.source == log.QcomGnss.MeasurementSource.sbas:
-      report_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0)
-    elif report.source == log.QcomGnss.MeasurementSource.glonass:
-      report_time = GPSTime.from_glonass(report.glonassYear,
-                                            report.glonassDay,
-                                            report.glonassMilliseconds / 1000.0)
-    else:
-      raise NotImplementedError(f'Unknownconstellation {report.source}')
-    return report_time
-
-  def is_good_report(self, gnss_msg):
-    if gnss_msg.which() == 'drMeasurementReport' and self.use_qcom:
-      # TODO: Understand and use remaining unknown constellations
-      try:
-        constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source)
-        good_constellation = constellation_id in [ConstellationId.GPS, ConstellationId.SBAS, ConstellationId.GLONASS]
-        report_time = self.gps_time_from_qcom_report(gnss_msg)
-      except NotImplementedError:
-        return False
-      # Garbage timestamps with week > 32767 are sometimes sent by module.
-      # This is an issue with gpsTime and GLONASS time.
-      good_week = report_time.week < np.iinfo(np.int16).max
-      return good_constellation and good_week
-    elif gnss_msg.which() == 'measurementReport' and not self.use_qcom:
-      return True
-    else:
-      return False
-
-  def read_report(self, gnss_msg):
-    if self.use_qcom:
-      # QCOM reports are per constellation, so we need to aggregate them
-      report = gnss_msg.drMeasurementReport
-      report_time = self.gps_time_from_qcom_report(gnss_msg)
-
-      if report_time - self.last_report_time > 0:
-        self.qcom_reports_received = max(1, len(self.qcom_reports))
-        self.qcom_reports = [report]
-      else:
-        self.qcom_reports.append(report)
-      self.last_report_time = report_time
-
-      new_meas = []
-      if len(self.qcom_reports) == self.qcom_reports_received:
-        for report in self.qcom_reports:
-          new_meas.extend(read_raw_qcom(report))
-
-    else:
-      report = gnss_msg.measurementReport
-      self.last_report_time = GPSTime(report.gpsWeek, report.rcvTow)
-      new_meas = read_raw_ublox(report)
-    return self.last_report_time, new_meas
-
-  def is_ephemeris(self, gnss_msg):
-    if self.use_qcom:
-      return gnss_msg.which() == 'drSvPoly'
-    else:
-      return gnss_msg.which() in ('ephemeris', 'glonassEphemeris')
-
-  def read_ephemeris(self, gnss_msg):
-    if self.use_qcom:
-      try:
-        ephem = parse_qcom_ephem(gnss_msg.drSvPoly)
-        self.astro_dog.add_qcom_polys({ephem.prn: [ephem]})
-      except Exception:
-        cloudlog.exception("Error parsing qcom svPoly ephemeris from qcom module")
-        return
-
-    else:
-      if gnss_msg.which() == 'ephemeris':
-        data_struct = ephemeris_structs.Ephemeris.new_message(**gnss_msg.ephemeris.to_dict())
-        try:
-          ephem = GPSEphemeris(data_struct, file_name='ublox')
-        except Exception:
-          cloudlog.exception("Error parsing GPS ephemeris from ublox")
-          return
-      elif gnss_msg.which() == 'glonassEphemeris':
-        data_struct = ephemeris_structs.GlonassEphemeris.new_message(**gnss_msg.glonassEphemeris.to_dict())
-        try:
-          ephem = GLONASSEphemeris(data_struct, file_name='ublox')
-        except Exception:
-          cloudlog.exception("Error parsing GLONASS ephemeris from ublox")
-          return
-      else:
-        cloudlog.error(f"Unsupported ephemeris type: {gnss_msg.which()}")
-        return
-      self.astro_dog.add_navs({ephem.prn: [ephem]})
-    self.cache_ephemeris()
-
-  def process_report(self, new_meas, t):
-    # Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites
-    new_meas = [m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7]
-    processed_measurements = process_measurements(new_meas, self.astro_dog)
-    if self.last_fix_pos is not None:
-      est_pos = self.last_fix_pos
-      correct_delay = True
-    else:
-      est_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist()
-      correct_delay = False
-    corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog, correct_delay=correct_delay)
-    return corrected_measurements
-
-  def calc_fix(self, t, measurements):
-    instant_fix = self.get_lsq_fix(t, measurements)
-    if instant_fix is None:
-      return None
-    else:
-      position_estimate, position_std, velocity_estimate, velocity_std = instant_fix
-      self.last_fix_t = t
-      self.last_fix_pos = position_estimate
-      self.lat_fix_pos_std = position_std
-      return position_estimate, position_std, velocity_estimate, velocity_std
-
-  def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False):
-    out_msg = messaging.new_message("gnssMeasurements")
-    t = gnss_mono_time * 1e-9
-    msg_dict: Dict[str, Any] = {"measTime": gnss_mono_time}
-    if self.first_log_time is None:
-      self.first_log_time = 1e-9 * gnss_mono_time
-    if self.is_ephemeris(gnss_msg):
-      self.read_ephemeris(gnss_msg)
-    elif self.is_good_report(gnss_msg):
-      report_t, new_meas = self.read_report(gnss_msg)
-      if report_t.week > 0:
-        if self.auto_fetch_navs:
-          self.fetch_navs(report_t, block)
-
-      corrected_measurements = self.process_report(new_meas, t)
-      msg_dict['correctedMeasurements'] = [create_measurement_msg(m) for m in corrected_measurements]
-
-      fix = self.calc_fix(t, corrected_measurements)
-      measurement_msg = log.LiveLocationKalman.Measurement.new_message
-      if fix is not None:
-        position_estimate, position_std, velocity_estimate, velocity_std = fix
-        if self.ttff <= 0:
-          self.ttff = max(1e-3, t - self.first_log_time)
-        msg_dict["positionECEF"] = measurement_msg(value=position_estimate, std=position_std.tolist(), valid=bool(self.last_fix_t == t))
-        msg_dict["velocityECEF"] = measurement_msg(value=velocity_estimate, std=velocity_std.tolist(), valid=bool(self.last_fix_t == t))
-
-      self.update_localizer(self.last_fix_pos, t, corrected_measurements)
-      P_diag = self.gnss_kf.P.diagonal()
-      kf_valid = all(self.kf_valid(t))
-      msg_dict["kalmanPositionECEF"] = measurement_msg(value=self.gnss_kf.x[GStates.ECEF_POS].tolist(),
-                                        std=np.sqrt(P_diag[GStates.ECEF_POS]).tolist(),
-                                        valid=kf_valid)
-      msg_dict["kalmanVelocityECEF"] = measurement_msg(value=self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist(),
-                                        std=np.sqrt(P_diag[GStates.ECEF_VELOCITY]).tolist(),
-                                        valid=kf_valid)
-
-    msg_dict['gpsWeek'] = self.last_report_time.week
-    msg_dict['gpsTimeOfWeek'] = self.last_report_time.tow
-    msg_dict['timeToFirstFix'] = self.ttff
-    msg_dict['ephemerisStatuses'] = self.create_ephem_statuses()
-    out_msg.gnssMeasurements = msg_dict
-    return out_msg
-
-  def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]):
-    # Check time and outputs are valid
-    valid = self.kf_valid(t)
-    if not all(valid):
-      if not valid[0]:  # Filter not initialized
-        pass
-      elif not valid[1]:
-        cloudlog.error("Time gap of over 10s detected, gnss kalman reset")
-      elif not valid[2]:
-        cloudlog.error("Gnss kalman filter state is nan")
-      if est_pos is not None and len(est_pos) > 0:
-        cloudlog.info(f"Reset kalman filter with {est_pos}")
-        self.init_gnss_localizer(est_pos)
-      else:
-        return
-    if len(measurements) > 0:
-      kf_add_observations(self.gnss_kf, t, measurements)
-    else:
-      # Ensure gnss filter is updated even with no new measurements
-      self.gnss_kf.predict(t)
-
-  def kf_valid(self, t: float) -> List[bool]:
-    filter_time = self.gnss_kf.filter.get_filter_time()
-    return [not math.isnan(filter_time),
-            abs(t - filter_time) < MAX_TIME_GAP,
-            all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS]))]
-
-  def init_gnss_localizer(self, est_pos):
-    x_initial, p_initial_diag = np.copy(GNSSKalman.x_initial), np.copy(np.diagonal(GNSSKalman.P_initial))
-    x_initial[GStates.ECEF_POS] = est_pos
-    p_initial_diag[GStates.ECEF_POS] = 1000 ** 2
-    self.gnss_kf.init_state(x_initial, covs_diag=p_initial_diag)
-
-  def fetch_navs(self, t: GPSTime, block):
-    # Download new navs if 1 hour of navs data left
-    if t + SECS_IN_HR not in self.astro_dog.navs_fetched_times and (abs(t - self.last_fetch_navs_t) > SECS_IN_MIN):
-      astro_dog_vars = self.astro_dog.valid_const, self.astro_dog.auto_update, self.astro_dog.valid_ephem_types, self.astro_dog.cache_dir
-      ret = None
-
-      if block:  # Used for testing purposes
-        ret = get_orbit_data(t, *astro_dog_vars)
-      elif self.orbit_fetch_future is None:
-        self.orbit_fetch_executor = ProcessPoolExecutor(max_workers=1)
-        self.orbit_fetch_future = self.orbit_fetch_executor.submit(get_orbit_data, t, *astro_dog_vars)
-      elif self.orbit_fetch_future.done():
-        ret = self.orbit_fetch_future.result()
-        self.orbit_fetch_executor = self.orbit_fetch_future = None
-
-      if ret is not None:
-        if ret[0] is None:
-          self.last_fetch_navs_t = ret[2]
-        else:
-          self.astro_dog.navs, self.astro_dog.navs_fetched_times, self.last_fetch_navs_t = ret
-          self.cache_ephemeris()
-
-
-def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types, cache_dir):
-  astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, cache_dir=cache_dir)
-  cloudlog.info(f"Start to download/parse navs for time {t.as_datetime()}")
-  start_time = time.monotonic()
-  try:
-    astro_dog.get_navs(t)
-    cloudlog.info(f"Done parsing navs. Took {time.monotonic() - start_time:.1f}s")
-    cloudlog.debug(f"Downloaded navs ({sum([len(v) for v in astro_dog.navs])}): {list(astro_dog.navs.keys())}" +
-                   f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in astro_dog.orbit_fetched_times._ranges]}")
-    return astro_dog.navs, astro_dog.navs_fetched_times, t
-  except (DownloadFailed, RuntimeError, ValueError, IOError) as e:
-    cloudlog.warning(f"No orbit data found or parsing failure: {e}")
-  return None, None, t
-
-
-def create_measurement_msg(meas: GNSSMeasurement):
-  c = log.GnssMeasurements.CorrectedMeasurement.new_message()
-  c.constellationId = meas.constellation_id.value
-  c.svId = meas.sv_id
-  c.glonassFrequency = meas.glonass_freq if meas.constellation_id == ConstellationId.GLONASS else 0
-  c.pseudorange = float(meas.observables_final['C1C'])
-  c.pseudorangeStd = float(meas.observables_std['C1C'])
-  c.pseudorangeRate = float(meas.observables_final['D1C'])
-  c.pseudorangeRateStd = float(meas.observables_std['D1C'])
-  c.satPos = meas.sat_pos_final.tolist()
-  c.satVel = meas.sat_vel.tolist()
-  c.satVel = meas.sat_vel.tolist()
-  return c
-
-def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMeasurement]):
-  ekf_data = defaultdict(list)
-  for m in measurements:
-    m_arr = m.as_array()
-    if m.constellation_id == ConstellationId.GPS:
-      ekf_data[ObservationKind.PSEUDORANGE_GPS].append(m_arr)
-    elif m.constellation_id == ConstellationId.GLONASS:
-      ekf_data[ObservationKind.PSEUDORANGE_GLONASS].append(m_arr)
-  ekf_data[ObservationKind.PSEUDORANGE_RATE_GPS] = ekf_data[ObservationKind.PSEUDORANGE_GPS]
-  ekf_data[ObservationKind.PSEUDORANGE_RATE_GLONASS] = ekf_data[ObservationKind.PSEUDORANGE_GLONASS]
-  for kind, data in ekf_data.items():
-    if len(data) > 0:
-      gnss_kf.predict_and_observe(t, kind, data)
-
-
-def clear_tmp_cache():
-  if os.path.exists(DOWNLOADS_CACHE_FOLDER):
-    shutil.rmtree(DOWNLOADS_CACHE_FOLDER)
-  os.mkdir(DOWNLOADS_CACHE_FOLDER)
-
-
-def main(sm=None, pm=None):
-  #clear_tmp_cache()
-
-  use_qcom = not Params().get_bool("UbloxAvailable")
-  if use_qcom:
-    raw_name = "qcomGnss"
-  else:
-    raw_name = "ubloxGnss"
-  raw_gnss_sock = messaging.sub_sock(raw_name, conflate=False)
-  if pm is None:
-    pm = messaging.PubMaster(['gnssMeasurements'])
-
-  # disable until set as main gps source, to better analyze startup time
-  # TODO ensure low CPU usage before enabling
-  use_internet = False  # "LAIKAD_NO_INTERNET" not in os.environ
-
-  replay = "REPLAY" in os.environ
-  laikad = Laikad(save_ephemeris=not replay, auto_fetch_navs=use_internet, use_qcom=use_qcom)
-
-  while True:
-    for in_msg in messaging.drain_sock(raw_gnss_sock, wait_for_one=True):
-      out_msg = laikad.process_gnss_msg(getattr(in_msg, raw_name), in_msg.logMonoTime, replay)
-      pm.send('gnssMeasurements', out_msg)
-
-
-if __name__ == "__main__":
-  main()
diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc
old mode 100755
new mode 100644
index 9b3e3b3b85..80882fc951
--- a/selfdrive/locationd/locationd.cc
+++ b/selfdrive/locationd/locationd.cc
@@ -1,9 +1,11 @@
+#include "selfdrive/locationd/locationd.h"
+
 #include <sys/time.h>
 #include <sys/resource.h>
 
+#include <algorithm>
 #include <cmath>
-
-#include "locationd.h"
+#include <vector>
 
 using namespace EKFS;
 using namespace Eigen;
@@ -19,9 +21,11 @@ const double VALID_TIME_SINCE_RESET = 1.0; // s
 const double VALID_POS_STD = 50.0; // m
 const double MAX_RESET_TRACKER = 5.0;
 const double SANE_GPS_UNCERTAINTY = 1500.0; // m
-const double INPUT_INVALID_THRESHOLD = 5.0; // same as reset tracker
-const double DECAY = 0.99995; // same as reset tracker
+const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker
+const double RESET_TRACKER_DECAY = 0.99995;
+const double DECAY = 0.9993; // ~10 secs to resume after a bad input
 const double MAX_FILTER_REWIND_TIME = 0.8; // s
+const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30;
 
 // TODO: GPS sensor time offsets are empirically calculated
 // They should be replaced with synced time from a real clock
@@ -240,8 +244,7 @@ void Localizer::handle_sensor(double current_time, const cereal::SensorEventData
     LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time");
     this->observation_timings_invalid = true;
     return;
-  }
-  else if (!this->is_timestamp_valid(sensor_time)) {
+  } else if (!this->is_timestamp_valid(sensor_time)) {
     this->observation_timings_invalid = true;
     return;
   }
@@ -255,11 +258,16 @@ void Localizer::handle_sensor(double current_time, const cereal::SensorEventData
   if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
     auto v = log.getGyroUncalibrated().getV();
     auto meas = Vector3d(-v[2], -v[1], -v[0]);
-    if (meas.norm() < ROTATION_SANITY_CHECK) {
+
+    VectorXd gyro_bias = this->kf->get_x().segment<STATE_GYRO_BIAS_LEN>(STATE_GYRO_BIAS_START);
+    float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]);
+    float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1];
+    bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold;
+
+    if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) {
       this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas });
       this->observation_values_invalid["gyroscope"] *= DECAY;
-    }
-    else{
+    } else {
       this->observation_values_invalid["gyroscope"] += 1.0;
     }
   }
@@ -277,8 +285,7 @@ void Localizer::handle_sensor(double current_time, const cereal::SensorEventData
     if (meas.norm() < ACCEL_SANITY_CHECK) {
       this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas });
       this->observation_values_invalid["accelerometer"] *= DECAY;
-    }
-    else{
+    } else {
       this->observation_values_invalid["accelerometer"] += 1.0;
     }
   }
@@ -293,8 +300,8 @@ void Localizer::input_fake_gps_observations(double current_time) {
   VectorXd current_x = this->kf->get_x();
   VectorXd ecef_pos = current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
   VectorXd ecef_vel = current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
-  MatrixXdr ecef_pos_R = this->kf->get_fake_gps_pos_cov();
-  MatrixXdr ecef_vel_R = this->kf->get_fake_gps_vel_cov();
+  const MatrixXdr &ecef_pos_R = this->kf->get_fake_gps_pos_cov();
+  const MatrixXdr &ecef_vel_R = this->kf->get_fake_gps_vel_cov();
 
   this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
   this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
@@ -360,7 +367,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
 
 void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) {
 
-  if(!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) {
+  if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) {
     this->determine_gps_mode(current_time);
     return;
   }
@@ -414,8 +421,7 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements:
   orientation_reset &= !this->standstill;
   if (orientation_reset) {
     this->orientation_reset_count++;
-  }
-  else {
+  } else {
     this->orientation_reset_count = 0;
   }
 
@@ -485,6 +491,7 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry
   this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION,
     { trans_device }, { trans_device_cov });
   this->observation_values_invalid["cameraOdometry"] *= DECAY;
+  this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]);
 }
 
 void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) {
@@ -509,8 +516,8 @@ void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibra
 }
 
 void Localizer::reset_kalman(double current_time) {
-  VectorXd init_x = this->kf->get_initial_x();
-  MatrixXdr init_P = this->kf->get_initial_P();
+  const VectorXd &init_x = this->kf->get_initial_x();
+  const MatrixXdr &init_P = this->kf->get_initial_P();
   this->reset_kalman(current_time, init_x, init_P);
 }
 
@@ -540,18 +547,18 @@ void Localizer::time_check(double current_time) {
 void Localizer::update_reset_tracker() {
   // reset tracker is tuned to trigger when over 1reset/10s over 2min period
   if (this->is_gps_ok()) {
-    this->reset_tracker *= DECAY;
+    this->reset_tracker *= RESET_TRACKER_DECAY;
   } else {
     this->reset_tracker = 0.0;
   }
 }
 
-void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos, VectorXd init_vel, MatrixXdr init_pos_R, MatrixXdr init_vel_R) {
+void Localizer::reset_kalman(double current_time, const VectorXd &init_orient, const VectorXd &init_pos, const VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R) {
   // too nonlinear to init on completely wrong
   VectorXd current_x = this->kf->get_x();
   MatrixXdr current_P = this->kf->get_P();
   MatrixXdr init_P = this->kf->get_initial_P();
-  MatrixXdr reset_orientation_P = this->kf->get_reset_orientation_P();
+  const MatrixXdr &reset_orientation_P = this->kf->get_reset_orientation_P();
   int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
 
   current_x.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START) = init_orient;
@@ -561,12 +568,13 @@ void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd
   init_P.block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal();
   init_P.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal();
   init_P.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal();
-  init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal();
+  init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START,
+    STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal();
 
   this->reset_kalman(current_time, current_x, init_P);
 }
 
-void Localizer::reset_kalman(double current_time, VectorXd init_x, MatrixXdr init_P) {
+void Localizer::reset_kalman(double current_time, const VectorXd &init_x, const MatrixXdr &init_P) {
   this->kf->init_state(init_x, init_P, current_time);
   this->last_reset_time = current_time;
   this->reset_tracker += 1.0;
@@ -621,7 +629,7 @@ bool Localizer::is_gps_ok() {
   return (this->kf->get_filter_time() - this->last_gps_msg) < 2.0;
 }
 
-bool Localizer::critical_services_valid(std::map<std::string, double> critical_services) {
+bool Localizer::critical_services_valid(const std::map<std::string, double> &critical_services) {
   for (auto &kv : critical_services){
     if (kv.second >= INPUT_INVALID_THRESHOLD){
       return false;
@@ -648,14 +656,13 @@ void Localizer::determine_gps_mode(double current_time) {
     if (this->gps_mode){
       this->gps_mode = false;
       this->reset_kalman(current_time);
-    }
-    else{
+    } else {
       this->input_fake_gps_observations(current_time);
     }
   }
 }
 
-void Localizer::configure_gnss_source(LocalizerGnssSource source) {
+void Localizer::configure_gnss_source(const LocalizerGnssSource &source) {
   this->gnss_source = source;
   if (source == LocalizerGnssSource::UBLOX) {
     this->gps_std_factor = 10.0;
@@ -713,7 +720,7 @@ int Localizer::locationd_thread() {
     // 100Hz publish for notcars, 20Hz for cars
     const char* trigger_msg = sm["carParams"].getCarParams().getNotCar() ? "accelerometer" : "cameraOdometry";
     if (sm.updated(trigger_msg)) {
-      bool inputsOK = sm.allAliveAndValid() && this->are_inputs_ok();
+      bool inputsOK = sm.allValid() && this->are_inputs_ok();
       bool gpsOK = this->is_gps_ok();
       bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"});
 
diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h
old mode 100755
new mode 100644
index e8f2f04a2c..47c8bf5627
--- a/selfdrive/locationd/locationd.h
+++ b/selfdrive/locationd/locationd.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <eigen3/Eigen/Dense>
+#include <deque>
 #include <fstream>
 #include <memory>
 #include <map>
@@ -32,13 +33,13 @@ public:
   int locationd_thread();
 
   void reset_kalman(double current_time = NAN);
-  void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos, Eigen::VectorXd init_vel, MatrixXdr init_pos_R, MatrixXdr init_vel_R);
-  void reset_kalman(double current_time, Eigen::VectorXd init_x, MatrixXdr init_P);
+  void reset_kalman(double current_time, const Eigen::VectorXd &init_orient, const Eigen::VectorXd &init_pos, const Eigen::VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R);
+  void reset_kalman(double current_time, const Eigen::VectorXd &init_x, const MatrixXdr &init_P);
   void finite_check(double current_time = NAN);
   void time_check(double current_time = NAN);
   void update_reset_tracker();
   bool is_gps_ok();
-  bool critical_services_valid(std::map<std::string, double> critical_services);
+  bool critical_services_valid(const std::map<std::string, double> &critical_services);
   bool is_timestamp_valid(double current_time);
   void determine_gps_mode(double current_time);
   bool are_inputs_ok();
@@ -93,6 +94,7 @@ private:
   float gps_variance_factor;
   float gps_vertical_variance_factor;
   double gps_time_offset;
+  Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std
 
-  void configure_gnss_source(LocalizerGnssSource source);
+  void configure_gnss_source(const LocalizerGnssSource &source);
 };
diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py
index 3faf4f8d4e..b87c83cac2 100755
--- a/selfdrive/locationd/models/car_kf.py
+++ b/selfdrive/locationd/models/car_kf.py
@@ -5,9 +5,9 @@ from typing import Any, Dict
 
 import numpy as np
 
-from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
-from selfdrive.locationd.models.constants import ObservationKind
-from system.swaglog import cloudlog
+from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.selfdrive.locationd.models.constants import ObservationKind
+from openpilot.system.swaglog import cloudlog
 
 from rednose.helpers.kalmanfilter import KalmanFilter
 
@@ -15,7 +15,7 @@ if __name__ == '__main__':  # Generating sympy
   import sympy as sp
   from rednose.helpers.ekf_sym import gen_code
 else:
-  from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx  # pylint: disable=no-name-in-module, import-error
+  from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
 
 
 i = 0
@@ -160,7 +160,7 @@ class CarKalman(KalmanFilter):
 
     gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
 
-  def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0, P_initial=None):  # pylint: disable=super-init-not-called
+  def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0, P_initial=None):
     dim_state = self.initial_x.shape[0]
     dim_state_err = self.P_initial.shape[0]
     x_init = self.initial_x
@@ -171,7 +171,8 @@ class CarKalman(KalmanFilter):
     if P_initial is not None:
       self.P_initial = P_initial
     # init filter
-    self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, self.P_initial, dim_state, dim_state_err, global_vars=self.global_vars, logger=cloudlog)
+    self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, self.P_initial,
+                              dim_state, dim_state_err, global_vars=self.global_vars, logger=cloudlog)
 
 
 if __name__ == "__main__":
diff --git a/selfdrive/locationd/models/gnss_helpers.py b/selfdrive/locationd/models/gnss_helpers.py
index b6c1771ec6..f412fafda9 100644
--- a/selfdrive/locationd/models/gnss_helpers.py
+++ b/selfdrive/locationd/models/gnss_helpers.py
@@ -1,19 +1,35 @@
 import numpy as np
-from laika.raw_gnss import GNSSMeasurement
+
+
+# source: GNSSMeasurement (https://github.com/commaai/laika/blob/master/laika/raw_gnss.py)
+class RawGNSSMeasurementIndices:
+  PRN = 0
+  RECV_TIME_WEEK = 1
+  RECV_TIME_SEC = 2
+  GLONASS_FREQ = 3
+
+  PR = 4
+  PR_STD = 5
+  PRR = 6
+  PRR_STD = 7
+
+  SAT_POS = slice(8, 11)
+  SAT_VEL = slice(11, 14)
+
 
 def parse_prr(m):
-  sat_pos_vel_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
-                                  m[GNSSMeasurement.SAT_VEL]))
-  R_i = np.atleast_2d(m[GNSSMeasurement.PRR_STD]**2)
-  z_i = m[GNSSMeasurement.PRR]
+  sat_pos_vel_i = np.concatenate((m[RawGNSSMeasurementIndices.SAT_POS],
+                                  m[RawGNSSMeasurementIndices.SAT_VEL]))
+  R_i = np.atleast_2d(m[RawGNSSMeasurementIndices.PRR_STD]**2)
+  z_i = m[RawGNSSMeasurementIndices.PRR]
   return z_i, R_i, sat_pos_vel_i
 
+
 def parse_pr(m):
-  pseudorange = m[GNSSMeasurement.PR]
-  pseudorange_stdev = m[GNSSMeasurement.PR_STD]
-  sat_pos_freq_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
-                                   np.array([m[GNSSMeasurement.GLONASS_FREQ]])))
+  pseudorange = m[RawGNSSMeasurementIndices.PR]
+  pseudorange_stdev = m[RawGNSSMeasurementIndices.PR_STD]
+  sat_pos_freq_i = np.concatenate((m[RawGNSSMeasurementIndices.SAT_POS],
+                                   np.array([m[RawGNSSMeasurementIndices.GLONASS_FREQ]])))
   z_i = np.atleast_1d(pseudorange)
   R_i = np.atleast_2d(pseudorange_stdev**2)
   return z_i, R_i, sat_pos_freq_i
-
diff --git a/selfdrive/locationd/models/gnss_kf.py b/selfdrive/locationd/models/gnss_kf.py
index 0d661dc321..c4f3b2e210 100755
--- a/selfdrive/locationd/models/gnss_kf.py
+++ b/selfdrive/locationd/models/gnss_kf.py
@@ -4,15 +4,15 @@ from typing import List
 
 import numpy as np
 
-from selfdrive.locationd.models.constants import ObservationKind
-from selfdrive.locationd.models.gnss_helpers import parse_pr, parse_prr
+from openpilot.selfdrive.locationd.models.constants import ObservationKind
+from openpilot.selfdrive.locationd.models.gnss_helpers import parse_pr, parse_prr
 
 if __name__ == '__main__':  # Generating sympy
   import sympy as sp
   from rednose.helpers.ekf_sym import gen_code
 else:
-  from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx  # pylint: disable=no-name-in-module,import-error
-  from rednose.helpers.ekf_sym import EKF_sym  # pylint: disable=no-name-in-module,import-error
+  from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
+  from rednose.helpers.ekf_sym import EKF_sym
 
 
 class States():
diff --git a/selfdrive/locationd/models/live_kf.cc b/selfdrive/locationd/models/live_kf.cc
old mode 100755
new mode 100644
index f8c03365e1..fc3bfb7246
--- a/selfdrive/locationd/models/live_kf.cc
+++ b/selfdrive/locationd/models/live_kf.cc
@@ -1,27 +1,27 @@
-#include "live_kf.h"
+#include "selfdrive/locationd/models/live_kf.h"
 
 using namespace EKFS;
 using namespace Eigen;
 
-Eigen::Map<Eigen::VectorXd> get_mapvec(Eigen::VectorXd& vec) {
-  return Eigen::Map<Eigen::VectorXd>(vec.data(), vec.rows(), vec.cols());
+Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec) {
+  return Eigen::Map<Eigen::VectorXd>((double*)vec.data(), vec.rows(), vec.cols());
 }
 
-Eigen::Map<MatrixXdr> get_mapmat(MatrixXdr& mat) {
-  return Eigen::Map<MatrixXdr>(mat.data(), mat.rows(), mat.cols());
+Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat) {
+  return Eigen::Map<MatrixXdr>((double*)mat.data(), mat.rows(), mat.cols());
 }
 
-std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(std::vector<Eigen::VectorXd>& vec_vec) {
+std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec) {
   std::vector<Eigen::Map<Eigen::VectorXd>> res;
-  for (Eigen::VectorXd& vec : vec_vec) {
+  for (const Eigen::VectorXd &vec : vec_vec) {
     res.push_back(get_mapvec(vec));
   }
   return res;
 }
 
-std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(std::vector<MatrixXdr>& mat_vec) {
+std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec) {
   std::vector<Eigen::Map<MatrixXdr>> res;
-  for (MatrixXdr& mat : mat_vec) {
+  for (const MatrixXdr &mat : mat_vec) {
     res.push_back(get_mapmat(mat));
   }
   return res;
@@ -43,20 +43,20 @@ LiveKalman::LiveKalman() {
 
   // init filter
   this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x),
-    get_mapmat(initial_P),  this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
+    get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
     std::vector<int>{3}, std::vector<std::string>(), 0.8);
 }
 
-void LiveKalman::init_state(VectorXd& state, VectorXd& covs_diag, double filter_time) {
+void LiveKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) {
   MatrixXdr covs = covs_diag.asDiagonal();
   this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
 }
 
-void LiveKalman::init_state(VectorXd& state, MatrixXdr& covs, double filter_time) {
+void LiveKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) {
   this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
 }
 
-void LiveKalman::init_state(VectorXd& state, double filter_time) {
+void LiveKalman::init_state(const VectorXd &state, double filter_time) {
   MatrixXdr covs = this->filter->covs();
   this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
 }
@@ -81,7 +81,7 @@ std::vector<MatrixXdr> LiveKalman::get_R(int kind, int n) {
   return R;
 }
 
-std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, std::vector<VectorXd> meas, std::vector<MatrixXdr> R) {
+std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) {
   std::optional<Estimate> r;
   if (R.size() == 0) {
     R = this->get_R(kind, meas.size());
@@ -94,29 +94,29 @@ void LiveKalman::predict(double t) {
   this->filter->predict(t);
 }
 
-Eigen::VectorXd LiveKalman::get_initial_x() {
+const Eigen::VectorXd &LiveKalman::get_initial_x() {
   return this->initial_x;
 }
 
-MatrixXdr LiveKalman::get_initial_P() {
+const MatrixXdr &LiveKalman::get_initial_P() {
   return this->initial_P;
 }
 
-MatrixXdr LiveKalman::get_fake_gps_pos_cov() {
+const MatrixXdr &LiveKalman::get_fake_gps_pos_cov() {
   return this->fake_gps_pos_cov;
 }
 
-MatrixXdr LiveKalman::get_fake_gps_vel_cov() {
+const MatrixXdr &LiveKalman::get_fake_gps_vel_cov() {
   return this->fake_gps_vel_cov;
 }
 
-MatrixXdr LiveKalman::get_reset_orientation_P() {
+const MatrixXdr &LiveKalman::get_reset_orientation_P() {
   return this->reset_orientation_P;
 }
 
-MatrixXdr LiveKalman::H(VectorXd in) {
+MatrixXdr LiveKalman::H(const VectorXd &in) {
   assert(in.size() == 6);
   Matrix<double, 3, 6, Eigen::RowMajor> res;
-  this->filter->get_extra_routine("H")(in.data(), res.data());
+  this->filter->get_extra_routine("H")((double*)in.data(), res.data());
   return res;
 }
diff --git a/selfdrive/locationd/models/live_kf.h b/selfdrive/locationd/models/live_kf.h
old mode 100755
new mode 100644
index 06ec3854cb..e4b3e326b3
--- a/selfdrive/locationd/models/live_kf.h
+++ b/selfdrive/locationd/models/live_kf.h
@@ -3,6 +3,8 @@
 #include <string>
 #include <cmath>
 #include <memory>
+#include <unordered_map>
+#include <vector>
 
 #include <eigen3/Eigen/Core>
 #include <eigen3/Eigen/Dense>
@@ -14,37 +16,37 @@
 
 using namespace EKFS;
 
-Eigen::Map<Eigen::VectorXd> get_mapvec(Eigen::VectorXd& vec);
-Eigen::Map<MatrixXdr> get_mapmat(MatrixXdr& mat);
-std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(std::vector<Eigen::VectorXd>& vec_vec);
-std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(std::vector<MatrixXdr>& mat_vec);
+Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec);
+Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat);
+std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec);
+std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec);
 
 class LiveKalman {
 public:
   LiveKalman();
 
-  void init_state(Eigen::VectorXd& state, Eigen::VectorXd& covs_diag, double filter_time);
-  void init_state(Eigen::VectorXd& state, MatrixXdr& covs, double filter_time);
-  void init_state(Eigen::VectorXd& state, double filter_time);
+  void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time);
+  void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time);
+  void init_state(const Eigen::VectorXd &state, double filter_time);
 
   Eigen::VectorXd get_x();
   MatrixXdr get_P();
   double get_filter_time();
   std::vector<MatrixXdr> get_R(int kind, int n);
 
-  std::optional<Estimate> predict_and_observe(double t, int kind, std::vector<Eigen::VectorXd> meas, std::vector<MatrixXdr> R = {});
+  std::optional<Estimate> predict_and_observe(double t, int kind, const std::vector<Eigen::VectorXd> &meas, std::vector<MatrixXdr> R = {});
   std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
   std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
   std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind);
   void predict(double t);
 
-  Eigen::VectorXd get_initial_x();
-  MatrixXdr get_initial_P();
-  MatrixXdr get_fake_gps_pos_cov();
-  MatrixXdr get_fake_gps_vel_cov();
-  MatrixXdr get_reset_orientation_P();
+  const Eigen::VectorXd &get_initial_x();
+  const MatrixXdr &get_initial_P();
+  const MatrixXdr &get_fake_gps_pos_cov();
+  const MatrixXdr &get_fake_gps_vel_cov();
+  const MatrixXdr &get_reset_orientation_P();
 
-  MatrixXdr H(Eigen::VectorXd in);
+  MatrixXdr H(const Eigen::VectorXd &in);
 
 private:
   std::string name = "live";
diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py
index 023479d10e..c4933a6129 100755
--- a/selfdrive/locationd/models/live_kf.py
+++ b/selfdrive/locationd/models/live_kf.py
@@ -4,7 +4,7 @@ import sys
 import os
 import numpy as np
 
-from selfdrive.locationd.models.constants import ObservationKind
+from openpilot.selfdrive.locationd.models.constants import ObservationKind
 
 import sympy as sp
 import inspect
@@ -212,14 +212,14 @@ class LiveKalman():
     live_kf_header = "#pragma once\n\n"
     live_kf_header += "#include <unordered_map>\n"
     live_kf_header += "#include <eigen3/Eigen/Dense>\n\n"
-    for state, slc in inspect.getmembers(States, lambda x: type(x) == slice):
+    for state, slc in inspect.getmembers(States, lambda x: isinstance(x, slice)):
       assert(slc.step is None)  # unsupported
       live_kf_header += f'#define STATE_{state}_START {slc.start}\n'
       live_kf_header += f'#define STATE_{state}_END {slc.stop}\n'
       live_kf_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n'
     live_kf_header += "\n"
 
-    for kind, val in inspect.getmembers(ObservationKind, lambda x: type(x) == int):
+    for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)):
       live_kf_header += f'#define OBSERVATION_{kind} {val}\n'
     live_kf_header += "\n"
 
diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py
index 1826ea4563..0e779c9e3e 100755
--- a/selfdrive/locationd/paramsd.py
+++ b/selfdrive/locationd/paramsd.py
@@ -7,12 +7,12 @@ import numpy as np
 import cereal.messaging as messaging
 from cereal import car
 from cereal import log
-from common.params import Params, put_nonblocking
-from common.realtime import config_realtime_process, DT_MDL
-from common.numpy_fast import clip
-from selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
-from selfdrive.locationd.models.constants import GENERATED_DIR
-from system.swaglog import cloudlog
+from openpilot.common.params import Params, put_nonblocking
+from openpilot.common.realtime import config_realtime_process, DT_MDL
+from openpilot.common.numpy_fast import clip
+from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
+from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
+from openpilot.system.swaglog import cloudlog
 
 
 MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL  # Max 20 deg/s
@@ -23,6 +23,8 @@ ROLL_STD_MAX = math.radians(1.5)
 LATERAL_ACC_SENSOR_THRESHOLD = 4.0
 OFFSET_MAX = 10.0
 OFFSET_LOWERED_MAX = 8.0
+MIN_ACTIVE_SPEED = 1.0
+LOW_ACTIVE_SPEED = 10.0
 
 
 class ParamsLearner:
@@ -85,8 +87,8 @@ class ParamsLearner:
         # We observe the current stiffness and steer ratio (with a high observation noise) to bound
         # the respective estimate STD. Otherwise the STDs keep increasing, causing rapid changes in the
         # states in longer routes (especially straight stretches).
-        stiffness = float(self.kf.x[States.STIFFNESS])
-        steer_ratio = float(self.kf.x[States.STEER_RATIO])
+        stiffness = float(self.kf.x[States.STIFFNESS].item())
+        steer_ratio = float(self.kf.x[States.STEER_RATIO].item())
         self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[stiffness]]))
         self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]]))
 
@@ -95,7 +97,7 @@ class ParamsLearner:
       self.speed = msg.vEgo
 
       in_linear_region = abs(self.steering_angle) < 45
-      self.active = self.speed > 1 and in_linear_region
+      self.active = self.speed > MIN_ACTIVE_SPEED and in_linear_region
 
       if self.active:
         self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]]))
@@ -115,16 +117,14 @@ def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: floa
   return current_valid
 
 
-def main(sm=None, pm=None):
+def main():
   config_realtime_process([0, 1, 2, 3], 5)
 
   DEBUG = bool(int(os.getenv("DEBUG", "0")))
   REPLAY = bool(int(os.getenv("REPLAY", "0")))
 
-  if sm is None:
-    sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll=['liveLocationKalman'])
-  if pm is None:
-    pm = messaging.PubMaster(['liveParameters'])
+  pm = messaging.PubMaster(['liveParameters'])
+  sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll=['liveLocationKalman'])
 
   params_reader = Params()
   # wait for stats about the car to come in from controls
@@ -198,12 +198,18 @@ def main(sm=None, pm=None):
         learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
         x = learner.kf.x
 
-      angle_offset_average = clip(math.degrees(x[States.ANGLE_OFFSET]), angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA)
-      angle_offset = clip(math.degrees(x[States.ANGLE_OFFSET] + x[States.ANGLE_OFFSET_FAST]), angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA)
-      roll = clip(float(x[States.ROAD_ROLL]), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA)
-      roll_std = float(P[States.ROAD_ROLL])
-      # Account for the opposite signs of the yaw rates
-      sensors_valid = bool(abs(learner.speed * (x[States.YAW_RATE] + learner.yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
+      angle_offset_average = clip(math.degrees(x[States.ANGLE_OFFSET].item()),
+                                  angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA)
+      angle_offset = clip(math.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
+                          angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA)
+      roll = clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA)
+      roll_std = float(P[States.ROAD_ROLL].item())
+      if learner.active and learner.speed > LOW_ACTIVE_SPEED:
+        # Account for the opposite signs of the yaw rates
+        # At low speeds, bumping into a curb can cause the yaw rate to be very high
+        sensors_valid = bool(abs(learner.speed * (x[States.YAW_RATE].item() + learner.yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
+      else:
+        sensors_valid = True
       avg_offset_valid = check_valid_with_hysteresis(avg_offset_valid, angle_offset_average, OFFSET_MAX, OFFSET_LOWERED_MAX)
       total_offset_valid = check_valid_with_hysteresis(total_offset_valid, angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
       roll_valid = check_valid_with_hysteresis(roll_valid, roll, ROLL_MAX, ROLL_LOWERED_MAX)
@@ -213,8 +219,8 @@ def main(sm=None, pm=None):
       liveParameters = msg.liveParameters
       liveParameters.posenetValid = True
       liveParameters.sensorValid = sensors_valid
-      liveParameters.steerRatio = float(x[States.STEER_RATIO])
-      liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
+      liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
+      liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
       liveParameters.roll = roll
       liveParameters.angleOffsetAverageDeg = angle_offset_average
       liveParameters.angleOffsetDeg = angle_offset
@@ -226,10 +232,10 @@ def main(sm=None, pm=None):
         0.2 <= liveParameters.stiffnessFactor <= 5.0,
         min_sr <= liveParameters.steerRatio <= max_sr,
       ))
-      liveParameters.steerRatioStd = float(P[States.STEER_RATIO])
-      liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS])
-      liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET])
-      liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST])
+      liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
+      liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
+      liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
+      liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item())
       if DEBUG:
         liveParameters.filterState = log.LiveLocationKalman.Measurement.new_message()
         liveParameters.filterState.value = x.tolist()
diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py
index aeccc889d2..ff1fac2c22 100755
--- a/selfdrive/locationd/torqued.py
+++ b/selfdrive/locationd/torqued.py
@@ -1,17 +1,18 @@
 #!/usr/bin/env python3
 import os
-import sys
 import signal
 import numpy as np
 from collections import deque, defaultdict
+from functools import partial
 
 import cereal.messaging as messaging
 from cereal import car, log
-from common.params import Params
-from common.realtime import config_realtime_process, DT_MDL
-from common.filter_simple import FirstOrderFilter
-from system.swaglog import cloudlog
-from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.common.params import Params
+from openpilot.common.realtime import config_realtime_process, DT_MDL
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.system.swaglog import cloudlog
+from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, cache_points_onexit
 
 HISTORY = 5  # secs
 POINTS_PER_BUCKET = 1500
@@ -43,56 +44,15 @@ def slope2rot(slope):
   return np.array([[cos, -sin], [sin, cos]])
 
 
-class NPQueue:
-  def __init__(self, maxlen, rowsize):
-    self.maxlen = maxlen
-    self.arr = np.empty((0, rowsize))
-
-  def __len__(self):
-    return len(self.arr)
-
-  def append(self, pt):
-    if len(self.arr) < self.maxlen:
-      self.arr = np.append(self.arr, [pt], axis=0)
-    else:
-      self.arr[:-1] = self.arr[1:]
-      self.arr[-1] = pt
-
-
-class PointBuckets:
-  def __init__(self, x_bounds, min_points, min_points_total):
-    self.x_bounds = x_bounds
-    self.buckets = {bounds: NPQueue(maxlen=POINTS_PER_BUCKET, rowsize=3) for bounds in x_bounds}
-    self.buckets_min_points = {bounds: min_point for bounds, min_point in zip(x_bounds, min_points)}
-    self.min_points_total = min_points_total
-
-  def bucket_lengths(self):
-    return [len(v) for v in self.buckets.values()]
-
-  def __len__(self):
-    return sum(self.bucket_lengths())
-
-  def is_valid(self):
-    return all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values())) and (self.__len__() >= self.min_points_total)
-
+class TorqueBuckets(PointBuckets):
   def add_point(self, x, y):
     for bound_min, bound_max in self.x_bounds:
       if (x >= bound_min) and (x < bound_max):
         self.buckets[(bound_min, bound_max)].append([x, 1.0, y])
         break
 
-  def get_points(self, num_points=None):
-    points = np.vstack([x.arr for x in self.buckets.values()])
-    if num_points is None:
-      return points
-    return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)]
-
-  def load_points(self, points):
-    for x, y in points:
-      self.add_point(x, y)
 
-
-class TorqueEstimator:
+class TorqueEstimator(ParameterEstimator):
   def __init__(self, CP, decimated=False):
     self.hist_len = int(HISTORY / DT_MDL)
     self.lag = CP.steerActuatorDelay + .2   # from controlsd
@@ -113,7 +73,7 @@ class TorqueEstimator:
     self.offline_friction = 0.0
     self.offline_latAccelFactor = 0.0
     self.resets = 0.0
-    self.use_params = CP.carName in ALLOWED_CARS
+    self.use_params = CP.carName in ALLOWED_CARS and CP.lateralTuning.which() == 'torque'
 
     if CP.lateralTuning.which() == 'torque':
       self.offline_friction = CP.lateralTuning.torque.friction
@@ -135,7 +95,7 @@ class TorqueEstimator:
 
     # try to restore cached params
     params = Params()
-    params_cache = params.get("LiveTorqueCarParams")
+    params_cache = params.get("CarParamsPrevRoute")
     torque_cache = params.get("LiveTorqueParameters")
     if params_cache is not None and torque_cache is not None:
       try:
@@ -156,7 +116,6 @@ class TorqueEstimator:
           cloudlog.info("restored torque params from cache")
       except Exception:
         cloudlog.exception("failed to restore cached torque params")
-        params.remove("LiveTorqueCarParams")
         params.remove("LiveTorqueParameters")
 
     self.filtered_params = {}
@@ -174,7 +133,11 @@ class TorqueEstimator:
     self.resets += 1.0
     self.decay = MIN_FILTER_DECAY
     self.raw_points = defaultdict(lambda: deque(maxlen=self.hist_len))
-    self.filtered_points = PointBuckets(x_bounds=STEER_BUCKET_BOUNDS, min_points=self.min_bucket_points, min_points_total=self.min_points_total)
+    self.filtered_points = TorqueBuckets(x_bounds=STEER_BUCKET_BOUNDS,
+                                         min_points=self.min_bucket_points,
+                                         min_points_total=self.min_points_total,
+                                         points_per_bucket=POINTS_PER_BUCKET,
+                                         rowsize=3)
 
   def estimate_params(self):
     points = self.filtered_points.get_points(self.fit_points)
@@ -230,7 +193,7 @@ class TorqueEstimator:
       liveTorqueParameters.latAccelOffsetRaw = float(latAccelOffset)
       liveTorqueParameters.frictionCoefficientRaw = float(frictionCoeff)
 
-      if any([val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, frictionCoeff]]):
+      if any(val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, frictionCoeff]):
         cloudlog.exception("Live torque parameters are invalid.")
         liveTorqueParameters.liveValid = False
         self.reset()
@@ -254,32 +217,18 @@ class TorqueEstimator:
     return msg
 
 
-def main(sm=None, pm=None):
+def main():
   config_realtime_process([0, 1, 2, 3], 5)
 
-  if sm is None:
-    sm = messaging.SubMaster(['carControl', 'carState', 'liveLocationKalman'], poll=['liveLocationKalman'])
-
-  if pm is None:
-    pm = messaging.PubMaster(['liveTorqueParameters'])
+  pm = messaging.PubMaster(['liveTorqueParameters'])
+  sm = messaging.SubMaster(['carControl', 'carState', 'liveLocationKalman'], poll=['liveLocationKalman'])
 
   params = Params()
   with car.CarParams.from_bytes(params.get("CarParams", block=True)) as CP:
     estimator = TorqueEstimator(CP)
 
-  def cache_params(sig, frame):
-    signal.signal(sig, signal.SIG_DFL)
-    cloudlog.warning("caching torque params")
-
-    params = Params()
-    params.put("LiveTorqueCarParams", CP.as_builder().to_bytes())
-
-    msg = estimator.get_msg(with_points=True)
-    params.put("LiveTorqueParameters", msg.to_bytes())
-
-    sys.exit(0)
   if "REPLAY" not in os.environ:
-    signal.signal(signal.SIGINT, cache_params)
+    signal.signal(signal.SIGINT, partial(cache_points_onexit, "LiveTorqueParameters", estimator))
 
   while True:
     sm.update()
diff --git a/selfdrive/manager/build.py b/selfdrive/manager/build.py
index 5b69e3dca7..247f3c8fb8 100755
--- a/selfdrive/manager/build.py
+++ b/selfdrive/manager/build.py
@@ -1,75 +1,79 @@
 #!/usr/bin/env python3
 import os
 import subprocess
-import textwrap
 from pathlib import Path
+from typing import List
 
 # NOTE: Do NOT import anything here that needs be built (e.g. params)
-from common.basedir import BASEDIR
-from common.spinner import Spinner
-from common.text_window import TextWindow
-from system.hardware import AGNOS
-from system.swaglog import cloudlog, add_file_handler
-from system.version import is_dirty
+from openpilot.common.basedir import BASEDIR
+from openpilot.common.spinner import Spinner
+from openpilot.common.text_window import TextWindow
+from openpilot.system.hardware import AGNOS
+from openpilot.system.swaglog import cloudlog, add_file_handler
+from openpilot.system.version import is_dirty
 
 MAX_CACHE_SIZE = 4e9 if "CI" in os.environ else 2e9
 CACHE_DIR = Path("/data/scons_cache" if AGNOS else "/tmp/scons_cache")
 
-TOTAL_SCONS_NODES = 2460
+TOTAL_SCONS_NODES = 2560
 MAX_BUILD_PROGRESS = 100
 PREBUILT = os.path.exists(os.path.join(BASEDIR, 'prebuilt'))
 
-
-def build(spinner: Spinner, dirty: bool = False) -> None:
+def build(spinner: Spinner, dirty: bool = False, minimal: bool = False) -> None:
   env = os.environ.copy()
   env['SCONS_PROGRESS'] = "1"
   nproc = os.cpu_count()
-  j_flag = "" if nproc is None else f"-j{nproc - 1}"
-
-  scons: subprocess.Popen = subprocess.Popen(["scons", j_flag, "--cache-populate"], cwd=BASEDIR, env=env, stderr=subprocess.PIPE)
-  assert scons.stderr is not None
-
-  compile_output = []
-
-  # Read progress from stderr and update spinner
-  while scons.poll() is None:
-    try:
-      line = scons.stderr.readline()
-      if line is None:
-        continue
-      line = line.rstrip()
-
-      prefix = b'progress: '
-      if line.startswith(prefix):
-        i = int(line[len(prefix):])
-        spinner.update_progress(MAX_BUILD_PROGRESS * min(1., i / TOTAL_SCONS_NODES), 100.)
-      elif len(line):
-        compile_output.append(line)
-        print(line.decode('utf8', 'replace'))
-    except Exception:
-      pass
+  if nproc is None:
+    nproc = 2
+
+  extra_args = ["--minimal"] if minimal else []
+
+  # building with all cores can result in using too
+  # much memory, so retry with less parallelism
+  compile_output: List[bytes] = []
+  for n in (nproc, nproc/2, 1):
+    compile_output.clear()
+    scons: subprocess.Popen = subprocess.Popen(["scons", f"-j{int(n)}", "--cache-populate", *extra_args], cwd=BASEDIR, env=env, stderr=subprocess.PIPE)
+    assert scons.stderr is not None
+
+    # Read progress from stderr and update spinner
+    while scons.poll() is None:
+      try:
+        line = scons.stderr.readline()
+        if line is None:
+          continue
+        line = line.rstrip()
+
+        prefix = b'progress: '
+        if line.startswith(prefix):
+          i = int(line[len(prefix):])
+          spinner.update_progress(MAX_BUILD_PROGRESS * min(1., i / TOTAL_SCONS_NODES), 100.)
+        elif len(line):
+          compile_output.append(line)
+          print(line.decode('utf8', 'replace'))
+      except Exception:
+        pass
+
+    if scons.returncode == 0:
+      break
 
   if scons.returncode != 0:
     # Read remaining output
-    r = scons.stderr.read().split(b'\n')
-    compile_output += r
+    if scons.stderr is not None:
+      compile_output += scons.stderr.read().split(b'\n')
 
     # Build failed log errors
-    errors = [line.decode('utf8', 'replace') for line in compile_output
-              if any(err in line for err in [b'error: ', b'not found, needed by target'])]
-    error_s = "\n".join(errors)
+    error_s = b"\n".join(compile_output).decode('utf8', 'replace')
     add_file_handler(cloudlog)
     cloudlog.error("scons build failed\n" + error_s)
 
     # Show TextWindow
     spinner.close()
     if not os.getenv("CI"):
-      error_s = "\n \n".join("\n".join(textwrap.wrap(e, 65)) for e in errors)
       with TextWindow("openpilot failed to build\n \n" + error_s) as t:
         t.wait_for_exit()
     exit(1)
 
-
   # enforce max cache size
   cache_files = [f for f in CACHE_DIR.rglob('*') if f.is_file()]
   cache_files.sort(key=lambda f: f.stat().st_mtime)
@@ -84,4 +88,4 @@ def build(spinner: Spinner, dirty: bool = False) -> None:
 if __name__ == "__main__" and not PREBUILT:
   spinner = Spinner()
   spinner.update_progress(0, 100)
-  build(spinner, is_dirty())
+  build(spinner, is_dirty(), minimal = AGNOS)
diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py
index e9a1b2cb5b..a739437de7 100755
--- a/selfdrive/manager/manager.py
+++ b/selfdrive/manager/manager.py
@@ -9,18 +9,18 @@ from typing import List, Tuple, Union
 
 from cereal import log
 import cereal.messaging as messaging
-import selfdrive.sentry as sentry
-from common.basedir import BASEDIR
-from common.params import Params, ParamKeyType
-from common.text_window import TextWindow
-from selfdrive.boardd.set_time import set_time
-from system.hardware import HARDWARE, PC
-from selfdrive.manager.helpers import unblock_stdout, write_onroad_params
-from selfdrive.manager.process import ensure_running
-from selfdrive.manager.process_config import managed_processes
-from selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID
-from system.swaglog import cloudlog, add_file_handler
-from system.version import is_dirty, get_commit, get_version, get_origin, get_short_branch, \
+import openpilot.selfdrive.sentry as sentry
+from openpilot.common.basedir import BASEDIR
+from openpilot.common.params import Params, ParamKeyType
+from openpilot.common.text_window import TextWindow
+from openpilot.selfdrive.boardd.set_time import set_time
+from openpilot.system.hardware import HARDWARE, PC
+from openpilot.selfdrive.manager.helpers import unblock_stdout, write_onroad_params
+from openpilot.selfdrive.manager.process import ensure_running
+from openpilot.selfdrive.manager.process_config import managed_processes
+from openpilot.selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID
+from openpilot.system.swaglog import cloudlog, add_file_handler
+from openpilot.system.version import is_dirty, get_commit, get_version, get_origin, get_short_branch, \
                            get_normalized_origin, terms_version, training_version, \
                            is_tested_branch, is_release_branch
 
diff --git a/selfdrive/manager/process.py b/selfdrive/manager/process.py
index 7d0993188c..82cc8d74a3 100644
--- a/selfdrive/manager/process.py
+++ b/selfdrive/manager/process.py
@@ -8,17 +8,14 @@ from typing import Optional, Callable, List, ValuesView
 from abc import ABC, abstractmethod
 from multiprocessing import Process
 
-from setproctitle import setproctitle  # pylint: disable=no-name-in-module
+from setproctitle import setproctitle
 
+from cereal import car, log
 import cereal.messaging as messaging
-import selfdrive.sentry as sentry
-from cereal import car
-from common.basedir import BASEDIR
-from common.params import Params
-from common.realtime import sec_since_boot
-from system.swaglog import cloudlog
-from system.hardware import HARDWARE
-from cereal import log
+import openpilot.selfdrive.sentry as sentry
+from openpilot.common.basedir import BASEDIR
+from openpilot.common.params import Params
+from openpilot.system.swaglog import cloudlog
 
 WATCHDOG_FN = "/dev/shm/wd_"
 ENABLE_WATCHDOG = os.getenv("NO_WATCHDOG") is None
@@ -40,7 +37,7 @@ def launcher(proc: str, name: str) -> None:
     sentry.set_tag("daemon", name)
 
     # exec the process
-    getattr(mod, 'main')()
+    mod.main()
   except KeyboardInterrupt:
     cloudlog.warning(f"child {proc} got SIGINT")
   except Exception:
@@ -67,12 +64,9 @@ def join_process(process: Process, timeout: float) -> None:
 
 
 class ManagerProcess(ABC):
-  unkillable = False
   daemon = False
   sigkill = False
-  onroad = True
-  offroad = False
-  callback: Optional[Callable[[bool, Params, car.CarParams], bool]] = None
+  should_run: Callable[[bool, Params, car.CarParams], bool]
   proc: Optional[Process] = None
   enabled = True
   name = ""
@@ -102,11 +96,11 @@ class ManagerProcess(ABC):
       fn = WATCHDOG_FN + str(self.proc.pid)
       with open(fn, "rb") as f:
         # TODO: why can't pylint find struct.unpack?
-        self.last_watchdog_time = struct.unpack('Q', f.read())[0] # pylint: disable=no-member
+        self.last_watchdog_time = struct.unpack('Q', f.read())[0]
     except Exception:
       pass
 
-    dt = sec_since_boot() - self.last_watchdog_time / 1e9
+    dt = time.monotonic() - self.last_watchdog_time / 1e9
 
     if dt > self.watchdog_max_dt:
       if self.watchdog_seen and ENABLE_WATCHDOG:
@@ -132,22 +126,11 @@ class ManagerProcess(ABC):
 
       join_process(self.proc, 5)
 
-      # If process failed to die send SIGKILL or reboot
+      # If process failed to die send SIGKILL
       if self.proc.exitcode is None and retry:
-        if self.unkillable:
-          cloudlog.critical(f"unkillable process {self.name} failed to exit! rebooting in 15 if it doesn't die")
-          join_process(self.proc, 15)
-
-          if self.proc.exitcode is None:
-            cloudlog.critical(f"unkillable process {self.name} failed to die!")
-            os.system("date >> /data/unkillable_reboot")
-            os.sync()
-            HARDWARE.reboot()
-            raise RuntimeError
-        else:
-          cloudlog.info(f"killing {self.name} with SIGKILL")
-          self.signal(signal.SIGKILL)
-          self.proc.join()
+        cloudlog.info(f"killing {self.name} with SIGKILL")
+        self.signal(signal.SIGKILL)
+        self.proc.join()
 
     ret = self.proc.exitcode
     cloudlog.info(f"{self.name} is dead with {ret}")
@@ -185,17 +168,15 @@ class ManagerProcess(ABC):
 
 
 class NativeProcess(ManagerProcess):
-  def __init__(self, name, cwd, cmdline, enabled=True, onroad=True, offroad=False, callback=None, unkillable=False, sigkill=False, watchdog_max_dt=None):
+  def __init__(self, name, cwd, cmdline, should_run, enabled=True, sigkill=False, watchdog_max_dt=None):
     self.name = name
     self.cwd = cwd
     self.cmdline = cmdline
+    self.should_run = should_run
     self.enabled = enabled
-    self.onroad = onroad
-    self.offroad = offroad
-    self.callback = callback
-    self.unkillable = unkillable
     self.sigkill = sigkill
     self.watchdog_max_dt = watchdog_max_dt
+    self.launcher = nativelauncher
 
   def prepare(self) -> None:
     pass
@@ -210,23 +191,21 @@ class NativeProcess(ManagerProcess):
 
     cwd = os.path.join(BASEDIR, self.cwd)
     cloudlog.info(f"starting process {self.name}")
-    self.proc = Process(name=self.name, target=nativelauncher, args=(self.cmdline, cwd, self.name))
+    self.proc = Process(name=self.name, target=self.launcher, args=(self.cmdline, cwd, self.name))
     self.proc.start()
     self.watchdog_seen = False
     self.shutting_down = False
 
 
 class PythonProcess(ManagerProcess):
-  def __init__(self, name, module, enabled=True, onroad=True, offroad=False, callback=None, unkillable=False, sigkill=False, watchdog_max_dt=None):
+  def __init__(self, name, module, should_run, enabled=True, sigkill=False, watchdog_max_dt=None):
     self.name = name
     self.module = module
+    self.should_run = should_run
     self.enabled = enabled
-    self.onroad = onroad
-    self.offroad = offroad
-    self.callback = callback
-    self.unkillable = unkillable
     self.sigkill = sigkill
     self.watchdog_max_dt = watchdog_max_dt
+    self.launcher = launcher
 
   def prepare(self) -> None:
     if self.enabled:
@@ -242,7 +221,7 @@ class PythonProcess(ManagerProcess):
       return
 
     cloudlog.info(f"starting python {self.module}")
-    self.proc = Process(name=self.name, target=launcher, args=(self.module, self.name))
+    self.proc = Process(name=self.name, target=self.launcher, args=(self.module, self.name))
     self.proc.start()
     self.watchdog_seen = False
     self.shutting_down = False
@@ -256,10 +235,12 @@ class DaemonProcess(ManagerProcess):
     self.module = module
     self.param_name = param_name
     self.enabled = enabled
-    self.onroad = True
-    self.offroad = True
     self.params = None
 
+  @staticmethod
+  def should_run(started, params, CP):
+    return True
+
   def prepare(self) -> None:
     pass
 
@@ -280,7 +261,7 @@ class DaemonProcess(ManagerProcess):
         pass
 
     cloudlog.info(f"starting daemon {self.name}")
-    proc = subprocess.Popen(['python', '-m', self.module],  # pylint: disable=subprocess-popen-preexec-fn
+    proc = subprocess.Popen(['python', '-m', self.module],
                                stdin=open('/dev/null'),
                                stdout=open('/dev/null', 'w'),
                                stderr=open('/dev/null', 'w'),
@@ -299,21 +280,7 @@ def ensure_running(procs: ValuesView[ManagerProcess], started: bool, params=None
 
   running = []
   for p in procs:
-    # Conditions that make a process run
-    run = any((
-      p.offroad and not started,
-      p.onroad and started,
-    ))
-    if p.callback is not None and None not in (params, CP):
-      run = run or p.callback(started, params, CP)
-
-    # Conditions that block a process from starting
-    run = run and not any((
-      not p.enabled,
-      p.name in not_run,
-    ))
-
-    if run:
+    if p.enabled and p.name not in not_run and p.should_run(started, params, CP):
       p.start()
       running.append(p)
     else:
diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py
index 71f63175fa..320958d344 100644
--- a/selfdrive/manager/process_config.py
+++ b/selfdrive/manager/process_config.py
@@ -1,17 +1,20 @@
 import os
 
 from cereal import car
-from common.params import Params
-from system.hardware import PC, TICI
-from selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess
+from openpilot.common.params import Params
+from openpilot.system.hardware import PC, TICI
+from openpilot.selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess
 
 WEBCAM = os.getenv("USE_WEBCAM") is not None
 
 def driverview(started: bool, params: Params, CP: car.CarParams) -> bool:
-  return params.get_bool("IsDriverViewEnabled")  # type: ignore
+  return started or params.get_bool("IsDriverViewEnabled")
 
 def notcar(started: bool, params: Params, CP: car.CarParams) -> bool:
-  return CP.notCar  # type: ignore
+  return started and CP.notCar
+
+def iscar(started: bool, params: Params, CP: car.CarParams) -> bool:
+  return started and not CP.notCar
 
 def logging(started, params, CP: car.CarParams) -> bool:
   run = (not CP.notCar) or not params.get_bool("DisableLogging")
@@ -29,51 +32,59 @@ def ublox(started, params, CP: car.CarParams) -> bool:
 def qcomgps(started, params, CP: car.CarParams) -> bool:
   return started and not ublox_available()
 
-procs = [
-  # due to qualcomm kernel bugs SIGKILLing camerad sometimes causes page table corruption
-  NativeProcess("camerad", "system/camerad", ["./camerad"], unkillable=True, callback=driverview),
-  NativeProcess("clocksd", "system/clocksd", ["./clocksd"]),
-  NativeProcess("logcatd", "system/logcatd", ["./logcatd"]),
-  NativeProcess("proclogd", "system/proclogd", ["./proclogd"]),
-  PythonProcess("logmessaged", "system.logmessaged", offroad=True),
-  PythonProcess("micd", "system.micd"),
-  PythonProcess("timezoned", "system.timezoned", enabled=not PC, offroad=True),
+def always_run(started, params, CP: car.CarParams) -> bool:
+  return True
+
+def only_onroad(started: bool, params, CP: car.CarParams) -> bool:
+  return started
+
+def only_offroad(started, params, CP: car.CarParams) -> bool:
+  return not started
 
+procs = [
   DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"),
-  NativeProcess("dmonitoringmodeld", "selfdrive/modeld", ["./dmonitoringmodeld"], enabled=(not PC or WEBCAM), callback=driverview),
-  NativeProcess("encoderd", "system/loggerd", ["./encoderd"]),
-  NativeProcess("loggerd", "system/loggerd", ["./loggerd"], onroad=False, callback=logging),
-  NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]),
-  NativeProcess("mapsd", "selfdrive/navd", ["./mapsd"]),
-  NativeProcess("navmodeld", "selfdrive/modeld", ["./navmodeld"]),
-  NativeProcess("sensord", "system/sensord", ["./sensord"], enabled=not PC),
-  NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if not PC else None)),
-  NativeProcess("soundd", "selfdrive/ui/soundd", ["./soundd"]),
-  NativeProcess("locationd", "selfdrive/locationd", ["./locationd"]),
-  NativeProcess("boardd", "selfdrive/boardd", ["./boardd"], enabled=False),
-  PythonProcess("calibrationd", "selfdrive.locationd.calibrationd"),
-  PythonProcess("torqued", "selfdrive.locationd.torqued"),
-  PythonProcess("controlsd", "selfdrive.controls.controlsd"),
-  PythonProcess("deleter", "system.loggerd.deleter", offroad=True),
-  PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), callback=driverview),
-  PythonProcess("laikad", "selfdrive.locationd.laikad"),
-  PythonProcess("rawgpsd", "system.sensord.rawgps.rawgpsd", enabled=TICI, onroad=False, callback=qcomgps),
-  PythonProcess("navd", "selfdrive.navd.navd"),
-  PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True),
-  PythonProcess("paramsd", "selfdrive.locationd.paramsd"),
-  NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], enabled=TICI, onroad=False, callback=ublox),
-  PythonProcess("pigeond", "system.sensord.pigeond", enabled=TICI, onroad=False, callback=ublox),
-  PythonProcess("plannerd", "selfdrive.controls.plannerd"),
-  PythonProcess("radard", "selfdrive.controls.radard"),
-  PythonProcess("thermald", "selfdrive.thermald.thermald", offroad=True),
-  PythonProcess("tombstoned", "selfdrive.tombstoned", enabled=not PC, offroad=True),
-  PythonProcess("updated", "selfdrive.updated", enabled=not PC, onroad=False, offroad=True),
-  PythonProcess("uploader", "system.loggerd.uploader", offroad=True),
-  PythonProcess("statsd", "selfdrive.statsd", offroad=True),
+
+  NativeProcess("camerad", "system/camerad", ["./camerad"], driverview),
+  NativeProcess("logcatd", "system/logcatd", ["./logcatd"], only_onroad),
+  NativeProcess("proclogd", "system/proclogd", ["./proclogd"], only_onroad),
+  PythonProcess("logmessaged", "system.logmessaged", always_run),
+  PythonProcess("micd", "system.micd", iscar),
+  PythonProcess("timezoned", "system.timezoned", always_run, enabled=not PC),
+
+  PythonProcess("dmonitoringmodeld", "selfdrive.modeld.dmonitoringmodeld", driverview, enabled=(not PC or WEBCAM)),
+  NativeProcess("encoderd", "system/loggerd", ["./encoderd"], only_onroad),
+  NativeProcess("stream_encoderd", "system/loggerd", ["./encoderd", "--stream"], notcar),
+  NativeProcess("loggerd", "system/loggerd", ["./loggerd"], logging),
+  NativeProcess("modeld", "selfdrive/modeld", ["./modeld"], only_onroad),
+  NativeProcess("mapsd", "selfdrive/navd", ["./mapsd"], only_onroad),
+  PythonProcess("navmodeld", "selfdrive.modeld.navmodeld", only_onroad),
+  NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC),
+  NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)),
+  NativeProcess("soundd", "selfdrive/ui/soundd", ["./soundd"], only_onroad),
+  NativeProcess("locationd", "selfdrive/locationd", ["./locationd"], only_onroad),
+  NativeProcess("boardd", "selfdrive/boardd", ["./boardd"], always_run, enabled=False),
+  PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad),
+  PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad),
+  PythonProcess("controlsd", "selfdrive.controls.controlsd", only_onroad),
+  PythonProcess("deleter", "system.loggerd.deleter", always_run),
+  PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(not PC or WEBCAM)),
+  PythonProcess("rawgpsd", "system.sensord.rawgps.rawgpsd", qcomgps, enabled=TICI),
+  PythonProcess("navd", "selfdrive.navd.navd", only_onroad),
+  PythonProcess("pandad", "selfdrive.boardd.pandad", always_run),
+  PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad),
+  NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], ublox, enabled=TICI),
+  PythonProcess("pigeond", "system.sensord.pigeond", ublox, enabled=TICI),
+  PythonProcess("plannerd", "selfdrive.controls.plannerd", only_onroad),
+  PythonProcess("radard", "selfdrive.controls.radard", only_onroad),
+  PythonProcess("thermald", "selfdrive.thermald.thermald", always_run),
+  PythonProcess("tombstoned", "selfdrive.tombstoned", always_run, enabled=not PC),
+  PythonProcess("updated", "selfdrive.updated", only_offroad, enabled=not PC),
+  PythonProcess("uploader", "system.loggerd.uploader", always_run),
+  PythonProcess("statsd", "selfdrive.statsd", always_run),
 
   # debug procs
-  NativeProcess("bridge", "cereal/messaging", ["./bridge"], onroad=False, callback=notcar),
-  PythonProcess("webjoystick", "tools.joystick.web", onroad=False, callback=notcar),
+  NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar),
+  PythonProcess("webjoystick", "tools.bodyteleop.web", notcar),
 ]
 
 managed_processes = {p.name: p for p in procs}
diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py
index 39bda1e156..0fa186baff 100755
--- a/selfdrive/manager/test/test_manager.py
+++ b/selfdrive/manager/test/test_manager.py
@@ -1,21 +1,24 @@
 #!/usr/bin/env python3
 import os
+import pytest
 import signal
 import time
 import unittest
 
 from cereal import car
-from common.params import Params
-import selfdrive.manager.manager as manager
-from selfdrive.manager.process import ensure_running
-from selfdrive.manager.process_config import managed_processes
-from system.hardware import HARDWARE
+from openpilot.common.params import Params
+import openpilot.selfdrive.manager.manager as manager
+from openpilot.selfdrive.manager.process import ensure_running
+from openpilot.selfdrive.manager.process_config import managed_processes
+from openpilot.system.hardware import HARDWARE
 
 os.environ['FAKEUPLOAD'] = "1"
 
 MAX_STARTUP_TIME = 3
 BLACKLIST_PROCS = ['manage_athenad', 'pandad', 'pigeond']
 
+
+@pytest.mark.tici
 class TestManager(unittest.TestCase):
   def setUp(self):
     os.environ['PASSIVE'] = '0'
@@ -32,6 +35,10 @@ class TestManager(unittest.TestCase):
     os.environ['PREPAREONLY'] = '1'
     manager.main()
 
+  def test_blacklisted_procs(self):
+    # TODO: ensure there are blacklisted procs until we have a dedicated test
+    self.assertTrue(len(BLACKLIST_PROCS), "No blacklisted procs to test not_run")
+
   def test_startup_time(self):
     for _ in range(10):
       start = time.monotonic()
@@ -59,9 +66,7 @@ class TestManager(unittest.TestCase):
         self.assertTrue(state.running, f"{p.name} not running")
         exit_code = p.stop(retry=False)
 
-        # TODO: mapsd should exit cleanly
-        if p.name == "mapsd":
-          continue
+        self.assertNotIn(p.name, BLACKLIST_PROCS, f"{p.name} was started")
 
         self.assertTrue(exit_code is not None, f"{p.name} failed to exit")
 
diff --git a/selfdrive/modeld/.gitignore b/selfdrive/modeld/.gitignore
new file mode 100644
index 0000000000..742d3d1205
--- /dev/null
+++ b/selfdrive/modeld/.gitignore
@@ -0,0 +1 @@
+*_pyx.cpp
diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript
index c7e0dc5d86..2d15223d1e 100644
--- a/selfdrive/modeld/SConscript
+++ b/selfdrive/modeld/SConscript
@@ -1,103 +1,72 @@
-import os
-
-Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'transformations')
+Import('env', 'envCython', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'transformations')
 lenv = env.Clone()
+lenvCython = envCython.Clone()
 
-libs = [cereal, messaging, common, visionipc, gpucommon,
-        'OpenCL', 'SNPE', 'capnp', 'zmq', 'kj', 'yuv']
-
-def get_dlsym_offset():
-  """Returns the offset between dlopen and dlsym in libdl.so"""
-  import ctypes
-  libdl = ctypes.PyDLL('libdl.so')
-  dlopen = ctypes.cast(libdl.dlopen, ctypes.c_void_p).value
-  dlsym = ctypes.cast(libdl.dlsym, ctypes.c_void_p).value
-  return dlsym - dlopen
-
+libs = [cereal, messaging, visionipc, gpucommon, common, 'capnp', 'zmq', 'kj', 'pthread']
+frameworks = []
 
 common_src = [
   "models/commonmodel.cc",
-  "runners/snpemodel.cc",
   "transforms/loadyuv.cc",
-  "transforms/transform.cc"
+  "transforms/transform.cc",
 ]
 
-thneed_src = [
+thneed_src_common = [
   "thneed/thneed_common.cc",
-  "thneed/thneed_qcom2.cc",
   "thneed/serialize.cc",
-  "runners/thneedmodel.cc",
 ]
 
-use_thneed = not GetOption('no_thneed')
+thneed_src_qcom = thneed_src_common + ["thneed/thneed_qcom2.cc"]
+thneed_src_pc = thneed_src_common + ["thneed/thneed_pc.cc"]
+thneed_src = thneed_src_qcom if arch == "larch64" else thneed_src_pc
 
-if arch == "larch64":
-  libs += ['gsl', 'CB', 'pthread', 'dl']
+# SNPE except on Mac and ARM Linux
+snpe_lib = []
+if arch != "Darwin" and arch != "aarch64":
+  common_src += ['runners/snpemodel.cc']
+  snpe_lib += ['SNPE']
 
-  if use_thneed:
-    common_src += thneed_src
-    dlsym_offset = get_dlsym_offset()
-    lenv['CXXFLAGS'].append("-DUSE_THNEED")
-    lenv['CXXFLAGS'].append(f"-DDLSYM_OFFSET={dlsym_offset}")
+# OpenCL is a framework on Mac
+if arch == "Darwin":
+  frameworks += ['OpenCL']
 else:
-  libs += ['pthread']
-
-  if not GetOption('snpe'):
-    # for onnx support
-    common_src += ['runners/onnxmodel.cc']
-
-    # tell runners to use onnx
-    lenv['CFLAGS'].append("-DUSE_ONNX_MODEL")
-    lenv['CXXFLAGS'].append("-DUSE_ONNX_MODEL")
-
-  if arch == "Darwin":
-    # fix OpenCL
-    del libs[libs.index('OpenCL')]
-    lenv['FRAMEWORKS'] = ['OpenCL']
-
-    # no SNPE on Mac
-    del libs[libs.index('SNPE')]
-    del common_src[common_src.index('runners/snpemodel.cc')]
-
-common_model = lenv.Object(common_src)
-
-lenv.Program('_dmonitoringmodeld', [
-    "dmonitoringmodeld.cc",
-    "models/dmonitoring.cc",
-  ]+common_model, LIBS=libs)
-
-# build thneed model
-if use_thneed and arch == "larch64" or GetOption('pc_thneed'):
-  fn = File("models/supercombo").abspath
-
-  tinygrad_opts = ["NATIVE_EXPLOG=1", "VALIDHACKS=1", "OPTLOCAL=1", "IMAGE=2", "GPU=1", "ENABLE_METHOD_CACHE=1"]
+  libs += ['OpenCL']
+
+# Set path definitions
+for pathdef, fn in {'TRANSFORM': 'transforms/transform.cl', 'LOADYUV': 'transforms/loadyuv.cl'}.items():
+  for xenv in (lenv, lenvCython):
+    xenv['CXXFLAGS'].append(f'-D{pathdef}_PATH=\\"{File(fn).abspath}\\"')
+
+# Compile cython
+snpe_rpath_qcom = "/data/pythonpath/third_party/snpe/larch64"
+snpe_rpath_pc = f"{Dir('#').abspath}/third_party/snpe/x86_64-linux-clang"
+snpe_rpath = lenvCython['RPATH'] + [snpe_rpath_qcom if arch == "larch64" else snpe_rpath_pc]
+
+cython_libs = envCython["LIBS"] + libs
+snpemodel_lib = lenv.Library('snpemodel', ['runners/snpemodel.cc'])
+commonmodel_lib = lenv.Library('commonmodel', common_src)
+
+lenvCython.Program('runners/runmodel_pyx.so', 'runners/runmodel_pyx.pyx', LIBS=cython_libs, FRAMEWORKS=frameworks)
+lenvCython.Program('runners/snpemodel_pyx.so', 'runners/snpemodel_pyx.pyx', LIBS=[snpemodel_lib, snpe_lib, *cython_libs], FRAMEWORKS=frameworks, RPATH=snpe_rpath)
+lenvCython.Program('models/commonmodel_pyx.so', 'models/commonmodel_pyx.pyx', LIBS=[commonmodel_lib, *cython_libs], FRAMEWORKS=frameworks)
+
+# Get model metadata
+fn = File("models/supercombo").abspath
+cmd = f'python3 {Dir("#selfdrive/modeld").abspath}/get_model_metadata.py {fn}.onnx'
+files = sum([lenv.Glob("#"+x) for x in open(File("#release/files_common").abspath).read().split("\n") if x.endswith("get_model_metadata.py")], [])
+lenv.Command(fn + "_metadata.pkl", [fn + ".onnx"]+files, cmd)
+
+# Build thneed model
+if arch == "larch64" or GetOption('pc_thneed'):
+  tinygrad_opts = []
   if not GetOption('pc_thneed'):
     # use FLOAT16 on device for speed + don't cache the CL kernels for space
     tinygrad_opts += ["FLOAT16=1", "PYOPENCL_NO_CACHE=1"]
-  cmd = f"cd {Dir('#').abspath}/tinygrad_repo && " + ' '.join(tinygrad_opts) + f" python3 openpilot/compile.py {fn}.onnx {fn}.thneed"
+  cmd = f"cd {Dir('#').abspath}/tinygrad_repo && " + ' '.join(tinygrad_opts) + f" python3 openpilot/compile2.py {fn}.onnx {fn}.thneed"
 
   tinygrad_files = sum([lenv.Glob("#"+x) for x in open(File("#release/files_common").abspath).read().split("\n") if x.startswith("tinygrad_repo/")], [])
   lenv.Command(fn + ".thneed", [fn + ".onnx"] + tinygrad_files, cmd)
 
-llenv = lenv.Clone()
-if GetOption('pc_thneed'):
-  pc_thneed_src = [
-    "thneed/thneed_common.cc",
-    "thneed/thneed_pc.cc",
-    "thneed/serialize.cc",
-    "runners/thneedmodel.cc",
-  ]
-  llenv['CFLAGS'].append("-DUSE_THNEED")
-  llenv['CXXFLAGS'].append("-DUSE_THNEED")
-  common_model += llenv.Object(pc_thneed_src)
-  libs += ['dl']
-
-llenv.Program('_modeld', [
-    "modeld.cc",
-    "models/driving.cc",
-  ]+common_model, LIBS=libs + transformations)
-
-lenv.Program('_navmodeld', [
-    "navmodeld.cc",
-    "models/nav.cc",
-  ]+common_model, LIBS=libs + transformations)
+  thneed_lib = env.SharedLibrary('thneed', thneed_src, LIBS=[gpucommon, common, 'zmq', 'OpenCL', 'dl'])
+  thneedmodel_lib = env.Library('thneedmodel', ['runners/thneedmodel.cc'])
+  lenvCython.Program('runners/thneedmodel_pyx.so', 'runners/thneedmodel_pyx.pyx', LIBS=envCython["LIBS"]+[thneedmodel_lib, thneed_lib, gpucommon, common, 'dl', 'zmq', 'OpenCL'])
diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py
index 125864b98b..4d3af51635 100644
--- a/selfdrive/modeld/constants.py
+++ b/selfdrive/modeld/constants.py
@@ -1,7 +1,82 @@
-IDX_N = 33
+import numpy as np
 
 def index_function(idx, max_val=192, max_idx=32):
   return (max_val) * ((idx/max_idx)**2)
 
+class ModelConstants:
+  # time and distance indices
+  IDX_N = 33
+  T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)]
+  X_IDXS = [index_function(idx, max_val=192.0) for idx in range(IDX_N)]
+  LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.]
+  LEAD_T_OFFSETS = [0., 2., 4.]
+  META_T_IDXS = [2., 4., 6., 8., 10.]
 
-T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)]
+  # model inputs constants
+  MODEL_FREQ = 20
+  FEATURE_LEN = 512
+  HISTORY_BUFFER_LEN = 99
+  DESIRE_LEN = 8
+  TRAFFIC_CONVENTION_LEN = 2
+  NAV_FEATURE_LEN = 256
+  NAV_INSTRUCTION_LEN = 150
+  DRIVING_STYLE_LEN = 12
+  LAT_PLANNER_STATE_LEN = 4
+
+  # model outputs constants
+  FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32)
+  FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32)
+  FCW_5MS2_PROBS_WIDTH = 5
+  FCW_3MS2_PROBS_WIDTH = 2
+
+  DISENGAGE_WIDTH = 5
+  POSE_WIDTH = 6
+  WIDE_FROM_DEVICE_WIDTH = 3
+  SIM_POSE_WIDTH = 6
+  LEAD_WIDTH = 4
+  LANE_LINES_WIDTH = 2
+  ROAD_EDGES_WIDTH = 2
+  PLAN_WIDTH = 15
+  DESIRE_PRED_WIDTH = 8
+  LAT_PLANNER_SOLUTION_WIDTH = 4
+
+  NUM_LANE_LINES = 4
+  NUM_ROAD_EDGES = 2
+
+  LEAD_TRAJ_LEN = 6
+  DESIRE_PRED_LEN = 4
+
+  PLAN_MHP_N = 5
+  LEAD_MHP_N = 2
+  PLAN_MHP_SELECTION = 1
+  LEAD_MHP_SELECTION = 3
+
+  FCW_THRESHOLD_5MS2_HIGH = 0.15
+  FCW_THRESHOLD_5MS2_LOW = 0.05
+  FCW_THRESHOLD_3MS2 = 0.7
+
+  CONFIDENCE_BUFFER_LEN = 5
+  RYG_GREEN = 0.01165
+  RYG_YELLOW = 0.06157
+
+# model outputs slices
+class Plan:
+  POSITION = slice(0, 3)
+  VELOCITY = slice(3, 6)
+  ACCELERATION = slice(6, 9)
+  T_FROM_CURRENT_EULER = slice(9, 12)
+  ORIENTATION_RATE = slice(12, 15)
+
+class Meta:
+  ENGAGED = slice(0, 1)
+  # next 2, 4, 6, 8, 10 seconds
+  GAS_DISENGAGE = slice(1, 36, 7)
+  BRAKE_DISENGAGE = slice(2, 36, 7)
+  STEER_OVERRIDE = slice(3, 36, 7)
+  HARD_BRAKE_3 = slice(4, 36, 7)
+  HARD_BRAKE_4 = slice(5, 36, 7)
+  HARD_BRAKE_5 = slice(6, 36, 7)
+  GAS_PRESS = slice(7, 36, 7)
+  # next 0, 2, 4, 6, 8, 10 seconds
+  LEFT_BLINKER = slice(36, 48, 2)
+  RIGHT_BLINKER = slice(37, 48, 2)
diff --git a/selfdrive/modeld/dmonitoringmodeld b/selfdrive/modeld/dmonitoringmodeld
deleted file mode 100755
index f292fe4c0b..0000000000
--- a/selfdrive/modeld/dmonitoringmodeld
+++ /dev/null
@@ -1,12 +0,0 @@
-#!/bin/sh
-
-DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
-cd $DIR
-
-if [ -f /TICI ]; then
-  export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/third_party/snpe/larch64:$LD_LIBRARY_PATH"
-  export ADSP_LIBRARY_PATH="/data/pythonpath/third_party/snpe/dsp/"
-else
-  export LD_LIBRARY_PATH="$DIR/../../third_party/snpe/x86_64-linux-clang:$DIR/../../openpilot/third_party/snpe/x86_64:$LD_LIBRARY_PATH"
-fi
-exec ./_dmonitoringmodeld
diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc
deleted file mode 100644
index bd3564714a..0000000000
--- a/selfdrive/modeld/dmonitoringmodeld.cc
+++ /dev/null
@@ -1,69 +0,0 @@
-#include <sys/resource.h>
-#include <limits.h>
-
-#include <cstdio>
-#include <cstdlib>
-
-#include "cereal/visionipc/visionipc_client.h"
-#include "common/params.h"
-#include "common/swaglog.h"
-#include "common/util.h"
-#include "selfdrive/modeld/models/dmonitoring.h"
-
-ExitHandler do_exit;
-
-void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) {
-  PubMaster pm({"driverStateV2"});
-  SubMaster sm({"liveCalibration"});
-  float calib[CALIB_LEN] = {0};
-  // double last = 0;
-
-  while (!do_exit) {
-    VisionIpcBufExtra extra = {};
-    VisionBuf *buf = vipc_client.recv(&extra);
-    if (buf == nullptr) continue;
-
-    sm.update(0);
-    if (sm.updated("liveCalibration")) {
-      auto calib_msg = sm["liveCalibration"].getLiveCalibration().getRpyCalib();
-      for (int i = 0; i < CALIB_LEN; i++) {
-        calib[i] = calib_msg[i];
-      }
-    }
-
-    double t1 = millis_since_boot();
-    DMonitoringModelResult model_res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height, buf->stride, buf->uv_offset, calib);
-    double t2 = millis_since_boot();
-
-    // send dm packet
-    dmonitoring_publish(pm, extra.frame_id, model_res, (t2 - t1) / 1000.0, model.output);
-
-    // printf("dmonitoring process: %.2fms, from last %.2fms\n", t2 - t1, t1 - last);
-    // last = t1;
-  }
-}
-
-int main(int argc, char **argv) {
-  setpriority(PRIO_PROCESS, 0, -15);
-
-  // init the models
-  DMonitoringModelState model;
-  dmonitoring_init(&model);
-
-  Params().putBool("DmModelInitialized", true);
-
-  LOGW("connecting to driver stream");
-  VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_DRIVER, true);
-  while (!do_exit && !vipc_client.connect(false)) {
-    util::sleep_for(100);
-  }
-
-  // run the models
-  if (vipc_client.connected) {
-    LOGW("connected with buffer size: %d", vipc_client.buffers[0].len);
-    run_model(model, vipc_client);
-  }
-
-  dmonitoring_free(&model);
-  return 0;
-}
diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py
new file mode 100755
index 0000000000..53c0af0ff3
--- /dev/null
+++ b/selfdrive/modeld/dmonitoringmodeld.py
@@ -0,0 +1,157 @@
+#!/usr/bin/env python3
+import os
+import gc
+import math
+import time
+import ctypes
+import numpy as np
+from pathlib import Path
+from typing import Tuple, Dict
+
+from cereal import messaging
+from cereal.messaging import PubMaster, SubMaster
+from cereal.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
+from openpilot.system.swaglog import cloudlog
+from openpilot.common.params import Params
+from openpilot.common.realtime import set_realtime_priority
+from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
+from openpilot.selfdrive.modeld.models.commonmodel_pyx import sigmoid
+
+CALIB_LEN = 3
+REG_SCALE = 0.25
+MODEL_WIDTH = 1440
+MODEL_HEIGHT = 960
+OUTPUT_SIZE = 84
+SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
+MODEL_PATHS = {
+  ModelRunner.SNPE: Path(__file__).parent / 'models/dmonitoring_model_q.dlc',
+  ModelRunner.ONNX: Path(__file__).parent / 'models/dmonitoring_model.onnx'}
+
+class DriverStateResult(ctypes.Structure):
+  _fields_ = [
+    ("face_orientation", ctypes.c_float*3),
+    ("face_position", ctypes.c_float*3),
+    ("face_orientation_std", ctypes.c_float*3),
+    ("face_position_std", ctypes.c_float*3),
+    ("face_prob", ctypes.c_float),
+    ("_unused_a", ctypes.c_float*8),
+    ("left_eye_prob", ctypes.c_float),
+    ("_unused_b", ctypes.c_float*8),
+    ("right_eye_prob", ctypes.c_float),
+    ("left_blink_prob", ctypes.c_float),
+    ("right_blink_prob", ctypes.c_float),
+    ("sunglasses_prob", ctypes.c_float),
+    ("occluded_prob", ctypes.c_float),
+    ("ready_prob", ctypes.c_float*4),
+    ("not_ready_prob", ctypes.c_float*2)]
+
+class DMonitoringModelResult(ctypes.Structure):
+  _fields_ = [
+    ("driver_state_lhd", DriverStateResult),
+    ("driver_state_rhd", DriverStateResult),
+    ("poor_vision_prob", ctypes.c_float),
+    ("wheel_on_right_prob", ctypes.c_float)]
+
+class ModelState:
+  inputs: Dict[str, np.ndarray]
+  output: np.ndarray
+  model: ModelRunner
+
+  def __init__(self):
+    assert ctypes.sizeof(DMonitoringModelResult) == OUTPUT_SIZE * ctypes.sizeof(ctypes.c_float)
+    self.output = np.zeros(OUTPUT_SIZE, dtype=np.float32)
+    self.inputs = {
+      'input_img': np.zeros(MODEL_HEIGHT * MODEL_WIDTH, dtype=np.uint8),
+      'calib': np.zeros(CALIB_LEN, dtype=np.float32)}
+
+    self.model = ModelRunner(MODEL_PATHS, self.output, Runtime.DSP, True, None)
+    self.model.addInput("input_img", None)
+    self.model.addInput("calib", self.inputs['calib'])
+
+  def run(self, buf:VisionBuf, calib:np.ndarray) -> Tuple[np.ndarray, float]:
+    self.inputs['calib'][:] = calib
+
+    v_offset = buf.height - MODEL_HEIGHT
+    h_offset = (buf.width - MODEL_WIDTH) // 2
+    buf_data = buf.data.reshape(-1, buf.stride)
+    input_data = self.inputs['input_img'].reshape(MODEL_HEIGHT, MODEL_WIDTH)
+    input_data[:] = buf_data[v_offset:v_offset+MODEL_HEIGHT, h_offset:h_offset+MODEL_WIDTH]
+
+    t1 = time.perf_counter()
+    self.model.setInputBuffer("input_img", self.inputs['input_img'].view(np.float32))
+    self.model.execute()
+    t2 = time.perf_counter()
+    return self.output, t2 - t1
+
+
+def fill_driver_state(msg, ds_result: DriverStateResult):
+  msg.faceOrientation = [x * REG_SCALE for x in ds_result.face_orientation]
+  msg.faceOrientationStd = [math.exp(x) for x in ds_result.face_orientation_std]
+  msg.facePosition = [x * REG_SCALE for x in ds_result.face_position[:2]]
+  msg.facePositionStd = [math.exp(x) for x in ds_result.face_position_std[:2]]
+  msg.faceProb = sigmoid(ds_result.face_prob)
+  msg.leftEyeProb = sigmoid(ds_result.left_eye_prob)
+  msg.rightEyeProb = sigmoid(ds_result.right_eye_prob)
+  msg.leftBlinkProb = sigmoid(ds_result.left_blink_prob)
+  msg.rightBlinkProb = sigmoid(ds_result.right_blink_prob)
+  msg.sunglassesProb = sigmoid(ds_result.sunglasses_prob)
+  msg.occludedProb = sigmoid(ds_result.occluded_prob)
+  msg.readyProb = [sigmoid(x) for x in ds_result.ready_prob]
+  msg.notReadyProb = [sigmoid(x) for x in ds_result.not_ready_prob]
+
+def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts: int, execution_time: float, dsp_execution_time: float):
+  model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(DMonitoringModelResult)).contents
+  msg = messaging.new_message('driverStateV2')
+  ds = msg.driverStateV2
+  ds.frameId = frame_id
+  ds.modelExecutionTime = execution_time
+  ds.dspExecutionTime = dsp_execution_time
+  ds.poorVisionProb = sigmoid(model_result.poor_vision_prob)
+  ds.wheelOnRightProb = sigmoid(model_result.wheel_on_right_prob)
+  ds.rawPredictions = model_output.tobytes() if SEND_RAW_PRED else b''
+  fill_driver_state(ds.leftDriverData, model_result.driver_state_lhd)
+  fill_driver_state(ds.rightDriverData, model_result.driver_state_rhd)
+  return msg
+
+
+def main():
+  gc.disable()
+  set_realtime_priority(1)
+
+  model = ModelState()
+  cloudlog.warning("models loaded, dmonitoringmodeld starting")
+  Params().put_bool("DmModelInitialized", True)
+
+  cloudlog.warning("connecting to driver stream")
+  vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_DRIVER, True)
+  while not vipc_client.connect(False):
+    time.sleep(0.1)
+  assert vipc_client.is_connected()
+  cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}")
+
+  sm = SubMaster(["liveCalibration"])
+  pm = PubMaster(["driverStateV2"])
+
+  calib = np.zeros(CALIB_LEN, dtype=np.float32)
+  # last = 0
+
+  while True:
+    buf = vipc_client.recv()
+    if buf is None:
+      continue
+
+    sm.update(0)
+    if sm.updated["liveCalibration"]:
+      calib[:] = np.array(sm["liveCalibration"].rpyCalib)
+
+    t1 = time.perf_counter()
+    model_output, dsp_execution_time = model.run(buf, calib)
+    t2 = time.perf_counter()
+
+    pm.send("driverStateV2", get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, dsp_execution_time))
+    # print("dmonitoring process: %.2fms, from last %.2fms\n" % (t2 - t1, t1 - last))
+    # last = t1
+
+
+if __name__ == "__main__":
+  main()
diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py
new file mode 100644
index 0000000000..7434e94287
--- /dev/null
+++ b/selfdrive/modeld/fill_model_msg.py
@@ -0,0 +1,193 @@
+import os
+import capnp
+import numpy as np
+from typing import Dict
+from cereal import log
+from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta
+
+SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
+
+ConfidenceClass = log.ModelDataV2.ConfidenceClass
+
+class PublishState:
+  def __init__(self):
+    self.disengage_buffer = np.zeros(ModelConstants.CONFIDENCE_BUFFER_LEN*ModelConstants.DISENGAGE_WIDTH, dtype=np.float32)
+    self.prev_brake_5ms2_probs = np.zeros(ModelConstants.FCW_5MS2_PROBS_WIDTH, dtype=np.float32)
+    self.prev_brake_3ms2_probs = np.zeros(ModelConstants.FCW_3MS2_PROBS_WIDTH, dtype=np.float32)
+
+def fill_xyzt(builder, t, x, y, z, x_std=None, y_std=None, z_std=None):
+  builder.t = t
+  builder.x = x.tolist()
+  builder.y = y.tolist()
+  builder.z = z.tolist()
+  if x_std is not None:
+    builder.xStd = x_std.tolist()
+  if y_std is not None:
+    builder.yStd = y_std.tolist()
+  if z_std is not None:
+    builder.zStd = z_std.tolist()
+
+def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std=None):
+  builder.t = t
+  builder.x = x.tolist()
+  builder.y = y.tolist()
+  builder.v = v.tolist()
+  builder.a = a.tolist()
+  if x_std is not None:
+    builder.xStd = x_std.tolist()
+  if y_std is not None:
+    builder.yStd = y_std.tolist()
+  if v_std is not None:
+    builder.vStd = v_std.tolist()
+  if a_std is not None:
+    builder.aStd = a_std.tolist()
+
+def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, np.ndarray], publish_state: PublishState,
+                   vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float,
+                   timestamp_eof: int, timestamp_llk: int, model_execution_time: float,
+                   nav_enabled: bool, valid: bool) -> None:
+  frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
+  msg.valid = valid
+
+  modelV2 = msg.modelV2
+  modelV2.frameId = vipc_frame_id
+  modelV2.frameIdExtra = vipc_frame_id_extra
+  modelV2.frameAge = frame_age
+  modelV2.frameDropPerc = frame_drop * 100
+  modelV2.timestampEof = timestamp_eof
+  modelV2.locationMonoTime = timestamp_llk
+  modelV2.modelExecutionTime = model_execution_time
+  modelV2.navEnabled = nav_enabled
+
+  # plan
+  position = modelV2.position
+  fill_xyzt(position, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.POSITION].T, *net_output_data['plan_stds'][0,:,Plan.POSITION].T)
+  velocity = modelV2.velocity
+  fill_xyzt(velocity, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.VELOCITY].T)
+  acceleration = modelV2.acceleration
+  fill_xyzt(acceleration, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ACCELERATION].T)
+  orientation = modelV2.orientation
+  fill_xyzt(orientation, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.T_FROM_CURRENT_EULER].T)
+  orientation_rate = modelV2.orientationRate
+  fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
+
+  # lateral planning
+  solution = modelV2.lateralPlannerSolution
+  solution.x, solution.y, solution.yaw, solution.yawRate = [net_output_data['lat_planner_solution'][0,:,i].tolist() for i in range(4)]
+  solution.xStd, solution.yStd, solution.yawStd, solution.yawRateStd = [net_output_data['lat_planner_solution_stds'][0,:,i].tolist() for i in range(4)]
+
+  # times at X_IDXS according to model plan
+  PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N
+  PLAN_T_IDXS[0] = 0.0
+  plan_x = net_output_data['plan'][0,:,Plan.POSITION][:,0].tolist()
+  for xidx in range(1, ModelConstants.IDX_N):
+    tidx = 0
+    # increment tidx until we find an element that's further away than the current xidx
+    while tidx < ModelConstants.IDX_N - 1 and plan_x[tidx+1] < ModelConstants.X_IDXS[xidx]:
+      tidx += 1
+    if tidx == ModelConstants.IDX_N - 1:
+      # if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break
+      PLAN_T_IDXS[xidx] = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1]
+      break
+    # interpolate to find `t` for the current xidx
+    current_x_val = plan_x[tidx]
+    next_x_val = plan_x[tidx+1]
+    p = (ModelConstants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) if abs(next_x_val - current_x_val) > 1e-9 else float('nan')
+    PLAN_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx+1] + (1 - p) * ModelConstants.T_IDXS[tidx]
+
+  # lane lines
+  modelV2.init('laneLines', 4)
+  for i in range(4):
+    lane_line = modelV2.laneLines[i]
+    fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1])
+  modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist()
+  modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist()
+
+  # road edges
+  modelV2.init('roadEdges', 2)
+  for i in range(2):
+    road_edge = modelV2.roadEdges[i]
+    fill_xyzt(road_edge, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['road_edges'][0,i,:,0], net_output_data['road_edges'][0,i,:,1])
+  modelV2.roadEdgeStds = net_output_data['road_edges_stds'][0,:,0,0].tolist()
+
+  # leads
+  modelV2.init('leadsV3', 3)
+  for i in range(3):
+    lead = modelV2.leadsV3[i]
+    fill_xyvat(lead, ModelConstants.LEAD_T_IDXS, *net_output_data['lead'][0,i].T, *net_output_data['lead_stds'][0,i].T)
+    lead.prob = net_output_data['lead_prob'][0,i].tolist()
+    lead.probTime = ModelConstants.LEAD_T_OFFSETS[i]
+
+  # meta
+  meta = modelV2.meta
+  meta.desireState = net_output_data['desire_state'][0].reshape(-1).tolist()
+  meta.desirePrediction = net_output_data['desire_pred'][0].reshape(-1).tolist()
+  meta.engagedProb = net_output_data['meta'][0,Meta.ENGAGED].item()
+  meta.init('disengagePredictions')
+  disengage_predictions = meta.disengagePredictions
+  disengage_predictions.t = ModelConstants.META_T_IDXS
+  disengage_predictions.brakeDisengageProbs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE].tolist()
+  disengage_predictions.gasDisengageProbs = net_output_data['meta'][0,Meta.GAS_DISENGAGE].tolist()
+  disengage_predictions.steerOverrideProbs = net_output_data['meta'][0,Meta.STEER_OVERRIDE].tolist()
+  disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_3].tolist()
+  disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_4].tolist()
+  disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_5].tolist()
+
+  publish_state.prev_brake_5ms2_probs[:-1] = publish_state.prev_brake_5ms2_probs[1:]
+  publish_state.prev_brake_5ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_5][0]
+  publish_state.prev_brake_3ms2_probs[:-1] = publish_state.prev_brake_3ms2_probs[1:]
+  publish_state.prev_brake_3ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_3][0]
+  hard_brake_predicted = (publish_state.prev_brake_5ms2_probs > ModelConstants.FCW_THRESHOLDS_5MS2).all() and \
+    (publish_state.prev_brake_3ms2_probs > ModelConstants.FCW_THRESHOLDS_3MS2).all()
+  meta.hardBrakePredicted = hard_brake_predicted.item()
+
+  # temporal pose
+  temporal_pose = modelV2.temporalPose
+  temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist()
+  temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist()
+  temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist()
+  temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist()
+
+  # confidence
+  if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0:
+    # any disengage prob
+    brake_disengage_probs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE]
+    gas_disengage_probs = net_output_data['meta'][0,Meta.GAS_DISENGAGE]
+    steer_override_probs = net_output_data['meta'][0,Meta.STEER_OVERRIDE]
+    any_disengage_probs = 1-((1-brake_disengage_probs)*(1-gas_disengage_probs)*(1-steer_override_probs))
+    # independent disengage prob for each 2s slice
+    ind_disengage_probs = np.r_[any_disengage_probs[0], np.diff(any_disengage_probs) / (1 - any_disengage_probs[:-1])]
+    # rolling buf for 2, 4, 6, 8, 10s
+    publish_state.disengage_buffer[:-ModelConstants.DISENGAGE_WIDTH] = publish_state.disengage_buffer[ModelConstants.DISENGAGE_WIDTH:]
+    publish_state.disengage_buffer[-ModelConstants.DISENGAGE_WIDTH:] = ind_disengage_probs
+
+  score = 0.
+  for i in range(ModelConstants.DISENGAGE_WIDTH):
+    score += publish_state.disengage_buffer[i*ModelConstants.DISENGAGE_WIDTH+ModelConstants.DISENGAGE_WIDTH-1-i].item() / ModelConstants.DISENGAGE_WIDTH
+  if score < ModelConstants.RYG_GREEN:
+    modelV2.confidence = ConfidenceClass.green
+  elif score < ModelConstants.RYG_YELLOW:
+    modelV2.confidence = ConfidenceClass.yellow
+  else:
+    modelV2.confidence = ConfidenceClass.red
+
+  # raw prediction if enabled
+  if SEND_RAW_PRED:
+    modelV2.rawPredictions = net_output_data['raw_pred'].tobytes()
+
+def fill_pose_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, np.ndarray],
+                  vipc_frame_id: int, vipc_dropped_frames: int, timestamp_eof: int, live_calib_seen: bool) -> None:
+  msg.valid = live_calib_seen & (vipc_dropped_frames < 1)
+  cameraOdometry = msg.cameraOdometry
+
+  cameraOdometry.frameId = vipc_frame_id
+  cameraOdometry.timestampEof = timestamp_eof
+
+  cameraOdometry.trans = net_output_data['pose'][0,:3].tolist()
+  cameraOdometry.rot = net_output_data['pose'][0,3:].tolist()
+  cameraOdometry.wideFromDeviceEuler = net_output_data['wide_from_device_euler'][0,:].tolist()
+  cameraOdometry.roadTransformTrans = net_output_data['road_transform'][0,:3].tolist()
+  cameraOdometry.transStd = net_output_data['pose_stds'][0,:3].tolist()
+  cameraOdometry.rotStd = net_output_data['pose_stds'][0,3:].tolist()
+  cameraOdometry.wideFromDeviceEulerStd = net_output_data['wide_from_device_euler_stds'][0,:].tolist()
+  cameraOdometry.roadTransformTransStd = net_output_data['road_transform_stds'][0,:3].tolist()
diff --git a/selfdrive/modeld/get_model_metadata.py b/selfdrive/modeld/get_model_metadata.py
new file mode 100755
index 0000000000..187f83399b
--- /dev/null
+++ b/selfdrive/modeld/get_model_metadata.py
@@ -0,0 +1,29 @@
+#!/usr/bin/env python3
+import sys
+import pathlib
+import onnx
+import codecs
+import pickle
+from typing import Tuple
+
+def get_name_and_shape(value_info:onnx.ValueInfoProto) -> Tuple[str, Tuple[int,...]]:
+  shape = tuple([int(dim.dim_value) for dim in value_info.type.tensor_type.shape.dim])
+  name = value_info.name
+  return name, shape
+
+if __name__ == "__main__":
+  model_path = pathlib.Path(sys.argv[1])
+  model = onnx.load(str(model_path))
+  i = [x.key for x in model.metadata_props].index('output_slices')
+  output_slices = model.metadata_props[i].value
+
+  metadata = {}
+  metadata['output_slices'] = pickle.loads(codecs.decode(output_slices.encode(), "base64"))
+  metadata['input_shapes'] = dict([get_name_and_shape(x) for x in model.graph.input])
+  metadata['output_shapes'] = dict([get_name_and_shape(x) for x in model.graph.output])
+
+  metadata_path = model_path.parent / (model_path.stem + '_metadata.pkl')
+  with open(metadata_path, 'wb') as f:
+    pickle.dump(metadata, f)
+
+  print(f'saved metadata to {metadata_path}')
diff --git a/selfdrive/modeld/modeld b/selfdrive/modeld/modeld
index c067cf3d62..14048ec9fd 100755
--- a/selfdrive/modeld/modeld
+++ b/selfdrive/modeld/modeld
@@ -1,11 +1,10 @@
-#!/bin/sh
+#!/usr/bin/env bash
 
 DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
-cd $DIR
+cd "$DIR/../../"
 
-if [ -f /TICI ]; then
-  export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/third_party/snpe/larch64:$LD_LIBRARY_PATH"
-else
-  export LD_LIBRARY_PATH="$DIR/../../third_party/snpe/x86_64-linux-clang:$DIR/../../openpilot/third_party/snpe/x86_64:$LD_LIBRARY_PATH"
+if [ -f "$DIR/libthneed.so" ]; then
+  export LD_PRELOAD="$DIR/libthneed.so"
 fi
-exec ./_modeld
+
+exec "$DIR/modeld.py"
diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc
deleted file mode 100644
index 1203704013..0000000000
--- a/selfdrive/modeld/modeld.cc
+++ /dev/null
@@ -1,253 +0,0 @@
-#include <cstdio>
-#include <cstdlib>
-#include <mutex>
-#include <cmath>
-
-#include <eigen3/Eigen/Dense>
-
-#include "cereal/messaging/messaging.h"
-#include "common/transformations/orientation.hpp"
-
-#include "cereal/visionipc/visionipc_client.h"
-#include "common/clutil.h"
-#include "common/params.h"
-#include "common/swaglog.h"
-#include "common/util.h"
-#include "system/hardware/hw.h"
-#include "selfdrive/modeld/models/driving.h"
-#include "selfdrive/modeld/models/nav.h"
-
-
-ExitHandler do_exit;
-
-mat3 update_calibration(Eigen::Vector3d device_from_calib_euler, bool wide_camera, bool bigmodel_frame) {
-  /*
-     import numpy as np
-     from common.transformations.model import medmodel_frame_from_calib_frame
-     medmodel_frame_from_calib_frame = medmodel_frame_from_calib_frame[:, :3]
-     calib_from_smedmodel_frame = np.linalg.inv(medmodel_frame_from_calib_frame)
-  */
-  static const auto calib_from_medmodel = (Eigen::Matrix<float, 3, 3>() <<
-     0.00000000e+00, 0.00000000e+00, 1.00000000e+00,
-     1.09890110e-03, 0.00000000e+00, -2.81318681e-01,
-    -2.25466395e-20, 1.09890110e-03,-5.23076923e-02).finished();
-
-  static const auto calib_from_sbigmodel = (Eigen::Matrix<float, 3, 3>() <<
-     0.00000000e+00,  7.31372216e-19,  1.00000000e+00,
-     2.19780220e-03,  4.11497335e-19, -5.62637363e-01,
-    -6.66298828e-20,  2.19780220e-03, -3.33626374e-01).finished();
-
-  static const auto view_from_device = (Eigen::Matrix<float, 3, 3>() <<
-     0.0,  1.0,  0.0,
-     0.0,  0.0,  1.0,
-     1.0,  0.0,  0.0).finished();
-
-
-  const auto cam_intrinsics = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>(wide_camera ? ECAM_INTRINSIC_MATRIX.v : FCAM_INTRINSIC_MATRIX.v);
-  Eigen::Matrix<float, 3, 3, Eigen::RowMajor>  device_from_calib = euler2rot(device_from_calib_euler).cast <float> ();
-  auto calib_from_model = bigmodel_frame ? calib_from_sbigmodel : calib_from_medmodel;
-  auto camera_from_calib = cam_intrinsics * view_from_device * device_from_calib;
-  auto warp_matrix = camera_from_calib * calib_from_model;
-
-  mat3 transform = {};
-  for (int i=0; i<3*3; i++) {
-    transform.v[i] = warp_matrix(i / 3, i % 3);
-  }
-  return transform;
-}
-
-
-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", "driverMonitoringState", "navModel"});
-
-  Params params;
-
-  // setup filter to track dropped frames
-  FirstOrderFilter frame_dropped_filter(0., 10., 1. / MODEL_FREQ);
-
-  uint32_t frame_id = 0, last_vipc_frame_id = 0;
-  // double last = 0;
-  uint32_t run_count = 0;
-
-  mat3 model_transform_main = {};
-  mat3 model_transform_extra = {};
-  bool nav_enabled = false;
-  bool live_calib_seen = false;
-  float driving_style[DRIVING_STYLE_LEN] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0};
-  float nav_features[NAV_FEATURE_LEN] = {0};
-
-  VisionBuf *buf_main = nullptr;
-  VisionBuf *buf_extra = nullptr;
-
-  VisionIpcBufExtra meta_main = {0};
-  VisionIpcBufExtra meta_extra = {0};
-
-  while (!do_exit) {
-    // Keep receiving frames until we are at least 1 frame ahead of previous extra frame
-    while (meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000ULL) {
-      buf_main = vipc_client_main.recv(&meta_main);
-      if (buf_main == nullptr)  break;
-    }
-
-    if (buf_main == nullptr) {
-      LOGE("vipc_client_main no frame");
-      continue;
-    }
-
-    if (use_extra_client) {
-      // Keep receiving extra frames until frame id matches main camera
-      do {
-        buf_extra = vipc_client_extra.recv(&meta_extra);
-      } while (buf_extra != nullptr && meta_main.timestamp_sof > meta_extra.timestamp_sof + 25000000ULL);
-
-      if (buf_extra == nullptr) {
-        LOGE("vipc_client_extra no frame");
-        continue;
-      }
-
-      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);
-      }
-    } else {
-      // Use single camera
-      buf_extra = buf_main;
-      meta_extra = meta_main;
-    }
-
-    // TODO: path planner timeout?
-    sm.update(0);
-    int desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire());
-    bool is_rhd = ((bool)sm["driverMonitoringState"].getDriverMonitoringState().getIsRHD());
-    frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId();
-    if (sm.updated("liveCalibration")) {
-      auto rpy_calib = sm["liveCalibration"].getLiveCalibration().getRpyCalib();
-      Eigen::Vector3d device_from_calib_euler;
-      for (int i=0; i<3; i++) {
-        device_from_calib_euler(i) = rpy_calib[i];
-      }
-      model_transform_main = update_calibration(device_from_calib_euler, main_wide_camera, false);
-      model_transform_extra = update_calibration(device_from_calib_euler, true, true);
-      live_calib_seen = true;
-    }
-
-    float vec_desire[DESIRE_LEN] = {0};
-    if (desire >= 0 && desire < DESIRE_LEN) {
-      vec_desire[desire] = 1.0;
-    }
-
-    // Enable/disable nav features
-    uint64_t timestamp_llk = sm["navModel"].getNavModel().getLocationMonoTime();
-    bool nav_valid = sm["navModel"].getValid() && (nanos_since_boot() - timestamp_llk < 1e9);
-    bool use_nav = nav_valid && params.getBool("ExperimentalMode");
-    if (!nav_enabled && use_nav) {
-      nav_enabled = true;
-    } else if (nav_enabled && !use_nav) {
-      memset(nav_features, 0, sizeof(float)*NAV_FEATURE_LEN);
-      nav_enabled = false;
-    }
-
-    if (nav_enabled && sm.updated("navModel")) {
-      auto nav_model_features = sm["navModel"].getNavModel().getFeatures();
-      for (int i=0; i<NAV_FEATURE_LEN; i++) {
-        nav_features[i] = nav_model_features[i];
-      }
-    }
-
-    // tracked dropped frames
-    uint32_t vipc_dropped_frames = meta_main.frame_id - last_vipc_frame_id - 1;
-    float frames_dropped = frame_dropped_filter.update((float)std::min(vipc_dropped_frames, 10U));
-    if (run_count < 10) { // let frame drops warm up
-      frame_dropped_filter.reset(0);
-      frames_dropped = 0.;
-    }
-    run_count++;
-
-    float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
-    bool prepare_only = vipc_dropped_frames > 0;
-
-    if (prepare_only) {
-      LOGE("skipping model eval. Dropped %d frames", vipc_dropped_frames);
-    }
-
-    double mt1 = millis_since_boot();
-    ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, is_rhd, driving_style, nav_features, prepare_only);
-    double mt2 = millis_since_boot();
-    float model_execution_time = (mt2 - mt1) / 1000.0;
-
-    if (model_output != nullptr) {
-      model_publish(&model, pm, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, *model_output, meta_main.timestamp_eof, timestamp_llk, model_execution_time,
-                    nav_enabled, live_calib_seen);
-      posenet_publish(pm, meta_main.frame_id, vipc_dropped_frames, *model_output, meta_main.timestamp_eof, live_calib_seen);
-    }
-
-    // printf("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f\n", mt2 - mt1, mt1 - last, extra.frame_id, frame_id, frame_drop_ratio);
-    // last = mt1;
-    last_vipc_frame_id = meta_main.frame_id;
-  }
-}
-
-int main(int argc, char **argv) {
-  if (!Hardware::PC()) {
-    int ret;
-    ret = util::set_realtime_priority(54);
-    assert(ret == 0);
-    util::set_core_affinity({7});
-    assert(ret == 0);
-  }
-
-  // cl init
-  cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
-  cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err));
-
-  // init the models
-  ModelState model;
-  model_init(&model, device_id, context);
-  LOGW("models loaded, modeld starting");
-
-  bool main_wide_camera = false;
-  bool use_extra_client = true; // set to false to use single camera
-  while (!do_exit) {
-    auto streams = VisionIpcClient::getAvailableStreams("camerad", false);
-    if (!streams.empty()) {
-      use_extra_client = streams.count(VISION_STREAM_WIDE_ROAD) > 0 && streams.count(VISION_STREAM_ROAD) > 0;
-      main_wide_camera = streams.count(VISION_STREAM_ROAD) == 0;
-      break;
-    }
-
-    util::sleep_for(100);
-  }
-
-  VisionIpcClient vipc_client_main = VisionIpcClient("camerad", main_wide_camera ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD, true, device_id, context);
-  VisionIpcClient vipc_client_extra = VisionIpcClient("camerad", VISION_STREAM_WIDE_ROAD, false, device_id, context);
-  LOGW("vision stream set up, main_wide_camera: %d, use_extra_client: %d", main_wide_camera, use_extra_client);
-
-  while (!do_exit && !vipc_client_main.connect(false)) {
-    util::sleep_for(100);
-  }
-
-  while (!do_exit && use_extra_client && !vipc_client_extra.connect(false)) {
-    util::sleep_for(100);
-  }
-
-  // run the models
-  // vipc_client.connected is false only when do_exit is true
-  if (!do_exit) {
-    const VisionBuf *b = &vipc_client_main.buffers[0];
-    LOGW("connected main cam with buffer size: %d (%d x %d)", b->len, b->width, b->height);
-
-    if (use_extra_client) {
-      const VisionBuf *wb = &vipc_client_extra.buffers[0];
-      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_client);
-  }
-
-  model_free(&model);
-  CL_CHECK(clReleaseContext(context));
-  return 0;
-}
diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py
new file mode 100755
index 0000000000..2b211b22e6
--- /dev/null
+++ b/selfdrive/modeld/modeld.py
@@ -0,0 +1,293 @@
+#!/usr/bin/env python3
+import os
+import time
+import pickle
+import numpy as np
+import cereal.messaging as messaging
+from pathlib import Path
+from typing import Dict, Optional
+from setproctitle import setproctitle
+from cereal.messaging import PubMaster, SubMaster
+from cereal.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
+from openpilot.system.swaglog import cloudlog
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.numpy_fast import interp
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.realtime import config_realtime_process
+from openpilot.common.transformations.model import get_warp_matrix
+from openpilot.selfdrive import sentry
+from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
+from openpilot.selfdrive.modeld.parse_model_outputs import Parser
+from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext
+
+PROCESS_NAME = "selfdrive.modeld.modeld"
+SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
+
+MODEL_PATHS = {
+  ModelRunner.THNEED: Path(__file__).parent / 'models/supercombo.thneed',
+  ModelRunner.ONNX: Path(__file__).parent / 'models/supercombo.onnx'}
+
+METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl'
+
+class FrameMeta:
+  frame_id: int = 0
+  timestamp_sof: int = 0
+  timestamp_eof: int = 0
+
+  def __init__(self, vipc=None):
+    if vipc is not None:
+      self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof
+
+class ModelState:
+  frame: ModelFrame
+  wide_frame: ModelFrame
+  inputs: Dict[str, np.ndarray]
+  output: np.ndarray
+  prev_desire: np.ndarray  # for tracking the rising edge of the pulse
+  model: ModelRunner
+
+  def __init__(self, context: CLContext):
+    self.frame = ModelFrame(context)
+    self.wide_frame = ModelFrame(context)
+    self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
+    self.inputs = {
+      'desire': np.zeros(ModelConstants.DESIRE_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32),
+      'traffic_convention': np.zeros(ModelConstants.TRAFFIC_CONVENTION_LEN, dtype=np.float32),
+      'lat_planner_state': np.zeros(ModelConstants.LAT_PLANNER_STATE_LEN, dtype=np.float32),
+      'nav_features': np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32),
+      'nav_instructions': np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32),
+      'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32),
+    }
+
+    with open(METADATA_PATH, 'rb') as f:
+      model_metadata = pickle.load(f)
+
+    self.output_slices = model_metadata['output_slices']
+    net_output_size = model_metadata['output_shapes']['outputs'][1]
+    self.output = np.zeros(net_output_size, dtype=np.float32)
+    self.parser = Parser()
+
+    self.model = ModelRunner(MODEL_PATHS, self.output, Runtime.GPU, False, context)
+    self.model.addInput("input_imgs", None)
+    self.model.addInput("big_input_imgs", None)
+    for k,v in self.inputs.items():
+      self.model.addInput(k, v)
+
+  def slice_outputs(self, model_outputs: np.ndarray) -> Dict[str, np.ndarray]:
+    parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in self.output_slices.items()}
+    if SEND_RAW_PRED:
+      parsed_model_outputs['raw_pred'] = model_outputs.copy()
+    return parsed_model_outputs
+
+  def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_wide: np.ndarray,
+                inputs: Dict[str, np.ndarray], prepare_only: bool) -> Optional[Dict[str, np.ndarray]]:
+    # Model decides when action is completed, so desire input is just a pulse triggered on rising edge
+    inputs['desire'][0] = 0
+    self.inputs['desire'][:-ModelConstants.DESIRE_LEN] = self.inputs['desire'][ModelConstants.DESIRE_LEN:]
+    self.inputs['desire'][-ModelConstants.DESIRE_LEN:] = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
+    self.prev_desire[:] = inputs['desire']
+
+    self.inputs['traffic_convention'][:] = inputs['traffic_convention']
+    self.inputs['nav_features'][:] = inputs['nav_features']
+    self.inputs['nav_instructions'][:] = inputs['nav_instructions']
+    # self.inputs['driving_style'][:] = inputs['driving_style']
+
+    # if getCLBuffer is not None, frame will be None
+    self.model.setInputBuffer("input_imgs", self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs")))
+    if wbuf is not None:
+      self.model.setInputBuffer("big_input_imgs", self.wide_frame.prepare(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs")))
+
+    if prepare_only:
+      return None
+
+    self.model.execute()
+    outputs = self.parser.parse_outputs(self.slice_outputs(self.output))
+
+    self.inputs['features_buffer'][:-ModelConstants.FEATURE_LEN] = self.inputs['features_buffer'][ModelConstants.FEATURE_LEN:]
+    self.inputs['features_buffer'][-ModelConstants.FEATURE_LEN:] = outputs['hidden_state'][0, :]
+    self.inputs['lat_planner_state'][2] = interp(DT_MDL, ModelConstants.T_IDXS, outputs['lat_planner_solution'][0, :, 2])
+    self.inputs['lat_planner_state'][3] = interp(DT_MDL, ModelConstants.T_IDXS, outputs['lat_planner_solution'][0, :, 3])
+    return outputs
+
+
+def main():
+  sentry.set_tag("daemon", PROCESS_NAME)
+  cloudlog.bind(daemon=PROCESS_NAME)
+  setproctitle(PROCESS_NAME)
+  config_realtime_process(7, 54)
+
+  cl_context = CLContext()
+  model = ModelState(cl_context)
+  cloudlog.warning("models loaded, modeld starting")
+
+  # visionipc clients
+  while True:
+    available_streams = VisionIpcClient.available_streams("camerad", block=False)
+    if available_streams:
+      use_extra_client = VisionStreamType.VISION_STREAM_WIDE_ROAD in available_streams and VisionStreamType.VISION_STREAM_ROAD in available_streams
+      main_wide_camera = VisionStreamType.VISION_STREAM_ROAD not in available_streams
+      break
+    time.sleep(.1)
+
+  vipc_client_main_stream = VisionStreamType.VISION_STREAM_WIDE_ROAD if main_wide_camera else VisionStreamType.VISION_STREAM_ROAD
+  vipc_client_main = VisionIpcClient("camerad", vipc_client_main_stream, True, cl_context)
+  vipc_client_extra = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, False, cl_context)
+  cloudlog.warning(f"vision stream set up, main_wide_camera: {main_wide_camera}, use_extra_client: {use_extra_client}")
+
+  while not vipc_client_main.connect(False):
+    time.sleep(0.1)
+  while use_extra_client and not vipc_client_extra.connect(False):
+    time.sleep(0.1)
+
+  cloudlog.warning(f"connected main cam with buffer size: {vipc_client_main.buffer_len} ({vipc_client_main.width} x {vipc_client_main.height})")
+  if use_extra_client:
+    cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})")
+
+  # messaging
+  pm = PubMaster(["modelV2", "cameraOdometry"])
+  sm = SubMaster(["lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction"])
+
+  publish_state = PublishState()
+  params = Params()
+
+  # setup filter to track dropped frames
+  frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_FREQ)
+  frame_id = 0
+  last_vipc_frame_id = 0
+  run_count = 0
+
+  model_transform_main = np.zeros((3, 3), dtype=np.float32)
+  model_transform_extra = np.zeros((3, 3), dtype=np.float32)
+  live_calib_seen = False
+  driving_style = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], dtype=np.float32)
+  nav_features = np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32)
+  nav_instructions = np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32)
+  buf_main, buf_extra = None, None
+  meta_main = FrameMeta()
+  meta_extra = FrameMeta()
+
+  while True:
+    # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
+    while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000:
+      buf_main = vipc_client_main.recv()
+      meta_main = FrameMeta(vipc_client_main)
+      if buf_main is None:
+        break
+
+    if buf_main is None:
+      cloudlog.error("vipc_client_main no frame")
+      continue
+
+    if use_extra_client:
+      # Keep receiving extra frames until frame id matches main camera
+      while True:
+        buf_extra = vipc_client_extra.recv()
+        meta_extra = FrameMeta(vipc_client_extra)
+        if buf_extra is None or meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000:
+          break
+
+      if buf_extra is None:
+        cloudlog.error("vipc_client_extra no frame")
+        continue
+
+      if abs(meta_main.timestamp_sof - meta_extra.timestamp_sof) > 10000000:
+        cloudlog.error("frames out of sync! main: {} ({:.5f}), extra: {} ({:.5f})".format(
+          meta_main.frame_id, meta_main.timestamp_sof / 1e9,
+          meta_extra.frame_id, meta_extra.timestamp_sof / 1e9))
+
+    else:
+      # Use single camera
+      buf_extra = buf_main
+      meta_extra = meta_main
+
+    # TODO: path planner timeout?
+    sm.update(0)
+    desire = sm["lateralPlan"].desire.raw
+    is_rhd = sm["driverMonitoringState"].isRHD
+    frame_id = sm["roadCameraState"].frameId
+    if sm.updated["liveCalibration"]:
+      device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
+      model_transform_main = get_warp_matrix(device_from_calib_euler, main_wide_camera, False).astype(np.float32)
+      model_transform_extra = get_warp_matrix(device_from_calib_euler, True, True).astype(np.float32)
+      live_calib_seen = True
+
+    traffic_convention = np.zeros(2)
+    traffic_convention[int(is_rhd)] = 1
+
+    vec_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
+    if desire >= 0 and desire < ModelConstants.DESIRE_LEN:
+      vec_desire[desire] = 1
+
+    # Enable/disable nav features
+    timestamp_llk = sm["navModel"].locationMonoTime
+    nav_valid = sm.valid["navModel"] # and (nanos_since_boot() - timestamp_llk < 1e9)
+    nav_enabled = nav_valid and params.get_bool("ExperimentalMode")
+
+    if not nav_enabled:
+      nav_features[:] = 0
+      nav_instructions[:] = 0
+
+    if nav_enabled and sm.updated["navModel"]:
+      nav_features = np.array(sm["navModel"].features)
+
+    if nav_enabled and sm.updated["navInstruction"]:
+      nav_instructions[:] = 0
+      for maneuver in sm["navInstruction"].allManeuvers:
+        distance_idx = 25 + int(maneuver.distance / 20)
+        direction_idx = 0
+        if maneuver.modifier in ("left", "slight left", "sharp left"):
+          direction_idx = 1
+        if maneuver.modifier in ("right", "slight right", "sharp right"):
+          direction_idx = 2
+        if 0 <= distance_idx < 50:
+          nav_instructions[distance_idx*3 + direction_idx] = 1
+
+    # tracked dropped frames
+    vipc_dropped_frames = max(0, meta_main.frame_id - last_vipc_frame_id - 1)
+    frames_dropped = frame_dropped_filter.update(min(vipc_dropped_frames, 10))
+    if run_count < 10: # let frame drops warm up
+      frame_dropped_filter.x = 0.
+      frames_dropped = 0.
+    run_count = run_count + 1
+
+    frame_drop_ratio = frames_dropped / (1 + frames_dropped)
+    prepare_only = vipc_dropped_frames > 0
+    if prepare_only:
+      cloudlog.error(f"skipping model eval. Dropped {vipc_dropped_frames} frames")
+
+    inputs:Dict[str, np.ndarray] = {
+      'desire': vec_desire,
+      'traffic_convention': traffic_convention,
+      'driving_style': driving_style,
+      'nav_features': nav_features,
+      'nav_instructions': nav_instructions}
+
+    mt1 = time.perf_counter()
+    model_output = model.run(buf_main, buf_extra, model_transform_main, model_transform_extra, inputs, prepare_only)
+    mt2 = time.perf_counter()
+    model_execution_time = mt2 - mt1
+
+    if model_output is not None:
+      modelv2_send = messaging.new_message('modelV2')
+      posenet_send = messaging.new_message('cameraOdometry')
+      fill_model_msg(modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio,
+                      meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen)
+
+      fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen)
+      pm.send('modelV2', modelv2_send)
+      pm.send('cameraOdometry', posenet_send)
+
+    last_vipc_frame_id = meta_main.frame_id
+
+
+if __name__ == "__main__":
+  try:
+    main()
+  except KeyboardInterrupt:
+    cloudlog.warning(f"child {PROCESS_NAME} got SIGINT")
+  except Exception:
+    sentry.capture_exception()
+    raise
diff --git a/laika/lib/__init__.py b/selfdrive/modeld/models/__init__.py
similarity index 100%
rename from laika/lib/__init__.py
rename to selfdrive/modeld/models/__init__.py
diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc
index b7c9051c6e..5e28e9b95d 100644
--- a/selfdrive/modeld/models/commonmodel.cc
+++ b/selfdrive/modeld/models/commonmodel.cc
@@ -55,14 +55,14 @@ ModelFrame::~ModelFrame() {
 void softmax(const float* input, float* output, size_t len) {
   const float max_val = *std::max_element(input, input + len);
   float denominator = 0;
-  for(int i = 0; i < len; i++) {
+  for (int i = 0; i < len; i++) {
     float const v_exp = expf(input[i] - max_val);
     denominator += v_exp;
     output[i] = v_exp;
   }
 
   const float inv_denominator = 1. / denominator;
-  for(int i = 0; i < len; i++) {
+  for (int i = 0; i < len; i++) {
     output[i] *= inv_denominator;
   }
 }
diff --git a/selfdrive/modeld/models/commonmodel.pxd b/selfdrive/modeld/models/commonmodel.pxd
new file mode 100644
index 0000000000..57b79aeafb
--- /dev/null
+++ b/selfdrive/modeld/models/commonmodel.pxd
@@ -0,0 +1,20 @@
+# distutils: language = c++
+
+from cereal.visionipc.visionipc cimport cl_device_id, cl_context, cl_mem
+
+cdef extern from "common/mat.h":
+  cdef struct mat3:
+    float v[9]
+
+cdef extern from "common/clutil.h":
+  cdef unsigned long CL_DEVICE_TYPE_DEFAULT
+  cl_device_id cl_get_device_id(unsigned long)
+  cl_context cl_create_context(cl_device_id)
+
+cdef extern from "selfdrive/modeld/models/commonmodel.h":
+  float sigmoid(float)
+
+  cppclass ModelFrame:
+    int buf_size
+    ModelFrame(cl_device_id, cl_context)
+    float * prepare(cl_mem, int, int, int, int, mat3, cl_mem*)
diff --git a/selfdrive/modeld/models/commonmodel_pyx.pxd b/selfdrive/modeld/models/commonmodel_pyx.pxd
new file mode 100644
index 0000000000..21c0716de4
--- /dev/null
+++ b/selfdrive/modeld/models/commonmodel_pyx.pxd
@@ -0,0 +1,13 @@
+# distutils: language = c++
+
+from cereal.visionipc.visionipc cimport cl_mem
+from cereal.visionipc.visionipc_pyx cimport CLContext as BaseCLContext
+
+cdef class CLContext(BaseCLContext):
+  pass
+
+cdef class CLMem:
+  cdef cl_mem * mem;
+
+  @staticmethod
+  cdef create(void*)
diff --git a/selfdrive/modeld/models/commonmodel_pyx.pyx b/selfdrive/modeld/models/commonmodel_pyx.pyx
new file mode 100644
index 0000000000..e33d301aff
--- /dev/null
+++ b/selfdrive/modeld/models/commonmodel_pyx.pyx
@@ -0,0 +1,43 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii
+
+import numpy as np
+cimport numpy as cnp
+from libc.string cimport memcpy
+
+from cereal.visionipc.visionipc cimport cl_mem
+from cereal.visionipc.visionipc_pyx cimport VisionBuf, CLContext as BaseCLContext
+from .commonmodel cimport CL_DEVICE_TYPE_DEFAULT, cl_get_device_id, cl_create_context
+from .commonmodel cimport mat3, sigmoid as cppSigmoid, ModelFrame as cppModelFrame
+
+def sigmoid(x):
+  return cppSigmoid(x)
+
+cdef class CLContext(BaseCLContext):
+  def __cinit__(self):
+    self.device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT)
+    self.context = cl_create_context(self.device_id)
+
+cdef class CLMem:
+  @staticmethod
+  cdef create(void * cmem):
+    mem = CLMem()
+    mem.mem = <cl_mem*> cmem
+    return mem
+
+cdef class ModelFrame:
+  cdef cppModelFrame * frame
+
+  def __cinit__(self, CLContext context):
+    self.frame = new cppModelFrame(context.device_id, context.context)
+
+  def __dealloc__(self):
+    del self.frame
+
+  def prepare(self, VisionBuf buf, float[:] projection, CLMem output):
+    cdef mat3 cprojection
+    memcpy(cprojection.v, &projection[0], 9*sizeof(float))
+    cdef float * data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, output.mem)
+    if not data:
+      return None
+    return np.asarray(<cnp.float32_t[:self.frame.buf_size]> data)
diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc
deleted file mode 100644
index f7c25fc690..0000000000
--- a/selfdrive/modeld/models/dmonitoring.cc
+++ /dev/null
@@ -1,135 +0,0 @@
-#include <cstring>
-
-#include "libyuv.h"
-
-#include "common/mat.h"
-#include "common/modeldata.h"
-#include "common/params.h"
-#include "common/timing.h"
-#include "system/hardware/hw.h"
-
-#include "selfdrive/modeld/models/dmonitoring.h"
-
-constexpr int MODEL_WIDTH = 1440;
-constexpr int MODEL_HEIGHT = 960;
-
-template <class T>
-static inline T *get_buffer(std::vector<T> &buf, const size_t size) {
-  if (buf.size() < size) buf.resize(size);
-  return buf.data();
-}
-
-void dmonitoring_init(DMonitoringModelState* s) {
-
-#ifdef USE_ONNX_MODEL
-  s->m = new ONNXModel("models/dmonitoring_model.onnx", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME, true);
-#else
-  s->m = new SNPEModel("models/dmonitoring_model_q.dlc", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME, true);
-#endif
-
-  s->m->addInput("input_imgs", NULL, 0);
-  s->m->addInput("calib", s->calib, CALIB_LEN);
-}
-
-void parse_driver_data(DriverStateResult &ds_res, const DMonitoringModelState* s, int out_idx_offset) {
-  for (int i = 0; i < 3; ++i) {
-    ds_res.face_orientation[i] = s->output[out_idx_offset+i] * REG_SCALE;
-    ds_res.face_orientation_std[i] = exp(s->output[out_idx_offset+6+i]);
-  }
-  for (int i = 0; i < 2; ++i) {
-    ds_res.face_position[i] = s->output[out_idx_offset+3+i] * REG_SCALE;
-    ds_res.face_position_std[i] = exp(s->output[out_idx_offset+9+i]);
-  }
-  for (int i = 0; i < 4; ++i) {
-    ds_res.ready_prob[i] = sigmoid(s->output[out_idx_offset+35+i]);
-  }
-  for (int i = 0; i < 2; ++i) {
-    ds_res.not_ready_prob[i] = sigmoid(s->output[out_idx_offset+39+i]);
-  }
-  ds_res.face_prob = sigmoid(s->output[out_idx_offset+12]);
-  ds_res.left_eye_prob = sigmoid(s->output[out_idx_offset+21]);
-  ds_res.right_eye_prob = sigmoid(s->output[out_idx_offset+30]);
-  ds_res.left_blink_prob = sigmoid(s->output[out_idx_offset+31]);
-  ds_res.right_blink_prob = sigmoid(s->output[out_idx_offset+32]);
-  ds_res.sunglasses_prob = sigmoid(s->output[out_idx_offset+33]);
-  ds_res.occluded_prob = sigmoid(s->output[out_idx_offset+34]);
-}
-
-void fill_driver_data(cereal::DriverStateV2::DriverData::Builder ddata, const DriverStateResult &ds_res) {
-  ddata.setFaceOrientation(ds_res.face_orientation);
-  ddata.setFaceOrientationStd(ds_res.face_orientation_std);
-  ddata.setFacePosition(ds_res.face_position);
-  ddata.setFacePositionStd(ds_res.face_position_std);
-  ddata.setFaceProb(ds_res.face_prob);
-  ddata.setLeftEyeProb(ds_res.left_eye_prob);
-  ddata.setRightEyeProb(ds_res.right_eye_prob);
-  ddata.setLeftBlinkProb(ds_res.left_blink_prob);
-  ddata.setRightBlinkProb(ds_res.right_blink_prob);
-  ddata.setSunglassesProb(ds_res.sunglasses_prob);
-  ddata.setOccludedProb(ds_res.occluded_prob);
-  ddata.setReadyProb(ds_res.ready_prob);
-  ddata.setNotReadyProb(ds_res.not_ready_prob);
-}
-
-DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib) {
-  int v_off = height - MODEL_HEIGHT;
-  int h_off = (width - MODEL_WIDTH) / 2;
-  int yuv_buf_len = MODEL_WIDTH * MODEL_HEIGHT;
-
-  uint8_t *raw_buf = (uint8_t *) stream_buf;
-  // vertical crop free
-  uint8_t *raw_y_start = raw_buf + stride * v_off;
-
-  uint8_t *net_input_buf = get_buffer(s->net_input_buf, yuv_buf_len);
-
-  // here makes a uint8 copy
-  for (int r = 0; r < MODEL_HEIGHT; ++r) {
-    memcpy(net_input_buf + r * MODEL_WIDTH, raw_y_start + r * stride + h_off, MODEL_WIDTH);
-  }
-
-  // printf("preprocess completed. %d \n", yuv_buf_len);
-  // FILE *dump_yuv_file = fopen("/tmp/rawdump.yuv", "wb");
-  // fwrite(net_input_buf, yuv_buf_len, sizeof(uint8_t), dump_yuv_file);
-  // fclose(dump_yuv_file);
-
-  double t1 = millis_since_boot();
-  s->m->setInputBuffer("input_imgs", (float*)net_input_buf, yuv_buf_len / sizeof(float));
-  for (int i = 0; i < CALIB_LEN; i++) {
-    s->calib[i] = calib[i];
-  }
-  s->m->execute();
-  double t2 = millis_since_boot();
-
-  DMonitoringModelResult model_res = {0};
-  parse_driver_data(model_res.driver_state_lhd, s, 0);
-  parse_driver_data(model_res.driver_state_rhd, s, 41);
-  model_res.poor_vision_prob = sigmoid(s->output[82]);
-  model_res.wheel_on_right_prob = sigmoid(s->output[83]);
-  model_res.dsp_execution_time = (t2 - t1) / 1000.;
-
-  return model_res;
-}
-
-void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringModelResult &model_res, float execution_time, kj::ArrayPtr<const float> raw_pred) {
-  // make msg
-  MessageBuilder msg;
-  auto framed = msg.initEvent().initDriverStateV2();
-  framed.setFrameId(frame_id);
-  framed.setModelExecutionTime(execution_time);
-  framed.setDspExecutionTime(model_res.dsp_execution_time);
-
-  framed.setPoorVisionProb(model_res.poor_vision_prob);
-  framed.setWheelOnRightProb(model_res.wheel_on_right_prob);
-  fill_driver_data(framed.initLeftDriverData(), model_res.driver_state_lhd);
-  fill_driver_data(framed.initRightDriverData(), model_res.driver_state_rhd);
-
-  if (send_raw_pred) {
-    framed.setRawPredictions(raw_pred.asBytes());
-  }
-
-  pm.send("driverStateV2", msg);
-}
-
-void dmonitoring_free(DMonitoringModelState* s) {
-  delete s->m;
-}
diff --git a/selfdrive/modeld/models/dmonitoring.h b/selfdrive/modeld/models/dmonitoring.h
deleted file mode 100644
index ae2bf05394..0000000000
--- a/selfdrive/modeld/models/dmonitoring.h
+++ /dev/null
@@ -1,50 +0,0 @@
-#pragma once
-
-#include <vector>
-
-#include "cereal/messaging/messaging.h"
-#include "common/util.h"
-#include "selfdrive/modeld/models/commonmodel.h"
-#include "selfdrive/modeld/runners/run.h"
-
-#define CALIB_LEN 3
-
-#define OUTPUT_SIZE 84
-#define REG_SCALE 0.25f
-
-typedef struct DriverStateResult {
-  float face_orientation[3];
-  float face_orientation_std[3];
-  float face_position[2];
-  float face_position_std[2];
-  float face_prob;
-  float left_eye_prob;
-  float right_eye_prob;
-  float left_blink_prob;
-  float right_blink_prob;
-  float sunglasses_prob;
-  float occluded_prob;
-  float ready_prob[4];
-  float not_ready_prob[2];
-} DriverStateResult;
-
-typedef struct DMonitoringModelResult {
-  DriverStateResult driver_state_lhd;
-  DriverStateResult driver_state_rhd;
-  float poor_vision_prob;
-  float wheel_on_right_prob;
-  float dsp_execution_time;
-} DMonitoringModelResult;
-
-typedef struct DMonitoringModelState {
-  RunModel *m;
-  float output[OUTPUT_SIZE];
-  std::vector<uint8_t> net_input_buf;
-  float calib[CALIB_LEN];
-} DMonitoringModelState;
-
-void dmonitoring_init(DMonitoringModelState* s);
-DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib);
-void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringModelResult &model_res, float execution_time, kj::ArrayPtr<const float> raw_pred);
-void dmonitoring_free(DMonitoringModelState* s);
-
diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc
deleted file mode 100644
index dedbc8edac..0000000000
--- a/selfdrive/modeld/models/driving.cc
+++ /dev/null
@@ -1,457 +0,0 @@
-#include "selfdrive/modeld/models/driving.h"
-
-#include <fcntl.h>
-#include <unistd.h>
-
-#include <cassert>
-#include <cstring>
-
-#include <eigen3/Eigen/Dense>
-
-#include "common/clutil.h"
-#include "common/params.h"
-#include "common/timing.h"
-#include "common/swaglog.h"
-
-constexpr float FCW_THRESHOLD_5MS2_HIGH = 0.15;
-constexpr float FCW_THRESHOLD_5MS2_LOW = 0.05;
-constexpr float FCW_THRESHOLD_3MS2 = 0.7;
-
-std::array<float, 5> prev_brake_5ms2_probs = {0,0,0,0,0};
-std::array<float, 3> prev_brake_3ms2_probs = {0,0,0};
-
-// #define DUMP_YUV
-
-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>("models/supercombo.thneed",
-#elif USE_ONNX_MODEL
-  s->m = std::make_unique<ONNXModel>("models/supercombo.onnx",
-#else
-  s->m = std::make_unique<SNPEModel>("models/supercombo.dlc",
-#endif
-   &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME, false, context);
-
-  s->m->addInput("input_imgs", NULL, 0);
-  s->m->addInput("big_input_imgs", NULL, 0);
-
-  // TODO: the input is important here, still need to fix this
-#ifdef DESIRE
-  s->m->addInput("desire_pulse", s->pulse_desire, DESIRE_LEN*(HISTORY_BUFFER_LEN+1));
-#endif
-
-#ifdef TRAFFIC_CONVENTION
-  s->m->addInput("traffic_convention", s->traffic_convention, TRAFFIC_CONVENTION_LEN);
-#endif
-
-#ifdef DRIVING_STYLE
-  s->m->addInput("driving_style", s->driving_style, DRIVING_STYLE_LEN);
-#endif
-
-#ifdef NAV
-  s->m->addInput("nav_features", s->nav_features, NAV_FEATURE_LEN);
-#endif
-
-#ifdef TEMPORAL
-  s->m->addInput("feature_buffer", &s->feature_buffer[0], TEMPORAL_SIZE);
-#endif
-
-}
-
-ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf,
-                              const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, float *driving_style, float *nav_features, bool prepare_only) {
-#ifdef DESIRE
-  std::memmove(&s->pulse_desire[0], &s->pulse_desire[DESIRE_LEN], sizeof(float) * DESIRE_LEN*HISTORY_BUFFER_LEN);
-  if (desire_in != NULL) {
-    for (int i = 1; i < DESIRE_LEN; i++) {
-      // Model decides when action is completed
-      // so desire input is just a pulse triggered on rising edge
-      if (desire_in[i] - s->prev_desire[i] > .99) {
-        s->pulse_desire[DESIRE_LEN*HISTORY_BUFFER_LEN+i] = desire_in[i];
-      } else {
-        s->pulse_desire[DESIRE_LEN*HISTORY_BUFFER_LEN+i] = 0.0;
-      }
-      s->prev_desire[i] = desire_in[i];
-    }
-  }
-LOGT("Desire enqueued");
-#endif
-
-#ifdef NAV
-  std::memcpy(s->nav_features, nav_features, sizeof(float)*NAV_FEATURE_LEN);
-#endif
-
-#ifdef DRIVING_STYLE
-  std::memcpy(s->driving_style, driving_style, sizeof(float)*DRIVING_STYLE_LEN);
-#endif
-
-  int rhd_idx = is_rhd;
-  s->traffic_convention[rhd_idx] = 1.0;
-  s->traffic_convention[1-rhd_idx] = 0.0;
-
-  // if getInputBuf is not NULL, net_input_buf will be
-  auto net_input_buf = s->frame->prepare(buf->buf_cl, buf->width, buf->height, buf->stride, buf->uv_offset, transform, static_cast<cl_mem*>(s->m->getCLBuffer("input_imgs")));
-  s->m->setInputBuffer("input_imgs", net_input_buf, s->frame->buf_size);
-  LOGT("Image added");
-
-  if (wbuf != nullptr) {
-    auto net_extra_buf = s->wide_frame->prepare(wbuf->buf_cl, wbuf->width, wbuf->height, wbuf->stride, wbuf->uv_offset, transform_wide, static_cast<cl_mem*>(s->m->getCLBuffer("big_input_imgs")));
-    s->m->setInputBuffer("big_input_imgs", net_extra_buf, s->wide_frame->buf_size);
-    LOGT("Extra image added");
-  }
-
-  if (prepare_only) {
-    return nullptr;
-  }
-
-  s->m->execute();
-  LOGT("Execution finished");
-
-  #ifdef TEMPORAL
-    std::memmove(&s->feature_buffer[0], &s->feature_buffer[FEATURE_LEN], sizeof(float) * FEATURE_LEN*(HISTORY_BUFFER_LEN-1));
-    std::memcpy(&s->feature_buffer[FEATURE_LEN*(HISTORY_BUFFER_LEN-1)], &s->output[OUTPUT_SIZE], sizeof(float) * FEATURE_LEN);
-    LOGT("Features enqueued");
-  #endif
-
-  return (ModelOutput*)&s->output;
-}
-
-void model_free(ModelState* s) {
-  delete s->frame;
-  delete s->wide_frame;
-}
-
-void fill_lead(cereal::ModelDataV2::LeadDataV3::Builder lead, const ModelOutputLeads &leads, int t_idx, float prob_t) {
-  std::array<float, LEAD_TRAJ_LEN> lead_t = {0.0, 2.0, 4.0, 6.0, 8.0, 10.0};
-  const auto &best_prediction = leads.get_best_prediction(t_idx);
-  lead.setProb(sigmoid(leads.prob[t_idx]));
-  lead.setProbTime(prob_t);
-  std::array<float, LEAD_TRAJ_LEN> lead_x, lead_y, lead_v, lead_a;
-  std::array<float, LEAD_TRAJ_LEN> lead_x_std, lead_y_std, lead_v_std, lead_a_std;
-  for (int i=0; i<LEAD_TRAJ_LEN; i++) {
-    lead_x[i] = best_prediction.mean[i].x;
-    lead_y[i] = best_prediction.mean[i].y;
-    lead_v[i] = best_prediction.mean[i].velocity;
-    lead_a[i] = best_prediction.mean[i].acceleration;
-    lead_x_std[i] = exp(best_prediction.std[i].x);
-    lead_y_std[i] = exp(best_prediction.std[i].y);
-    lead_v_std[i] = exp(best_prediction.std[i].velocity);
-    lead_a_std[i] = exp(best_prediction.std[i].acceleration);
-  }
-  lead.setT(to_kj_array_ptr(lead_t));
-  lead.setX(to_kj_array_ptr(lead_x));
-  lead.setY(to_kj_array_ptr(lead_y));
-  lead.setV(to_kj_array_ptr(lead_v));
-  lead.setA(to_kj_array_ptr(lead_a));
-  lead.setXStd(to_kj_array_ptr(lead_x_std));
-  lead.setYStd(to_kj_array_ptr(lead_y_std));
-  lead.setVStd(to_kj_array_ptr(lead_v_std));
-  lead.setAStd(to_kj_array_ptr(lead_a_std));
-}
-
-void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const ModelOutputMeta &meta_data) {
-  std::array<float, DESIRE_LEN> desire_state_softmax;
-  softmax(meta_data.desire_state_prob.array.data(), desire_state_softmax.data(), DESIRE_LEN);
-
-  std::array<float, DESIRE_PRED_LEN * DESIRE_LEN> desire_pred_softmax;
-  for (int i=0; i<DESIRE_PRED_LEN; i++) {
-    softmax(meta_data.desire_pred_prob[i].array.data(), desire_pred_softmax.data() + (i * DESIRE_LEN), DESIRE_LEN);
-  }
-
-  std::array<float, DISENGAGE_LEN> lat_long_t = {2,4,6,8,10};
-  std::array<float, DISENGAGE_LEN> gas_disengage_sigmoid, brake_disengage_sigmoid, steer_override_sigmoid,
-                                   brake_3ms2_sigmoid, brake_4ms2_sigmoid, brake_5ms2_sigmoid;
-  for (int i=0; i<DISENGAGE_LEN; i++) {
-    gas_disengage_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].gas_disengage);
-    brake_disengage_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_disengage);
-    steer_override_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].steer_override);
-    brake_3ms2_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_3ms2);
-    brake_4ms2_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_4ms2);
-    brake_5ms2_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_5ms2);
-    //gas_pressed_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].gas_pressed);
-  }
-
-  std::memmove(prev_brake_5ms2_probs.data(), &prev_brake_5ms2_probs[1], 4*sizeof(float));
-  std::memmove(prev_brake_3ms2_probs.data(), &prev_brake_3ms2_probs[1], 2*sizeof(float));
-  prev_brake_5ms2_probs[4] = brake_5ms2_sigmoid[0];
-  prev_brake_3ms2_probs[2] = brake_3ms2_sigmoid[0];
-
-  bool above_fcw_threshold = true;
-  for (int i=0; i<prev_brake_5ms2_probs.size(); i++) {
-    float threshold = i < 2 ? FCW_THRESHOLD_5MS2_LOW : FCW_THRESHOLD_5MS2_HIGH;
-    above_fcw_threshold = above_fcw_threshold && prev_brake_5ms2_probs[i] > threshold;
-  }
-  for (int i=0; i<prev_brake_3ms2_probs.size(); i++) {
-    above_fcw_threshold = above_fcw_threshold && prev_brake_3ms2_probs[i] > FCW_THRESHOLD_3MS2;
-  }
-
-  auto disengage = meta.initDisengagePredictions();
-  disengage.setT(to_kj_array_ptr(lat_long_t));
-  disengage.setGasDisengageProbs(to_kj_array_ptr(gas_disengage_sigmoid));
-  disengage.setBrakeDisengageProbs(to_kj_array_ptr(brake_disengage_sigmoid));
-  disengage.setSteerOverrideProbs(to_kj_array_ptr(steer_override_sigmoid));
-  disengage.setBrake3MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_3ms2_sigmoid));
-  disengage.setBrake4MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_4ms2_sigmoid));
-  disengage.setBrake5MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_5ms2_sigmoid));
-
-  meta.setEngagedProb(sigmoid(meta_data.engaged_prob));
-  meta.setDesirePrediction(to_kj_array_ptr(desire_pred_softmax));
-  meta.setDesireState(to_kj_array_ptr(desire_state_softmax));
-  meta.setHardBrakePredicted(above_fcw_threshold);
-}
-
-void fill_confidence(ModelState* s, cereal::ModelDataV2::Builder &framed) {
-  if (framed.getFrameId() % (2*MODEL_FREQ) == 0) {
-    // update every 2s to match predictions interval
-    auto dbps = framed.getMeta().getDisengagePredictions().getBrakeDisengageProbs();
-    auto dgps = framed.getMeta().getDisengagePredictions().getGasDisengageProbs();
-    auto dsps = framed.getMeta().getDisengagePredictions().getSteerOverrideProbs();
-
-    float any_dp[DISENGAGE_LEN];
-    float dp_ind[DISENGAGE_LEN];
-
-    for (int i = 0; i < DISENGAGE_LEN; i++) {
-      any_dp[i] = 1 - ((1-dbps[i])*(1-dgps[i])*(1-dsps[i])); // any disengage prob
-    }
-
-    dp_ind[0] = any_dp[0];
-    for (int i = 0; i < DISENGAGE_LEN-1; i++) {
-      dp_ind[i+1] = (any_dp[i+1] - any_dp[i]) / (1 - any_dp[i]); // independent disengage prob for each 2s slice
-    }
-
-    // rolling buf for 2, 4, 6, 8, 10s
-    std::memmove(&s->disengage_buffer[0], &s->disengage_buffer[DISENGAGE_LEN], sizeof(float) * DISENGAGE_LEN * (DISENGAGE_LEN-1));
-    std::memcpy(&s->disengage_buffer[DISENGAGE_LEN * (DISENGAGE_LEN-1)], &dp_ind[0], sizeof(float) * DISENGAGE_LEN);
-  }
-
-  float score = 0;
-  for (int i = 0; i < DISENGAGE_LEN; i++) {
-    score += s->disengage_buffer[i*DISENGAGE_LEN+DISENGAGE_LEN-1-i] / DISENGAGE_LEN;
-  }
-
-  if (score < RYG_GREEN) {
-    framed.setConfidence(cereal::ModelDataV2::ConfidenceClass::GREEN);
-  } else if (score < RYG_YELLOW) {
-    framed.setConfidence(cereal::ModelDataV2::ConfidenceClass::YELLOW);
-  } else {
-    framed.setConfidence(cereal::ModelDataV2::ConfidenceClass::RED);
-  }
-}
-
-template<size_t size>
-void fill_xyzt(cereal::XYZTData::Builder xyzt, const std::array<float, size> &t,
-               const std::array<float, size> &x, const std::array<float, size> &y, const std::array<float, size> &z) {
-  xyzt.setT(to_kj_array_ptr(t));
-  xyzt.setX(to_kj_array_ptr(x));
-  xyzt.setY(to_kj_array_ptr(y));
-  xyzt.setZ(to_kj_array_ptr(z));
-}
-
-template<size_t size>
-void fill_xyzt(cereal::XYZTData::Builder xyzt, const std::array<float, size> &t,
-               const std::array<float, size> &x, const std::array<float, size> &y, const std::array<float, size> &z,
-               const std::array<float, size> &x_std, const std::array<float, size> &y_std, const std::array<float, size> &z_std) {
-  fill_xyzt(xyzt, t, x, y, z);
-  xyzt.setXStd(to_kj_array_ptr(x_std));
-  xyzt.setYStd(to_kj_array_ptr(y_std));
-  xyzt.setZStd(to_kj_array_ptr(z_std));
-}
-
-void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPrediction &plan) {
-  std::array<float, TRAJECTORY_SIZE> pos_x, pos_y, pos_z;
-  std::array<float, TRAJECTORY_SIZE> pos_x_std, pos_y_std, pos_z_std;
-  std::array<float, TRAJECTORY_SIZE> vel_x, vel_y, vel_z;
-  std::array<float, TRAJECTORY_SIZE> rot_x, rot_y, rot_z;
-  std::array<float, TRAJECTORY_SIZE> acc_x, acc_y, acc_z;
-  std::array<float, TRAJECTORY_SIZE> rot_rate_x, rot_rate_y, rot_rate_z;
-
-  for(int i=0; i<TRAJECTORY_SIZE; i++) {
-    pos_x[i] = plan.mean[i].position.x;
-    pos_y[i] = plan.mean[i].position.y;
-    pos_z[i] = plan.mean[i].position.z;
-    pos_x_std[i] = exp(plan.std[i].position.x);
-    pos_y_std[i] = exp(plan.std[i].position.y);
-    pos_z_std[i] = exp(plan.std[i].position.z);
-    vel_x[i] = plan.mean[i].velocity.x;
-    vel_y[i] = plan.mean[i].velocity.y;
-    vel_z[i] = plan.mean[i].velocity.z;
-    acc_x[i] = plan.mean[i].acceleration.x;
-    acc_y[i] = plan.mean[i].acceleration.y;
-    acc_z[i] = plan.mean[i].acceleration.z;
-    rot_x[i] = plan.mean[i].rotation.x;
-    rot_y[i] = plan.mean[i].rotation.y;
-    rot_z[i] = plan.mean[i].rotation.z;
-    rot_rate_x[i] = plan.mean[i].rotation_rate.x;
-    rot_rate_y[i] = plan.mean[i].rotation_rate.y;
-    rot_rate_z[i] = plan.mean[i].rotation_rate.z;
-  }
-
-  fill_xyzt(framed.initPosition(), T_IDXS_FLOAT, pos_x, pos_y, pos_z, pos_x_std, pos_y_std, pos_z_std);
-  fill_xyzt(framed.initVelocity(), T_IDXS_FLOAT, vel_x, vel_y, vel_z);
-  fill_xyzt(framed.initAcceleration(), T_IDXS_FLOAT, acc_x, acc_y, acc_z);
-  fill_xyzt(framed.initOrientation(), T_IDXS_FLOAT, rot_x, rot_y, rot_z);
-  fill_xyzt(framed.initOrientationRate(), T_IDXS_FLOAT, rot_rate_x, rot_rate_y, rot_rate_z);
-}
-
-void fill_lane_lines(cereal::ModelDataV2::Builder &framed, const std::array<float, TRAJECTORY_SIZE> &plan_t,
-                     const ModelOutputLaneLines &lanes) {
-  std::array<float, TRAJECTORY_SIZE> left_far_y, left_far_z;
-  std::array<float, TRAJECTORY_SIZE> left_near_y, left_near_z;
-  std::array<float, TRAJECTORY_SIZE> right_near_y, right_near_z;
-  std::array<float, TRAJECTORY_SIZE> right_far_y, right_far_z;
-  for (int j=0; j<TRAJECTORY_SIZE; j++) {
-    left_far_y[j] = lanes.mean.left_far[j].y;
-    left_far_z[j] = lanes.mean.left_far[j].z;
-    left_near_y[j] = lanes.mean.left_near[j].y;
-    left_near_z[j] = lanes.mean.left_near[j].z;
-    right_near_y[j] = lanes.mean.right_near[j].y;
-    right_near_z[j] = lanes.mean.right_near[j].z;
-    right_far_y[j] = lanes.mean.right_far[j].y;
-    right_far_z[j] = lanes.mean.right_far[j].z;
-  }
-
-  auto lane_lines = framed.initLaneLines(4);
-  fill_xyzt(lane_lines[0], plan_t, X_IDXS_FLOAT, left_far_y, left_far_z);
-  fill_xyzt(lane_lines[1], plan_t, X_IDXS_FLOAT, left_near_y, left_near_z);
-  fill_xyzt(lane_lines[2], plan_t, X_IDXS_FLOAT, right_near_y, right_near_z);
-  fill_xyzt(lane_lines[3], plan_t, X_IDXS_FLOAT, right_far_y, right_far_z);
-
-  framed.setLaneLineStds({
-    exp(lanes.std.left_far[0].y),
-    exp(lanes.std.left_near[0].y),
-    exp(lanes.std.right_near[0].y),
-    exp(lanes.std.right_far[0].y),
-  });
-
-  framed.setLaneLineProbs({
-    sigmoid(lanes.prob.left_far.val),
-    sigmoid(lanes.prob.left_near.val),
-    sigmoid(lanes.prob.right_near.val),
-    sigmoid(lanes.prob.right_far.val),
-  });
-}
-
-void fill_road_edges(cereal::ModelDataV2::Builder &framed, const std::array<float, TRAJECTORY_SIZE> &plan_t,
-                     const ModelOutputRoadEdges &edges) {
-  std::array<float, TRAJECTORY_SIZE> left_y, left_z;
-  std::array<float, TRAJECTORY_SIZE> right_y, right_z;
-  for (int j=0; j<TRAJECTORY_SIZE; j++) {
-    left_y[j] = edges.mean.left[j].y;
-    left_z[j] = edges.mean.left[j].z;
-    right_y[j] = edges.mean.right[j].y;
-    right_z[j] = edges.mean.right[j].z;
-  }
-
-  auto road_edges = framed.initRoadEdges(2);
-  fill_xyzt(road_edges[0], plan_t, X_IDXS_FLOAT, left_y, left_z);
-  fill_xyzt(road_edges[1], plan_t, X_IDXS_FLOAT, right_y, right_z);
-
-  framed.setRoadEdgeStds({
-    exp(edges.std.left[0].y),
-    exp(edges.std.right[0].y),
-  });
-}
-
-void fill_model(ModelState* s, cereal::ModelDataV2::Builder &framed, const ModelOutput &net_outputs) {
-  const auto &best_plan = net_outputs.plans.get_best_prediction();
-  std::array<float, TRAJECTORY_SIZE> plan_t;
-  std::fill_n(plan_t.data(), plan_t.size(), NAN);
-  plan_t[0] = 0.0;
-  for (int xidx=1, tidx=0; xidx<TRAJECTORY_SIZE; xidx++) {
-    // increment tidx until we find an element that's further away than the current xidx
-    for (int next_tid = tidx + 1; next_tid < TRAJECTORY_SIZE && best_plan.mean[next_tid].position.x < X_IDXS[xidx]; next_tid++) {
-      tidx++;
-    }
-    if (tidx == TRAJECTORY_SIZE - 1) {
-      // if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break
-      plan_t[xidx] = T_IDXS[TRAJECTORY_SIZE - 1];
-      break;
-    }
-
-    // interpolate to find `t` for the current xidx
-    float current_x_val = best_plan.mean[tidx].position.x;
-    float next_x_val = best_plan.mean[tidx+1].position.x;
-    float p = (X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val);
-    plan_t[xidx] = p * T_IDXS[tidx+1] + (1 - p) * T_IDXS[tidx];
-  }
-
-  fill_plan(framed, best_plan);
-  fill_lane_lines(framed, plan_t, net_outputs.lane_lines);
-  fill_road_edges(framed, plan_t, net_outputs.road_edges);
-
-  // meta
-  fill_meta(framed.initMeta(), net_outputs.meta);
-
-  // confidence
-  fill_confidence(s, framed);
-
-  // leads
-  auto leads = framed.initLeadsV3(LEAD_MHP_SELECTION);
-  std::array<float, LEAD_MHP_SELECTION> t_offsets = {0.0, 2.0, 4.0};
-  for (int i=0; i<LEAD_MHP_SELECTION; i++) {
-    fill_lead(leads[i], net_outputs.leads, i, t_offsets[i]);
-  }
-
-  // temporal pose
-  const auto &v_mean = net_outputs.temporal_pose.velocity_mean;
-  const auto &r_mean = net_outputs.temporal_pose.rotation_mean;
-  const auto &v_std = net_outputs.temporal_pose.velocity_std;
-  const auto &r_std = net_outputs.temporal_pose.rotation_std;
-  auto temporal_pose = framed.initTemporalPose();
-  temporal_pose.setTrans({v_mean.x, v_mean.y, v_mean.z});
-  temporal_pose.setRot({r_mean.x, r_mean.y, r_mean.z});
-  temporal_pose.setTransStd({exp(v_std.x), exp(v_std.y), exp(v_std.z)});
-  temporal_pose.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)});
-}
-
-void model_publish(ModelState* s, 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, uint64_t timestamp_llk,
-                   float model_execution_time, const bool nav_enabled, 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);
-  framed.setLocationMonoTime(timestamp_llk);
-  framed.setModelExecutionTime(model_execution_time);
-  framed.setNavEnabled(nav_enabled);
-  if (send_raw_pred) {
-    framed.setRawPredictions((kj::ArrayPtr<const float>(s->output.data(), s->output.size())).asBytes());
-  }
-  fill_model(s, framed, net_outputs);
-  pm.send("modelV2", msg);
-}
-
-void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
-                     const ModelOutput &net_outputs, uint64_t timestamp_eof, const bool valid) {
-  MessageBuilder msg;
-  const auto &v_mean = net_outputs.pose.velocity_mean;
-  const auto &r_mean = net_outputs.pose.rotation_mean;
-  const auto &t_mean = net_outputs.wide_from_device_euler.mean;
-  const auto &v_std = net_outputs.pose.velocity_std;
-  const auto &r_std = net_outputs.pose.rotation_std;
-  const auto &t_std = net_outputs.wide_from_device_euler.std;
-  const auto &road_transform_trans_mean = net_outputs.road_transform.position_mean;
-  const auto &road_transform_trans_std = net_outputs.road_transform.position_std;
-
-  auto posenetd = msg.initEvent(valid && (vipc_dropped_frames < 1)).initCameraOdometry();
-  posenetd.setTrans({v_mean.x, v_mean.y, v_mean.z});
-  posenetd.setRot({r_mean.x, r_mean.y, r_mean.z});
-  posenetd.setWideFromDeviceEuler({t_mean.x, t_mean.y, t_mean.z});
-  posenetd.setRoadTransformTrans({road_transform_trans_mean.x, road_transform_trans_mean.y, road_transform_trans_mean.z});
-  posenetd.setTransStd({exp(v_std.x), exp(v_std.y), exp(v_std.z)});
-  posenetd.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)});
-  posenetd.setWideFromDeviceEulerStd({exp(t_std.x), exp(t_std.y), exp(t_std.z)});
-  posenetd.setRoadTransformTransStd({exp(road_transform_trans_std.x), exp(road_transform_trans_std.y), exp(road_transform_trans_std.z)});
-
-  posenetd.setTimestampEof(timestamp_eof);
-  posenetd.setFrameId(vipc_frame_id);
-
-  pm.send("cameraOdometry", msg);
-}
diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h
deleted file mode 100644
index f664a4f397..0000000000
--- a/selfdrive/modeld/models/driving.h
+++ /dev/null
@@ -1,291 +0,0 @@
-#pragma once
-
-#include <array>
-#include <memory>
-
-#include "cereal/messaging/messaging.h"
-#include "cereal/visionipc/visionipc_client.h"
-#include "common/mat.h"
-#include "common/modeldata.h"
-#include "common/util.h"
-#include "selfdrive/modeld/models/commonmodel.h"
-#include "selfdrive/modeld/models/nav.h"
-#include "selfdrive/modeld/runners/run.h"
-
-// gate this here
-#define TEMPORAL
-#define DESIRE
-#define TRAFFIC_CONVENTION
-#define NAV
-
-constexpr int FEATURE_LEN = 128;
-constexpr int HISTORY_BUFFER_LEN = 99;
-constexpr int DESIRE_LEN = 8;
-constexpr int DESIRE_PRED_LEN = 4;
-constexpr int TRAFFIC_CONVENTION_LEN = 2;
-constexpr int DRIVING_STYLE_LEN = 12;
-constexpr int MODEL_FREQ = 20;
-
-constexpr int DISENGAGE_LEN = 5;
-constexpr int BLINKER_LEN = 6;
-constexpr int META_STRIDE = 7;
-
-constexpr int PLAN_MHP_N = 5;
-
-constexpr int LEAD_MHP_N = 2;
-constexpr int LEAD_TRAJ_LEN = 6;
-constexpr int LEAD_PRED_DIM = 4;
-constexpr int LEAD_MHP_SELECTION = 3;
-// Padding to get output shape as multiple of 4
-constexpr int PAD_SIZE = 2;
-
-struct ModelOutputXYZ {
-  float x;
-  float y;
-  float z;
-};
-static_assert(sizeof(ModelOutputXYZ) == sizeof(float)*3);
-
-struct ModelOutputYZ {
-  float y;
-  float z;
-};
-static_assert(sizeof(ModelOutputYZ) == sizeof(float)*2);
-
-struct ModelOutputPlanElement {
-  ModelOutputXYZ position;
-  ModelOutputXYZ velocity;
-  ModelOutputXYZ acceleration;
-  ModelOutputXYZ rotation;
-  ModelOutputXYZ rotation_rate;
-};
-static_assert(sizeof(ModelOutputPlanElement) == sizeof(ModelOutputXYZ)*5);
-
-struct ModelOutputPlanPrediction {
-  std::array<ModelOutputPlanElement, TRAJECTORY_SIZE> mean;
-  std::array<ModelOutputPlanElement, TRAJECTORY_SIZE> std;
-  float prob;
-};
-static_assert(sizeof(ModelOutputPlanPrediction) == (sizeof(ModelOutputPlanElement)*TRAJECTORY_SIZE*2) + sizeof(float));
-
-struct ModelOutputPlans {
-  std::array<ModelOutputPlanPrediction, PLAN_MHP_N> prediction;
-
-  constexpr const ModelOutputPlanPrediction &get_best_prediction() const {
-    int max_idx = 0;
-    for (int i = 1; i < prediction.size(); i++) {
-      if (prediction[i].prob > prediction[max_idx].prob) {
-        max_idx = i;
-      }
-    }
-    return prediction[max_idx];
-  }
-};
-static_assert(sizeof(ModelOutputPlans) == sizeof(ModelOutputPlanPrediction)*PLAN_MHP_N);
-
-struct ModelOutputLinesXY {
-  std::array<ModelOutputYZ, TRAJECTORY_SIZE> left_far;
-  std::array<ModelOutputYZ, TRAJECTORY_SIZE> left_near;
-  std::array<ModelOutputYZ, TRAJECTORY_SIZE> right_near;
-  std::array<ModelOutputYZ, TRAJECTORY_SIZE> right_far;
-};
-static_assert(sizeof(ModelOutputLinesXY) == sizeof(ModelOutputYZ)*TRAJECTORY_SIZE*4);
-
-struct ModelOutputLineProbVal {
-  float val_deprecated;
-  float val;
-};
-static_assert(sizeof(ModelOutputLineProbVal) == sizeof(float)*2);
-
-struct ModelOutputLinesProb {
-  ModelOutputLineProbVal left_far;
-  ModelOutputLineProbVal left_near;
-  ModelOutputLineProbVal right_near;
-  ModelOutputLineProbVal right_far;
-};
-static_assert(sizeof(ModelOutputLinesProb) == sizeof(ModelOutputLineProbVal)*4);
-
-struct ModelOutputLaneLines {
-  ModelOutputLinesXY mean;
-  ModelOutputLinesXY std;
-  ModelOutputLinesProb prob;
-};
-static_assert(sizeof(ModelOutputLaneLines) == (sizeof(ModelOutputLinesXY)*2) + sizeof(ModelOutputLinesProb));
-
-struct ModelOutputEdgessXY {
-  std::array<ModelOutputYZ, TRAJECTORY_SIZE> left;
-  std::array<ModelOutputYZ, TRAJECTORY_SIZE> right;
-};
-static_assert(sizeof(ModelOutputEdgessXY) == sizeof(ModelOutputYZ)*TRAJECTORY_SIZE*2);
-
-struct ModelOutputRoadEdges {
-  ModelOutputEdgessXY mean;
-  ModelOutputEdgessXY std;
-};
-static_assert(sizeof(ModelOutputRoadEdges) == (sizeof(ModelOutputEdgessXY)*2));
-
-struct ModelOutputLeadElement {
-  float x;
-  float y;
-  float velocity;
-  float acceleration;
-};
-static_assert(sizeof(ModelOutputLeadElement) == sizeof(float)*4);
-
-struct ModelOutputLeadPrediction {
-  std::array<ModelOutputLeadElement, LEAD_TRAJ_LEN> mean;
-  std::array<ModelOutputLeadElement, LEAD_TRAJ_LEN> std;
-  std::array<float, LEAD_MHP_SELECTION> prob;
-};
-static_assert(sizeof(ModelOutputLeadPrediction) == (sizeof(ModelOutputLeadElement)*LEAD_TRAJ_LEN*2) + (sizeof(float)*LEAD_MHP_SELECTION));
-
-struct ModelOutputLeads {
-  std::array<ModelOutputLeadPrediction, LEAD_MHP_N> prediction;
-  std::array<float, LEAD_MHP_SELECTION> prob;
-
-  constexpr const ModelOutputLeadPrediction &get_best_prediction(int t_idx) const {
-    int max_idx = 0;
-    for (int i = 1; i < prediction.size(); i++) {
-      if (prediction[i].prob[t_idx] > prediction[max_idx].prob[t_idx]) {
-        max_idx = i;
-      }
-    }
-    return prediction[max_idx];
-  }
-};
-static_assert(sizeof(ModelOutputLeads) == (sizeof(ModelOutputLeadPrediction)*LEAD_MHP_N) + (sizeof(float)*LEAD_MHP_SELECTION));
-
-
-struct ModelOutputPose {
-  ModelOutputXYZ velocity_mean;
-  ModelOutputXYZ rotation_mean;
-  ModelOutputXYZ velocity_std;
-  ModelOutputXYZ rotation_std;
-};
-static_assert(sizeof(ModelOutputPose) == sizeof(ModelOutputXYZ)*4);
-
-struct ModelOutputWideFromDeviceEuler {
-  ModelOutputXYZ mean;
-  ModelOutputXYZ std;
-};
-static_assert(sizeof(ModelOutputWideFromDeviceEuler) == sizeof(ModelOutputXYZ)*2);
-
-struct ModelOutputTemporalPose {
-  ModelOutputXYZ velocity_mean;
-  ModelOutputXYZ rotation_mean;
-  ModelOutputXYZ velocity_std;
-  ModelOutputXYZ rotation_std;
-};
-static_assert(sizeof(ModelOutputTemporalPose) == sizeof(ModelOutputXYZ)*4);
-
-struct ModelOutputRoadTransform {
-  ModelOutputXYZ position_mean;
-  ModelOutputXYZ rotation_mean;
-  ModelOutputXYZ position_std;
-  ModelOutputXYZ rotation_std;
-};
-static_assert(sizeof(ModelOutputRoadTransform) == sizeof(ModelOutputXYZ)*4);
-
-struct ModelOutputDisengageProb {
-  float gas_disengage;
-  float brake_disengage;
-  float steer_override;
-  float brake_3ms2;
-  float brake_4ms2;
-  float brake_5ms2;
-  float gas_pressed;
-};
-static_assert(sizeof(ModelOutputDisengageProb) == sizeof(float)*7);
-
-struct ModelOutputBlinkerProb {
-  float left;
-  float right;
-};
-static_assert(sizeof(ModelOutputBlinkerProb) == sizeof(float)*2);
-
-struct ModelOutputDesireProb {
-  union {
-    struct {
-      float none;
-      float turn_left;
-      float turn_right;
-      float lane_change_left;
-      float lane_change_right;
-      float keep_left;
-      float keep_right;
-      float null;
-    };
-    struct {
-      std::array<float, DESIRE_LEN> array;
-    };
-  };
-};
-static_assert(sizeof(ModelOutputDesireProb) == sizeof(float)*DESIRE_LEN);
-
-struct ModelOutputMeta {
-  ModelOutputDesireProb desire_state_prob;
-  float engaged_prob;
-  std::array<ModelOutputDisengageProb, DISENGAGE_LEN> disengage_prob;
-  std::array<ModelOutputBlinkerProb, BLINKER_LEN> blinker_prob;
-  std::array<ModelOutputDesireProb, DESIRE_PRED_LEN> desire_pred_prob;
-};
-static_assert(sizeof(ModelOutputMeta) == sizeof(ModelOutputDesireProb) + sizeof(float) + (sizeof(ModelOutputDisengageProb)*DISENGAGE_LEN) + (sizeof(ModelOutputBlinkerProb)*BLINKER_LEN) + (sizeof(ModelOutputDesireProb)*DESIRE_PRED_LEN));
-
-struct ModelOutputFeatures {
-  std::array<float, FEATURE_LEN> feature;
-};
-static_assert(sizeof(ModelOutputFeatures) == (sizeof(float)*FEATURE_LEN));
-
-struct ModelOutput {
-  const ModelOutputPlans plans;
-  const ModelOutputLaneLines lane_lines;
-  const ModelOutputRoadEdges road_edges;
-  const ModelOutputLeads leads;
-  const ModelOutputMeta meta;
-  const ModelOutputPose pose;
-  const ModelOutputWideFromDeviceEuler wide_from_device_euler;
-  const ModelOutputTemporalPose temporal_pose;
-  const ModelOutputRoadTransform road_transform;
-};
-
-constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float);
-
-#ifdef TEMPORAL
-  constexpr int TEMPORAL_SIZE = HISTORY_BUFFER_LEN * FEATURE_LEN;
-#else
-  constexpr int TEMPORAL_SIZE = 0;
-#endif
-constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + FEATURE_LEN + PAD_SIZE;
-
-// TODO: convert remaining arrays to std::array and update model runners
-struct ModelState {
-  ModelFrame *frame = nullptr;
-  ModelFrame *wide_frame = nullptr;
-  std::array<float, HISTORY_BUFFER_LEN * FEATURE_LEN> feature_buffer = {};
-  std::array<float, DISENGAGE_LEN * DISENGAGE_LEN> disengage_buffer = {};
-  std::array<float, NET_OUTPUT_SIZE> output = {};
-  std::unique_ptr<RunModel> m;
-#ifdef DESIRE
-  float prev_desire[DESIRE_LEN] = {};
-  float pulse_desire[DESIRE_LEN*(HISTORY_BUFFER_LEN+1)] = {};
-#endif
-#ifdef TRAFFIC_CONVENTION
-  float traffic_convention[TRAFFIC_CONVENTION_LEN] = {};
-#endif
-#ifdef DRIVING_STYLE
-  float driving_style[DRIVING_STYLE_LEN] = {};
-#endif
-#ifdef NAV
-  float nav_features[NAV_FEATURE_LEN] = {};
-#endif
-};
-
-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, bool is_rhd, float *driving_style, float *nav_features, bool prepare_only);
-void model_free(ModelState* s);
-void model_publish(ModelState* s, 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, uint64_t timestamp_llk,
-                   float model_execution_time, const bool nav_enabled, const bool valid);
-void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
-                     const ModelOutput &net_outputs, uint64_t timestamp_eof, const bool valid);
diff --git a/selfdrive/modeld/models/nav.cc b/selfdrive/modeld/models/nav.cc
deleted file mode 100644
index 8208b0bfef..0000000000
--- a/selfdrive/modeld/models/nav.cc
+++ /dev/null
@@ -1,71 +0,0 @@
-#include "selfdrive/modeld/models/nav.h"
-
-#include <cstdio>
-#include <cstring>
-
-#include "common/mat.h"
-#include "common/modeldata.h"
-#include "common/timing.h"
-
-
-void navmodel_init(NavModelState* s) {
-  #ifdef USE_ONNX_MODEL
-    s->m = new ONNXModel("models/navmodel.onnx", &s->output[0], NAV_NET_OUTPUT_SIZE, USE_DSP_RUNTIME, true);
-  #else
-    s->m = new SNPEModel("models/navmodel_q.dlc", &s->output[0], NAV_NET_OUTPUT_SIZE, USE_DSP_RUNTIME, true);
-  #endif
-
-  s->m->addInput("map", NULL, 0);
-}
-
-NavModelResult* navmodel_eval_frame(NavModelState* s, VisionBuf* buf) {
-  memcpy(s->net_input_buf, buf->addr, NAV_INPUT_SIZE);
-
-  double t1 = millis_since_boot();
-  s->m->setInputBuffer("map", (float*)s->net_input_buf, NAV_INPUT_SIZE/sizeof(float));
-  s->m->execute();
-  double t2 = millis_since_boot();
-
-  NavModelResult *model_res = (NavModelResult*)&s->output;
-  model_res->dsp_execution_time = (t2 - t1) / 1000.;
-  return model_res;
-}
-
-void fill_plan(cereal::NavModelData::Builder &framed, const NavModelOutputPlan &plan) {
-  std::array<float, TRAJECTORY_SIZE> pos_x, pos_y;
-  std::array<float, TRAJECTORY_SIZE> pos_x_std, pos_y_std;
-
-  for (int i=0; i<TRAJECTORY_SIZE; i++) {
-    pos_x[i] = plan.mean[i].x;
-    pos_y[i] = plan.mean[i].y;
-    pos_x_std[i] = exp(plan.std[i].x);
-    pos_y_std[i] = exp(plan.std[i].y);
-  }
-
-  auto position = framed.initPosition();
-  position.setX(to_kj_array_ptr(pos_x));
-  position.setY(to_kj_array_ptr(pos_y));
-  position.setXStd(to_kj_array_ptr(pos_x_std));
-  position.setYStd(to_kj_array_ptr(pos_y_std));
-}
-
-void navmodel_publish(PubMaster &pm, VisionIpcBufExtra &extra, const NavModelResult &model_res, float execution_time, bool route_valid) {
-  // make msg
-  MessageBuilder msg;
-  auto evt = msg.initEvent();
-  auto framed = evt.initNavModel();
-  evt.setValid(extra.valid && route_valid);
-  framed.setFrameId(extra.frame_id);
-  framed.setLocationMonoTime(extra.timestamp_sof);
-  framed.setModelExecutionTime(execution_time);
-  framed.setDspExecutionTime(model_res.dsp_execution_time);
-  framed.setFeatures(to_kj_array_ptr(model_res.features.values));
-  framed.setDesirePrediction(to_kj_array_ptr(model_res.desire_pred.values));
-  fill_plan(framed, model_res.plan);
-
-  pm.send("navModel", msg);
-}
-
-void navmodel_free(NavModelState* s) {
-  delete s->m;
-}
diff --git a/selfdrive/modeld/models/nav.h b/selfdrive/modeld/models/nav.h
deleted file mode 100644
index c6a517f558..0000000000
--- a/selfdrive/modeld/models/nav.h
+++ /dev/null
@@ -1,56 +0,0 @@
-#pragma once
-
-#include "cereal/messaging/messaging.h"
-#include "cereal/visionipc/visionipc_client.h"
-#include "common/util.h"
-#include "common/modeldata.h"
-#include "selfdrive/modeld/models/commonmodel.h"
-#include "selfdrive/modeld/runners/run.h"
-
-constexpr int NAV_INPUT_SIZE = 256*256;
-constexpr int NAV_FEATURE_LEN = 256;
-constexpr int NAV_DESIRE_LEN = 32;
-
-struct NavModelOutputXY {
-  float x;
-  float y;
-};
-static_assert(sizeof(NavModelOutputXY) == sizeof(float)*2);
-
-struct NavModelOutputPlan {
-  std::array<NavModelOutputXY, TRAJECTORY_SIZE> mean;
-  std::array<NavModelOutputXY, TRAJECTORY_SIZE> std;
-};
-static_assert(sizeof(NavModelOutputPlan) == sizeof(NavModelOutputXY)*TRAJECTORY_SIZE*2);
-
-struct NavModelOutputDesirePrediction {
-  std::array<float, NAV_DESIRE_LEN> values;
-};
-static_assert(sizeof(NavModelOutputDesirePrediction) == sizeof(float)*NAV_DESIRE_LEN);
-
-struct NavModelOutputFeatures {
-  std::array<float, NAV_FEATURE_LEN> values;
-};
-static_assert(sizeof(NavModelOutputFeatures) == sizeof(float)*NAV_FEATURE_LEN);
-
-struct NavModelResult {
-  const NavModelOutputPlan plan;
-  const NavModelOutputDesirePrediction desire_pred;
-  const NavModelOutputFeatures features;
-  float dsp_execution_time;
-};
-static_assert(sizeof(NavModelResult) == sizeof(NavModelOutputPlan) + sizeof(NavModelOutputDesirePrediction) + sizeof(NavModelOutputFeatures) + sizeof(float));
-
-constexpr int NAV_OUTPUT_SIZE = sizeof(NavModelResult) / sizeof(float);
-constexpr int NAV_NET_OUTPUT_SIZE = NAV_OUTPUT_SIZE - 1;
-
-struct NavModelState {
-  RunModel *m;
-  uint8_t net_input_buf[NAV_INPUT_SIZE];
-  float output[NAV_OUTPUT_SIZE];
-};
-
-void navmodel_init(NavModelState* s);
-NavModelResult* navmodel_eval_frame(NavModelState* s, VisionBuf* buf);
-void navmodel_publish(PubMaster &pm, VisionIpcBufExtra &extra, const NavModelResult &model_res, float execution_time, bool route_valid);
-void navmodel_free(NavModelState* s);
diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx
index 0116e1f469..181c264b01 100644
Binary files a/selfdrive/modeld/models/supercombo.onnx and b/selfdrive/modeld/models/supercombo.onnx differ
diff --git a/selfdrive/modeld/navmodeld b/selfdrive/modeld/navmodeld
deleted file mode 100755
index 079afd9677..0000000000
--- a/selfdrive/modeld/navmodeld
+++ /dev/null
@@ -1,12 +0,0 @@
-#!/bin/sh
-
-DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
-cd $DIR
-
-if [ -f /TICI ]; then
-  export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/third_party/snpe/larch64:$LD_LIBRARY_PATH"
-  export ADSP_LIBRARY_PATH="/data/pythonpath/third_party/snpe/dsp/"
-else
-  export LD_LIBRARY_PATH="$DIR/../../third_party/snpe/x86_64-linux-clang:$DIR/../../openpilot/third_party/snpe/x86_64:$LD_LIBRARY_PATH"
-fi
-exec ./_navmodeld
diff --git a/selfdrive/modeld/navmodeld.cc b/selfdrive/modeld/navmodeld.cc
deleted file mode 100644
index bd78e7a48b..0000000000
--- a/selfdrive/modeld/navmodeld.cc
+++ /dev/null
@@ -1,70 +0,0 @@
-#include <sys/resource.h>
-#include <limits.h>
-
-#include <cstdio>
-#include <cstdlib>
-
-#include "cereal/visionipc/visionipc_client.h"
-#include "common/params.h"
-#include "common/swaglog.h"
-#include "common/util.h"
-#include "selfdrive/modeld/models/nav.h"
-
-ExitHandler do_exit;
-
-void run_model(NavModelState &model, VisionIpcClient &vipc_client) {
-  SubMaster sm({"navInstruction"});
-  PubMaster pm({"navModel"});
-
-  double last_ts = 0;
-  uint32_t last_frame_id = 0;
-  VisionIpcBufExtra extra = {};
-
-  while (!do_exit) {
-    VisionBuf *buf = vipc_client.recv(&extra);
-    if (buf == nullptr) continue;
-
-    sm.update(0);
-
-    double t1 = millis_since_boot();
-    NavModelResult *model_res = navmodel_eval_frame(&model, buf);
-    double t2 = millis_since_boot();
-
-    // send navmodel packet
-    navmodel_publish(pm, extra, *model_res, (t2 - t1) / 1000.0, sm["navInstruction"].getValid());
-
-    //printf("navmodel process: %.2fms, from last %.2fms\n", t2 - t1, t1 - last_ts);
-    last_ts = t1;
-    last_frame_id = extra.frame_id;
-  }
-}
-
-int main(int argc, char **argv) {
-  setpriority(PRIO_PROCESS, 0, -15);
-
-  // there exists a race condition when two processes try to create a
-  // SNPE model runner at the same time, wait for dmonitoringmodeld to finish
-  LOGW("waiting for dmonitoringmodeld to initialize");
-  if (!Params().getBool("DmModelInitialized", true)) {
-    return 0;
-  }
-
-  // init the models
-  NavModelState model;
-  navmodel_init(&model);
-  LOGW("models loaded, navmodeld starting");
-
-  VisionIpcClient vipc_client = VisionIpcClient("navd", VISION_STREAM_MAP, true);
-  while (!do_exit && !vipc_client.connect(false)) {
-    util::sleep_for(100);
-  }
-
-  // run the models
-  if (vipc_client.connected) {
-    LOGW("connected with buffer size: %d", vipc_client.buffers[0].len);
-    run_model(model, vipc_client);
-  }
-
-  navmodel_free(&model);
-  return 0;
-}
diff --git a/selfdrive/modeld/navmodeld.py b/selfdrive/modeld/navmodeld.py
new file mode 100755
index 0000000000..90b9762800
--- /dev/null
+++ b/selfdrive/modeld/navmodeld.py
@@ -0,0 +1,118 @@
+#!/usr/bin/env python3
+import gc
+import math
+import time
+import ctypes
+import numpy as np
+from pathlib import Path
+from typing import Tuple, Dict
+
+from cereal import messaging
+from cereal.messaging import PubMaster, SubMaster
+from cereal.visionipc import VisionIpcClient, VisionStreamType
+from openpilot.system.swaglog import cloudlog
+from openpilot.common.params import Params
+from openpilot.common.realtime import set_realtime_priority
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
+
+NAV_INPUT_SIZE = 256*256
+NAV_FEATURE_LEN = 256
+NAV_DESIRE_LEN = 32
+NAV_OUTPUT_SIZE = 2*2*ModelConstants.IDX_N + NAV_DESIRE_LEN + NAV_FEATURE_LEN
+MODEL_PATHS = {
+  ModelRunner.SNPE: Path(__file__).parent / 'models/navmodel_q.dlc',
+  ModelRunner.ONNX: Path(__file__).parent / 'models/navmodel.onnx'}
+
+class NavModelOutputXY(ctypes.Structure):
+  _fields_ = [
+    ("x", ctypes.c_float),
+    ("y", ctypes.c_float)]
+
+class NavModelOutputPlan(ctypes.Structure):
+  _fields_ = [
+    ("mean", NavModelOutputXY*ModelConstants.IDX_N),
+    ("std", NavModelOutputXY*ModelConstants.IDX_N)]
+
+class NavModelResult(ctypes.Structure):
+  _fields_ = [
+    ("plan", NavModelOutputPlan),
+    ("desire_pred", ctypes.c_float*NAV_DESIRE_LEN),
+    ("features", ctypes.c_float*NAV_FEATURE_LEN)]
+
+class ModelState:
+  inputs: Dict[str, np.ndarray]
+  output: np.ndarray
+  model: ModelRunner
+
+  def __init__(self):
+    assert ctypes.sizeof(NavModelResult) == NAV_OUTPUT_SIZE * ctypes.sizeof(ctypes.c_float)
+    self.output = np.zeros(NAV_OUTPUT_SIZE, dtype=np.float32)
+    self.inputs = {'input_img': np.zeros(NAV_INPUT_SIZE, dtype=np.uint8)}
+    self.model = ModelRunner(MODEL_PATHS, self.output, Runtime.DSP, True, None)
+    self.model.addInput("input_img", None)
+
+  def run(self, buf:np.ndarray) -> Tuple[np.ndarray, float]:
+    self.inputs['input_img'][:] = buf
+
+    t1 = time.perf_counter()
+    self.model.setInputBuffer("input_img", self.inputs['input_img'].view(np.float32))
+    self.model.execute()
+    t2 = time.perf_counter()
+    return self.output, t2 - t1
+
+def get_navmodel_packet(model_output: np.ndarray, valid: bool, frame_id: int, location_ts: int, execution_time: float, dsp_execution_time: float):
+  model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(NavModelResult)).contents
+  msg = messaging.new_message('navModel')
+  msg.valid = valid
+  msg.navModel.frameId = frame_id
+  msg.navModel.locationMonoTime = location_ts
+  msg.navModel.modelExecutionTime = execution_time
+  msg.navModel.dspExecutionTime = dsp_execution_time
+  msg.navModel.features = model_result.features[:]
+  msg.navModel.desirePrediction = model_result.desire_pred[:]
+  msg.navModel.position.x = [p.x for p in model_result.plan.mean]
+  msg.navModel.position.y = [p.y for p in model_result.plan.mean]
+  msg.navModel.position.xStd = [math.exp(p.x) for p in model_result.plan.std]
+  msg.navModel.position.yStd = [math.exp(p.y) for p in model_result.plan.std]
+  return msg
+
+
+def main():
+  gc.disable()
+  set_realtime_priority(1)
+
+  # there exists a race condition when two processes try to create a
+  # SNPE model runner at the same time, wait for dmonitoringmodeld to finish
+  cloudlog.warning("waiting for dmonitoringmodeld to initialize")
+  if not Params().get_bool("DmModelInitialized", True):
+    return
+
+  model = ModelState()
+  cloudlog.warning("models loaded, navmodeld starting")
+
+  vipc_client = VisionIpcClient("navd", VisionStreamType.VISION_STREAM_MAP, True)
+  while not vipc_client.connect(False):
+    time.sleep(0.1)
+  assert vipc_client.is_connected()
+  cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}")
+
+  sm = SubMaster(["navInstruction"])
+  pm = PubMaster(["navModel"])
+
+  while True:
+    buf = vipc_client.recv()
+    if buf is None:
+      continue
+
+    sm.update(0)
+    t1 = time.perf_counter()
+    model_output, dsp_execution_time = model.run(buf.data[:buf.uv_offset])
+    t2 = time.perf_counter()
+
+    valid = vipc_client.valid and sm.valid["navInstruction"]
+    pm.send("navModel", get_navmodel_packet(model_output, valid, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, dsp_execution_time))
+
+
+if __name__ == "__main__":
+  main()
diff --git a/selfdrive/modeld/parse_model_outputs.py b/selfdrive/modeld/parse_model_outputs.py
new file mode 100644
index 0000000000..9d37a5fad4
--- /dev/null
+++ b/selfdrive/modeld/parse_model_outputs.py
@@ -0,0 +1,101 @@
+import numpy as np
+from typing import Dict
+from openpilot.selfdrive.modeld.constants import ModelConstants
+
+def sigmoid(x):
+  return 1. / (1. + np.exp(-x))
+
+def softmax(x, axis=-1):
+  x -= np.max(x, axis=axis, keepdims=True)
+  if x.dtype == np.float32 or x.dtype == np.float64:
+    np.exp(x, out=x)
+  else:
+    x = np.exp(x)
+  x /= np.sum(x, axis=axis, keepdims=True)
+  return x
+
+class Parser:
+  def __init__(self, ignore_missing=False):
+    self.ignore_missing = ignore_missing
+
+  def check_missing(self, outs, name):
+    if name not in outs and not self.ignore_missing:
+      raise ValueError(f"Missing output {name}")
+    return name not in outs
+
+  def parse_categorical_crossentropy(self, name, outs, out_shape=None):
+    if self.check_missing(outs, name):
+      return
+    raw = outs[name]
+    if out_shape is not None:
+      raw = raw.reshape((raw.shape[0],) + out_shape)
+    outs[name] = softmax(raw, axis=-1)
+
+  def parse_binary_crossentropy(self, name, outs):
+    if self.check_missing(outs, name):
+      return
+    raw = outs[name]
+    outs[name] = sigmoid(raw)
+
+  def parse_mdn(self, name, outs, in_N=0, out_N=1, out_shape=None):
+    if self.check_missing(outs, name):
+      return
+    raw = outs[name]
+    raw = raw.reshape((raw.shape[0], max(in_N, 1), -1))
+
+    pred_mu = raw[:,:,:(raw.shape[2] - out_N)//2]
+    n_values = (raw.shape[2] - out_N)//2
+    pred_mu = raw[:,:,:n_values]
+    pred_std = np.exp(raw[:,:,n_values: 2*n_values])
+
+    if in_N > 1:
+      weights = np.zeros((raw.shape[0], in_N, out_N), dtype=raw.dtype)
+      for i in range(out_N):
+        weights[:,:,i - out_N] = softmax(raw[:,:,i - out_N], axis=-1)
+
+      if out_N == 1:
+        for fidx in range(weights.shape[0]):
+          idxs = np.argsort(weights[fidx][:,0])[::-1]
+          weights[fidx] = weights[fidx][idxs]
+          pred_mu[fidx] = pred_mu[fidx][idxs]
+          pred_std[fidx] = pred_std[fidx][idxs]
+      full_shape = tuple([raw.shape[0], in_N] + list(out_shape))
+      outs[name + '_weights'] = weights
+      outs[name + '_hypotheses'] = pred_mu.reshape(full_shape)
+      outs[name + '_stds_hypotheses'] = pred_std.reshape(full_shape)
+
+      pred_mu_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
+      pred_std_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
+      for fidx in range(weights.shape[0]):
+        for hidx in range(out_N):
+          idxs = np.argsort(weights[fidx,:,hidx])[::-1]
+          pred_mu_final[fidx, hidx] = pred_mu[fidx, idxs[0]]
+          pred_std_final[fidx, hidx] = pred_std[fidx, idxs[0]]
+    else:
+      pred_mu_final = pred_mu
+      pred_std_final = pred_std
+
+    if out_N > 1:
+      final_shape = tuple([raw.shape[0], out_N] + list(out_shape))
+    else:
+      final_shape = tuple([raw.shape[0],] + list(out_shape))
+    outs[name] = pred_mu_final.reshape(final_shape)
+    outs[name + '_stds'] = pred_std_final.reshape(final_shape)
+
+  def parse_outputs(self, outs: Dict[str, np.ndarray]) -> Dict[str, np.ndarray]:
+    self.parse_mdn('plan', outs, in_N=ModelConstants.PLAN_MHP_N, out_N=ModelConstants.PLAN_MHP_SELECTION,
+                   out_shape=(ModelConstants.IDX_N,ModelConstants.PLAN_WIDTH))
+    self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
+    self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
+    self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
+    self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
+    self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
+    self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
+    self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
+                   out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
+    self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N,ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
+    for k in ['lead_prob', 'lane_lines_prob', 'meta']:
+      self.parse_binary_crossentropy(k, outs)
+    self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
+    self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH))
+    return outs
diff --git a/selfdrive/modeld/runners/__init__.py b/selfdrive/modeld/runners/__init__.py
new file mode 100644
index 0000000000..4c29bf3f1c
--- /dev/null
+++ b/selfdrive/modeld/runners/__init__.py
@@ -0,0 +1,27 @@
+import os
+from openpilot.system.hardware import TICI
+from openpilot.selfdrive.modeld.runners.runmodel_pyx import RunModel, Runtime
+assert Runtime
+
+USE_THNEED = int(os.getenv('USE_THNEED', str(int(TICI))))
+USE_SNPE = int(os.getenv('USE_SNPE', str(int(TICI))))
+
+class ModelRunner(RunModel):
+  THNEED = 'THNEED'
+  SNPE = 'SNPE'
+  ONNX = 'ONNX'
+
+  def __new__(cls, paths, *args, **kwargs):
+    if ModelRunner.THNEED in paths and USE_THNEED:
+      from openpilot.selfdrive.modeld.runners.thneedmodel_pyx import ThneedModel as Runner
+      runner_type = ModelRunner.THNEED
+    elif ModelRunner.SNPE in paths and USE_SNPE:
+      from openpilot.selfdrive.modeld.runners.snpemodel_pyx import SNPEModel as Runner
+      runner_type = ModelRunner.SNPE
+    elif ModelRunner.ONNX in paths:
+      from openpilot.selfdrive.modeld.runners.onnxmodel import ONNXModel as Runner
+      runner_type = ModelRunner.ONNX
+    else:
+      raise Exception("Couldn't select a model runner, make sure to pass at least one valid model path")
+
+    return Runner(str(paths[runner_type]), *args, **kwargs)
diff --git a/selfdrive/modeld/runners/onnx_runner.py b/selfdrive/modeld/runners/onnx_runner.py
deleted file mode 100755
index ee51e489ed..0000000000
--- a/selfdrive/modeld/runners/onnx_runner.py
+++ /dev/null
@@ -1,78 +0,0 @@
-#!/usr/bin/env python3
-
-import os
-import sys
-import numpy as np
-from typing import Tuple, Dict, Union, Any
-
-os.environ["OMP_NUM_THREADS"] = "4"
-os.environ["OMP_WAIT_POLICY"] = "PASSIVE"
-
-import onnxruntime as ort # pylint: disable=import-error
-
-ORT_TYPES_TO_NP_TYPES = {'tensor(float16)': np.float16, 'tensor(float)': np.float32, 'tensor(uint8)': np.uint8}
-
-def read(sz, tf8=False):
-  dd = []
-  gt = 0
-  szof = 1 if tf8 else 4
-  while gt < sz * szof:
-    st = os.read(0, sz * szof - gt)
-    assert(len(st) > 0)
-    dd.append(st)
-    gt += len(st)
-  r = np.frombuffer(b''.join(dd), dtype=np.uint8 if tf8 else np.float32)
-  if tf8:
-    r = r / 255.
-  return r
-
-def write(d):
-  os.write(1, d.tobytes())
-
-def run_loop(m, tf8_input=False):
-  ishapes = [[1]+ii.shape[1:] for ii in m.get_inputs()]
-  keys = [x.name for x in m.get_inputs()]
-  itypes = [ORT_TYPES_TO_NP_TYPES[x.type] for x in m.get_inputs()]
-
-  # run once to initialize CUDA provider
-  if "CUDAExecutionProvider" in m.get_providers():
-    m.run(None, dict(zip(keys, [np.zeros(shp, dtype=itp) for shp, itp in zip(ishapes, itypes)])))
-
-  print("ready to run onnx model", keys, ishapes, file=sys.stderr)
-  while 1:
-    inputs = []
-    for k, shp, itp in zip(keys, ishapes, itypes):
-      ts = np.product(shp)
-      #print("reshaping %s with offset %d" % (str(shp), offset), file=sys.stderr)
-      inputs.append(read(ts, (k=='input_img' and tf8_input)).reshape(shp).astype(itp))
-    ret = m.run(None, dict(zip(keys, inputs)))
-    #print(ret, file=sys.stderr)
-    for r in ret:
-      write(r.astype(np.float32))
-
-
-if __name__ == "__main__":
-  print(sys.argv, file=sys.stderr)
-  print("Onnx available providers: ", ort.get_available_providers(), file=sys.stderr)
-  options = ort.SessionOptions()
-  options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_DISABLE_ALL
-
-  provider: Union[str, Tuple[str, Dict[Any, Any]]]
-  if 'OpenVINOExecutionProvider' in ort.get_available_providers() and 'ONNXCPU' not in os.environ:
-    provider = 'OpenVINOExecutionProvider'
-  elif 'CUDAExecutionProvider' in ort.get_available_providers() and 'ONNXCPU' not in os.environ:
-    options.intra_op_num_threads = 2
-    provider = ('CUDAExecutionProvider', {'cudnn_conv_algo_search': 'DEFAULT'})
-  else:
-    options.intra_op_num_threads = 2
-    options.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL
-    options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL
-    provider = 'CPUExecutionProvider'
-
-  try:
-    print("Onnx selected provider: ", [provider], file=sys.stderr)
-    ort_session = ort.InferenceSession(sys.argv[1], options, providers=[provider])
-    print("Onnx using ", ort_session.get_providers(), file=sys.stderr)
-    run_loop(ort_session, tf8_input=("--use_tf8" in sys.argv))
-  except KeyboardInterrupt:
-    pass
diff --git a/selfdrive/modeld/runners/onnxmodel.cc b/selfdrive/modeld/runners/onnxmodel.cc
deleted file mode 100644
index e9a5d09406..0000000000
--- a/selfdrive/modeld/runners/onnxmodel.cc
+++ /dev/null
@@ -1,88 +0,0 @@
-#include "selfdrive/modeld/runners/onnxmodel.h"
-
-#include <csignal>
-#include <cstdio>
-#include <cstdlib>
-#include <poll.h>
-#include <unistd.h>
-
-#include "common/util.h"
-
-ONNXModel::ONNXModel(const std::string path, float *_output, size_t _output_size, int runtime, bool _use_tf8, cl_context context) {
-  LOGD("loading model %s", path.c_str());
-
-  output = _output;
-  output_size = _output_size;
-  use_tf8 = _use_tf8;
-
-  int err = pipe(pipein);
-  assert(err == 0);
-  err = pipe(pipeout);
-  assert(err == 0);
-
-  std::string exe_dir = util::dir_name(util::readlink("/proc/self/exe"));
-  std::string onnx_runner = exe_dir + "/runners/onnx_runner.py";
-  std::string tf8_arg = use_tf8 ? "--use_tf8" : "";
-
-  proc_pid = fork();
-  if (proc_pid == 0) {
-    LOGD("spawning onnx process %s", onnx_runner.c_str());
-    char *argv[] = {(char*)onnx_runner.c_str(), (char*)path.c_str(), (char*)tf8_arg.c_str(), nullptr};
-    dup2(pipein[0], 0);
-    dup2(pipeout[1], 1);
-    close(pipein[0]);
-    close(pipein[1]);
-    close(pipeout[0]);
-    close(pipeout[1]);
-    execvp(onnx_runner.c_str(), argv);
-  }
-
-  // parent
-  close(pipein[0]);
-  close(pipeout[1]);
-}
-
-ONNXModel::~ONNXModel() {
-  close(pipein[1]);
-  close(pipeout[0]);
-  kill(proc_pid, SIGTERM);
-}
-
-void ONNXModel::pwrite(float *buf, int size) {
-  char *cbuf = (char *)buf;
-  int tw = size*sizeof(float);
-  while (tw > 0) {
-    int err = write(pipein[1], cbuf, tw);
-    //printf("host write %d\n", err);
-    assert(err >= 0);
-    cbuf += err;
-    tw -= err;
-  }
-  LOGD("host write of size %d done", size);
-}
-
-void ONNXModel::pread(float *buf, int size) {
-  char *cbuf = (char *)buf;
-  int tr = size*sizeof(float);
-  struct pollfd fds[1];
-  fds[0].fd = pipeout[0];
-  fds[0].events = POLLIN;
-  while (tr > 0) {
-    int err;
-    err = poll(fds, 1, 10000);  // 10 second timeout
-    assert(err == 1 || (err == -1 && errno == EINTR));
-    LOGD("host read remaining %d/%d poll %d", tr, size*sizeof(float), err);
-    err = read(pipeout[0], cbuf, tr);
-    assert(err > 0 || (err == 0 && errno == EINTR));
-    cbuf += err;
-    tr -= err;
-  }
-  LOGD("host read done");
-}
-
-void ONNXModel::execute() {
-  for (auto &input : inputs) {
-    pwrite(input->buffer, input->size);
-  }
-  pread(output, output_size);
-}
diff --git a/selfdrive/modeld/runners/onnxmodel.h b/selfdrive/modeld/runners/onnxmodel.h
deleted file mode 100644
index 6c325f644e..0000000000
--- a/selfdrive/modeld/runners/onnxmodel.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#pragma once
-
-#include "selfdrive/modeld/runners/runmodel.h"
-
-class ONNXModel : public RunModel {
-public:
-  ONNXModel(const std::string path, float *output, size_t output_size, int runtime, bool _use_tf8 = false, cl_context context = NULL);
-	~ONNXModel();
-  void execute();
-private:
-  int proc_pid;
-  float *output;
-  size_t output_size;
-  bool use_tf8;
-
-  // pipe to communicate to onnx_runner subprocess
-  void pread(float *buf, int size);
-  void pwrite(float *buf, int size);
-  int pipein[2];
-  int pipeout[2];
-};
diff --git a/selfdrive/modeld/runners/onnxmodel.py b/selfdrive/modeld/runners/onnxmodel.py
new file mode 100644
index 0000000000..f86bee3878
--- /dev/null
+++ b/selfdrive/modeld/runners/onnxmodel.py
@@ -0,0 +1,93 @@
+import onnx
+import itertools
+import os
+import sys
+import numpy as np
+from typing import Tuple, Dict, Union, Any
+
+from openpilot.selfdrive.modeld.runners.runmodel_pyx import RunModel
+
+ORT_TYPES_TO_NP_TYPES = {'tensor(float16)': np.float16, 'tensor(float)': np.float32, 'tensor(uint8)': np.uint8}
+
+def attributeproto_fp16_to_fp32(attr):
+  float32_list = np.frombuffer(attr.raw_data, dtype=np.float16)
+  attr.data_type = 1
+  attr.raw_data = float32_list.astype(np.float32).tobytes()
+
+def convert_fp16_to_fp32(path):
+  model = onnx.load(path)
+  for i in model.graph.initializer:
+    if i.data_type == 10:
+      attributeproto_fp16_to_fp32(i)
+  for i in itertools.chain(model.graph.input, model.graph.output):
+    if i.type.tensor_type.elem_type == 10:
+      i.type.tensor_type.elem_type = 1
+  for i in model.graph.node:
+    for a in i.attribute:
+      if hasattr(a, 't'):
+        if a.t.data_type == 10:
+          attributeproto_fp16_to_fp32(a.t)
+  return model.SerializeToString()
+
+def create_ort_session(path, fp16_to_fp32):
+  os.environ["OMP_NUM_THREADS"] = "4"
+  os.environ["OMP_WAIT_POLICY"] = "PASSIVE"
+
+  import onnxruntime as ort
+  print("Onnx available providers: ", ort.get_available_providers(), file=sys.stderr)
+  options = ort.SessionOptions()
+  options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_DISABLE_ALL
+
+  provider: Union[str, Tuple[str, Dict[Any, Any]]]
+  if 'OpenVINOExecutionProvider' in ort.get_available_providers() and 'ONNXCPU' not in os.environ:
+    provider = 'OpenVINOExecutionProvider'
+  elif 'CUDAExecutionProvider' in ort.get_available_providers() and 'ONNXCPU' not in os.environ:
+    options.intra_op_num_threads = 2
+    provider = ('CUDAExecutionProvider', {'cudnn_conv_algo_search': 'DEFAULT'})
+  else:
+    options.intra_op_num_threads = 2
+    options.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL
+    options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL
+    provider = 'CPUExecutionProvider'
+
+  model_data = convert_fp16_to_fp32(path) if fp16_to_fp32 else path
+  print("Onnx selected provider: ", [provider], file=sys.stderr)
+  ort_session = ort.InferenceSession(model_data, options, providers=[provider])
+  print("Onnx using ", ort_session.get_providers(), file=sys.stderr)
+  return ort_session
+
+
+class ONNXModel(RunModel):
+  def __init__(self, path, output, runtime, use_tf8, cl_context):
+    self.inputs = {}
+    self.output = output
+    self.use_tf8 = use_tf8
+
+    self.session = create_ort_session(path, fp16_to_fp32=True)
+    self.input_names = [x.name for x in self.session.get_inputs()]
+    self.input_shapes = {x.name: [1, *x.shape[1:]] for x in self.session.get_inputs()}
+    self.input_dtypes = {x.name: ORT_TYPES_TO_NP_TYPES[x.type] for x in self.session.get_inputs()}
+
+    # run once to initialize CUDA provider
+    if "CUDAExecutionProvider" in self.session.get_providers():
+      self.session.run(None, {k: np.zeros(self.input_shapes[k], dtype=self.input_dtypes[k]) for k in self.input_names})
+    print("ready to run onnx model", self.input_shapes, file=sys.stderr)
+
+  def addInput(self, name, buffer):
+    assert name in self.input_names
+    self.inputs[name] = buffer
+
+  def setInputBuffer(self, name, buffer):
+    assert name in self.inputs
+    self.inputs[name] = buffer
+
+  def getCLBuffer(self, name):
+    return None
+
+  def execute(self):
+    inputs = {k: (v.view(np.uint8) / 255. if self.use_tf8 and k == 'input_img' else v) for k,v in self.inputs.items()}
+    inputs = {k: v.reshape(self.input_shapes[k]).astype(self.input_dtypes[k]) for k,v in inputs.items()}
+    outputs = self.session.run(None, inputs)
+    assert len(outputs) == 1, "Only single model outputs are supported"
+    self.output[:] = outputs[0]
+    return self.output
diff --git a/selfdrive/modeld/runners/run.h b/selfdrive/modeld/runners/run.h
index c64f300fe2..36ad262a5b 100644
--- a/selfdrive/modeld/runners/run.h
+++ b/selfdrive/modeld/runners/run.h
@@ -1,10 +1,4 @@
 #pragma once
 
-#include "runmodel.h"
-#include "snpemodel.h"
-
-#if defined(USE_THNEED)
-#include "thneedmodel.h"
-#elif defined(USE_ONNX_MODEL)
-#include "onnxmodel.h"
-#endif
+#include "selfdrive/modeld/runners/runmodel.h"
+#include "selfdrive/modeld/runners/snpemodel.h"
diff --git a/selfdrive/modeld/runners/runmodel.h b/selfdrive/modeld/runners/runmodel.h
index 00c88131bf..18cc180cb7 100644
--- a/selfdrive/modeld/runners/runmodel.h
+++ b/selfdrive/modeld/runners/runmodel.h
@@ -8,6 +8,10 @@
 #include "common/clutil.h"
 #include "common/swaglog.h"
 
+#define USE_CPU_RUNTIME 0
+#define USE_GPU_RUNTIME 1
+#define USE_DSP_RUNTIME 2
+
 struct ModelInput {
   const std::string name;
   float *buffer;
diff --git a/selfdrive/modeld/runners/runmodel.pxd b/selfdrive/modeld/runners/runmodel.pxd
new file mode 100644
index 0000000000..01b2a9cf2c
--- /dev/null
+++ b/selfdrive/modeld/runners/runmodel.pxd
@@ -0,0 +1,14 @@
+# distutils: language = c++
+
+from libcpp.string cimport string
+
+cdef extern from "selfdrive/modeld/runners/runmodel.h":
+  cdef int USE_CPU_RUNTIME
+  cdef int USE_GPU_RUNTIME
+  cdef int USE_DSP_RUNTIME
+
+  cdef cppclass RunModel:
+    void addInput(string, float*, int)
+    void setInputBuffer(string, float*, int)
+    void * getCLBuffer(string)
+    void execute()
diff --git a/selfdrive/modeld/runners/runmodel_pyx.pxd b/selfdrive/modeld/runners/runmodel_pyx.pxd
new file mode 100644
index 0000000000..b6ede7cf37
--- /dev/null
+++ b/selfdrive/modeld/runners/runmodel_pyx.pxd
@@ -0,0 +1,6 @@
+# distutils: language = c++
+
+from .runmodel cimport RunModel as cppRunModel
+
+cdef class RunModel:
+  cdef cppRunModel * model
diff --git a/selfdrive/modeld/runners/runmodel_pyx.pyx b/selfdrive/modeld/runners/runmodel_pyx.pyx
new file mode 100644
index 0000000000..cdc62a79be
--- /dev/null
+++ b/selfdrive/modeld/runners/runmodel_pyx.pyx
@@ -0,0 +1,38 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii
+
+from libcpp.string cimport string
+from libc.string cimport memcpy
+
+from .runmodel cimport USE_CPU_RUNTIME, USE_GPU_RUNTIME, USE_DSP_RUNTIME
+from selfdrive.modeld.models.commonmodel_pyx cimport CLMem
+
+class Runtime:
+  CPU = USE_CPU_RUNTIME
+  GPU = USE_GPU_RUNTIME
+  DSP = USE_DSP_RUNTIME
+
+cdef class RunModel:
+  def __dealloc__(self):
+    del self.model
+
+  def addInput(self, string name, float[:] buffer):
+    if buffer is not None:
+      self.model.addInput(name, &buffer[0], len(buffer))
+    else:
+      self.model.addInput(name, NULL, 0)
+
+  def setInputBuffer(self, string name, float[:] buffer):
+    if buffer is not None:
+      self.model.setInputBuffer(name, &buffer[0], len(buffer))
+    else:
+      self.model.setInputBuffer(name, NULL, 0)
+
+  def getCLBuffer(self, string name):
+    cdef void * cl_buf = self.model.getCLBuffer(name)
+    if not cl_buf:
+      return None
+    return CLMem.create(cl_buf)
+
+  def execute(self):
+    self.model.execute()
diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc
index 441122c522..15c1db0086 100644
--- a/selfdrive/modeld/runners/snpemodel.cc
+++ b/selfdrive/modeld/runners/snpemodel.cc
@@ -3,6 +3,10 @@
 #include "selfdrive/modeld/runners/snpemodel.h"
 
 #include <cstring>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
 
 #include "common/util.h"
 #include "common/timing.h"
@@ -71,12 +75,6 @@ SNPEModel::SNPEModel(const std::string path, float *_output, size_t _output_size
   std::vector<size_t> output_strides = {output_size * sizeof(float), sizeof(float)};
   output_buffer = ub_factory.createUserBuffer(output, output_size * sizeof(float), output_strides, &ub_encoding_float);
   output_map.add(output_tensor_name, output_buffer.get());
-
-#ifdef USE_THNEED
-  if (snpe_runtime == zdl::DlSystem::Runtime_t::GPU) {
-    thneed.reset(new Thneed());
-  }
-#endif
 }
 
 void SNPEModel::addInput(const std::string name, float *buffer, int size) {
diff --git a/selfdrive/modeld/runners/snpemodel.h b/selfdrive/modeld/runners/snpemodel.h
index e646e5225b..86b2c86084 100644
--- a/selfdrive/modeld/runners/snpemodel.h
+++ b/selfdrive/modeld/runners/snpemodel.h
@@ -1,6 +1,10 @@
 #pragma once
 #pragma clang diagnostic ignored "-Wdeprecated-declarations"
 
+#include <memory>
+#include <string>
+#include <utility>
+
 #include <DlContainer/IDlContainer.hpp>
 #include <DlSystem/DlError.hpp>
 #include <DlSystem/ITensor.hpp>
@@ -13,14 +17,6 @@
 
 #include "selfdrive/modeld/runners/runmodel.h"
 
-#define USE_CPU_RUNTIME 0
-#define USE_GPU_RUNTIME 1
-#define USE_DSP_RUNTIME 2
-
-#ifdef USE_THNEED
-#include "selfdrive/modeld/thneed/thneed.h"
-#endif
-
 struct SNPEModelInput : public ModelInput {
   std::unique_ptr<zdl::DlSystem::IUserBuffer> snpe_buffer;
 
@@ -37,11 +33,6 @@ public:
   void addInput(const std::string name, float *buffer, int size);
   void execute();
 
-#ifdef USE_THNEED
-  std::unique_ptr<Thneed> thneed;
-  bool thneed_recorded = false;
-#endif
-
 private:
   std::string model_data;
 
diff --git a/selfdrive/modeld/runners/snpemodel.pxd b/selfdrive/modeld/runners/snpemodel.pxd
new file mode 100644
index 0000000000..1f928da332
--- /dev/null
+++ b/selfdrive/modeld/runners/snpemodel.pxd
@@ -0,0 +1,9 @@
+# distutils: language = c++
+
+from libcpp.string cimport string
+
+from cereal.visionipc.visionipc cimport cl_context
+
+cdef extern from "selfdrive/modeld/runners/snpemodel.h":
+  cdef cppclass SNPEModel:
+    SNPEModel(string, float*, size_t, int, bool, cl_context)
diff --git a/selfdrive/modeld/runners/snpemodel_pyx.pyx b/selfdrive/modeld/runners/snpemodel_pyx.pyx
new file mode 100644
index 0000000000..c3b2b7e9bd
--- /dev/null
+++ b/selfdrive/modeld/runners/snpemodel_pyx.pyx
@@ -0,0 +1,17 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii
+
+import os
+from libcpp cimport bool
+from libcpp.string cimport string
+
+from .snpemodel cimport SNPEModel as cppSNPEModel
+from selfdrive.modeld.models.commonmodel_pyx cimport CLContext
+from selfdrive.modeld.runners.runmodel_pyx cimport RunModel
+from selfdrive.modeld.runners.runmodel cimport RunModel as cppRunModel
+
+os.environ['ADSP_LIBRARY_PATH'] = "/data/pythonpath/third_party/snpe/dsp/"
+
+cdef class SNPEModel(RunModel):
+  def __cinit__(self, string path, float[:] output, int runtime, bool use_tf8, CLContext context):
+    self.model = <cppRunModel *> new cppSNPEModel(path, &output[0], len(output), runtime, use_tf8, context.context)
diff --git a/selfdrive/modeld/runners/thneedmodel.cc b/selfdrive/modeld/runners/thneedmodel.cc
index 0f35c94800..a16d8b42aa 100644
--- a/selfdrive/modeld/runners/thneedmodel.cc
+++ b/selfdrive/modeld/runners/thneedmodel.cc
@@ -1,5 +1,7 @@
 #include "selfdrive/modeld/runners/thneedmodel.h"
 
+#include <string>
+
 #include "common/swaglog.h"
 
 ThneedModel::ThneedModel(const std::string path, float *_output, size_t _output_size, int runtime, bool luse_tf8, cl_context context) {
diff --git a/selfdrive/modeld/runners/thneedmodel.h b/selfdrive/modeld/runners/thneedmodel.h
index 90c40239bf..6ed479c081 100644
--- a/selfdrive/modeld/runners/thneedmodel.h
+++ b/selfdrive/modeld/runners/thneedmodel.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <string>
+
 #include "selfdrive/modeld/runners/runmodel.h"
 #include "selfdrive/modeld/thneed/thneed.h"
 
diff --git a/selfdrive/modeld/runners/thneedmodel.pxd b/selfdrive/modeld/runners/thneedmodel.pxd
new file mode 100644
index 0000000000..90af972865
--- /dev/null
+++ b/selfdrive/modeld/runners/thneedmodel.pxd
@@ -0,0 +1,9 @@
+# distutils: language = c++
+
+from libcpp.string cimport string
+
+from cereal.visionipc.visionipc cimport cl_context
+
+cdef extern from "selfdrive/modeld/runners/thneedmodel.h":
+  cdef cppclass ThneedModel:
+    ThneedModel(string, float*, size_t, int, bool, cl_context)
diff --git a/selfdrive/modeld/runners/thneedmodel_pyx.pyx b/selfdrive/modeld/runners/thneedmodel_pyx.pyx
new file mode 100644
index 0000000000..53487afa1b
--- /dev/null
+++ b/selfdrive/modeld/runners/thneedmodel_pyx.pyx
@@ -0,0 +1,14 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii
+
+from libcpp cimport bool
+from libcpp.string cimport string
+
+from .thneedmodel cimport ThneedModel as cppThneedModel
+from selfdrive.modeld.models.commonmodel_pyx cimport CLContext
+from selfdrive.modeld.runners.runmodel_pyx cimport RunModel
+from selfdrive.modeld.runners.runmodel cimport RunModel as cppRunModel
+
+cdef class ThneedModel(RunModel):
+  def __cinit__(self, string path, float[:] output, int runtime, bool use_tf8, CLContext context):
+    self.model = <cppRunModel *> new cppThneedModel(path, &output[0], len(output), runtime, use_tf8, context.context)
diff --git a/selfdrive/modeld/thneed/lib.py b/selfdrive/modeld/thneed/lib.py
deleted file mode 100644
index 38ef3f4262..0000000000
--- a/selfdrive/modeld/thneed/lib.py
+++ /dev/null
@@ -1,31 +0,0 @@
-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_bytes = 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_bytes)
diff --git a/selfdrive/modeld/thneed/serialize.cc b/selfdrive/modeld/thneed/serialize.cc
index f789e5bf57..6ed5c08e81 100644
--- a/selfdrive/modeld/thneed/serialize.cc
+++ b/selfdrive/modeld/thneed/serialize.cc
@@ -1,7 +1,7 @@
 #include <cassert>
 #include <set>
 
-#include "json11.hpp"
+#include "third_party/json11/json11.hpp"
 #include "common/util.h"
 #include "common/clutil.h"
 #include "selfdrive/modeld/thneed/thneed.h"
@@ -75,8 +75,7 @@ void Thneed::load(const char *filename) {
 #endif
       if (clbuf == NULL) {
         printf("clError: %s create image %zux%zu rp %zu with buffer %p\n", cl_get_error_string(errcode),
-          desc.image_width, desc.image_height, desc.image_row_pitch, desc.buffer
-        );
+          desc.image_width, desc.image_height, desc.image_row_pitch, desc.buffer);
       }
       assert(clbuf != NULL);
     }
diff --git a/selfdrive/modeld/thneed/thneed.h b/selfdrive/modeld/thneed/thneed.h
index 6475577734..47e18e0be3 100644
--- a/selfdrive/modeld/thneed/thneed.h
+++ b/selfdrive/modeld/thneed/thneed.h
@@ -12,7 +12,7 @@
 
 #include <CL/cl.h>
 
-#include "msm_kgsl.h"
+#include "third_party/linux/include/msm_kgsl.h"
 
 using namespace std;
 
diff --git a/selfdrive/modeld/thneed/thneed_qcom2.cc b/selfdrive/modeld/thneed/thneed_qcom2.cc
index a3bfb8a8c2..21de15d17c 100644
--- a/selfdrive/modeld/thneed/thneed_qcom2.cc
+++ b/selfdrive/modeld/thneed/thneed_qcom2.cc
@@ -107,7 +107,8 @@ int ioctl(int filedes, unsigned long request, void *argp) {
   }
 
   int ret = my_ioctl(filedes, request, argp);
-  if (ret != 0) printf("ioctl returned %d with errno %d\n", ret, errno);
+  // NOTE: This error message goes into stdout and messes up pyenv
+  // if (ret != 0) printf("ioctl returned %d with errno %d\n", ret, errno);
   return ret;
 }
 
diff --git a/selfdrive/modeld/transforms/loadyuv.cc b/selfdrive/modeld/transforms/loadyuv.cc
index 39f404a897..c7ce7b0830 100644
--- a/selfdrive/modeld/transforms/loadyuv.cc
+++ b/selfdrive/modeld/transforms/loadyuv.cc
@@ -15,7 +15,7 @@ void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int w
            "-cl-fast-relaxed-math -cl-denorms-are-zero "
            "-DTRANSFORMED_WIDTH=%d -DTRANSFORMED_HEIGHT=%d",
            width, height);
-  cl_program prg = cl_program_from_file(ctx, device_id, "transforms/loadyuv.cl", args);
+  cl_program prg = cl_program_from_file(ctx, device_id, LOADYUV_PATH, args);
 
   s->loadys_krnl = CL_CHECK_ERR(clCreateKernel(prg, "loadys", &err));
   s->loaduv_krnl = CL_CHECK_ERR(clCreateKernel(prg, "loaduv", &err));
diff --git a/selfdrive/modeld/transforms/transform.cc b/selfdrive/modeld/transforms/transform.cc
index f341314144..305643cf42 100644
--- a/selfdrive/modeld/transforms/transform.cc
+++ b/selfdrive/modeld/transforms/transform.cc
@@ -8,7 +8,7 @@
 void transform_init(Transform* s, cl_context ctx, cl_device_id device_id) {
   memset(s, 0, sizeof(*s));
 
-  cl_program prg = cl_program_from_file(ctx, device_id, "transforms/transform.cl", "");
+  cl_program prg = cl_program_from_file(ctx, device_id, TRANSFORM_PATH, "");
   s->krnl = CL_CHECK_ERR(clCreateKernel(prg, "warpPerspective", &err));
   // done with this
   CL_CHECK(clReleaseProgram(prg));
diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py
index 836ed9cc4f..ff2ee71e74 100755
--- a/selfdrive/monitoring/dmonitoringd.py
+++ b/selfdrive/monitoring/dmonitoringd.py
@@ -4,21 +4,18 @@ import gc
 import cereal.messaging as messaging
 from cereal import car
 from cereal import log
-from common.params import Params, put_bool_nonblocking
-from common.realtime import set_realtime_priority
-from selfdrive.controls.lib.events import Events
-from selfdrive.monitoring.driver_monitor import DriverStatus
+from openpilot.common.params import Params, put_bool_nonblocking
+from openpilot.common.realtime import set_realtime_priority
+from openpilot.selfdrive.controls.lib.events import Events
+from openpilot.selfdrive.monitoring.driver_monitor import DriverStatus
 
 
-def dmonitoringd_thread(sm=None, pm=None):
+def dmonitoringd_thread():
   gc.disable()
   set_realtime_priority(2)
 
-  if pm is None:
-    pm = messaging.PubMaster(['driverMonitoringState'])
-
-  if sm is None:
-    sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverStateV2'])
+  pm = messaging.PubMaster(['driverMonitoringState'])
+  sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverStateV2'])
 
   driver_status = DriverStatus(rhd_saved=Params().get_bool("IsRhdDetected"))
 
@@ -89,8 +86,8 @@ def dmonitoringd_thread(sm=None, pm=None):
      driver_status.wheel_on_right == (driver_status.wheelpos_learner.filtered_stat.M > driver_status.settings._WHEELPOS_THRESHOLD)):
       put_bool_nonblocking("IsRhdDetected", driver_status.wheel_on_right)
 
-def main(sm=None, pm=None):
-  dmonitoringd_thread(sm, pm)
+def main():
+  dmonitoringd_thread()
 
 
 if __name__ == '__main__':
diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py
index a2cddc2462..ab82da301f 100644
--- a/selfdrive/monitoring/driver_monitor.py
+++ b/selfdrive/monitoring/driver_monitor.py
@@ -1,11 +1,11 @@
 from math import atan2
 
 from cereal import car
-from common.numpy_fast import interp
-from common.realtime import DT_DMON
-from common.filter_simple import FirstOrderFilter
-from common.stat_live import RunningStatFilter
-from common.transformations.camera import tici_d_frame_size
+from openpilot.common.numpy_fast import interp
+from openpilot.common.realtime import DT_DMON
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.stat_live import RunningStatFilter
+from openpilot.common.transformations.camera import tici_d_frame_size
 
 EventName = car.CarEvent.EventName
 
@@ -119,7 +119,9 @@ class DriverBlink():
     self.right_blink = 0.
 
 class DriverStatus():
-  def __init__(self, rhd_saved=False, settings=DRIVER_MONITOR_SETTINGS()):
+  def __init__(self, rhd_saved=False, settings=None):
+    if settings is None:
+      settings = DRIVER_MONITOR_SETTINGS()
     # init policy settings
     self.settings = settings
 
@@ -212,7 +214,8 @@ class DriverStatus():
       distracted_types.append(DistractedType.DISTRACTED_BLINK)
 
     if self.ee1_calibrated:
-      ee1_dist = self.eev1 > max(min(self.ee1_offseter.filtered_stat.M, self.settings._EE_MAX_OFFSET1), self.settings._EE_MIN_OFFSET1) * self.settings._EE_THRESH12
+      ee1_dist = self.eev1 > max(min(self.ee1_offseter.filtered_stat.M, self.settings._EE_MAX_OFFSET1), self.settings._EE_MIN_OFFSET1) \
+                              * self.settings._EE_THRESH12
     else:
       ee1_dist = self.eev1 > self.settings._EE_THRESH11
     # if self.ee2_calibrated:
@@ -263,14 +266,17 @@ class DriverStatus():
     self.pose.yaw_std = driver_data.faceOrientationStd[1]
     model_std_max = max(self.pose.pitch_std, self.pose.yaw_std)
     self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD
-    self.blink.left_blink = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
-    self.blink.right_blink = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
+    self.blink.left_blink = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \
+                                                                  * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
+    self.blink.right_blink = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \
+                                                                  * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
     self.eev1 = driver_data.notReadyProb[0]
     self.eev2 = driver_data.readyProb[0]
 
     self.distracted_types = self._get_distracted_types()
-    self.driver_distracted = (DistractedType.DISTRACTED_E2E in self.distracted_types or DistractedType.DISTRACTED_POSE in self.distracted_types or DistractedType.DISTRACTED_BLINK in self.distracted_types) and \
-                                          driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std
+    self.driver_distracted = (DistractedType.DISTRACTED_E2E in self.distracted_types or DistractedType.DISTRACTED_POSE in self.distracted_types
+                                or DistractedType.DISTRACTED_BLINK in self.distracted_types) \
+                              and driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std
     self.driver_distraction_filter.update(self.driver_distracted)
 
     # update offseter
@@ -306,7 +312,8 @@ class DriverStatus():
         self._reset_awareness()
         return
       # only restore awareness when paying attention and alert is not red
-      self.awareness = min(self.awareness + ((self.settings._RECOVERY_FACTOR_MAX-self.settings._RECOVERY_FACTOR_MIN)*(1.-self.awareness)+self.settings._RECOVERY_FACTOR_MIN)*self.step_change, 1.)
+      self.awareness = min(self.awareness + ((self.settings._RECOVERY_FACTOR_MAX-self.settings._RECOVERY_FACTOR_MIN)*
+                                             (1.-self.awareness)+self.settings._RECOVERY_FACTOR_MIN)*self.step_change, 1.)
       if self.awareness == 1.:
         self.awareness_passive = min(self.awareness_passive + self.step_change, 1.)
       # don't display alert banner when awareness is recovering and has cleared orange
diff --git a/selfdrive/navd/SConscript b/selfdrive/navd/SConscript
index 6c0026fe03..c116ef1535 100644
--- a/selfdrive/navd/SConscript
+++ b/selfdrive/navd/SConscript
@@ -4,9 +4,9 @@ map_env = qt_env.Clone()
 libs = ['qt_widgets', 'qt_util', 'qmapboxgl', common, messaging, cereal, visionipc, transformations,
         'zmq', 'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto', 'pthread', 'json11'] + map_env["LIBS"]
 if arch == 'larch64':
-  libs.append('EGL')
+  libs.append(':libEGL_mesa.so.0')
 
-if arch in ['larch64', 'x86_64']:
+if arch in ['larch64', 'aarch64', 'x86_64']:
   if arch == 'x86_64':
     rpath = Dir(f"#third_party/mapbox-gl-native-qt/{arch}").srcnode().abspath
     map_env["RPATH"] += [rpath, ]
diff --git a/selfdrive/navd/helpers.py b/selfdrive/navd/helpers.py
index 011a6c5fb8..55c3f88a9a 100644
--- a/selfdrive/navd/helpers.py
+++ b/selfdrive/navd/helpers.py
@@ -4,9 +4,12 @@ import json
 import math
 from typing import Any, Dict, List, Optional, Tuple, Union, cast
 
-from common.conversions import Conversions
-from common.numpy_fast import clip
-from common.params import Params
+from openpilot.common.conversions import Conversions
+from openpilot.common.numpy_fast import clip
+from openpilot.common.params import Params
+
+DIRECTIONS = ('left', 'right', 'straight')
+MODIFIABLE_DIRECTIONS = ('left', 'right')
 
 EARTH_MEAN_RADIUS = 6371007.2
 SPEED_CONVERSIONS = {
@@ -119,8 +122,10 @@ def coordinate_from_param(param: str, params: Optional[Params] = None) -> Option
 
 
 def string_to_direction(direction: str) -> str:
-  for d in ['left', 'right', 'straight']:
+  for d in DIRECTIONS:
     if d in direction:
+      if 'slight' in direction and d in MODIFIABLE_DIRECTIONS:
+        return 'slight' + d.capitalize()
       return d
   return 'none'
 
@@ -135,32 +140,33 @@ def field_valid(dat: dict, field: str) -> bool:
   return field in dat and dat[field] is not None
 
 
-def parse_banner_instructions(instruction: Any, banners: Any, distance_to_maneuver: float = 0.0) -> None:
+def parse_banner_instructions(banners: Any, distance_to_maneuver: float = 0.0) -> Optional[Dict[str, Any]]:
   if not len(banners):
-    return
+    return None
 
-  current_banner = banners[0]
+  instruction = {}
 
   # A segment can contain multiple banners, find one that we need to show now
+  current_banner = banners[0]
   for banner in banners:
     if distance_to_maneuver < banner['distanceAlongGeometry']:
       current_banner = banner
 
   # Only show banner when close enough to maneuver
-  instruction.showFull = distance_to_maneuver < current_banner['distanceAlongGeometry']
+  instruction['showFull'] = distance_to_maneuver < current_banner['distanceAlongGeometry']
 
   # Primary
   p = current_banner['primary']
   if field_valid(p, 'text'):
-    instruction.maneuverPrimaryText = p['text']
+    instruction['maneuverPrimaryText'] = p['text']
   if field_valid(p, 'type'):
-    instruction.maneuverType = p['type']
+    instruction['maneuverType'] = p['type']
   if field_valid(p, 'modifier'):
-    instruction.maneuverModifier = p['modifier']
+    instruction['maneuverModifier'] = p['modifier']
 
   # Secondary
   if field_valid(current_banner, 'secondary'):
-    instruction.maneuverSecondaryText = current_banner['secondary']['text']
+    instruction['maneuverSecondaryText'] = current_banner['secondary']['text']
 
   # Lane lines
   if field_valid(current_banner, 'sub'):
@@ -178,4 +184,6 @@ def parse_banner_instructions(instruction: Any, banners: Any, distance_to_maneuv
         lane['activeDirection'] = string_to_direction(component['active_direction'])
 
       lanes.append(lane)
-    instruction.lanes = lanes
+    instruction['lanes'] = lanes
+
+  return instruction
diff --git a/selfdrive/navd/main.cc b/selfdrive/navd/main.cc
index f8501bf4a5..5251b046fb 100644
--- a/selfdrive/navd/main.cc
+++ b/selfdrive/navd/main.cc
@@ -4,14 +4,19 @@
 #include <QApplication>
 #include <QDebug>
 
+#include "common/util.h"
 #include "selfdrive/ui/qt/util.h"
 #include "selfdrive/ui/qt/maps/map_helpers.h"
 #include "selfdrive/navd/map_renderer.h"
 #include "system/hardware/hw.h"
 
 int main(int argc, char *argv[]) {
+  Hardware::config_cpu_rendering();
+
   qInstallMessageHandler(swagLogMessageHandler);
   setpriority(PRIO_PROCESS, 0, -20);
+  int ret = util::set_core_affinity({0, 1, 2, 3});
+  assert(ret == 0);
 
   QApplication app(argc, argv);
   std::signal(SIGINT, sigTermHandler);
diff --git a/selfdrive/navd/map_renderer.cc b/selfdrive/navd/map_renderer.cc
index 44acc67b94..543515c631 100644
--- a/selfdrive/navd/map_renderer.cc
+++ b/selfdrive/navd/map_renderer.cc
@@ -64,6 +64,7 @@ MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool online) : m_set
   m_map->setCoordinateZoom(QMapbox::Coordinate(0, 0), DEFAULT_ZOOM);
   m_map->setStyleJson(style.c_str());
   m_map->createRenderer();
+  ever_loaded = false;
 
   m_map->resize(fbo->size());
   m_map->setFramebufferObject(fbo->handle(), fbo->size());
@@ -72,11 +73,13 @@ MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool online) : m_set
   QObject::connect(m_map.data(), &QMapboxGL::mapChanged, [=](QMapboxGL::MapChange change) {
     // Ignore expected signals
     // https://github.com/mapbox/mapbox-gl-native/blob/cf734a2fec960025350d8de0d01ad38aeae155a0/platform/qt/include/qmapboxgl.hpp#L116
-    if (change != QMapboxGL::MapChange::MapChangeRegionWillChange &&
-        change != QMapboxGL::MapChange::MapChangeRegionDidChange &&
-        change != QMapboxGL::MapChange::MapChangeWillStartRenderingFrame &&
-        change != QMapboxGL::MapChange::MapChangeDidFinishRenderingFrameFullyRendered) {
-      LOGD("New map state: %d", change);
+    if (ever_loaded) {
+      if (change != QMapboxGL::MapChange::MapChangeRegionWillChange &&
+          change != QMapboxGL::MapChange::MapChangeRegionDidChange &&
+          change != QMapboxGL::MapChange::MapChangeWillStartRenderingFrame &&
+          change != QMapboxGL::MapChange::MapChangeDidFinishRenderingFrameFullyRendered) {
+        LOGD("New map state: %d", change);
+      }
     }
   });
 
@@ -188,6 +191,7 @@ void MapRenderer::publish(const double render_time, const bool loaded) {
 
   auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman();
   bool valid = loaded && (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && location.getPositionGeodetic().getValid();
+  ever_loaded = ever_loaded || loaded;
   uint64_t ts = nanos_since_boot();
   VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_MAP);
   VisionIpcBufExtra extra = {
diff --git a/selfdrive/navd/map_renderer.h b/selfdrive/navd/map_renderer.h
index 5739ba88f2..dc92c70b0f 100644
--- a/selfdrive/navd/map_renderer.h
+++ b/selfdrive/navd/map_renderer.h
@@ -49,6 +49,7 @@ private:
   }
 
   QTimer* timer;
+  bool ever_loaded = false;
 
 public slots:
   void updatePosition(QMapbox::Coordinate position, float bearing);
diff --git a/selfdrive/navd/map_renderer.py b/selfdrive/navd/map_renderer.py
index aa5682169f..8d525ac73e 100755
--- a/selfdrive/navd/map_renderer.py
+++ b/selfdrive/navd/map_renderer.py
@@ -7,8 +7,8 @@ import numpy as np
 import polyline
 from cffi import FFI
 
-from common.ffi_wrapper import suffix
-from common.basedir import BASEDIR
+from openpilot.common.ffi_wrapper import suffix
+from openpilot.common.basedir import BASEDIR
 
 HEIGHT = WIDTH = SIZE = 256
 METERS_PER_PIXEL = 2
@@ -74,7 +74,7 @@ if __name__ == "__main__":
   renderer = lib.map_renderer_init(ffi.NULL, ffi.NULL)
   wait_ready(lib, renderer)
 
-  geometry = r"{yxk}@|obn~Eg@@eCFqc@J{RFw@?kA@gA?q|@Riu@NuJBgi@ZqVNcRBaPBkG@iSD{I@_H@cH?gG@mG@gG?aD@{LDgDDkVVyQLiGDgX@q_@@qI@qKhS{R~[}NtYaDbGoIvLwNfP_b@|f@oFnF_JxHel@bf@{JlIuxAlpAkNnLmZrWqFhFoh@jd@kX|TkJxH_RnPy^|[uKtHoZ~Um`DlkCorC``CuShQogCtwB_ThQcr@fk@sVrWgRhVmSb\\oj@jxA{Qvg@u]tbAyHzSos@xjBeKbWszAbgEc~@~jCuTrl@cYfo@mRn\\_m@v}@ij@jp@om@lk@y|A`pAiXbVmWzUod@xj@wNlTw}@|uAwSn\\kRfYqOdS_IdJuK`KmKvJoOhLuLbHaMzGwO~GoOzFiSrEsOhD}PhCqw@vJmnAxSczA`Vyb@bHk[fFgl@pJeoDdl@}}@zIyr@hG}X`BmUdBcM^aRR}Oe@iZc@mR_@{FScHxAn_@vz@zCzH~GjPxAhDlB~DhEdJlIbMhFfG|F~GlHrGjNjItLnGvQ~EhLnBfOn@p`@AzAAvn@CfC?fc@`@lUrArStCfSxEtSzGxM|ElFlBrOzJlEbDnC~BfDtCnHjHlLvMdTnZzHpObOf^pKla@~G|a@dErg@rCbj@zArYlj@ttJ~AfZh@r]LzYg@`TkDbj@gIdv@oE|i@kKzhA{CdNsEfOiGlPsEvMiDpLgBpHyB`MkB|MmArPg@|N?|P^rUvFz~AWpOCdAkB|PuB`KeFfHkCfGy@tAqC~AsBPkDs@uAiAcJwMe@s@eKkPMoXQux@EuuCoH?eI?Kas@}Dy@wAUkMOgDL"
+  geometry = r"{yxk}@|obn~Eg@@eCFqc@J{RFw@?kA@gA?q|@Riu@NuJBgi@ZqVNcRBaPBkG@iSD{I@_H@cH?gG@mG@gG?aD@{LDgDDkVVyQLiGDgX@q_@@qI@qKhS{R~[}NtYaDbGoIvLwNfP_b@|f@oFnF_JxHel@bf@{JlIuxAlpAkNnLmZrWqFhFoh@jd@kX|TkJxH_RnPy^|[uKtHoZ~Um`DlkCorC``CuShQogCtwB_ThQcr@fk@sVrWgRhVmSb\\oj@jxA{Qvg@u]tbAyHzSos@xjBeKbWszAbgEc~@~jCuTrl@cYfo@mRn\\_m@v}@ij@jp@om@lk@y|A`pAiXbVmWzUod@xj@wNlTw}@|uAwSn\\kRfYqOdS_IdJuK`KmKvJoOhLuLbHaMzGwO~GoOzFiSrEsOhD}PhCqw@vJmnAxSczA`Vyb@bHk[fFgl@pJeoDdl@}}@zIyr@hG}X`BmUdBcM^aRR}Oe@iZc@mR_@{FScHxAn_@vz@zCzH~GjPxAhDlB~DhEdJlIbMhFfG|F~GlHrGjNjItLnGvQ~EhLnBfOn@p`@AzAAvn@CfC?fc@`@lUrArStCfSxEtSzGxM|ElFlBrOzJlEbDnC~BfDtCnHjHlLvMdTnZzHpObOf^pKla@~G|a@dErg@rCbj@zArYlj@ttJ~AfZh@r]LzYg@`TkDbj@gIdv@oE|i@kKzhA{CdNsEfOiGlPsEvMiDpLgBpHyB`MkB|MmArPg@|N?|P^rUvFz~AWpOCdAkB|PuB`KeFfHkCfGy@tAqC~AsBPkDs@uAiAcJwMe@s@eKkPMoXQux@EuuCoH?eI?Kas@}Dy@wAUkMOgDL" # noqa: E501
   lib.map_renderer_update_route(renderer, geometry.encode())
 
   POSITIONS = [
diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py
index 70a6b62aec..da2b8c06b9 100755
--- a/selfdrive/navd/navd.py
+++ b/selfdrive/navd/navd.py
@@ -5,23 +5,20 @@ import os
 import threading
 
 import requests
-import numpy as np
 
 import cereal.messaging as messaging
 from cereal import log
-from common.api import Api
-from common.params import Params
-from common.realtime import Ratekeeper
-from common.transformations.coordinates import ecef2geodetic
-from selfdrive.navd.helpers import (Coordinate, coordinate_from_param,
+from openpilot.common.api import Api
+from openpilot.common.params import Params
+from openpilot.common.realtime import Ratekeeper
+from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param,
                                     distance_along_geometry, maxspeed_to_ms,
                                     minimum_distance,
                                     parse_banner_instructions)
-from system.swaglog import cloudlog
+from openpilot.system.swaglog import cloudlog
 
 REROUTE_DISTANCE = 25
 MANEUVER_TRANSITION_THRESHOLD = 10
-VALID_POS_STD = 50.0
 REROUTE_COUNTER_MIN = 3
 
 
@@ -79,21 +76,13 @@ class RouteEngine:
 
   def update_location(self):
     location = self.sm['liveLocationKalman']
-    laikad = self.sm['gnssMeasurements']
+    self.gps_ok = location.gpsOK
 
-    locationd_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
-    laikad_valid = laikad.positionECEF.valid and np.linalg.norm(laikad.positionECEF.std) < VALID_POS_STD
+    self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
 
-    self.localizer_valid = locationd_valid or laikad_valid
-    self.gps_ok = location.gpsOK or laikad_valid
-
-    if locationd_valid:
+    if self.localizer_valid:
       self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
       self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
-    elif laikad_valid:
-      geodetic = ecef2geodetic(laikad.positionECEF.value)
-      self.last_position = Coordinate(geodetic[0], geodetic[1])
-      self.last_bearing = None
 
   def recompute_route(self):
     if self.last_position is None:
@@ -226,7 +215,32 @@ class RouteEngine:
 
     # Current instruction
     msg.navInstruction.maneuverDistance = distance_to_maneuver_along_geometry
-    parse_banner_instructions(msg.navInstruction, banner_step['bannerInstructions'], distance_to_maneuver_along_geometry)
+    instruction = parse_banner_instructions(banner_step['bannerInstructions'], distance_to_maneuver_along_geometry)
+    if instruction is not None:
+      for k,v in instruction.items():
+        setattr(msg.navInstruction, k, v)
+
+    # All instructions
+    maneuvers = []
+    for i, step_i in enumerate(self.route):
+      if i < self.step_idx:
+        distance_to_maneuver = -sum(self.route[j]['distance'] for j in range(i+1, self.step_idx)) - along_geometry
+      elif i == self.step_idx:
+        distance_to_maneuver = distance_to_maneuver_along_geometry
+      else:
+        distance_to_maneuver = distance_to_maneuver_along_geometry + sum(self.route[j]['distance'] for j in range(self.step_idx+1, i+1))
+
+      instruction = parse_banner_instructions(step_i['bannerInstructions'], distance_to_maneuver)
+      if instruction is None:
+        continue
+      maneuver = {'distance': distance_to_maneuver}
+      if 'maneuverType' in instruction:
+        maneuver['type'] = instruction['maneuverType']
+      if 'maneuverModifier' in instruction:
+        maneuver['modifier'] = instruction['maneuverModifier']
+      maneuvers.append(maneuver)
+
+    msg.navInstruction.allManeuvers = maneuvers
 
     # Compute total remaining time and distance
     remaining = 1.0 - along_geometry / max(step['distance'], 1)
@@ -330,11 +344,9 @@ class RouteEngine:
     # TODO: Check for going wrong way in segment
 
 
-def main(sm=None, pm=None):
-  if sm is None:
-    sm = messaging.SubMaster(['liveLocationKalman', 'gnssMeasurements', 'managerState'])
-  if pm is None:
-    pm = messaging.PubMaster(['navInstruction', 'navRoute'])
+def main():
+  pm = messaging.PubMaster(['navInstruction', 'navRoute'])
+  sm = messaging.SubMaster(['liveLocationKalman', 'managerState'])
 
   rk = Ratekeeper(1.0)
   route_engine = RouteEngine(sm, pm)
diff --git a/selfdrive/navd/set_destination.py b/selfdrive/navd/set_destination.py
index e6158dbdee..811aa576d1 100755
--- a/selfdrive/navd/set_destination.py
+++ b/selfdrive/navd/set_destination.py
@@ -2,7 +2,7 @@
 import json
 import sys
 
-from common.params import Params
+from openpilot.common.params import Params
 
 if __name__ == "__main__":
   params = Params()
diff --git a/selfdrive/navd/tests/test_map_renderer.py b/selfdrive/navd/tests/test_map_renderer.py
index 934377fed4..dbd6add995 100755
--- a/selfdrive/navd/tests/test_map_renderer.py
+++ b/selfdrive/navd/tests/test_map_renderer.py
@@ -1,28 +1,95 @@
 #!/usr/bin/env python3
+import numpy as np
 import os
+import pytest
 import unittest
-
+import requests
+import threading
+import http.server
 import cereal.messaging as messaging
+
+from typing import Any
 from cereal.visionipc import VisionIpcClient, VisionStreamType
-from selfdrive.manager.process_config import managed_processes
+from openpilot.selfdrive.manager.process_config import managed_processes
 
 LLK_DECIMATION = 10
 CACHE_PATH = "/data/mbgl-cache-navd.db"
 
-def gen_llk():
+LOCATION1 = (32.7174, -117.16277)
+LOCATION2 = (32.7558, -117.2037)
+
+DEFAULT_ITERATIONS = 30 * LLK_DECIMATION
+
+LOCATION1_REPEATED = [LOCATION1] * DEFAULT_ITERATIONS
+LOCATION2_REPEATED = [LOCATION2] * DEFAULT_ITERATIONS
+
+def gen_llk(location=LOCATION1):
   msg = messaging.new_message('liveLocationKalman')
-  msg.liveLocationKalman.positionGeodetic = {'value': [32.7174, -117.16277, 0], 'std': [0., 0., 0.], 'valid': True}
+  msg.liveLocationKalman.positionGeodetic = {'value': [*location, 0], 'std': [0., 0., 0.], 'valid': True}
   msg.liveLocationKalman.calibratedOrientationNED = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True}
   msg.liveLocationKalman.status = 'valid'
   return msg
 
 
+class MapBoxInternetDisabledRequestHandler(http.server.BaseHTTPRequestHandler):
+  INTERNET_ACTIVE = True
+
+  def do_GET(self):
+    if not self.INTERNET_ACTIVE:
+      self.send_response(500)
+      self.end_headers()
+      return
+
+    url = f'https://api.mapbox.com{self.path}'
+
+    headers = dict(self.headers)
+    headers["Host"] = "api.mapbox.com"
+
+    r = requests.get(url, headers=headers, timeout=5)
+
+    self.send_response(r.status_code)
+    self.end_headers()
+    self.wfile.write(r.content)
+
+  def log_message(self, *args: Any) -> None:
+    return
+
+  def log_error(self, *args: Any) -> None:
+    return
+
+
+class MapBoxInternetDisabledServer(threading.Thread):
+  def run(self):
+    self.server = http.server.HTTPServer(("127.0.0.1", 5000), MapBoxInternetDisabledRequestHandler)
+    self.server.serve_forever()
+
+  def stop(self):
+    self.server.shutdown()
+
+  def disable_internet(self):
+    MapBoxInternetDisabledRequestHandler.INTERNET_ACTIVE = False
+
+  def enable_internet(self):
+    MapBoxInternetDisabledRequestHandler.INTERNET_ACTIVE = True
+
+
 class TestMapRenderer(unittest.TestCase):
+  server = MapBoxInternetDisabledServer()
+
   @classmethod
   def setUpClass(cls):
     assert "MAPBOX_TOKEN" in os.environ
+    cls.original_token = os.environ["MAPBOX_TOKEN"]
+    cls.server.start()
+
+  @classmethod
+  def tearDownClass(cls) -> None:
+    cls.server.stop()
 
   def setUp(self):
+    self.server.enable_internet()
+    os.environ['MAPS_HOST'] = 'http://localhost:5000'
+
     self.sm = messaging.SubMaster(['mapRenderState'])
     self.pm = messaging.PubMaster(['liveLocationKalman'])
     self.vipc = VisionIpcClient("navd", VisionStreamType.VISION_STREAM_MAP, True)
@@ -33,39 +100,52 @@ class TestMapRenderer(unittest.TestCase):
   def tearDown(self):
     managed_processes['mapsd'].stop()
 
-  def _run_test(self, expect_valid):
+  def _setup_test(self):
     # start + sync up
     managed_processes['mapsd'].start()
+
     assert self.pm.wait_for_readers_to_update("liveLocationKalman", 10)
 
     assert VisionIpcClient.available_streams("navd", False) == {VisionStreamType.VISION_STREAM_MAP, }
     assert self.vipc.connect(False)
     self.vipc.recv()
 
+  def _run_test(self, expect_valid, locations=LOCATION1_REPEATED):
+    starting_frame_id = None
+
+    render_times = []
+
     # run test
     prev_frame_id = -1
-    for i in range(30*LLK_DECIMATION):
+    for i, location in enumerate(locations):
       frame_expected = (i+1) % LLK_DECIMATION == 0
 
       if self.sm.logMonoTime['mapRenderState'] == 0:
         prev_valid = False
         prev_frame_id = -1
       else:
-        prev_frame_id = self.sm['mapRenderState'].frameId
         prev_valid = self.sm.valid['mapRenderState']
+        prev_frame_id = self.sm['mapRenderState'].frameId
+
+      if starting_frame_id is None:
+        starting_frame_id = prev_frame_id
 
-      llk = gen_llk()
+      llk = gen_llk(location)
       self.pm.send("liveLocationKalman", llk)
       self.pm.wait_for_readers_to_update("liveLocationKalman", 10)
       self.sm.update(1000 if frame_expected else 0)
       assert self.sm.updated['mapRenderState'] == frame_expected, "renderer running at wrong frequency"
 
       if not frame_expected:
-
         continue
 
-      # give a few frames to go valid
-      if expect_valid and not self.sm.valid['mapRenderState'] and not prev_valid and self.sm['mapRenderState'].frameId < 5:
+      frames_since_test_start = self.sm['mapRenderState'].frameId - starting_frame_id
+
+      # give a few frames to switch from valid to invalid, or vice versa
+      invalid_and_not_previously_valid = (expect_valid and not self.sm.valid['mapRenderState'] and not prev_valid)
+      valid_and_not_previously_invalid = (not expect_valid and self.sm.valid['mapRenderState'] and prev_valid)
+
+      if (invalid_and_not_previously_valid or valid_and_not_previously_invalid) and frames_since_test_start < 5:
         continue
 
       # check output
@@ -76,6 +156,7 @@ class TestMapRenderer(unittest.TestCase):
         assert self.sm['mapRenderState'].renderTime == 0.
       else:
         assert 0. < self.sm['mapRenderState'].renderTime < 0.1
+        render_times.append(self.sm['mapRenderState'].renderTime)
 
       # check vision ipc output
       assert self.vipc.recv() is not None
@@ -83,16 +164,58 @@ class TestMapRenderer(unittest.TestCase):
       assert self.vipc.timestamp_sof == llk.logMonoTime
       assert self.vipc.frame_id == self.sm['mapRenderState'].frameId
 
+    return render_times
+
   def test_with_internet(self):
+    self._setup_test()
     self._run_test(True)
 
   def test_with_no_internet(self):
-    token = os.environ['MAPBOX_TOKEN']
-    try:
-      os.environ['MAPBOX_TOKEN'] = 'invalid_token'
-      self._run_test(False)
-    finally:
-      os.environ['MAPBOX_TOKEN'] = token
+    self.server.disable_internet()
+    self._setup_test()
+    self._run_test(False)
+
+  def test_recover_from_no_internet(self):
+    self._setup_test()
+    self._run_test(True)
+
+    self.server.disable_internet()
+
+    # change locations to force mapsd to refetch
+    self._run_test(False, LOCATION2_REPEATED)
+
+    self.server.enable_internet()
+    self._run_test(True, LOCATION2_REPEATED)
+
+    self._run_test(True, LOCATION2_REPEATED)
+
+  @pytest.mark.tici
+  def test_render_time_distribution(self):
+    self._setup_test()
+    # from location1 -> location2 and back
+    locations = np.array([*np.linspace(LOCATION1, LOCATION2, 2000), *np.linspace(LOCATION2, LOCATION1, 2000)]).tolist()
+
+    render_times = self._run_test(True, locations)
+
+    _min = np.min(render_times)
+    _max = np.max(render_times)
+    _mean = np.mean(render_times)
+    _median = np.median(render_times)
+    _stddev = np.std(render_times)
+
+    print(f"Stats: min: {_min}, max: {_max}, mean: {_mean}, median: {_median}, stddev: {_stddev}, count: {len(render_times)}")
+
+    def assert_stat(stat, nominal, tol=0.3):
+      tol = (nominal / (1+tol)), (nominal * (1+tol))
+      self.assertTrue(tol[0] < stat < tol[1], f"{stat} not in tolerance {tol}")
+
+    assert_stat(_mean,   0.030)
+    assert_stat(_median, 0.027)
+    assert_stat(_stddev, 0.0078)
+
+    self.assertLess(_max, 0.065)
+    self.assertGreater(_min, 0.015)
+
 
 if __name__ == "__main__":
   unittest.main()
diff --git a/selfdrive/rtshield.py b/selfdrive/rtshield.py
deleted file mode 100755
index 45571fe2db..0000000000
--- a/selfdrive/rtshield.py
+++ /dev/null
@@ -1,21 +0,0 @@
-#!/usr/bin/env python3
-import os
-import time
-from typing import NoReturn
-
-from common.realtime import set_core_affinity, set_realtime_priority
-
-# RT shield - ensure CPU 3 always remains available for RT processes
-#   runs as SCHED_FIFO with minimum priority to ensure kthreads don't
-#   get scheduled onto CPU 3, but it's always preemptible by realtime
-#   openpilot processes
-
-def main() -> NoReturn:
-  set_core_affinity([int(os.getenv("CORE", "3")), ])
-  set_realtime_priority(1)
-
-  while True:
-    time.sleep(0.000001)
-
-if __name__ == "__main__":
-  main()
diff --git a/selfdrive/sentry.py b/selfdrive/sentry.py
index aa409ea394..b8bc8eff12 100644
--- a/selfdrive/sentry.py
+++ b/selfdrive/sentry.py
@@ -3,11 +3,11 @@ import sentry_sdk
 from enum import Enum
 from sentry_sdk.integrations.threading import ThreadingIntegration
 
-from common.params import Params
-from selfdrive.athena.registration import is_registered_device
-from system.hardware import HARDWARE, PC
-from system.swaglog import cloudlog
-from system.version import get_branch, get_commit, get_origin, get_version, \
+from openpilot.common.params import Params
+from openpilot.selfdrive.athena.registration import is_registered_device
+from openpilot.system.hardware import HARDWARE, PC
+from openpilot.system.swaglog import cloudlog
+from openpilot.system.version import get_branch, get_commit, get_origin, get_version, \
                               is_comma_remote, is_dirty, is_tested_branch
 
 
@@ -42,11 +42,11 @@ def set_tag(key: str, value: str) -> None:
   sentry_sdk.set_tag(key, value)
 
 
-def init(project: SentryProject) -> None:
+def init(project: SentryProject) -> bool:
   # forks like to mess with this, so double check
   comma_remote = is_comma_remote() and "commaai" in get_origin(default="")
   if not comma_remote or not is_registered_device() or PC:
-    return
+    return False
 
   env = "release" if is_tested_branch() else "master"
   dongle_id = Params().get("DongleId", encoding='utf-8')
@@ -73,3 +73,5 @@ def init(project: SentryProject) -> None:
 
   if project == SentryProject.SELFDRIVE:
     sentry_sdk.Hub.current.start_session()
+
+  return True
diff --git a/selfdrive/statsd.py b/selfdrive/statsd.py
index e64907149c..8acf406515 100755
--- a/selfdrive/statsd.py
+++ b/selfdrive/statsd.py
@@ -7,13 +7,13 @@ from collections import defaultdict
 from datetime import datetime, timezone
 from typing import NoReturn, Union, List, Dict
 
-from common.params import Params
+from openpilot.common.params import Params
 from cereal.messaging import SubMaster
-from system.swaglog import cloudlog
-from system.hardware import HARDWARE
-from common.file_helpers import atomic_write_in_dir
-from system.version import get_normalized_origin, get_short_branch, get_short_version, is_dirty
-from system.loggerd.config import STATS_DIR, STATS_DIR_FILE_LIMIT, STATS_SOCKET, STATS_FLUSH_TIME_S
+from openpilot.system.swaglog import cloudlog
+from openpilot.system.hardware import HARDWARE
+from openpilot.common.file_helpers import atomic_write_in_dir
+from openpilot.system.version import get_normalized_origin, get_short_branch, get_short_version, is_dirty
+from openpilot.system.loggerd.config import STATS_DIR, STATS_DIR_FILE_LIMIT, STATS_SOCKET, STATS_FLUSH_TIME_S
 
 
 class METRIC_TYPE:
@@ -23,6 +23,8 @@ class METRIC_TYPE:
 class StatLog:
   def __init__(self):
     self.pid = None
+    self.zctx = None
+    self.sock = None
 
   def connect(self) -> None:
     self.zctx = zmq.Context()
@@ -31,6 +33,12 @@ class StatLog:
     self.sock.connect(STATS_SOCKET)
     self.pid = os.getpid()
 
+  def __del__(self):
+    if self.sock is not None:
+      self.sock.close()
+    if self.zctx is not None:
+      self.zctx.term()
+
   def _send(self, metric: str) -> None:
     if os.getpid() != self.pid:
       self.connect()
@@ -68,7 +76,7 @@ def main() -> NoReturn:
     return res
 
   # open statistics socket
-  ctx = zmq.Context().instance()
+  ctx = zmq.Context.instance()
   sock = ctx.socket(zmq.PULL)
   sock.bind(STATS_SOCKET)
 
@@ -92,70 +100,74 @@ def main() -> NoReturn:
   last_flush_time = time.monotonic()
   gauges = {}
   samples: Dict[str, List[float]] = defaultdict(list)
-  while True:
-    started_prev = sm['deviceState'].started
-    sm.update()
-
-    # Update metrics
+  try:
     while True:
-      try:
-        metric = sock.recv_string(zmq.NOBLOCK)
+      started_prev = sm['deviceState'].started
+      sm.update()
+
+      # Update metrics
+      while True:
         try:
-          metric_type = metric.split('|')[1]
-          metric_name = metric.split(':')[0]
-          metric_value = float(metric.split('|')[0].split(':')[1])
-
-          if metric_type == METRIC_TYPE.GAUGE:
-            gauges[metric_name] = metric_value
-          elif metric_type == METRIC_TYPE.SAMPLE:
-            samples[metric_name].append(metric_value)
-          else:
-            cloudlog.event("unknown metric type", metric_type=metric_type)
-        except Exception:
-          cloudlog.event("malformed metric", metric=metric)
-      except zmq.error.Again:
-        break
-
-    # flush when started state changes or after FLUSH_TIME_S
-    if (time.monotonic() > last_flush_time + STATS_FLUSH_TIME_S) or (sm['deviceState'].started != started_prev):
-      result = ""
-      current_time = datetime.utcnow().replace(tzinfo=timezone.utc)
-      tags['started'] = sm['deviceState'].started
-
-      for key, value in gauges.items():
-        result += get_influxdb_line(f"gauge.{key}", value, current_time, tags)
-
-      for key, values in samples.items():
-        values.sort()
-        sample_count = len(values)
-        sample_sum = sum(values)
-
-        stats = {
-          'count': sample_count,
-          'min': values[0],
-          'max': values[-1],
-          'mean': sample_sum / sample_count,
-        }
-        for percentile in [0.05, 0.5, 0.95]:
-          value = values[int(round(percentile * (sample_count - 1)))]
-          stats[f"p{int(percentile * 100)}"] = value
-
-        result += get_influxdb_line(f"sample.{key}", stats, current_time, tags)
-
-      # clear intermediate data
-      gauges.clear()
-      samples.clear()
-      last_flush_time = time.monotonic()
-
-      # check that we aren't filling up the drive
-      if len(os.listdir(STATS_DIR)) < STATS_DIR_FILE_LIMIT:
-        if len(result) > 0:
-          stats_path = os.path.join(STATS_DIR, f"{current_time.timestamp():.0f}_{idx}")
-          with atomic_write_in_dir(stats_path) as f:
-            f.write(result)
-          idx += 1
-      else:
-        cloudlog.error("stats dir full")
+          metric = sock.recv_string(zmq.NOBLOCK)
+          try:
+            metric_type = metric.split('|')[1]
+            metric_name = metric.split(':')[0]
+            metric_value = float(metric.split('|')[0].split(':')[1])
+
+            if metric_type == METRIC_TYPE.GAUGE:
+              gauges[metric_name] = metric_value
+            elif metric_type == METRIC_TYPE.SAMPLE:
+              samples[metric_name].append(metric_value)
+            else:
+              cloudlog.event("unknown metric type", metric_type=metric_type)
+          except Exception:
+            cloudlog.event("malformed metric", metric=metric)
+        except zmq.error.Again:
+          break
+
+      # flush when started state changes or after FLUSH_TIME_S
+      if (time.monotonic() > last_flush_time + STATS_FLUSH_TIME_S) or (sm['deviceState'].started != started_prev):
+        result = ""
+        current_time = datetime.utcnow().replace(tzinfo=timezone.utc)
+        tags['started'] = sm['deviceState'].started
+
+        for key, value in gauges.items():
+          result += get_influxdb_line(f"gauge.{key}", value, current_time, tags)
+
+        for key, values in samples.items():
+          values.sort()
+          sample_count = len(values)
+          sample_sum = sum(values)
+
+          stats = {
+            'count': sample_count,
+            'min': values[0],
+            'max': values[-1],
+            'mean': sample_sum / sample_count,
+          }
+          for percentile in [0.05, 0.5, 0.95]:
+            value = values[int(round(percentile * (sample_count - 1)))]
+            stats[f"p{int(percentile * 100)}"] = value
+
+          result += get_influxdb_line(f"sample.{key}", stats, current_time, tags)
+
+        # clear intermediate data
+        gauges.clear()
+        samples.clear()
+        last_flush_time = time.monotonic()
+
+        # check that we aren't filling up the drive
+        if len(os.listdir(STATS_DIR)) < STATS_DIR_FILE_LIMIT:
+          if len(result) > 0:
+            stats_path = os.path.join(STATS_DIR, f"{current_time.timestamp():.0f}_{idx}")
+            with atomic_write_in_dir(stats_path) as f:
+              f.write(result)
+            idx += 1
+        else:
+          cloudlog.error("stats dir full")
+  finally:
+    sock.close()
+    ctx.term()
 
 
 if __name__ == "__main__":
diff --git a/selfdrive/test/fuzzy_generation.py b/selfdrive/test/fuzzy_generation.py
index 0b8bd0206d..28c70a0ff4 100644
--- a/selfdrive/test/fuzzy_generation.py
+++ b/selfdrive/test/fuzzy_generation.py
@@ -70,7 +70,7 @@ class FuzzyGenerator:
   def generate_struct(self, schema: capnp.lib.capnp._StructSchema, event: Optional[str] = None) -> st.SearchStrategy[Dict[str, Any]]:
     full_fill: List[str] = list(schema.non_union_fields)
     single_fill: List[str] = [event] if event else [self.draw(st.sampled_from(schema.union_fields))] if schema.union_fields else []
-    return st.fixed_dictionaries(dict((field, self.generate_field(schema.fields[field])) for field in full_fill + single_fill))
+    return st.fixed_dictionaries({field: self.generate_field(schema.fields[field]) for field in full_fill + single_fill})
 
   @classmethod
   def get_random_msg(cls, draw: DrawType, struct: capnp.lib.capnp._StructModule, real_floats: bool = False) -> Dict[str, Any]:
diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py
index f7dab576f3..552070f024 100644
--- a/selfdrive/test/helpers.py
+++ b/selfdrive/test/helpers.py
@@ -1,12 +1,13 @@
 import os
 import time
+
 from functools import wraps
 
 import cereal.messaging as messaging
-from common.params import Params
-from selfdrive.manager.process_config import managed_processes
-from system.hardware import PC
-from system.version import training_version, terms_version
+from openpilot.common.params import Params
+from openpilot.selfdrive.manager.process_config import managed_processes
+from openpilot.system.hardware import PC
+from openpilot.system.version import training_version, terms_version
 
 
 def set_params_enabled():
@@ -67,3 +68,7 @@ def with_processes(processes, init_time=0, ignore_stopped=None):
 
     return wrap
   return wrapper
+
+
+def noop(*args, **kwargs):
+  pass
\ No newline at end of file
diff --git a/selfdrive/test/pytest-tici.ini b/selfdrive/test/pytest-tici.ini
new file mode 100644
index 0000000000..a553018309
--- /dev/null
+++ b/selfdrive/test/pytest-tici.ini
@@ -0,0 +1,5 @@
+[pytest]
+addopts = -Werror --strict-config --strict-markers
+markers = 
+  slow: tests that take awhile to run and can be skipped with -m 'not slow'
+  tici: tests that are only meant to run on the C3/C3X
diff --git a/selfdrive/test/setup_device_ci.sh b/selfdrive/test/setup_device_ci.sh
index 7137bfad2b..c34e097f73 100755
--- a/selfdrive/test/setup_device_ci.sh
+++ b/selfdrive/test/setup_device_ci.sh
@@ -42,6 +42,13 @@ while true; do
   if ! sudo systemctl is-active -q ssh; then
     sudo systemctl start ssh
   fi
+
+  #if ! pgrep -f 'ciui.py' > /dev/null 2>&1; then
+  #  echo 'starting UI'
+  #  cp $SOURCE_DIR/selfdrive/test/ciui.py /data/
+  #  /data/ciui.py &
+  #fi
+
   sleep 5s
 done
 
@@ -49,29 +56,68 @@ sleep infinity
 EOF
 chmod +x $CONTINUE_PATH
 
+safe_checkout() {
+  # completely clean TEST_DIR
+
+  cd $SOURCE_DIR
+
+  # cleanup orphaned locks
+  find .git -type f -name "*.lock" -exec rm {} +
+
+  git reset --hard
+  git fetch --no-tags --no-recurse-submodules -j4 --verbose --depth 1 origin $GIT_COMMIT
+  find . -maxdepth 1 -not -path './.git' -not -name '.' -not -name '..' -exec rm -rf '{}' \;
+  git reset --hard $GIT_COMMIT
+  git checkout $GIT_COMMIT
+  git clean -xdff
+  git submodule sync
+  git submodule update --init --recursive
+  git submodule foreach --recursive "git reset --hard && git clean -xdff"
+
+  git lfs pull
+  (ulimit -n 65535 && git lfs prune)
+
+  echo "git checkout done, t=$SECONDS"
+  du -hs $SOURCE_DIR $SOURCE_DIR/.git
+
+  if [ -z "SKIP_COPY" ]; then
+    rsync -a --delete $SOURCE_DIR $TEST_DIR
+  fi
+}
+
+unsafe_checkout() {
+  # checkout directly in test dir, leave old build products
+
+  cd $TEST_DIR
+
+  # cleanup orphaned locks
+  find .git -type f -name "*.lock" -exec rm {} +
+
+  git fetch --no-tags --no-recurse-submodules -j8 --verbose --depth 1 origin $GIT_COMMIT
+  git checkout --force --no-recurse-submodules $GIT_COMMIT
+  git reset --hard $GIT_COMMIT
+  git clean -df
+  git submodule sync
+  git submodule update --init --recursive
+  git submodule foreach --recursive "git reset --hard && git clean -df"
+
+  git lfs pull
+  (ulimit -n 65535 && git lfs prune)
+}
+
+export GIT_PACK_THREADS=8
+
 # set up environment
 if [ ! -d "$SOURCE_DIR" ]; then
   git clone https://github.com/commaai/openpilot.git $SOURCE_DIR
 fi
-cd $SOURCE_DIR
-
-rm -f .git/index.lock
-git reset --hard
-git fetch --no-tags --no-recurse-submodules -j4 --verbose --depth 1 origin $GIT_COMMIT
-find . -maxdepth 1 -not -path './.git' -not -name '.' -not -name '..' -exec rm -rf '{}' \;
-git reset --hard $GIT_COMMIT
-git checkout $GIT_COMMIT
-git clean -xdff
-git submodule sync
-git submodule update --init --recursive
-git submodule foreach --recursive "git reset --hard && git clean -xdff"
-
-git lfs pull
-(ulimit -n 65535 && git lfs prune)
-
-echo "git checkout done, t=$SECONDS"
-du -hs $SOURCE_DIR $SOURCE_DIR/.git
-
-rsync -a --delete $SOURCE_DIR $TEST_DIR
+
+if [ ! -z "$UNSAFE" ]; then
+  echo "doing unsafe checkout"
+  unsafe_checkout
+else
+  echo "doing safe checkout"
+  safe_checkout
+fi
 
 echo "$TEST_DIR synced with $GIT_COMMIT, t=$SECONDS"
diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py
index e6c3658a18..0e9d207da3 100755
--- a/selfdrive/test/test_onroad.py
+++ b/selfdrive/test/test_onroad.py
@@ -1,7 +1,11 @@
 #!/usr/bin/env python3
+import bz2
 import math
 import json
 import os
+import pathlib
+import psutil
+import pytest
 import shutil
 import subprocess
 import time
@@ -13,15 +17,15 @@ from pathlib import Path
 
 from cereal import car
 import cereal.messaging as messaging
-from cereal.services import service_list
-from common.basedir import BASEDIR
-from common.timeout import Timeout
-from common.params import Params
-from selfdrive.controls.lib.events import EVENTS, ET
-from system.hardware import HARDWARE
-from system.loggerd.config import ROOT
-from selfdrive.test.helpers import set_params_enabled, release_only
-from tools.lib.logreader import LogReader
+from cereal.services import SERVICE_LIST
+from openpilot.common.basedir import BASEDIR
+from openpilot.common.timeout import Timeout
+from openpilot.common.params import Params
+from openpilot.selfdrive.controls.lib.events import EVENTS, ET
+from openpilot.system.hardware import HARDWARE
+from openpilot.selfdrive.test.helpers import set_params_enabled, release_only
+from openpilot.system.hardware.hw import Paths
+from openpilot.tools.lib.logreader import LogReader
 
 # Baseline CPU usage by process
 PROCS = {
@@ -30,23 +34,22 @@ PROCS = {
   "./encoderd": 17.0,
   "./camerad": 14.5,
   "./locationd": 11.0,
-  "./mapsd": 2.0,
+  "./mapsd": 1.5,
   "selfdrive.controls.plannerd": 16.5,
   "./_ui": 18.0,
   "selfdrive.locationd.paramsd": 9.0,
-  "./_sensord": 12.0,
+  "./sensord": 7.0,
   "selfdrive.controls.radard": 4.5,
-  "./_modeld": 4.48,
-  "./_dmonitoringmodeld": 5.0,
-  "./_navmodeld": 1.0,
+  "selfdrive.modeld.modeld": 13.0,
+  "selfdrive.modeld.dmonitoringmodeld": 8.0,
+  "selfdrive.modeld.navmodeld": 1.0,
   "selfdrive.thermald.thermald": 3.87,
   "selfdrive.locationd.calibrationd": 2.0,
   "selfdrive.locationd.torqued": 5.0,
-  "./_soundd": (15.0, 65.0),
+  "./_soundd": (1.0, 65.0),
   "selfdrive.monitoring.dmonitoringd": 4.0,
   "./proclogd": 1.54,
   "system.logmessaged": 0.2,
-  "./clocksd": 0.02,
   "selfdrive.tombstoned": 0,
   "./logcatd": 0,
   "system.micd": 10.0,
@@ -56,7 +59,6 @@ PROCS = {
   "selfdrive.navd.navd": 0.4,
   "system.loggerd.uploader": 3.0,
   "system.loggerd.deleter": 0.1,
-  "selfdrive.locationd.laikad": (1.0, 80.0),  # TODO: better GPS setup in testing closet
 }
 
 PROCS.update({
@@ -69,7 +71,7 @@ PROCS.update({
      "./boardd": 19.0,
     "system.sensord.rawgps.rawgpsd": 1.0,
   }
-}[HARDWARE.get_device_type()])
+}.get(HARDWARE.get_device_type(), {}))
 
 TIMINGS = {
   # rtols: max/min, rsd
@@ -97,12 +99,13 @@ def cputime_total(ct):
   return ct.cpuUser + ct.cpuSystem + ct.cpuChildrenUser + ct.cpuChildrenSystem
 
 
+@pytest.mark.tici
 class TestOnroad(unittest.TestCase):
 
   @classmethod
   def setUpClass(cls):
     if "DEBUG" in os.environ:
-      segs = filter(lambda x: os.path.exists(os.path.join(x, "rlog")), Path(ROOT).iterdir())
+      segs = filter(lambda x: os.path.exists(os.path.join(x, "rlog")), Path(Paths.log_root()).iterdir())
       segs = sorted(segs, key=lambda x: x.stat().st_mtime)
       print(segs[-3])
       cls.lr = list(LogReader(os.path.join(segs[-3], "rlog")))
@@ -115,8 +118,8 @@ class TestOnroad(unittest.TestCase):
     params.remove("CurrentRoute")
     set_params_enabled()
     os.environ['TESTING_CLOSET'] = '1'
-    if os.path.exists(ROOT):
-      shutil.rmtree(ROOT)
+    if os.path.exists(Paths.log_root()):
+      shutil.rmtree(Paths.log_root())
     os.system("rm /dev/shm/*")
 
     # Make sure athena isn't running
@@ -143,8 +146,8 @@ class TestOnroad(unittest.TestCase):
 
         while len(cls.segments) < 3:
           segs = set()
-          if Path(ROOT).exists():
-            segs = set(Path(ROOT).glob(f"{route}--*"))
+          if Path(Paths.log_root()).exists():
+            segs = set(Path(Paths.log_root()).glob(f"{route}--*"))
           cls.segments = sorted(segs, key=lambda s: int(str(s).rsplit('--')[-1]))
           time.sleep(2)
 
@@ -152,6 +155,8 @@ class TestOnroad(unittest.TestCase):
       cls.segments = cls.segments[:-1]
 
     finally:
+      cls.gpu_procs = {psutil.Process(int(f.name)).name() for f in pathlib.Path('/sys/devices/virtual/kgsl/kgsl/proc/').iterdir() if f.is_dir()}
+
       if proc is not None:
         proc.terminate()
         if proc.wait(60) is None:
@@ -161,6 +166,7 @@ class TestOnroad(unittest.TestCase):
 
     # use the second segment by default as it's the first full segment
     cls.lr = list(LogReader(os.path.join(str(cls.segments[1]), "rlog")))
+    cls.log_path = cls.segments[1]
 
   @cached_property
   def service_msgs(self):
@@ -179,7 +185,7 @@ class TestOnroad(unittest.TestCase):
         continue
 
       with self.subTest(service=s):
-        assert len(msgs) >= math.floor(service_list[s].frequency*55)
+        assert len(msgs) >= math.floor(SERVICE_LIST[s].frequency*55)
 
   def test_cloudlog_size(self):
     msgs = [m for m in self.lr if m.which() == 'logMessage']
@@ -191,6 +197,26 @@ class TestOnroad(unittest.TestCase):
     big_logs = [f for f, n in cnt.most_common(3) if n / sum(cnt.values()) > 30.]
     self.assertEqual(len(big_logs), 0, f"Log spam: {big_logs}")
 
+  def test_log_sizes(self):
+    for f in self.log_path.iterdir():
+      assert f.is_file()
+
+      sz = f.stat().st_size / 1e6
+      if f.name in ("qlog", "rlog"):
+        with open(f, 'rb') as ff:
+          sz = len(bz2.compress(ff.read())) / 1e6
+
+      if f.name == "qcamera.ts":
+        assert 2.15 < sz < 2.35
+      elif f.name == "qlog":
+        assert 0.7 < sz < 1.0
+      elif f.name == "rlog":
+        assert 5 < sz < 50
+      elif f.name.endswith('.hevc'):
+        assert 70 < sz < 77
+      else:
+        raise NotImplementedError
+
   def test_ui_timings(self):
     result = "\n"
     result += "------------------------------------------------\n"
@@ -258,7 +284,7 @@ class TestOnroad(unittest.TestCase):
         cpu_ok = False
 
     # Ensure there's no missing procs
-    all_procs = set([p.name for p in self.service_msgs['managerState'][0].managerState.processes if p.shouldBeRunning])
+    all_procs = {p.name for p in self.service_msgs['managerState'][0].managerState.processes if p.shouldBeRunning}
     for p in all_procs:
       with self.subTest(proc=p):
         assert any(p in pp for pp in PROCS.keys()), f"Expected CPU usage missing for {p}"
@@ -276,13 +302,16 @@ class TestOnroad(unittest.TestCase):
     # expected to go up while the MSGQ buffers fill up
     self.assertLessEqual(max(mems) - min(mems), 3.0)
 
+  def test_gpu_usage(self):
+    self.assertEqual(self.gpu_procs, {"weston", "_ui", "camerad", "selfdrive.modeld.modeld"})
+
   def test_camera_processing_time(self):
     result = "\n"
     result += "------------------------------------------------\n"
     result += "-------------- Debayer Timing ------------------\n"
     result += "------------------------------------------------\n"
 
-    ts = [getattr(getattr(m, m.which()), "processingTime") for m in self.lr if 'CameraState' in m.which()]
+    ts = [getattr(m, m.which()).processingTime for m in self.lr if 'CameraState' in m.which()]
     self.assertLess(min(ts), 0.025, f"high execution time: {min(ts)}")
     result += f"execution time: min  {min(ts):.5f}s\n"
     result += f"execution time: max  {max(ts):.5f}s\n"
@@ -297,7 +326,7 @@ class TestOnroad(unittest.TestCase):
     result += "-----------------  SoF Timing ------------------\n"
     result += "------------------------------------------------\n"
     for name in ['roadCameraState', 'wideRoadCameraState', 'driverCameraState']:
-      ts = [getattr(getattr(m, m.which()), "timestampSof") for m in self.lr if name in m.which()]
+      ts = [getattr(m, m.which()).timestampSof for m in self.lr if name in m.which()]
       d_ms = np.diff(ts) / 1e6
       d50 = np.abs(d_ms-50)
       self.assertLess(max(d50), 1.0, f"high sof delta vs 50ms: {max(d50)}")
@@ -315,7 +344,7 @@ class TestOnroad(unittest.TestCase):
 
     cfgs = [("lateralPlan", 0.05, 0.05), ("longitudinalPlan", 0.05, 0.05)]
     for (s, instant_max, avg_max) in cfgs:
-      ts = [getattr(getattr(m, s), "solverExecutionTime") for m in self.service_msgs[s]]
+      ts = [getattr(m, s).solverExecutionTime for m in self.service_msgs[s]]
       self.assertLess(max(ts), instant_max, f"high '{s}' execution time: {max(ts)}")
       self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}")
       result += f"'{s}' execution time: min  {min(ts):.5f}s\n"
@@ -335,7 +364,7 @@ class TestOnroad(unittest.TestCase):
       ("driverStateV2", 0.050, 0.026),
     ]
     for (s, instant_max, avg_max) in cfgs:
-      ts = [getattr(getattr(m, s), "modelExecutionTime") for m in self.service_msgs[s]]
+      ts = [getattr(m, s).modelExecutionTime for m in self.service_msgs[s]]
       self.assertLess(max(ts), instant_max, f"high '{s}' execution time: {max(ts)}")
       self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}")
       result += f"'{s}' execution time: min  {min(ts):.5f}s\n"
@@ -356,7 +385,7 @@ class TestOnroad(unittest.TestCase):
         raise Exception(f"missing {s}")
 
       ts = np.diff(msgs) / 1e9
-      dt = 1 / service_list[s].frequency
+      dt = 1 / SERVICE_LIST[s].frequency
 
       try:
         np.testing.assert_allclose(np.mean(ts), dt, rtol=0.03, err_msg=f"{s} - failed mean timing check")
diff --git a/selfdrive/test/test_time_to_onroad.py b/selfdrive/test/test_time_to_onroad.py
index 5991250945..cc2cc6514e 100755
--- a/selfdrive/test/test_time_to_onroad.py
+++ b/selfdrive/test/test_time_to_onroad.py
@@ -1,14 +1,16 @@
 #!/usr/bin/env python3
 import os
+import pytest
 import time
 import subprocess
 
 import cereal.messaging as messaging
-from common.basedir import BASEDIR
-from common.timeout import Timeout
-from selfdrive.test.helpers import set_params_enabled
+from openpilot.common.basedir import BASEDIR
+from openpilot.common.timeout import Timeout
+from openpilot.selfdrive.test.helpers import set_params_enabled
 
 
+@pytest.mark.tici
 def test_time_to_onroad():
   # launch
   set_params_enabled()
@@ -16,7 +18,7 @@ def test_time_to_onroad():
   proc = subprocess.Popen(["python", manager_path])
 
   start_time = time.monotonic()
-  sm = messaging.SubMaster(['controlsState', 'deviceState'])
+  sm = messaging.SubMaster(['controlsState', 'deviceState', 'carEvents'])
   try:
     # wait for onroad
     with Timeout(20, "timed out waiting to go onroad"):
@@ -38,7 +40,7 @@ def test_time_to_onroad():
     # once we're enageable, must be for the next few seconds
     for _ in range(500):
       sm.update(100)
-      assert sm['controlsState'].engageable
+      assert sm['controlsState'].engageable, f"events: {sm['carEvents']}"
   finally:
     proc.terminate()
     if proc.wait(60) is None:
diff --git a/selfdrive/thermald/fan_controller.py b/selfdrive/thermald/fan_controller.py
old mode 100644
new mode 100755
index f3e822da51..64cd4c78ee
--- a/selfdrive/thermald/fan_controller.py
+++ b/selfdrive/thermald/fan_controller.py
@@ -1,10 +1,10 @@
 #!/usr/bin/env python3
 from abc import ABC, abstractmethod
 
-from common.realtime import DT_TRML
-from common.numpy_fast import interp
-from system.swaglog import cloudlog
-from selfdrive.controls.lib.pid import PIDController
+from openpilot.common.realtime import DT_TRML
+from openpilot.common.numpy_fast import interp
+from openpilot.system.swaglog import cloudlog
+from openpilot.selfdrive.controls.lib.pid import PIDController
 
 class BaseFanController(ABC):
   @abstractmethod
diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py
index 06e2b5e8f9..0b3c73a1c0 100644
--- a/selfdrive/thermald/power_monitoring.py
+++ b/selfdrive/thermald/power_monitoring.py
@@ -1,11 +1,11 @@
+import time
 import threading
 from typing import Optional
 
-from common.params import Params, put_nonblocking
-from common.realtime import sec_since_boot
-from system.hardware import HARDWARE
-from system.swaglog import cloudlog
-from selfdrive.statsd import statlog
+from openpilot.common.params import Params, put_nonblocking
+from openpilot.system.hardware import HARDWARE
+from openpilot.system.swaglog import cloudlog
+from openpilot.selfdrive.statsd import statlog
 
 CAR_VOLTAGE_LOW_PASS_K = 0.011 # LPF gain for 45s tau (dt/tau / (dt/tau + 1))
 
@@ -41,7 +41,7 @@ class PowerMonitoring:
   # Calculation tick
   def calculate(self, voltage: Optional[int], ignition: bool):
     try:
-      now = sec_since_boot()
+      now = time.monotonic()
 
       # If peripheralState is None, we're probably not in a car, so we don't care
       if voltage is None:
@@ -113,7 +113,7 @@ class PowerMonitoring:
     if offroad_timestamp is None:
       return False
 
-    now = sec_since_boot()
+    now = time.monotonic()
     should_shutdown = False
     offroad_time = (now - offroad_timestamp)
     low_voltage_shutdown = (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3) and
diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py
index e76b202dba..3ccb9349e1 100755
--- a/selfdrive/thermald/thermald.py
+++ b/selfdrive/thermald/thermald.py
@@ -13,19 +13,20 @@ import psutil
 
 import cereal.messaging as messaging
 from cereal import log
-from common.dict_helpers import strip_deprecated_keys
-from common.time import MIN_DATE
-from common.filter_simple import FirstOrderFilter
-from common.params import Params
-from common.realtime import DT_TRML, sec_since_boot
-from selfdrive.controls.lib.alertmanager import set_offroad_alert
-from system.hardware import HARDWARE, TICI, AGNOS
-from system.loggerd.config import get_available_percent
-from selfdrive.statsd import statlog
-from system.swaglog import cloudlog
-from selfdrive.thermald.power_monitoring import PowerMonitoring
-from selfdrive.thermald.fan_controller import TiciFanController
-from system.version import terms_version, training_version
+from cereal.services import SERVICE_LIST
+from openpilot.common.dict_helpers import strip_deprecated_keys
+from openpilot.common.time import MIN_DATE
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_TRML
+from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
+from openpilot.system.hardware import HARDWARE, TICI, AGNOS
+from openpilot.system.loggerd.config import get_available_percent
+from openpilot.selfdrive.statsd import statlog
+from openpilot.system.swaglog import cloudlog
+from openpilot.selfdrive.thermald.power_monitoring import PowerMonitoring
+from openpilot.selfdrive.thermald.fan_controller import TiciFanController
+from openpilot.system.version import terms_version, training_version
 
 ThermalStatus = log.DeviceState.ThermalStatus
 NetworkType = log.DeviceState.NetworkType
@@ -33,10 +34,11 @@ NetworkStrength = log.DeviceState.NetworkStrength
 CURRENT_TAU = 15.   # 15s time constant
 TEMP_TAU = 5.   # 5s time constant
 DISCONNECT_TIMEOUT = 5.  # wait 5 seconds before going offroad after disconnect so you get an alert
-PANDA_STATES_TIMEOUT = int(1000 * 1.5 * DT_TRML)  # 1.5x the expected pandaState frequency
+PANDA_STATES_TIMEOUT = round(1000 / SERVICE_LIST['pandaStates'].frequency * 1.5)  # 1.5x the expected pandaState frequency
 
 ThermalBand = namedtuple("ThermalBand", ['min_temp', 'max_temp'])
-HardwareState = namedtuple("HardwareState", ['network_type', 'network_info', 'network_strength', 'network_stats', 'network_metered', 'nvme_temps', 'modem_temps'])
+HardwareState = namedtuple("HardwareState", ['network_type', 'network_info', 'network_strength', 'network_stats',
+                                             'network_metered', '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.
@@ -60,7 +62,7 @@ def populate_tz_by_type():
     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"))
+      tz_by_type[f.read().strip()] = int(n.removeprefix("thermal_zone"))
 
 def read_tz(x):
   if x is None:
@@ -117,8 +119,8 @@ def hw_state_thread(end_event, hw_queue):
 
         # Log modem version once
         if AGNOS and ((modem_version is None) or (modem_nv is None)):
-          modem_version = HARDWARE.get_modem_version()  # pylint: disable=assignment-from-none
-          modem_nv = HARDWARE.get_modem_nv()  # pylint: disable=assignment-from-none
+          modem_version = HARDWARE.get_modem_version()
+          modem_nv = HARDWARE.get_modem_nv()
 
           if (modem_version is not None) and (modem_nv is not None):
             cloudlog.event("modem version", version=modem_version, nv=modem_nv)
@@ -164,7 +166,7 @@ def hw_state_thread(end_event, hw_queue):
     time.sleep(DT_TRML)
 
 
-def thermald_thread(end_event, hw_queue):
+def thermald_thread(end_event, hw_queue) -> None:
   pm = messaging.PubMaster(['deviceState'])
   sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "controlsState", "pandaStates"], poll=["pandaStates"])
 
@@ -176,10 +178,10 @@ def thermald_thread(end_event, hw_queue):
   startup_conditions: Dict[str, bool] = {}
   startup_conditions_prev: Dict[str, bool] = {}
 
-  off_ts = None
-  started_ts = None
+  off_ts: Optional[float] = None
+  started_ts: Optional[float] = None
   started_seen = False
-  startup_blocked_ts = None
+  startup_blocked_ts: Optional[float] = None
   thermal_status = ThermalStatus.yellow
 
   last_hw_state = HardwareState(
@@ -209,6 +211,10 @@ def thermald_thread(end_event, hw_queue):
   while not end_event.is_set():
     sm.update(PANDA_STATES_TIMEOUT)
 
+    # Run at 2Hz
+    if sm.frame % round(SERVICE_LIST['pandaStates'].frequency * DT_TRML) != 0:
+      continue
+
     pandaStates = sm['pandaStates']
     peripheralState = sm['peripheralState']
     peripheral_panda_present = peripheralState.pandaType != log.PandaState.PandaType.unknown
@@ -229,7 +235,7 @@ def thermald_thread(end_event, hw_queue):
         if TICI:
           fan_controller = TiciFanController()
 
-    elif (sec_since_boot() - sm.rcv_time['pandaStates']) > DISCONNECT_TIMEOUT:
+    elif (time.monotonic() - sm.rcv_time['pandaStates']) > DISCONNECT_TIMEOUT:
       if onroad_conditions["ignition"]:
         onroad_conditions["ignition"] = False
         cloudlog.error("panda timed out onroad")
@@ -272,7 +278,7 @@ def thermald_thread(end_event, hw_queue):
     if fan_controller is not None:
       msg.deviceState.fanSpeedPercentDesired = fan_controller.update(all_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))
+    is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (time.monotonic() - off_ts > 60 * 5))
     if is_offroad_for_5_min and offroad_comp_temp > OFFROAD_DANGER_TEMP:
       # if device is offroad and already hot without the extra onroad load,
       # we want to cool down first before increasing load
@@ -353,10 +359,10 @@ def thermald_thread(end_event, hw_queue):
     if should_start:
       off_ts = None
       if started_ts is None:
-        started_ts = sec_since_boot()
+        started_ts = time.monotonic()
         started_seen = True
         if startup_blocked_ts is not None:
-          cloudlog.event("Startup after block", block_duration=(sec_since_boot() - startup_blocked_ts),
+          cloudlog.event("Startup after block", block_duration=(time.monotonic() - startup_blocked_ts),
                          startup_conditions=startup_conditions, onroad_conditions=onroad_conditions,
                          startup_conditions_prev=startup_conditions_prev, error=True)
       startup_blocked_ts = None
@@ -364,11 +370,11 @@ def thermald_thread(end_event, hw_queue):
       if onroad_conditions["ignition"] and (startup_conditions != startup_conditions_prev):
         cloudlog.event("Startup blocked", startup_conditions=startup_conditions, onroad_conditions=onroad_conditions, error=True)
         startup_conditions_prev = startup_conditions.copy()
-        startup_blocked_ts = sec_since_boot()
+        startup_blocked_ts = time.monotonic()
 
       started_ts = None
       if off_ts is None:
-        off_ts = sec_since_boot()
+        off_ts = time.monotonic()
 
     # Offroad power monitoring
     voltage = None if peripheralState.pandaType == log.PandaState.PandaType.unknown else peripheralState.voltage
diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py
index 65fb45b678..3f2fb28d23 100755
--- a/selfdrive/tombstoned.py
+++ b/selfdrive/tombstoned.py
@@ -9,11 +9,11 @@ import time
 import glob
 from typing import NoReturn
 
-from common.file_helpers import mkdirs_exists_ok
-from system.loggerd.config import ROOT
-import selfdrive.sentry as sentry
-from system.swaglog import cloudlog
-from system.version import get_commit
+from openpilot.common.file_helpers import mkdirs_exists_ok
+import openpilot.selfdrive.sentry as sentry
+from openpilot.system.hardware.hw import Paths
+from openpilot.system.swaglog import cloudlog
+from openpilot.system.version import get_commit
 
 MAX_SIZE = 1_000_000 * 100  # allow up to 100M
 MAX_TOMBSTONE_FN_LEN = 62  # 85 - 23 ("<dongle id>/crash/")
@@ -38,7 +38,7 @@ def clear_apport_folder():
 def get_apport_stacktrace(fn):
   try:
     cmd = f'apport-retrace -s <(cat <(echo "Package: openpilot") "{fn}")'
-    return subprocess.check_output(cmd, shell=True, encoding='utf8', timeout=30, executable='/bin/bash')  # pylint: disable=unexpected-keyword-arg
+    return subprocess.check_output(cmd, shell=True, encoding='utf8', timeout=30, executable='/bin/bash')
   except subprocess.CalledProcessError:
     return "Error getting stacktrace"
   except subprocess.TimeoutExpired:
@@ -46,19 +46,16 @@ def get_apport_stacktrace(fn):
 
 
 def get_tombstones():
-  """Returns list of (filename, ctime) for all tombstones in /data/tombstones
-  and apport crashlogs in /var/crash"""
+  """Returns list of (filename, ctime) for all crashlogs"""
   files = []
-  for folder in [TOMBSTONE_DIR, APPORT_DIR]:
-    if os.path.exists(folder):
-      with os.scandir(folder) as d:
-
-        # Loop over first 1000 directory entries
-        for _, f in zip(range(1000), d):
-          if f.name.startswith("tombstone"):
-            files.append((f.path, int(f.stat().st_ctime)))
-          elif f.name.endswith(".crash") and f.stat().st_mode == 0o100640:
-            files.append((f.path, int(f.stat().st_ctime)))
+  if os.path.exists(APPORT_DIR):
+    with os.scandir(APPORT_DIR) as d:
+      # Loop over first 1000 directory entries
+      for _, f in zip(range(1000), d, strict=False):
+        if f.name.startswith("tombstone"):
+          files.append((f.path, int(f.stat().st_ctime)))
+        elif f.name.endswith(".crash") and f.stat().st_mode == 0o100640:
+          files.append((f.path, int(f.stat().st_ctime)))
   return files
 
 
@@ -95,7 +92,7 @@ def report_tombstone_apport(fn):
 
         try:
           sig_num = int(line.strip().split(': ')[-1])
-          message += " (" + signal.Signals(sig_num).name + ")"  # pylint: disable=no-member
+          message += " (" + signal.Signals(sig_num).name + ")"
         except ValueError:
           pass
 
@@ -130,7 +127,7 @@ def report_tombstone_apport(fn):
 
   new_fn = f"{date}_{get_commit(default='nocommit')[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN]
 
-  crashlog_dir = os.path.join(ROOT, "crash")
+  crashlog_dir = os.path.join(Paths.log_root(), "crash")
   mkdirs_exists_ok(crashlog_dir)
 
   # Files could be on different filesystems, copy, then delete
@@ -143,7 +140,7 @@ def report_tombstone_apport(fn):
 
 
 def main() -> NoReturn:
-  sentry.init(sentry.SentryProject.SELFDRIVE_NATIVE)
+  should_report = sentry.init(sentry.SentryProject.SELFDRIVE_NATIVE)
 
   # Clear apport folder on start, otherwise duplicate crashes won't register
   clear_apport_folder()
@@ -153,6 +150,14 @@ def main() -> NoReturn:
     now_tombstones = set(get_tombstones())
 
     for fn, _ in (now_tombstones - initial_tombstones):
+      # clear logs if we're not interested in them
+      if not should_report:
+        try:
+          os.remove(fn)
+        except Exception:
+          pass
+        continue
+
       try:
         cloudlog.info(f"reporting new tombstone {fn}")
         if fn.endswith(".crash"):
diff --git a/selfdrive/ui/.gitignore b/selfdrive/ui/.gitignore
index cb95652191..8ad5d39493 100644
--- a/selfdrive/ui/.gitignore
+++ b/selfdrive/ui/.gitignore
@@ -3,7 +3,6 @@ moc_*
 
 translations/main_test_en.*
 
-_mui
 watch3
 installer/installers/*
 qt/text
diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript
index 3a4077e4f6..916f1017a3 100644
--- a/selfdrive/ui/SConscript
+++ b/selfdrive/ui/SConscript
@@ -9,9 +9,9 @@ base_libs = [common, messaging, cereal, visionipc, transformations, 'zmq',
 if arch == 'larch64':
   base_libs.append('EGL')
 
-maps = arch in ['larch64', 'x86_64']
+maps = arch in ['larch64', 'aarch64', 'x86_64']
 
-if maps and arch == 'x86_64':
+if maps and arch != 'larch64':
   rpath = [Dir(f"#third_party/mapbox-gl-native-qt/{arch}").srcnode().abspath]
   qt_env["RPATH"] += rpath
 
@@ -20,11 +20,11 @@ if arch == "Darwin":
   qt_env['FRAMEWORKS'] += ['OpenCL']
 
 qt_util = qt_env.Library("qt_util", ["#selfdrive/ui/qt/api.cc", "#selfdrive/ui/qt/util.cc"], LIBS=base_libs)
-widgets_src = ["ui.cc", "qt/widgets/input.cc", "qt/widgets/drive_stats.cc", "qt/widgets/wifi.cc",
+widgets_src = ["ui.cc", "qt/widgets/input.cc", "qt/widgets/wifi.cc",
                "qt/widgets/ssh_keys.cc", "qt/widgets/toggle.cc", "qt/widgets/controls.cc",
                "qt/widgets/offroad_alerts.cc", "qt/widgets/prime.cc", "qt/widgets/keyboard.cc",
                "qt/widgets/scrollview.cc", "qt/widgets/cameraview.cc", "#third_party/qrcode/QrCode.cc",
-               "qt/request_repeater.cc", "qt/qt_window.cc", "qt/offroad/networking.cc", "qt/offroad/wifiManager.cc"]
+               "qt/request_repeater.cc", "qt/qt_window.cc", "qt/network/networking.cc", "qt/network/wifi_manager.cc"]
 
 qt_env['CPPDEFINES'] = []
 if maps:
@@ -37,16 +37,42 @@ widgets = qt_env.Library("qt_widgets", widgets_src, LIBS=base_libs)
 Export('widgets')
 qt_libs = [widgets, qt_util] + base_libs
 
+qt_src = ["main.cc", "qt/sidebar.cc", "qt/onroad.cc", "qt/body.cc",
+          "qt/window.cc", "qt/home.cc", "qt/offroad/settings.cc",
+          "qt/offroad/software_settings.cc", "qt/offroad/onboarding.cc",
+          "qt/offroad/driverview.cc", "qt/offroad/experimental_mode.cc"]
+
+# build translation files
+with open(File("translations/languages.json").abspath) as f:
+  languages = json.loads(f.read())
+translation_sources = [f"#selfdrive/ui/translations/{l}.ts" for l in languages.values()]
+translation_targets = [src.replace(".ts", ".qm") for src in translation_sources]
+lrelease_bin = 'third_party/qt5/larch64/bin/lrelease' if arch == 'larch64' else 'lrelease'
+
+lupdate = qt_env.Command(translation_sources, qt_src + widgets_src, "selfdrive/ui/update_translations.py")
+lrelease = qt_env.Command(translation_targets, translation_sources, f"{lrelease_bin} $SOURCES")
+qt_env.Depends(lrelease, lupdate)
+qt_env.NoClean(translation_sources)
+qt_env.Precious(translation_sources)
+qt_env.NoCache(lupdate)
+
+# create qrc file for compiled translations to include with assets
+translations_assets_src = "#selfdrive/assets/translations_assets.qrc"
+with open(File(translations_assets_src).abspath, 'w') as f:
+  f.write('<!DOCTYPE RCC><RCC version="1.0">\n<qresource>\n')
+  f.write('\n'.join([f'<file alias="{l}">../ui/translations/{l}.qm</file>' for l in languages.values()]))
+  f.write('\n</qresource>\n</RCC>')
+
 # build assets
 assets = "#selfdrive/assets/assets.cc"
 assets_src = "#selfdrive/assets/assets.qrc"
-qt_env.Command(assets, assets_src, f"rcc $SOURCES -o $TARGET")
-qt_env.Depends(assets, Glob('#selfdrive/assets/*', exclude=[assets, assets_src, "#selfdrive/assets/assets.o"]))
+qt_env.Command(assets, [assets_src, translations_assets_src], f"rcc $SOURCES -o $TARGET")
+qt_env.Depends(assets, Glob('#selfdrive/assets/*', exclude=[assets, assets_src, translations_assets_src, "#selfdrive/assets/assets.o"]) + [lrelease])
 asset_obj = qt_env.Object("assets", assets)
 
 # build soundd
 qt_env.Program("soundd/_soundd", ["soundd/main.cc", "soundd/sound.cc"], LIBS=qt_libs)
-if GetOption('test'):
+if GetOption('extras'):
   qt_env.Program("tests/playsound", "tests/playsound.cc", LIBS=base_libs)
   qt_env.Program('tests/test_sound', ['tests/test_runner.cc', 'soundd/sound.cc', 'tests/test_sound.cc'], LIBS=qt_libs)
 
@@ -57,45 +83,22 @@ qt_env.Program("qt/text", ["qt/text.cc"], LIBS=qt_libs)
 qt_env.Program("qt/spinner", ["qt/spinner.cc"], LIBS=qt_libs)
 
 # build main UI
-qt_src = ["main.cc", "qt/sidebar.cc", "qt/onroad.cc", "qt/body.cc",
-          "qt/window.cc", "qt/home.cc", "qt/offroad/settings.cc",
-          "qt/offroad/software_settings.cc", "qt/offroad/onboarding.cc",
-          "qt/offroad/driverview.cc", "qt/offroad/experimental_mode.cc"]
 qt_env.Program("_ui", qt_src + [asset_obj], LIBS=qt_libs)
-if GetOption('test'):
+if GetOption('extras'):
   qt_src.remove("main.cc")  # replaced by test_runner
   qt_env.Program('tests/test_translations', [asset_obj, 'tests/test_runner.cc', 'tests/test_translations.cc'] + qt_src, LIBS=qt_libs)
   qt_env.Program('tests/ui_snapshot', [asset_obj, "tests/ui_snapshot.cc"] + qt_src, LIBS=qt_libs)
 
 
-# build translation files
-with open(File("translations/languages.json").abspath) as f:
-  languages = json.loads(f.read())
-translation_sources = [f"#selfdrive/ui/translations/{l}.ts" for l in languages.values()]
-translation_targets = [src.replace(".ts", ".qm") for src in translation_sources]
-lrelease_bin = 'third_party/qt5/larch64/bin/lrelease' if arch == 'larch64' else 'lrelease'
-
-lupdate = qt_env.Command(translation_sources, qt_src, "selfdrive/ui/update_translations.py")
-lrelease = qt_env.Command(translation_targets, translation_sources, f"{lrelease_bin} $SOURCES")
-qt_env.Depends(lrelease, lupdate)
-qt_env.NoClean(translation_sources)
-qt_env.Precious(translation_sources)
-qt_env.NoCache(lupdate)
-
-# setup and factory resetter
-if GetOption('extras'):
+if GetOption('extras') and arch != "Darwin":
+  # setup and factory resetter
   qt_env.Program("qt/setup/reset", ["qt/setup/reset.cc"], LIBS=qt_libs)
   qt_env.Program("qt/setup/setup", ["qt/setup/setup.cc", asset_obj],
                  LIBS=qt_libs + ['curl', 'common', 'json11'])
 
-
-if GetOption('extras'):
   # build updater UI
   qt_env.Program("qt/setup/updater", ["qt/setup/updater.cc", asset_obj], LIBS=qt_libs)
 
-  # build mui
-  qt_env.Program("_mui", ["mui.cc"], LIBS=qt_libs)
-
   # build installers
   senv = qt_env.Clone()
   senv['LINKFLAGS'].append('-Wl,-strip-debug')
@@ -131,5 +134,5 @@ if GetOption('extras'):
     assert f[0].get_size() < 300*1e3
 
 # build watch3
-if arch in ['x86_64', 'Darwin'] or GetOption('extras'):
+if arch in ['x86_64', 'aarch64', 'Darwin'] or GetOption('extras'):
   qt_env.Program("watch3", ["watch3.cc"], LIBS=qt_libs + ['common', 'json11', 'zmq', 'visionipc', 'messaging'])
diff --git a/selfdrive/ui/installer/installer.cc b/selfdrive/ui/installer/installer.cc
index 1af72c04df..179ce60c63 100644
--- a/selfdrive/ui/installer/installer.cc
+++ b/selfdrive/ui/installer/installer.cc
@@ -4,6 +4,7 @@
 #include <cstdlib>
 #include <fstream>
 #include <map>
+#include <string>
 
 #include <QDebug>
 #include <QDir>
diff --git a/selfdrive/ui/main.cc b/selfdrive/ui/main.cc
index ed54d5aa19..4903a3db3d 100644
--- a/selfdrive/ui/main.cc
+++ b/selfdrive/ui/main.cc
@@ -16,7 +16,7 @@ int main(int argc, char *argv[]) {
 
   QTranslator translator;
   QString translation_file = QString::fromStdString(Params().get("LanguageSetting"));
-  if (!translator.load(translation_file, "translations") && translation_file.length()) {
+  if (!translator.load(QString(":/%1").arg(translation_file)) && translation_file.length()) {
     qCritical() << "Failed to load translation file:" << translation_file;
   }
 
diff --git a/selfdrive/ui/mui.cc b/selfdrive/ui/mui.cc
deleted file mode 100644
index e2b4358b65..0000000000
--- a/selfdrive/ui/mui.cc
+++ /dev/null
@@ -1,132 +0,0 @@
-#include <QApplication>
-#include <QtWidgets>
-#include <QTimer>
-#include <QGraphicsScene>
-
-#include "cereal/messaging/messaging.h"
-#include "selfdrive/ui/ui.h"
-#include "selfdrive/ui/qt/qt_window.h"
-
-class StatusBar : public QGraphicsRectItem {
-  private:
-    QLinearGradient linear_gradient;
-    QRadialGradient radial_gradient;
-    QTimer animation_timer;
-    const int animation_length = 10;
-    int animation_index = 0;
-
-  public:
-    StatusBar(double x, double y, double width, double height) : QGraphicsRectItem {x, y, width, height} {
-      linear_gradient = QLinearGradient(0, 0, 0, height/2);
-      linear_gradient.setSpread(QGradient::ReflectSpread);
-
-      radial_gradient = QRadialGradient(width/2, height/2, width/8);
-      QObject::connect(&animation_timer, &QTimer::timeout, [=]() {
-        animation_index++;
-        animation_index %= animation_length;
-      });
-      animation_timer.start(50);
-    }
-
-    void solidColor(QColor color) {
-      QColor dark_color = QColor(color);
-      dark_color.setAlphaF(0.5);
-
-      linear_gradient.setColorAt(0, dark_color);
-      linear_gradient.setColorAt(1, color);
-      setBrush(QBrush(linear_gradient));
-    }
-
-    // these need to be called continuously for the animations to work.
-    // can probably clean that up with some more abstractions
-    void blinkingColor(QColor color) {
-      QColor dark_color = QColor(color);
-      dark_color.setAlphaF(0.1);
-
-      int radius = (rect().width() / animation_length) * animation_index;
-      QPoint center = QPoint(rect().width()/2, rect().height()/2);
-      radial_gradient.setCenter(center);
-      radial_gradient.setFocalPoint(center);
-      radial_gradient.setRadius(radius);
-
-      radial_gradient.setColorAt(1, dark_color);
-      radial_gradient.setColorAt(0, color);
-      setBrush(QBrush(radial_gradient));
-    }
-
-    void laneChange(cereal::LateralPlan::LaneChangeDirection direction) {
-      QColor dark_color = QColor(bg_colors[STATUS_ENGAGED]);
-      dark_color.setAlphaF(0.1);
-
-      int x = (rect().width() / animation_length) * animation_index;
-      QPoint center = QPoint(((direction == cereal::LateralPlan::LaneChangeDirection::RIGHT) ? x : (rect().width() - x)), rect().height()/2);
-      radial_gradient.setCenter(center);
-      radial_gradient.setFocalPoint(center);
-      radial_gradient.setRadius(rect().width()/5);
-
-      radial_gradient.setColorAt(1, dark_color);
-      radial_gradient.setColorAt(0, bg_colors[STATUS_ENGAGED]);
-      setBrush(QBrush(radial_gradient));
-    }
-
-    void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget = nullptr) override {
-      painter->setPen(QPen());
-      painter->setBrush(brush());
-
-      double rounding_radius = rect().height()/2;
-      painter->drawRoundedRect(rect(), rounding_radius, rounding_radius);
-    }
-};
-
-int main(int argc, char *argv[]) {
-  QApplication a(argc, argv);
-  QWidget w;
-  setMainWindow(&w);
-
-  w.setStyleSheet("background-color: black;");
-
-  // our beautiful UI
-  QVBoxLayout *layout = new QVBoxLayout(&w);
-
-  QGraphicsScene *scene = new QGraphicsScene();
-  StatusBar *status_bar = new StatusBar(0, 0, 1000, 50);
-  scene->addItem(status_bar);
-
-  QGraphicsView *graphics_view = new QGraphicsView(scene);
-  layout->insertSpacing(0, 400);
-  layout->addWidget(graphics_view, 0, Qt::AlignCenter);
-
-  QTimer timer;
-  QObject::connect(&timer, &QTimer::timeout, [=]() {
-    static SubMaster sm({"deviceState", "controlsState", "lateralPlan"});
-
-    bool onroad_prev = sm.allAliveAndValid({"deviceState"}) &&
-                       sm["deviceState"].getDeviceState().getStarted();
-    sm.update(0);
-
-    bool onroad = sm.allAliveAndValid({"deviceState"}) &&
-                  sm["deviceState"].getDeviceState().getStarted();
-
-    if (onroad) {
-      auto cs = sm["controlsState"].getControlsState();
-      UIStatus status = cs.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED;
-      auto lp = sm["lateralPlan"].getLateralPlan();
-      if (lp.getLaneChangeState() == cereal::LateralPlan::LaneChangeState::PRE_LANE_CHANGE) {
-        status_bar->blinkingColor(bg_colors[status]);
-      } else if (lp.getLaneChangeState() == cereal::LateralPlan::LaneChangeState::LANE_CHANGE_STARTING ||
-                 lp.getLaneChangeState() == cereal::LateralPlan::LaneChangeState::LANE_CHANGE_FINISHING) {
-        status_bar->laneChange(lp.getLaneChangeDirection());
-      } else {
-        status_bar->solidColor(bg_colors[status]);
-      }
-    }
-
-    if ((onroad != onroad_prev) || sm.frame < 2) {
-      Hardware::set_brightness(50);
-      Hardware::set_display_power(onroad);
-    }
-  });
-  timer.start(50);
-
-  return a.exec();
-}
diff --git a/selfdrive/ui/qt/api.cc b/selfdrive/ui/qt/api.cc
index 84e1a4032e..0e321d4e10 100644
--- a/selfdrive/ui/qt/api.cc
+++ b/selfdrive/ui/qt/api.cc
@@ -12,6 +12,8 @@
 #include <QJsonDocument>
 #include <QNetworkRequest>
 
+#include <string>
+
 #include "common/params.h"
 #include "common/util.h"
 #include "system/hardware/hw.h"
@@ -83,7 +85,7 @@ void HttpRequest::sendRequest(const QString &requestURL, const HttpRequest::Meth
     return;
   }
   QString token;
-  if(create_jwt) {
+  if (create_jwt) {
     token = CommaApi::create_jwt();
   } else {
     QString token_json = QString::fromStdString(util::read_file(util::getenv("HOME") + "/.comma/auth.json"));
diff --git a/selfdrive/ui/qt/body.cc b/selfdrive/ui/qt/body.cc
index f2628d304f..e01adbe063 100644
--- a/selfdrive/ui/qt/body.cc
+++ b/selfdrive/ui/qt/body.cc
@@ -63,9 +63,9 @@ BodyWindow::BodyWindow(QWidget *parent) : fuel_filter(1.0, 5., 1. / UI_FREQ), QW
   face = new QLabel();
   face->setAlignment(Qt::AlignCenter);
   layout->addWidget(face);
-  awake = new QMovie("../assets/body/awake.gif");
+  awake = new QMovie("../assets/body/awake.gif", {}, this);
   awake->setCacheMode(QMovie::CacheAll);
-  sleep = new QMovie("../assets/body/sleep.gif");
+  sleep = new QMovie("../assets/body/sleep.gif", {}, this);
   sleep->setCacheMode(QMovie::CacheAll);
 
   // record button
diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc
index b674d39fbd..9dbe7cbae3 100644
--- a/selfdrive/ui/qt/home.cc
+++ b/selfdrive/ui/qt/home.cc
@@ -11,8 +11,6 @@
 
 #ifdef ENABLE_MAPS
 #include "selfdrive/ui/qt/maps/map_settings.h"
-#else
-#include "selfdrive/ui/qt/widgets/drive_stats.h"
 #endif
 
 // HomeWindow: the container for the offroad and onroad UIs
@@ -152,14 +150,14 @@ OffroadHome::OffroadHome(QWidget* parent) : QFrame(parent) {
 #ifdef ENABLE_MAPS
     left_widget->addWidget(new MapSettings);
 #else
-    left_widget->addWidget(new DriveStats);
+    left_widget->addWidget(new QWidget);
 #endif
     left_widget->addWidget(new PrimeAdWidget);
     left_widget->setStyleSheet("border-radius: 10px;");
 
-    left_widget->setCurrentIndex(uiState()->primeType() ? 0 : 1);
-    connect(uiState(), &UIState::primeTypeChanged, [=](int prime_type) {
-      left_widget->setCurrentIndex(prime_type ? 0 : 1);
+    left_widget->setCurrentIndex(uiState()->hasPrime() ? 0 : 1);
+    connect(uiState(), &UIState::primeChanged, [=](bool prime) {
+      left_widget->setCurrentIndex(prime ? 0 : 1);
     });
 
     home_layout->addWidget(left_widget, 1);
diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc
index bcc028d803..db56fcdcfc 100644
--- a/selfdrive/ui/qt/maps/map.cc
+++ b/selfdrive/ui/qt/maps/map.cc
@@ -1,16 +1,16 @@
 #include "selfdrive/ui/qt/maps/map.h"
 
+#include <algorithm>
 #include <eigen3/Eigen/Dense>
 
 #include <QDebug>
 
-#include "common/transformations/coordinates.hpp"
 #include "selfdrive/ui/qt/maps/map_helpers.h"
 #include "selfdrive/ui/qt/util.h"
 #include "selfdrive/ui/ui.h"
 
 
-const int PAN_TIMEOUT = 100;
+const int INTERACTION_TIMEOUT = 100;
 
 const float MAX_ZOOM = 17;
 const float MIN_ZOOM = 14;
@@ -18,7 +18,7 @@ const float MAX_PITCH = 50;
 const float MIN_PITCH = 0;
 const float MAP_SCALE = 2;
 
-MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings), velocity_filter(0, 10, 0.05) {
+MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings), velocity_filter(0, 10, 0.05, false) {
   QObject::connect(uiState(), &UIState::uiUpdate, this, &MapWindow::updateState);
 
   map_overlay = new QWidget (this);
@@ -35,7 +35,7 @@ MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings),
   map_eta->setFixedHeight(120);
 
   error = new QLabel(this);
-  error->setStyleSheet(R"(color:white;padding:50px 11px;font-size: 90px; background-color:rgb(0, 0, 0, 150);)");
+  error->setStyleSheet(R"(color:white;padding:50px 11px;font-size: 90px; background-color:rgba(0, 0, 0, 150);)");
   error->setAlignment(Qt::AlignCenter);
 
   overlay_layout->addWidget(error);
@@ -43,11 +43,7 @@ MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings),
   overlay_layout->addStretch(1);
   overlay_layout->addWidget(map_eta);
 
-  auto last_gps_position = coordinate_from_param("LastGPSPosition");
-  if (last_gps_position.has_value()) {
-    last_position = *last_gps_position;
-  }
-
+  last_position = coordinate_from_param("LastGPSPosition");
   grabGesture(Qt::GestureType::PinchGesture);
   qDebug() << "MapWindow initialized";
 }
@@ -155,18 +151,20 @@ void MapWindow::updateState(const UIState &s) {
     if (locationd_valid) {
       last_position = QMapbox::Coordinate(locationd_pos.getValue()[0], locationd_pos.getValue()[1]);
       last_bearing = RAD2DEG(locationd_orientation.getValue()[2]);
-      velocity_filter.update(locationd_velocity.getValue()[0]);
+      velocity_filter.update(std::max(10.0, locationd_velocity.getValue()[0]));
     }
   }
 
   if (sm.updated("navRoute") && sm["navRoute"].getNavRoute().getCoordinates().size()) {
+    auto nav_dest = coordinate_from_param("NavDestination");
+    bool allow_open = std::exchange(last_valid_nav_dest, nav_dest) != nav_dest &&
+                      nav_dest && !isVisible();
     qWarning() << "Got new navRoute from navd. Opening map:" << allow_open;
 
-    // Only open the map on setting destination the first time
+    // Show map on destination set/change
     if (allow_open) {
       emit requestSettings(false);
-      emit requestVisible(true); // Show map on destination set/change
-      allow_open = false;
+      emit requestVisible(true);
     }
   }
 
@@ -193,19 +191,19 @@ void MapWindow::updateState(const UIState &s) {
     carPosSource["type"] = "geojson";
     carPosSource["data"] = QVariant::fromValue<QMapbox::Feature>(feature1);
     m_map->updateSource("carPosSource", carPosSource);
+
+    // Map bearing isn't updated when interacting, keep location marker up to date
+    if (last_bearing) {
+      m_map->setLayoutProperty("carPosLayer", "icon-rotate", *last_bearing - m_map->bearing());
+    }
   }
 
-  if (pan_counter == 0) {
+  if (interaction_counter == 0) {
     if (last_position) m_map->setCoordinate(*last_position);
     if (last_bearing) m_map->setBearing(*last_bearing);
-  } else {
-    pan_counter--;
-  }
-
-  if (zoom_counter == 0) {
     m_map->setZoom(util::map_val<float>(velocity_filter.x(), 0, 30, MAX_ZOOM, MIN_ZOOM));
   } else {
-    zoom_counter--;
+    interaction_counter--;
   }
 
   if (sm.updated("navInstruction")) {
@@ -213,8 +211,7 @@ void MapWindow::updateState(const UIState &s) {
     // - API exception/no internet
     // - route response is empty
     // - any time navd is waiting for recompute_countdown
-    auto dest = coordinate_from_param("NavDestination");
-    routing_problem = !sm.valid("navInstruction") && dest.has_value();
+    routing_problem = !sm.valid("navInstruction") && coordinate_from_param("NavDestination").has_value();
 
     if (sm.valid("navInstruction")) {
       auto i = sm["navInstruction"].getNavInstruction();
@@ -269,7 +266,7 @@ void MapWindow::initializeGL() {
 
   m_map->setMargins({0, 350, 0, 50});
   m_map->setPitch(MIN_PITCH);
-  m_map->setStyleUrl("mapbox://styles/commaai/clj7g5vrp007b01qzb5ro0i4j");
+  m_map->setStyleUrl("mapbox://styles/commaai/clkqztk0f00ou01qyhsa5bzpj");
 
   QObject::connect(m_map.data(), &QMapboxGL::mapChanged, [=](QMapboxGL::MapChange change) {
     // set global animation duration to 0 ms so visibility changes are instant
@@ -296,7 +293,7 @@ void MapWindow::clearRoute() {
 
   map_instructions->setVisible(false);
   map_eta->setVisible(false);
-  allow_open = true;
+  last_valid_nav_dest = std::nullopt;
 }
 
 void MapWindow::mousePressEvent(QMouseEvent *ev) {
@@ -310,15 +307,14 @@ void MapWindow::mouseDoubleClickEvent(QMouseEvent *ev) {
   m_map->setZoom(util::map_val<float>(velocity_filter.x(), 0, 30, MAX_ZOOM, MIN_ZOOM));
   update();
 
-  pan_counter = 0;
-  zoom_counter = 0;
+  interaction_counter = 0;
 }
 
 void MapWindow::mouseMoveEvent(QMouseEvent *ev) {
   QPointF delta = ev->localPos() - m_lastPos;
 
   if (!delta.isNull()) {
-    pan_counter = PAN_TIMEOUT;
+    interaction_counter = INTERACTION_TIMEOUT;
     m_map->moveBy(delta / MAP_SCALE);
     update();
   }
@@ -340,7 +336,7 @@ void MapWindow::wheelEvent(QWheelEvent *ev) {
   m_map->scaleBy(1 + factor, ev->pos() / MAP_SCALE);
   update();
 
-  zoom_counter = PAN_TIMEOUT;
+  interaction_counter = INTERACTION_TIMEOUT;
   ev->accept();
 }
 
@@ -365,7 +361,7 @@ void MapWindow::pinchTriggered(QPinchGesture *gesture) {
     // TODO: figure out why gesture centerPoint doesn't work
     m_map->scaleBy(gesture->scaleFactor(), {width() / 2.0 / MAP_SCALE, height() / 2.0 / MAP_SCALE});
     update();
-    zoom_counter = PAN_TIMEOUT;
+    interaction_counter = INTERACTION_TIMEOUT;
   }
 }
 
@@ -382,8 +378,6 @@ void MapWindow::offroadTransition(bool offroad) {
 }
 
 void MapWindow::updateDestinationMarker() {
-  m_map->setPaintProperty("pinLayer", "visibility", "none");
-
   auto nav_dest = coordinate_from_param("NavDestination");
   if (nav_dest.has_value()) {
     auto point = coordinate_to_collection(*nav_dest);
@@ -393,5 +387,7 @@ void MapWindow::updateDestinationMarker() {
     pinSource["data"] = QVariant::fromValue<QMapbox::Feature>(feature);
     m_map->updateSource("pinSource", pinSource);
     m_map->setPaintProperty("pinLayer", "visibility", "visible");
+  } else {
+    m_map->setPaintProperty("pinLayer", "visibility", "none");
   }
 }
diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h
index d57f6517b5..5fe79f8b15 100644
--- a/selfdrive/ui/qt/maps/map.h
+++ b/selfdrive/ui/qt/maps/map.h
@@ -50,14 +50,13 @@ private:
   void setError(const QString &err_str);
 
   bool loaded_once = false;
-  bool allow_open = true;
 
   // Panning
   QPointF m_lastPos;
-  int pan_counter = 0;
-  int zoom_counter = 0;
+  int interaction_counter = 0;
 
   // Position
+  std::optional<QMapbox::Coordinate> last_valid_nav_dest;
   std::optional<QMapbox::Coordinate> last_position;
   std::optional<float> last_bearing;
   FirstOrderFilter velocity_filter;
diff --git a/selfdrive/ui/qt/maps/map_eta.cc b/selfdrive/ui/qt/maps/map_eta.cc
index 4262258cfb..322861ed15 100644
--- a/selfdrive/ui/qt/maps/map_eta.cc
+++ b/selfdrive/ui/qt/maps/map_eta.cc
@@ -3,6 +3,7 @@
 #include <QDateTime>
 #include <QPainter>
 
+#include "selfdrive/ui/qt/maps/map_helpers.h"
 #include "selfdrive/ui/ui.h"
 
 const float MANEUVER_TRANSITION_THRESHOLD = 10;
@@ -30,12 +31,12 @@ void MapETA::paintEvent(QPaintEvent *event) {
 void MapETA::updateETA(float s, float s_typical, float d) {
   // ETA
   auto eta_t = QDateTime::currentDateTime().addSecs(s).time();
-  auto eta = format_24h ? std::array{eta_t.toString("HH:mm"), tr("eta")}
-                        : std::array{eta_t.toString("h:mm a").split(' ')[0], eta_t.toString("a")};
+  auto eta = format_24h ? std::pair{eta_t.toString("HH:mm"), tr("eta")}
+                        : std::pair{eta_t.toString("h:mm a").split(' ')[0], eta_t.toString("a")};
 
   // Remaining time
-  auto remaining = s < 3600 ? std::array{QString::number(int(s / 60)), tr("min")}
-                            : std::array{QString("%1:%2").arg((int)s / 3600).arg(((int)s % 3600) / 60, 2, 10, QLatin1Char('0')), tr("hr")};
+  auto remaining = s < 3600 ? std::pair{QString::number(int(s / 60)), tr("min")}
+                            : std::pair{QString("%1:%2").arg((int)s / 3600).arg(((int)s % 3600) / 60, 2, 10, QLatin1Char('0')), tr("hr")};
   QString color = "#25DA6E";
   if (s / s_typical > 1.5)
     color = "#DA3025";
@@ -43,14 +44,12 @@ void MapETA::updateETA(float s, float s_typical, float d) {
     color = "#DAA725";
 
   // Distance
-  float num = uiState()->scene.is_metric ? (d / 1000.0) : (d * METER_TO_MILE);
-  auto distance = std::array{QString::number(num, 'f', num < 100 ? 1 : 0),
-                             uiState()->scene.is_metric ? tr("km") : tr("mi")};
+  auto distance = map_format_distance(d, uiState()->scene.is_metric);
 
-  eta_doc.setHtml(QString(R"(<body><table><tr><td><b>%1</b></td><td>%2</td>
+  eta_doc.setHtml(QString(R"(<body><table><tr style="vertical-align:bottom;"><td><b>%1</b></td><td>%2</td>
                              <td style="padding-left:40px;color:%3;"><b>%4</b></td><td style="padding-right:40px;color:%3;">%5</td>
                              <td><b>%6</b></td><td>%7</td></tr></body>)")
-                      .arg(eta[0], eta[1], color, remaining[0], remaining[1], distance[0], distance[1]));
+                      .arg(eta.first, eta.second, color, remaining.first, remaining.second, distance.first, distance.second));
 
   setVisible(d >= MANEUVER_TRANSITION_THRESHOLD);
   update();
diff --git a/selfdrive/ui/qt/maps/map_helpers.cc b/selfdrive/ui/qt/maps/map_helpers.cc
index 95db4f2bbd..022355e3ce 100644
--- a/selfdrive/ui/qt/maps/map_helpers.cc
+++ b/selfdrive/ui/qt/maps/map_helpers.cc
@@ -1,5 +1,9 @@
 #include "selfdrive/ui/qt/maps/map_helpers.h"
 
+#include <algorithm>
+#include <string>
+#include <utility>
+
 #include <QJsonDocument>
 #include <QJsonObject>
 
@@ -17,6 +21,7 @@ QMapboxGLSettings get_mapbox_settings() {
 
   if (!Hardware::PC()) {
     settings.setCacheDatabasePath(MAPS_CACHE_PATH);
+    settings.setCacheDatabaseMaximumSize(100 * 1024 * 1024);
   }
   settings.setApiBaseUrl(MAPS_HOST);
   settings.setAccessToken(get_mapbox_token());
@@ -57,7 +62,7 @@ QMapbox::CoordinatesCollections coordinate_to_collection(const QMapbox::Coordina
 
 QMapbox::CoordinatesCollections capnp_coordinate_list_to_collection(const capnp::List<cereal::NavRoute::Coordinate>::Reader& coordinate_list) {
   QMapbox::Coordinates coordinates;
-  for (auto const &c: coordinate_list) {
+  for (auto const &c : coordinate_list) {
     coordinates.push_back({c.getLatitude(), c.getLongitude()});
   }
   return {QMapbox::CoordinatesCollection{coordinates}};
@@ -129,7 +134,19 @@ std::optional<QMapbox::Coordinate> coordinate_from_param(const std::string &para
   }
 }
 
-double angle_difference(double angle1, double angle2) {
-  double diff = fmod(angle2 - angle1 + 180.0, 360.0) - 180.0;
-  return diff < -180.0 ? diff + 360.0 : diff;
+// return {distance, unit}
+std::pair<QString, QString> map_format_distance(float d, bool is_metric) {
+  auto round_distance = [](float d) -> float {
+    return (d > 10) ? std::nearbyint(d) : std::nearbyint(d * 10) / 10.0;
+  };
+
+  d = std::max(d, 0.0f);
+  if (is_metric) {
+    return (d > 500) ? std::pair{QString::number(round_distance(d / 1000)), QObject::tr("km")}
+                     : std::pair{QString::number(50 * std::nearbyint(d / 50)), QObject::tr("m")};
+  } else {
+    float feet = d * METER_TO_FOOT;
+    return (feet > 500) ? std::pair{QString::number(round_distance(d * METER_TO_MILE)), QObject::tr("mi")}
+                        : std::pair{QString::number(50 * std::nearbyint(d / 50)), QObject::tr("ft")};
+  }
 }
diff --git a/selfdrive/ui/qt/maps/map_helpers.h b/selfdrive/ui/qt/maps/map_helpers.h
index f9c56107e3..4d6e9b5382 100644
--- a/selfdrive/ui/qt/maps/map_helpers.h
+++ b/selfdrive/ui/qt/maps/map_helpers.h
@@ -1,6 +1,8 @@
 #pragma once
 
 #include <optional>
+#include <string>
+#include <utility>
 #include <eigen3/Eigen/Dense>
 #include <QMapboxGL>
 #include <QGeoCoordinate>
@@ -25,6 +27,5 @@ QMapbox::CoordinatesCollections coordinate_to_collection(const QMapbox::Coordina
 QMapbox::CoordinatesCollections capnp_coordinate_list_to_collection(const capnp::List<cereal::NavRoute::Coordinate>::Reader &coordinate_list);
 QMapbox::CoordinatesCollections coordinate_list_to_collection(const QList<QGeoCoordinate> &coordinate_list);
 QList<QGeoCoordinate> polyline_to_coordinate_list(const QString &polylineString);
-
 std::optional<QMapbox::Coordinate> coordinate_from_param(const std::string &param);
-double angle_difference(double angle1, double angle2);
+std::pair<QString, QString> map_format_distance(float d, bool is_metric);
diff --git a/selfdrive/ui/qt/maps/map_instructions.cc b/selfdrive/ui/qt/maps/map_instructions.cc
index 9009ab1390..ba8cb356bd 100644
--- a/selfdrive/ui/qt/maps/map_instructions.cc
+++ b/selfdrive/ui/qt/maps/map_instructions.cc
@@ -3,33 +3,38 @@
 #include <QDir>
 #include <QVBoxLayout>
 
+#include "selfdrive/ui/qt/maps/map_helpers.h"
 #include "selfdrive/ui/ui.h"
 
 const QString ICON_SUFFIX = ".png";
 
 MapInstructions::MapInstructions(QWidget *parent) : QWidget(parent) {
   is_rhd = Params().getBool("IsRhdDetected");
-  QHBoxLayout *main_layout = new QHBoxLayout(this);
-  main_layout->setContentsMargins(11, 50, 11, 11);
-  main_layout->addWidget(icon_01 = new QLabel, 0, Qt::AlignTop);
+  QVBoxLayout *main_layout = new QVBoxLayout(this);
+  main_layout->setContentsMargins(11, UI_BORDER_SIZE, 11, 20);
 
-  QWidget *right_container = new QWidget(this);
-  right_container->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
-  QVBoxLayout *layout = new QVBoxLayout(right_container);
+  QHBoxLayout *top_layout = new QHBoxLayout;
+  top_layout->addWidget(icon_01 = new QLabel, 0, Qt::AlignTop);
 
-  layout->addWidget(distance = new QLabel);
+  QVBoxLayout *right_layout = new QVBoxLayout;
+  right_layout->setContentsMargins(9, 9, 9, 0);
+  right_layout->addWidget(distance = new QLabel);
   distance->setStyleSheet(R"(font-size: 90px;)");
 
-  layout->addWidget(primary = new QLabel);
+  right_layout->addWidget(primary = new QLabel);
   primary->setStyleSheet(R"(font-size: 60px;)");
   primary->setWordWrap(true);
 
-  layout->addWidget(secondary = new QLabel);
+  right_layout->addWidget(secondary = new QLabel);
   secondary->setStyleSheet(R"(font-size: 50px;)");
   secondary->setWordWrap(true);
 
-  layout->addLayout(lane_layout = new QHBoxLayout);
-  main_layout->addWidget(right_container);
+  top_layout->addLayout(right_layout);
+
+  main_layout->addLayout(top_layout);
+  main_layout->addLayout(lane_layout = new QHBoxLayout);
+  lane_layout->setAlignment(Qt::AlignHCenter);
+  lane_layout->setSpacing(10);
 
   setStyleSheet("color:white");
   QPalette pal = palette();
@@ -63,18 +68,6 @@ void MapInstructions::buildPixmapCache() {
   }
 }
 
-QString MapInstructions::getDistance(float d) {
-  d = std::max(d, 0.0f);
-  if (uiState()->scene.is_metric) {
-    return (d > 500) ? QString::number(d / 1000, 'f', 1) + tr(" km")
-                     : QString::number(50 * int(d / 50)) + tr(" m");
-  } else {
-    float feet = d * METER_TO_FOOT;
-    return (feet > 500) ? QString::number(d * METER_TO_MILE, 'f', 1) + tr(" mi")
-                        : QString::number(50 * int(feet / 50)) + tr(" ft");
-  }
-}
-
 void MapInstructions::updateInstructions(cereal::NavInstruction::Reader instruction) {
   setUpdatesEnabled(false);
 
@@ -85,7 +78,9 @@ void MapInstructions::updateInstructions(cereal::NavInstruction::Reader instruct
   primary->setText(primary_str);
   secondary->setVisible(secondary_str.length() > 0);
   secondary->setText(secondary_str);
-  distance->setText(getDistance(instruction.getManeuverDistance()));
+
+  auto distance_str_pair = map_format_distance(instruction.getManeuverDistance(), uiState()->scene.is_metric);
+  distance->setText(QString("%1 %2").arg(distance_str_pair.first, distance_str_pair.second));
 
   // Show arrow with direction
   QString type = QString::fromStdString(instruction.getManeuverType());
@@ -100,6 +95,8 @@ void MapInstructions::updateInstructions(cereal::NavInstruction::Reader instruct
     icon_01->setPixmap(pixmap_cache[!rhd ? fn : "rhd_" + fn]);
     icon_01->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
     icon_01->setVisible(true);
+  } else {
+    icon_01->setVisible(false);
   }
 
   // Hide distance after arrival
@@ -109,23 +106,21 @@ void MapInstructions::updateInstructions(cereal::NavInstruction::Reader instruct
   auto lanes = instruction.getLanes();
   for (int i = 0; i < lanes.size(); ++i) {
     bool active = lanes[i].getActive();
-
-    // TODO: only use active direction if active
-    bool left = false, straight = false, right = false;
-    for (auto const &direction : lanes[i].getDirections()) {
-      left |= direction == cereal::NavInstruction::Direction::LEFT;
-      right |= direction == cereal::NavInstruction::Direction::RIGHT;
-      straight |= direction == cereal::NavInstruction::Direction::STRAIGHT;
-    }
+    const auto active_direction = lanes[i].getActiveDirection();
 
     // TODO: Make more images based on active direction and combined directions
     QString fn = "lane_direction_";
-    if (left) {
-      fn += "turn_left";
-    } else if (right) {
-      fn += "turn_right";
-    } else if (straight) {
-      fn += "turn_straight";
+
+    // active direction has precedence
+    if (active && active_direction != cereal::NavInstruction::Direction::NONE) {
+      fn += "turn_" + DIRECTIONS[active_direction];
+    } else {
+      for (auto const &direction : lanes[i].getDirections()) {
+        if (direction != cereal::NavInstruction::Direction::NONE) {
+          fn += "turn_" + DIRECTIONS[direction];
+          break;
+        }
+      }
     }
 
     if (!active) {
diff --git a/selfdrive/ui/qt/maps/map_instructions.h b/selfdrive/ui/qt/maps/map_instructions.h
index 83ad3b87a4..06a943d27f 100644
--- a/selfdrive/ui/qt/maps/map_instructions.h
+++ b/selfdrive/ui/qt/maps/map_instructions.h
@@ -1,11 +1,23 @@
 #pragma once
 
+#include <map>
+#include <vector>
+
 #include <QHash>
 #include <QHBoxLayout>
 #include <QLabel>
 
 #include "cereal/gen/cpp/log.capnp.h"
 
+static std::map<cereal::NavInstruction::Direction, QString> DIRECTIONS = {
+  {cereal::NavInstruction::Direction::NONE, "none"},
+  {cereal::NavInstruction::Direction::LEFT, "left"},
+  {cereal::NavInstruction::Direction::RIGHT, "right"},
+  {cereal::NavInstruction::Direction::STRAIGHT, "straight"},
+  {cereal::NavInstruction::Direction::SLIGHT_LEFT, "slight_left"},
+  {cereal::NavInstruction::Direction::SLIGHT_RIGHT, "slight_right"},
+};
+
 class MapInstructions : public QWidget {
   Q_OBJECT
 
@@ -22,6 +34,5 @@ private:
 public:
   MapInstructions(QWidget * parent=nullptr);
   void buildPixmapCache();
-  QString getDistance(float d);
   void updateInstructions(cereal::NavInstruction::Reader instruction);
 };
diff --git a/selfdrive/ui/qt/maps/map_panel.cc b/selfdrive/ui/qt/maps/map_panel.cc
index f1b8f812fa..0a2286ff6f 100644
--- a/selfdrive/ui/qt/maps/map_panel.cc
+++ b/selfdrive/ui/qt/maps/map_panel.cc
@@ -14,21 +14,21 @@ MapPanel::MapPanel(const QMapboxGLSettings &mapboxSettings, QWidget *parent) : Q
 
   auto map = new MapWindow(mapboxSettings);
   QObject::connect(uiState(), &UIState::offroadTransition, map, &MapWindow::offroadTransition);
-  QObject::connect(device(), &Device::interactiveTimeout, [=]() {
+  QObject::connect(device(), &Device::interactiveTimeout, this, [=]() {
     content_stack->setCurrentIndex(0);
   });
-  QObject::connect(map, &MapWindow::requestVisible, [=](bool visible) {
+  QObject::connect(map, &MapWindow::requestVisible, this, [=](bool visible) {
     // when we show the map for a new route, signal HomeWindow to hide the sidebar
     if (visible) { emit mapPanelRequested(); }
     setVisible(visible);
   });
-  QObject::connect(map, &MapWindow::requestSettings, [=](bool settings) {
+  QObject::connect(map, &MapWindow::requestSettings, this, [=](bool settings) {
     content_stack->setCurrentIndex(settings ? 1 : 0);
   });
   content_stack->addWidget(map);
 
   auto settings = new MapSettings(true, parent);
-  QObject::connect(settings, &MapSettings::closeSettings, [=]() {
+  QObject::connect(settings, &MapSettings::closeSettings, this, [=]() {
     content_stack->setCurrentIndex(0);
   });
   content_stack->addWidget(settings);
diff --git a/selfdrive/ui/qt/maps/map_settings.cc b/selfdrive/ui/qt/maps/map_settings.cc
index 8229252188..4d655be36c 100644
--- a/selfdrive/ui/qt/maps/map_settings.cc
+++ b/selfdrive/ui/qt/maps/map_settings.cc
@@ -1,4 +1,6 @@
-#include "map_settings.h"
+#include "selfdrive/ui/qt/maps/map_settings.h"
+
+#include <utility>
 
 #include <QApplication>
 #include <QDebug>
@@ -7,8 +9,20 @@
 #include "selfdrive/ui/qt/request_repeater.h"
 #include "selfdrive/ui/qt/widgets/scrollview.h"
 
+static void swap(QJsonValueRef v1, QJsonValueRef v2) { std::swap(v1, v2); }
+
+static bool locationEqual(const QJsonValue &v1, const QJsonValue &v2) {
+  return v1["latitude"] == v2["latitude"] && v1["longitude"] == v2["longitude"];
+}
+
+static qint64 convertTimestampToEpoch(const QString &timestamp) {
+  QDateTime dt = QDateTime::fromString(timestamp, Qt::ISODate);
+  return dt.isValid() ? dt.toSecsSinceEpoch() : 0;
+}
+
 MapSettings::MapSettings(bool closeable, QWidget *parent) : QFrame(parent) {
   setContentsMargins(0, 0, 0, 0);
+  setAttribute(Qt::WA_NoMousePropagation);
 
   auto *frame = new QVBoxLayout(this);
   frame->setContentsMargins(40, 40, 40, 0);
@@ -58,11 +72,8 @@ MapSettings::MapSettings(bool closeable, QWidget *parent) : QFrame(parent) {
   frame->addSpacing(32);
 
   current_widget = new DestinationWidget(this);
-  QObject::connect(current_widget, &DestinationWidget::actionClicked, [=]() {
-    if (current_destination.empty()) return;
-    params.remove("NavDestination");
-    updateCurrentRoute();
-  });
+  QObject::connect(current_widget, &DestinationWidget::actionClicked,
+                   []() { NavManager::instance()->setCurrentDestination({}); });
   frame->addWidget(current_widget);
   frame->addSpacing(32);
 
@@ -81,45 +92,16 @@ MapSettings::MapSettings(bool closeable, QWidget *parent) : QFrame(parent) {
   frame->addWidget(destinations_scroller);
 
   setStyleSheet("MapSettings { background-color: #333333; }");
-
-  QObject::connect(NavigationRequest::instance(), &NavigationRequest::locationsUpdated, this, &MapSettings::updateLocations);
-  QObject::connect(NavigationRequest::instance(), &NavigationRequest::nextDestinationUpdated, this, &MapSettings::updateCurrentRoute);
-
-  current_locations = NavigationRequest::instance()->currentLocations();
-}
-
-void MapSettings::mousePressEvent(QMouseEvent *ev) {
-  // Prevent mouse event from propagating up
-  ev->accept();
+  QObject::connect(NavManager::instance(), &NavManager::updated, this, &MapSettings::refresh);
 }
 
 void MapSettings::showEvent(QShowEvent *event) {
-  updateCurrentRoute();
-}
-
-void MapSettings::updateCurrentRoute() {
-  auto dest = QString::fromStdString(params.get("NavDestination"));
-  if (dest.size()) {
-    QJsonDocument doc = QJsonDocument::fromJson(dest.trimmed().toUtf8());
-    if (doc.isNull()) {
-      qWarning() << "JSON Parse failed on NavDestination" << dest;
-      return;
-    }
-    current_destination = doc.object();
-    current_widget->set(current_destination, true);
-  } else {
-    current_destination = {};
-    current_widget->unset("", true);
-  }
-  if (isVisible()) refresh();
-}
-
-void MapSettings::updateLocations(const QJsonArray &locations) {
-  current_locations = locations;
   refresh();
 }
 
 void MapSettings::refresh() {
+  if (!isVisible()) return;
+
   setUpdatesEnabled(false);
 
   auto get_w = [this](int i) {
@@ -131,11 +113,17 @@ void MapSettings::refresh() {
     return w;
   };
 
+  const auto current_dest = NavManager::instance()->currentDestination();
+  if (!current_dest.isEmpty()) {
+    current_widget->set(current_dest, true);
+  } else {
+    current_widget->unset("", true);
+  }
   home_widget->unset(NAV_FAVORITE_LABEL_HOME);
   work_widget->unset(NAV_FAVORITE_LABEL_WORK);
 
   int n = 0;
-  for (auto location : current_locations) {
+  for (auto location : NavManager::instance()->currentLocations()) {
     DestinationWidget *w = nullptr;
     auto dest = location.toObject();
     if (dest["save_type"].toString() == NAV_TYPE_FAVORITE) {
@@ -145,7 +133,7 @@ void MapSettings::refresh() {
     }
     w = w ? w : get_w(n++);
     w->set(dest, false);
-    w->setVisible(dest != current_destination);
+    w->setVisible(!locationEqual(dest, current_dest));
   }
   for (; n < widgets.size(); ++n) widgets[n]->setVisible(false);
 
@@ -153,9 +141,7 @@ void MapSettings::refresh() {
 }
 
 void MapSettings::navigateTo(const QJsonObject &place) {
-  QJsonDocument doc(place);
-  params.put("NavDestination", doc.toJson().toStdString());
-  updateCurrentRoute();
+  NavManager::instance()->setCurrentDestination(place);
   emit closeSettings();
 }
 
@@ -281,24 +267,26 @@ void DestinationWidget::unset(const QString &label, bool current) {
   setVisible(true);
 }
 
-// singleton NavigationRequest
+// singleton NavManager
 
-NavigationRequest *NavigationRequest::instance() {
-  static NavigationRequest *request = new NavigationRequest(qApp);
+NavManager *NavManager::instance() {
+  static NavManager *request = new NavManager(qApp);
   return request;
 }
 
-NavigationRequest::NavigationRequest(QObject *parent) : QObject(parent) {
+NavManager::NavManager(QObject *parent) : QObject(parent) {
+  locations = QJsonDocument::fromJson(params.get("NavPastDestinations").c_str()).array();
+  current_dest = QJsonDocument::fromJson(params.get("NavDestination").c_str()).object();
   if (auto dongle_id = getDongleId()) {
     {
       // Fetch favorite and recent locations
       QString url = CommaApi::BASE_URL + "/v1/navigation/" + *dongle_id + "/locations";
       RequestRepeater *repeater = new RequestRepeater(this, url, "ApiCache_NavDestinations", 30, true);
-      QObject::connect(repeater, &RequestRepeater::requestDone, this, &NavigationRequest::parseLocationsResponse);
+      QObject::connect(repeater, &RequestRepeater::requestDone, this, &NavManager::parseLocationsResponse);
     }
     {
       auto param_watcher = new ParamWatcher(this);
-      QObject::connect(param_watcher, &ParamWatcher::paramChanged, this, &NavigationRequest::nextDestinationUpdated);
+      QObject::connect(param_watcher, &ParamWatcher::paramChanged, this, &NavManager::updated);
 
       // Destination set while offline
       QString url = CommaApi::BASE_URL + "/v1/navigation/" + *dongle_id + "/next";
@@ -318,14 +306,14 @@ NavigationRequest::NavigationRequest(QObject *parent) : QObject(parent) {
 
         // athena can set destination at any time
         param_watcher->addParam("NavDestination");
+        current_dest = QJsonDocument::fromJson(params.get("NavDestination").c_str()).object();
+        emit updated();
       });
     }
   }
 }
 
-static void swap(QJsonValueRef v1, QJsonValueRef v2) { std::swap(v1, v2); }
-
-void NavigationRequest::parseLocationsResponse(const QString &response, bool success) {
+void NavManager::parseLocationsResponse(const QString &response, bool success) {
   if (!success || response == prev_response) return;
 
   prev_response = response;
@@ -335,13 +323,63 @@ void NavigationRequest::parseLocationsResponse(const QString &response, bool suc
     return;
   }
 
-  // Sort: alphabetical FAVORITES, and then most recent (as returned by API).
+  // set last activity time.
+  auto remote_locations = doc.array();
+  for (QJsonValueRef loc : remote_locations) {
+    auto obj = loc.toObject();
+    auto serverTime = convertTimestampToEpoch(obj["modified"].toString());
+    obj.insert("time", qMax(serverTime, getLastActivity(obj)));
+    loc = obj;
+  }
+
+  locations = remote_locations;
+  sortLocations();
+  emit updated();
+}
+
+void NavManager::sortLocations() {
+  // Sort: alphabetical FAVORITES, and then most recent.
   // We don't need to care about the ordering of HOME and WORK. DestinationWidget always displays them at the top.
-  locations = doc.array();
   std::stable_sort(locations.begin(), locations.end(), [](const QJsonValue &a, const QJsonValue &b) {
-    bool has_favorite = a["save_type"] == NAV_TYPE_FAVORITE || b["save_type"] == NAV_TYPE_FAVORITE;
-    return has_favorite && (std::tuple(a["save_type"].toString(), a["place_name"].toString()) <
-                            std::tuple(b["save_type"].toString(), b["place_name"].toString()));
+    if (a["save_type"] == NAV_TYPE_FAVORITE || b["save_type"] == NAV_TYPE_FAVORITE) {
+      return (std::tuple(a["save_type"].toString(), a["place_name"].toString()) <
+              std::tuple(b["save_type"].toString(), b["place_name"].toString()));
+    } else {
+      return a["time"].toVariant().toLongLong() > b["time"].toVariant().toLongLong();
+    }
+  });
+
+  write_param_future = std::async(std::launch::async, [destinations = QJsonArray(locations)]() {
+    Params().put("NavPastDestinations", QJsonDocument(destinations).toJson().toStdString());
   });
-  emit locationsUpdated(locations);
+}
+
+qint64 NavManager::getLastActivity(const QJsonObject &loc) const {
+  qint64 last_activity = 0;
+  auto it = std::find_if(locations.begin(), locations.end(),
+                         [&loc](const QJsonValue &l) { return locationEqual(loc, l); });
+  if (it != locations.end()) {
+    auto tm = it->toObject().value("time");
+    if (!tm.isUndefined() && !tm.isNull()) {
+      last_activity = tm.toVariant().toLongLong();
+    }
+  }
+  return last_activity;
+}
+
+void NavManager::setCurrentDestination(const QJsonObject &loc) {
+  current_dest = loc;
+  if (!current_dest.isEmpty()) {
+    current_dest["time"] = QDateTime::currentSecsSinceEpoch();
+    auto it = std::find_if(locations.begin(), locations.end(),
+                           [&loc](const QJsonValue &l) { return locationEqual(loc, l); });
+    if (it != locations.end()) {
+      *it = current_dest;
+      sortLocations();
+    }
+    params.put("NavDestination", QJsonDocument(current_dest).toJson().toStdString());
+  } else {
+    params.remove("NavDestination");
+  }
+  emit updated();
 }
diff --git a/selfdrive/ui/qt/maps/map_settings.h b/selfdrive/ui/qt/maps/map_settings.h
index 1bef04ac5d..0e151df4ad 100644
--- a/selfdrive/ui/qt/maps/map_settings.h
+++ b/selfdrive/ui/qt/maps/map_settings.h
@@ -1,5 +1,8 @@
 #pragma once
 
+#include <future>
+#include <vector>
+
 #include <QFrame>
 #include <QJsonArray>
 #include <QJsonDocument>
@@ -20,43 +23,41 @@ const QString NAV_FAVORITE_LABEL_WORK = "work";
 
 class DestinationWidget;
 
-class NavigationRequest : public QObject {
+class NavManager : public QObject {
   Q_OBJECT
 
 public:
-  static NavigationRequest *instance();
-  QJsonArray currentLocations() const { return locations; };
+  static NavManager *instance();
+  QJsonArray currentLocations() const { return locations; }
+  QJsonObject currentDestination() const { return current_dest; }
+  void setCurrentDestination(const QJsonObject &loc);
+  qint64 getLastActivity(const QJsonObject &loc) const;
 
 signals:
-  void locationsUpdated(const QJsonArray &locations);
-  void nextDestinationUpdated();
+  void updated();
 
 private:
-  NavigationRequest(QObject *parent);
+  NavManager(QObject *parent);
   void parseLocationsResponse(const QString &response, bool success);
+  void sortLocations();
 
   Params params;
   QString prev_response;
   QJsonArray locations;
+  QJsonObject current_dest;
+  std::future<void> write_param_future;
 };
 
 class MapSettings : public QFrame {
   Q_OBJECT
 public:
   explicit MapSettings(bool closeable = false, QWidget *parent = nullptr);
-
   void navigateTo(const QJsonObject &place);
-  void updateLocations(const QJsonArray &locations);
-  void updateCurrentRoute();
 
 private:
-  void mousePressEvent(QMouseEvent *ev) override;
   void showEvent(QShowEvent *event) override;
   void refresh();
 
-  Params params;
-  QJsonArray current_locations;
-  QJsonObject current_destination;
   QVBoxLayout *destinations_layout;
   DestinationWidget *current_widget;
   DestinationWidget *home_widget;
diff --git a/selfdrive/ui/qt/offroad/networking.cc b/selfdrive/ui/qt/network/networking.cc
similarity index 72%
rename from selfdrive/ui/qt/offroad/networking.cc
rename to selfdrive/ui/qt/network/networking.cc
index c146345eb4..090b9b578c 100644
--- a/selfdrive/ui/qt/offroad/networking.cc
+++ b/selfdrive/ui/qt/network/networking.cc
@@ -1,20 +1,18 @@
-#include "selfdrive/ui/qt/offroad/networking.h"
+#include "selfdrive/ui/qt/network/networking.h"
 
 #include <algorithm>
 
-#include <QDebug>
 #include <QHBoxLayout>
-#include <QLabel>
-#include <QPainter>
 #include <QScrollBar>
+#include <QStyle>
 
 #include "selfdrive/ui/ui.h"
-#include "selfdrive/ui/qt/util.h"
 #include "selfdrive/ui/qt/qt_window.h"
+#include "selfdrive/ui/qt/util.h"
 #include "selfdrive/ui/qt/widgets/controls.h"
-#include "selfdrive/ui/qt/widgets/prime.h"
 #include "selfdrive/ui/qt/widgets/scrollview.h"
 
+static const int ICON_WIDTH = 49;
 
 // Networking functions
 
@@ -79,10 +77,9 @@ void Networking::refresh() {
   an->refresh();
 }
 
-void Networking::connectToNetwork(const Network &n) {
+void Networking::connectToNetwork(const Network n) {
   if (wifi->isKnownConnection(n.ssid)) {
     wifi->activateWifiConnection(n.ssid);
-    wifiWidget->refresh();
   } else if (n.security_type == SecurityType::OPEN) {
     wifi->connect(n);
   } else if (n.security_type == SecurityType::WPA) {
@@ -108,6 +105,7 @@ void Networking::showEvent(QShowEvent *event) {
 }
 
 void Networking::hideEvent(QHideEvent *event) {
+  main_layout->setCurrentWidget(wifiScreen);
   wifi->stop();
 }
 
@@ -186,7 +184,7 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid
   // Set initial config
   wifi->updateGsmSettings(roamingEnabled, QString::fromStdString(params.get("GsmApn")), metered);
 
-  connect(uiState(), &UIState::primeTypeChanged, this, [=](int prime_type) {
+  connect(uiState(), &UIState::primeTypeChanged, this, [=](PrimeType prime_type) {
     bool gsmVisible = prime_type == PrimeType::NONE || prime_type == PrimeType::LITE;
     roamingToggle->setVisible(gsmVisible);
     editApnButton->setVisible(gsmVisible);
@@ -211,7 +209,7 @@ void AdvancedNetworking::toggleTethering(bool enabled) {
 // WifiUI functions
 
 WifiUI::WifiUI(QWidget *parent, WifiManager* wifi) : QWidget(parent), wifi(wifi) {
-  main_layout = new QVBoxLayout(this);
+  QVBoxLayout *main_layout = new QVBoxLayout(this);
   main_layout->setContentsMargins(0, 0, 0, 0);
   main_layout->setSpacing(0);
 
@@ -220,16 +218,17 @@ WifiUI::WifiUI(QWidget *parent, WifiManager* wifi) : QWidget(parent), wifi(wifi)
     QPixmap pix(ASSET_PATH + "/offroad/icon_wifi_strength_" + s + ".svg");
     strengths.push_back(pix.scaledToHeight(68, Qt::SmoothTransformation));
   }
-  lock = QPixmap(ASSET_PATH + "offroad/icon_lock_closed.svg").scaledToWidth(49, Qt::SmoothTransformation);
-  checkmark = QPixmap(ASSET_PATH + "offroad/icon_checkmark.svg").scaledToWidth(49, Qt::SmoothTransformation);
-  circled_slash = QPixmap(ASSET_PATH + "img_circled_slash.svg").scaledToWidth(49, Qt::SmoothTransformation);
+  lock = QPixmap(ASSET_PATH + "offroad/icon_lock_closed.svg").scaledToWidth(ICON_WIDTH, Qt::SmoothTransformation);
+  checkmark = QPixmap(ASSET_PATH + "offroad/icon_checkmark.svg").scaledToWidth(ICON_WIDTH, Qt::SmoothTransformation);
+  circled_slash = QPixmap(ASSET_PATH + "img_circled_slash.svg").scaledToWidth(ICON_WIDTH, Qt::SmoothTransformation);
 
   scanningLabel = new QLabel(tr("Scanning for networks..."));
   scanningLabel->setStyleSheet("font-size: 65px;");
   main_layout->addWidget(scanningLabel, 0, Qt::AlignCenter);
 
-  list_layout = new QVBoxLayout;
-  main_layout->addLayout(list_layout);
+  wifi_list_widget = new ListWidget(this);
+  wifi_list_widget->setVisible(false);
+  main_layout->addWidget(wifi_list_widget);
 
   setStyleSheet(R"(
     QScrollBar::handle:vertical {
@@ -262,16 +261,11 @@ WifiUI::WifiUI(QWidget *parent, WifiManager* wifi) : QWidget(parent), wifi(wifi)
       background-color: black;
     }
     #ssidLabel {
-      font-size: 55px;
-      font-weight: 300;
       text-align: left;
       border: none;
       padding-top: 50px;
       padding-bottom: 50px;
     }
-    #ssidLabel[disconnected=false] {
-      font-weight: 500;
-    }
     #ssidLabel:disabled {
       color: #696969;
     }
@@ -279,76 +273,86 @@ WifiUI::WifiUI(QWidget *parent, WifiManager* wifi) : QWidget(parent), wifi(wifi)
 }
 
 void WifiUI::refresh() {
-  // TODO: don't rebuild this every time
-  clearLayout(list_layout);
-
   bool is_empty = wifi->seenNetworks.isEmpty();
   scanningLabel->setVisible(is_empty);
+  wifi_list_widget->setVisible(!is_empty);
   if (is_empty) return;
 
+  setUpdatesEnabled(false);
+
   const bool is_tethering_enabled = wifi->isTetheringEnabled();
   QList<Network> sortedNetworks = wifi->seenNetworks.values();
   std::sort(sortedNetworks.begin(), sortedNetworks.end(), compare_by_strength);
 
-  // add networks
-  ListWidget *list = new ListWidget(this);
+  int n = 0;
   for (Network &network : sortedNetworks) {
-    QHBoxLayout *hlayout = new QHBoxLayout;
-    hlayout->setContentsMargins(44, 0, 73, 0);
-    hlayout->setSpacing(50);
-
-    // Clickable SSID label
-    ElidedLabel *ssidLabel = new ElidedLabel(network.ssid);
-    ssidLabel->setObjectName("ssidLabel");
-    ssidLabel->setEnabled(network.security_type != SecurityType::UNSUPPORTED);
-    ssidLabel->setProperty("disconnected", network.connected == ConnectedType::DISCONNECTED);
-    if (network.connected == ConnectedType::DISCONNECTED) {
-      QObject::connect(ssidLabel, &ElidedLabel::clicked, this, [=]() { emit connectToNetwork(network); });
-    }
-    hlayout->addWidget(ssidLabel, network.connected == ConnectedType::CONNECTING ? 0 : 1);
-
-    if (network.connected == ConnectedType::CONNECTING) {
-      QPushButton *connecting = new QPushButton(tr("CONNECTING..."));
-      connecting->setObjectName("connecting");
-      hlayout->addWidget(connecting, 2, Qt::AlignLeft);
-    }
-
-    // Forget button
-    if (wifi->isKnownConnection(network.ssid) && !is_tethering_enabled) {
-      QPushButton *forgetBtn = new QPushButton(tr("FORGET"));
-      forgetBtn->setObjectName("forgetBtn");
-      QObject::connect(forgetBtn, &QPushButton::clicked, [=]() {
-        if (ConfirmationDialog::confirm(tr("Forget Wi-Fi Network \"%1\"?").arg(QString::fromUtf8(network.ssid)), tr("Forget"), this)) {
-          wifi->forgetConnection(network.ssid);
-        }
-      });
-      hlayout->addWidget(forgetBtn, 0, Qt::AlignRight);
-    }
-
-    // Status icon
+    QPixmap status_icon;
     if (network.connected == ConnectedType::CONNECTED) {
-      QLabel *connectIcon = new QLabel();
-      connectIcon->setPixmap(checkmark);
-      hlayout->addWidget(connectIcon, 0, Qt::AlignRight);
+      status_icon = checkmark;
     } else if (network.security_type == SecurityType::UNSUPPORTED) {
-      QLabel *unsupportedIcon = new QLabel();
-      unsupportedIcon->setPixmap(circled_slash);
-      hlayout->addWidget(unsupportedIcon, 0, Qt::AlignRight);
+      status_icon = circled_slash;
     } else if (network.security_type == SecurityType::WPA) {
-      QLabel *lockIcon = new QLabel();
-      lockIcon->setPixmap(lock);
-      hlayout->addWidget(lockIcon, 0, Qt::AlignRight);
-    } else {
-      hlayout->addSpacing(lock.width() + hlayout->spacing());
+      status_icon = lock;
     }
+    bool show_forget_btn = wifi->isKnownConnection(network.ssid) && !is_tethering_enabled;
+    QPixmap strength = strengths[strengthLevel(network.strength)];
+
+    auto item = getItem(n++);
+    item->setItem(network, status_icon, show_forget_btn, strength);
+    item->setVisible(true);
+  }
+  for (; n < wifi_items.size(); ++n) wifi_items[n]->setVisible(false);
 
-    // Strength indicator
-    QLabel *strength = new QLabel();
-    strength->setPixmap(strengths[std::clamp((int)round(network.strength / 33.), 0, 3)]);
-    hlayout->addWidget(strength, 0, Qt::AlignRight);
+  setUpdatesEnabled(true);
+}
 
-    list->addItem(hlayout);
+WifiItem *WifiUI::getItem(int n) {
+  auto item = n < wifi_items.size() ? wifi_items[n] : wifi_items.emplace_back(new WifiItem(tr("CONNECTING..."), tr("FORGET")));
+  if (!item->parentWidget()) {
+    QObject::connect(item, &WifiItem::connectToNetwork, this, &WifiUI::connectToNetwork);
+    QObject::connect(item, &WifiItem::forgotNetwork, [this](const Network n) {
+      if (ConfirmationDialog::confirm(tr("Forget Wi-Fi Network \"%1\"?").arg(QString::fromUtf8(n.ssid)), tr("Forget"), this))
+        wifi->forgetConnection(n.ssid);
+    });
+    wifi_list_widget->addItem(item);
   }
-  list_layout->addWidget(list);
-  list_layout->addStretch(1);
+  return item;
+}
+
+// WifiItem
+
+WifiItem::WifiItem(const QString &connecting_text, const QString &forget_text, QWidget *parent) : QWidget(parent) {
+  QHBoxLayout *hlayout = new QHBoxLayout(this);
+  hlayout->setContentsMargins(44, 0, 73, 0);
+  hlayout->setSpacing(50);
+
+  hlayout->addWidget(ssidLabel = new ElidedLabel());
+  ssidLabel->setObjectName("ssidLabel");
+  ssidLabel->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
+  hlayout->addWidget(connecting = new QPushButton(connecting_text), 0, Qt::AlignRight);
+  connecting->setObjectName("connecting");
+  hlayout->addWidget(forgetBtn = new QPushButton(forget_text), 0, Qt::AlignRight);
+  forgetBtn->setObjectName("forgetBtn");
+  hlayout->addWidget(iconLabel = new QLabel(), 0, Qt::AlignRight);
+  hlayout->addWidget(strengthLabel = new QLabel(), 0, Qt::AlignRight);
+
+  iconLabel->setFixedWidth(ICON_WIDTH);
+  QObject::connect(forgetBtn, &QPushButton::clicked, [this]() { emit forgotNetwork(network); });
+  QObject::connect(ssidLabel, &ElidedLabel::clicked, [this]() {
+    if (network.connected == ConnectedType::DISCONNECTED) emit connectToNetwork(network);
+  });
+}
+
+void WifiItem::setItem(const Network &n, const QPixmap &status_icon, bool show_forget_btn, const QPixmap &strength_icon) {
+  network = n;
+
+  ssidLabel->setText(n.ssid);
+  ssidLabel->setEnabled(n.security_type != SecurityType::UNSUPPORTED);
+  ssidLabel->setFont(InterFont(55, network.connected == ConnectedType::DISCONNECTED ? QFont::Normal : QFont::Bold));
+
+  connecting->setVisible(n.connected == ConnectedType::CONNECTING);
+  forgetBtn->setVisible(show_forget_btn);
+
+  iconLabel->setPixmap(status_icon);
+  strengthLabel->setPixmap(strength_icon);
 }
diff --git a/selfdrive/ui/qt/offroad/networking.h b/selfdrive/ui/qt/network/networking.h
similarity index 63%
rename from selfdrive/ui/qt/offroad/networking.h
rename to selfdrive/ui/qt/network/networking.h
index 79cbcc3493..4ff7380f42 100644
--- a/selfdrive/ui/qt/offroad/networking.h
+++ b/selfdrive/ui/qt/network/networking.h
@@ -1,14 +1,32 @@
 #pragma once
 
-#include <QButtonGroup>
-#include <QVBoxLayout>
-#include <QWidget>
+#include <vector>
 
-#include "selfdrive/ui/qt/offroad/wifiManager.h"
+#include "selfdrive/ui/qt/network/wifi_manager.h"
 #include "selfdrive/ui/qt/widgets/input.h"
 #include "selfdrive/ui/qt/widgets/ssh_keys.h"
 #include "selfdrive/ui/qt/widgets/toggle.h"
 
+class WifiItem : public QWidget {
+  Q_OBJECT
+public:
+  explicit WifiItem(const QString &connecting_text, const QString &forget_text, QWidget* parent = nullptr);
+  void setItem(const Network& n, const QPixmap &icon, bool show_forget_btn, const QPixmap &strength);
+
+signals:
+  // Cannot pass Network by reference. it may change after the signal is sent.
+  void connectToNetwork(const Network n);
+  void forgotNetwork(const Network n);
+
+protected:
+  ElidedLabel* ssidLabel;
+  QPushButton* connecting;
+  QPushButton* forgetBtn;
+  QLabel* iconLabel;
+  QLabel* strengthLabel;
+  Network network;
+};
+
 class WifiUI : public QWidget {
   Q_OBJECT
 
@@ -16,17 +34,19 @@ public:
   explicit WifiUI(QWidget *parent = 0, WifiManager* wifi = 0);
 
 private:
+  WifiItem *getItem(int n);
+
   WifiManager *wifi = nullptr;
-  QVBoxLayout *list_layout = nullptr;
   QLabel *scanningLabel = nullptr;
-  QVBoxLayout* main_layout;
   QPixmap lock;
   QPixmap checkmark;
   QPixmap circled_slash;
   QVector<QPixmap> strengths;
+  ListWidget *wifi_list_widget = nullptr;
+  std::vector<WifiItem*> wifi_items;
 
 signals:
-  void connectToNetwork(const Network &n);
+  void connectToNetwork(const Network n);
 
 public slots:
   void refresh();
@@ -65,10 +85,8 @@ private:
   QStackedLayout* main_layout = nullptr;
   QWidget* wifiScreen = nullptr;
   AdvancedNetworking* an = nullptr;
-
   WifiUI* wifiWidget;
 
-protected:
   void showEvent(QShowEvent* event) override;
   void hideEvent(QHideEvent* event) override;
 
@@ -76,6 +94,6 @@ public slots:
   void refresh();
 
 private slots:
-  void connectToNetwork(const Network &n);
+  void connectToNetwork(const Network n);
   void wrongPassword(const QString &ssid);
 };
diff --git a/selfdrive/ui/qt/offroad/networkmanager.h b/selfdrive/ui/qt/network/networkmanager.h
similarity index 99%
rename from selfdrive/ui/qt/offroad/networkmanager.h
rename to selfdrive/ui/qt/network/networkmanager.h
index 31b33fc9f5..2896b0fff7 100644
--- a/selfdrive/ui/qt/offroad/networkmanager.h
+++ b/selfdrive/ui/qt/network/networkmanager.h
@@ -1,3 +1,5 @@
+#pragma once
+
 /**
  * We are using a NetworkManager DBUS API : https://developer.gnome.org/NetworkManager/1.26/spec.html
  * */
diff --git a/selfdrive/ui/qt/offroad/wifiManager.cc b/selfdrive/ui/qt/network/wifi_manager.cc
similarity index 93%
rename from selfdrive/ui/qt/offroad/wifiManager.cc
rename to selfdrive/ui/qt/network/wifi_manager.cc
index be9da34d45..03c6896f7a 100644
--- a/selfdrive/ui/qt/offroad/wifiManager.cc
+++ b/selfdrive/ui/qt/network/wifi_manager.cc
@@ -1,4 +1,4 @@
-#include "selfdrive/ui/qt/offroad/wifiManager.h"
+#include "selfdrive/ui/qt/network/wifi_manager.h"
 
 #include "selfdrive/ui/ui.h"
 #include "selfdrive/ui/qt/widgets/prime.h"
@@ -8,11 +8,8 @@
 #include "selfdrive/ui/qt/util.h"
 
 bool compare_by_strength(const Network &a, const Network &b) {
-  if (a.connected == ConnectedType::CONNECTED) return true;
-  if (b.connected == ConnectedType::CONNECTED) return false;
-  if (a.connected == ConnectedType::CONNECTING) return true;
-  if (b.connected == ConnectedType::CONNECTING) return false;
-  return a.strength > b.strength;
+  return std::tuple(a.connected, strengthLevel(a.strength), b.ssid) >
+         std::tuple(b.connected, strengthLevel(b.strength), a.ssid);
 }
 
 template <typename T = QDBusMessage, typename... Args>
@@ -40,6 +37,10 @@ QDBusPendingCall asyncCall(const QString &path, const QString &interface, const
   return nm.asyncCall(method, args...);
 }
 
+bool emptyPath(const QString &path) {
+  return path == "" || path == "/";
+}
+
 WifiManager::WifiManager(QObject *parent) : QObject(parent) {
   qDBusRegisterMetaType<Connection>();
   qDBusRegisterMetaType<IpConfig>();
@@ -102,15 +103,22 @@ void WifiManager::refreshFinished(QDBusPendingCallWatcher *watcher) {
     auto properties = replay.value();
 
     const QByteArray ssid = properties["Ssid"].toByteArray();
-    uint32_t strength = properties["Strength"].toUInt();
-    if (ssid.isEmpty() || (seenNetworks.contains(ssid) && strength <= seenNetworks[ssid].strength)) continue;
+    if (ssid.isEmpty()) continue;
+
+    // May be multiple access points for each SSID.
+    // Use first for ssid and security type, then update connected status and strength using all
+    if (!seenNetworks.contains(ssid)) {
+      seenNetworks[ssid] = {ssid, 0U, ConnectedType::DISCONNECTED, getSecurityType(properties)};
+    }
 
-    SecurityType security = getSecurityType(properties);
-    ConnectedType ctype = ConnectedType::DISCONNECTED;
     if (path.path() == activeAp) {
-      ctype = (ssid == connecting_to_network) ? ConnectedType::CONNECTING : ConnectedType::CONNECTED;
+      seenNetworks[ssid].connected = (ssid == connecting_to_network) ? ConnectedType::CONNECTING : ConnectedType::CONNECTED;
+    }
+
+    uint32_t strength = properties["Strength"].toUInt();
+    if (seenNetworks[ssid].strength < strength) {
+      seenNetworks[ssid].strength = strength;
     }
-    seenNetworks[ssid] = {ssid, strength, ctype, security};
   }
 
   emit refreshSignal();
@@ -158,8 +166,7 @@ SecurityType WifiManager::getSecurityType(const QVariantMap &properties) {
 }
 
 void WifiManager::connect(const Network &n, const QString &password, const QString &username) {
-  connecting_to_network = n.ssid;
-  seenNetworks[n.ssid].connected = ConnectedType::CONNECTING;
+  setCurrentConnecting(n.ssid);
   forgetConnection(n.ssid);  // Clear all connections that may already exist to the network we are connecting
   Connection connection;
   connection["connection"]["type"] = "802-11-wireless";
@@ -186,7 +193,7 @@ void WifiManager::connect(const Network &n, const QString &password, const QStri
 void WifiManager::deactivateConnectionBySsid(const QString &ssid) {
   for (QDBusObjectPath active_connection : getActiveConnections()) {
     auto pth = call<QDBusObjectPath>(active_connection.path(), NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_ACTIVE_CONNECTION, "SpecificObject");
-    if (pth.path() != "" && pth.path() != "/") {
+    if (!emptyPath(pth.path())) {
       QString Ssid = get_property(pth.path(), "Ssid");
       if (Ssid == ssid) {
         deactivateConnection(active_connection);
@@ -224,6 +231,14 @@ void WifiManager::forgetConnection(const QString &ssid) {
   }
 }
 
+void WifiManager::setCurrentConnecting(const QString &ssid) {
+  connecting_to_network = ssid;
+  for (auto &network : seenNetworks) {
+    network.connected = (network.ssid == ssid) ? ConnectedType::CONNECTING : ConnectedType::DISCONNECTED;
+  }
+  emit refreshSignal();
+}
+
 uint WifiManager::getAdapterType(const QDBusObjectPath &path) {
   return call<uint>(path.path(), NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_DEVICE, "DeviceType");
 }
@@ -269,7 +284,7 @@ void WifiManager::propertyChange(const QString &interface, const QVariantMap &pr
 }
 
 void WifiManager::deviceAdded(const QDBusObjectPath &path) {
-  if (getAdapterType(path) == NM_DEVICE_TYPE_WIFI && (adapter.isEmpty() || adapter == "/")) {
+  if (getAdapterType(path) == NM_DEVICE_TYPE_WIFI && emptyPath(adapter)) {
     adapter = path.path();
     setup();
   }
@@ -312,7 +327,7 @@ void WifiManager::initConnections() {
 std::optional<QDBusPendingCall> WifiManager::activateWifiConnection(const QString &ssid) {
   const QDBusObjectPath &path = getConnectionPath(ssid);
   if (!path.path().isEmpty()) {
-    connecting_to_network = ssid;
+    setCurrentConnecting(ssid);
     return asyncCall(NM_DBUS_PATH, NM_DBUS_INTERFACE, "ActivateConnection", QVariant::fromValue(path), QVariant::fromValue(QDBusObjectPath(adapter)), QVariant::fromValue(QDBusObjectPath("/")));
   }
   return std::nullopt;
@@ -446,7 +461,7 @@ void WifiManager::setTetheringEnabled(bool enabled) {
 }
 
 bool WifiManager::isTetheringEnabled() {
-  if (activeAp != "" && activeAp != "/") {
+  if (!emptyPath(activeAp)) {
     return get_property(activeAp, "Ssid") == tethering_ssid;
   }
   return false;
diff --git a/selfdrive/ui/qt/offroad/wifiManager.h b/selfdrive/ui/qt/network/wifi_manager.h
similarity index 93%
rename from selfdrive/ui/qt/offroad/wifiManager.h
rename to selfdrive/ui/qt/network/wifi_manager.h
index 8be4c6c31b..7debffa452 100644
--- a/selfdrive/ui/qt/offroad/wifiManager.h
+++ b/selfdrive/ui/qt/network/wifi_manager.h
@@ -4,7 +4,7 @@
 #include <QtDBus>
 #include <QTimer>
 
-#include "selfdrive/ui/qt/offroad/networkmanager.h"
+#include "selfdrive/ui/qt/network/networkmanager.h"
 
 enum class SecurityType {
   OPEN,
@@ -33,6 +33,7 @@ struct Network {
   SecurityType security_type;
 };
 bool compare_by_strength(const Network &a, const Network &b);
+inline int strengthLevel(unsigned int strength) { return std::clamp((int)round(strength / 33.), 0, 3); }
 
 class WifiManager : public QObject {
   Q_OBJECT
@@ -84,6 +85,7 @@ private:
   void refreshNetworks();
   void activateModemConnection(const QDBusObjectPath &path);
   void addTetheringConnection();
+  void setCurrentConnecting(const QString &ssid);
 
 signals:
   void wrongPassword(const QString &ssid);
diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc
index 0a216766a2..693a0253b4 100644
--- a/selfdrive/ui/qt/offroad/driverview.cc
+++ b/selfdrive/ui/qt/offroad/driverview.cc
@@ -1,5 +1,6 @@
 #include "selfdrive/ui/qt/offroad/driverview.h"
 
+#include <algorithm>
 #include <QPainter>
 
 #include "selfdrive/ui/qt/qt_window.h"
@@ -19,20 +20,29 @@ DriverViewWindow::DriverViewWindow(QWidget* parent) : QWidget(parent) {
   connect(cameraView, &CameraWidget::vipcThreadFrameReceived, scene, &DriverViewScene::frameUpdated);
   layout->addWidget(scene);
   layout->setCurrentWidget(scene);
+
+  QObject::connect(device(), &Device::interactiveTimeout, this, &DriverViewWindow::closeView);
+}
+
+void DriverViewWindow::closeView() {
+  if (isVisible()) {
+    cameraView->stopVipcThread();
+    emit done();
+  }
 }
 
 void DriverViewWindow::mouseReleaseEvent(QMouseEvent* e) {
-  cameraView->stopVipcThread();
-  emit done();
+  closeView();
 }
 
-DriverViewScene::DriverViewScene(QWidget* parent) : sm({"driverStateV2"}), QWidget(parent) {
+DriverViewScene::DriverViewScene(QWidget* parent) : QWidget(parent) {
   face_img = loadPixmap("../assets/img_driver_face_static.png", {FACE_IMG_SIZE, FACE_IMG_SIZE});
 }
 
 void DriverViewScene::showEvent(QShowEvent* event) {
   frame_updated = false;
   params.putBool("IsDriverViewEnabled", true);
+  device()->resetInteractiveTimeout(60);
 }
 
 void DriverViewScene::hideEvent(QHideEvent* event) {
@@ -41,7 +51,6 @@ void DriverViewScene::hideEvent(QHideEvent* event) {
 
 void DriverViewScene::frameUpdated() {
   frame_updated = true;
-  sm.update(0);
   update();
 }
 
@@ -57,6 +66,7 @@ void DriverViewScene::paintEvent(QPaintEvent* event) {
     return;
   }
 
+  const auto &sm = *(uiState()->sm);
   cereal::DriverStateV2::Reader driver_state = sm["driverStateV2"].getDriverStateV2();
   cereal::DriverStateV2::DriverData::Reader driver_data;
 
diff --git a/selfdrive/ui/qt/offroad/driverview.h b/selfdrive/ui/qt/offroad/driverview.h
index 255857970d..8bfc7a4b7b 100644
--- a/selfdrive/ui/qt/offroad/driverview.h
+++ b/selfdrive/ui/qt/offroad/driverview.h
@@ -1,10 +1,7 @@
 #pragma once
 
-#include <memory>
-
 #include <QStackedLayout>
 
-#include "common/util.h"
 #include "selfdrive/ui/qt/widgets/cameraview.h"
 
 class DriverViewScene : public QWidget {
@@ -23,7 +20,6 @@ protected:
 
 private:
   Params params;
-  SubMaster sm;
   QPixmap face_img;
   bool is_rhd = false;
   bool frame_updated = false;
@@ -40,8 +36,8 @@ signals:
 
 protected:
   void mouseReleaseEvent(QMouseEvent* e) override;
+  void closeView();
 
-private:
   CameraWidget *cameraView;
   DriverViewScene *scene;
   QStackedLayout *layout;
diff --git a/selfdrive/ui/qt/offroad/onboarding.cc b/selfdrive/ui/qt/offroad/onboarding.cc
index 5bf1b6fc43..c7c22f0ea3 100644
--- a/selfdrive/ui/qt/offroad/onboarding.cc
+++ b/selfdrive/ui/qt/offroad/onboarding.cc
@@ -1,5 +1,7 @@
 #include "selfdrive/ui/qt/offroad/onboarding.h"
 
+#include <string>
+
 #include <QLabel>
 #include <QPainter>
 #include <QQmlContext>
diff --git a/selfdrive/ui/qt/offroad/onboarding.h b/selfdrive/ui/qt/offroad/onboarding.h
index 2fdae35de0..a1b6895ba0 100644
--- a/selfdrive/ui/qt/offroad/onboarding.h
+++ b/selfdrive/ui/qt/offroad/onboarding.h
@@ -63,7 +63,7 @@ class TermsPage : public QFrame {
   Q_OBJECT
 
 public:
-  explicit TermsPage(QWidget *parent = 0) : QFrame(parent) {};
+  explicit TermsPage(QWidget *parent = 0) : QFrame(parent) {}
 
 public slots:
   void enableAccept();
@@ -82,7 +82,7 @@ class DeclinePage : public QFrame {
   Q_OBJECT
 
 public:
-  explicit DeclinePage(QWidget *parent = 0) : QFrame(parent) {};
+  explicit DeclinePage(QWidget *parent = 0) : QFrame(parent) {}
 
 private:
   void showEvent(QShowEvent *event) override;
diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc
index a23683e047..669de5eae9 100644
--- a/selfdrive/ui/qt/offroad/settings.cc
+++ b/selfdrive/ui/qt/offroad/settings.cc
@@ -3,10 +3,12 @@
 #include <cassert>
 #include <cmath>
 #include <string>
+#include <tuple>
+#include <vector>
 
 #include <QDebug>
 
-#include "selfdrive/ui/qt/offroad/networking.h"
+#include "selfdrive/ui/qt/network/networking.h"
 
 #include "common/params.h"
 #include "common/watchdog.h"
@@ -20,7 +22,6 @@
 #include "selfdrive/ui/ui.h"
 #include "selfdrive/ui/qt/util.h"
 #include "selfdrive/ui/qt/qt_window.h"
-#include "selfdrive/ui/qt/widgets/input.h"
 
 TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
   // param, title, desc, icon
@@ -36,7 +37,8 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
       tr("openpilot Longitudinal Control (Alpha)"),
       QString("<b>%1</b><br><br>%2")
       .arg(tr("WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB)."))
-      .arg(tr("On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha.")),
+      .arg(tr("On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. "
+              "Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha.")),
       "../assets/offroad/icon_speed_limit.png",
     },
     {
@@ -88,7 +90,8 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
 
   std::vector<QString> longi_button_texts{tr("Aggressive"), tr("Standard"), tr("Relaxed")};
   long_personality_setting = new ButtonParamControl("LongitudinalPersonality", tr("Driving Personality"),
-                                          tr("Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars."),
+                                          tr("Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. "
+                                             "In relaxed mode openpilot will stay further away from lead cars."),
                                           "../assets/offroad/icon_speed_limit.png",
                                           longi_button_texts);
   for (auto &[param, title, desc, icon] : toggle_defs) {
@@ -135,16 +138,17 @@ void TogglesPanel::updateToggles() {
                                           "<h4>%6</h4><br>"
                                           "%7")
                                   .arg(tr("openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below:"))
-                                  .arg(tr("End-to-End Longitudinal Control" ))
+                                  .arg(tr("End-to-End Longitudinal Control"))
                                   .arg(tr("Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. "
-                                       "Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected."))
+                                          "Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; "
+                                          "mistakes should be expected."))
                                   .arg(tr("Navigate on openpilot"))
-                                  .arg(tr("When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. "
-                                          "Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits/forks."
-					  "These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc."))
+                                  .arg(tr("When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right "
+                                          "appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around "
+                                          "exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc."))
                                   .arg(tr("New Driving Visualization"))
-                                  .arg(tr("The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner."
-				          "When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green."));
+                                  .arg(tr("The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. "
+                                          "When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green."));
 
   const bool is_release = params.getBool("IsReleaseBranch");
   auto cp_bytes = params.get("CarParamsPersistent");
@@ -279,7 +283,7 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) {
 void DevicePanel::updateCalibDescription() {
   QString desc =
       tr("openpilot requires the device to be mounted within 4° left or right and "
-         "within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required.");
+         "within 5° up or 9° down. openpilot is continuously calibrating, resetting is rarely required.");
   std::string calib_bytes = params.get("CalibrationParams");
   if (!calib_bytes.empty()) {
     try {
@@ -345,10 +349,6 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) {
   QVBoxLayout *sidebar_layout = new QVBoxLayout(sidebar_widget);
   sidebar_layout->setMargin(0);
   panel_widget = new QStackedWidget();
-  panel_widget->setStyleSheet(R"(
-    border-radius: 30px;
-    background-color: #292929;
-  )");
 
   // close button
   QPushButton *close_btn = new QPushButton(tr("×"));
@@ -437,5 +437,9 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) {
     SettingsWindow {
       background-color: black;
     }
+    QStackedWidget, ScrollView {
+      background-color: #292929;
+      border-radius: 30px;
+    }
   )");
 }
diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h
index edba5be800..a5dd25b14f 100644
--- a/selfdrive/ui/qt/offroad/settings.h
+++ b/selfdrive/ui/qt/offroad/settings.h
@@ -1,5 +1,8 @@
 #pragma once
 
+#include <map>
+#include <string>
+
 #include <QButtonGroup>
 #include <QFrame>
 #include <QLabel>
diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc
index 0c126304de..971eb673e2 100644
--- a/selfdrive/ui/qt/onroad.cc
+++ b/selfdrive/ui/qt/onroad.cc
@@ -1,6 +1,10 @@
 #include "selfdrive/ui/qt/onroad.h"
 
+#include <algorithm>
 #include <cmath>
+#include <map>
+#include <memory>
+#include <sstream>
 
 #include <QDebug>
 #include <QMouseEvent>
@@ -12,6 +16,17 @@
 #include "selfdrive/ui/qt/maps/map_panel.h"
 #endif
 
+static void drawIcon(QPainter &p, const QPoint &center, const QPixmap &img, const QBrush &bg, float opacity) {
+  p.setRenderHint(QPainter::Antialiasing);
+  p.setOpacity(1.0);  // bg dictates opacity of ellipse
+  p.setPen(Qt::NoPen);
+  p.setBrush(bg);
+  p.drawEllipse(center, btn_size / 2, btn_size / 2);
+  p.setOpacity(opacity);
+  p.drawPixmap(center - QPoint(img.width() / 2, img.height() / 2), img);
+  p.setOpacity(1.0);
+}
+
 OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) {
   QVBoxLayout *main_layout  = new QVBoxLayout(this);
   main_layout->setMargin(UI_BORDER_SIZE);
@@ -49,9 +64,14 @@ OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) {
   setAttribute(Qt::WA_OpaquePaintEvent);
   QObject::connect(uiState(), &UIState::uiUpdate, this, &OnroadWindow::updateState);
   QObject::connect(uiState(), &UIState::offroadTransition, this, &OnroadWindow::offroadTransition);
+  QObject::connect(uiState(), &UIState::primeChanged, this, &OnroadWindow::primeChanged);
 }
 
 void OnroadWindow::updateState(const UIState &s) {
+  if (!s.scene.started) {
+    return;
+  }
+
   QColor bgColor = bg_colors[s.status];
   Alert alert = Alert::get(*(s.sm), s.scene.started_frame);
   alerts->updateAlert(alert);
@@ -87,7 +107,7 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
 void OnroadWindow::offroadTransition(bool offroad) {
 #ifdef ENABLE_MAPS
   if (!offroad) {
-    if (map == nullptr && (uiState()->primeType() || !MAPBOX_TOKEN.isEmpty())) {
+    if (map == nullptr && (uiState()->hasPrime() || !MAPBOX_TOKEN.isEmpty())) {
       auto m = new MapPanel(get_mapbox_settings());
       map = m;
 
@@ -107,6 +127,17 @@ void OnroadWindow::offroadTransition(bool offroad) {
   alerts->updateAlert({});
 }
 
+void OnroadWindow::primeChanged(bool prime) {
+#ifdef ENABLE_MAPS
+  if (map && (!prime && MAPBOX_TOKEN.isEmpty())) {
+    nvg->map_settings_btn->setEnabled(false);
+    nvg->map_settings_btn->setVisible(false);
+    map->deleteLater();
+    map = nullptr;
+  }
+#endif
+}
+
 void OnroadWindow::paintEvent(QPaintEvent *event) {
   QPainter p(this);
   p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255));
@@ -183,7 +214,6 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) {
 ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(false), engageable(false), QPushButton(parent) {
   setFixedSize(btn_size, btn_size);
 
-  params = Params();
   engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size});
   experimental_img = loadPixmap("../assets/img_experimental.svg", {img_size, img_size});
   QObject::connect(this, &QPushButton::clicked, this, &ExperimentalButton::changeMode);
@@ -209,17 +239,8 @@ void ExperimentalButton::updateState(const UIState &s) {
 
 void ExperimentalButton::paintEvent(QPaintEvent *event) {
   QPainter p(this);
-  p.setRenderHint(QPainter::Antialiasing);
-
-  QPoint center(btn_size / 2, btn_size / 2);
   QPixmap img = experimental_mode ? experimental_img : engage_img;
-
-  p.setOpacity(1.0);
-  p.setPen(Qt::NoPen);
-  p.setBrush(QColor(0, 0, 0, 166));
-  p.drawEllipse(center, btn_size / 2, btn_size / 2);
-  p.setOpacity((isDown() || !engageable) ? 0.6 : 1.0);
-  p.drawPixmap((btn_size - img_size) / 2, (btn_size - img_size) / 2, img);
+  drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, QColor(0, 0, 0, 166), (isDown() || !engageable) ? 0.6 : 1.0);
 }
 
 
@@ -235,16 +256,7 @@ MapSettingsButton::MapSettingsButton(QWidget *parent) : QPushButton(parent) {
 
 void MapSettingsButton::paintEvent(QPaintEvent *event) {
   QPainter p(this);
-  p.setRenderHint(QPainter::Antialiasing);
-
-  QPoint center(btn_size / 2, btn_size / 2);
-
-  p.setOpacity(1.0);
-  p.setPen(Qt::NoPen);
-  p.setBrush(QColor(0, 0, 0, 166));
-  p.drawEllipse(center, btn_size / 2, btn_size / 2);
-  p.setOpacity(isDown() ? 0.6 : 1.0);
-  p.drawPixmap((btn_size - img_size) / 2, (btn_size - img_size) / 2, settings_img);
+  drawIcon(p, QPoint(btn_size / 2, btn_size / 2), settings_img, QColor(0, 0, 0, 166), isDown() ? 0.6 : 1.0);
 }
 
 
@@ -271,51 +283,42 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
 
   const bool cs_alive = sm.alive("controlsState");
   const bool nav_alive = sm.alive("navInstruction") && sm["navInstruction"].getValid();
-
   const auto cs = sm["controlsState"].getControlsState();
+  const auto car_state = sm["carState"].getCarState();
+  const auto nav_instruction = sm["navInstruction"].getNavInstruction();
 
   // Handle older routes where vCruiseCluster is not set
   float v_cruise =  cs.getVCruiseCluster() == 0.0 ? cs.getVCruise() : cs.getVCruiseCluster();
-  float set_speed = cs_alive ? v_cruise : SET_SPEED_NA;
-  bool cruise_set = set_speed > 0 && (int)set_speed != SET_SPEED_NA;
-  if (cruise_set && !s.scene.is_metric) {
-    set_speed *= KM_TO_MILE;
+  setSpeed = cs_alive ? v_cruise : SET_SPEED_NA;
+  is_cruise_set = setSpeed > 0 && (int)setSpeed != SET_SPEED_NA;
+  if (is_cruise_set && !s.scene.is_metric) {
+    setSpeed *= KM_TO_MILE;
   }
 
   // Handle older routes where vEgoCluster is not set
-  float v_ego;
-  if (sm["carState"].getCarState().getVEgoCluster() == 0.0 && !v_ego_cluster_seen) {
-    v_ego = sm["carState"].getCarState().getVEgo();
-  } else {
-    v_ego = sm["carState"].getCarState().getVEgoCluster();
-    v_ego_cluster_seen = true;
-  }
-  float cur_speed = cs_alive ? std::max<float>(0.0, v_ego) : 0.0;
-  cur_speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH;
-
-  auto speed_limit_sign = sm["navInstruction"].getNavInstruction().getSpeedLimitSign();
-  float speed_limit = nav_alive ? sm["navInstruction"].getNavInstruction().getSpeedLimit() : 0.0;
-  speed_limit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
-
-  setProperty("speedLimit", speed_limit);
-  setProperty("has_us_speed_limit", nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD);
-  setProperty("has_eu_speed_limit", nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA);
-
-  setProperty("is_cruise_set", cruise_set);
-  setProperty("is_metric", s.scene.is_metric);
-  setProperty("speed", cur_speed);
-  setProperty("setSpeed", set_speed);
-  setProperty("speedUnit", s.scene.is_metric ? tr("km/h") : tr("mph"));
-  setProperty("hideBottomIcons", (cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE));
-  setProperty("status", s.status);
+  v_ego_cluster_seen = v_ego_cluster_seen || car_state.getVEgoCluster() != 0.0;
+  float v_ego = v_ego_cluster_seen ? car_state.getVEgoCluster() : car_state.getVEgo();
+  speed = cs_alive ? std::max<float>(0.0, v_ego) : 0.0;
+  speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH;
+
+  auto speed_limit_sign = nav_instruction.getSpeedLimitSign();
+  speedLimit = nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
+  speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
+
+  has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD);
+  has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA);
+  is_metric = s.scene.is_metric;
+  speedUnit =  s.scene.is_metric ? tr("km/h") : tr("mph");
+  hideBottomIcons = (cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE);
+  status = s.status;
 
   // update engageability/experimental mode button
   experimental_btn->updateState(s);
 
   // update DM icon
   auto dm_state = sm["driverMonitoringState"].getDriverMonitoringState();
-  setProperty("dmActive", dm_state.getIsActiveMode());
-  setProperty("rightHandDM", dm_state.getIsRHD());
+  dmActive = dm_state.getIsActiveMode();
+  rightHandDM = dm_state.getIsRHD();
   // DM icon transition
   dm_fade_state = std::clamp(dm_fade_state+0.2*(0.5-dmActive), 0.0, 1.0);
 
@@ -432,16 +435,6 @@ void AnnotatedCameraWidget::drawText(QPainter &p, int x, int y, const QString &t
   p.drawText(real_rect.x(), real_rect.bottom(), text);
 }
 
-void AnnotatedCameraWidget::drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, float opacity) {
-  p.setOpacity(1.0);  // bg dictates opacity of ellipse
-  p.setPen(Qt::NoPen);
-  p.setBrush(bg);
-  p.drawEllipse(x - btn_size / 2, y - btn_size / 2, btn_size, btn_size);
-  p.setOpacity(opacity);
-  p.drawPixmap(x - img.size().width() / 2, y - img.size().height() / 2, img);
-  p.setOpacity(1.0);
-}
-
 void AnnotatedCameraWidget::initializeGL() {
   CameraWidget::initializeGL();
   qInfo() << "OpenGL version:" << QString((const char*)glGetString(GL_VERSION));
@@ -540,7 +533,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s)
   int x = rightHandDM ? width() - offset : offset;
   int y = height() - offset;
   float opacity = dmActive ? 0.65 : 0.2;
-  drawIcon(painter, x, y, dm_img, blackColor(70), opacity);
+  drawIcon(painter, QPoint(x, y), dm_img, blackColor(70), opacity);
 
   // face
   QPointF face_kpts_draw[std::size(default_face_kpts_3d)];
diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h
index 0dd95877a0..b3ba411453 100644
--- a/selfdrive/ui/qt/onroad.h
+++ b/selfdrive/ui/qt/onroad.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <memory>
+
 #include <QPushButton>
 #include <QStackedLayout>
 #include <QWidget>
@@ -18,7 +20,7 @@ class OnroadAlerts : public QWidget {
   Q_OBJECT
 
 public:
-  OnroadAlerts(QWidget *parent = 0) : QWidget(parent) {};
+  OnroadAlerts(QWidget *parent = 0) : QWidget(parent) {}
   void updateAlert(const Alert &a);
 
 protected:
@@ -63,19 +65,6 @@ private:
 // container window for the NVG UI
 class AnnotatedCameraWidget : public CameraWidget {
   Q_OBJECT
-  Q_PROPERTY(float speed MEMBER speed);
-  Q_PROPERTY(QString speedUnit MEMBER speedUnit);
-  Q_PROPERTY(float setSpeed MEMBER setSpeed);
-  Q_PROPERTY(float speedLimit MEMBER speedLimit);
-  Q_PROPERTY(bool is_cruise_set MEMBER is_cruise_set);
-  Q_PROPERTY(bool has_eu_speed_limit MEMBER has_eu_speed_limit);
-  Q_PROPERTY(bool has_us_speed_limit MEMBER has_us_speed_limit);
-  Q_PROPERTY(bool is_metric MEMBER is_metric);
-
-  Q_PROPERTY(bool dmActive MEMBER dmActive);
-  Q_PROPERTY(bool hideBottomIcons MEMBER hideBottomIcons);
-  Q_PROPERTY(bool rightHandDM MEMBER rightHandDM);
-  Q_PROPERTY(int status MEMBER status);
 
 public:
   explicit AnnotatedCameraWidget(VisionStreamType type, QWidget* parent = 0);
@@ -84,7 +73,6 @@ public:
   MapSettingsButton *map_settings_btn;
 
 private:
-  void drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, float opacity);
   void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255);
 
   QVBoxLayout *main_layout;
@@ -149,5 +137,6 @@ private:
 
 private slots:
   void offroadTransition(bool offroad);
+  void primeChanged(bool prime);
   void updateState(const UIState &s);
 };
diff --git a/selfdrive/ui/qt/setup/setup.cc b/selfdrive/ui/qt/setup/setup.cc
index de5021c8bc..3b3666b19a 100644
--- a/selfdrive/ui/qt/setup/setup.cc
+++ b/selfdrive/ui/qt/setup/setup.cc
@@ -3,6 +3,7 @@
 #include <cstdio>
 #include <cstdlib>
 #include <sstream>
+#include <string>
 
 #include <QApplication>
 #include <QLabel>
@@ -14,7 +15,8 @@
 #include "system/hardware/hw.h"
 #include "selfdrive/ui/qt/api.h"
 #include "selfdrive/ui/qt/qt_window.h"
-#include "selfdrive/ui/qt/offroad/networking.h"
+#include "selfdrive/ui/qt/network/networking.h"
+#include "selfdrive/ui/qt/util.h"
 #include "selfdrive/ui/qt/widgets/input.h"
 
 const std::string USER_AGENT = "AGNOSSetup-";
@@ -306,6 +308,10 @@ void Setup::nextPage() {
 }
 
 Setup::Setup(QWidget *parent) : QStackedWidget(parent) {
+  if (std::getenv("MULTILANG")) {
+    selectLanguage();
+  }
+
   std::stringstream buffer;
   buffer << std::ifstream("/sys/class/hwmon/hwmon1/in1_input").rdbuf();
   float voltage = (float)std::atoi(buffer.str().c_str()) / 1000.;
@@ -368,6 +374,18 @@ Setup::Setup(QWidget *parent) : QStackedWidget(parent) {
   )");
 }
 
+void Setup::selectLanguage() {
+  QMap<QString, QString> langs = getSupportedLanguages();
+  QString selection = MultiOptionDialog::getSelection(tr("Select a language"), langs.keys(), "", this);
+  if (!selection.isEmpty()) {
+    QString selectedLang = langs[selection];
+    Params().put("LanguageSetting", selectedLang.toStdString());
+    if (translator.load(":/" + selectedLang)) {
+      qApp->installTranslator(&translator);
+    }
+  }
+}
+
 int main(int argc, char *argv[]) {
   QApplication a(argc, argv);
   Setup setup;
diff --git a/selfdrive/ui/qt/setup/setup.h b/selfdrive/ui/qt/setup/setup.h
index bf5d97070d..8c33acc380 100644
--- a/selfdrive/ui/qt/setup/setup.h
+++ b/selfdrive/ui/qt/setup/setup.h
@@ -3,6 +3,7 @@
 #include <QLabel>
 #include <QStackedWidget>
 #include <QString>
+#include <QTranslator>
 #include <QWidget>
 
 class Setup : public QStackedWidget {
@@ -12,6 +13,7 @@ public:
   explicit Setup(QWidget *parent = 0);
 
 private:
+  void selectLanguage();
   QWidget *low_voltage();
   QWidget *getting_started();
   QWidget *network_setup();
@@ -20,6 +22,7 @@ private:
 
   QWidget *failed_widget;
   QWidget *downloading_widget;
+  QTranslator translator;
 
 signals:
   void finished(const QString &url, const QString &error = "");
diff --git a/selfdrive/ui/qt/setup/updater.cc b/selfdrive/ui/qt/setup/updater.cc
index ae5f26c77e..ed47590aa3 100644
--- a/selfdrive/ui/qt/setup/updater.cc
+++ b/selfdrive/ui/qt/setup/updater.cc
@@ -6,7 +6,7 @@
 #include "selfdrive/ui/qt/util.h"
 #include "selfdrive/ui/qt/qt_window.h"
 #include "selfdrive/ui/qt/setup/updater.h"
-#include "selfdrive/ui/qt/offroad/networking.h"
+#include "selfdrive/ui/qt/network/networking.h"
 
 Updater::Updater(const QString &updater_path, const QString &manifest_path, QWidget *parent)
   : updater(updater_path), manifest(manifest_path), QStackedWidget(parent) {
diff --git a/selfdrive/ui/qt/sidebar.cc b/selfdrive/ui/qt/sidebar.cc
index 15f7c47607..e952940056 100644
--- a/selfdrive/ui/qt/sidebar.cc
+++ b/selfdrive/ui/qt/sidebar.cc
@@ -84,7 +84,9 @@ void Sidebar::updateState(const UIState &s) {
   if (last_ping == 0) {
     connectStatus = ItemStatus{{tr("CONNECT"), tr("OFFLINE")}, warning_color};
   } else {
-    connectStatus = nanos_since_boot() - last_ping < 80e9 ? ItemStatus{{tr("CONNECT"), tr("ONLINE")}, good_color} : ItemStatus{{tr("CONNECT"), tr("ERROR")}, danger_color};
+    connectStatus = nanos_since_boot() - last_ping < 80e9
+                        ? ItemStatus{{tr("CONNECT"), tr("ONLINE")}, good_color}
+                        : ItemStatus{{tr("CONNECT"), tr("ERROR")}, danger_color};
   }
   setProperty("connectStatus", QVariant::fromValue(connectStatus));
 
diff --git a/selfdrive/ui/qt/sidebar.h b/selfdrive/ui/qt/sidebar.h
index fb96e1d540..f627aac810 100644
--- a/selfdrive/ui/qt/sidebar.h
+++ b/selfdrive/ui/qt/sidebar.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <memory>
+
 #include <QFrame>
 #include <QMap>
 
diff --git a/selfdrive/ui/qt/spinner.cc b/selfdrive/ui/qt/spinner.cc
index 8f13576fb2..2404efa668 100644
--- a/selfdrive/ui/qt/spinner.cc
+++ b/selfdrive/ui/qt/spinner.cc
@@ -1,5 +1,6 @@
 #include "selfdrive/ui/qt/spinner.h"
 
+#include <algorithm>
 #include <cstdio>
 #include <iostream>
 #include <string>
@@ -93,7 +94,7 @@ Spinner::Spinner(QWidget *parent) : QWidget(parent) {
 
   notifier = new QSocketNotifier(fileno(stdin), QSocketNotifier::Read);
   QObject::connect(notifier, &QSocketNotifier::activated, this, &Spinner::update);
-};
+}
 
 void Spinner::update(int n) {
   std::string line;
diff --git a/selfdrive/ui/qt/util.cc b/selfdrive/ui/qt/util.cc
index a2926548ed..78da183503 100644
--- a/selfdrive/ui/qt/util.cc
+++ b/selfdrive/ui/qt/util.cc
@@ -1,5 +1,9 @@
 #include "selfdrive/ui/qt/util.h"
 
+#include <map>
+#include <string>
+#include <vector>
+
 #include <QApplication>
 #include <QFile>
 #include <QFileInfo>
@@ -39,7 +43,7 @@ std::optional<QString> getDongleId() {
 }
 
 QMap<QString, QString> getSupportedLanguages() {
-  QFile f("translations/languages.json");
+  QFile f(":/languages.json");
   f.open(QIODevice::ReadOnly | QIODevice::Text);
   QString val = f.readAll();
 
@@ -51,19 +55,6 @@ QMap<QString, QString> getSupportedLanguages() {
   return map;
 }
 
-void clearLayout(QLayout* layout) {
-  while (layout->count() > 0) {
-    QLayoutItem* item = layout->takeAt(0);
-    if (QWidget* widget = item->widget()) {
-      widget->deleteLater();
-    }
-    if (QLayout* childLayout = item->layout()) {
-      clearLayout(childLayout);
-    }
-    delete item;
-  }
-}
-
 QString timeAgo(const QDateTime &date) {
   int diff = date.secsTo(QDateTime::currentDateTimeUtc());
 
@@ -143,7 +134,7 @@ void swagLogMessageHandler(QtMsgType type, const QMessageLogContext &context, co
 }
 
 
-QWidget* topWidget (QWidget* widget) {
+QWidget* topWidget(QWidget* widget) {
   while (widget->parentWidget() != nullptr) widget=widget->parentWidget();
   return widget;
 }
@@ -206,8 +197,7 @@ QColor interpColor(float xv, std::vector<float> xp, std::vector<QColor> fp) {
       (xv - xp[low]) * (fp[hi].red() - fp[low].red()) / (xp[hi] - xp[low]) + fp[low].red(),
       (xv - xp[low]) * (fp[hi].green() - fp[low].green()) / (xp[hi] - xp[low]) + fp[low].green(),
       (xv - xp[low]) * (fp[hi].blue() - fp[low].blue()) / (xp[hi] - xp[low]) + fp[low].blue(),
-      (xv - xp[low]) * (fp[hi].alpha() - fp[low].alpha()) / (xp[hi] - xp[low]) + fp[low].alpha()
-    );
+      (xv - xp[low]) * (fp[hi].alpha() - fp[low].alpha()) / (xp[hi] - xp[low]) + fp[low].alpha());
   }
 }
 
diff --git a/selfdrive/ui/qt/util.h b/selfdrive/ui/qt/util.h
index 68c7bd2712..2aae97b860 100644
--- a/selfdrive/ui/qt/util.h
+++ b/selfdrive/ui/qt/util.h
@@ -1,10 +1,10 @@
 #pragma once
 
 #include <optional>
+#include <vector>
 
 #include <QDateTime>
 #include <QFileSystemWatcher>
-#include <QLayout>
 #include <QPainter>
 #include <QPixmap>
 #include <QSurfaceFormat>
@@ -18,13 +18,12 @@ QString getBrand();
 QString getUserAgent();
 std::optional<QString> getDongleId();
 QMap<QString, QString> getSupportedLanguages();
-void clearLayout(QLayout* layout);
 void setQtSurfaceFormat();
 void sigTermHandler(int s);
 QString timeAgo(const QDateTime &date);
 void swagLogMessageHandler(QtMsgType type, const QMessageLogContext &context, const QString &msg);
 void initApp(int argc, char *argv[], bool disable_hidpi = true);
-QWidget* topWidget (QWidget* widget);
+QWidget* topWidget(QWidget* widget);
 QPixmap loadPixmap(const QString &fileName, const QSize &size = {}, Qt::AspectRatioMode aspectRatioMode = Qt::KeepAspectRatio);
 QPixmap bootstrapPixmap(const QString &id);
 
diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc
index 4da5b7b18e..6a5064e8ca 100644
--- a/selfdrive/ui/qt/widgets/cameraview.cc
+++ b/selfdrive/ui/qt/widgets/cameraview.cc
@@ -7,6 +7,9 @@
 #endif
 
 #include <cmath>
+#include <set>
+#include <string>
+#include <utility>
 
 #include <QOpenGLBuffer>
 #include <QOffscreenSurface>
@@ -202,7 +205,9 @@ void CameraWidget::updateFrameMat() {
 
   if (zoomed_view) {
     if (active_stream_type == VISION_STREAM_DRIVER) {
-      frame_mat = get_driver_view_transform(w, h, stream_width, stream_height);
+      if (stream_width > 0 && stream_height > 0) {
+        frame_mat = get_driver_view_transform(w, h, stream_width, stream_height);
+      }
     } else {
       // Project point at "infinity" to compute x and y offsets
       // to ensure this ends up in the middle of the screen
diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h
index 67568ea55c..fcd5b1b18f 100644
--- a/selfdrive/ui/qt/widgets/cameraview.h
+++ b/selfdrive/ui/qt/widgets/cameraview.h
@@ -1,7 +1,12 @@
 #pragma once
 
+#include <deque>
+#include <map>
 #include <memory>
 #include <mutex>
+#include <set>
+#include <string>
+#include <utility>
 
 #include <QOpenGLFunctions>
 #include <QOpenGLShaderProgram>
@@ -60,7 +65,7 @@ protected:
   bool zoomed_view;
   GLuint frame_vao, frame_vbo, frame_ibo;
   GLuint textures[2];
-  mat4 frame_mat;
+  mat4 frame_mat = {};
   std::unique_ptr<QOpenGLShaderProgram> program;
   QColor bg = QColor("#000000");
 
diff --git a/selfdrive/ui/qt/widgets/controls.cc b/selfdrive/ui/qt/widgets/controls.cc
index e440bc6441..87304a4585 100644
--- a/selfdrive/ui/qt/widgets/controls.cc
+++ b/selfdrive/ui/qt/widgets/controls.cc
@@ -23,7 +23,7 @@ AbstractControl::AbstractControl(const QString &title, const QString &desc, cons
   // title
   title_label = new QPushButton(title);
   title_label->setFixedHeight(120);
-  title_label->setStyleSheet("font-size: 50px; font-weight: 400; text-align: left");
+  title_label->setStyleSheet("font-size: 50px; font-weight: 400; text-align: left; border: none;");
   hlayout->addWidget(title_label, 1);
 
   // value next to control button
diff --git a/selfdrive/ui/qt/widgets/controls.h b/selfdrive/ui/qt/widgets/controls.h
index fac66de9ed..811595726d 100644
--- a/selfdrive/ui/qt/widgets/controls.h
+++ b/selfdrive/ui/qt/widgets/controls.h
@@ -1,5 +1,8 @@
 #pragma once
 
+#include <string>
+#include <vector>
+
 #include <QButtonGroup>
 #include <QFrame>
 #include <QHBoxLayout>
diff --git a/selfdrive/ui/qt/widgets/drive_stats.cc b/selfdrive/ui/qt/widgets/drive_stats.cc
deleted file mode 100644
index 31009f03ca..0000000000
--- a/selfdrive/ui/qt/widgets/drive_stats.cc
+++ /dev/null
@@ -1,97 +0,0 @@
-#include "selfdrive/ui/qt/widgets/drive_stats.h"
-
-#include <QDebug>
-#include <QGridLayout>
-#include <QJsonObject>
-#include <QVBoxLayout>
-
-#include "common/params.h"
-#include "selfdrive/ui/qt/request_repeater.h"
-#include "selfdrive/ui/qt/util.h"
-
-static QLabel* newLabel(const QString& text, const QString &type) {
-  QLabel* label = new QLabel(text);
-  label->setProperty("type", type);
-  return label;
-}
-
-DriveStats::DriveStats(QWidget* parent) : QFrame(parent) {
-  metric_ = Params().getBool("IsMetric");
-
-  QVBoxLayout* main_layout = new QVBoxLayout(this);
-  main_layout->setContentsMargins(50, 50, 50, 60);
-
-  auto add_stats_layouts = [=](const QString &title, StatsLabels& labels) {
-    QGridLayout* grid_layout = new QGridLayout;
-    grid_layout->setVerticalSpacing(10);
-    grid_layout->setContentsMargins(0, 10, 0, 10);
-
-    int row = 0;
-    grid_layout->addWidget(newLabel(title, "title"), row++, 0, 1, 3);
-    grid_layout->addItem(new QSpacerItem(0, 50), row++, 0, 1, 1);
-
-    grid_layout->addWidget(labels.routes = newLabel("0", "number"), row, 0, Qt::AlignLeft);
-    grid_layout->addWidget(labels.distance = newLabel("0", "number"), row, 1, Qt::AlignLeft);
-    grid_layout->addWidget(labels.hours = newLabel("0", "number"), row, 2, Qt::AlignLeft);
-
-    grid_layout->addWidget(newLabel((tr("Drives")), "unit"), row + 1, 0, Qt::AlignLeft);
-    grid_layout->addWidget(labels.distance_unit = newLabel(getDistanceUnit(), "unit"), row + 1, 1, Qt::AlignLeft);
-    grid_layout->addWidget(newLabel(tr("Hours"), "unit"), row + 1, 2, Qt::AlignLeft);
-
-    main_layout->addLayout(grid_layout);
-  };
-
-  add_stats_layouts(tr("ALL TIME"), all_);
-  main_layout->addStretch();
-  add_stats_layouts(tr("PAST WEEK"), week_);
-
-  if (auto dongleId = getDongleId()) {
-    QString url = CommaApi::BASE_URL + "/v1.1/devices/" + *dongleId + "/stats";
-    RequestRepeater* repeater = new RequestRepeater(this, url, "ApiCache_DriveStats", 30);
-    QObject::connect(repeater, &RequestRepeater::requestDone, this, &DriveStats::parseResponse);
-  }
-
-  setStyleSheet(R"(
-    DriveStats {
-      background-color: #333333;
-      border-radius: 10px;
-    }
-
-    QLabel[type="title"] { font-size: 51px; font-weight: 500; }
-    QLabel[type="number"] { font-size: 78px; font-weight: 500; }
-    QLabel[type="unit"] { font-size: 51px; font-weight: 300; color: #A0A0A0; }
-  )");
-}
-
-void DriveStats::updateStats() {
-  auto update = [=](const QJsonObject& obj, StatsLabels& labels) {
-    labels.routes->setText(QString::number((int)obj["routes"].toDouble()));
-    labels.distance->setText(QString::number(int(obj["distance"].toDouble() * (metric_ ? MILE_TO_KM : 1))));
-    labels.distance_unit->setText(getDistanceUnit());
-    labels.hours->setText(QString::number((int)(obj["minutes"].toDouble() / 60)));
-  };
-
-  QJsonObject json = stats_.object();
-  update(json["all"].toObject(), all_);
-  update(json["week"].toObject(), week_);
-}
-
-void DriveStats::parseResponse(const QString& response, bool success) {
-  if (!success) return;
-
-  QJsonDocument doc = QJsonDocument::fromJson(response.trimmed().toUtf8());
-  if (doc.isNull()) {
-    qDebug() << "JSON Parse failed on getting past drives statistics";
-    return;
-  }
-  stats_ = doc;
-  updateStats();
-}
-
-void DriveStats::showEvent(QShowEvent* event) {
-  bool metric = Params().getBool("IsMetric");
-  if (metric_ != metric) {
-    metric_ = metric;
-    updateStats();
-  }
-}
diff --git a/selfdrive/ui/qt/widgets/drive_stats.h b/selfdrive/ui/qt/widgets/drive_stats.h
deleted file mode 100644
index 5e2d96b240..0000000000
--- a/selfdrive/ui/qt/widgets/drive_stats.h
+++ /dev/null
@@ -1,25 +0,0 @@
-#pragma once
-
-#include <QJsonDocument>
-#include <QLabel>
-
-class DriveStats : public QFrame {
-  Q_OBJECT
-
-public:
-  explicit DriveStats(QWidget* parent = 0);
-
-private:
-  void showEvent(QShowEvent *event) override;
-  void updateStats();
-  inline QString getDistanceUnit() const { return metric_ ? tr("KM") : tr("Miles"); }
-
-  bool metric_;
-  QJsonDocument stats_;
-  struct StatsLabels {
-    QLabel *routes, *distance, *distance_unit, *hours;
-  } all_, week_;
-
-private slots:
-  void parseResponse(const QString &response, bool success);
-};
diff --git a/selfdrive/ui/qt/widgets/input.cc b/selfdrive/ui/qt/widgets/input.cc
index 49fbdff222..52d0c5cd6e 100644
--- a/selfdrive/ui/qt/widgets/input.cc
+++ b/selfdrive/ui/qt/widgets/input.cc
@@ -9,7 +9,7 @@
 #include "selfdrive/ui/qt/widgets/scrollview.h"
 
 
-QDialogBase::QDialogBase(QWidget *parent) : QDialog(parent) {
+DialogBase::DialogBase(QWidget *parent) : QDialog(parent) {
   Q_ASSERT(parent != nullptr);
   parent->installEventFilter(this);
 
@@ -19,7 +19,7 @@ QDialogBase::QDialogBase(QWidget *parent) : QDialog(parent) {
       color: white;
       font-family: Inter;
     }
-    QDialogBase {
+    DialogBase {
       background-color: black;
     }
     QPushButton {
@@ -36,19 +36,19 @@ QDialogBase::QDialogBase(QWidget *parent) : QDialog(parent) {
   )");
 }
 
-bool QDialogBase::eventFilter(QObject *o, QEvent *e) {
+bool DialogBase::eventFilter(QObject *o, QEvent *e) {
   if (o == parent() && e->type() == QEvent::Hide) {
     reject();
   }
   return QDialog::eventFilter(o, e);
 }
 
-int QDialogBase::exec() {
+int DialogBase::exec() {
   setMainWindow(this);
   return QDialog::exec();
 }
 
-InputDialog::InputDialog(const QString &title, QWidget *parent, const QString &subtitle, bool secret) : QDialogBase(parent) {
+InputDialog::InputDialog(const QString &title, QWidget *parent, const QString &subtitle, bool secret) : DialogBase(parent) {
   main_layout = new QVBoxLayout(this);
   main_layout->setContentsMargins(50, 55, 50, 50);
   main_layout->setSpacing(0);
@@ -188,7 +188,7 @@ void InputDialog::setMinLength(int length) {
 // ConfirmationDialog
 
 ConfirmationDialog::ConfirmationDialog(const QString &prompt_text, const QString &confirm_text, const QString &cancel_text,
-                                       const bool rich, QWidget *parent) : QDialogBase(parent) {
+                                       const bool rich, QWidget *parent) : DialogBase(parent) {
   QFrame *container = new QFrame(this);
   container->setStyleSheet(R"(
     QFrame { background-color: #1B1B1B; color: #C9C9C9; }
@@ -245,7 +245,7 @@ bool ConfirmationDialog::rich(const QString &prompt_text, QWidget *parent) {
 
 // MultiOptionDialog
 
-MultiOptionDialog::MultiOptionDialog(const QString &prompt_text, const QStringList &l, const QString &current, QWidget *parent) : QDialogBase(parent) {
+MultiOptionDialog::MultiOptionDialog(const QString &prompt_text, const QStringList &l, const QString &current, QWidget *parent) : DialogBase(parent) {
   QFrame *container = new QFrame(this);
   container->setStyleSheet(R"(
     QFrame { background-color: #1B1B1B; }
diff --git a/selfdrive/ui/qt/widgets/input.h b/selfdrive/ui/qt/widgets/input.h
index e6c0fba86d..089e54e4a0 100644
--- a/selfdrive/ui/qt/widgets/input.h
+++ b/selfdrive/ui/qt/widgets/input.h
@@ -10,23 +10,23 @@
 #include "selfdrive/ui/qt/widgets/keyboard.h"
 
 
-class QDialogBase : public QDialog {
+class DialogBase : public QDialog {
   Q_OBJECT
 
 protected:
-  QDialogBase(QWidget *parent);
+  DialogBase(QWidget *parent);
   bool eventFilter(QObject *o, QEvent *e) override;
 
 public slots:
   int exec() override;
 };
 
-class InputDialog : public QDialogBase {
+class InputDialog : public DialogBase {
   Q_OBJECT
 
 public:
   explicit InputDialog(const QString &title, QWidget *parent, const QString &subtitle = "", bool secret = false);
-  static QString getText(const QString &title, QWidget *parent, const QString &substitle = "",
+  static QString getText(const QString &title, QWidget *parent, const QString &subtitle = "",
                          bool secret = false, int minLength = -1, const QString &defaultText = "");
   QString text();
   void setMessage(const QString &message, bool clearInputField = true);
@@ -50,7 +50,7 @@ signals:
   void emitText(const QString &text);
 };
 
-class ConfirmationDialog : public QDialogBase {
+class ConfirmationDialog : public DialogBase {
   Q_OBJECT
 
 public:
@@ -61,7 +61,7 @@ public:
   static bool rich(const QString &prompt_text, QWidget *parent);
 };
 
-class MultiOptionDialog : public QDialogBase {
+class MultiOptionDialog : public DialogBase {
   Q_OBJECT
 
 public:
diff --git a/selfdrive/ui/qt/widgets/keyboard.cc b/selfdrive/ui/qt/widgets/keyboard.cc
index 162d27db02..370e9a53cc 100644
--- a/selfdrive/ui/qt/widgets/keyboard.cc
+++ b/selfdrive/ui/qt/widgets/keyboard.cc
@@ -104,37 +104,37 @@ Keyboard::Keyboard(QWidget *parent) : QFrame(parent) {
 
   // lowercase
   std::vector<QVector<QString>> lowercase = {
-    {"q","w","e","r","t","y","u","i","o","p"},
-    {"a","s","d","f","g","h","j","k","l"},
-    {"↑","z","x","c","v","b","n","m",BACKSPACE_KEY},
-    {"123","  ",".",ENTER_KEY},
+    {"q", "w", "e", "r", "t", "y", "u", "i", "o", "p"},
+    {"a", "s", "d", "f", "g", "h", "j", "k", "l"},
+    {"↑", "z", "x", "c", "v", "b", "n", "m", BACKSPACE_KEY},
+    {"123", "  ", ".", ENTER_KEY},
   };
   main_layout->addWidget(new KeyboardLayout(this, lowercase));
 
   // uppercase
   std::vector<QVector<QString>> uppercase = {
-    {"Q","W","E","R","T","Y","U","I","O","P"},
-    {"A","S","D","F","G","H","J","K","L"},
-    {"↓","Z","X","C","V","B","N","M",BACKSPACE_KEY},
-    {"123","  ",".",ENTER_KEY},
+    {"Q", "W", "E", "R", "T", "Y", "U", "I", "O", "P"},
+    {"A", "S", "D", "F", "G", "H", "J", "K", "L"},
+    {"↓", "Z", "X", "C", "V", "B", "N", "M", BACKSPACE_KEY},
+    {"123", "  ", ".", ENTER_KEY},
   };
   main_layout->addWidget(new KeyboardLayout(this, uppercase));
 
   // numbers + specials
   std::vector<QVector<QString>> numbers = {
-    {"1","2","3","4","5","6","7","8","9","0"},
-    {"-","/",":",";","(",")","$","&&","@","\""},
-    {"#+=",".",",","?","!","`",BACKSPACE_KEY},
-    {"ABC","  ",".",ENTER_KEY},
+    {"1", "2", "3", "4", "5", "6", "7", "8", "9", "0"},
+    {"-", "/", ":", ";", "(", ")", "$", "&&", "@", "\""},
+    {"#+=", ".", ",", "?", "!", "`", BACKSPACE_KEY},
+    {"ABC", "  ", ".", ENTER_KEY},
   };
   main_layout->addWidget(new KeyboardLayout(this, numbers));
 
   // extra specials
   std::vector<QVector<QString>> specials = {
-    {"[","]","{","}","#","%","^","*","+","="},
-    {"_","\\","|","~","<",">","€","£","¥","•"},
-    {"123",".",",","?","!","'",BACKSPACE_KEY},
-    {"ABC","  ",".",ENTER_KEY},
+    {"[", "]", "{", "}", "#", "%", "^", "*", "+", "="},
+    {"_", "\\", "|", "~", "<", ">", "€", "£", "¥", "•"},
+    {"123", ".", ",", "?", "!", "'", BACKSPACE_KEY},
+    {"ABC", "  ", ".", ENTER_KEY},
   };
   main_layout->addWidget(new KeyboardLayout(this, specials));
 
diff --git a/selfdrive/ui/qt/widgets/keyboard.h b/selfdrive/ui/qt/widgets/keyboard.h
index 516105719b..efc02d075d 100644
--- a/selfdrive/ui/qt/widgets/keyboard.h
+++ b/selfdrive/ui/qt/widgets/keyboard.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <vector>
+
 #include <QFrame>
 #include <QPushButton>
 #include <QStackedLayout>
diff --git a/selfdrive/ui/qt/widgets/offroad_alerts.cc b/selfdrive/ui/qt/widgets/offroad_alerts.cc
index cdfa86c8eb..74ece36d15 100644
--- a/selfdrive/ui/qt/widgets/offroad_alerts.cc
+++ b/selfdrive/ui/qt/widgets/offroad_alerts.cc
@@ -1,5 +1,10 @@
 #include "selfdrive/ui/qt/widgets/offroad_alerts.h"
 
+#include <algorithm>
+#include <string>
+#include <vector>
+#include <utility>
+
 #include <QHBoxLayout>
 #include <QJsonDocument>
 #include <QJsonObject>
diff --git a/selfdrive/ui/qt/widgets/offroad_alerts.h b/selfdrive/ui/qt/widgets/offroad_alerts.h
index 69c12b0602..ace2e75456 100644
--- a/selfdrive/ui/qt/widgets/offroad_alerts.h
+++ b/selfdrive/ui/qt/widgets/offroad_alerts.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <map>
+#include <string>
 
 #include <QLabel>
 #include <QPushButton>
diff --git a/selfdrive/ui/qt/widgets/prime.cc b/selfdrive/ui/qt/widgets/prime.cc
index 782b7cb5ce..324d6cf6ae 100644
--- a/selfdrive/ui/qt/widgets/prime.cc
+++ b/selfdrive/ui/qt/widgets/prime.cc
@@ -68,7 +68,7 @@ void PairingQRWidget::paintEvent(QPaintEvent *e) {
 }
 
 
-PairingPopup::PairingPopup(QWidget *parent) : QDialogBase(parent) {
+PairingPopup::PairingPopup(QWidget *parent) : DialogBase(parent) {
   QHBoxLayout *hlayout = new QHBoxLayout(this);
   hlayout->setContentsMargins(0, 0, 0, 0);
   hlayout->setSpacing(0);
@@ -117,28 +117,19 @@ PairingPopup::PairingPopup(QWidget *parent) : QDialogBase(parent) {
 }
 
 
-PrimeUserWidget::PrimeUserWidget(QWidget* parent) : QFrame(parent) {
+PrimeUserWidget::PrimeUserWidget(QWidget *parent) : QFrame(parent) {
+  setObjectName("primeWidget");
   QVBoxLayout *mainLayout = new QVBoxLayout(this);
-  mainLayout->setContentsMargins(0, 0, 0, 0);
-  mainLayout->setSpacing(30);
-
-  // subscribed prime layout
-  QWidget *primeWidget = new QWidget;
-  primeWidget->setObjectName("primeWidget");
-  QVBoxLayout *primeLayout = new QVBoxLayout(primeWidget);
-  primeLayout->setContentsMargins(56, 40, 56, 40);
-  primeLayout->setSpacing(20);
+  mainLayout->setContentsMargins(56, 40, 56, 40);
+  mainLayout->setSpacing(20);
 
   QLabel *subscribed = new QLabel(tr("✓ SUBSCRIBED"));
   subscribed->setStyleSheet("font-size: 41px; font-weight: bold; color: #86FF4E;");
-  primeLayout->addWidget(subscribed);
+  mainLayout->addWidget(subscribed);
 
   QLabel *commaPrime = new QLabel(tr("comma prime"));
   commaPrime->setStyleSheet("font-size: 75px; font-weight: bold;");
-  primeLayout->addWidget(commaPrime);
-
-  mainLayout->addWidget(primeWidget);
-  mainLayout->addStretch();
+  mainLayout->addWidget(commaPrime);
 }
 
 
@@ -165,7 +156,7 @@ PrimeAdWidget::PrimeAdWidget(QWidget* parent) : QFrame(parent) {
   main_layout->addSpacing(30);
 
   QVector<QString> bullets = {tr("Remote access"), tr("24/7 LTE connectivity"), tr("1 year of drive storage"), tr("Turn-by-turn navigation")};
-  for (auto &b: bullets) {
+  for (auto &b : bullets) {
     const QString check = "<b><font color='#465BEA'>✓</font></b> ";
     QLabel *l = new QLabel(check + b);
     l->setAlignment(Qt::AlignLeft);
@@ -278,7 +269,7 @@ void SetupWidget::replyFinished(const QString &response, bool success) {
   }
 
   QJsonObject json = doc.object();
-  int prime_type = json["prime_type"].toInt();
+  PrimeType prime_type = static_cast<PrimeType>(json["prime_type"].toInt());
   uiState()->setPrimeType(prime_type);
 
   if (!json["is_paired"].toBool()) {
diff --git a/selfdrive/ui/qt/widgets/prime.h b/selfdrive/ui/qt/widgets/prime.h
index b41bab1695..63341c4cea 100644
--- a/selfdrive/ui/qt/widgets/prime.h
+++ b/selfdrive/ui/qt/widgets/prime.h
@@ -7,15 +7,6 @@
 
 #include "selfdrive/ui/qt/widgets/input.h"
 
-enum PrimeType {
-  NONE = 0,
-  MAGENTA = 1,
-  LITE = 2,
-  BLUE = 3,
-  MAGENTA_NEW = 4,
-};
-
-
 // pairing QR code
 class PairingQRWidget : public QWidget {
   Q_OBJECT
@@ -37,7 +28,7 @@ private slots:
 
 
 // pairing popup widget
-class PairingPopup : public QDialogBase {
+class PairingPopup : public DialogBase {
   Q_OBJECT
 
 public:
diff --git a/selfdrive/ui/qt/widgets/scrollview.cc b/selfdrive/ui/qt/widgets/scrollview.cc
index 5536593016..978bf83a63 100644
--- a/selfdrive/ui/qt/widgets/scrollview.cc
+++ b/selfdrive/ui/qt/widgets/scrollview.cc
@@ -10,7 +10,7 @@ ScrollView::ScrollView(QWidget *w, QWidget *parent) : QScrollArea(parent) {
   setWidgetResizable(true);
   setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
   setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
-  setStyleSheet("background-color: transparent;");
+  setStyleSheet("background-color: transparent; border:none");
 
   QString style = R"(
     QScrollBar:vertical {
diff --git a/selfdrive/ui/qt/widgets/ssh_keys.cc b/selfdrive/ui/qt/widgets/ssh_keys.cc
index 1097a89268..26743952de 100644
--- a/selfdrive/ui/qt/widgets/ssh_keys.cc
+++ b/selfdrive/ui/qt/widgets/ssh_keys.cc
@@ -4,7 +4,10 @@
 #include "selfdrive/ui/qt/api.h"
 #include "selfdrive/ui/qt/widgets/input.h"
 
-SshControl::SshControl() : ButtonControl(tr("SSH Keys"), "", tr("Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username.")) {
+SshControl::SshControl() :
+  ButtonControl(tr("SSH Keys"), "", tr("Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username "
+                                       "other than your own. A comma employee will NEVER ask you to add their GitHub username.")) {
+
   QObject::connect(this, &ButtonControl::clicked, [=]() {
     if (text() == tr("ADD")) {
       QString username = InputDialog::getText(tr("Enter your GitHub username"), this);
diff --git a/selfdrive/ui/qt/widgets/wifi.cc b/selfdrive/ui/qt/widgets/wifi.cc
index ed5825ab77..9c5289a22d 100644
--- a/selfdrive/ui/qt/widgets/wifi.cc
+++ b/selfdrive/ui/qt/widgets/wifi.cc
@@ -18,8 +18,8 @@ WiFiPromptWidget::WiFiPromptWidget(QWidget *parent) : QFrame(parent) {
     title_layout->setSpacing(32);
     {
       QLabel *icon = new QLabel;
-      QPixmap *pixmap = new QPixmap("../assets/offroad/icon_wifi_strength_full.svg");
-      icon->setPixmap(pixmap->scaledToWidth(80, Qt::SmoothTransformation));
+      QPixmap pixmap("../assets/offroad/icon_wifi_strength_full.svg");
+      icon->setPixmap(pixmap.scaledToWidth(80, Qt::SmoothTransformation));
       title_layout->addWidget(icon);
 
       QLabel *title = new QLabel(tr("Setup Wi-Fi"));
@@ -68,8 +68,8 @@ WiFiPromptWidget::WiFiPromptWidget(QWidget *parent) : QFrame(parent) {
       title_layout->addStretch();
 
       QLabel *icon = new QLabel;
-      QPixmap *pixmap = new QPixmap("../assets/offroad/icon_wifi_uploading.svg");
-      icon->setPixmap(pixmap->scaledToWidth(120, Qt::SmoothTransformation));
+      QPixmap pixmap("../assets/offroad/icon_wifi_uploading.svg");
+      icon->setPixmap(pixmap.scaledToWidth(120, Qt::SmoothTransformation));
       title_layout->addWidget(icon);
     }
     uploading_layout->addLayout(title_layout);
diff --git a/selfdrive/ui/soundd/sound.cc b/selfdrive/ui/soundd/sound.cc
index 49c28373c5..a5884f113f 100644
--- a/selfdrive/ui/soundd/sound.cc
+++ b/selfdrive/ui/soundd/sound.cc
@@ -12,48 +12,38 @@
 // TODO: detect when we can't play sounds
 // TODO: detect when we can't display the UI
 
-Sound::Sound(QObject *parent) : sm({"controlsState", "deviceState", "microphone"}) {
+Sound::Sound(QObject *parent) : sm({"controlsState", "microphone"}) {
   qInfo() << "default audio device: " << QAudioDeviceInfo::defaultOutputDevice().deviceName();
 
-  for (auto &[alert, fn, loops] : sound_list) {
+  for (auto &[alert, fn, loops, volume] : sound_list) {
     QSoundEffect *s = new QSoundEffect(this);
     QObject::connect(s, &QSoundEffect::statusChanged, [=]() {
       assert(s->status() != QSoundEffect::Error);
     });
     s->setSource(QUrl::fromLocalFile("../../assets/sounds/" + fn));
+    s->setVolume(volume);
     sounds[alert] = {s, loops};
   }
 
   QTimer *timer = new QTimer(this);
   QObject::connect(timer, &QTimer::timeout, this, &Sound::update);
   timer->start(1000 / UI_FREQ);
-};
+}
 
 void Sound::update() {
-  const bool started_prev = sm["deviceState"].getDeviceState().getStarted();
   sm.update(0);
 
-  const bool started = sm["deviceState"].getDeviceState().getStarted();
-  if (started && !started_prev) {
-    started_frame = sm.frame;
-  }
-
-  // no sounds while offroad
-  // also no sounds if nothing is alive in case thermald crashes while offroad
-  const bool crashed = (sm.frame - std::max(sm.rcv_frame("deviceState"), sm.rcv_frame("controlsState"))) > 10*UI_FREQ;
-  if (!started || crashed) {
-    setAlert({});
-    return;
-  }
-
   // scale volume using ambient noise level
   if (sm.updated("microphone")) {
     float volume = util::map_val(sm["microphone"].getMicrophone().getFilteredSoundPressureWeightedDb(), 30.f, 60.f, 0.f, 1.f);
     volume = QAudio::convertVolume(volume, QAudio::LogarithmicVolumeScale, QAudio::LinearVolumeScale);
-    Hardware::set_volume(volume);
+    // set volume on changes
+    if (std::exchange(current_volume, std::nearbyint(volume * 10)) != current_volume) {
+      Hardware::set_volume(volume);
+    }
   }
 
-  setAlert(Alert::get(sm, started_frame));
+  setAlert(Alert::get(sm, 0));
 }
 
 void Sound::setAlert(const Alert &alert) {
diff --git a/selfdrive/ui/soundd/sound.h b/selfdrive/ui/soundd/sound.h
index 7e009d28ad..4fcb2e1bce 100644
--- a/selfdrive/ui/soundd/sound.h
+++ b/selfdrive/ui/soundd/sound.h
@@ -1,3 +1,7 @@
+#pragma once
+
+#include <tuple>
+
 #include <QMap>
 #include <QSoundEffect>
 #include <QString>
@@ -5,18 +9,21 @@
 #include "system/hardware/hw.h"
 #include "selfdrive/ui/ui.h"
 
-const std::tuple<AudibleAlert, QString, int> sound_list[] = {
+
+const float MAX_VOLUME = 1.0;
+
+const std::tuple<AudibleAlert, QString, int, float> sound_list[] = {
   // AudibleAlert, file name, loop count
-  {AudibleAlert::ENGAGE, "engage.wav", 0},
-  {AudibleAlert::DISENGAGE, "disengage.wav", 0},
-  {AudibleAlert::REFUSE, "refuse.wav", 0},
+  {AudibleAlert::ENGAGE, "engage.wav", 0, MAX_VOLUME},
+  {AudibleAlert::DISENGAGE, "disengage.wav", 0, MAX_VOLUME},
+  {AudibleAlert::REFUSE, "refuse.wav", 0, MAX_VOLUME},
 
-  {AudibleAlert::PROMPT, "prompt.wav", 0},
-  {AudibleAlert::PROMPT_REPEAT, "prompt.wav", QSoundEffect::Infinite},
-  {AudibleAlert::PROMPT_DISTRACTED, "prompt_distracted.wav", QSoundEffect::Infinite},
+  {AudibleAlert::PROMPT, "prompt.wav", 0, MAX_VOLUME},
+  {AudibleAlert::PROMPT_REPEAT, "prompt.wav", QSoundEffect::Infinite, MAX_VOLUME},
+  {AudibleAlert::PROMPT_DISTRACTED, "prompt_distracted.wav", QSoundEffect::Infinite, MAX_VOLUME},
 
-  {AudibleAlert::WARNING_SOFT, "warning_soft.wav", QSoundEffect::Infinite},
-  {AudibleAlert::WARNING_IMMEDIATE, "warning_immediate.wav", QSoundEffect::Infinite},
+  {AudibleAlert::WARNING_SOFT, "warning_soft.wav", QSoundEffect::Infinite, MAX_VOLUME},
+  {AudibleAlert::WARNING_IMMEDIATE, "warning_immediate.wav", QSoundEffect::Infinite, MAX_VOLUME},
 };
 
 class Sound : public QObject {
@@ -27,8 +34,8 @@ protected:
   void update();
   void setAlert(const Alert &alert);
 
+  SubMaster sm;
   Alert current_alert = {};
   QMap<AudibleAlert, QPair<QSoundEffect *, int>> sounds;
-  SubMaster sm;
-  uint64_t started_frame;
+  int current_volume = -1;
 };
diff --git a/selfdrive/ui/soundd/soundd b/selfdrive/ui/soundd/soundd
index 66dda46d5b..9b7a32fec9 100755
--- a/selfdrive/ui/soundd/soundd
+++ b/selfdrive/ui/soundd/soundd
@@ -1,5 +1,4 @@
 #!/bin/sh
 cd "$(dirname "$0")"
-export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH"
 export QT_QPA_PLATFORM="offscreen"
 exec ./_soundd
diff --git a/selfdrive/ui/spinner b/selfdrive/ui/spinner
index 6c8e533cc8..35feab3f7e 100755
--- a/selfdrive/ui/spinner
+++ b/selfdrive/ui/spinner
@@ -4,5 +4,4 @@ if [ -f /TICI ] && [ ! -f qt/spinner ]; then
   cp qt/spinner_larch64 qt/spinner
 fi
 
-export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH"
 exec ./qt/spinner "$1"
diff --git a/selfdrive/ui/tests/test_translations.py b/selfdrive/ui/tests/test_translations.py
index 26d6c39349..622d198f7d 100755
--- a/selfdrive/ui/tests/test_translations.py
+++ b/selfdrive/ui/tests/test_translations.py
@@ -6,11 +6,12 @@ import shutil
 import unittest
 import xml.etree.ElementTree as ET
 
-from selfdrive.ui.update_translations import TRANSLATIONS_DIR, LANGUAGES_FILE, update_translations
+from openpilot.selfdrive.ui.update_translations import TRANSLATIONS_DIR, LANGUAGES_FILE, update_translations
 
 TMP_TRANSLATIONS_DIR = os.path.join(TRANSLATIONS_DIR, "tmp")
 UNFINISHED_TRANSLATION_TAG = "<translation type=\"unfinished\""  # non-empty translations can be marked unfinished
 LOCATION_TAG = "<location "
+FORMAT_ARG = re.compile("%[0-9]+")
 
 
 class TestTranslations(unittest.TestCase):
@@ -67,11 +68,15 @@ class TestTranslations(unittest.TestCase):
         self.assertTrue("<translation type=\"vanished\">" not in cur_translations,
                         f"{file} ({name}) translation file has obsolete translations. Run selfdrive/ui/update_translations.py --vanish to remove them")
 
-  def test_plural_translations(self):
+  def test_finished_translations(self):
     """
-      Tests:
-      - that any numerus (plural) translations marked "finished" have all plural forms non-empty
+      Tests ran on each translation marked "finished"
+      Plural:
+      - that any numerus (plural) translations have all plural forms non-empty
       - that the correct format specifier is used (%n)
+      Non-plural:
+      - that translation is not empty
+      - that translation format arguments are consistent
     """
     for name, file in self.translation_files.items():
       with self.subTest(name=name, file=file):
@@ -79,17 +84,28 @@ class TestTranslations(unittest.TestCase):
 
         for context in tr_xml.getroot():
           for message in context.iterfind("message"):
+            translation = message.find("translation")
+            source_text = message.find("source").text
+
+            # Do not test unfinished translations
+            if translation.get("type") == "unfinished":
+              continue
+
             if message.get("numerus") == "yes":
-              translation = message.find("translation")
               numerusform = [t.text for t in translation.findall("numerusform")]
 
-              # Do not assert finished translations
-              if translation.get("type") == "unfinished":
-                continue
+              for nf in numerusform:
+                self.assertIsNotNone(nf, f"Ensure all plural translation forms are completed: {source_text}")
+                self.assertIn("%n", nf, "Ensure numerus argument (%n) exists in translation.")
+                self.assertIsNone(FORMAT_ARG.search(nf), "Plural translations must use %n, not %1, %2, etc.: {}".format(numerusform))
 
-              self.assertNotIn(None, numerusform, "Ensure all plural translation forms are completed.")
-              self.assertTrue(all([re.search("%[0-9]+", t) is None for t in numerusform]),
-                              "Plural translations must use %n, not %1, %2, etc.: {}".format(numerusform))
+            else:
+              self.assertIsNotNone(translation.text, f"Ensure translation is completed: {source_text}")
+
+              source_args = FORMAT_ARG.findall(source_text)
+              translation_args = FORMAT_ARG.findall(translation.text)
+              self.assertEqual(sorted(source_args), sorted(translation_args),
+                               f"Ensure format arguments are consistent: `{source_text}` vs. `{translation.text}`")
 
   def test_no_locations(self):
     for name, file in self.translation_files.items():
@@ -98,6 +114,13 @@ class TestTranslations(unittest.TestCase):
           self.assertFalse(line.strip().startswith(LOCATION_TAG),
                            f"Line contains location tag: {line.strip()}, remove all line numbers.")
 
+  def test_entities_error(self):
+    for name, file in self.translation_files.items():
+      with self.subTest(name=name, file=file):
+        cur_translations = self._read_translation_file(TRANSLATIONS_DIR, file)
+        matches = re.findall(r'@(\w+);', cur_translations)
+        self.assertEqual(len(matches), 0, f"The string(s) {matches} were found with '@' instead of '&'")
+
 
 if __name__ == "__main__":
   unittest.main()
diff --git a/selfdrive/ui/text b/selfdrive/ui/text
index 2577e3006b..b44bec42bd 100755
--- a/selfdrive/ui/text
+++ b/selfdrive/ui/text
@@ -4,5 +4,4 @@ if [ -f /TICI ] && [ ! -f qt/text ]; then
   cp qt/text_larch64 qt/text
 fi
 
-export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH"
 exec ./qt/text "$1"
diff --git a/selfdrive/ui/translations/languages.json b/selfdrive/ui/translations/languages.json
index 86d3e62d87..321cdeedd2 100644
--- a/selfdrive/ui/translations/languages.json
+++ b/selfdrive/ui/translations/languages.json
@@ -1,7 +1,11 @@
 {
   "English": "main_en",
   "Deutsch": "main_de",
+  "Français": "main_fr",
   "Português": "main_pt-BR",
+  "Türkçe": "main_tr",
+  "العربية": "main_ar",
+  "ไทย": "main_th",
   "中文（繁體）": "main_zh-CHT",
   "中文（简体）": "main_zh-CHS",
   "한국어": "main_ko",
diff --git a/selfdrive/ui/translations/main_ar.ts b/selfdrive/ui/translations/main_ar.ts
index 07a84fca08..4159f28c31 100644
--- a/selfdrive/ui/translations/main_ar.ts
+++ b/selfdrive/ui/translations/main_ar.ts
@@ -5,7 +5,7 @@
     <name>AbstractAlert</name>
     <message>
         <source>Close</source>
-        <translation>أغلق</translation>
+        <translation>إغلاق</translation>
     </message>
     <message>
         <source>Snooze Update</source>
@@ -20,7 +20,7 @@
     <name>AdvancedNetworking</name>
     <message>
         <source>Back</source>
-        <translation>خلف</translation>
+        <translation>السابق</translation>
     </message>
     <message>
         <source>Enable Tethering</source>
@@ -28,7 +28,7 @@
     </message>
     <message>
         <source>Tethering Password</source>
-        <translation>كلمة مرور للربط</translation>
+        <translation>كلمة مرور الربط</translation>
     </message>
     <message>
         <source>EDIT</source>
@@ -36,7 +36,7 @@
     </message>
     <message>
         <source>Enter new tethering password</source>
-        <translation>أدخل كلمة مرور جديدة للربط</translation>
+        <translation>أدخل كلمة مرور الربط الجديدة</translation>
     </message>
     <message>
         <source>IP Address</source>
@@ -48,46 +48,46 @@
     </message>
     <message>
         <source>APN Setting</source>
-        <translation>إعداد APN</translation>
+        <translation>إعدادات APN</translation>
     </message>
     <message>
         <source>Enter APN</source>
-        <translation>أدخل APN</translation>
+        <translation>إدخال APN</translation>
     </message>
     <message>
         <source>leave blank for automatic configuration</source>
-        <translation>اتركه فارغا للتكوين التلقائي</translation>
+        <translation>اتركه فارغاً من أجل التكوين التلقائي</translation>
     </message>
     <message>
         <source>Cellular Metered</source>
-        <translation type="unfinished"></translation>
+        <translation>محدود بالاتصال الخلوي</translation>
     </message>
     <message>
         <source>Prevent large data uploads when on a metered connection</source>
-        <translation type="unfinished"></translation>
+        <translation>منع تحميل البيانات الكبيرة عندما يكون الاتصال محدوداً</translation>
     </message>
 </context>
 <context>
     <name>AnnotatedCameraWidget</name>
     <message>
         <source>km/h</source>
-        <translation>km/h</translation>
+        <translation>كم/س</translation>
     </message>
     <message>
         <source>mph</source>
-        <translation>mph</translation>
+        <translation>ميل/س</translation>
     </message>
     <message>
         <source>MAX</source>
-        <translation>الأعلى</translation>
+        <translation>MAX</translation>
     </message>
     <message>
         <source>SPEED</source>
-        <translation>سرعة</translation>
+        <translation>SPEED</translation>
     </message>
     <message>
         <source>LIMIT</source>
-        <translation>حد</translation>
+        <translation>LIMIT</translation>
     </message>
 </context>
 <context>
@@ -109,11 +109,38 @@
     </message>
     <message>
         <source>Back</source>
-        <translation>خلف</translation>
+        <translation>السابق</translation>
     </message>
     <message>
         <source>Decline, uninstall %1</source>
-        <translation>رفض ، قم بإلغاء تثبيت %1</translation>
+        <translation>رفض، إلغاء التثبيت %1</translation>
+    </message>
+</context>
+<context>
+    <name>DestinationWidget</name>
+    <message>
+        <source>Home</source>
+        <translation>المنزل</translation>
+    </message>
+    <message>
+        <source>Work</source>
+        <translation>العمل</translation>
+    </message>
+    <message>
+        <source>No destination set</source>
+        <translation>لم يتم ضبط الوجهة</translation>
+    </message>
+    <message>
+        <source>home</source>
+        <translation>المنزل</translation>
+    </message>
+    <message>
+        <source>work</source>
+        <translation>العمل</translation>
+    </message>
+    <message>
+        <source>No %1 location set</source>
+        <translation>لم يتم ضبط %1 موقع</translation>
     </message>
 </context>
 <context>
@@ -128,19 +155,19 @@
     </message>
     <message>
         <source>Serial</source>
-        <translation>التسلسلي</translation>
+        <translation>الرقم التسلسلي</translation>
     </message>
     <message>
         <source>Driver Camera</source>
-        <translation>كاميرا السائق</translation>
+        <translation>كاميرة السائق</translation>
     </message>
     <message>
         <source>PREVIEW</source>
-        <translation>لمح</translation>
+        <translation>معاينة</translation>
     </message>
     <message>
         <source>Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off)</source>
-        <translation>قم بمعاينة الكاميرا المواجهة للسائق للتأكد من أن نظام مراقبة السائق يتمتع برؤية جيدة. (يجب أن تكون السيارة معطلة)</translation>
+        <translation>قم بمعاينة الكاميرا المواجهة للسائق للتأكد من أن نظام مراقبة السائق يتمتع برؤية جيدة. (يجب أن تكون السيارة متوقفة)</translation>
     </message>
     <message>
         <source>Reset Calibration</source>
@@ -148,7 +175,7 @@
     </message>
     <message>
         <source>RESET</source>
-        <translation>إعادة تعيين</translation>
+        <translation>إعادة الضبط</translation>
     </message>
     <message>
         <source>Are you sure you want to reset calibration?</source>
@@ -164,7 +191,7 @@
     </message>
     <message>
         <source>Review the rules, features, and limitations of openpilot</source>
-        <translation>راجع القواعد والميزات والقيود الخاصة بـ openpilot</translation>
+        <translation>مراجعة الأدوار والميزات والقيود في openpilot</translation>
     </message>
     <message>
         <source>Are you sure you want to review the training guide?</source>
@@ -172,7 +199,7 @@
     </message>
     <message>
         <source>Regulatory</source>
-        <translation>تنظيمية</translation>
+        <translation>التنظيمية</translation>
     </message>
     <message>
         <source>VIEW</source>
@@ -192,43 +219,43 @@
     </message>
     <message>
         <source>Reboot</source>
-        <translation>اعادة التشغيل</translation>
+        <translation>إعادة التشغيل</translation>
     </message>
     <message>
         <source>Power Off</source>
-        <translation>أطفاء</translation>
+        <translation>إيقاف التشغيل</translation>
     </message>
     <message>
-        <source>openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required.</source>
-        <translation>يتطلب openpilot أن يتم تركيب الجهاز في حدود 4 درجات يسارًا أو يمينًا و 5 درجات لأعلى أو 8 درجات لأسفل. يقوم برنامج openpilot بالمعايرة بشكل مستمر ، ونادراً ما تكون إعادة الضبط مطلوبة.</translation>
+        <source>openpilot requires the device to be mounted within 4° left or right and within 5° up or 9° down. openpilot is continuously calibrating, resetting is rarely required.</source>
+        <translation>يحتاج openpilot أن يتم ضبط الجهاز ضمن حدود 4 درجات يميناً أو يساراً و5 درجات نحو الأعلى أو 9 نحو الأسفل. يقوم openpilot بالمعايرة باستمرار، ونادراً ما يحتاج إلى عملية إعادة الضبط.</translation>
     </message>
     <message>
         <source> Your device is pointed %1° %2 and %3° %4.</source>
-        <translation> جهازك يشير %1° %2 و %3° %4.</translation>
+        <translation> يشير جهازك إلى %1 درجة %2، و%3 درجة %4.</translation>
     </message>
     <message>
         <source>down</source>
-        <translation>لأسفل</translation>
+        <translation>نحو الأسفل</translation>
     </message>
     <message>
         <source>up</source>
-        <translation>إلى أعلى</translation>
+        <translation>نحو الأعلى</translation>
     </message>
     <message>
         <source>left</source>
-        <translation>إلى اليسار</translation>
+        <translation>نحو اليسار</translation>
     </message>
     <message>
         <source>right</source>
-        <translation>إلى اليمين</translation>
+        <translation>نحو اليمين</translation>
     </message>
     <message>
         <source>Are you sure you want to reboot?</source>
-        <translation>هل أنت متأكد أنك تريد إعادة التشغيل؟</translation>
+        <translation>هل أنت متأكد  أنك تريد إعادة التشغيل؟</translation>
     </message>
     <message>
         <source>Disengage to Reboot</source>
-        <translation>فك الارتباط لإعادة التشغيل</translation>
+        <translation>فك الارتباط من أجل إعادة التشغيل</translation>
     </message>
     <message>
         <source>Are you sure you want to power off?</source>
@@ -236,34 +263,15 @@
     </message>
     <message>
         <source>Disengage to Power Off</source>
-        <translation>فك الارتباط لإيقاف التشغيل</translation>
-    </message>
-</context>
-<context>
-    <name>DriveStats</name>
-    <message>
-        <source>Drives</source>
-        <translation>أرقام القيادة</translation>
-    </message>
-    <message>
-        <source>Hours</source>
-        <translation>ساعات</translation>
-    </message>
-    <message>
-        <source>ALL TIME</source>
-        <translation>في كل وقت</translation>
-    </message>
-    <message>
-        <source>PAST WEEK</source>
-        <translation>الأسبوع الماضي</translation>
+        <translation>فك الارتباط من أجل إيقاف التشغيل</translation>
     </message>
     <message>
-        <source>KM</source>
-        <translation>كم</translation>
+        <source>Reset</source>
+        <translation>إعادة الضبط</translation>
     </message>
     <message>
-        <source>Miles</source>
-        <translation>اميال</translation>
+        <source>Review</source>
+        <translation>مراجعة</translation>
     </message>
 </context>
 <context>
@@ -273,6 +281,17 @@
         <translation>بدء تشغيل الكاميرا</translation>
     </message>
 </context>
+<context>
+    <name>ExperimentalModeButton</name>
+    <message>
+        <source>EXPERIMENTAL MODE ON</source>
+        <translation>تشغيل الوضع التجريبي</translation>
+    </message>
+    <message>
+        <source>CHILL MODE ON</source>
+        <translation>تشغيل وضع الراحة</translation>
+    </message>
+</context>
 <context>
     <name>InputDialog</name>
     <message>
@@ -282,12 +301,12 @@
     <message numerus="yes">
         <source>Need at least %n character(s)!</source>
         <translation>
-            <numerusform>تحتاج على الأقل %n حرف!</numerusform>
-            <numerusform>تحتاج على الأقل %n حرف!</numerusform>
-            <numerusform>تحتاج على الأقل %n احرف!</numerusform>
-            <numerusform>تحتاج على الأقل %n احرف!</numerusform>
-            <numerusform>تحتاج على الأقل %n احرف!</numerusform>
-            <numerusform>تحتاج على الأقل %n احرف!</numerusform>
+            <numerusform>تحتاج إلى حرف %n على الأقل!</numerusform>
+            <numerusform>تحتاج إلى حرف %n على الأقل!</numerusform>
+            <numerusform>تحتاج إلى حرفين %n على الأقل!</numerusform>
+            <numerusform>تحتاج إلى %n أحرف على الأقل!</numerusform>
+            <numerusform>تحتاج إلى %n أحرف على الأقل!</numerusform>
+            <numerusform>تحتاج إلى %n حرف على الأقل!</numerusform>
         </translation>
     </message>
 </context>
@@ -295,143 +314,135 @@
     <name>Installer</name>
     <message>
         <source>Installing...</source>
-        <translation>جارٍ التثبيت ...</translation>
-    </message>
-    <message>
-        <source>Receiving objects: </source>
-        <translation>استقبال الكائنات: </translation>
-    </message>
-    <message>
-        <source>Resolving deltas: </source>
-        <translation>حل دلتا: </translation>
-    </message>
-    <message>
-        <source>Updating files: </source>
-        <translation>جارٍ تحديث الملفات: </translation>
+        <translation>جارٍ التثبيت...</translation>
     </message>
 </context>
 <context>
     <name>MapETA</name>
     <message>
         <source>eta</source>
-        <translation>eta</translation>
+        <translation>الوصول</translation>
     </message>
     <message>
         <source>min</source>
-        <translation>دق</translation>
+        <translation>د</translation>
     </message>
     <message>
         <source>hr</source>
-        <translation>سع</translation>
+        <translation>س</translation>
     </message>
+</context>
+<context>
+    <name>MapSettings</name>
     <message>
-        <source>km</source>
-        <translation>كم</translation>
+        <source>NAVIGATION</source>
+        <translation>التنقل</translation>
     </message>
     <message>
-        <source>mi</source>
-        <translation>مل</translation>
+        <source>Manage at connect.comma.ai</source>
+        <translation>الإدارة في connect.comma.ai</translation>
     </message>
 </context>
 <context>
-    <name>MapInstructions</name>
+    <name>MapWindow</name>
     <message>
-        <source> km</source>
-        <translation> كم</translation>
+        <source>Map Loading</source>
+        <translation>تحميل الخريطة</translation>
+    </message>
+    <message>
+        <source>Waiting for GPS</source>
+        <translation>بانتظار GPS</translation>
     </message>
     <message>
-        <source> m</source>
-        <translation> م</translation>
+        <source>Waiting for route</source>
+        <translation>بانتظار الطريق</translation>
     </message>
+</context>
+<context>
+    <name>MultiOptionDialog</name>
     <message>
-        <source> mi</source>
-        <translation> مل</translation>
+        <source>Select</source>
+        <translation>اختيار</translation>
     </message>
     <message>
-        <source> ft</source>
-        <translation> قد</translation>
+        <source>Cancel</source>
+        <translation>إلغاء</translation>
     </message>
 </context>
 <context>
-    <name>MapPanel</name>
+    <name>Networking</name>
     <message>
-        <source>Current Destination</source>
-        <translation>الوجهة الحالية</translation>
+        <source>Advanced</source>
+        <translation>متقدم</translation>
     </message>
     <message>
-        <source>CLEAR</source>
-        <translation>مسح</translation>
+        <source>Enter password</source>
+        <translation>أدخل كلمة المرور</translation>
     </message>
     <message>
-        <source>Recent Destinations</source>
-        <translation>الوجهات الأخيرة</translation>
+        <source>for &quot;%1&quot;</source>
+        <translation>من أجل &quot;%1&quot;</translation>
     </message>
     <message>
-        <source>Try the Navigation Beta</source>
-        <translation>جرب التنقل التجريبي</translation>
+        <source>Wrong password</source>
+        <translation>كلمة مرور خاطئة</translation>
     </message>
+</context>
+<context>
+    <name>OffroadAlert</name>
     <message>
-        <source>Get turn-by-turn directions displayed and more with a comma
-prime subscription. Sign up now: https://connect.comma.ai</source>
-        <translation>احصل على الاتجاهات خطوة بخطوة معروضة والمزيد باستخدام comma
-الاشتراك الرئيسي. اشترك الآن: https://connect.comma.ai</translation>
+        <source>Device temperature too high. System cooling down before starting. Current internal component temperature: %1</source>
+        <translation>درجة حرارة الجهاز مرتفعة جداً. يقوم النظام بالتبريد قبل البدء. درجة الحرارة الحالية للمكونات الداخلية: %1</translation>
     </message>
     <message>
-        <source>No home
-location set</source>
-        <translation>لم يتم تعيين
-موقع المنزل</translation>
+        <source>Immediately connect to the internet to check for updates. If you do not connect to the internet, openpilot won&apos;t engage in %1</source>
+        <translation>اتصل فوراً بالإنترنت للتحقق من وجود تحديثات. إذا لم تكم متصلاً بالإنترنت فإن openpilot لن يساهم في %1</translation>
     </message>
     <message>
-        <source>No work
-location set</source>
-        <translation>لم يتم تعيين
-موقع العمل</translation>
+        <source>Connect to internet to check for updates. openpilot won&apos;t automatically start until it connects to internet to check for updates.</source>
+        <translation>اتصل بالإنترنت للتحقق من وجود تحديثات. لا يعمل openpilot تلقائياً إلا إذا اتصل بالإنترنت من أجل التحقق من التحديثات.</translation>
     </message>
     <message>
-        <source>no recent destinations</source>
-        <translation>لا توجد وجهات حديثة</translation>
+        <source>Unable to download updates
+%1</source>
+        <translation>غير قادر على تحميل التحديثات
+%1</translation>
     </message>
-</context>
-<context>
-    <name>MapWindow</name>
     <message>
-        <source>Map Loading</source>
-        <translation>تحميل الخريطة</translation>
+        <source>Invalid date and time settings, system won&apos;t start. Connect to internet to set time.</source>
+        <translation>إعدادات التاريخ والتوقيت غير صحيحة، لن يبدأ النظام. اتصل بالإنترنت من أجل ضبط الوقت.</translation>
     </message>
     <message>
-        <source>Waiting for GPS</source>
-        <translation>في انتظار GPS</translation>
+        <source>Taking camera snapshots. System won&apos;t start until finished.</source>
+        <translation>التقاط لقطات كاميرا. لن يبدأ النظام حتى تنتهي هذه العملية.</translation>
     </message>
-</context>
-<context>
-    <name>MultiOptionDialog</name>
     <message>
-        <source>Select</source>
-        <translation>اختر</translation>
+        <source>An update to your device&apos;s operating system is downloading in the background. You will be prompted to update when it&apos;s ready to install.</source>
+        <translation>يتم تنزيل تحديث لنظام تشغيل جهازك في الخلفية. سيطلَب منك التحديث عندما يصبح جاهزاً للتثبيت.</translation>
     </message>
     <message>
-        <source>Cancel</source>
-        <translation>إلغاء</translation>
+        <source>Device failed to register. It will not connect to or upload to comma.ai servers, and receives no support from comma.ai. If this is an official device, visit https://comma.ai/support.</source>
+        <translation>فشل تسجيل الجهاز. لن يقوم بالاتصال أو تحميل خوادم comma.ai، ولا تلقي الدعم من comma.ai. إذا كان هذا الجهاز نظامياً فيرجى زيارة الموقع https://comma.ai/support.</translation>
     </message>
-</context>
-<context>
-    <name>Networking</name>
     <message>
-        <source>Advanced</source>
-        <translation>متقدم</translation>
+        <source>NVMe drive not mounted.</source>
+        <translation>محرك NVMe غير مثبَّت.</translation>
     </message>
     <message>
-        <source>Enter password</source>
-        <translation>أدخل كلمة المرور</translation>
+        <source>Unsupported NVMe drive detected. Device may draw significantly more power and overheat due to the unsupported NVMe.</source>
+        <translation>تم اكتشاف محرك NVMe غير مدعوم. قد يستهلك الجهاز قدراً أكبر بكثير من الطاقة، وزيادة في ارتفاع درجة الحرارة بسبب وجود NVMe غير مدعوم.</translation>
     </message>
     <message>
-        <source>for &quot;%1&quot;</source>
-        <translation>ل &quot;%1&quot;</translation>
+        <source>openpilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please submit a pull request to add the firmware versions to the proper vehicle. Need help? Join discord.comma.ai.</source>
+        <translation>لم يكن openpilot قادراً على تحديد سيارتك. إما أن تكون سيارتك غير مدعومة أو أنه لم يتم التعرف على وحدة التحكم الإلكتروني (ECUs) فيها. يرجى تقديم طلب سحب من أجل إضافة نسخ برمجيات ثابتة إلى السيارة المناسبة. هل تحتاج إلى أي مساعدة؟ لا تتردد في التواصل مع doscord.comma.ai.</translation>
     </message>
     <message>
-        <source>Wrong password</source>
-        <translation>كلمة مرور خاطئة</translation>
+        <source>openpilot was unable to identify your car. Check integrity of cables and ensure all connections are secure, particularly that the comma power is fully inserted in the OBD-II port of the vehicle. Need help? Join discord.comma.ai.</source>
+        <translation>لم يتمكن openpilot من تحديد سيارتك. تحقق من سلامة الكابلات وتأكد من تأمين جميع الوصلات، لا سيما أنه قد تم إدخال طاقة الفاصلة بالكامل في منفذ OBD-II في السيارة. هل تريد أي مساعدة؟ لا تتردد في الانضمام إلى discord.comma.ai.</translation>
+    </message>
+    <message>
+        <source>openpilot detected a change in the device&apos;s mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.</source>
+        <translation>لقد اكتشف openpilot تغييراً في موقع تركيب الجهاز. تأكد من تثبيت الجهاز بشكل كامل في موقعه وتثبيته بإحكام على الزجاج الأمامي.</translation>
     </message>
 </context>
 <context>
@@ -442,7 +453,7 @@ location set</source>
     </message>
     <message>
         <source> ALERTS</source>
-        <translation> تنبيهات</translation>
+        <translation> التنبهات</translation>
     </message>
     <message>
         <source> ALERT</source>
@@ -453,46 +464,61 @@ location set</source>
     <name>PairingPopup</name>
     <message>
         <source>Pair your device to your comma account</source>
-        <translation>قم بإقران جهازك بحساب comma الخاص بك</translation>
+        <translation>اقرن جهازك مع حسابك على comma</translation>
     </message>
     <message>
         <source>Go to https://connect.comma.ai on your phone</source>
-        <translation>اذهب إلى https://connect.comma.ai من هاتفك</translation>
+        <translation>انتقل إلى https://connect.comma.ai على جوالك</translation>
     </message>
     <message>
         <source>Click &quot;add new device&quot; and scan the QR code on the right</source>
-        <translation>انقر على &quot;إضافة جهاز جديد&quot; وامسح رمز الاستجابة السريعة على اليمين</translation>
+        <translation>انقر &quot;،إضافة جهاز جديد&quot;، وامسح رمز الاستجابة السريعة (QR) على اليمين</translation>
     </message>
     <message>
         <source>Bookmark connect.comma.ai to your home screen to use it like an app</source>
-        <translation>ضع إشارة مرجعية على connect.comma.ai على شاشتك الرئيسية لاستخدامه مثل أي تطبيق</translation>
+        <translation>اجعل لـconnect.comma.ai إشارة مرجعية على شاشتك  الرئيسية من أجل استخدامه مثل أي تطبيق</translation>
+    </message>
+</context>
+<context>
+    <name>ParamControl</name>
+    <message>
+        <source>Enable</source>
+        <translation>تمكين</translation>
+    </message>
+    <message>
+        <source>Cancel</source>
+        <translation>إلغاء</translation>
     </message>
 </context>
 <context>
     <name>PrimeAdWidget</name>
     <message>
         <source>Upgrade Now</source>
-        <translation>قم بالترقية الآن</translation>
+        <translation>الترقية الآن</translation>
     </message>
     <message>
         <source>Become a comma prime member at connect.comma.ai</source>
-        <translation>كن عضوًا comme prime في connect.comma.ai</translation>
+        <translation>كن عضوًا في comma prime على connect.comma.ai</translation>
     </message>
     <message>
         <source>PRIME FEATURES:</source>
-        <translation>ميزات PRIME:</translation>
+        <translation>الميزات الأساسية:</translation>
     </message>
     <message>
         <source>Remote access</source>
-        <translation>الوصول عن بعد</translation>
+        <translation>التحكم عن بعد</translation>
+    </message>
+    <message>
+        <source>24/7 LTE connectivity</source>
+        <translation>اتصال LTE على مدار الساعة 24/7</translation>
     </message>
     <message>
-        <source>1 year of storage</source>
-        <translation>سنة واحدة من التخزين</translation>
+        <source>1 year of drive storage</source>
+        <translation>سنة واحدة من تخزين القرص</translation>
     </message>
     <message>
-        <source>Developer perks</source>
-        <translation>امتيازات المطور</translation>
+        <source>Turn-by-turn navigation</source>
+        <translation>التنقل خطوة بخطوة</translation>
     </message>
 </context>
 <context>
@@ -505,24 +531,16 @@ location set</source>
         <source>comma prime</source>
         <translation>comma prime</translation>
     </message>
-    <message>
-        <source>CONNECT.COMMA.AI</source>
-        <translation>CONNECT.COMMA.AI</translation>
-    </message>
-    <message>
-        <source>COMMA POINTS</source>
-        <translation>COMMA POINTS</translation>
-    </message>
 </context>
 <context>
     <name>QObject</name>
     <message>
         <source>Reboot</source>
-        <translation>اعادة التشغيل</translation>
+        <translation>إعادة التشغيل</translation>
     </message>
     <message>
         <source>Exit</source>
-        <translation>أغلق</translation>
+        <translation>إغلاق</translation>
     </message>
     <message>
         <source>dashcam</source>
@@ -537,10 +555,10 @@ location set</source>
         <translation>
             <numerusform>منذ %n دقيقة</numerusform>
             <numerusform>منذ %n دقيقة</numerusform>
+            <numerusform>منذ دقيقتين %n</numerusform>
             <numerusform>منذ %n دقائق</numerusform>
             <numerusform>منذ %n دقائق</numerusform>
-            <numerusform>منذ %n دقائق</numerusform>
-            <numerusform>منذ %n دقائق</numerusform>
+            <numerusform>منذ %n دقيقة</numerusform>
         </translation>
     </message>
     <message numerus="yes">
@@ -548,10 +566,10 @@ location set</source>
         <translation>
             <numerusform>منذ %n ساعة</numerusform>
             <numerusform>منذ %n ساعة</numerusform>
+            <numerusform>منذ ساعتين %n</numerusform>
             <numerusform>منذ %n ساعات</numerusform>
             <numerusform>منذ %n ساعات</numerusform>
-            <numerusform>منذ %n ساعات</numerusform>
-            <numerusform>منذ %n ساعات</numerusform>
+            <numerusform>منذ %n ساعة</numerusform>
         </translation>
     </message>
     <message numerus="yes">
@@ -559,34 +577,42 @@ location set</source>
         <translation>
             <numerusform>منذ %n يوم</numerusform>
             <numerusform>منذ %n يوم</numerusform>
-            <numerusform>منذ %n ايام</numerusform>
-            <numerusform>منذ %n ايام</numerusform>
-            <numerusform>منذ %n ايام</numerusform>
-            <numerusform>منذ %n ايام</numerusform>
+            <numerusform>منذ يومين %n</numerusform>
+            <numerusform>منذ %n أيام</numerusform>
+            <numerusform>منذ %n أيام</numerusform>
+            <numerusform>منذ %n يوم</numerusform>
         </translation>
     </message>
+    <message>
+        <source>km</source>
+        <translation>كم</translation>
+    </message>
+    <message>
+        <source>m</source>
+        <translation>م</translation>
+    </message>
+    <message>
+        <source>mi</source>
+        <translation>ميل</translation>
+    </message>
+    <message>
+        <source>ft</source>
+        <translation>قدم</translation>
+    </message>
 </context>
 <context>
     <name>Reset</name>
     <message>
         <source>Reset failed. Reboot to try again.</source>
-        <translation>فشل إعادة التعيين. أعد التشغيل للمحاولة مرة أخرى.</translation>
+        <translation>فشل إعاة الضبط. أعد التشغيل للمحاولة من جديد.</translation>
     </message>
     <message>
         <source>Are you sure you want to reset your device?</source>
         <translation>هل أنت متأكد أنك تريد إعادة ضبط جهازك؟</translation>
     </message>
-    <message>
-        <source>Resetting device...</source>
-        <translation>جارٍ إعادة ضبط الجهاز ...</translation>
-    </message>
     <message>
         <source>System Reset</source>
-        <translation>إعادة تعيين النظام</translation>
-    </message>
-    <message>
-        <source>System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot.</source>
-        <translation>تم تشغيل إعادة تعيين النظام. اضغط على تأكيد لمسح كل المحتوى والإعدادات. اضغط على إلغاء لاستئناف التمهيد.</translation>
+        <translation>إعادة ضبط النظام</translation>
     </message>
     <message>
         <source>Cancel</source>
@@ -594,108 +620,95 @@ location set</source>
     </message>
     <message>
         <source>Reboot</source>
-        <translation>اعادة التشغيل</translation>
+        <translation>إعادة التشغيل</translation>
     </message>
     <message>
         <source>Confirm</source>
         <translation>تأكيد</translation>
     </message>
     <message>
-        <source>Unable to mount data partition. Press confirm to reset your device.</source>
-        <translation>تعذر تحميل قسم البيانات. اضغط على تأكيد لإعادة ضبط جهازك.</translation>
+        <source>Resetting device...
+This may take up to a minute.</source>
+        <translation>يتم إعادة ضبط الجهاز...
+قد يستغرق الأمر حوالي الدقيقة.</translation>
     </message>
-</context>
-<context>
-    <name>RichTextDialog</name>
     <message>
-        <source>Ok</source>
-        <translation>موافق</translation>
+        <source>Press confirm to erase all content and settings. Press cancel to resume boot.</source>
+        <translation>اضغط على تأكيد لمسح جميع المحتويات والإعدادات. اضغط على إلغاء لمتابعة التشغيل.</translation>
+    </message>
+    <message>
+        <source>Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device.</source>
+        <translation>غير قادر على تحميل جزء البيانات. قد يكون الجزء تالفاً. اضغط على تأكيد لمسح جهازك وإعادة ضبطه.</translation>
     </message>
 </context>
 <context>
     <name>SettingsWindow</name>
     <message>
         <source>×</source>
-        <translation>x</translation>
+        <translation>×</translation>
     </message>
     <message>
         <source>Device</source>
-        <translation>جهاز</translation>
+        <translation>الجهاز</translation>
     </message>
     <message>
         <source>Network</source>
-        <translation>شبكة الاتصال</translation>
+        <translation>الشبكة</translation>
     </message>
     <message>
         <source>Toggles</source>
-        <translation>التبديل</translation>
+        <translation>المثبتتات</translation>
     </message>
     <message>
         <source>Software</source>
-        <translation>برمجة</translation>
-    </message>
-    <message>
-        <source>Navigation</source>
-        <translation>ملاحة</translation>
+        <translation>البرنامج</translation>
     </message>
 </context>
 <context>
     <name>Setup</name>
     <message>
         <source>WARNING: Low Voltage</source>
-        <translation>تحذير: الجهد المنخفض</translation>
+        <translation>تحذير: الجهد منخفض</translation>
     </message>
     <message>
         <source>Power your device in a car with a harness or proceed at your own risk.</source>
-        <translation>قم بتشغيل جهازك في سيارة باستخدام أداة تثبيت أو المضي قدمًا على مسؤوليتك الخاصة.</translation>
+        <translation>شغل جهازك في السيارة عن طريق شرطان التوصيل، أو تابع على مسؤوليتك.</translation>
     </message>
     <message>
         <source>Power off</source>
-        <translation>اطفئ الجهاز</translation>
+        <translation>إيقاف التشغيل</translation>
     </message>
     <message>
         <source>Continue</source>
-        <translation>أكمل</translation>
+        <translation>متابعة</translation>
     </message>
     <message>
         <source>Getting Started</source>
-        <translation>ابدء</translation>
+        <translation>البدء</translation>
     </message>
     <message>
         <source>Before we get on the road, let’s finish installation and cover some details.</source>
-        <translation>قبل أن ننطلق على الطريق ، دعنا ننتهي من التثبيت ونغطي بعض التفاصيل.</translation>
+        <translation>قبل أن ننطلق في الطريق، دعنا ننتهي من التثبيت ونغطي بعض التفاصيل.</translation>
     </message>
     <message>
         <source>Connect to Wi-Fi</source>
-        <translation>اتصل بشبكة Wi-Fi</translation>
+        <translation>الاتصال بشبكة الواي فاي</translation>
     </message>
     <message>
         <source>Back</source>
-        <translation>خلف</translation>
+        <translation>السابق</translation>
     </message>
     <message>
         <source>Continue without Wi-Fi</source>
-        <translation>استمر بدون Wi-Fi</translation>
+        <translation>المتابعة بدون شبكة الواي فاي</translation>
     </message>
     <message>
         <source>Waiting for internet</source>
-        <translation>في انتظار الاتصال بالإنترنت</translation>
-    </message>
-    <message>
-        <source>Choose Software to Install</source>
-        <translation>اختر البرنامج المراد تثبيته</translation>
-    </message>
-    <message>
-        <source>Dashcam</source>
-        <translation>Dashcam</translation>
-    </message>
-    <message>
-        <source>Custom Software</source>
-        <translation>برامج مخصصة</translation>
+        <translation>بانتظار الاتصال بالإنترنت</translation>
     </message>
     <message>
         <source>Enter URL</source>
-        <translation>إدخال عنوان الموقع</translation>
+        <translation>أدخل رابط URL</translation>
     </message>
     <message>
         <source>for Custom Software</source>
@@ -703,7 +716,7 @@ location set</source>
     </message>
     <message>
         <source>Downloading...</source>
-        <translation>جارى التحميل...</translation>
+        <translation>يتم الآن التنزيل...</translation>
     </message>
     <message>
         <source>Download Failed</source>
@@ -711,15 +724,27 @@ location set</source>
     </message>
     <message>
         <source>Ensure the entered URL is valid, and the device’s internet connection is good.</source>
-        <translation>تأكد من أن عنوان موقع الويب الذي تم إدخاله صالح ، وأن اتصال الجهاز بالإنترنت جيد.</translation>
+        <translation>تأكد من أن رابط URL الذي أدخلته صالح، وأن اتصال الجهاز بالإنترنت جيد.</translation>
     </message>
     <message>
         <source>Reboot device</source>
-        <translation>إعادة تشغيل الجهاز</translation>
+        <translation>إعادة التشغيل</translation>
     </message>
     <message>
         <source>Start over</source>
-        <translation>ابدأ من جديد</translation>
+        <translation>البدء من جديد</translation>
+    </message>
+    <message>
+        <source>Something went wrong. Reboot the device.</source>
+        <translation>حدث خطأ ما. أعد التشغيل الجهاز.</translation>
+    </message>
+    <message>
+        <source>No custom software found at this URL.</source>
+        <translation>لم يتم العثور على برنامج خاص لعنوان URL ها.</translation>
+    </message>
+    <message>
+        <source>Select a language</source>
+        <translation>اختر لغة</translation>
     </message>
 </context>
 <context>
@@ -730,11 +755,11 @@ location set</source>
     </message>
     <message>
         <source>Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer.</source>
-        <translation>قم بإقران جهازك بفاصلة connect (connect.comma.ai) واطلب عرض comma prime الخاص بك.</translation>
+        <translation>اقرن جهازك بجهاز (connect.comma.ai) واحصل على عرضك من comma prime.</translation>
     </message>
     <message>
         <source>Pair device</source>
-        <translation>إقران الجهاز</translation>
+        <translation>اقتران الجهاز</translation>
     </message>
 </context>
 <context>
@@ -761,7 +786,7 @@ location set</source>
     </message>
     <message>
         <source>HIGH</source>
-        <translation>عالي</translation>
+        <translation>مرتفع</translation>
     </message>
     <message>
         <source>GOOD</source>
@@ -773,7 +798,7 @@ location set</source>
     </message>
     <message>
         <source>VEHICLE</source>
-        <translation>مركبة</translation>
+        <translation>المركبة</translation>
     </message>
     <message>
         <source>NO</source>
@@ -822,140 +847,112 @@ location set</source>
 </context>
 <context>
     <name>SoftwarePanel</name>
-    <message>
-        <source>Git Branch</source>
-        <translation type="vanished">Git Branch</translation>
-    </message>
-    <message>
-        <source>Git Commit</source>
-        <translation type="vanished">Git Commit</translation>
-    </message>
-    <message>
-        <source>OS Version</source>
-        <translation type="vanished">إصدار نظام التشغيل</translation>
-    </message>
-    <message>
-        <source>Version</source>
-        <translation type="vanished">إصدار</translation>
-    </message>
-    <message>
-        <source>Last Update Check</source>
-        <translation type="vanished">التحقق من آخر تحديث</translation>
-    </message>
-    <message>
-        <source>The last time openpilot successfully checked for an update. The updater only runs while the car is off.</source>
-        <translation type="vanished">آخر مرة نجح برنامج openpilot في التحقق من التحديث. يعمل المحدث فقط أثناء إيقاف تشغيل السيارة.</translation>
-    </message>
-    <message>
-        <source>Check for Update</source>
-        <translation type="vanished">فحص التحديثات</translation>
-    </message>
-    <message>
-        <source>CHECKING</source>
-        <translation type="vanished">تدقيق</translation>
-    </message>
-    <message>
-        <source>Switch Branch</source>
-        <translation type="vanished">تبديل الفرع</translation>
-    </message>
-    <message>
-        <source>ENTER</source>
-        <translation type="vanished">أدخل</translation>
-    </message>
-    <message>
-        <source>The new branch will be pulled the next time the updater runs.</source>
-        <translation type="vanished">سيتم سحب الفرع الجديد في المرة التالية التي يتم فيها تشغيل أداة التحديث.</translation>
-    </message>
-    <message>
-        <source>Enter branch name</source>
-        <translation type="vanished">أدخل اسم الفرع</translation>
-    </message>
     <message>
         <source>UNINSTALL</source>
-        <translation>الغاء التثبيت</translation>
+        <translation>إلغاء التثبيت</translation>
     </message>
     <message>
         <source>Uninstall %1</source>
-        <translation>الغاء التثبيت %1</translation>
+        <translation>إلغاء التثبيت %1</translation>
     </message>
     <message>
         <source>Are you sure you want to uninstall?</source>
         <translation>هل أنت متأكد أنك تريد إلغاء التثبيت؟</translation>
     </message>
-    <message>
-        <source>failed to fetch update</source>
-        <translation type="vanished">فشل في جلب التحديث</translation>
-    </message>
     <message>
         <source>CHECK</source>
-        <translation>تأكد الان</translation>
+        <translation>التحقق</translation>
     </message>
     <message>
         <source>Updates are only downloaded while the car is off.</source>
-        <translation type="unfinished"></translation>
+        <translation>يتم تحميل التحديثات فقط عندما تكون السيارة متوقفة.</translation>
     </message>
     <message>
         <source>Current Version</source>
-        <translation type="unfinished"></translation>
+        <translation>النسخة الحالية</translation>
     </message>
     <message>
         <source>Download</source>
-        <translation type="unfinished"></translation>
+        <translation>تنزيل</translation>
     </message>
     <message>
         <source>Install Update</source>
-        <translation type="unfinished"></translation>
+        <translation>تثبيت التحديث</translation>
     </message>
     <message>
         <source>INSTALL</source>
-        <translation type="unfinished"></translation>
+        <translation>تثبيت</translation>
     </message>
     <message>
         <source>Target Branch</source>
-        <translation type="unfinished"></translation>
+        <translation>فرع الهدف</translation>
     </message>
     <message>
         <source>SELECT</source>
-        <translation type="unfinished"></translation>
+        <translation>اختيار</translation>
     </message>
     <message>
         <source>Select a branch</source>
-        <translation type="unfinished"></translation>
+        <translation>اختر فرعاً</translation>
+    </message>
+    <message>
+        <source>Uninstall</source>
+        <translation>إلغاء التثبيت</translation>
+    </message>
+    <message>
+        <source>failed to check for update</source>
+        <translation>فشل التحقق من التحديث</translation>
+    </message>
+    <message>
+        <source>DOWNLOAD</source>
+        <translation>تنزيل</translation>
+    </message>
+    <message>
+        <source>update available</source>
+        <translation>يتوفر تحديث</translation>
+    </message>
+    <message>
+        <source>never</source>
+        <translation>إطلاقاً</translation>
+    </message>
+    <message>
+        <source>up to date, last checked %1</source>
+        <translation>أحدث نسخة، آخر تحقق %1</translation>
     </message>
 </context>
 <context>
     <name>SshControl</name>
     <message>
         <source>SSH Keys</source>
-        <translation>SSH Keys</translation>
+        <translation>مفاتيح SSH</translation>
     </message>
     <message>
         <source>Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username.</source>
-        <translation>تحذير: هذا يمنح SSH الوصول إلى جميع المفاتيح العامة في إعدادات GitHub. لا تدخل أبدًا اسم مستخدم GitHub بخلاف اسم المستخدم الخاص بك. لن يطلب منك موظف comma أبدًا إضافة اسم مستخدم GitHub الخاص به.</translation>
+        <translation>تنبيه: هذا يمنح SSH إمكانية الوصول إلى جميع المفاتيح العامة في إعدادات GitHub. لا تقم بإدخال اسم مستخدم GitHub بدلاً من اسمك. لن تطلب منك comma employee إطلاقاً أن تضيف اسم مستخدم GitHub الخاص بهم.</translation>
     </message>
     <message>
         <source>ADD</source>
-        <translation>أضف</translation>
+        <translation>إضافة</translation>
     </message>
     <message>
         <source>Enter your GitHub username</source>
-        <translation>أدخل اسم مستخدم GitHub الخاص بك</translation>
+        <translation>ادخل اسم المستخدم GitHub الخاص بك</translation>
     </message>
     <message>
         <source>LOADING</source>
-        <translation>جار التحميل</translation>
+        <translation>يتم التحميل</translation>
     </message>
     <message>
         <source>REMOVE</source>
-        <translation>نزع</translation>
+        <translation>إزالة</translation>
     </message>
     <message>
         <source>Username &apos;%1&apos; has no keys on GitHub</source>
-        <translation>لا يحتوي اسم المستخدم &apos;%1&apos; على مفاتيح على GitHub</translation>
+        <translation>لا يحتوي اسم المستخدم &apos;%1&apos; أي مفاتيح على GitHub</translation>
     </message>
     <message>
         <source>Request timed out</source>
-        <translation>انتهت مهلة الطلب</translation>
+        <translation>انتهى وقت الطلب</translation>
     </message>
     <message>
         <source>Username &apos;%1&apos; doesn&apos;t exist on GitHub</source>
@@ -966,18 +963,18 @@ location set</source>
     <name>SshToggle</name>
     <message>
         <source>Enable SSH</source>
-        <translation>تفعيل SSH</translation>
+        <translation>تمكين SSH</translation>
     </message>
 </context>
 <context>
     <name>TermsPage</name>
     <message>
         <source>Terms &amp; Conditions</source>
-        <translation>البنود و الظروف</translation>
+        <translation>الشروط والأحكام</translation>
     </message>
     <message>
         <source>Decline</source>
-        <translation>انحدار</translation>
+        <translation>رفض</translation>
     </message>
     <message>
         <source>Scroll to accept</source>
@@ -985,34 +982,34 @@ location set</source>
     </message>
     <message>
         <source>Agree</source>
-        <translation>موافق</translation>
+        <translation>أوافق</translation>
     </message>
 </context>
 <context>
     <name>TogglesPanel</name>
     <message>
         <source>Enable openpilot</source>
-        <translation>تمكين openpilot</translation>
+        <translation>تمكين</translation>
     </message>
     <message>
         <source>Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off.</source>
-        <translation>استخدم نظام الطيار المفتوح للتحكم التكيفي في ثبات السرعة والحفاظ على مساعدة السائق. انتباهك مطلوب في جميع الأوقات لاستخدام هذه الميزة. يسري تغيير هذا الإعداد عند إيقاف تشغيل السيارة.</translation>
+        <translation>استخدم نظام openpilot من أجل الضبط التكيفي للسرعة والحفاظ على مساعدة السائق للبقاء في المسار. انتباهك مطلوب في جميع الأوقات مع استخدام هذه الميزة. يعمل هذا التغيير في الإعدادات عند إيقاف تشغيل السيارة.</translation>
     </message>
     <message>
         <source>Enable Lane Departure Warnings</source>
-        <translation>قم بتمكين تحذيرات مغادرة حارة السير</translation>
+        <translation>قم بتمكين تحذيرات مغادرة المسار</translation>
     </message>
     <message>
         <source>Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h).</source>
-        <translation>تلقي تنبيهات للتوجه مرة أخرى إلى الحارة عندما تنجرف سيارتك فوق خط المسار المكتشف دون تنشيط إشارة الانعطاف أثناء القيادة لمسافة تزيد عن 31 ميلاً في الساعة (50 كم / ساعة).</translation>
+        <translation>تلقي التنبيهات من أجل الالتفاف للعودة إلى المسار عندما تنحرف سيارتك فوق الخط المحدد للمسار دون تشغيل إشارة الانعطاف عند القيادة لمسافة تزيد عن 31 ميل/سا (50 كم/سا).</translation>
     </message>
     <message>
         <source>Use Metric System</source>
-        <translation>استخدم النظام المتري</translation>
+        <translation>استخدام النظام المتري</translation>
     </message>
     <message>
         <source>Display speed in km/h instead of mph.</source>
-        <translation>عرض السرعة بالكيلو متر في الساعة بدلاً من ميل في الساعة.</translation>
+        <translation>عرض السرعة بواحدات كم/سا بدلاً من ميل/سا.</translation>
     </message>
     <message>
         <source>Record and Upload Driver Camera</source>
@@ -1020,117 +1017,192 @@ location set</source>
     </message>
     <message>
         <source>Upload data from the driver facing camera and help improve the driver monitoring algorithm.</source>
-        <translation>قم بتحميل البيانات من الكاميرا المواجهة للسائق وساعد في تحسين خوارزمية مراقبة السائق.</translation>
+        <translation>تحميل البيانات من الكاميرا المواجهة للسائق، والمساعدة في تحسين خوارزمية مراقبة السائق.</translation>
     </message>
     <message>
         <source>Disengage on Accelerator Pedal</source>
-        <translation>فك الارتباط على دواسة التسريع</translation>
+        <translation>فك الارتباط عن دواسة الوقود</translation>
     </message>
     <message>
         <source>When enabled, pressing the accelerator pedal will disengage openpilot.</source>
-        <translation>عند التمكين ، سيؤدي الضغط على دواسة الوقود إلى فصل الطيار المفتوح.</translation>
+        <translation>عند تمكين هذه الميزة، فإن الضغط على دواسة الوقود سيؤدي إلى فك ارتباط openpilot.</translation>
     </message>
     <message>
         <source>Show ETA in 24h Format</source>
-        <translation>إظهار الوقت المقدر للوصول بتنسيق 24 ساعة</translation>
+        <translation>إظهار الوقت المقدر للوصول بصيغة 24 ساعة</translation>
     </message>
     <message>
         <source>Use 24h format instead of am/pm</source>
-        <translation>استخدم تنسيق 24 ساعة بدلاً من صباحًا / مساءً</translation>
+        <translation>استخدام صيغة 24 ساعة بدلاً من صباحاً/مساء</translation>
     </message>
     <message>
         <source>Show Map on Left Side of UI</source>
-        <translation>إظهار الخريطة على الجانب الأيسر من واجهة المستخدم</translation>
+        <translation>عرض الخريطة على الجانب الأيسر من واجهة المستخدم</translation>
     </message>
     <message>
         <source>Show map on left side when in split screen view.</source>
-        <translation>إظهار الخريطة على الجانب الأيسر عندما تكون في طريقة عرض الشاشة المنقسمة.</translation>
+        <translation>عرض الخريطة عل الجانب الأيسر عندما تكون وضعية العرض بطريقة الشاشة المنقسمة.</translation>
+    </message>
+    <message>
+        <source>openpilot Longitudinal Control (Alpha)</source>
+        <translation>التحكم الطولي openpilot (ألفا)</translation>
+    </message>
+    <message>
+        <source>WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB).</source>
+        <translation>تحذير: التحكم الطولي في openpilot في المرحلة ألفا لهذه السيارة، وسيقوم بتعطيل مكابح الطوارئ الآلية (AEB).</translation>
+    </message>
+    <message>
+        <source>On this car, openpilot defaults to the car&apos;s built-in ACC instead of openpilot&apos;s longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha.</source>
+        <translation>في هذه السيارة يعمل openpilot افتراضياً بالشكل المدمج في التحكم التكيفي في السرعة بدلاً من التحكم الطولي. قم بتمكين هذا الخيار من أجل الانتقال إلى التحكم الطولي. يوصى بتمكين الوضع التجريبي عند استخدام وضع التحكم الطولي ألفا من openpilot.</translation>
+    </message>
+    <message>
+        <source>Experimental Mode</source>
+        <translation>الوضع التجريبي</translation>
+    </message>
+    <message>
+        <source>Aggressive</source>
+        <translation>الهجومي</translation>
     </message>
     <message>
-        <source>openpilot Longitudinal Control</source>
-        <translation type="vanished">openpilot التحكم الطولي</translation>
+        <source>Standard</source>
+        <translation>القياسي</translation>
     </message>
     <message>
-        <source>openpilot will disable the car&apos;s radar and will take over control of gas and brakes. Warning: this disables AEB!</source>
-        <translation type="vanished">سوف يقوم برنامج openpilot بتعطيل رادار السيارة وسيتولى التحكم في الغاز والمكابح. تحذير: هذا يعطل AEB!</translation>
+        <source>Relaxed</source>
+        <translation>الراحة</translation>
     </message>
     <message>
-        <source>🌮 End-to-end longitudinal (extremely alpha) 🌮</source>
-        <translation type="unfinished"></translation>
+        <source>Driving Personality</source>
+        <translation>شخصية القيادة</translation>
     </message>
     <message>
-        <source>Experimental openpilot Longitudinal Control</source>
-        <translation type="unfinished"></translation>
+        <source>Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars.</source>
+        <translation>يوصى بالوضع القياسي. في الوضع الهجومي، سيتبع openpilot السيارات الرائدة بشكل أقرب، ويصبح أكثر هجومية في دواسات الوقود والمكابح. في وضعية الراحة يبقى openplot بعيداً لمسافة جيدة عن السيارة الرائدة.</translation>
     </message>
     <message>
-        <source>&lt;b&gt;WARNING: openpilot longitudinal control is experimental for this car and will disable AEB.&lt;/b&gt;</source>
-        <translation type="unfinished"></translation>
+        <source>openpilot defaults to driving in &lt;b&gt;chill mode&lt;/b&gt;. Experimental mode enables &lt;b&gt;alpha-level features&lt;/b&gt; that aren&apos;t ready for chill mode. Experimental features are listed below:</source>
+        <translation>يتم وضع openpilot بشكل قياسي في &lt;b&gt;وضعية الراحة&lt;/b&gt;. يمكن الوضع التجريبي &lt;b&gt;ميزات المستوى ألفا&lt;/b&gt; التي لا تكون جاهزة في وضع الراحة:</translation>
     </message>
     <message>
-        <source>Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would. Super experimental.</source>
-        <translation type="unfinished"></translation>
+        <source>End-to-End Longitudinal Control</source>
+        <translation>التحكم الطولي من طرف إلى طرف</translation>
     </message>
     <message>
-        <source>openpilot longitudinal control is not currently available for this car.</source>
-        <translation type="unfinished"></translation>
+        <source>Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected.</source>
+        <translation>دع نظام القيادة يتحكم بالوقود والمكابح. سيقوم openpilot بالقيادة كما لو أنه كائن بشري، بما في ذلك التوقف عند الإشارة الحمراء، وإشارات التوقف. وبما أن نمط القيادة يحدد سرعة القيادة، فإن السرعة المضبوطة تشكل الحد الأقصى فقط. هذه خاصية الجودة ألفا، فيجب توقع حدوث الأخطاء.</translation>
     </message>
     <message>
-        <source>Enable experimental longitudinal control to enable this.</source>
-        <translation type="unfinished"></translation>
+        <source>Navigate on openpilot</source>
+        <translation>التنقل على openpilot</translation>
+    </message>
+    <message>
+        <source>When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc.</source>
+        <translation>عندما يكون هناك وجهة للتنقل، فإن openpilot سيقوم بإدخال معلومات الخريطة في هذا النموذج. وهذا يقدم سياقاً مفيداً ويسمح لـopenpilot بالبقاء يساراً أو يميناً بالشكل المناسب عند المنعطفات/المخارج. يبقى سلوك تغيير المسار مفعلاً عند السائق،. هذه هي خاصية الجودة ألفا، ولذلك يجب توقع الأخطاء لا سيما عند المخارج والمنعطفات هذه الأخطاء قد تشمل العبور غير المقصود لخطوط المسارات، والتأخر في الخروج، والقيادة نحو الحواجز الفاصلة في المناطق المثلثة بين الطريق الرئيسي والمخارج، وغير ذلك من الأخطاء المشابهة.</translation>
+    </message>
+    <message>
+        <source>New Driving Visualization</source>
+        <translation>تصور القيادة الديد</translation>
+    </message>
+    <message>
+        <source>The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green.</source>
+        <translation>تصور القيادة سينتقل إلى الكاميرا واسعة الزاوية المواجهة للطريق في السرعات المنخفضة من أجل إظهار بعض المنعطفات بشكل أفضل. سيتم أيضاً إظهار شعار الوضع التجريبي في الزاوية العلوية اليمنى. عند تحديد وجهة التنقل، واستخدام نظام القيادة لها كوضع مدخل، سيتحول مسار القيادة على الخريطة إلى اللون الأخضر.</translation>
+    </message>
+    <message>
+        <source>Experimental mode is currently unavailable on this car since the car&apos;s stock ACC is used for longitudinal control.</source>
+        <translation>الوضع التجريبي غير متوفر حالياً في هذه السيارة نظراً لاستخدام رصيد التحكم التكيفي بالسرعة من أجل التحكم الطولي.</translation>
+    </message>
+    <message>
+        <source>openpilot longitudinal control may come in a future update.</source>
+        <translation>قد يتم الحصول على التحكم الطولي في openpilot في عمليات التحديث المستقبلية.</translation>
+    </message>
+    <message>
+        <source>An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches.</source>
+        <translation>يمكن اختبار نسخة ألفا من التحكم الطولي من openpilot، مع الوضع التجريبي، لكن على الفروع غير المطلقة.</translation>
+    </message>
+    <message>
+        <source>Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode.</source>
+        <translation>تمكين التحكم الطولي من openpilot (ألفا) للسماح بالوضع التجريبي.</translation>
     </message>
 </context>
 <context>
     <name>Updater</name>
     <message>
         <source>Update Required</source>
-        <translation>مطلوب التحديث</translation>
+        <translation>التحديث مطلوب</translation>
     </message>
     <message>
         <source>An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB.</source>
-        <translation>مطلوب تحديث نظام التشغيل. قم بتوصيل جهازك بشبكة Wi-Fi للحصول على أسرع تجربة تحديث. حجم التنزيل 1 غيغابايت تقريبًا.</translation>
+        <translation>تحديث نظام التشغيل مطلوب. قم بوصل جهازك بشبكة واي فاي من أجل تحديث أسرع. حجم التحميل حوالي 1 غيغا بايت تقريباً.</translation>
     </message>
     <message>
         <source>Connect to Wi-Fi</source>
-        <translation>اتصل بشبكة Wi-Fi</translation>
+        <translation>الاتصال بشبكة الواي فاي</translation>
     </message>
     <message>
         <source>Install</source>
-        <translation>ثبيت</translation>
+        <translation>تثبيت</translation>
     </message>
     <message>
         <source>Back</source>
-        <translation>خلف</translation>
+        <translation>السابق</translation>
     </message>
     <message>
         <source>Loading...</source>
-        <translation>جار التحميل...</translation>
+        <translation>يتم التحميل...</translation>
     </message>
     <message>
         <source>Reboot</source>
-        <translation>اعادة التشغيل</translation>
+        <translation>إعادة التشغيل</translation>
     </message>
     <message>
         <source>Update failed</source>
         <translation>فشل التحديث</translation>
     </message>
 </context>
+<context>
+    <name>WiFiPromptWidget</name>
+    <message>
+        <source>Setup Wi-Fi</source>
+        <translation>إعداد شبكة الواي فاي</translation>
+    </message>
+    <message>
+        <source>Connect to Wi-Fi to upload driving data and help improve openpilot</source>
+        <translation>الاتصال بشبكة الواي فاي لتحميل بيانات القيادة والمساهمة في تحسين openpilot</translation>
+    </message>
+    <message>
+        <source>Open Settings</source>
+        <translation>فتح الإعدادات</translation>
+    </message>
+    <message>
+        <source>Ready to upload</source>
+        <translation>جاهز للتحميل</translation>
+    </message>
+    <message>
+        <source>Training data will be pulled periodically while your device is on Wi-Fi</source>
+        <translation>سيتم سحب بيانات التدريب دورياً عندما يكون جهازك متصل بشبكة واي فاي</translation>
+    </message>
+</context>
 <context>
     <name>WifiUI</name>
     <message>
         <source>Scanning for networks...</source>
-        <translation>جارٍ البحث عن شبكات ...</translation>
+        <translation>يتم البحث عن شبكات...</translation>
     </message>
     <message>
         <source>CONNECTING...</source>
-        <translation>جارٍ الاتصال ...</translation>
+        <translation>يتم الاتصال...</translation>
     </message>
     <message>
         <source>FORGET</source>
-        <translation>نزع</translation>
+        <translation>نسيان هذه الشبكة</translation>
     </message>
     <message>
         <source>Forget Wi-Fi Network &quot;%1&quot;?</source>
-        <translation>نزع شبكة اWi-Fi &quot;%1&quot;?</translation>
+        <translation>هل تريد نسيان شبكة الواي فاي &quot;%1&quot;؟</translation>
+    </message>
+    <message>
+        <source>Forget</source>
+        <translation>نسيان</translation>
     </message>
 </context>
 </TS>
diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts
index f62810576e..e2e5771905 100644
--- a/selfdrive/ui/translations/main_de.ts
+++ b/selfdrive/ui/translations/main_de.ts
@@ -226,8 +226,8 @@
         <translation>Ausschalten</translation>
     </message>
     <message>
-        <source>openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required.</source>
-        <translation>Damit Openpilot funktioniert, darf die Installationsposition nicht mehr als 4° nach rechts/links, 5° nach oben und 8° nach unten abweichen. Openpilot kalibriert sich durchgehend, ein Zurücksetzen ist selten notwendig.</translation>
+        <source>openpilot requires the device to be mounted within 4° left or right and within 5° up or 9° down. openpilot is continuously calibrating, resetting is rarely required.</source>
+        <translation>Damit Openpilot funktioniert, darf die Installationsposition nicht mehr als 4° nach rechts/links, 5° nach oben und 9° nach unten abweichen. Openpilot kalibriert sich durchgehend, ein Zurücksetzen ist selten notwendig.</translation>
     </message>
     <message>
         <source> Your device is pointed %1° %2 and %3° %4.</source>
@@ -274,33 +274,6 @@
         <translation>Überprüfen</translation>
     </message>
 </context>
-<context>
-    <name>DriveStats</name>
-    <message>
-        <source>Drives</source>
-        <translation>Fahrten</translation>
-    </message>
-    <message>
-        <source>Hours</source>
-        <translation>Stunden</translation>
-    </message>
-    <message>
-        <source>ALL TIME</source>
-        <translation>Gesamtzeit</translation>
-    </message>
-    <message>
-        <source>PAST WEEK</source>
-        <translation>Letzte Woche</translation>
-    </message>
-    <message>
-        <source>KM</source>
-        <translation>KM</translation>
-    </message>
-    <message>
-        <source>Miles</source>
-        <translation>Meilen</translation>
-    </message>
-</context>
 <context>
     <name>DriverViewScene</name>
     <message>
@@ -354,33 +327,6 @@
         <source>hr</source>
         <translation>std</translation>
     </message>
-    <message>
-        <source>km</source>
-        <translation>km</translation>
-    </message>
-    <message>
-        <source>mi</source>
-        <translation>mi</translation>
-    </message>
-</context>
-<context>
-    <name>MapInstructions</name>
-    <message>
-        <source> km</source>
-        <translation> km</translation>
-    </message>
-    <message>
-        <source> m</source>
-        <translation> m</translation>
-    </message>
-    <message>
-        <source> mi</source>
-        <translation> mi</translation>
-    </message>
-    <message>
-        <source> ft</source>
-        <translation> fuß</translation>
-    </message>
 </context>
 <context>
     <name>MapSettings</name>
@@ -620,6 +566,22 @@
             <numerusform>vor %n Tagen</numerusform>
         </translation>
     </message>
+    <message>
+        <source>km</source>
+        <translation>km</translation>
+    </message>
+    <message>
+        <source>m</source>
+        <translation>m</translation>
+    </message>
+    <message>
+        <source>mi</source>
+        <translation>mi</translation>
+    </message>
+    <message>
+        <source>ft</source>
+        <translation>fuß</translation>
+    </message>
 </context>
 <context>
     <name>Reset</name>
@@ -762,6 +724,10 @@ This may take up to a minute.</source>
         <source>Something went wrong. Reboot the device.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Select a language</source>
+        <translation>Sprache wählen</translation>
+    </message>
 </context>
 <context>
     <name>SetupWidget</name>
@@ -1083,10 +1049,6 @@ This may take up to a minute.</source>
         <source>Experimental mode is currently unavailable on this car since the car&apos;s stock ACC is used for longitudinal control.</source>
         <translation>Der experimentelle Modus ist momentan für dieses Auto nicht verfügbar da es den eingebauten adaptiven Tempomaten des Autos benutzt.</translation>
     </message>
-    <message>
-        <source>openpilot longitudinal control may come in a future update.</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>openpilot Longitudinal Control (Alpha)</source>
         <translation type="unfinished"></translation>
@@ -1095,10 +1057,6 @@ This may take up to a minute.</source>
         <source>WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB).</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>On this car, openpilot defaults to the car&apos;s built-in ACC instead of openpilot&apos;s longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha.</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Aggressive</source>
         <translation type="unfinished"></translation>
@@ -1115,12 +1073,16 @@ This may take up to a minute.</source>
         <source>Driving Personality</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>On this car, openpilot defaults to the car&apos;s built-in ACC instead of openpilot&apos;s longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha.</source>
+        <translation type="unfinished"></translation>
+    </message>
     <message>
         <source>Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars.</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
-        <source>An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches.</source>
+        <source>End-to-End Longitudinal Control</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
@@ -1128,19 +1090,23 @@ This may take up to a minute.</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
-        <source>Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode.</source>
+        <source>When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc.</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
-        <source>End-to-End Longitudinal Control</source>
+        <source>The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>openpilot longitudinal control may come in a future update.</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
-        <source>When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits/forks.These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc.</source>
+        <source>An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches.</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
-        <source>The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner.When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green.</source>
+        <source>Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode.</source>
         <translation type="unfinished"></translation>
     </message>
 </context>
diff --git a/selfdrive/ui/translations/main_fr.ts b/selfdrive/ui/translations/main_fr.ts
new file mode 100644
index 0000000000..456efe6366
--- /dev/null
+++ b/selfdrive/ui/translations/main_fr.ts
@@ -0,0 +1,1192 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!DOCTYPE TS>
+<TS version="2.1" language="fr_FR">
+<context>
+    <name>AbstractAlert</name>
+    <message>
+        <source>Close</source>
+        <translation>Fermer</translation>
+    </message>
+    <message>
+        <source>Snooze Update</source>
+        <translation>Reporter la mise à jour</translation>
+    </message>
+    <message>
+        <source>Reboot and Update</source>
+        <translation>Redémarrer et mettre à jour</translation>
+    </message>
+</context>
+<context>
+    <name>AdvancedNetworking</name>
+    <message>
+        <source>Back</source>
+        <translation>Retour</translation>
+    </message>
+    <message>
+        <source>Enable Tethering</source>
+        <translation>Activer le partage de connexion</translation>
+    </message>
+    <message>
+        <source>Tethering Password</source>
+        <translation>Mot de passe du partage de connexion</translation>
+    </message>
+    <message>
+        <source>EDIT</source>
+        <translation>MODIFIER</translation>
+    </message>
+    <message>
+        <source>Enter new tethering password</source>
+        <translation>Entrez le nouveau mot de passe du partage de connexion</translation>
+    </message>
+    <message>
+        <source>IP Address</source>
+        <translation>Adresse IP</translation>
+    </message>
+    <message>
+        <source>Enable Roaming</source>
+        <translation>Activer l&apos;itinérance</translation>
+    </message>
+    <message>
+        <source>APN Setting</source>
+        <translation>Paramètre APN</translation>
+    </message>
+    <message>
+        <source>Enter APN</source>
+        <translation>Entrer le nom du point d&apos;accès</translation>
+    </message>
+    <message>
+        <source>leave blank for automatic configuration</source>
+        <translation>laisser vide pour une configuration automatique</translation>
+    </message>
+    <message>
+        <source>Cellular Metered</source>
+        <translation>Connexion cellulaire limitée</translation>
+    </message>
+    <message>
+        <source>Prevent large data uploads when on a metered connection</source>
+        <translation>Éviter les transferts de données importants sur une connexion limitée</translation>
+    </message>
+</context>
+<context>
+    <name>AnnotatedCameraWidget</name>
+    <message>
+        <source>km/h</source>
+        <translation>km/h</translation>
+    </message>
+    <message>
+        <source>mph</source>
+        <translation>mi/h</translation>
+    </message>
+    <message>
+        <source>MAX</source>
+        <translation>MAX</translation>
+    </message>
+    <message>
+        <source>SPEED</source>
+        <translation>VITESSE</translation>
+    </message>
+    <message>
+        <source>LIMIT</source>
+        <translation>LIMITE</translation>
+    </message>
+</context>
+<context>
+    <name>ConfirmationDialog</name>
+    <message>
+        <source>Ok</source>
+        <translation>Ok</translation>
+    </message>
+    <message>
+        <source>Cancel</source>
+        <translation>Annuler</translation>
+    </message>
+</context>
+<context>
+    <name>DeclinePage</name>
+    <message>
+        <source>You must accept the Terms and Conditions in order to use openpilot.</source>
+        <translation>Vous devez accepter les conditions générales pour utiliser openpilot.</translation>
+    </message>
+    <message>
+        <source>Back</source>
+        <translation>Retour</translation>
+    </message>
+    <message>
+        <source>Decline, uninstall %1</source>
+        <translation>Refuser, désinstaller %1</translation>
+    </message>
+</context>
+<context>
+    <name>DestinationWidget</name>
+    <message>
+        <source>Home</source>
+        <translation>Domicile</translation>
+    </message>
+    <message>
+        <source>Work</source>
+        <translation>Travail</translation>
+    </message>
+    <message>
+        <source>No destination set</source>
+        <translation>Aucune destination définie</translation>
+    </message>
+    <message>
+        <source>home</source>
+        <translation>domicile</translation>
+    </message>
+    <message>
+        <source>work</source>
+        <translation>travail</translation>
+    </message>
+    <message>
+        <source>No %1 location set</source>
+        <translation>Aucun lieu %1 défini</translation>
+    </message>
+</context>
+<context>
+    <name>DevicePanel</name>
+    <message>
+        <source>Dongle ID</source>
+        <translation>Dongle ID</translation>
+    </message>
+    <message>
+        <source>N/A</source>
+        <translation>N/A</translation>
+    </message>
+    <message>
+        <source>Serial</source>
+        <translation>N° de série</translation>
+    </message>
+    <message>
+        <source>Driver Camera</source>
+        <translation>Caméra conducteur</translation>
+    </message>
+    <message>
+        <source>PREVIEW</source>
+        <translation>APERÇU</translation>
+    </message>
+    <message>
+        <source>Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off)</source>
+        <translation>Aperçu de la caméra orientée vers le conducteur pour assurer une bonne visibilité de la surveillance du conducteur. (véhicule doit être éteint)</translation>
+    </message>
+    <message>
+        <source>Reset Calibration</source>
+        <translation>Réinitialiser la calibration</translation>
+    </message>
+    <message>
+        <source>RESET</source>
+        <translation>RÉINITIALISER</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to reset calibration?</source>
+        <translation>Êtes-vous sûr de vouloir réinitialiser la calibration ?</translation>
+    </message>
+    <message>
+        <source>Reset</source>
+        <translation>Réinitialiser</translation>
+    </message>
+    <message>
+        <source>Review Training Guide</source>
+        <translation>Revoir le guide de formation</translation>
+    </message>
+    <message>
+        <source>REVIEW</source>
+        <translation>REVOIR</translation>
+    </message>
+    <message>
+        <source>Review the rules, features, and limitations of openpilot</source>
+        <translation>Revoir les règles, fonctionnalités et limitations d&apos;openpilot</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to review the training guide?</source>
+        <translation>Êtes-vous sûr de vouloir revoir le guide de formation ?</translation>
+    </message>
+    <message>
+        <source>Review</source>
+        <translation>Revoir</translation>
+    </message>
+    <message>
+        <source>Regulatory</source>
+        <translation>Réglementaire</translation>
+    </message>
+    <message>
+        <source>VIEW</source>
+        <translation>VOIR</translation>
+    </message>
+    <message>
+        <source>Change Language</source>
+        <translation>Changer de langue</translation>
+    </message>
+    <message>
+        <source>CHANGE</source>
+        <translation>CHANGER</translation>
+    </message>
+    <message>
+        <source>Select a language</source>
+        <translation>Choisir une langue</translation>
+    </message>
+    <message>
+        <source>Reboot</source>
+        <translation>Redémarrer</translation>
+    </message>
+    <message>
+        <source>Power Off</source>
+        <translation>Éteindre</translation>
+    </message>
+    <message>
+        <source>openpilot requires the device to be mounted within 4° left or right and within 5° up or 9° down. openpilot is continuously calibrating, resetting is rarely required.</source>
+        <translation>openpilot nécessite que l&apos;appareil soit monté à 4° à gauche ou à droite et à 5° vers le haut ou 9° vers le bas. openpilot se calibre en continu, la réinitialisation est rarement nécessaire.</translation>
+    </message>
+    <message>
+        <source> Your device is pointed %1° %2 and %3° %4.</source>
+        <translation> Votre appareil est orienté %1° %2 et %3° %4.</translation>
+    </message>
+    <message>
+        <source>down</source>
+        <translation>bas</translation>
+    </message>
+    <message>
+        <source>up</source>
+        <translation>haut</translation>
+    </message>
+    <message>
+        <source>left</source>
+        <translation>gauche</translation>
+    </message>
+    <message>
+        <source>right</source>
+        <translation>droite</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to reboot?</source>
+        <translation>Êtes-vous sûr de vouloir redémarrer ?</translation>
+    </message>
+    <message>
+        <source>Disengage to Reboot</source>
+        <translation>Désengager pour redémarrer</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to power off?</source>
+        <translation>Êtes-vous sûr de vouloir éteindre ?</translation>
+    </message>
+    <message>
+        <source>Disengage to Power Off</source>
+        <translation>Désengager pour éteindre</translation>
+    </message>
+</context>
+<context>
+    <name>DriverViewScene</name>
+    <message>
+        <source>camera starting</source>
+        <translation>démarrage de la caméra</translation>
+    </message>
+</context>
+<context>
+    <name>ExperimentalModeButton</name>
+    <message>
+        <source>EXPERIMENTAL MODE ON</source>
+        <translation>MODE EXPÉRIMENTAL ACTIVÉ</translation>
+    </message>
+    <message>
+        <source>CHILL MODE ON</source>
+        <translation>MODE DÉTENTE ACTIVÉ</translation>
+    </message>
+</context>
+<context>
+    <name>InputDialog</name>
+    <message>
+        <source>Cancel</source>
+        <translation>Annuler</translation>
+    </message>
+    <message numerus="yes">
+        <source>Need at least %n character(s)!</source>
+        <translation>
+            <numerusform>Besoin d&apos;au moins %n caractère !</numerusform>
+            <numerusform>Besoin d&apos;au moins %n caractères !</numerusform>
+        </translation>
+    </message>
+</context>
+<context>
+    <name>Installer</name>
+    <message>
+        <source>Installing...</source>
+        <translation>Installation...</translation>
+    </message>
+</context>
+<context>
+    <name>MapETA</name>
+    <message>
+        <source>eta</source>
+        <translation>eta</translation>
+    </message>
+    <message>
+        <source>min</source>
+        <translation>min</translation>
+    </message>
+    <message>
+        <source>hr</source>
+        <translation>h</translation>
+    </message>
+</context>
+<context>
+    <name>MapSettings</name>
+    <message>
+        <source>NAVIGATION</source>
+        <translation>NAVIGATION</translation>
+    </message>
+    <message>
+        <source>Manage at connect.comma.ai</source>
+        <translation>Gérer sur connect.comma.ai</translation>
+    </message>
+</context>
+<context>
+    <name>MapWindow</name>
+    <message>
+        <source>Map Loading</source>
+        <translation>Chargement de la carte</translation>
+    </message>
+    <message>
+        <source>Waiting for GPS</source>
+        <translation>En attente du GPS</translation>
+    </message>
+    <message>
+        <source>Waiting for route</source>
+        <translation>En attente d&apos;un trajet</translation>
+    </message>
+</context>
+<context>
+    <name>MultiOptionDialog</name>
+    <message>
+        <source>Select</source>
+        <translation>Sélectionner</translation>
+    </message>
+    <message>
+        <source>Cancel</source>
+        <translation>Annuler</translation>
+    </message>
+</context>
+<context>
+    <name>Networking</name>
+    <message>
+        <source>Advanced</source>
+        <translation>Avancé</translation>
+    </message>
+    <message>
+        <source>Enter password</source>
+        <translation>Entrer le mot de passe</translation>
+    </message>
+    <message>
+        <source>for &quot;%1&quot;</source>
+        <translation>pour &quot;%1&quot;</translation>
+    </message>
+    <message>
+        <source>Wrong password</source>
+        <translation>Mot de passe incorrect</translation>
+    </message>
+</context>
+<context>
+    <name>OffroadAlert</name>
+    <message>
+        <source>Device temperature too high. System cooling down before starting. Current internal component temperature: %1</source>
+        <translation>Température de l&apos;appareil trop élevée. Le système doit refroidir avant de démarrer. Température actuelle de l&apos;appareil : %1</translation>
+    </message>
+    <message>
+        <source>Immediately connect to the internet to check for updates. If you do not connect to the internet, openpilot won&apos;t engage in %1</source>
+        <translation>Connectez-vous immédiatement à internet pour vérifier les mises à jour. Si vous ne vous connectez pas à internet, openpilot ne s&apos;engagera pas dans %1</translation>
+    </message>
+    <message>
+        <source>Connect to internet to check for updates. openpilot won&apos;t automatically start until it connects to internet to check for updates.</source>
+        <translation>Connectez l&apos;appareil à internet pour vérifier les mises à jour. openpilot ne démarrera pas automatiquement tant qu&apos;il ne se connecte pas à internet pour vérifier les mises à jour.</translation>
+    </message>
+    <message>
+        <source>Unable to download updates
+%1</source>
+        <translation>Impossible de télécharger les mises à jour
+%1</translation>
+    </message>
+    <message>
+        <source>Invalid date and time settings, system won&apos;t start. Connect to internet to set time.</source>
+        <translation>Paramètres de date et d&apos;heure invalides, le système ne démarrera pas. Connectez l&apos;appareil à Internet pour régler l&apos;heure.</translation>
+    </message>
+    <message>
+        <source>Taking camera snapshots. System won&apos;t start until finished.</source>
+        <translation>Capture de clichés photo. Le système ne démarrera pas tant qu&apos;il n&apos;est pas terminé.</translation>
+    </message>
+    <message>
+        <source>An update to your device&apos;s operating system is downloading in the background. You will be prompted to update when it&apos;s ready to install.</source>
+        <translation>Une mise à jour du système d&apos;exploitation de votre appareil est en cours de téléchargement en arrière-plan. Vous serez invité à effectuer la mise à jour lorsqu&apos;elle sera prête à être installée.</translation>
+    </message>
+    <message>
+        <source>Device failed to register. It will not connect to or upload to comma.ai servers, and receives no support from comma.ai. If this is an official device, visit https://comma.ai/support.</source>
+        <translation>L&apos;appareil n&apos;a pas réussi à s&apos;enregistrer. Il ne se connectera pas aux serveurs de comma.ai, n&apos;enverra rien et ne recevra aucune assistance de comma.ai. S&apos;il s&apos;agit d&apos;un appareil officiel, visitez https://comma.ai/support.</translation>
+    </message>
+    <message>
+        <source>NVMe drive not mounted.</source>
+        <translation>Le disque NVMe n&apos;est pas monté.</translation>
+    </message>
+    <message>
+        <source>Unsupported NVMe drive detected. Device may draw significantly more power and overheat due to the unsupported NVMe.</source>
+        <translation>Disque NVMe non supporté détecté. L&apos;appareil peut consommer beaucoup plus d&apos;énergie et surchauffer en raison du NVMe non supporté.</translation>
+    </message>
+    <message>
+        <source>openpilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please submit a pull request to add the firmware versions to the proper vehicle. Need help? Join discord.comma.ai.</source>
+        <translation>openpilot n&apos;a pas pu identifier votre voiture. Votre voiture n&apos;est pas supportée ou ses ECUs ne sont pas reconnues. Veuillez soumettre un pull request pour ajouter les versions de firmware au véhicule approprié. Besoin d&apos;aide ? Rejoignez discord.comma.ai.</translation>
+    </message>
+    <message>
+        <source>openpilot was unable to identify your car. Check integrity of cables and ensure all connections are secure, particularly that the comma power is fully inserted in the OBD-II port of the vehicle. Need help? Join discord.comma.ai.</source>
+        <translation>openpilot n&apos;a pas pu identifier votre voiture. Vérifiez l&apos;intégrité des câbles et assurez-vous que toutes les connexions sont correctes, en particulier l&apos;alimentation du comma est totalement insérée dans le port OBD-II du véhicule. Besoin d&apos;aide ? Rejoignez discord.comma.ai.</translation>
+    </message>
+    <message>
+        <source>openpilot detected a change in the device&apos;s mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.</source>
+        <translation>openpilot a détecté un changement dans la position de montage de l&apos;appareil. Assurez-vous que l&apos;appareil est totalement inséré dans le support et que le support est fermement fixé au pare-brise.</translation>
+    </message>
+</context>
+<context>
+    <name>OffroadHome</name>
+    <message>
+        <source>UPDATE</source>
+        <translation>MISE À JOUR</translation>
+    </message>
+    <message>
+        <source> ALERTS</source>
+        <translation> ALERTES</translation>
+    </message>
+    <message>
+        <source> ALERT</source>
+        <translation> ALERTE</translation>
+    </message>
+</context>
+<context>
+    <name>PairingPopup</name>
+    <message>
+        <source>Pair your device to your comma account</source>
+        <translation>Associez votre appareil à votre compte comma</translation>
+    </message>
+    <message>
+        <source>Go to https://connect.comma.ai on your phone</source>
+        <translation>Allez sur https://connect.comma.ai sur votre téléphone</translation>
+    </message>
+    <message>
+        <source>Click &quot;add new device&quot; and scan the QR code on the right</source>
+        <translation>Cliquez sur &quot;ajouter un nouvel appareil&quot; et scannez le code QR à droite</translation>
+    </message>
+    <message>
+        <source>Bookmark connect.comma.ai to your home screen to use it like an app</source>
+        <translation>Ajoutez connect.comma.ai à votre écran d&apos;accueil pour l&apos;utiliser comme une application</translation>
+    </message>
+</context>
+<context>
+    <name>ParamControl</name>
+    <message>
+        <source>Enable</source>
+        <translation>Activer</translation>
+    </message>
+    <message>
+        <source>Cancel</source>
+        <translation>Annuler</translation>
+    </message>
+</context>
+<context>
+    <name>PrimeAdWidget</name>
+    <message>
+        <source>Upgrade Now</source>
+        <translation>Mettre à niveau</translation>
+    </message>
+    <message>
+        <source>Become a comma prime member at connect.comma.ai</source>
+        <translation>Devenez membre comma prime sur connect.comma.ai</translation>
+    </message>
+    <message>
+        <source>PRIME FEATURES:</source>
+        <translation>FONCTIONNALITÉS PRIME :</translation>
+    </message>
+    <message>
+        <source>Remote access</source>
+        <translation>Accès à distance</translation>
+    </message>
+    <message>
+        <source>24/7 LTE connectivity</source>
+        <translation>Connexion LTE 24/7</translation>
+    </message>
+    <message>
+        <source>1 year of drive storage</source>
+        <translation>1 an de stockage de trajets</translation>
+    </message>
+    <message>
+        <source>Turn-by-turn navigation</source>
+        <translation>Navigation étape par étape</translation>
+    </message>
+</context>
+<context>
+    <name>PrimeUserWidget</name>
+    <message>
+        <source>✓ SUBSCRIBED</source>
+        <translation>✓ ABONNÉ</translation>
+    </message>
+    <message>
+        <source>comma prime</source>
+        <translation>comma prime</translation>
+    </message>
+</context>
+<context>
+    <name>QObject</name>
+    <message>
+        <source>Reboot</source>
+        <translation>Redémarrer</translation>
+    </message>
+    <message>
+        <source>Exit</source>
+        <translation>Quitter</translation>
+    </message>
+    <message>
+        <source>dashcam</source>
+        <translation>dashcam</translation>
+    </message>
+    <message>
+        <source>openpilot</source>
+        <translation>openpilot</translation>
+    </message>
+    <message numerus="yes">
+        <source>%n minute(s) ago</source>
+        <translation>
+            <numerusform>il y a %n minute</numerusform>
+            <numerusform>il y a %n minutes</numerusform>
+        </translation>
+    </message>
+    <message numerus="yes">
+        <source>%n hour(s) ago</source>
+        <translation>
+            <numerusform>il y a %n heure</numerusform>
+            <numerusform>il y a %n heures</numerusform>
+        </translation>
+    </message>
+    <message numerus="yes">
+        <source>%n day(s) ago</source>
+        <translation>
+            <numerusform>il y a %n jour</numerusform>
+            <numerusform>il y a %n jours</numerusform>
+        </translation>
+    </message>
+    <message>
+        <source>km</source>
+        <translation>km</translation>
+    </message>
+    <message>
+        <source>m</source>
+        <translation>m</translation>
+    </message>
+    <message>
+        <source>mi</source>
+        <translation>mi</translation>
+    </message>
+    <message>
+        <source>ft</source>
+        <translation>ft</translation>
+    </message>
+</context>
+<context>
+    <name>Reset</name>
+    <message>
+        <source>Reset failed. Reboot to try again.</source>
+        <translation>Réinitialisation échouée. Redémarrez pour réessayer.</translation>
+    </message>
+    <message>
+        <source>Resetting device...
+This may take up to a minute.</source>
+        <translation>Réinitialisation de l&apos;appareil...
+Cela peut prendre jusqu&apos;à une minute.</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to reset your device?</source>
+        <translation>Êtes-vous sûr de vouloir réinitialiser votre appareil ?</translation>
+    </message>
+    <message>
+        <source>System Reset</source>
+        <translation>Réinitialisation du système</translation>
+    </message>
+    <message>
+        <source>Press confirm to erase all content and settings. Press cancel to resume boot.</source>
+        <translation>Appuyez sur confirmer pour effacer tout le contenu et les paramètres. Appuyez sur annuler pour reprendre le démarrage.</translation>
+    </message>
+    <message>
+        <source>Cancel</source>
+        <translation>Annuler</translation>
+    </message>
+    <message>
+        <source>Reboot</source>
+        <translation>Redémarrer</translation>
+    </message>
+    <message>
+        <source>Confirm</source>
+        <translation>Confirmer</translation>
+    </message>
+    <message>
+        <source>Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device.</source>
+        <translation>Impossible de monter la partition data. La partition peut être corrompue. Appuyez sur confirmer pour effacer et réinitialiser votre appareil.</translation>
+    </message>
+</context>
+<context>
+    <name>SettingsWindow</name>
+    <message>
+        <source>×</source>
+        <translation>×</translation>
+    </message>
+    <message>
+        <source>Device</source>
+        <translation>Appareil</translation>
+    </message>
+    <message>
+        <source>Network</source>
+        <translation>Réseau</translation>
+    </message>
+    <message>
+        <source>Toggles</source>
+        <translation>Options</translation>
+    </message>
+    <message>
+        <source>Software</source>
+        <translation>Logiciel</translation>
+    </message>
+</context>
+<context>
+    <name>Setup</name>
+    <message>
+        <source>Something went wrong. Reboot the device.</source>
+        <translation>Un problème est survenu. Redémarrez l&apos;appareil.</translation>
+    </message>
+    <message>
+        <source>Ensure the entered URL is valid, and the device’s internet connection is good.</source>
+        <translation>Assurez-vous que l&apos;URL saisie est valide et que la connexion internet de l&apos;appareil est bonne.</translation>
+    </message>
+    <message>
+        <source>No custom software found at this URL.</source>
+        <translation>Aucun logiciel personnalisé trouvé à cette URL.</translation>
+    </message>
+    <message>
+        <source>WARNING: Low Voltage</source>
+        <translation>ATTENTION : Tension faible</translation>
+    </message>
+    <message>
+        <source>Power your device in a car with a harness or proceed at your own risk.</source>
+        <translation>Alimentez votre appareil dans une voiture avec un harness ou continuez à vos risques et périls.</translation>
+    </message>
+    <message>
+        <source>Power off</source>
+        <translation>Éteindre</translation>
+    </message>
+    <message>
+        <source>Continue</source>
+        <translation>Continuer</translation>
+    </message>
+    <message>
+        <source>Getting Started</source>
+        <translation>Commencer</translation>
+    </message>
+    <message>
+        <source>Before we get on the road, let’s finish installation and cover some details.</source>
+        <translation>Avant de prendre la route, terminons l&apos;installation et passons en revue quelques détails.</translation>
+    </message>
+    <message>
+        <source>Connect to Wi-Fi</source>
+        <translation>Se connecter au Wi-Fi</translation>
+    </message>
+    <message>
+        <source>Back</source>
+        <translation>Retour</translation>
+    </message>
+    <message>
+        <source>Enter URL</source>
+        <translation>Entrer l&apos;URL</translation>
+    </message>
+    <message>
+        <source>for Custom Software</source>
+        <translation>pour logiciel personnalisé</translation>
+    </message>
+    <message>
+        <source>Continue without Wi-Fi</source>
+        <translation>Continuer sans Wi-Fi</translation>
+    </message>
+    <message>
+        <source>Waiting for internet</source>
+        <translation>En attente d&apos;internet</translation>
+    </message>
+    <message>
+        <source>Downloading...</source>
+        <translation>Téléchargement...</translation>
+    </message>
+    <message>
+        <source>Download Failed</source>
+        <translation>Échec du téléchargement</translation>
+    </message>
+    <message>
+        <source>Reboot device</source>
+        <translation>Redémarrer l&apos;appareil</translation>
+    </message>
+    <message>
+        <source>Start over</source>
+        <translation>Recommencer</translation>
+    </message>
+    <message>
+        <source>Select a language</source>
+        <translation>Choisir une langue</translation>
+    </message>
+</context>
+<context>
+    <name>SetupWidget</name>
+    <message>
+        <source>Finish Setup</source>
+        <translation>Terminer l&apos;installation</translation>
+    </message>
+    <message>
+        <source>Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer.</source>
+        <translation>Associez votre appareil avec comma connect (connect.comma.ai) et profitez de l&apos;offre comma prime.</translation>
+    </message>
+    <message>
+        <source>Pair device</source>
+        <translation>Associer l&apos;appareil</translation>
+    </message>
+</context>
+<context>
+    <name>Sidebar</name>
+    <message>
+        <source>CONNECT</source>
+        <translation>CONNECTER</translation>
+    </message>
+    <message>
+        <source>OFFLINE</source>
+        <translation>HORS LIGNE</translation>
+    </message>
+    <message>
+        <source>ONLINE</source>
+        <translation>EN LIGNE</translation>
+    </message>
+    <message>
+        <source>ERROR</source>
+        <translation>ERREUR</translation>
+    </message>
+    <message>
+        <source>TEMP</source>
+        <translation>TEMP</translation>
+    </message>
+    <message>
+        <source>HIGH</source>
+        <translation>HAUT</translation>
+    </message>
+    <message>
+        <source>GOOD</source>
+        <translation>BON</translation>
+    </message>
+    <message>
+        <source>OK</source>
+        <translation>OK</translation>
+    </message>
+    <message>
+        <source>VEHICLE</source>
+        <translation>VÉHICULE</translation>
+    </message>
+    <message>
+        <source>NO</source>
+        <translation>NON</translation>
+    </message>
+    <message>
+        <source>PANDA</source>
+        <translation>PANDA</translation>
+    </message>
+    <message>
+        <source>GPS</source>
+        <translation>GPS</translation>
+    </message>
+    <message>
+        <source>SEARCH</source>
+        <translation>RECHERCHE</translation>
+    </message>
+    <message>
+        <source>--</source>
+        <translation>--</translation>
+    </message>
+    <message>
+        <source>Wi-Fi</source>
+        <translation>Wi-Fi</translation>
+    </message>
+    <message>
+        <source>ETH</source>
+        <translation>ETH</translation>
+    </message>
+    <message>
+        <source>2G</source>
+        <translation>2G</translation>
+    </message>
+    <message>
+        <source>3G</source>
+        <translation>3G</translation>
+    </message>
+    <message>
+        <source>LTE</source>
+        <translation>LTE</translation>
+    </message>
+    <message>
+        <source>5G</source>
+        <translation>5G</translation>
+    </message>
+</context>
+<context>
+    <name>SoftwarePanel</name>
+    <message>
+        <source>Updates are only downloaded while the car is off.</source>
+        <translation>Les MàJ sont téléchargées uniquement si la voiture est éteinte.</translation>
+    </message>
+    <message>
+        <source>Current Version</source>
+        <translation>Version actuelle</translation>
+    </message>
+    <message>
+        <source>Download</source>
+        <translation>Télécharger</translation>
+    </message>
+    <message>
+        <source>CHECK</source>
+        <translation>VÉRIFIER</translation>
+    </message>
+    <message>
+        <source>Install Update</source>
+        <translation>Installer la mise à jour</translation>
+    </message>
+    <message>
+        <source>INSTALL</source>
+        <translation>INSTALLER</translation>
+    </message>
+    <message>
+        <source>Target Branch</source>
+        <translation>Branche cible</translation>
+    </message>
+    <message>
+        <source>SELECT</source>
+        <translation>SÉLECTIONNER</translation>
+    </message>
+    <message>
+        <source>Select a branch</source>
+        <translation>Sélectionner une branche</translation>
+    </message>
+    <message>
+        <source>Uninstall %1</source>
+        <translation>Désinstaller %1</translation>
+    </message>
+    <message>
+        <source>UNINSTALL</source>
+        <translation>DÉSINSTALLER</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to uninstall?</source>
+        <translation>Êtes-vous sûr de vouloir désinstaller ?</translation>
+    </message>
+    <message>
+        <source>Uninstall</source>
+        <translation>Désinstaller</translation>
+    </message>
+    <message>
+        <source>failed to check for update</source>
+        <translation>échec de la vérification de la mise à jour</translation>
+    </message>
+    <message>
+        <source>DOWNLOAD</source>
+        <translation>TÉLÉCHARGER</translation>
+    </message>
+    <message>
+        <source>update available</source>
+        <translation>mise à jour disponible</translation>
+    </message>
+    <message>
+        <source>never</source>
+        <translation>jamais</translation>
+    </message>
+    <message>
+        <source>up to date, last checked %1</source>
+        <translation>à jour, dernière vérification %1</translation>
+    </message>
+</context>
+<context>
+    <name>SshControl</name>
+    <message>
+        <source>SSH Keys</source>
+        <translation>Clés SSH</translation>
+    </message>
+    <message>
+        <source>Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username.</source>
+        <translation>Attention : Ceci accorde l&apos;accès SSH à toutes les clés publiques de vos paramètres GitHub. N&apos;entrez jamais un nom d&apos;utilisateur GitHub autre que le vôtre. Un employé de comma ne vous demandera JAMAIS d&apos;ajouter son nom d&apos;utilisateur GitHub.</translation>
+    </message>
+    <message>
+        <source>ADD</source>
+        <translation>AJOUTER</translation>
+    </message>
+    <message>
+        <source>Enter your GitHub username</source>
+        <translation>Entrez votre nom d&apos;utilisateur GitHub</translation>
+    </message>
+    <message>
+        <source>LOADING</source>
+        <translation>CHARGEMENT</translation>
+    </message>
+    <message>
+        <source>REMOVE</source>
+        <translation>SUPPRIMER</translation>
+    </message>
+    <message>
+        <source>Username &apos;%1&apos; has no keys on GitHub</source>
+        <translation>L&apos;utilisateur &apos;%1&apos; n&apos;a pas de clés sur GitHub</translation>
+    </message>
+    <message>
+        <source>Request timed out</source>
+        <translation>Délai de la demande dépassé</translation>
+    </message>
+    <message>
+        <source>Username &apos;%1&apos; doesn&apos;t exist on GitHub</source>
+        <translation>L&apos;utilisateur &apos;%1&apos; n&apos;existe pas sur GitHub</translation>
+    </message>
+</context>
+<context>
+    <name>SshToggle</name>
+    <message>
+        <source>Enable SSH</source>
+        <translation>Activer SSH</translation>
+    </message>
+</context>
+<context>
+    <name>TermsPage</name>
+    <message>
+        <source>Terms &amp; Conditions</source>
+        <translation>Termes &amp; Conditions</translation>
+    </message>
+    <message>
+        <source>Decline</source>
+        <translation>Refuser</translation>
+    </message>
+    <message>
+        <source>Scroll to accept</source>
+        <translation>Faire défiler pour accepter</translation>
+    </message>
+    <message>
+        <source>Agree</source>
+        <translation>Accepter</translation>
+    </message>
+</context>
+<context>
+    <name>TogglesPanel</name>
+    <message>
+        <source>Enable openpilot</source>
+        <translation>Activer openpilot</translation>
+    </message>
+    <message>
+        <source>Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off.</source>
+        <translation>Utilisez le système openpilot pour le régulateur de vitesse adaptatif et l&apos;assistance au maintien de voie. Votre attention est requise en permanence pour utiliser cette fonctionnalité. La modification de ce paramètre prend effet lorsque la voiture est éteinte.</translation>
+    </message>
+    <message>
+        <source>openpilot Longitudinal Control (Alpha)</source>
+        <translation>Contrôle longitudinal openpilot (Alpha)</translation>
+    </message>
+    <message>
+        <source>WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB).</source>
+        <translation>ATTENTION : le contrôle longitudinal openpilot est en alpha pour cette voiture et désactivera le freinage d&apos;urgence automatique (AEB).</translation>
+    </message>
+    <message>
+        <source>On this car, openpilot defaults to the car&apos;s built-in ACC instead of openpilot&apos;s longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha.</source>
+        <translation>Sur cette voiture, openpilot utilise par défaut le régulateur de vitesse adaptatif intégré à la voiture plutôt que le contrôle longitudinal d&apos;openpilot. Activez ceci pour passer au contrôle longitudinal openpilot. Il est recommandé d&apos;activer le mode expérimental lors de l&apos;activation du contrôle longitudinal openpilot alpha.</translation>
+    </message>
+    <message>
+        <source>Experimental Mode</source>
+        <translation>Mode expérimental</translation>
+    </message>
+    <message>
+        <source>Disengage on Accelerator Pedal</source>
+        <translation>Désengager avec la pédale d&apos;accélérateur</translation>
+    </message>
+    <message>
+        <source>When enabled, pressing the accelerator pedal will disengage openpilot.</source>
+        <translation>Lorsqu&apos;il est activé, appuyer sur la pédale d&apos;accélérateur désengagera openpilot.</translation>
+    </message>
+    <message>
+        <source>Enable Lane Departure Warnings</source>
+        <translation>Activer les avertissements de sortie de voie</translation>
+    </message>
+    <message>
+        <source>Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h).</source>
+        <translation>Recevez des alertes pour revenir dans la voie lorsque votre véhicule dérive au-delà d&apos;une ligne de voie détectée sans clignotant activé en roulant à plus de 31 mph (50 km/h).</translation>
+    </message>
+    <message>
+        <source>Record and Upload Driver Camera</source>
+        <translation>Enregistrer et télécharger la caméra conducteur</translation>
+    </message>
+    <message>
+        <source>Upload data from the driver facing camera and help improve the driver monitoring algorithm.</source>
+        <translation>Publiez les données de la caméra orientée vers le conducteur et aidez à améliorer l&apos;algorithme de surveillance du conducteur.</translation>
+    </message>
+    <message>
+        <source>Use Metric System</source>
+        <translation>Utiliser le système métrique</translation>
+    </message>
+    <message>
+        <source>Display speed in km/h instead of mph.</source>
+        <translation>Afficher la vitesse en km/h au lieu de mph.</translation>
+    </message>
+    <message>
+        <source>Show ETA in 24h Format</source>
+        <translation>Afficher l&apos;heure d&apos;arrivée en format 24h</translation>
+    </message>
+    <message>
+        <source>Use 24h format instead of am/pm</source>
+        <translation>Utiliser le format 24h plutôt que am/pm</translation>
+    </message>
+    <message>
+        <source>Show Map on Left Side of UI</source>
+        <translation>Afficher la carte à gauche de l&apos;interface</translation>
+    </message>
+    <message>
+        <source>Show map on left side when in split screen view.</source>
+        <translation>Afficher la carte à gauche en mode écran scindé.</translation>
+    </message>
+    <message>
+        <source>Aggressive</source>
+        <translation>Aggressif</translation>
+    </message>
+    <message>
+        <source>Standard</source>
+        <translation>Standard</translation>
+    </message>
+    <message>
+        <source>Relaxed</source>
+        <translation>Détendu</translation>
+    </message>
+    <message>
+        <source>Driving Personality</source>
+        <translation>Personnalité de conduite</translation>
+    </message>
+    <message>
+        <source>Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars.</source>
+        <translation>Le mode standard est recommandé. En mode agressif, openpilot suivra de plus près les voitures de tête et sera plus agressif avec l&apos;accélérateur et le frein. En mode détendu, openpilot restera plus éloigné des voitures de tête.</translation>
+    </message>
+    <message>
+        <source>openpilot defaults to driving in &lt;b&gt;chill mode&lt;/b&gt;. Experimental mode enables &lt;b&gt;alpha-level features&lt;/b&gt; that aren&apos;t ready for chill mode. Experimental features are listed below:</source>
+        <translation>Par défaut, openpilot conduit en &lt;b&gt;mode détente&lt;/b&gt;. Le mode expérimental permet d&apos;activer des &lt;b&gt;fonctionnalités alpha&lt;/b&gt; qui ne sont pas prêtes pour le mode détente. Les fonctionnalités expérimentales sont listées ci-dessous :</translation>
+    </message>
+    <message>
+        <source>Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected.</source>
+        <translation>Laissez le modèle de conduite contrôler l&apos;accélérateur et les freins. openpilot conduira comme il pense qu&apos;un humain le ferait, y compris s&apos;arrêter aux feux rouges et aux panneaux stop. Comme le modèle de conduite décide de la vitesse à adopter, la vitesse définie ne servira que de limite supérieure. Cette fonctionnalité est de qualité alpha ; des erreurs sont à prévoir.</translation>
+    </message>
+    <message>
+        <source>New Driving Visualization</source>
+        <translation>Nouvelle visualisation de la conduite</translation>
+    </message>
+    <message>
+        <source>Experimental mode is currently unavailable on this car since the car&apos;s stock ACC is used for longitudinal control.</source>
+        <translation>Le mode expérimental est actuellement indisponible pour cette voiture car le régulateur de vitesse adaptatif d&apos;origine est utilisé pour le contrôle longitudinal.</translation>
+    </message>
+    <message>
+        <source>openpilot longitudinal control may come in a future update.</source>
+        <translation>Le contrôle longitudinal openpilot pourrait être disponible dans une future mise à jour.</translation>
+    </message>
+    <message>
+        <source>An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches.</source>
+        <translation>Une version alpha du contrôle longitudinal openpilot peut être testée, avec le mode expérimental, sur des branches non publiées.</translation>
+    </message>
+    <message>
+        <source>End-to-End Longitudinal Control</source>
+        <translation>Contrôle longitudinal de bout en bout</translation>
+    </message>
+    <message>
+        <source>Navigate on openpilot</source>
+        <translation>Navigation avec openpilot</translation>
+    </message>
+    <message>
+        <source>When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc.</source>
+        <translation>Lorsque la navigation dispose d&apos;une destination, openpilot entrera les informations de la carte dans le modèle. Cela fournit un contexte utile pour le modèle et permet à openpilot de se diriger à gauche ou à droite de manière appropriée aux bifurcations/sorties. Le comportement relatif au changement de voie reste inchangé et doit toujours être activé par le conducteur. Il s&apos;agit d&apos;une fonctionnalité alpha ; il faut s&apos;attendre à des erreurs, en particulier aux abords des sorties et des bifurcations. Ces erreurs peuvent inclure des franchissements involontaires de passages piétons, des prises de sortie tardives, la conduite vers des zones de séparation de type zebras, etc.</translation>
+    </message>
+    <message>
+        <source>The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green.</source>
+        <translation>La visualisation de la conduite passera sur la caméra grand angle dirigée vers la route à faible vitesse afin de mieux montrer certains virages. Le logo du mode expérimental s&apos;affichera également dans le coin supérieur droit. Lorsqu&apos;une destination de navigation est définie et que le modèle de conduite l&apos;utilise comme entrée, la trajectoire de conduite sur la carte deviendra verte.</translation>
+    </message>
+    <message>
+        <source>Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode.</source>
+        <translation>Activer le contrôle longitudinal d&apos;openpilot (en alpha) pour autoriser le mode expérimental.</translation>
+    </message>
+</context>
+<context>
+    <name>Updater</name>
+    <message>
+        <source>Update Required</source>
+        <translation>Mise à jour requise</translation>
+    </message>
+    <message>
+        <source>An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB.</source>
+        <translation>Une mise à jour du système d&apos;exploitation est requise. Connectez votre appareil au Wi-Fi pour une mise à jour plus rapide. La taille du téléchargement est d&apos;environ 1 Go.</translation>
+    </message>
+    <message>
+        <source>Connect to Wi-Fi</source>
+        <translation>Se connecter au Wi-Fi</translation>
+    </message>
+    <message>
+        <source>Install</source>
+        <translation>Installer</translation>
+    </message>
+    <message>
+        <source>Back</source>
+        <translation>Retour</translation>
+    </message>
+    <message>
+        <source>Loading...</source>
+        <translation>Chargement...</translation>
+    </message>
+    <message>
+        <source>Reboot</source>
+        <translation>Redémarrer</translation>
+    </message>
+    <message>
+        <source>Update failed</source>
+        <translation>Échec de la mise à jour</translation>
+    </message>
+</context>
+<context>
+    <name>WiFiPromptWidget</name>
+    <message>
+        <source>Setup Wi-Fi</source>
+        <translation>Configurer Wi-Fi</translation>
+    </message>
+    <message>
+        <source>Connect to Wi-Fi to upload driving data and help improve openpilot</source>
+        <translation>Connectez-vous au Wi-Fi pour publier les données de conduite et aider à améliorer openpilot</translation>
+    </message>
+    <message>
+        <source>Open Settings</source>
+        <translation>Ouvrir les paramètres</translation>
+    </message>
+    <message>
+        <source>Ready to upload</source>
+        <translation>Prêt à uploader</translation>
+    </message>
+    <message>
+        <source>Training data will be pulled periodically while your device is on Wi-Fi</source>
+        <translation>Les données d&apos;entraînement seront envoyées périodiquement lorsque votre appareil est connecté au réseau Wi-Fi</translation>
+    </message>
+</context>
+<context>
+    <name>WifiUI</name>
+    <message>
+        <source>Scanning for networks...</source>
+        <translation>Recherche de réseaux...</translation>
+    </message>
+    <message>
+        <source>CONNECTING...</source>
+        <translation>CONNEXION...</translation>
+    </message>
+    <message>
+        <source>FORGET</source>
+        <translation>OUBLIER</translation>
+    </message>
+    <message>
+        <source>Forget Wi-Fi Network &quot;%1&quot;?</source>
+        <translation>Oublier le réseau Wi-Fi &quot;%1&quot; ?</translation>
+    </message>
+    <message>
+        <source>Forget</source>
+        <translation>Oublier</translation>
+    </message>
+</context>
+</TS>
diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts
index 88c584e361..63de8f0471 100644
--- a/selfdrive/ui/translations/main_ja.ts
+++ b/selfdrive/ui/translations/main_ja.ts
@@ -226,8 +226,8 @@
         <translation>電源を切る</translation>
     </message>
     <message>
-        <source>openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required.</source>
-        <translation>openpilotの本体は、左右4°以内、上5°、下8°以内の角度で取付ける必要があります。継続してキャリブレーションを続けているので、手動でリセットを行う必要はほぼありません。</translation>
+        <source>openpilot requires the device to be mounted within 4° left or right and within 5° up or 9° down. openpilot is continuously calibrating, resetting is rarely required.</source>
+        <translation>openpilotの本体は、左右4°以内、上5°、下9°以内の角度で取付ける必要があります。継続してキャリブレーションを続けているので、手動でリセットを行う必要はほぼありません。</translation>
     </message>
     <message>
         <source> Your device is pointed %1° %2 and %3° %4.</source>
@@ -274,33 +274,6 @@
         <translation>確認</translation>
     </message>
 </context>
-<context>
-    <name>DriveStats</name>
-    <message>
-        <source>Drives</source>
-        <translation>運転履歴</translation>
-    </message>
-    <message>
-        <source>Hours</source>
-        <translation>時間</translation>
-    </message>
-    <message>
-        <source>ALL TIME</source>
-        <translation>累計</translation>
-    </message>
-    <message>
-        <source>PAST WEEK</source>
-        <translation>先週</translation>
-    </message>
-    <message>
-        <source>KM</source>
-        <translation>km</translation>
-    </message>
-    <message>
-        <source>Miles</source>
-        <translation>マイル</translation>
-    </message>
-</context>
 <context>
     <name>DriverViewScene</name>
     <message>
@@ -353,33 +326,6 @@
         <source>hr</source>
         <translation>時間</translation>
     </message>
-    <message>
-        <source>km</source>
-        <translation>ｷﾛﾒｰﾄﾙ</translation>
-    </message>
-    <message>
-        <source>mi</source>
-        <translation>ﾏｲﾙ</translation>
-    </message>
-</context>
-<context>
-    <name>MapInstructions</name>
-    <message>
-        <source> km</source>
-        <translation> ｷﾛﾒｰﾄﾙ</translation>
-    </message>
-    <message>
-        <source> m</source>
-        <translation> ﾒｰﾄﾙ</translation>
-    </message>
-    <message>
-        <source> mi</source>
-        <translation> ﾏｲﾙ</translation>
-    </message>
-    <message>
-        <source> ft</source>
-        <translation> ﾌｨｰﾄ</translation>
-    </message>
 </context>
 <context>
     <name>MapSettings</name>
@@ -616,6 +562,22 @@
             <numerusform>%n 日前</numerusform>
         </translation>
     </message>
+    <message>
+        <source>km</source>
+        <translation>ｷﾛﾒｰﾄﾙ</translation>
+    </message>
+    <message>
+        <source>m</source>
+        <translation>ﾒｰﾄﾙ</translation>
+    </message>
+    <message>
+        <source>mi</source>
+        <translation>ﾏｲﾙ</translation>
+    </message>
+    <message>
+        <source>ft</source>
+        <translation>ﾌｨｰﾄ</translation>
+    </message>
 </context>
 <context>
     <name>Reset</name>
@@ -758,6 +720,10 @@ This may take up to a minute.</source>
         <source>Something went wrong. Reboot the device.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Select a language</source>
+        <translation>言語を選択</translation>
+    </message>
 </context>
 <context>
     <name>SetupWidget</name>
@@ -1075,10 +1041,6 @@ This may take up to a minute.</source>
         <source>Experimental mode is currently unavailable on this car since the car&apos;s stock ACC is used for longitudinal control.</source>
         <translation>この車のACCがアクセル制御を行うため実験モードを利用することができません。</translation>
     </message>
-    <message>
-        <source>openpilot longitudinal control may come in a future update.</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>openpilot Longitudinal Control (Alpha)</source>
         <translation type="unfinished"></translation>
@@ -1087,10 +1049,6 @@ This may take up to a minute.</source>
         <source>WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB).</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>On this car, openpilot defaults to the car&apos;s built-in ACC instead of openpilot&apos;s longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha.</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Aggressive</source>
         <translation type="unfinished"></translation>
@@ -1107,12 +1065,16 @@ This may take up to a minute.</source>
         <source>Driving Personality</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>On this car, openpilot defaults to the car&apos;s built-in ACC instead of openpilot&apos;s longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha.</source>
+        <translation type="unfinished"></translation>
+    </message>
     <message>
         <source>Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars.</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
-        <source>An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches.</source>
+        <source>End-to-End Longitudinal Control</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
@@ -1120,19 +1082,23 @@ This may take up to a minute.</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
-        <source>Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode.</source>
+        <source>When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc.</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
-        <source>End-to-End Longitudinal Control</source>
+        <source>The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>openpilot longitudinal control may come in a future update.</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
-        <source>When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits/forks.These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc.</source>
+        <source>An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches.</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
-        <source>The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner.When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green.</source>
+        <source>Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode.</source>
         <translation type="unfinished"></translation>
     </message>
 </context>
diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts
index 725870d3e2..bc0c5f2b0e 100644
--- a/selfdrive/ui/translations/main_ko.ts
+++ b/selfdrive/ui/translations/main_ko.ts
@@ -9,7 +9,7 @@
     </message>
     <message>
         <source>Snooze Update</source>
-        <translation>업데이트 일시중지</translation>
+        <translation>업데이트 일시 중지</translation>
     </message>
     <message>
         <source>Reboot and Update</source>
@@ -56,7 +56,7 @@
     </message>
     <message>
         <source>leave blank for automatic configuration</source>
-        <translation>자동설정하려면 공백으로 두세요</translation>
+        <translation>자동 설정하려면 빈 칸으로 두세요</translation>
     </message>
     <message>
         <source>Cellular Metered</source>
@@ -147,7 +147,7 @@
     <name>DevicePanel</name>
     <message>
         <source>Dongle ID</source>
-        <translation>Dongle ID</translation>
+        <translation>동글 ID</translation>
     </message>
     <message>
         <source>N/A</source>
@@ -155,7 +155,7 @@
     </message>
     <message>
         <source>Serial</source>
-        <translation>Serial</translation>
+        <translation>시리얼</translation>
     </message>
     <message>
         <source>Driver Camera</source>
@@ -167,23 +167,23 @@
     </message>
     <message>
         <source>Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off)</source>
-        <translation>운전자 모니터링이 잘 되는지 확인하기 위해 카메라를 향한 운전자를 미리 봅니다. (차량연결은 해제되어있어야 합니다)</translation>
+        <translation>운전자 모니터링이 잘 되는지 확인하기 위해 후면 카메라를 미리 봅니다. (차량 시동이 꺼져 있어야 합니다)</translation>
     </message>
     <message>
         <source>Reset Calibration</source>
-        <translation>캘리브레이션</translation>
+        <translation>캘리브레이션 초기화</translation>
     </message>
     <message>
         <source>RESET</source>
-        <translation>재설정</translation>
+        <translation>초기화</translation>
     </message>
     <message>
         <source>Are you sure you want to reset calibration?</source>
-        <translation>캘리브레이션을 재설정하시겠습니까?</translation>
+        <translation>캘리브레이션을 초기화하시겠습니까?</translation>
     </message>
     <message>
         <source>Review Training Guide</source>
-        <translation>트레이닝 가이드</translation>
+        <translation>트레이닝 가이드 다시보기</translation>
     </message>
     <message>
         <source>REVIEW</source>
@@ -191,11 +191,11 @@
     </message>
     <message>
         <source>Review the rules, features, and limitations of openpilot</source>
-        <translation>openpilot의 규칙, 기능 및 제한 다시보기</translation>
+        <translation>openpilot의 규칙, 기능 및 제한 다시 확인</translation>
     </message>
     <message>
         <source>Are you sure you want to review the training guide?</source>
-        <translation>트레이닝 가이드를 다시보시겠습니까?</translation>
+        <translation>트레이닝 가이드를 다시 확인하시겠습니까?</translation>
     </message>
     <message>
         <source>Regulatory</source>
@@ -223,15 +223,15 @@
     </message>
     <message>
         <source>Power Off</source>
-        <translation>전원 종료</translation>
+        <translation>전원 끄기</translation>
     </message>
     <message>
-        <source>openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required.</source>
-        <translation>openpilot 장치는 좌우측 4° 이내, 위쪽 5° 아래쪽 8° 이내로 장착되어야 합니다. openpilot은 지속적으로 보정되며 재설정은 거의 필요하지 않습니다.</translation>
+        <source>openpilot requires the device to be mounted within 4° left or right and within 5° up or 9° down. openpilot is continuously calibrating, resetting is rarely required.</source>
+        <translation>openpilot 장치는 좌우 4°, 위로 5°, 아래로 9° 이내 각도로 장착되어야 합니다. openpilot은 지속적으로 자동 보정되며 재설정은 거의 필요하지 않습니다.</translation>
     </message>
     <message>
         <source> Your device is pointed %1° %2 and %3° %4.</source>
-        <translation> 사용자의 장치는 %1° %2 및 %3° %4 의 위치에 장착되어 있습니다.</translation>
+        <translation> 사용자의 장치는 %2 %1° 및 %4 %3° 의 방향으로 장착되어 있습니다.</translation>
     </message>
     <message>
         <source>down</source>
@@ -251,72 +251,45 @@
     </message>
     <message>
         <source>Are you sure you want to reboot?</source>
-        <translation>재부팅 하시겠습니까?</translation>
+        <translation>재부팅하시겠습니까?</translation>
     </message>
     <message>
         <source>Disengage to Reboot</source>
-        <translation>재부팅 하려면 해제하세요</translation>
+        <translation>재부팅하려면 연결을 해제하세요</translation>
     </message>
     <message>
         <source>Are you sure you want to power off?</source>
-        <translation>전원을 종료하시겠습니까?</translation>
+        <translation>전원을 끄시겠습니까?</translation>
     </message>
     <message>
         <source>Disengage to Power Off</source>
-        <translation>전원을 종료하려면 해제하세요</translation>
+        <translation>전원을 끄려면 연결을 해제하세요</translation>
     </message>
     <message>
         <source>Reset</source>
-        <translation>리셋</translation>
+        <translation>초기화</translation>
     </message>
     <message>
         <source>Review</source>
         <translation>다시보기</translation>
     </message>
 </context>
-<context>
-    <name>DriveStats</name>
-    <message>
-        <source>Drives</source>
-        <translation>주행</translation>
-    </message>
-    <message>
-        <source>Hours</source>
-        <translation>시간</translation>
-    </message>
-    <message>
-        <source>ALL TIME</source>
-        <translation>전체</translation>
-    </message>
-    <message>
-        <source>PAST WEEK</source>
-        <translation>지난주</translation>
-    </message>
-    <message>
-        <source>KM</source>
-        <translation>Km</translation>
-    </message>
-    <message>
-        <source>Miles</source>
-        <translation>Miles</translation>
-    </message>
-</context>
 <context>
     <name>DriverViewScene</name>
     <message>
         <source>camera starting</source>
-        <translation>카메라 시작중</translation>
+        <translation>카메라 시작 중</translation>
     </message>
 </context>
 <context>
     <name>ExperimentalModeButton</name>
     <message>
         <source>EXPERIMENTAL MODE ON</source>
-        <translation>실험적 모드 사용</translation>
+        <translation>실험 모드 사용</translation>
     </message>
     <message>
         <source>CHILL MODE ON</source>
-        <translation>안정적 모드 사용</translation>
+        <translation>안정 모드 사용</translation>
     </message>
 </context>
 <context>
@@ -328,7 +301,7 @@
     <message numerus="yes">
         <source>Need at least %n character(s)!</source>
         <translation>
-            <numerusform>최소 %n 자가 필요합니다!</numerusform>
+            <numerusform>최소 %n자 이상이어야 합니다!</numerusform>
         </translation>
     </message>
 </context>
@@ -336,7 +309,7 @@
     <name>Installer</name>
     <message>
         <source>Installing...</source>
-        <translation>설치중...</translation>
+        <translation>설치 중...</translation>
     </message>
 </context>
 <context>
@@ -353,33 +326,6 @@
         <source>hr</source>
         <translation>시간</translation>
     </message>
-    <message>
-        <source>km</source>
-        <translation>km</translation>
-    </message>
-    <message>
-        <source>mi</source>
-        <translation>mi</translation>
-    </message>
-</context>
-<context>
-    <name>MapInstructions</name>
-    <message>
-        <source> km</source>
-        <translation> km</translation>
-    </message>
-    <message>
-        <source> m</source>
-        <translation> m</translation>
-    </message>
-    <message>
-        <source> mi</source>
-        <translation> mi</translation>
-    </message>
-    <message>
-        <source> ft</source>
-        <translation> ft</translation>
-    </message>
 </context>
 <context>
     <name>MapSettings</name>
@@ -389,22 +335,22 @@
     </message>
     <message>
         <source>Manage at connect.comma.ai</source>
-        <translation>connect.comma.ai에서 관리됩니다</translation>
+        <translation>connect.comma.ai에서 관리하세요</translation>
     </message>
 </context>
 <context>
     <name>MapWindow</name>
     <message>
         <source>Map Loading</source>
-        <translation>지도 로딩</translation>
+        <translation>지도 로딩 중</translation>
     </message>
     <message>
         <source>Waiting for GPS</source>
-        <translation>GPS 수신중</translation>
+        <translation>GPS 수신 중</translation>
     </message>
     <message>
         <source>Waiting for route</source>
-        <translation>경로를 기다리는중</translation>
+        <translation>경로를 기다리는 중</translation>
     </message>
 </context>
 <context>
@@ -450,7 +396,7 @@
     <message>
         <source>Unable to download updates
 %1</source>
-        <translation>업데이트를 다운로드할수 없습니다
+        <translation>업데이트를 다운로드할 수 없습니다
 %1</translation>
     </message>
     <message>
@@ -463,11 +409,11 @@
     </message>
     <message>
         <source>An update to your device&apos;s operating system is downloading in the background. You will be prompted to update when it&apos;s ready to install.</source>
-        <translation>백그라운드에서 운영 체제에 대한 업데이트가 다운로드되고 있습니다. 설치준비가 완료되면 업데이트하라는 메시지가 표시됩니다.</translation>
+        <translation>백그라운드에서 운영 체제에 대한 업데이트가 다운로드되고 있습니다. 설치가 준비되면 업데이트 메시지가 표시됩니다.</translation>
     </message>
     <message>
         <source>Device failed to register. It will not connect to or upload to comma.ai servers, and receives no support from comma.ai. If this is an official device, visit https://comma.ai/support.</source>
-        <translation>장치를 등록하지 못했습니다. comma.ai 서버에 연결하거나 업로드하지 않으며 comma.ai에서 지원을 받지 않습니다. 공식 장치인경우 https://comma.ai/support 에 방문하여 문의하세요.</translation>
+        <translation>장치를 등록하지 못했습니다. comma.ai 서버에 연결하거나 데이터를 업로드하지 않으며 comma.ai에서 지원을 받지 않습니다. 공식 장치인 경우 https://comma.ai/support 에 방문하여 문의하세요.</translation>
     </message>
     <message>
         <source>NVMe drive not mounted.</source>
@@ -475,23 +421,23 @@
     </message>
     <message>
         <source>Unsupported NVMe drive detected. Device may draw significantly more power and overheat due to the unsupported NVMe.</source>
-        <translation>지원되지 않는 NVMe 드라이브가 감지되었습니다. 지원되지 않는 NVMe 드라이브로 인해 장치가 훨씬 더 많은 전력을 소비하고 과열될 수 있습니다.</translation>
+        <translation>지원되지 않는 NVMe 드라이브가 감지되었습니다. 지원되지 않는 NVMe 드라이브는 많은 전력을 소비하고 장치를 과열시킬 수 있습니다.</translation>
     </message>
     <message>
         <source>openpilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please submit a pull request to add the firmware versions to the proper vehicle. Need help? Join discord.comma.ai.</source>
-        <translation>opepilot이 차량을 식별할수 없었습니다. 지원되지 않는 차량이거나 ECU가 인식되지 않습니다. 해당 차량에 펌웨어 버전을 추가하려면 PR을 제출하세요. 도움이 필요하시면 discord.comma.ai에 가입하세요.</translation>
+        <translation>openpilot이 차량을 식별할 수 없었습니다. 지원되지 않는 차량이거나 ECU가 인식되지 않습니다. 해당 차량에 맞는 펌웨어 버전을 추가하려면 PR을 제출하세요. 도움이 필요하시면 discord.comma.ai에 가입하세요.</translation>
     </message>
     <message>
         <source>openpilot was unable to identify your car. Check integrity of cables and ensure all connections are secure, particularly that the comma power is fully inserted in the OBD-II port of the vehicle. Need help? Join discord.comma.ai.</source>
-        <translation>openpilot이 차량을 식별할수 없었습니다. 케이블의 무결성을 점검하고 모든 연결부, 특히 comma power가 차량의 OBD-II 포트에 완전히 삽입되었는지 확인하세요. 도움이 필요하시면 discord.comma.ai에 가입하세요.</translation>
+        <translation>openpilot이 차량을 식별할 수 없었습니다. 케이블의 무결성을 점검하고 모든 연결부, 특히 comma power가 차량의 OBD-II 포트에 제대로 삽입되었는지 확인하세요. 도움이 필요하시면 discord.comma.ai에 가입하세요.</translation>
     </message>
     <message>
         <source>openpilot detected a change in the device&apos;s mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.</source>
-        <translation>openpilot 장치의 장착 위치 변경을 감지했습니다. 장치가 마운트에 완전히 장착되고 마운트가 앞유리에 단단히 고정되었는지 확인하세요. </translation>
+        <translation>openpilot 장치의 장착 위치가 변경되었습니다. 장치가 마운트에 완전히 장착되고 마운트가 앞유리에 단단히 고정되었는지 확인하세요. </translation>
     </message>
     <message>
         <source>Device temperature too high. System cooling down before starting. Current internal component temperature: %1</source>
-        <translation>장치 온도가 너무 높습니다. 시작하기 전에 장치온도를 낮춰주세요. 현재 내부 구성 요소 온도: %1</translation>
+        <translation>장치 온도가 너무 높습니다. 시작하기 전에 온도를 낮춰주세요. 현재 내부 부품 온도: %1</translation>
     </message>
 </context>
 <context>
@@ -513,7 +459,7 @@
     <name>PairingPopup</name>
     <message>
         <source>Pair your device to your comma account</source>
-        <translation>장치를 comma 계정과 페어링합니다</translation>
+        <translation>장치를 comma 계정에 페어링합니다</translation>
     </message>
     <message>
         <source>Go to https://connect.comma.ai on your phone</source>
@@ -521,11 +467,11 @@
     </message>
     <message>
         <source>Click &quot;add new device&quot; and scan the QR code on the right</source>
-        <translation>&quot;새 장치 추가&quot;를 클릭하고 오른쪽 QR 코드를 검색합니다</translation>
+        <translation>&quot;새 장치 추가&quot;를 클릭하고 오른쪽 QR 코드를 스캔하세요</translation>
     </message>
     <message>
         <source>Bookmark connect.comma.ai to your home screen to use it like an app</source>
-        <translation>connect.comma.ai를 앱처럼 사용하려면 홈 화면에 바로가기를 만드세요.</translation>
+        <translation>connect.comma.ai를 앱처럼 사용하려면 홈 화면에 바로가기를 만드세요</translation>
     </message>
 </context>
 <context>
@@ -536,22 +482,22 @@
     </message>
     <message>
         <source>Enable</source>
-        <translation>사용</translation>
+        <translation>활성화</translation>
     </message>
 </context>
 <context>
     <name>PrimeAdWidget</name>
     <message>
         <source>Upgrade Now</source>
-        <translation>지금 업그레이드 하세요</translation>
+        <translation>지금 업그레이드하세요</translation>
     </message>
     <message>
         <source>Become a comma prime member at connect.comma.ai</source>
-        <translation>connect.comma.ai에 접속하여 comma prime 회원이 되세요</translation>
+        <translation>connect.comma.ai에 접속하여 comma prime 회원으로 등록하세요</translation>
     </message>
     <message>
         <source>PRIME FEATURES:</source>
-        <translation>PRIME 기능：</translation>
+        <translation>PRIME 기능:</translation>
     </message>
     <message>
         <source>Remote access</source>
@@ -567,7 +513,7 @@
     </message>
     <message>
         <source>1 year of drive storage</source>
-        <translation>1년간 저장</translation>
+        <translation>1년간 드라이브 로그 저장</translation>
     </message>
 </context>
 <context>
@@ -593,7 +539,7 @@
     </message>
     <message>
         <source>dashcam</source>
-        <translation>dashcam</translation>
+        <translation>블랙박스</translation>
     </message>
     <message>
         <source>openpilot</source>
@@ -602,31 +548,47 @@
     <message numerus="yes">
         <source>%n minute(s) ago</source>
         <translation>
-            <numerusform>%n 분전</numerusform>
+            <numerusform>%n 분 전</numerusform>
         </translation>
     </message>
     <message numerus="yes">
         <source>%n hour(s) ago</source>
         <translation>
-            <numerusform>%n 시간전</numerusform>
+            <numerusform>%n 시간 전</numerusform>
         </translation>
     </message>
     <message numerus="yes">
         <source>%n day(s) ago</source>
         <translation>
-            <numerusform>%n 일전</numerusform>
+            <numerusform>%n 일 전</numerusform>
         </translation>
     </message>
+    <message>
+        <source>km</source>
+        <translation>km</translation>
+    </message>
+    <message>
+        <source>m</source>
+        <translation>m</translation>
+    </message>
+    <message>
+        <source>mi</source>
+        <translation>mi</translation>
+    </message>
+    <message>
+        <source>ft</source>
+        <translation>ft</translation>
+    </message>
 </context>
 <context>
     <name>Reset</name>
     <message>
         <source>Reset failed. Reboot to try again.</source>
-        <translation>초기화 실패. 재부팅후 다시 시도하세요.</translation>
+        <translation>초기화 실패. 재부팅 후 다시 시도하세요.</translation>
     </message>
     <message>
         <source>Are you sure you want to reset your device?</source>
-        <translation>장치를 초기화 하시겠습니까?</translation>
+        <translation>장치를 초기화하시겠습니까?</translation>
     </message>
     <message>
         <source>System Reset</source>
@@ -646,11 +608,11 @@
     </message>
     <message>
         <source>Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device.</source>
-        <translation>데이터 파티션을 마운트할 수 없습니다. 파티션이 손상되었을 수 있습니다. 모든 내용을 지우고 장치를 초기화하려면 확인을 누르세요.</translation>
+        <translation>데이터 파티션을 마운트할 수 없습니다. 파티션이 손상되었을 수 있습니다. 모든 설정을 삭제하고 장치를 초기화하려면 확인을 누르세요.</translation>
     </message>
     <message>
         <source>Press confirm to erase all content and settings. Press cancel to resume boot.</source>
-        <translation>모든 콘텐츠와 설정을 지우려면 확인을 누르세요. 부팅을 재개하려면 취소를 누르세요.</translation>
+        <translation>모든 콘텐츠와 설정을 삭제하려면 확인을 누르세요. 계속 부팅하려면 취소를 누르세요.</translation>
     </message>
     <message>
         <source>Resetting device...
@@ -690,11 +652,11 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Power your device in a car with a harness or proceed at your own risk.</source>
-        <translation>하네스 보드에 차량의 전원을 연결하세요.</translation>
+        <translation>장치를 하네스를 통해 차량 전원에 연결하세요. USB 전원에서는 예상치 못한 문제가 생길 수 있습니다.</translation>
     </message>
     <message>
         <source>Power off</source>
-        <translation>전원 종료</translation>
+        <translation>전원 끄기</translation>
     </message>
     <message>
         <source>Continue</source>
@@ -702,15 +664,15 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Getting Started</source>
-        <translation>설정 시작</translation>
+        <translation>시작하기</translation>
     </message>
     <message>
         <source>Before we get on the road, let’s finish installation and cover some details.</source>
-        <translation>출발하기 전에 설정을 완료하고 몇 가지 세부 사항을 살펴보겠습니다.</translation>
+        <translation>출발 전 설정을 완료하고 세부 사항을 살펴봅니다.</translation>
     </message>
     <message>
         <source>Connect to Wi-Fi</source>
-        <translation>wifi 연결</translation>
+        <translation>Wi-Fi 연결</translation>
     </message>
     <message>
         <source>Back</source>
@@ -718,11 +680,11 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Continue without Wi-Fi</source>
-        <translation>wifi 연결없이 계속하기</translation>
+        <translation>Wi-Fi 연결 없이 진행</translation>
     </message>
     <message>
         <source>Waiting for internet</source>
-        <translation>인터넷 대기중</translation>
+        <translation>인터넷 연결 대기 중</translation>
     </message>
     <message>
         <source>Enter URL</source>
@@ -734,7 +696,7 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Downloading...</source>
-        <translation>다운로드중...</translation>
+        <translation>다운로드 중...</translation>
     </message>
     <message>
         <source>Download Failed</source>
@@ -742,11 +704,11 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Ensure the entered URL is valid, and the device’s internet connection is good.</source>
-        <translation>입력된 URL이 유효하고 장치의 인터넷 연결이 양호한지 확인하세요.</translation>
+        <translation>입력된 URL이 유효하고 인터넷 연결이 원활한지 확인하세요.</translation>
     </message>
     <message>
         <source>Reboot device</source>
-        <translation>재부팅</translation>
+        <translation>장치 재부팅</translation>
     </message>
     <message>
         <source>Start over</source>
@@ -760,6 +722,10 @@ This may take up to a minute.</source>
         <source>No custom software found at this URL.</source>
         <translation>이 URL에서 커스텀 소프트웨어를 찾을 수 없습니다.</translation>
     </message>
+    <message>
+        <source>Select a language</source>
+        <translation>언어를 선택하세요</translation>
+    </message>
 </context>
 <context>
     <name>SetupWidget</name>
@@ -769,7 +735,7 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer.</source>
-        <translation>장치를 comma connect (connect.comma.ai)에서 페어링하고 comma prime 제안을 요청하세요.</translation>
+        <translation>장치를 comma connect (connect.comma.ai)에서 페어링하고 comma prime 무료 이용권을 사용하세요.</translation>
     </message>
     <message>
         <source>Pair device</source>
@@ -780,11 +746,11 @@ This may take up to a minute.</source>
     <name>Sidebar</name>
     <message>
         <source>CONNECT</source>
-        <translation>연결</translation>
+        <translation>연결됨</translation>
     </message>
     <message>
         <source>OFFLINE</source>
-        <translation>오프라인</translation>
+        <translation>연결 안됨</translation>
     </message>
     <message>
         <source>ONLINE</source>
@@ -808,7 +774,7 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>OK</source>
-        <translation>경고</translation>
+        <translation>OK</translation>
     </message>
     <message>
         <source>VEHICLE</source>
@@ -836,7 +802,7 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Wi-Fi</source>
-        <translation>wifi</translation>
+        <translation>Wi-Fi</translation>
     </message>
     <message>
         <source>ETH</source>
@@ -863,7 +829,7 @@ This may take up to a minute.</source>
     <name>SoftwarePanel</name>
     <message>
         <source>Updates are only downloaded while the car is off.</source>
-        <translation>업데이트는 차량 연결이 해제되어 있는 동안에만 다운로드됩니다.</translation>
+        <translation>업데이트는 차량 시동이 꺼졌을 때 다운로드됩니다.</translation>
     </message>
     <message>
         <source>Current Version</source>
@@ -915,11 +881,11 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>failed to check for update</source>
-        <translation>업데이트 확인 실패</translation>
+        <translation>업데이트를 확인하지 못했습니다</translation>
     </message>
     <message>
         <source>up to date, last checked %1</source>
-        <translation>최신 상태 입니다, %1에 마지막으로 확인</translation>
+        <translation>최신 버전입니다. 마지막 확인: %1</translation>
     </message>
     <message>
         <source>DOWNLOAD</source>
@@ -942,7 +908,7 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username.</source>
-        <translation>경고: 허용으로 설정하면 GitHub 설정의 모든 공용 키에 대한 SSH 액세스 권한이 부여됩니다. GitHub 사용자 ID 이외에는 입력하지 마십시오. comma에서는 GitHub ID를 추가하라는 요청을 하지 않습니다.</translation>
+        <translation>경고: 이 설정은 GitHub에 등록된 모든 공용 키에 대해 SSH 액세스 권한을 부여합니다. 본인의 GitHub 사용자 아이디 이외에는 입력하지 마십시오. comma에서는 GitHub 아이디를 추가하라는 요청을 하지 않습니다.</translation>
     </message>
     <message>
         <source>ADD</source>
@@ -954,15 +920,15 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>LOADING</source>
-        <translation>로딩</translation>
+        <translation>로딩 중</translation>
     </message>
     <message>
         <source>REMOVE</source>
-        <translation>제거</translation>
+        <translation>삭제</translation>
     </message>
     <message>
         <source>Username &apos;%1&apos; has no keys on GitHub</source>
-        <translation>사용자 &apos;%1&apos;의 키가 GitHub에 없습니다</translation>
+        <translation>사용자 &apos;%1&apos;의 GitHub에 키가 등록되어 있지 않습니다</translation>
     </message>
     <message>
         <source>Request timed out</source>
@@ -970,7 +936,7 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Username &apos;%1&apos; doesn&apos;t exist on GitHub</source>
-        <translation>사용자 &apos;%1&apos;는 GitHub에 없습니다</translation>
+        <translation>GitHub 사용자 &apos;%1&apos;를 찾지 못했습니다</translation>
     </message>
 </context>
 <context>
@@ -984,7 +950,7 @@ This may take up to a minute.</source>
     <name>TermsPage</name>
     <message>
         <source>Terms &amp; Conditions</source>
-        <translation>약관</translation>
+        <translation>이용약관</translation>
     </message>
     <message>
         <source>Decline</source>
@@ -1007,15 +973,15 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off.</source>
-        <translation>어댑티브 크루즈 컨트롤 및 차선 유지 운전자 보조를 위해 openpilot 시스템을 사용하십시오. 이 기능을 사용하려면 항상 주의를 기울여야 합니다. 설정변경은 장치 재부팅후 적용됩니다.</translation>
+        <translation>어댑티브 크루즈 컨트롤 및 차로 유지 보조를 위해 openpilot 시스템을 사용할 수 있습니다. 이 기능을 사용할 때에는 언제나 주의를 기울여야 합니다. 설정을 변경하면 차량 시동이 꺼졌을 때 적용됩니다.</translation>
     </message>
     <message>
         <source>Enable Lane Departure Warnings</source>
-        <translation>차선 이탈 경고 사용</translation>
+        <translation>차선 이탈 경고 활성화</translation>
     </message>
     <message>
         <source>Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h).</source>
-        <translation>차량이 50km/h(31mph) 이상의 속도로 주행하는 동안 방향지시등이 켜지지 않은 상태에서 차량이 감지된 차선을 벗어나면 차선이탈 경고를 합니다.</translation>
+        <translation>차량이 50km/h(31mph) 이상의 속도로 주행할 때 방향지시등이 켜지지 않은 상태에서 차선을 벗어나면 경고합니다.</translation>
     </message>
     <message>
         <source>Use Metric System</source>
@@ -1031,11 +997,11 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Upload data from the driver facing camera and help improve the driver monitoring algorithm.</source>
-        <translation>운전자 카메라에서 데이터를 업로드하고 운전자 모니터링 알고리즘을 개선합니다.</translation>
+        <translation>운전자 카메라의 영상 데이터를 업로드하여 운전자 모니터링 알고리즘을 개선합니다.</translation>
     </message>
     <message>
         <source>Disengage on Accelerator Pedal</source>
-        <translation>가속페달 조작시 해제</translation>
+        <translation>가속페달 조작 시 해제</translation>
     </message>
     <message>
         <source>When enabled, pressing the accelerator pedal will disengage openpilot.</source>
@@ -1043,7 +1009,7 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Show ETA in 24h Format</source>
-        <translation>24시간 형식으로 도착예정시간 표시</translation>
+        <translation>24시간 형식으로 도착 예정 시간 표시</translation>
     </message>
     <message>
         <source>Use 24h format instead of am/pm</source>
@@ -1059,15 +1025,15 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Experimental Mode</source>
-        <translation>실험적 모드</translation>
+        <translation>실험 모드</translation>
     </message>
     <message>
         <source>openpilot defaults to driving in &lt;b&gt;chill mode&lt;/b&gt;. Experimental mode enables &lt;b&gt;alpha-level features&lt;/b&gt; that aren&apos;t ready for chill mode. Experimental features are listed below:</source>
-        <translation>openpilot은 기본적으로 &lt;b&gt;안정적 모드&lt;/b&gt;로 주행합니다. 실험적 모드는 안정적 모드에 준비되지 않은 &lt;b&gt;알파 수준 기능&lt;/b&gt;을 활성화 합니다. 실험적 모드의 특징은 아래에 나열되어 있습니다</translation>
+        <translation>openpilot은 기본적으로 &lt;b&gt;안정 모드&lt;/b&gt;로 주행합니다. 실험 모드는 안정화되지 않은 &lt;b&gt;알파 수준의 기능&lt;/b&gt;을 활성화합니다. 실험 모드의 기능은 아래와 같습니다:</translation>
     </message>
     <message>
         <source>Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected.</source>
-        <translation>주행모델이 가속과 감속을 제어하도록 합니다. openpilot은 신호등과 정지표지판을 보고 멈추는 것을 포함하여 운전자가 생각하는것처럼 주행합니다. 주행 모델이 주행할 속도를 결정하므로 설정된 속도는 상한선으로만 작용합니다. 이것은 알파 기능이므로 사용에 주의해야 합니다.</translation>
+        <translation>openpilot의 주행모델이 가감속을 제어합니다. openpilot은 신호등과 정지 표지판을 보고 멈추는 것을 포함하여 인간이 운전하는 것처럼 생각하고 주행합니다. 주행 모델이 주행할 속도를 결정하므로 설정된 속도는 최대 주행 속도로만 기능합니다. 이 기능은 알파 수준이므로 사용에 각별히 주의해야 합니다.</translation>
     </message>
     <message>
         <source>New Driving Visualization</source>
@@ -1075,23 +1041,23 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Experimental mode is currently unavailable on this car since the car&apos;s stock ACC is used for longitudinal control.</source>
-        <translation>차량에 장착된 ACC가 롱컨트롤에 사용되기 때문에 현재 이 차량은 실험적 모드를 사용할 수 없습니다.</translation>
+        <translation>차량에 장착된 ACC로 가감속을 제어하기 때문에 현재 이 차량에서는 실험 모드를 사용할 수 없습니다.</translation>
     </message>
     <message>
         <source>openpilot longitudinal control may come in a future update.</source>
-        <translation>openpilot 롱컨트롤은 향후 업데이트에서 제공될 수 있습니다.</translation>
+        <translation>openpilot 가감속 제어는 향후 업데이트에서 지원될 수 있습니다.</translation>
     </message>
     <message>
         <source>openpilot Longitudinal Control (Alpha)</source>
-        <translation>openpilot 롱컨트롤 (알파)</translation>
+        <translation>openpilot 가감속 제어 (알파)</translation>
     </message>
     <message>
         <source>WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB).</source>
-        <translation>경고: openpilot 롱컨트롤은 알파 기능으로 차량의 자동긴급제동(AEB)를 비활성화합니다.</translation>
+        <translation>경고: openpilot 가감속 제어 알파 기능으로 차량의 자동긴급제동(AEB)을 비활성화합니다.</translation>
     </message>
     <message>
         <source>On this car, openpilot defaults to the car&apos;s built-in ACC instead of openpilot&apos;s longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha.</source>
-        <translation>이 차량은 openpilot 롱컨트롤 대신 차량의 ACC로 기본 설정됩니다. openpilot 롱컨트롤으로 전환하려면 이 기능을 활성화하세요. openpilot 롱컨트롤 알파를 활성화하는경우 실험적 모드 활성화를 권장합니다.</translation>
+        <translation>이 차량은 openpilot 가감속 제어 대신 기본적으로 차량의 ACC로 가감속을 제어합니다. openpilot의 가감속 제어로 전환하려면 이 기능을 활성화하세요. openpilot 가감속 제어 알파를 활성화하는 경우 실험 모드 활성화를 권장합니다.</translation>
     </message>
     <message>
         <source>Aggressive</source>
@@ -1111,31 +1077,31 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars.</source>
-        <translation>표준 모드를 권장합니다. 공격적 모드에서는 openpilot은 앞차를 더 가까이 따라가며 가속과 감속을 더 공격적으로 사용합니다. 편안한 모드에서 openpilot은 선두 차량에서 더 멀리 떨어져 있습니다.</translation>
+        <translation>표준 모드를 권장합니다. 공격적 모드에서 openpilot은 앞 차량을 더 가까이 따라가며 적극적으로 가감속합니다. 편안한 모드에서 openpilot은 앞 차량을 더 멀리서 따라갑니다.</translation>
     </message>
     <message>
         <source>An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches.</source>
-        <translation>openpilot 롱컨트롤 알파 버전은 비 릴리스 분기에서 실험적 모드와 함께 테스트할 수 있습니다.</translation>
+        <translation>openpilot 가감속 제어 알파 버전은 비 릴리즈 브랜치에서 실험 모드와 함께 테스트할 수 있습니다.</translation>
     </message>
     <message>
         <source>Navigate on openpilot</source>
-        <translation>Navigate on openpilot (NOO)</translation>
+        <translation>openpilot 내비게이트</translation>
     </message>
     <message>
         <source>Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode.</source>
-        <translation>openpilot E2E 롱컨트롤 (알파) 토글을 활성화하여 실험적 모드를 허용합니다.</translation>
+        <translation>실험 모드를 사용하려면 openpilot E2E 가감속 제어 (알파) 토글을 활성화하세요.</translation>
     </message>
     <message>
         <source>End-to-End Longitudinal Control</source>
-        <translation>E2E 롱컨트롤</translation>
+        <translation>E2E 가감속 제어</translation>
     </message>
     <message>
-        <source>When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits/forks.These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc.</source>
-        <translation>내비게이션에 목적지가 있으면 openpilot이 지도 정보를 모델에 입력합니다. 이는 모델에 유용한 컨텍스트를 제공하고 openpilot이 분기점에서 적절하게 왼쪽 또는 오른쪽을 유지할 수 있도록 합니다. 차선 변경 기능은 여전히 운전자의 조작에 의해 활성화됩니다. 이것은 알파 상태의 기능입니다. 실수는 특히 출구 분기점 주변에서 발생될수 있으며 이러한 실수에는 의도하지 않은 차선 이탈, 늦은 출구 이용, 도로 가장자리의 분리대 또는 경계석을 향해 운전하는 등이 포함될 수 있습니다.</translation>
+        <source>When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc.</source>
+        <translation>내비게이션에 목적지가 설정되어 있으면 openpilot이 지도 정보를 주행 모델에 입력합니다. 이는 모델에 유용한 정보를 제공하고 openpilot이 진출입로 및 램프에서 적절하게 왼쪽 또는 오른쪽을 유지할 수 있도록 해 줍니다. 차선 변경 기능은 여전히 운전자의 조작에 의해 활성화됩니다. 이 기능은 알파 버전입니다. 특히 진출입로 및 분기점 주변에서 실수가 발생할 수 있으며 이러한 실수에는 의도하지 않은 차선 이탈, 늦은 진출, 도로 가장자리의 분리대 또는 경계석을 향해 운전하는 행동 등이 포함됩니다.</translation>
     </message>
     <message>
-        <source>The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner.When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green.</source>
-        <translation>주행 시각화는 저속으로 주행시 도로를 향한 광각 카메라로 전환되어 일부 회전을 더 잘 보여줍니다. 실험적 모드 로고도 우측 상단에 표시됩니다. 내비게이션 목적지가 설정되고 주행 모델에 입력되면 지도의 주행 경로가 녹색으로 바뀝니다.</translation>
+        <source>The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green.</source>
+        <translation>주행 시각화는 저속으로 주행 시 도로를 향한 광각 카메라로 자동 전환되어 일부 곡선 경로를 더 잘 보여줍니다. 실험 모드 로고는 우측 상단에 표시됩니다. 내비게이션 목적지가 설정되고 주행 모델에 입력되면 지도의 주행 경로가 녹색으로 바뀝니다.</translation>
     </message>
 </context>
 <context>
@@ -1146,11 +1112,11 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB.</source>
-        <translation>OS 업데이트가 필요합니다. 장치를 wifi에 연결하면 가장 빠른 업데이트 경험을 제공합니다. 다운로드 크기는 약 1GB입니다.</translation>
+        <translation>OS 업데이트가 필요합니다. 장치를 Wi-Fi에 연결하면 가장 빠르게 업데이트할 수 있습니다. 다운로드 크기는 약 1GB입니다.</translation>
     </message>
     <message>
         <source>Connect to Wi-Fi</source>
-        <translation>wifi 연결</translation>
+        <translation>Wi-Fi 연결</translation>
     </message>
     <message>
         <source>Install</source>
@@ -1162,7 +1128,7 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Loading...</source>
-        <translation>로딩중...</translation>
+        <translation>로딩 중...</translation>
     </message>
     <message>
         <source>Reboot</source>
@@ -1177,11 +1143,11 @@ This may take up to a minute.</source>
     <name>WiFiPromptWidget</name>
     <message>
         <source>Setup Wi-Fi</source>
-        <translation>wifi 설정</translation>
+        <translation>Wi-Fi 설정</translation>
     </message>
     <message>
         <source>Connect to Wi-Fi to upload driving data and help improve openpilot</source>
-        <translation>wifi에 연결하여 주행 데이터를 업로드하고 openpilot 개선에 참여하세요.</translation>
+        <translation>Wi-Fi에 연결하여 주행 데이터를 업로드하고 openpilot 개선에 기여하세요</translation>
     </message>
     <message>
         <source>Open Settings</source>
@@ -1189,11 +1155,11 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Ready to upload</source>
-        <translation>업로드 준비완료</translation>
+        <translation>업로드 준비 완료</translation>
     </message>
     <message>
         <source>Training data will be pulled periodically while your device is on Wi-Fi</source>
-        <translation>기기가 wifi에 연결되어 있는 동안 트레이닝 데이터를 주기적으로 전송합니다.</translation>
+        <translation>기기가 Wi-Fi에 연결되어 있는 동안 트레이닝 데이터를 주기적으로 전송합니다</translation>
     </message>
 </context>
 <context>
@@ -1204,19 +1170,19 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>CONNECTING...</source>
-        <translation>연결중...</translation>
+        <translation>연결 중...</translation>
     </message>
     <message>
         <source>FORGET</source>
-        <translation>저장안함</translation>
+        <translation>삭제</translation>
     </message>
     <message>
         <source>Forget Wi-Fi Network &quot;%1&quot;?</source>
-        <translation>&quot;%1&quot;를 저장하지 않겠습니까?</translation>
+        <translation>Wi-Fi &quot;%1&quot;에 자동으로 연결하지 않겠습니까?</translation>
     </message>
     <message>
         <source>Forget</source>
-        <translation>저장안함</translation>
+        <translation>삭제</translation>
     </message>
 </context>
 </TS>
diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts
index dd04488b73..4cf37cc585 100644
--- a/selfdrive/ui/translations/main_pt-BR.ts
+++ b/selfdrive/ui/translations/main_pt-BR.ts
@@ -226,8 +226,8 @@
         <translation>Desligar</translation>
     </message>
     <message>
-        <source>openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required.</source>
-        <translation>O openpilot requer que o dispositivo seja montado dentro de 4° esquerda ou direita e dentro de 5° para cima ou 8° para baixo. O openpilot está continuamente calibrando, resetar raramente é necessário.</translation>
+        <source>openpilot requires the device to be mounted within 4° left or right and within 5° up or 9° down. openpilot is continuously calibrating, resetting is rarely required.</source>
+        <translation>O openpilot requer que o dispositivo seja montado dentro de 4° esquerda ou direita e dentro de 5° para cima ou 9° para baixo. O openpilot está continuamente calibrando, resetar raramente é necessário.</translation>
     </message>
     <message>
         <source> Your device is pointed %1° %2 and %3° %4.</source>
@@ -274,33 +274,6 @@
         <translation>Revisar</translation>
     </message>
 </context>
-<context>
-    <name>DriveStats</name>
-    <message>
-        <source>Drives</source>
-        <translation>Dirigidas</translation>
-    </message>
-    <message>
-        <source>Hours</source>
-        <translation>Horas</translation>
-    </message>
-    <message>
-        <source>ALL TIME</source>
-        <translation>TOTAL</translation>
-    </message>
-    <message>
-        <source>PAST WEEK</source>
-        <translation>SEMANA PASSADA</translation>
-    </message>
-    <message>
-        <source>KM</source>
-        <translation>KM</translation>
-    </message>
-    <message>
-        <source>Miles</source>
-        <translation>Milhas</translation>
-    </message>
-</context>
 <context>
     <name>DriverViewScene</name>
     <message>
@@ -354,33 +327,6 @@
         <source>hr</source>
         <translation>hr</translation>
     </message>
-    <message>
-        <source>km</source>
-        <translation>km</translation>
-    </message>
-    <message>
-        <source>mi</source>
-        <translation>mi</translation>
-    </message>
-</context>
-<context>
-    <name>MapInstructions</name>
-    <message>
-        <source> km</source>
-        <translation> km</translation>
-    </message>
-    <message>
-        <source> m</source>
-        <translation> m</translation>
-    </message>
-    <message>
-        <source> mi</source>
-        <translation> milha</translation>
-    </message>
-    <message>
-        <source> ft</source>
-        <translation> pés</translation>
-    </message>
 </context>
 <context>
     <name>MapSettings</name>
@@ -621,6 +567,22 @@
             <numerusform>há %n dias</numerusform>
         </translation>
     </message>
+    <message>
+        <source>km</source>
+        <translation>km</translation>
+    </message>
+    <message>
+        <source>m</source>
+        <translation>m</translation>
+    </message>
+    <message>
+        <source>mi</source>
+        <translation>milha</translation>
+    </message>
+    <message>
+        <source>ft</source>
+        <translation>pés</translation>
+    </message>
 </context>
 <context>
     <name>Reset</name>
@@ -764,6 +726,10 @@ Isso pode levar até um minuto.</translation>
         <source>Something went wrong. Reboot the device.</source>
         <translation>Algo deu errado. Reinicie o dispositivo.</translation>
     </message>
+    <message>
+        <source>Select a language</source>
+        <translation>Selecione o Idioma</translation>
+    </message>
 </context>
 <context>
     <name>SetupWidget</name>
@@ -1134,12 +1100,12 @@ Isso pode levar até um minuto.</translation>
         <translation>Controle Longitudinal de Ponta a Ponta</translation>
     </message>
     <message>
-        <source>When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits/forks.These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc.</source>
-        <translation>Quando a navegação tem um destino, o openpilot insere as informações do mapa no modelo. Isso fornece contexto útil para o modelo e permite que o openpilot mantenha a esquerda ou a direita apropriadamente em bifurcações/saídas. O comportamento de mudança de faixa permanece inalterado e ainda ativado pelo motorista. Este é um recurso de qualidade embrionária; erros devem ser esperados, particularmente em torno de saídas/bifurcações. Esses erros podem incluir travessias não intencionais na faixa de rodagem, saída tardia, condução em direção a barreiras divisórias nas áreas delimitadas por faixas pintadas, etc.</translation>
+        <source>When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc.</source>
+        <translation>Quando a navegação tem um destino, o openpilot insere as informações do mapa no modelo. Isso fornece contexto útil para o modelo e permite que o openpilot mantenha a esquerda ou a direita apropriadamente em bifurcações/saídas. O comportamento de mudança de faixa permanece inalterado e ainda é ativado somente pelo motorista. Este é um recurso de qualidade embrionária; erros devem ser esperados, principalmente em torno de saídas e bifurcações. Esses erros podem incluir travessias não intencionais na faixa de rodagem, saída tardia, condução em direção a barreiras divisórias nas áreas de marcas de canalização, etc.</translation>
     </message>
     <message>
-        <source>The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner.When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green.</source>
-        <translation>A visualização de condução fará a transição para a câmera grande angular voltada para a estrada em baixas velocidades para mostrar melhor algumas curvas. O logotipo do modo Experimental também será mostrado no canto superior direito. Quando um destino de navegação é definido e o modelo de condução o utiliza como entrada, o caminho de condução no mapa fica verde.</translation>
+        <source>The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green.</source>
+        <translation>A visualização de condução fará a transição para a câmera grande angular voltada para a estrada em baixas velocidades para mostrar melhor algumas curvas. O logotipo do modo Experimental também será mostrado no canto superior direito. Quando um destino de navegação é definido e o modelo de condução o utiliza como entrada o caminho de condução no mapa fica verde.</translation>
     </message>
 </context>
 <context>
diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts
index 75665eb84e..a1748a6951 100644
--- a/selfdrive/ui/translations/main_th.ts
+++ b/selfdrive/ui/translations/main_th.ts
@@ -226,8 +226,8 @@
         <translation>ปิดเครื่อง</translation>
     </message>
     <message>
-        <source>openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required.</source>
-        <translation>openpilot กำหนดให้ติดตั้งอุปกรณ์ โดยสามารถเอียงด้านซ้ายหรือขวาไม่เกิน 4° และเอียงขึ้นด้านบนไม่เกิน 5° หรือเอียงลงด้านล่างไม่เกิน 8° openpilot ทำการคาลิเบรทอย่างต่อเนื่อง แทบจะไม่จำเป็นต้องทำการรีเซ็ตการคาลิเบรท</translation>
+        <source>openpilot requires the device to be mounted within 4° left or right and within 5° up or 9° down. openpilot is continuously calibrating, resetting is rarely required.</source>
+        <translation>openpilot กำหนดให้ติดตั้งอุปกรณ์ โดยสามารถเอียงด้านซ้ายหรือขวาไม่เกิน 4° และเอียงขึ้นด้านบนไม่เกิน 5° หรือเอียงลงด้านล่างไม่เกิน 9° openpilot ทำการคาลิเบรทอย่างต่อเนื่อง แทบจะไม่จำเป็นต้องทำการรีเซ็ตการคาลิเบรท</translation>
     </message>
     <message>
         <source> Your device is pointed %1° %2 and %3° %4.</source>
@@ -274,33 +274,6 @@
         <translation>ทบทวน</translation>
     </message>
 </context>
-<context>
-    <name>DriveStats</name>
-    <message>
-        <source>Drives</source>
-        <translation>การขับขี่</translation>
-    </message>
-    <message>
-        <source>Hours</source>
-        <translation>ชั่วโมง</translation>
-    </message>
-    <message>
-        <source>ALL TIME</source>
-        <translation>ทั้งหมด</translation>
-    </message>
-    <message>
-        <source>PAST WEEK</source>
-        <translation>สัปดาห์ที่ผ่านมา</translation>
-    </message>
-    <message>
-        <source>KM</source>
-        <translation>กิโลเมตร</translation>
-    </message>
-    <message>
-        <source>Miles</source>
-        <translation>ไมล์</translation>
-    </message>
-</context>
 <context>
     <name>DriverViewScene</name>
     <message>
@@ -353,33 +326,6 @@
         <source>hr</source>
         <translation>ชม.</translation>
     </message>
-    <message>
-        <source>km</source>
-        <translation>กม.</translation>
-    </message>
-    <message>
-        <source>mi</source>
-        <translation>ไมล์</translation>
-    </message>
-</context>
-<context>
-    <name>MapInstructions</name>
-    <message>
-        <source> km</source>
-        <translation> กม.</translation>
-    </message>
-    <message>
-        <source> m</source>
-        <translation> ม.</translation>
-    </message>
-    <message>
-        <source> mi</source>
-        <translation> ไมล์</translation>
-    </message>
-    <message>
-        <source> ft</source>
-        <translation> ฟุต</translation>
-    </message>
 </context>
 <context>
     <name>MapSettings</name>
@@ -402,6 +348,10 @@
         <source>Waiting for GPS</source>
         <translation>กำลังรอสัญญาณ GPS</translation>
     </message>
+    <message>
+        <source>Waiting for route</source>
+        <translation>กำลังรอเส้นทาง</translation>
+    </message>
 </context>
 <context>
     <name>MultiOptionDialog</name>
@@ -554,12 +504,16 @@
         <translation>การเข้าถึงระยะไกล</translation>
     </message>
     <message>
-        <source>1 year of storage</source>
-        <translation>จัดเก็บข้อมูลนาน 1 ปี</translation>
+        <source>24/7 LTE connectivity</source>
+        <translation>การเชื่อมต่อ LTE แบบ 24/7</translation>
     </message>
     <message>
-        <source>Developer perks</source>
-        <translation>สิทธิพิเศษสำหรับนักพัฒนา</translation>
+        <source>1 year of drive storage</source>
+        <translation>จัดเก็บข้อมูลการขับขี่นาน 1 ปี</translation>
+    </message>
+    <message>
+        <source>Turn-by-turn navigation</source>
+        <translation>การนำทางแบบเลี้ยวต่อเลี้ยว</translation>
     </message>
 </context>
 <context>
@@ -609,6 +563,22 @@
             <numerusform>%n วันที่แล้ว</numerusform>
         </translation>
     </message>
+    <message>
+        <source>km</source>
+        <translation>กม.</translation>
+    </message>
+    <message>
+        <source>m</source>
+        <translation>ม.</translation>
+    </message>
+    <message>
+        <source>mi</source>
+        <translation>ไมล์</translation>
+    </message>
+    <message>
+        <source>ft</source>
+        <translation>ฟุต</translation>
+    </message>
 </context>
 <context>
     <name>Reset</name>
@@ -752,6 +722,10 @@ This may take up to a minute.</source>
         <source>No custom software found at this URL.</source>
         <translation>ไม่พบซอฟต์แวร์ที่กำหนดเองที่ URL นี้</translation>
     </message>
+    <message>
+        <source>Select a language</source>
+        <translation>เลือกภาษา</translation>
+    </message>
 </context>
 <context>
     <name>SetupWidget</name>
@@ -1057,10 +1031,6 @@ This may take up to a minute.</source>
         <source>openpilot defaults to driving in &lt;b&gt;chill mode&lt;/b&gt;. Experimental mode enables &lt;b&gt;alpha-level features&lt;/b&gt; that aren&apos;t ready for chill mode. Experimental features are listed below:</source>
         <translation>โดยปกติ openpilot จะขับใน&lt;b&gt;โหมดชิล&lt;/b&gt; เปิดโหมดทดลองเพื่อใช้&lt;b&gt;ความสามารถในขั้นพัฒนา&lt;/b&gt; ซึ่งยังไม่พร้อมสำหรับโหมดชิล ความสามารถในขั้นพัฒนามีดังนี้:</translation>
     </message>
-    <message>
-        <source>🌮 End-to-End Longitudinal Control 🌮</source>
-        <translation>🌮 ควบคุมเร่ง/เบรคแบบ End-to-End 🌮</translation>
-    </message>
     <message>
         <source>Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected.</source>
         <translation>ให้ openpilot ควบคุมการเร่ง/เบรค โดย openpilot จะขับอย่างที่มนุษย์คิด รวมถึงการหยุดที่ไฟแดง และป้ายหยุดรถ เนื่องจาก openpilot จะกำหนดความเร็วในการขับด้วยตัวเอง การตั้งความเร็วจะเป็นเพียงการกำหนดความเร็วสูงสูดเท่านั้น ความสามารถนี้ยังอยู่ในขั้นพัฒนา อาจเกิดข้อผิดพลาดขึ้นได้</translation>
@@ -1069,10 +1039,6 @@ This may take up to a minute.</source>
         <source>New Driving Visualization</source>
         <translation>การแสดงภาพการขับขี่แบบใหม่</translation>
     </message>
-    <message>
-        <source>The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner.</source>
-        <translation>การแสดงภาพการขับขี่จะเปลี่ยนไปใช้กล้องมุมกว้างที่หันหน้าไปทางถนนเมื่ออยู่ในความเร็วต่ำ เพื่อแสดงภาพการเลี้ยวที่ดีขึ้น โลโก้โหมดการทดลองจะแสดงที่มุมบนขวาด้วย</translation>
-    </message>
     <message>
         <source>Experimental mode is currently unavailable on this car since the car&apos;s stock ACC is used for longitudinal control.</source>
         <translation>ขณะนี้โหมดทดลองไม่สามารถใช้งานได้ในรถคันนี้ เนื่องจากเปิดใช้ระบบควบคุมการเร่ง/เบรคของรถที่ติดตั้งจากโรงงานอยู่</translation>
@@ -1081,10 +1047,6 @@ This may take up to a minute.</source>
         <source>openpilot longitudinal control may come in a future update.</source>
         <translation>ระบบควบคุมการเร่ง/เบรคโดย openpilot อาจมาในการอัปเดตในอนาคต</translation>
     </message>
-    <message>
-        <source>Enable experimental longitudinal control to allow Experimental mode.</source>
-        <translation>เปิดระบบควบคุมการเร่ง/เบรคขั้นพัฒนาโดย openpilot เพื่อเปิดใช้งานโหมดทดลอง</translation>
-    </message>
     <message>
         <source>openpilot Longitudinal Control (Alpha)</source>
         <translation>ระบบควบคุมการเร่ง/เบรคโดย openpilot (Alpha)</translation>
@@ -1121,6 +1083,26 @@ This may take up to a minute.</source>
         <source>An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches.</source>
         <translation>ระบบควบคุมการเร่ง/เบรคโดย openpilot เวอร์ชัน alpha สามารถทดสอบได้พร้อมกับโหมดการทดลอง บน branch ที่กำลังพัฒนา</translation>
     </message>
+    <message>
+        <source>End-to-End Longitudinal Control</source>
+        <translation>ควบคุมเร่ง/เบรคแบบ End-to-End</translation>
+    </message>
+    <message>
+        <source>Navigate on openpilot</source>
+        <translation>การนำทางบน openpilot</translation>
+    </message>
+    <message>
+        <source>When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc.</source>
+        <translation>เมื่อการนำทางมีจุดหมายปลายทาง openpilot จะป้อนข้อมูลแผนที่เข้าไปยังโมเดล ซึ่งจะเป็นบริบทที่มีประโยชน์สำหรับโมเดลและจะทำให้ openpilot สามารถรักษาเลนซ้ายหรือขวาได้อย่างเหมาะสมบริเวณทางแยกหรือทางออก พฤติกรรมการเปลี่ยนเลนยังคงเหมือนเดิมและยังคงต้องถูกเริ่มโดยคนขับ ความสามารถนี้ยังอยู่ในระดับ alpha ซึ่งอาจะเกิดความผิดพลาดได้โดยเฉพาะบริเวณทางแยกหรือทางออก ความผิดพลาดที่อาจเกิดขึ้นได้อาจรวมถึงการข้ามเส้นแบ่งเลนโดยไม่ตั้งใจ, การเข้าช่องทางออกช้ากว่าปกติ, การขับเข้าหาแบริเออร์ในเขตปลอดภัย, ฯลฯ</translation>
+    </message>
+    <message>
+        <source>The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green.</source>
+        <translation>การแสดงภาพการขับขี่จะเปลี่ยนไปใช้กล้องมุมกว้างที่หันหน้าไปทางถนนเมื่ออยู่ในความเร็วต่ำ เพื่อแสดงภาพการเลี้ยวที่ดีขึ้น โลโก้โหมดการทดลองจะแสดงที่มุมบนขวาด้วย เมื่อเป้าหมายการนำทางถูกเลือกและโมเดลการขับขี่กำลังใช้เป็นอินพุต เส้นทางการขับขี่บนแผนที่จะเปลี่ยนเป็นสีเขียว</translation>
+    </message>
+    <message>
+        <source>Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode.</source>
+        <translation>เปิดระบบควบคุมการเร่ง/เบรคโดย openpilot (alpha) เพื่อเปิดใช้งานโหมดทดลอง</translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>
@@ -1172,12 +1154,12 @@ This may take up to a minute.</source>
         <translation>เปิดการตั้งค่า</translation>
     </message>
     <message>
-        <source>Uploading training data</source>
-        <translation>กำลังอัปโหลดข้อมูลสำหรับการฝึก</translation>
+        <source>Ready to upload</source>
+        <translation>พร้อมจะอัปโหลด</translation>
     </message>
     <message>
-        <source>Your data is used to train driving models and help improve openpilot</source>
-        <translation>ข้อมูลของคุณถูกใช้เพื่อฝึกโมเดลการขับขี่และช่วยปรับปรุง openpilot</translation>
+        <source>Training data will be pulled periodically while your device is on Wi-Fi</source>
+        <translation>ข้อมูลการฝึกฝนจะถูกดึงเป็นระยะระหว่างที่อุปกรณ์ของคุณเชื่อมต่อกับ Wi-Fi</translation>
     </message>
 </context>
 <context>
diff --git a/selfdrive/ui/translations/main_tr.ts b/selfdrive/ui/translations/main_tr.ts
new file mode 100644
index 0000000000..51fdddd5ce
--- /dev/null
+++ b/selfdrive/ui/translations/main_tr.ts
@@ -0,0 +1,1186 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!DOCTYPE TS>
+<TS version="2.1" language="tr_TR">
+<context>
+    <name>AbstractAlert</name>
+    <message>
+        <source>Close</source>
+        <translation>Kapat</translation>
+    </message>
+    <message>
+        <source>Snooze Update</source>
+        <translation>Güncellemeyi sessize al</translation>
+    </message>
+    <message>
+        <source>Reboot and Update</source>
+        <translation>Güncelle ve Yeniden başlat</translation>
+    </message>
+</context>
+<context>
+    <name>AdvancedNetworking</name>
+    <message>
+        <source>Back</source>
+        <translation>Geri dön</translation>
+    </message>
+    <message>
+        <source>Enable Tethering</source>
+        <translation>Kişisel erişim noktasını aç</translation>
+    </message>
+    <message>
+        <source>Tethering Password</source>
+        <translation>Kişisel erişim noktasının parolası</translation>
+    </message>
+    <message>
+        <source>EDIT</source>
+        <translation>DÜZENLE</translation>
+    </message>
+    <message>
+        <source>Enter new tethering password</source>
+        <translation>Erişim noktasına yeni bir sonraki başlatılışında çekilir. parola belirleyin.</translation>
+    </message>
+    <message>
+        <source>IP Address</source>
+        <translation>IP Adresi</translation>
+    </message>
+    <message>
+        <source>Enable Roaming</source>
+        <translation>Hücresel veri aç</translation>
+    </message>
+    <message>
+        <source>APN Setting</source>
+        <translation>APN Ayarları</translation>
+    </message>
+    <message>
+        <source>Enter APN</source>
+        <translation>APN Gir</translation>
+    </message>
+    <message>
+        <source>leave blank for automatic configuration</source>
+        <translation>otomatik yapılandırma için boş bırakın</translation>
+    </message>
+    <message>
+        <source>Cellular Metered</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Prevent large data uploads when on a metered connection</source>
+        <translation type="unfinished"></translation>
+    </message>
+</context>
+<context>
+    <name>AnnotatedCameraWidget</name>
+    <message>
+        <source>km/h</source>
+        <translation type="unfinished">km/h</translation>
+    </message>
+    <message>
+        <source>mph</source>
+        <translation type="unfinished">mph</translation>
+    </message>
+    <message>
+        <source>MAX</source>
+        <translation type="unfinished">MAX</translation>
+    </message>
+    <message>
+        <source>SPEED</source>
+        <translation type="unfinished">HIZ</translation>
+    </message>
+    <message>
+        <source>LIMIT</source>
+        <translation type="unfinished">LİMİT</translation>
+    </message>
+</context>
+<context>
+    <name>ConfirmationDialog</name>
+    <message>
+        <source>Ok</source>
+        <translation>Tamam</translation>
+    </message>
+    <message>
+        <source>Cancel</source>
+        <translation>Vazgeç</translation>
+    </message>
+</context>
+<context>
+    <name>DeclinePage</name>
+    <message>
+        <source>You must accept the Terms and Conditions in order to use openpilot.</source>
+        <translation>Openpilotu kullanmak için Kullanıcı Koşullarını kabul etmelisiniz.</translation>
+    </message>
+    <message>
+        <source>Back</source>
+        <translation>Geri</translation>
+    </message>
+    <message>
+        <source>Decline, uninstall %1</source>
+        <translation>Reddet, Kurulumu kaldır. %1</translation>
+    </message>
+</context>
+<context>
+    <name>DestinationWidget</name>
+    <message>
+        <source>Home</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Work</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>No destination set</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>home</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>work</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>No %1 location set</source>
+        <translation type="unfinished"></translation>
+    </message>
+</context>
+<context>
+    <name>DevicePanel</name>
+    <message>
+        <source>Dongle ID</source>
+        <translation>Adaptör ID</translation>
+    </message>
+    <message>
+        <source>N/A</source>
+        <translation>N/A</translation>
+    </message>
+    <message>
+        <source>Serial</source>
+        <translation>Seri Numara</translation>
+    </message>
+    <message>
+        <source>Driver Camera</source>
+        <translation>Sürücü Kamerası</translation>
+    </message>
+    <message>
+        <source>PREVIEW</source>
+        <translation>ÖN İZLEME</translation>
+    </message>
+    <message>
+        <source>Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off)</source>
+        <translation>Sürücü kamerasının görüş açısını test etmek için kamerayı önizleyin (Araç kapalı olmalıdır.) </translation>
+    </message>
+    <message>
+        <source>Reset Calibration</source>
+        <translation>Kalibrasyonu sıfırla</translation>
+    </message>
+    <message>
+        <source>RESET</source>
+        <translation>SIFIRLA</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to reset calibration?</source>
+        <translation>Kalibrasyon ayarını sıfırlamak istediğinizden emin misiniz? </translation>
+    </message>
+    <message>
+        <source>Review Training Guide</source>
+        <translation>Eğitim kılavuzunu inceleyin</translation>
+    </message>
+    <message>
+        <source>REVIEW</source>
+        <translation>GÖZDEN GEÇİR</translation>
+    </message>
+    <message>
+        <source>Review the rules, features, and limitations of openpilot</source>
+        <translation>openpilot sisteminin kurallarını ve sınırlamalarını gözden geçirin.</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to review the training guide?</source>
+        <translation>Eğitim kılavuzunu incelemek istediğinizden emin misiniz?</translation>
+    </message>
+    <message>
+        <source>Regulatory</source>
+        <translation>Mevzuat</translation>
+    </message>
+    <message>
+        <source>VIEW</source>
+        <translation>BAK</translation>
+    </message>
+    <message>
+        <source>Change Language</source>
+        <translation>Dili değiştir</translation>
+    </message>
+    <message>
+        <source>CHANGE</source>
+        <translation>DEĞİŞTİR</translation>
+    </message>
+    <message>
+        <source>Select a language</source>
+        <translation>Dil seçin</translation>
+    </message>
+    <message>
+        <source>Reboot</source>
+        <translation>Yeniden başlat</translation>
+    </message>
+    <message>
+        <source>Power Off</source>
+        <translation>Sistemi kapat</translation>
+    </message>
+    <message>
+        <source>openpilot requires the device to be mounted within 4° left or right and within 5° up or 9° down. openpilot is continuously calibrating, resetting is rarely required.</source>
+        <translation>openpilot, cihazın 4° sola veya 5° yukarı yada 9° aşağı bakıcak şekilde monte edilmesi gerekmektedir. openpilot sürekli kendisini kalibre edilmektedir ve nadiren sıfırlama gerebilir.</translation>
+    </message>
+    <message>
+        <source> Your device is pointed %1° %2 and %3° %4.</source>
+        <translation> Cihazınız %1° %2 ve %3° %4 yönünde ayarlı</translation>
+    </message>
+    <message>
+        <source>down</source>
+        <translation>aşağı</translation>
+    </message>
+    <message>
+        <source>up</source>
+        <translation>yukarı</translation>
+    </message>
+    <message>
+        <source>left</source>
+        <translation>sol</translation>
+    </message>
+    <message>
+        <source>right</source>
+        <translation>sağ</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to reboot?</source>
+        <translation>Cihazı Tekrar başlatmak istediğinizden eminmisiniz?</translation>
+    </message>
+    <message>
+        <source>Disengage to Reboot</source>
+        <translation>Bağlantıyı kes ve Cihazı Yeniden başlat</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to power off?</source>
+        <translation>Cihazı kapatmak istediğizden eminmisiniz?</translation>
+    </message>
+    <message>
+        <source>Disengage to Power Off</source>
+        <translation>Bağlantıyı kes ve Cihazı kapat</translation>
+    </message>
+    <message>
+        <source>Reset</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Review</source>
+        <translation type="unfinished"></translation>
+    </message>
+</context>
+<context>
+    <name>DriverViewScene</name>
+    <message>
+        <source>camera starting</source>
+        <translation>kamera başlatılıyor</translation>
+    </message>
+</context>
+<context>
+    <name>ExperimentalModeButton</name>
+    <message>
+        <source>EXPERIMENTAL MODE ON</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>CHILL MODE ON</source>
+        <translation type="unfinished"></translation>
+    </message>
+</context>
+<context>
+    <name>InputDialog</name>
+    <message>
+        <source>Cancel</source>
+        <translation>Kapat</translation>
+    </message>
+    <message numerus="yes">
+        <source>Need at least %n character(s)!</source>
+        <translation>
+            <numerusform>En az %n karakter gerekli!</numerusform>
+        </translation>
+    </message>
+</context>
+<context>
+    <name>Installer</name>
+    <message>
+        <source>Installing...</source>
+        <translation>Yükleniyor...</translation>
+    </message>
+</context>
+<context>
+    <name>MapETA</name>
+    <message>
+        <source>eta</source>
+        <translation>tahmini varış süresi</translation>
+    </message>
+    <message>
+        <source>min</source>
+        <translation>dk</translation>
+    </message>
+    <message>
+        <source>hr</source>
+        <translation>saat</translation>
+    </message>
+</context>
+<context>
+    <name>MapSettings</name>
+    <message>
+        <source>NAVIGATION</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Manage at connect.comma.ai</source>
+        <translation type="unfinished"></translation>
+    </message>
+</context>
+<context>
+    <name>MapWindow</name>
+    <message>
+        <source>Map Loading</source>
+        <translation>Harita yükleniyor</translation>
+    </message>
+    <message>
+        <source>Waiting for GPS</source>
+        <translation>GPS verisi bekleniyor...</translation>
+    </message>
+    <message>
+        <source>Waiting for route</source>
+        <translation type="unfinished"></translation>
+    </message>
+</context>
+<context>
+    <name>MultiOptionDialog</name>
+    <message>
+        <source>Select</source>
+        <translation>Seç</translation>
+    </message>
+    <message>
+        <source>Cancel</source>
+        <translation>İptal et</translation>
+    </message>
+</context>
+<context>
+    <name>Networking</name>
+    <message>
+        <source>Advanced</source>
+        <translation>Gelişmiş Seçenekler</translation>
+    </message>
+    <message>
+        <source>Enter password</source>
+        <translation>Parolayı girin</translation>
+    </message>
+    <message>
+        <source>for &quot;%1&quot;</source>
+        <translation>için &quot;%1&quot;</translation>
+    </message>
+    <message>
+        <source>Wrong password</source>
+        <translation>Yalnış parola</translation>
+    </message>
+</context>
+<context>
+    <name>OffroadAlert</name>
+    <message>
+        <source>Device temperature too high. System cooling down before starting. Current internal component temperature: %1</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Immediately connect to the internet to check for updates. If you do not connect to the internet, openpilot won&apos;t engage in %1</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Connect to internet to check for updates. openpilot won&apos;t automatically start until it connects to internet to check for updates.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Unable to download updates
+%1</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Invalid date and time settings, system won&apos;t start. Connect to internet to set time.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Taking camera snapshots. System won&apos;t start until finished.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>An update to your device&apos;s operating system is downloading in the background. You will be prompted to update when it&apos;s ready to install.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Device failed to register. It will not connect to or upload to comma.ai servers, and receives no support from comma.ai. If this is an official device, visit https://comma.ai/support.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>NVMe drive not mounted.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Unsupported NVMe drive detected. Device may draw significantly more power and overheat due to the unsupported NVMe.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>openpilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please submit a pull request to add the firmware versions to the proper vehicle. Need help? Join discord.comma.ai.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>openpilot was unable to identify your car. Check integrity of cables and ensure all connections are secure, particularly that the comma power is fully inserted in the OBD-II port of the vehicle. Need help? Join discord.comma.ai.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>openpilot detected a change in the device&apos;s mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.</source>
+        <translation type="unfinished"></translation>
+    </message>
+</context>
+<context>
+    <name>OffroadHome</name>
+    <message>
+        <source>UPDATE</source>
+        <translation>GÜNCELLE</translation>
+    </message>
+    <message>
+        <source> ALERTS</source>
+        <translation> UYARILAR</translation>
+    </message>
+    <message>
+        <source> ALERT</source>
+        <translation> UYARI</translation>
+    </message>
+</context>
+<context>
+    <name>PairingPopup</name>
+    <message>
+        <source>Pair your device to your comma account</source>
+        <translation>comma.ai hesabınız ile cihazı eşleştirin</translation>
+    </message>
+    <message>
+        <source>Go to https://connect.comma.ai on your phone</source>
+        <translation>Telefonuzdan https://connect.comma.ai sitesine gidin</translation>
+    </message>
+    <message>
+        <source>Click &quot;add new device&quot; and scan the QR code on the right</source>
+        <translation>Yeni cihaz eklemek için sağdaki QR kodunu okutun</translation>
+    </message>
+    <message>
+        <source>Bookmark connect.comma.ai to your home screen to use it like an app</source>
+        <translation>Uygulama gibi kullanmak için connect.comma.ai sitesini yer işaretlerine ekleyin.</translation>
+    </message>
+</context>
+<context>
+    <name>ParamControl</name>
+    <message>
+        <source>Enable</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished"></translation>
+    </message>
+</context>
+<context>
+    <name>PrimeAdWidget</name>
+    <message>
+        <source>Upgrade Now</source>
+        <translation>Hemen yükselt</translation>
+    </message>
+    <message>
+        <source>Become a comma prime member at connect.comma.ai</source>
+        <translation>connect.comma.ai üzerinden comma prime üyesi olun</translation>
+    </message>
+    <message>
+        <source>PRIME FEATURES:</source>
+        <translation>PRIME ABONELİĞİNİN ÖZELLİKLERİ:</translation>
+    </message>
+    <message>
+        <source>Remote access</source>
+        <translation>Uzaktan erişim</translation>
+    </message>
+    <message>
+        <source>24/7 LTE connectivity</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>1 year of drive storage</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Turn-by-turn navigation</source>
+        <translation type="unfinished"></translation>
+    </message>
+</context>
+<context>
+    <name>PrimeUserWidget</name>
+    <message>
+        <source>✓ SUBSCRIBED</source>
+        <translation>✓ ABONE</translation>
+    </message>
+    <message>
+        <source>comma prime</source>
+        <translation>comma prime</translation>
+    </message>
+</context>
+<context>
+    <name>QObject</name>
+    <message>
+        <source>Reboot</source>
+        <translation>Yeniden başlat</translation>
+    </message>
+    <message>
+        <source>Exit</source>
+        <translation>Çık</translation>
+    </message>
+    <message>
+        <source>dashcam</source>
+        <translation>araç yol kamerası</translation>
+    </message>
+    <message>
+        <source>openpilot</source>
+        <translation>openpilot</translation>
+    </message>
+    <message numerus="yes">
+        <source>%n minute(s) ago</source>
+        <translation>
+            <numerusform>%n dakika önce</numerusform>
+        </translation>
+    </message>
+    <message numerus="yes">
+        <source>%n hour(s) ago</source>
+        <translation>
+            <numerusform>%n saat önce</numerusform>
+        </translation>
+    </message>
+    <message numerus="yes">
+        <source>%n day(s) ago</source>
+        <translation>
+            <numerusform>%n gün önce</numerusform>
+        </translation>
+    </message>
+    <message>
+        <source>km</source>
+        <translation>km</translation>
+    </message>
+    <message>
+        <source>m</source>
+        <translation>m</translation>
+    </message>
+    <message>
+        <source>mi</source>
+        <translation>mil</translation>
+    </message>
+    <message>
+        <source>ft</source>
+        <translation>ft</translation>
+    </message>
+</context>
+<context>
+    <name>Reset</name>
+    <message>
+        <source>Reset failed. Reboot to try again.</source>
+        <translation>Sıfırlama başarız oldu. Cihazı yeniden başlatın ve tekrar deneyin.</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to reset your device?</source>
+        <translation>Cihazı sıfırlamak istediğinizden eminmisiniz ?</translation>
+    </message>
+    <message>
+        <source>System Reset</source>
+        <translation>Sistemi sıfırla</translation>
+    </message>
+    <message>
+        <source>Cancel</source>
+        <translation>İptal</translation>
+    </message>
+    <message>
+        <source>Reboot</source>
+        <translation>Yeniden başlat</translation>
+    </message>
+    <message>
+        <source>Confirm</source>
+        <translation>Onayla</translation>
+    </message>
+    <message>
+        <source>Resetting device...
+This may take up to a minute.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Press confirm to erase all content and settings. Press cancel to resume boot.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device.</source>
+        <translation type="unfinished"></translation>
+    </message>
+</context>
+<context>
+    <name>SettingsWindow</name>
+    <message>
+        <source>×</source>
+        <translation>x</translation>
+    </message>
+    <message>
+        <source>Device</source>
+        <translation>Cihaz</translation>
+    </message>
+    <message>
+        <source>Network</source>
+        <translation>Ağ</translation>
+    </message>
+    <message>
+        <source>Toggles</source>
+        <translation>Değiştirme</translation>
+    </message>
+    <message>
+        <source>Software</source>
+        <translation>Yazılım</translation>
+    </message>
+</context>
+<context>
+    <name>Setup</name>
+    <message>
+        <source>WARNING: Low Voltage</source>
+        <translation>UYARI: Düşük voltaj</translation>
+    </message>
+    <message>
+        <source>Power your device in a car with a harness or proceed at your own risk.</source>
+        <translation>Cihazınızı emniyet kemeri olan bir arabada çalıştırın veya riski kabul ederek devam edin.</translation>
+    </message>
+    <message>
+        <source>Power off</source>
+        <translation>Sistemi kapat</translation>
+    </message>
+    <message>
+        <source>Continue</source>
+        <translation>Devam et</translation>
+    </message>
+    <message>
+        <source>Getting Started</source>
+        <translation>Başlarken</translation>
+    </message>
+    <message>
+        <source>Before we get on the road, let’s finish installation and cover some details.</source>
+        <translation>Yola çıkmadan önce kurulumu bitirin ve bazı detayları gözden geçirin..</translation>
+    </message>
+    <message>
+        <source>Connect to Wi-Fi</source>
+        <translation>Wi-Fi ile bağlan</translation>
+    </message>
+    <message>
+        <source>Back</source>
+        <translation>Geri</translation>
+    </message>
+    <message>
+        <source>Continue without Wi-Fi</source>
+        <translation>Wi-Fi bağlantısı olmadan devam edin</translation>
+    </message>
+    <message>
+        <source>Waiting for internet</source>
+        <translation>İnternet bağlantısı bekleniyor.</translation>
+    </message>
+    <message>
+        <source>Enter URL</source>
+        <translation>URL girin</translation>
+    </message>
+    <message>
+        <source>for Custom Software</source>
+        <translation>özel yazılım için</translation>
+    </message>
+    <message>
+        <source>Downloading...</source>
+        <translation>İndiriliyor...</translation>
+    </message>
+    <message>
+        <source>Download Failed</source>
+        <translation>İndirme başarısız.</translation>
+    </message>
+    <message>
+        <source>Ensure the entered URL is valid, and the device’s internet connection is good.</source>
+        <translation>Girilen URL nin geçerli olduğundan ve cihazın internet bağlantısının olduğunu kontrol edin</translation>
+    </message>
+    <message>
+        <source>Reboot device</source>
+        <translation>Cihazı yeniden başlat</translation>
+    </message>
+    <message>
+        <source>Start over</source>
+        <translation>Zacznij od początku</translation>
+    </message>
+    <message>
+        <source>Something went wrong. Reboot the device.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>No custom software found at this URL.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Select a language</source>
+        <translation>Dil seçin</translation>
+    </message>
+</context>
+<context>
+    <name>SetupWidget</name>
+    <message>
+        <source>Finish Setup</source>
+        <translation>Kurulumu bitir</translation>
+    </message>
+    <message>
+        <source>Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer.</source>
+        <translation>Cihazınızı comma connect (connect.comma.ai) ile eşleştirin ve comma prime aboneliğine göz atın.</translation>
+    </message>
+    <message>
+        <source>Pair device</source>
+        <translation>Cihazı eşleştirme</translation>
+    </message>
+</context>
+<context>
+    <name>Sidebar</name>
+    <message>
+        <source>CONNECT</source>
+        <translation>BAĞLANTI</translation>
+    </message>
+    <message>
+        <source>OFFLINE</source>
+        <translation>ÇEVRİMDIŞI</translation>
+    </message>
+    <message>
+        <source>ONLINE</source>
+        <translation>ÇEVRİMİÇİ</translation>
+    </message>
+    <message>
+        <source>ERROR</source>
+        <translation>HATA</translation>
+    </message>
+    <message>
+        <source>TEMP</source>
+        <translation>SICAKLIK</translation>
+    </message>
+    <message>
+        <source>HIGH</source>
+        <translation>YÜKSEK</translation>
+    </message>
+    <message>
+        <source>GOOD</source>
+        <translation>İYİ</translation>
+    </message>
+    <message>
+        <source>OK</source>
+        <translation>TAMAM</translation>
+    </message>
+    <message>
+        <source>VEHICLE</source>
+        <translation>ARAÇ</translation>
+    </message>
+    <message>
+        <source>NO</source>
+        <translation>HAYIR</translation>
+    </message>
+    <message>
+        <source>PANDA</source>
+        <translation>PANDA</translation>
+    </message>
+    <message>
+        <source>GPS</source>
+        <translation>GPS</translation>
+    </message>
+    <message>
+        <source>SEARCH</source>
+        <translation>ARA</translation>
+    </message>
+    <message>
+        <source>--</source>
+        <translation>--</translation>
+    </message>
+    <message>
+        <source>Wi-Fi</source>
+        <translation>Wi-FI</translation>
+    </message>
+    <message>
+        <source>ETH</source>
+        <translation>ETH</translation>
+    </message>
+    <message>
+        <source>2G</source>
+        <translation>2G</translation>
+    </message>
+    <message>
+        <source>3G</source>
+        <translation>3G</translation>
+    </message>
+    <message>
+        <source>LTE</source>
+        <translation>LTE</translation>
+    </message>
+    <message>
+        <source>5G</source>
+        <translation>5G</translation>
+    </message>
+</context>
+<context>
+    <name>SoftwarePanel</name>
+    <message>
+        <source>Uninstall %1</source>
+        <translation>Kaldır %1</translation>
+    </message>
+    <message>
+        <source>UNINSTALL</source>
+        <translation>KALDIR</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to uninstall?</source>
+        <translation>Kaldırmak istediğinden eminmisin?</translation>
+    </message>
+    <message>
+        <source>CHECK</source>
+        <translation>KONTROL ET</translation>
+    </message>
+    <message>
+        <source>Updates are only downloaded while the car is off.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Current Version</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Download</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Install Update</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>INSTALL</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Target Branch</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>SELECT</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Select a branch</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Uninstall</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>failed to check for update</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>DOWNLOAD</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>update available</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>never</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>up to date, last checked %1</source>
+        <translation type="unfinished"></translation>
+    </message>
+</context>
+<context>
+    <name>SshControl</name>
+    <message>
+        <source>SSH Keys</source>
+        <translation>SSH Anahtarları</translation>
+    </message>
+    <message>
+        <source>Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username.</source>
+        <translation>UYARI: Bu, GitHub ayarlarınızdaki tüm ortak anahtarlara SSH erişimi sağlar. Asla kendi kullanıcı adınız dışında bir sonraki başlatılışında çekilir. GitHub kullanıcı adı girmeyin.</translation>
+    </message>
+    <message>
+        <source>ADD</source>
+        <translation>EKLE</translation>
+    </message>
+    <message>
+        <source>Enter your GitHub username</source>
+        <translation>Github kullanıcı adınızı giriniz</translation>
+    </message>
+    <message>
+        <source>LOADING</source>
+        <translation>YÜKLENİYOR</translation>
+    </message>
+    <message>
+        <source>REMOVE</source>
+        <translation>KALDIR</translation>
+    </message>
+    <message>
+        <source>Username &apos;%1&apos; has no keys on GitHub</source>
+        <translation>Kullanısının &apos;%1&apos; Github erişim anahtarı yok</translation>
+    </message>
+    <message>
+        <source>Request timed out</source>
+        <translation>İstek zaman aşımına uğradı</translation>
+    </message>
+    <message>
+        <source>Username &apos;%1&apos; doesn&apos;t exist on GitHub</source>
+        <translation>Github kullanıcısı %1 bulunamadı</translation>
+    </message>
+</context>
+<context>
+    <name>SshToggle</name>
+    <message>
+        <source>Enable SSH</source>
+        <translation>SSH aç</translation>
+    </message>
+</context>
+<context>
+    <name>TermsPage</name>
+    <message>
+        <source>Terms &amp; Conditions</source>
+        <translation>Şartlar ve Koşullar</translation>
+    </message>
+    <message>
+        <source>Decline</source>
+        <translation>Reddet</translation>
+    </message>
+    <message>
+        <source>Scroll to accept</source>
+        <translation>Kabul etmek için kaydırın</translation>
+    </message>
+    <message>
+        <source>Agree</source>
+        <translation>Kabul et</translation>
+    </message>
+</context>
+<context>
+    <name>TogglesPanel</name>
+    <message>
+        <source>Enable openpilot</source>
+        <translation>openpilot&apos;u aktifleştir</translation>
+    </message>
+    <message>
+        <source>Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off.</source>
+        <translation>Ayarlanabilir hız sabitleyici ve şeritte kalma yardımı için openpilot sistemini kullanın. Bu özelliği kullanırken her zaman dikkatli olmanız gerekiyor. Bu ayarın değiştirilmesi için araç kapatılıp açılması gerekiyor.</translation>
+    </message>
+    <message>
+        <source>Enable Lane Departure Warnings</source>
+        <translation>Şerit ihlali uyarı alın</translation>
+    </message>
+    <message>
+        <source>Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h).</source>
+        <translation>50 km/s (31 mph) hızın üzerinde sürüş sırasında aracınız dönüş sinyali vermeden algılanan bir sonraki başlatılışında çekilir. şerit çizgisi ihlalinde şeride geri dönmek için uyarılar alın.</translation>
+    </message>
+    <message>
+        <source>Use Metric System</source>
+        <translation>Metrik sistemi kullan</translation>
+    </message>
+    <message>
+        <source>Display speed in km/h instead of mph.</source>
+        <translation>Hızı mph yerine km/h şeklinde görüntüleyin.</translation>
+    </message>
+    <message>
+        <source>Record and Upload Driver Camera</source>
+        <translation>Sürücü kamerasını kayıt et.</translation>
+    </message>
+    <message>
+        <source>Upload data from the driver facing camera and help improve the driver monitoring algorithm.</source>
+        <translation>Sürücüye bakan kamera verisini yükleyin ve Cihazın algoritmasını geliştirmemize yardımcı olun.</translation>
+    </message>
+    <message>
+        <source>When enabled, pressing the accelerator pedal will disengage openpilot.</source>
+        <translation>Aktifleştirilirse eğer gaz pedalına basınca openpilot devre dışı kalır.</translation>
+    </message>
+    <message>
+        <source>Show ETA in 24h Format</source>
+        <translation>Tahmini varış süresini 24 saat formatı şeklinde göster</translation>
+    </message>
+    <message>
+        <source>Use 24h format instead of am/pm</source>
+        <translation>24 saat formatını kullan</translation>
+    </message>
+    <message>
+        <source>Show Map on Left Side of UI</source>
+        <translation>Haritayı arayüzün sol tarafında göster</translation>
+    </message>
+    <message>
+        <source>Show map on left side when in split screen view.</source>
+        <translation>Bölünmüş ekran görünümündeyken haritayı sol tarafta göster.</translation>
+    </message>
+    <message>
+        <source>openpilot Longitudinal Control (Alpha)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB).</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Experimental Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Disengage on Accelerator Pedal</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Aggressive</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Standard</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Relaxed</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Driving Personality</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>On this car, openpilot defaults to the car&apos;s built-in ACC instead of openpilot&apos;s longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>openpilot defaults to driving in &lt;b&gt;chill mode&lt;/b&gt;. Experimental mode enables &lt;b&gt;alpha-level features&lt;/b&gt; that aren&apos;t ready for chill mode. Experimental features are listed below:</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>End-to-End Longitudinal Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Navigate on openpilot</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>New Driving Visualization</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Experimental mode is currently unavailable on this car since the car&apos;s stock ACC is used for longitudinal control.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>openpilot longitudinal control may come in a future update.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode.</source>
+        <translation type="unfinished"></translation>
+    </message>
+</context>
+<context>
+    <name>Updater</name>
+    <message>
+        <source>Update Required</source>
+        <translation>Güncelleme yapılması gerekli</translation>
+    </message>
+    <message>
+        <source>An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB.</source>
+        <translation>İşletim sistemi güncellemesi gerekmektedir. Lütfen Cihazı daha hızlı günceleyebilmesi için bir sonraki başlatılışında çekilir. Wi-Fi ağına bağlayın. Dosyanın boyutu yaklaşık 1GB dır. </translation>
+    </message>
+    <message>
+        <source>Connect to Wi-Fi</source>
+        <translation>Wi-Fi ağına bağlan</translation>
+    </message>
+    <message>
+        <source>Install</source>
+        <translation>Yükle</translation>
+    </message>
+    <message>
+        <source>Back</source>
+        <translation>Geri</translation>
+    </message>
+    <message>
+        <source>Loading...</source>
+        <translation>Yükleniyor...</translation>
+    </message>
+    <message>
+        <source>Reboot</source>
+        <translation>Yeniden başlat</translation>
+    </message>
+    <message>
+        <source>Update failed</source>
+        <translation>Güncelleme başarız oldu</translation>
+    </message>
+</context>
+<context>
+    <name>WiFiPromptWidget</name>
+    <message>
+        <source>Setup Wi-Fi</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Connect to Wi-Fi to upload driving data and help improve openpilot</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Open Settings</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Ready to upload</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Training data will be pulled periodically while your device is on Wi-Fi</source>
+        <translation type="unfinished"></translation>
+    </message>
+</context>
+<context>
+    <name>WifiUI</name>
+    <message>
+        <source>Scanning for networks...</source>
+        <translation>Ağ aranıyor...</translation>
+    </message>
+    <message>
+        <source>CONNECTING...</source>
+        <translation>BAĞLANILIYOR...</translation>
+    </message>
+    <message>
+        <source>FORGET</source>
+        <translation>UNUT</translation>
+    </message>
+    <message>
+        <source>Forget Wi-Fi Network &quot;%1&quot;?</source>
+        <translation>Wi-Fi ağını unut &quot;%1&quot;?</translation>
+    </message>
+    <message>
+        <source>Forget</source>
+        <translation type="unfinished"></translation>
+    </message>
+</context>
+</TS>
diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts
index 313c57a903..7b682535ce 100644
--- a/selfdrive/ui/translations/main_zh-CHS.ts
+++ b/selfdrive/ui/translations/main_zh-CHS.ts
@@ -120,27 +120,27 @@
     <name>DestinationWidget</name>
     <message>
         <source>Home</source>
-        <translation type="unfinished"></translation>
+        <translation>住家</translation>
     </message>
     <message>
         <source>Work</source>
-        <translation type="unfinished"></translation>
+        <translation>工作</translation>
     </message>
     <message>
         <source>No destination set</source>
-        <translation type="unfinished"></translation>
+        <translation>尚未设置目的地</translation>
     </message>
     <message>
         <source>No %1 location set</source>
-        <translation type="unfinished"></translation>
+        <translation>尚未设置 %1 的位置</translation>
     </message>
     <message>
         <source>home</source>
-        <translation type="unfinished"></translation>
+        <translation>住家</translation>
     </message>
     <message>
         <source>work</source>
-        <translation type="unfinished"></translation>
+        <translation>工作</translation>
     </message>
 </context>
 <context>
@@ -191,7 +191,7 @@
     </message>
     <message>
         <source>Review the rules, features, and limitations of openpilot</source>
-        <translation>查看openpilot的使用规则，以及其功能和限制。</translation>
+        <translation>查看 openpilot 的使用规则，以及其功能和限制</translation>
     </message>
     <message>
         <source>Are you sure you want to review the training guide?</source>
@@ -226,8 +226,8 @@
         <translation>关机</translation>
     </message>
     <message>
-        <source>openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required.</source>
-        <translation>openpilot要求设备安装的偏航角在左4°和右4°之间，俯仰角在上5°和下8°之间。一般来说，openpilot会持续更新校准，很少需要重置。</translation>
+        <source>openpilot requires the device to be mounted within 4° left or right and within 5° up or 9° down. openpilot is continuously calibrating, resetting is rarely required.</source>
+        <translation>openpilot要求设备安装的偏航角在左4°和右4°之间，俯仰角在上5°和下9°之间。一般来说，openpilot会持续更新校准，很少需要重置。</translation>
     </message>
     <message>
         <source> Your device is pointed %1° %2 and %3° %4.</source>
@@ -274,33 +274,6 @@
         <translation>预览</translation>
     </message>
 </context>
-<context>
-    <name>DriveStats</name>
-    <message>
-        <source>Drives</source>
-        <translation>旅程数</translation>
-    </message>
-    <message>
-        <source>Hours</source>
-        <translation>小时</translation>
-    </message>
-    <message>
-        <source>ALL TIME</source>
-        <translation>全部</translation>
-    </message>
-    <message>
-        <source>PAST WEEK</source>
-        <translation>过去一周</translation>
-    </message>
-    <message>
-        <source>KM</source>
-        <translation>公里</translation>
-    </message>
-    <message>
-        <source>Miles</source>
-        <translation>英里</translation>
-    </message>
-</context>
 <context>
     <name>DriverViewScene</name>
     <message>
@@ -343,7 +316,7 @@
     <name>MapETA</name>
     <message>
         <source>eta</source>
-        <translation>埃塔</translation>
+        <translation>抵达</translation>
     </message>
     <message>
         <source>min</source>
@@ -353,43 +326,16 @@
         <source>hr</source>
         <translation>小时</translation>
     </message>
-    <message>
-        <source>km</source>
-        <translation>km</translation>
-    </message>
-    <message>
-        <source>mi</source>
-        <translation>mi</translation>
-    </message>
-</context>
-<context>
-    <name>MapInstructions</name>
-    <message>
-        <source> km</source>
-        <translation> km</translation>
-    </message>
-    <message>
-        <source> m</source>
-        <translation> m</translation>
-    </message>
-    <message>
-        <source> mi</source>
-        <translation> mi</translation>
-    </message>
-    <message>
-        <source> ft</source>
-        <translation> ft</translation>
-    </message>
 </context>
 <context>
     <name>MapSettings</name>
     <message>
         <source>NAVIGATION</source>
-        <translation type="unfinished"></translation>
+        <translation>导航</translation>
     </message>
     <message>
         <source>Manage at connect.comma.ai</source>
-        <translation type="unfinished"></translation>
+        <translation>请在 connect.comma.ai 上管理</translation>
     </message>
 </context>
 <context>
@@ -404,7 +350,7 @@
     </message>
     <message>
         <source>Waiting for route</source>
-        <translation type="unfinished"></translation>
+        <translation>等待路线</translation>
     </message>
 </context>
 <context>
@@ -441,56 +387,57 @@
     <name>OffroadAlert</name>
     <message>
         <source>Immediately connect to the internet to check for updates. If you do not connect to the internet, openpilot won&apos;t engage in %1</source>
-        <translation type="unfinished"></translation>
+        <translation>请立即连接网络检查更新。如果不连接网络，openpilot 将在 %1 后便无法使用</translation>
     </message>
     <message>
         <source>Connect to internet to check for updates. openpilot won&apos;t automatically start until it connects to internet to check for updates.</source>
-        <translation type="unfinished"></translation>
+        <translation>请连接至互联网以检查更新。在连接至互联网并完成更新检查之前，openpilot 将不会自动启动。</translation>
     </message>
     <message>
         <source>Unable to download updates
 %1</source>
-        <translation type="unfinished"></translation>
+        <translation>无法下载更新
+%1</translation>
     </message>
     <message>
         <source>Invalid date and time settings, system won&apos;t start. Connect to internet to set time.</source>
-        <translation type="unfinished"></translation>
+        <translation>日期和时间设置无效，系统无法启动。请连接至互联网以设置时间。</translation>
     </message>
     <message>
         <source>Taking camera snapshots. System won&apos;t start until finished.</source>
-        <translation type="unfinished"></translation>
+        <translation>正在使用相机拍摄中。在完成之前，系统将无法启动。</translation>
     </message>
     <message>
         <source>An update to your device&apos;s operating system is downloading in the background. You will be prompted to update when it&apos;s ready to install.</source>
-        <translation type="unfinished"></translation>
+        <translation>一个针对您设备的操作系统更新正在后台下载中。当更新准备好安装时，您将收到提示进行更新。</translation>
     </message>
     <message>
         <source>Device failed to register. It will not connect to or upload to comma.ai servers, and receives no support from comma.ai. If this is an official device, visit https://comma.ai/support.</source>
-        <translation type="unfinished"></translation>
+        <translation>设备注册失败。它将无法连接或上传至 comma.ai 服务器，并且无法获得 comma.ai 的支持。如果这是一个官方设备，请访问 https://comma.ai/support。</translation>
     </message>
     <message>
         <source>NVMe drive not mounted.</source>
-        <translation type="unfinished"></translation>
+        <translation>NVMe固态硬盘未被挂载。</translation>
     </message>
     <message>
         <source>Unsupported NVMe drive detected. Device may draw significantly more power and overheat due to the unsupported NVMe.</source>
-        <translation type="unfinished"></translation>
+        <translation>检测到不支持的 NVMe 固态硬盘。您的设备因为使用了不支持的 NVMe 固态硬盘可能会消耗更多电力并更易过热。</translation>
     </message>
     <message>
         <source>openpilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please submit a pull request to add the firmware versions to the proper vehicle. Need help? Join discord.comma.ai.</source>
-        <translation type="unfinished"></translation>
+        <translation>openpilot 无法识别您的车辆。您的车辆可能未被支持，或是其电控单元 (ECU) 未被识别。请提交一个 Pull Request 为您的车辆添加正确的固件版本。需要帮助吗？请加入 discord.comma.ai。</translation>
     </message>
     <message>
         <source>openpilot was unable to identify your car. Check integrity of cables and ensure all connections are secure, particularly that the comma power is fully inserted in the OBD-II port of the vehicle. Need help? Join discord.comma.ai.</source>
-        <translation type="unfinished"></translation>
+        <translation>openpilot 无法识别您的车辆。请检查线路是否正确安装并确保所有的连接都牢固，特别是确保 comma power 完全插入车辆的 OBD-II 接口。需要帮助吗？请加入 discord.comma.ai。</translation>
     </message>
     <message>
         <source>openpilot detected a change in the device&apos;s mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.</source>
-        <translation type="unfinished"></translation>
+        <translation>openpilot 检测到设备的安装位置发生变化。请确保设备完全安装在支架上，并确保支架牢固地固定在挡风玻璃上。</translation>
     </message>
     <message>
         <source>Device temperature too high. System cooling down before starting. Current internal component temperature: %1</source>
-        <translation type="unfinished"></translation>
+        <translation>设备温度过高。系统正在冷却中，等冷却完毕后才会启动。目前内部组件温度：%1</translation>
     </message>
 </context>
 <context>
@@ -558,15 +505,15 @@
     </message>
     <message>
         <source>24/7 LTE connectivity</source>
-        <translation type="unfinished"></translation>
+        <translation>全天候 LTE 連線</translation>
     </message>
     <message>
         <source>Turn-by-turn navigation</source>
-        <translation type="unfinished"></translation>
+        <translation>领航功能</translation>
     </message>
     <message>
         <source>1 year of drive storage</source>
-        <translation type="unfinished"></translation>
+        <translation>一年的行驶记录储存空间</translation>
     </message>
 </context>
 <context>
@@ -616,6 +563,22 @@
             <numerusform>%n 天前</numerusform>
         </translation>
     </message>
+    <message>
+        <source>km</source>
+        <translation>km</translation>
+    </message>
+    <message>
+        <source>m</source>
+        <translation>m</translation>
+    </message>
+    <message>
+        <source>mi</source>
+        <translation>mi</translation>
+    </message>
+    <message>
+        <source>ft</source>
+        <translation>ft</translation>
+    </message>
 </context>
 <context>
     <name>Reset</name>
@@ -645,16 +608,17 @@
     </message>
     <message>
         <source>Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device.</source>
-        <translation type="unfinished"></translation>
+        <translation>无法挂载数据分区。分区可能已经损坏。请确认是否要删除并重新设置。</translation>
     </message>
     <message>
         <source>Press confirm to erase all content and settings. Press cancel to resume boot.</source>
-        <translation type="unfinished"></translation>
+        <translation>按下确认以删除所有内容及设置。按下取消来继续开机。</translation>
     </message>
     <message>
         <source>Resetting device...
 This may take up to a minute.</source>
-        <translation type="unfinished"></translation>
+        <translation>设备重置中…
+这可能需要一分钟的时间。</translation>
     </message>
 </context>
 <context>
@@ -752,11 +716,15 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>No custom software found at this URL.</source>
-        <translation type="unfinished"></translation>
+        <translation>在此网址找不到自定义软件。</translation>
     </message>
     <message>
         <source>Something went wrong. Reboot the device.</source>
-        <translation type="unfinished"></translation>
+        <translation>发生了一些错误。请重新启动您的设备。</translation>
+    </message>
+    <message>
+        <source>Select a language</source>
+        <translation>选择语言</translation>
     </message>
 </context>
 <context>
@@ -913,23 +881,23 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>failed to check for update</source>
-        <translation type="unfinished"></translation>
+        <translation>检查更新失败</translation>
     </message>
     <message>
         <source>up to date, last checked %1</source>
-        <translation type="unfinished"></translation>
+        <translation>已经是最新版本，上次检查时间为 %1</translation>
     </message>
     <message>
         <source>DOWNLOAD</source>
-        <translation type="unfinished"></translation>
+        <translation>下载</translation>
     </message>
     <message>
         <source>update available</source>
-        <translation type="unfinished"></translation>
+        <translation>有可用的更新</translation>
     </message>
     <message>
         <source>never</source>
-        <translation type="unfinished"></translation>
+        <translation>从未更新</translation>
     </message>
 </context>
 <context>
@@ -1077,63 +1045,63 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>openpilot longitudinal control may come in a future update.</source>
-        <translation type="unfinished"></translation>
+        <translation>openpilot纵向控制可能会在未来的更新中提供。</translation>
     </message>
     <message>
         <source>openpilot Longitudinal Control (Alpha)</source>
-        <translation type="unfinished"></translation>
+        <translation>openpilot纵向控制（Alpha 版）</translation>
     </message>
     <message>
         <source>WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB).</source>
-        <translation type="unfinished"></translation>
+        <translation>警告：此车辆的 openpilot 纵向控制功能目前处于Alpha版本，使用此功能将会停用自动紧急制动（AEB）功能。</translation>
     </message>
     <message>
         <source>On this car, openpilot defaults to the car&apos;s built-in ACC instead of openpilot&apos;s longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha.</source>
-        <translation type="unfinished"></translation>
+        <translation>在这辆车上，openpilot 默认使用车辆内建的主动巡航控制（ACC），而非 openpilot 的纵向控制。启用此项功能可切换至 openpilot 的纵向控制。当启用 openpilot 纵向控制 Alpha 版本时，建议同时启用实验性模式（Experimental mode）。</translation>
     </message>
     <message>
         <source>Aggressive</source>
-        <translation type="unfinished"></translation>
+        <translation>积极</translation>
     </message>
     <message>
         <source>Standard</source>
-        <translation type="unfinished"></translation>
+        <translation>标准</translation>
     </message>
     <message>
         <source>Relaxed</source>
-        <translation type="unfinished"></translation>
+        <translation>舒适</translation>
     </message>
     <message>
         <source>Driving Personality</source>
-        <translation type="unfinished"></translation>
+        <translation>驾驶风格</translation>
     </message>
     <message>
         <source>Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars.</source>
-        <translation type="unfinished"></translation>
+        <translation>推荐使用标准模式。在积极模式中，openpilot 会更靠近前车并在加速和刹车方面更积极。在舒适模式中，openpilot 会与前车保持较远的距离。</translation>
     </message>
     <message>
         <source>An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches.</source>
-        <translation type="unfinished"></translation>
+        <translation>在正式（release）版本以外的分支上，可以测试 openpilot 纵向控制的 Alpha 版本以及实验模式。</translation>
     </message>
     <message>
         <source>Navigate on openpilot</source>
-        <translation type="unfinished"></translation>
+        <translation>Navigate on openpilot</translation>
     </message>
     <message>
         <source>Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode.</source>
-        <translation type="unfinished"></translation>
+        <translation>启用 openpilot 纵向控制（alpha）开关以允许实验模式。</translation>
     </message>
     <message>
         <source>End-to-End Longitudinal Control</source>
-        <translation type="unfinished"></translation>
+        <translation>端到端纵向控制</translation>
     </message>
     <message>
-        <source>When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits/forks.These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc.</source>
-        <translation type="unfinished"></translation>
+        <source>When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc.</source>
+        <translation>当导航有目的地时，openpilot 将输入地图信息到模型中。这为模型提供了有用的背景信息，使 openpilot 能够在叉路/出口时适当地保持左侧或右侧行驶。车道变换行为保持不变，仍由驾驶员激活。这是一个 Alpha 版的功能；可能会出现错误，特别是在出口和分叉处。这些错误可能包括意外的车道越界、晚出口、朝着分隔栏驶向安全地带等。</translation>
     </message>
     <message>
-        <source>The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner.When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green.</source>
-        <translation type="unfinished"></translation>
+        <source>The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green.</source>
+        <translation>行驶画面将在低速时切换到道路朝向的广角摄像头，以更好地显示一些转弯。实验模式标志也将显示在右上角。当设置了导航目的地并且驾驶模型正在使用它作为输入时，地图上的驾驶路径将变为绿色。</translation>
     </message>
 </context>
 <context>
@@ -1175,23 +1143,23 @@ This may take up to a minute.</source>
     <name>WiFiPromptWidget</name>
     <message>
         <source>Setup Wi-Fi</source>
-        <translation type="unfinished"></translation>
+        <translation>设置 Wi-Fi 连接</translation>
     </message>
     <message>
         <source>Connect to Wi-Fi to upload driving data and help improve openpilot</source>
-        <translation type="unfinished"></translation>
+        <translation>请连接至 Wi-Fi 上传驾驶数据以协助改进openpilot</translation>
     </message>
     <message>
         <source>Open Settings</source>
-        <translation type="unfinished"></translation>
+        <translation>打开设置</translation>
     </message>
     <message>
         <source>Ready to upload</source>
-        <translation type="unfinished"></translation>
+        <translation>准备好上传</translation>
     </message>
     <message>
         <source>Training data will be pulled periodically while your device is on Wi-Fi</source>
-        <translation type="unfinished"></translation>
+        <translation>训练数据将定期通过 Wi-Fi 上载</translation>
     </message>
 </context>
 <context>
diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts
index aa63ee7026..1794cc78ac 100644
--- a/selfdrive/ui/translations/main_zh-CHT.ts
+++ b/selfdrive/ui/translations/main_zh-CHT.ts
@@ -171,15 +171,15 @@
     </message>
     <message>
         <source>Reset Calibration</source>
-        <translation>重置校準</translation>
+        <translation>重設校準</translation>
     </message>
     <message>
         <source>RESET</source>
-        <translation>重置</translation>
+        <translation>重設</translation>
     </message>
     <message>
         <source>Are you sure you want to reset calibration?</source>
-        <translation>您確定要重置校準嗎？</translation>
+        <translation>您確定要重設校準嗎？</translation>
     </message>
     <message>
         <source>Review Training Guide</source>
@@ -226,8 +226,8 @@
         <translation>關機</translation>
     </message>
     <message>
-        <source>openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required.</source>
-        <translation>openpilot 需要將設備固定在左右偏差 4° 以內，朝上偏差 5° 以内或朝下偏差 8° 以内。鏡頭在後台會持續自動校準，很少有需要重置的情况。</translation>
+        <source>openpilot requires the device to be mounted within 4° left or right and within 5° up or 9° down. openpilot is continuously calibrating, resetting is rarely required.</source>
+        <translation>openpilot 需要將設備固定在左右偏差 4° 以內，朝上偏差 5° 以內或朝下偏差 9° 以內。鏡頭在後台會持續自動校準，很少有需要重設的情況。</translation>
     </message>
     <message>
         <source> Your device is pointed %1° %2 and %3° %4.</source>
@@ -274,33 +274,6 @@
         <translation>回顧</translation>
     </message>
 </context>
-<context>
-    <name>DriveStats</name>
-    <message>
-        <source>Drives</source>
-        <translation>旅程</translation>
-    </message>
-    <message>
-        <source>Hours</source>
-        <translation>小時</translation>
-    </message>
-    <message>
-        <source>ALL TIME</source>
-        <translation>總共</translation>
-    </message>
-    <message>
-        <source>PAST WEEK</source>
-        <translation>上周</translation>
-    </message>
-    <message>
-        <source>KM</source>
-        <translation>公里</translation>
-    </message>
-    <message>
-        <source>Miles</source>
-        <translation>英里</translation>
-    </message>
-</context>
 <context>
     <name>DriverViewScene</name>
     <message>
@@ -353,33 +326,6 @@
         <source>hr</source>
         <translation>小時</translation>
     </message>
-    <message>
-        <source>km</source>
-        <translation>km</translation>
-    </message>
-    <message>
-        <source>mi</source>
-        <translation>mi</translation>
-    </message>
-</context>
-<context>
-    <name>MapInstructions</name>
-    <message>
-        <source> km</source>
-        <translation> km</translation>
-    </message>
-    <message>
-        <source> m</source>
-        <translation> m</translation>
-    </message>
-    <message>
-        <source> mi</source>
-        <translation> mi</translation>
-    </message>
-    <message>
-        <source> ft</source>
-        <translation> ft</translation>
-    </message>
 </context>
 <context>
     <name>MapSettings</name>
@@ -389,14 +335,14 @@
     </message>
     <message>
         <source>Manage at connect.comma.ai</source>
-        <translation>請在 connect.comma.ai 上進行管理</translation>
+        <translation>請在 connect.comma.ai 上管理</translation>
     </message>
 </context>
 <context>
     <name>MapWindow</name>
     <message>
         <source>Map Loading</source>
-        <translation>地圖加載中</translation>
+        <translation>地圖載入中</translation>
     </message>
     <message>
         <source>Waiting for GPS</source>
@@ -404,7 +350,7 @@
     </message>
     <message>
         <source>Waiting for route</source>
-        <translation type="unfinished"></translation>
+        <translation>等待路線</translation>
     </message>
 </context>
 <context>
@@ -479,15 +425,15 @@
     </message>
     <message>
         <source>openpilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please submit a pull request to add the firmware versions to the proper vehicle. Need help? Join discord.comma.ai.</source>
-        <translation>openpilot 無法識別您的車輛。您的車輛可能未被支援，或是其電控單元 (ECU) 未被識別。請提交一個 Pull Request 為您的車輛添加正確的固件版本。需要幫助嗎？請加入 discord.comma.ai 。</translation>
+        <translation>openpilot 無法識別您的車輛。您的車輛可能未被支援，或是其電控單元 (ECU) 未被識別。請提交一個 Pull Request 為您的車輛添加正確的韌體版本。需要幫助嗎？請加入 discord.comma.ai 。</translation>
     </message>
     <message>
         <source>openpilot was unable to identify your car. Check integrity of cables and ensure all connections are secure, particularly that the comma power is fully inserted in the OBD-II port of the vehicle. Need help? Join discord.comma.ai.</source>
-        <translation>openpilot 無法識別您的車輛。請檢查線路是否正確的安裝並確保所有的連接都牢固，特別是確保 comma power 完全插入車輛的 OBD-II 接口。需要幫助嗎？請加入 discord.comma.ai 。</translation>
+        <translation>openpilot 無法識別您的車輛。請檢查線路是否正確的安裝並確保所有的連接都牢固，特別是確保 comma power 完全插入車輛的 OBD-II 介面。需要幫助嗎？請加入 discord.comma.ai 。</translation>
     </message>
     <message>
         <source>openpilot detected a change in the device&apos;s mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.</source>
-        <translation>openpilot偵測到設備的安裝位置發生變化。請確保設備完全安裝在支架上，並確保支架牢固地固定在擋風玻璃上。</translation>
+        <translation>openpilot 偵測到設備的安裝位置發生變化。請確保設備完全安裝在支架上，並確保支架牢固地固定在擋風玻璃上。</translation>
     </message>
     <message>
         <source>Device temperature too high. System cooling down before starting. Current internal component temperature: %1</source>
@@ -525,7 +471,7 @@
     </message>
     <message>
         <source>Bookmark connect.comma.ai to your home screen to use it like an app</source>
-        <translation>將 connect.comma.ai 加入您的主屏幕，以便像手機 App 一樣使用它</translation>
+        <translation>將 connect.comma.ai 加入您的主螢幕，以便像手機 App 一樣使用它</translation>
     </message>
 </context>
 <context>
@@ -555,7 +501,7 @@
     </message>
     <message>
         <source>Remote access</source>
-        <translation>遠程訪問</translation>
+        <translation>遠端存取</translation>
     </message>
     <message>
         <source>24/7 LTE connectivity</source>
@@ -617,20 +563,36 @@
             <numerusform>%n 天前</numerusform>
         </translation>
     </message>
+    <message>
+        <source>km</source>
+        <translation>km</translation>
+    </message>
+    <message>
+        <source>m</source>
+        <translation>m</translation>
+    </message>
+    <message>
+        <source>mi</source>
+        <translation>mi</translation>
+    </message>
+    <message>
+        <source>ft</source>
+        <translation>ft</translation>
+    </message>
 </context>
 <context>
     <name>Reset</name>
     <message>
         <source>Reset failed. Reboot to try again.</source>
-        <translation>重置失敗。請重新啟動後再試。</translation>
+        <translation>重設失敗。請重新啟動後再試。</translation>
     </message>
     <message>
         <source>Are you sure you want to reset your device?</source>
-        <translation>您確定要重置你的設備嗎？</translation>
+        <translation>您確定要重設你的設備嗎？</translation>
     </message>
     <message>
         <source>System Reset</source>
-        <translation>系統重置</translation>
+        <translation>系統重設</translation>
     </message>
     <message>
         <source>Cancel</source>
@@ -655,7 +617,7 @@
     <message>
         <source>Resetting device...
 This may take up to a minute.</source>
-        <translation>設備重置中…
+        <translation>設備重設中…
 這可能需要一分鐘的時間。</translation>
     </message>
 </context>
@@ -710,7 +672,7 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Connect to Wi-Fi</source>
-        <translation>連接到無線網絡</translation>
+        <translation>連接到無線網路</translation>
     </message>
     <message>
         <source>Back</source>
@@ -730,7 +692,7 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>for Custom Software</source>
-        <translation>定制的軟體</translation>
+        <translation>訂製的軟體</translation>
     </message>
     <message>
         <source>Downloading...</source>
@@ -760,6 +722,10 @@ This may take up to a minute.</source>
         <source>Something went wrong. Reboot the device.</source>
         <translation>發生了一些錯誤。請重新啟動您的設備。</translation>
     </message>
+    <message>
+        <source>Select a language</source>
+        <translation>選擇語言</translation>
+    </message>
 </context>
 <context>
     <name>SetupWidget</name>
@@ -836,27 +802,27 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Wi-Fi</source>
-        <translation></translation>
+        <translation>Wi-Fi</translation>
     </message>
     <message>
         <source>ETH</source>
-        <translation></translation>
+        <translation>ETH</translation>
     </message>
     <message>
         <source>2G</source>
-        <translation></translation>
+        <translation>2G</translation>
     </message>
     <message>
         <source>3G</source>
-        <translation></translation>
+        <translation>3G</translation>
     </message>
     <message>
         <source>LTE</source>
-        <translation></translation>
+        <translation>LTE</translation>
     </message>
     <message>
         <source>5G</source>
-        <translation></translation>
+        <translation>5G</translation>
     </message>
 </context>
 <context>
@@ -938,11 +904,11 @@ This may take up to a minute.</source>
     <name>SshControl</name>
     <message>
         <source>SSH Keys</source>
-        <translation>SSH 密鑰</translation>
+        <translation>SSH 金鑰</translation>
     </message>
     <message>
         <source>Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username.</source>
-        <translation>警告：這將授權給 GitHub 帳號中所有公鑰 SSH 訪問權限。切勿輸入非您自己的 GitHub 用戶名。comma 員工「永遠不會」要求您添加他們的 GitHub 用戶名。</translation>
+        <translation>警告：這將授權給 GitHub 帳號中所有公鑰 SSH 訪問權限。切勿輸入非您自己的 GitHub 使用者名稱。comma 員工「永遠不會」要求您添加他們的 GitHub 使用者名稱。</translation>
     </message>
     <message>
         <source>ADD</source>
@@ -950,7 +916,7 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Enter your GitHub username</source>
-        <translation>請輸入您 GitHub 的用戶名</translation>
+        <translation>請輸入您 GitHub 的使用者名稱</translation>
     </message>
     <message>
         <source>LOADING</source>
@@ -962,7 +928,7 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Username &apos;%1&apos; has no keys on GitHub</source>
-        <translation>GitHub 用戶 &apos;%1&apos; 沒有設定任何密鑰</translation>
+        <translation>GitHub 用戶 &apos;%1&apos; 沒有設定任何金鑰</translation>
     </message>
     <message>
         <source>Request timed out</source>
@@ -1031,7 +997,7 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Upload data from the driver facing camera and help improve the driver monitoring algorithm.</source>
-        <translation>上傳駕駛監控的錄像來協助我們提升駕駛監控的準確率。</translation>
+        <translation>上傳駕駛監控的錄影來協助我們提升駕駛監控的準確率。</translation>
     </message>
     <message>
         <source>Disengage on Accelerator Pedal</source>
@@ -1087,11 +1053,11 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB).</source>
-        <translation>警告：此車輛的 Openpilot 縱向控制功能目前處於 Alpha 版本，使用此功能將會停用自動緊急制動（AEB）功能。</translation>
+        <translation>警告：此車輛的 openpilot 縱向控制功能目前處於 Alpha 版本，使用此功能將會停用自動緊急煞車（AEB）功能。</translation>
     </message>
     <message>
         <source>On this car, openpilot defaults to the car&apos;s built-in ACC instead of openpilot&apos;s longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha.</source>
-        <translation>在這輛車上，Openpilot 預設使用車輛內建的主動巡航控制（ACC），而非 Openpilot 的縱向控制。啟用此項功能可切換至 Openpilot 的縱向控制。當啟用 Openpilot 縱向控制 Alpha 版本時，建議同時啟用實驗性模式（Experimental mode）。</translation>
+        <translation>在這輛車上，openpilot 預設使用車輛內建的主動巡航控制（ACC），而非 openpilot 的縱向控制。啟用此項功能可切換至 openpilot 的縱向控制。當啟用 openpilot 縱向控制 Alpha 版本時，建議同時啟用實驗性模式（Experimental mode）。</translation>
     </message>
     <message>
         <source>Aggressive</source>
@@ -1115,27 +1081,27 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches.</source>
-        <translation>在正式 (release) 版以外的分支上可以測試 openpilot 縱向控制的 Alpha 版本，以及實驗模式。</translation>
+        <translation>在正式 (release) 版以外的分支上可以測試 openpilot 縱向控制的 Alpha 版本以及實驗模式。</translation>
     </message>
     <message>
         <source>Navigate on openpilot</source>
-        <translation type="unfinished"></translation>
+        <translation>Navigate on openpilot</translation>
     </message>
     <message>
         <source>Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode.</source>
-        <translation type="unfinished"></translation>
+        <translation>啟用 openpilot 縱向控制（alpha）切換以允許實驗模式。</translation>
     </message>
     <message>
         <source>End-to-End Longitudinal Control</source>
-        <translation type="unfinished"></translation>
+        <translation>端到端縱向控制</translation>
     </message>
     <message>
-        <source>When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits/forks.These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc.</source>
-        <translation type="unfinished"></translation>
+        <source>When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc.</source>
+        <translation>當導航有目的地時，openpilot 將把地圖資訊輸入模型中。這為模型提供了有用的背景資訊，使 openpilot 能夠在叉路/出口時適當地保持左側或右側行駛。車道變換行為保持不變，仍由駕駛員啟用。這是一個 Alpha 版的功能；可能會出現錯誤，特別是在出口和分叉處。這些錯誤可能包括意外的車道越界、晚出口、朝著分隔欄駛向分隔帶區域等。</translation>
     </message>
     <message>
-        <source>The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner.When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green.</source>
-        <translation type="unfinished"></translation>
+        <source>The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green.</source>
+        <translation>行駛畫面將在低速時切換至道路朝向的廣角鏡頭，以更好地顯示一些轉彎。實驗模式圖示也將顯示在右上角。當設定了導航目的地並且行駛模型正在將其作為輸入時，地圖上的行駛路徑將變為綠色。</translation>
     </message>
 </context>
 <context>
@@ -1150,7 +1116,7 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Connect to Wi-Fi</source>
-        <translation>連接到無線網絡</translation>
+        <translation>連接到無線網路</translation>
     </message>
     <message>
         <source>Install</source>
@@ -1181,7 +1147,7 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Connect to Wi-Fi to upload driving data and help improve openpilot</source>
-        <translation>請連接至 Wi-Fi 以上傳駕駛數據，並協助改進 openpilot</translation>
+        <translation>請連接至 Wi-Fi 傳駕駛數據以協助改進 openpilot</translation>
     </message>
     <message>
         <source>Open Settings</source>
@@ -1189,11 +1155,11 @@ This may take up to a minute.</source>
     </message>
     <message>
         <source>Ready to upload</source>
-        <translation type="unfinished"></translation>
+        <translation>準備好上傳</translation>
     </message>
     <message>
         <source>Training data will be pulled periodically while your device is on Wi-Fi</source>
-        <translation type="unfinished"></translation>
+        <translation>訓練數據將定期經過 Wi-Fi 上傳</translation>
     </message>
 </context>
 <context>
diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui
index c9f81c0539..acb2a705a8 100755
--- a/selfdrive/ui/ui
+++ b/selfdrive/ui/ui
@@ -1,5 +1,4 @@
 #!/bin/sh
 cd "$(dirname "$0")"
-export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH"
 export QT_DBL_CLICK_DIST=150
 exec ./_ui
diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
index 95be0e3ed9..03df2cf57a 100644
--- a/selfdrive/ui/ui.cc
+++ b/selfdrive/ui/ui.cc
@@ -1,5 +1,6 @@
 #include "selfdrive/ui/ui.h"
 
+#include <algorithm>
 #include <cassert>
 #include <cmath>
 
@@ -168,15 +169,15 @@ static void update_state(UIState *s) {
     Eigen::Matrix3d device_from_calib = euler2rot(rpy);
     Eigen::Matrix3d wide_from_device = euler2rot(wfde);
     Eigen::Matrix3d view_from_device;
-    view_from_device << 0,1,0,
-                        0,0,1,
-                        1,0,0;
+    view_from_device << 0, 1, 0,
+                        0, 0, 1,
+                        1, 0, 0;
     Eigen::Matrix3d view_from_calib = view_from_device * device_from_calib;
-    Eigen::Matrix3d view_from_wide_calib = view_from_device * wide_from_device * device_from_calib ;
+    Eigen::Matrix3d view_from_wide_calib = view_from_device * wide_from_device * device_from_calib;
     for (int i = 0; i < 3; i++) {
       for (int j = 0; j < 3; j++) {
-        scene.view_from_calib.v[i*3 + j] = view_from_calib(i,j);
-        scene.view_from_wide_calib.v[i*3 + j] = view_from_wide_calib(i,j);
+        scene.view_from_calib.v[i*3 + j] = view_from_calib(i, j);
+        scene.view_from_wide_calib.v[i*3 + j] = view_from_wide_calib(i, j);
       }
     }
     scene.calibration_valid = live_calib.getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED;
@@ -244,8 +245,11 @@ UIState::UIState(QObject *parent) : QObject(parent) {
   });
 
   Params params;
-  prime_type = std::atoi(params.get("PrimeType").c_str());
   language = QString::fromStdString(params.get("LanguageSetting"));
+  auto prime_value = params.get("PrimeType");
+  if (!prime_value.empty()) {
+    prime_type = static_cast<PrimeType>(std::atoi(prime_value.c_str()));
+  }
 
   // update timer
   timer = new QTimer(this);
@@ -264,11 +268,18 @@ void UIState::update() {
   emit uiUpdate(*this);
 }
 
-void UIState::setPrimeType(int type) {
+void UIState::setPrimeType(PrimeType type) {
   if (type != prime_type) {
+    bool prev_prime = hasPrime();
+
     prime_type = type;
     Params().put("PrimeType", std::to_string(prime_type));
     emit primeTypeChanged(prime_type);
+
+    bool prime = hasPrime();
+    if (prev_prime != prime) {
+      emit primeChanged(prime);
+    }
   }
 }
 
@@ -293,8 +304,11 @@ void Device::setAwake(bool on) {
   }
 }
 
-void Device::resetInteractiveTimeout() {
-  interactive_timeout = (ignition_on ? 10 : 30) * UI_FREQ;
+void Device::resetInteractiveTimeout(int timeout) {
+  if (timeout == -1) {
+    timeout = (ignition_on ? 10 : 30);
+  }
+  interactive_timeout = timeout * UI_FREQ;
 }
 
 void Device::updateBrightness(const UIState &s) {
diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h
index 572065d961..dc180eebfb 100644
--- a/selfdrive/ui/ui.h
+++ b/selfdrive/ui/ui.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <map>
 #include <memory>
 #include <string>
 #include <optional>
@@ -25,7 +26,7 @@ typedef cereal::CarControl::HUDControl::AudibleAlert AudibleAlert;
 
 const mat3 DEFAULT_CALIBRATION = {{ 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0 }};
 
-const vec3 default_face_kpts_3d[] = {
+constexpr vec3 default_face_kpts_3d[] = {
   {-5.98, -51.20, 8.00}, {-17.64, -49.14, 8.00}, {-23.81, -46.40, 8.00}, {-29.98, -40.91, 8.00}, {-32.04, -37.49, 8.00},
   {-34.10, -32.00, 8.00}, {-36.16, -21.03, 8.00}, {-36.16, 6.40, 8.00}, {-35.47, 10.51, 8.00}, {-32.73, 19.43, 8.00},
   {-29.30, 26.29, 8.00}, {-24.50, 33.83, 8.00}, {-19.01, 41.37, 8.00}, {-14.21, 46.17, 8.00}, {-12.16, 47.54, 8.00},
@@ -95,6 +96,16 @@ typedef enum UIStatus {
   STATUS_ENGAGED,
 } UIStatus;
 
+enum PrimeType {
+  UNKNOWN = -1,
+  NONE = 0,
+  MAGENTA = 1,
+  LITE = 2,
+  BLUE = 3,
+  MAGENTA_NEW = 4,
+  PURPLE = 5,
+};
+
 const QColor bg_colors [] = {
   [STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8),
   [STATUS_OVERRIDE] = QColor(0x91, 0x9b, 0x95, 0xf1),
@@ -152,8 +163,9 @@ public:
     return scene.started && (*sm)["controlsState"].getControlsState().getEnabled();
   }
 
-  void setPrimeType(int type);
-  inline int primeType() const { return prime_type; }
+  void setPrimeType(PrimeType type);
+  inline PrimeType primeType() const { return prime_type; }
+  inline bool hasPrime() const { return prime_type != PrimeType::UNKNOWN && prime_type != PrimeType::NONE; }
 
   int fb_w = 0, fb_h = 0;
 
@@ -169,7 +181,8 @@ public:
 signals:
   void uiUpdate(const UIState &s);
   void offroadTransition(bool offroad);
-  void primeTypeChanged(int prime_type);
+  void primeChanged(bool prime);
+  void primeTypeChanged(PrimeType prime_type);
 
 private slots:
   void update();
@@ -177,7 +190,7 @@ private slots:
 private:
   QTimer *timer;
   bool started_prev = false;
-  int prime_type = -1;
+  PrimeType prime_type = PrimeType::UNKNOWN;
 };
 
 UIState *uiState();
@@ -212,7 +225,7 @@ signals:
   void interactiveTimeout();
 
 public slots:
-  void resetInteractiveTimeout();
+  void resetInteractiveTimeout(int timeout = -1);
   void update(const UIState &s);
 };
 
diff --git a/selfdrive/ui/update_translations.py b/selfdrive/ui/update_translations.py
index 3de6e0f27c..004c6425dc 100755
--- a/selfdrive/ui/update_translations.py
+++ b/selfdrive/ui/update_translations.py
@@ -3,7 +3,7 @@ import argparse
 import json
 import os
 
-from common.basedir import BASEDIR
+from openpilot.common.basedir import BASEDIR
 
 UI_DIR = os.path.join(BASEDIR, "selfdrive", "ui")
 TRANSLATIONS_DIR = os.path.join(UI_DIR, "translations")
@@ -13,7 +13,7 @@ TRANSLATIONS_INCLUDE_FILE = os.path.join(TRANSLATIONS_DIR, "alerts_generated.h")
 
 def generate_translations_include():
   # offroad alerts
-  # TODO translate events from selfdrive/controls/lib/events.py
+  # TODO translate events from openpilot.selfdrive/controls/lib/events.py
   content = "// THIS IS AN AUTOGENERATED FILE, PLEASE EDIT alerts_offroad.json\n"
   with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f:
     for alert in json.load(f).values():
@@ -46,7 +46,8 @@ if __name__ == "__main__":
   parser = argparse.ArgumentParser(description="Update translation files for UI",
                                    formatter_class=argparse.ArgumentDefaultsHelpFormatter)
   parser.add_argument("--vanish", action="store_true", help="Remove translations with source text no longer found")
-  parser.add_argument("--plural-only", type=str, nargs="*", default=["main_en"], help="Translation codes to only create plural translations for (ie. the base language)")
+  parser.add_argument("--plural-only", type=str, nargs="*", default=["main_en"],
+                      help="Translation codes to only create plural translations for (ie. the base language)")
   args = parser.parse_args()
 
   update_translations(args.vanish, args.plural_only)
diff --git a/selfdrive/updated.py b/selfdrive/updated.py
index 24df5914af..1e128fb11c 100755
--- a/selfdrive/updated.py
+++ b/selfdrive/updated.py
@@ -14,13 +14,13 @@ from pathlib import Path
 from typing import List, Union, Optional
 from markdown_it import MarkdownIt
 
-from common.basedir import BASEDIR
-from common.params import Params
-from common.time import system_time_valid
-from system.hardware import AGNOS, HARDWARE
-from system.swaglog import cloudlog
-from selfdrive.controls.lib.alertmanager import set_offroad_alert
-from system.version import is_tested_branch
+from openpilot.common.basedir import BASEDIR
+from openpilot.common.params import Params
+from openpilot.common.time import system_time_valid
+from openpilot.system.hardware import AGNOS, HARDWARE
+from openpilot.system.swaglog import cloudlog
+from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
+from openpilot.system.version import is_tested_branch
 
 LOCK_FILE = os.getenv("UPDATER_LOCK_FILE", "/tmp/safe_staging_overlay.lock")
 STAGING_ROOT = os.getenv("UPDATER_STAGING_ROOT", "/data/safe_staging")
@@ -192,7 +192,7 @@ def finalize_update() -> None:
 
 
 def handle_agnos_update() -> None:
-  from system.hardware.tici.agnos import flash_agnos_update, get_target_slot_number
+  from openpilot.system.hardware.tici.agnos import flash_agnos_update, get_target_slot_number
 
   cur_version = HARDWARE.get_os_version()
   updated_version = run(["bash", "-c", r"unset AGNOS_VERSION && source launch_env.sh && \
@@ -444,7 +444,7 @@ def main() -> None:
       if not system_time_valid():
         wait_helper.sleep(60)
         continue
-        
+
       update_failed_count += 1
 
       # check for update
diff --git a/system/camerad/SConscript b/system/camerad/SConscript
index 3ecc3f6d72..ffd7278bbb 100644
--- a/system/camerad/SConscript
+++ b/system/camerad/SConscript
@@ -8,7 +8,7 @@ env.Program('camerad', [
     camera_obj,
   ], LIBS=libs)
 
-if GetOption("test") and arch == "x86_64":
+if GetOption("extras") and arch == "x86_64":
   env.Program('test/ae_gray_test',
               ['test/ae_gray_test.cc', camera_obj],
               LIBS=libs)
diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc
index 1b2594bc80..23b28357da 100644
--- a/system/camerad/cameras/camera_common.cc
+++ b/system/camerad/cameras/camera_common.cc
@@ -5,9 +5,10 @@
 #include <cassert>
 #include <cstdio>
 #include <chrono>
+#include <string>
 #include <thread>
 
-#include "libyuv.h"
+#include "third_party/libyuv/include/libyuv.h"
 #include <jpeglib.h>
 
 #include "system/camerad/imgproc/utils.h"
@@ -16,7 +17,7 @@
 #include "common/swaglog.h"
 #include "common/util.h"
 #include "system/hardware/hw.h"
-#include "msm_media_info.h"
+#include "third_party/linux/include/msm_media_info.h"
 
 #include "system/camerad/cameras/camera_qcom2.h"
 #ifdef QCOM2
diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h
index 3e56f5690d..0f69aa3774 100644
--- a/system/camerad/cameras/camera_common.h
+++ b/system/camerad/cameras/camera_common.h
@@ -26,7 +26,7 @@
 #define CAMERA_ID_OX03C10 9
 #define CAMERA_ID_MAX 10
 
-const int YUV_BUFFER_COUNT = 40;
+const int YUV_BUFFER_COUNT = 20;
 
 enum CameraType {
   RoadCam = 0,
diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc
index 74ac77482e..3a9ecb467b 100644
--- a/system/camerad/cameras/camera_qcom2.cc
+++ b/system/camerad/cameras/camera_qcom2.cc
@@ -6,12 +6,15 @@
 #include <sys/mman.h>
 #include <unistd.h>
 
+#include <algorithm>
 #include <atomic>
 #include <cassert>
 #include <cerrno>
 #include <cmath>
 #include <cstdio>
 #include <cstring>
+#include <string>
+#include <vector>
 
 #include "media/cam_defs.h"
 #include "media/cam_isp.h"
@@ -143,7 +146,7 @@ void CameraState::sensors_start() {
 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 *)mm.alloc(size, &cam_packet_handle);
+  auto pkt = mm.alloc<struct cam_packet>(size, &cam_packet_handle);
   pkt->num_cmd_buf = 0;
   pkt->kmd_cmd_buf_index = -1;
   pkt->header.size = size;
@@ -156,15 +159,13 @@ void CameraState::sensors_poke(int request_id) {
     enabled = false;
     return;
   }
-
-  mm.free(pkt);
 }
 
 void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word) {
   // 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 *)mm.alloc(size, &cam_packet_handle);
+  auto pkt = mm.alloc<struct cam_packet>(size, &cam_packet_handle);
   pkt->num_cmd_buf = 1;
   pkt->kmd_cmd_buf_index = -1;
   pkt->header.size = size;
@@ -174,7 +175,7 @@ void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op
   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 *)mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
+  auto i2c_random_wr = mm.alloc<struct cam_cmd_i2c_random_wr>(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;
@@ -188,9 +189,6 @@ void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op
     enabled = false;
     return;
   }
-
-  mm.free(i2c_random_wr);
-  mm.free(pkt);
 }
 
 static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) {
@@ -199,12 +197,12 @@ static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) {
   unconditional_wait->delay = delay_ms;
   unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND;
   return (struct cam_cmd_power *)(unconditional_wait + 1);
-};
+}
 
 int CameraState::sensors_init() {
   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 *)mm.alloc(size, &cam_packet_handle);
+  auto pkt = mm.alloc<struct cam_packet>(size, &cam_packet_handle);
   pkt->num_cmd_buf = 2;
   pkt->kmd_cmd_buf_index = -1;
   pkt->header.op_code = 0x1000000 | CAM_SENSOR_PACKET_OPCODE_SENSOR_PROBE;
@@ -213,8 +211,8 @@ int CameraState::sensors_init() {
 
   buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_cmd_i2c_info) + sizeof(struct cam_cmd_probe);
   buf_desc[0].type = CAM_CMD_BUF_LEGACY;
-  struct cam_cmd_i2c_info *i2c_info = (struct cam_cmd_i2c_info *)mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
-  auto probe = (struct cam_cmd_probe *)(i2c_info + 1);
+  auto i2c_info = mm.alloc<struct cam_cmd_i2c_info>(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
+  auto probe = (struct cam_cmd_probe *)(i2c_info.get() + 1);
 
   probe->camera_id = camera_num;
   switch (camera_num) {
@@ -255,11 +253,11 @@ int CameraState::sensors_init() {
   //buf_desc[1].size = buf_desc[1].length = 148;
   buf_desc[1].size = buf_desc[1].length = 196;
   buf_desc[1].type = CAM_CMD_BUF_I2C;
-  struct cam_cmd_power *power_settings = (struct cam_cmd_power *)mm.alloc(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle);
-  memset(power_settings, 0, buf_desc[1].size);
+  auto power_settings = mm.alloc<struct cam_cmd_power>(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle);
+  memset(power_settings.get(), 0, buf_desc[1].size);
 
   // power on
-  struct cam_cmd_power *power = power_settings;
+  struct cam_cmd_power *power = power_settings.get();
   power->count = 4;
   power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP;
   power->power_settings[0].power_seq_type = 3; // clock??
@@ -315,11 +313,6 @@ int CameraState::sensors_init() {
 
   int ret = do_cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0);
   LOGD("probing the sensor: %d", ret);
-
-  mm.free(i2c_info);
-  mm.free(power_settings);
-  mm.free(pkt);
-
   return ret;
 }
 
@@ -329,7 +322,7 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b
   if (io_mem_handle != 0) {
     size += sizeof(struct cam_buf_io_cfg);
   }
-  struct cam_packet *pkt = (struct cam_packet *)mm.alloc(size, &cam_packet_handle);
+  auto pkt = mm.alloc<struct cam_packet>(size, &cam_packet_handle);
   pkt->num_cmd_buf = 2;
   pkt->kmd_cmd_buf_index = 0;
   // YUV has kmd_cmd_buf_offset = 1780
@@ -424,25 +417,25 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b
   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 *)mm.alloc(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle);
-  memcpy(buf2, &tmp, sizeof(tmp));
+  auto buf2 = mm.alloc<uint32_t>(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle);
+  memcpy(buf2.get(), &tmp, sizeof(tmp));
 
   if (io_mem_handle != 0) {
     io_cfg[0].mem_handle[0] = io_mem_handle;
-		io_cfg[0].planes[0] = (struct cam_plane_cfg){
-		 .width = ci.frame_width,
-		 .height = ci.frame_height + ci.extra_height,
-		 .plane_stride = ci.frame_stride,
-		 .slice_height = ci.frame_height + ci.extra_height,
-		 .meta_stride = 0x0,    // YUV has meta(stride=0x400, size=0x5000)
-		 .meta_size = 0x0,
-		 .meta_offset = 0x0,
-		 .packer_config = 0x0,  // 0xb for YUV
-		 .mode_config = 0x0,    // 0x9ef for YUV
-		 .tile_config = 0x0,
-		 .h_init = 0x0,
-		 .v_init = 0x0,
-		};
+    io_cfg[0].planes[0] = (struct cam_plane_cfg){
+      .width = ci.frame_width,
+      .height = ci.frame_height + ci.extra_height,
+      .plane_stride = ci.frame_stride,
+      .slice_height = ci.frame_height + ci.extra_height,
+      .meta_stride = 0x0,  // YUV has meta(stride=0x400, size=0x5000)
+      .meta_size = 0x0,
+      .meta_offset = 0x0,
+      .packer_config = 0x0,  // 0xb for YUV
+      .mode_config = 0x0,    // 0x9ef for YUV
+      .tile_config = 0x0,
+      .h_init = 0x0,
+      .v_init = 0x0,
+    };
     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
@@ -459,9 +452,6 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b
   if (ret != 0) {
     LOGE("isp config failed");
   }
-
-  mm.free(buf2);
-  mm.free(pkt);
 }
 
 void CameraState::enqueue_buffer(int i, bool dp) {
@@ -518,7 +508,7 @@ void CameraState::enqueue_buffer(int i, bool dp) {
 }
 
 void CameraState::enqueue_req_multi(int start, int n, bool dp) {
-  for (int i=start;i<start+n;++i) {
+  for (int i=start; i<start+n; ++i) {
     request_ids[(i - 1) % FRAME_BUF_COUNT] = i;
     enqueue_buffer((i - 1) % FRAME_BUF_COUNT, dp);
   }
@@ -738,7 +728,8 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num
   LOGD("acquire csiphy dev");
 
   // config ISP
-  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);
+  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);
 
   // config csiphy
@@ -746,7 +737,7 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num
   {
     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 *)mm.alloc(size, &cam_packet_handle);
+    auto pkt = mm.alloc<struct cam_packet>(size, &cam_packet_handle);
     pkt->num_cmd_buf = 1;
     pkt->kmd_cmd_buf_index = -1;
     pkt->header.size = size;
@@ -755,7 +746,7 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num
     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 *)mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
+    auto csiphy_info = mm.alloc<struct cam_csiphy_info>(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
@@ -767,9 +758,6 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num
 
     int ret_ = device_config(csiphy_fd, session_handle, csiphy_dev_handle, cam_packet_handle);
     assert(ret_ == 0);
-
-    mm.free(csiphy_info);
-    mm.free(pkt);
   }
 
   // link devices
@@ -1289,9 +1277,9 @@ void cameras_run(MultiCameraState *s) {
     if (ret == 0) {
       if (ev.type == V4L_EVENT_CAM_REQ_MGR_EVENT) {
         struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)ev.u.data;
-        // LOGD("v4l2 event: sess_hdl 0x%X, link_hdl 0x%X, frame_id %d, req_id %lld, timestamp 0x%llx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status);
         if (env_debug_frames) {
-          printf("sess_hdl 0x%6X, link_hdl 0x%6X, frame_id %lu, req_id %lu, timestamp %.2f ms, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp/1e6, event_data->u.frame_msg.sof_status);
+          printf("sess_hdl 0x%6X, link_hdl 0x%6X, frame_id %lu, req_id %lu, timestamp %.2f ms, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl,
+                 event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp/1e6, event_data->u.frame_msg.sof_status);
         }
 
         if (event_data->session_hdl == s->road_cam.session_handle) {
diff --git a/system/camerad/cameras/camera_util.cc b/system/camerad/cameras/camera_util.cc
index 6d139590e4..74c81a878a 100644
--- a/system/camerad/cameras/camera_util.cc
+++ b/system/camerad/cameras/camera_util.cc
@@ -37,7 +37,7 @@ std::optional<int32_t> device_acquire(int fd, int32_t session_handle, void *data
   };
   int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd));
   return err == 0 ? std::make_optional(cmd.dev_handle) : std::nullopt;
-};
+}
 
 int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle) {
   struct cam_config_dev_cmd cmd = {
@@ -98,7 +98,7 @@ void release_fd(int video0_fd, uint32_t handle) {
   release(video0_fd, handle);
 }
 
-void *MemoryManager::alloc(int size, uint32_t *handle) {
+void *MemoryManager::alloc_buf(int size, uint32_t *handle) {
   lock.lock();
   void *ptr;
   if (!cached_allocations[size].empty()) {
diff --git a/system/camerad/cameras/camera_util.h b/system/camerad/cameras/camera_util.h
index e408f6c0e2..b36f404c0f 100644
--- a/system/camerad/cameras/camera_util.h
+++ b/system/camerad/cameras/camera_util.h
@@ -1,6 +1,8 @@
 #pragma once
 
+ #include <functional>
 #include <map>
+#include <memory>
 #include <mutex>
 #include <optional>
 #include <queue>
@@ -18,10 +20,17 @@ void release(int video0_fd, uint32_t handle);
 class MemoryManager {
   public:
     void init(int _video0_fd) { video0_fd = _video0_fd; }
-    void *alloc(int len, uint32_t *handle);
-    void free(void *ptr);
     ~MemoryManager();
+
+    template <class T>
+    auto alloc(int len, uint32_t *handle) {
+      return std::unique_ptr<T, std::function<void(void *)>>((T*)alloc_buf(len, handle), [this](void *ptr) { this->free(ptr); });
+    }
+
   private:
+    void *alloc_buf(int len, uint32_t *handle);
+    void free(void *ptr);
+
     std::mutex lock;
     std::map<void *, uint32_t> handle_lookup;
     std::map<void *, int> size_lookup;
diff --git a/system/camerad/snapshot/snapshot.py b/system/camerad/snapshot/snapshot.py
index 447062fb2e..ede97d1a79 100755
--- a/system/camerad/snapshot/snapshot.py
+++ b/system/camerad/snapshot/snapshot.py
@@ -7,11 +7,11 @@ from PIL import Image
 
 import cereal.messaging as messaging
 from cereal.visionipc import VisionIpcClient, VisionStreamType
-from common.params import Params
-from common.realtime import DT_MDL
-from system.hardware import PC
-from selfdrive.controls.lib.alertmanager import set_offroad_alert
-from selfdrive.manager.process_config import managed_processes
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.system.hardware import PC
+from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
+from openpilot.selfdrive.manager.process_config import managed_processes
 
 LM_THRESH = 120  # defined in system/camerad/imgproc/utils.h
 
diff --git a/system/clocksd/.gitignore b/system/clocksd/.gitignore
deleted file mode 100644
index a6d841d65e..0000000000
--- a/system/clocksd/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-clocksd
diff --git a/system/clocksd/SConscript b/system/clocksd/SConscript
deleted file mode 100644
index d1cf13e9e8..0000000000
--- a/system/clocksd/SConscript
+++ /dev/null
@@ -1,2 +0,0 @@
-Import('env', 'common', 'cereal', 'messaging')
-env.Program('clocksd.cc', LIBS=[common, cereal, messaging, 'capnp', 'zmq', 'kj'])
diff --git a/system/clocksd/clocksd.cc b/system/clocksd/clocksd.cc
deleted file mode 100644
index a5912cf51a..0000000000
--- a/system/clocksd/clocksd.cc
+++ /dev/null
@@ -1,73 +0,0 @@
-#include <sys/resource.h>
-#include <sys/time.h>
-#include <unistd.h>
-
-#include <cstdint>
-#include <cstdio>
-
-// Apple doesn't have timerfd
-#ifdef __APPLE__
-#include <thread>
-#else
-#include <sys/timerfd.h>
-#endif
-
-#include <cassert>
-#include <chrono>
-
-#include "cereal/messaging/messaging.h"
-#include "common/timing.h"
-#include "common/util.h"
-
-ExitHandler do_exit;
-
-int main() {
-  setpriority(PRIO_PROCESS, 0, -13);
-  PubMaster pm({"clocks"});
-
-#ifndef __APPLE__
-  int timerfd = timerfd_create(CLOCK_BOOTTIME, 0);
-  assert(timerfd >= 0);
-
-  struct itimerspec spec = {0};
-  spec.it_interval.tv_sec = 1;
-  spec.it_interval.tv_nsec = 0;
-  spec.it_value.tv_sec = 1;
-  spec.it_value.tv_nsec = 0;
-
-  int err = timerfd_settime(timerfd, 0, &spec, 0);
-  assert(err == 0);
-
-  uint64_t expirations = 0;
-  while (!do_exit && (err = read(timerfd, &expirations, sizeof(expirations)))) {
-    if (err < 0) {
-      if (errno == EINTR) continue;
-      break;
-    }
-#else
-  // Just run at 1Hz on apple
-  while (!do_exit) {
-    util::sleep_for(1000);
-#endif
-
-    uint64_t boottime = nanos_since_boot();
-    uint64_t monotonic = nanos_monotonic();
-    uint64_t monotonic_raw = nanos_monotonic_raw();
-    uint64_t wall_time = nanos_since_epoch();
-
-    MessageBuilder msg;
-    auto clocks = msg.initEvent().initClocks();
-
-    clocks.setBootTimeNanos(boottime);
-    clocks.setMonotonicNanos(monotonic);
-    clocks.setMonotonicRawNanos(monotonic_raw);
-    clocks.setWallTimeNanos(wall_time);
-
-    pm.send("clocks", msg);
-  }
-
-#ifndef __APPLE__
-  close(timerfd);
-#endif
-  return 0;
-}
diff --git a/system/hardware/__init__.py b/system/hardware/__init__.py
index 77bb0e5e2a..99079b5ef3 100644
--- a/system/hardware/__init__.py
+++ b/system/hardware/__init__.py
@@ -1,9 +1,9 @@
 import os
 from typing import cast
 
-from system.hardware.base import HardwareBase
-from system.hardware.tici.hardware import Tici
-from system.hardware.pc.hardware import Pc
+from openpilot.system.hardware.base import HardwareBase
+from openpilot.system.hardware.tici.hardware import Tici
+from openpilot.system.hardware.pc.hardware import Pc
 
 TICI = os.path.isfile('/TICI')
 AGNOS = os.path.isfile('/AGNOS')
diff --git a/system/hardware/base.h b/system/hardware/base.h
index 5460099723..43f5db023d 100644
--- a/system/hardware/base.h
+++ b/system/hardware/base.h
@@ -2,6 +2,9 @@
 
 #include <cstdlib>
 #include <fstream>
+#include <map>
+#include <string>
+
 #include "cereal/messaging/messaging.h"
 
 // no-op base hw class
@@ -11,10 +14,10 @@ public:
   static constexpr float MIN_VOLUME = 0.2;
 
   static std::string get_os_version() { return ""; }
-  static std::string get_name() { return ""; };
-  static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::UNKNOWN; };
-  static int get_voltage() { return 0; };
-  static int get_current() { return 0; };
+  static std::string get_name() { return ""; }
+  static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::UNKNOWN; }
+  static int get_voltage() { return 0; }
+  static int get_current() { return 0; }
 
   static std::string get_serial() { return "cccccc"; }
 
@@ -31,6 +34,8 @@ public:
   static bool get_ssh_enabled() { return false; }
   static void set_ssh_enabled(bool enabled) {}
 
+  static void config_cpu_rendering();
+
   static bool PC() { return false; }
   static bool TICI() { return false; }
   static bool AGNOS() { return false; }
diff --git a/system/hardware/hw.h b/system/hardware/hw.h
index 3b0583a10b..2f6ccfffda 100644
--- a/system/hardware/hw.h
+++ b/system/hardware/hw.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <string>
+
 #include "system/hardware/base.h"
 #include "common/util.h"
 
@@ -12,16 +14,37 @@
 #endif
 
 namespace Path {
-inline std::string log_root() {
-  if (const char *env = getenv("LOG_ROOT")) {
-    return env;
+  inline std::string openpilot_prefix() {
+    return util::getenv("OPENPILOT_PREFIX", "");
+  }
+
+  inline std::string comma_home() {
+    return util::getenv("HOME") + "/.comma" + Path::openpilot_prefix();
+  }
+
+  inline std::string log_root() {
+    if (const char *env = getenv("LOG_ROOT")) {
+      return env;
+    }
+    return Hardware::PC() ? Path::comma_home() + "/media/0/realdata" : "/data/media/0/realdata";
+  }
+
+  inline std::string params() {
+    return Hardware::PC() ? util::getenv("PARAMS_ROOT", Path::comma_home() + "/params") : "/data/params";
+  }
+
+  inline std::string rsa_file() {
+    return Hardware::PC() ? Path::comma_home() + "/persist/comma/id_rsa" : "/persist/comma/id_rsa";
+  }
+  
+  inline std::string swaglog_ipc() {
+    return "ipc:///tmp/logmessage" + Path::openpilot_prefix();
+  }
+
+  inline std::string download_cache_root() {
+    if (const char *env = getenv("COMMA_CACHE")) {
+      return env;
+    }
+    return "/tmp/comma_download_cache" + Path::openpilot_prefix() + "/";
   }
-  return Hardware::PC() ? util::getenv("HOME") + "/.comma/media/0/realdata" : "/data/media/0/realdata";
-}
-inline std::string params() {
-  return Hardware::PC() ? util::getenv("PARAMS_ROOT", util::getenv("HOME") + "/.comma/params") : "/data/params";
-}
-inline std::string rsa_file() {
-  return Hardware::PC() ? util::getenv("HOME") + "/.comma/persist/comma/id_rsa" : "/persist/comma/id_rsa";
-}
 }  // namespace Path
diff --git a/system/hardware/hw.py b/system/hardware/hw.py
new file mode 100644
index 0000000000..119ca48c86
--- /dev/null
+++ b/system/hardware/hw.py
@@ -0,0 +1,35 @@
+import os
+from pathlib import Path
+
+from openpilot.system.hardware import PC
+
+class Paths:
+  @staticmethod
+  def comma_home() -> str:
+    return os.path.join(str(Path.home()), ".comma" + os.environ.get("OPENPILOT_PREFIX", ""))
+
+  @staticmethod
+  def log_root() -> str:
+    if os.environ.get('LOG_ROOT', False):
+      return os.environ['LOG_ROOT']
+    elif PC:
+      return str(Path(Paths.comma_home()) / "media" / "0" / "realdata")
+    else:
+      return '/data/media/0/realdata/'
+
+  @staticmethod
+  def swaglog_root() -> str:
+    if PC:
+      return os.path.join(Paths.comma_home(), "log")
+    else:
+      return "/data/log/"
+
+  @staticmethod
+  def swaglog_ipc() -> str:
+    return "ipc:///tmp/logmessage" + os.environ.get("OPENPILOT_PREFIX", "")
+
+  @staticmethod
+  def download_cache_root() -> str:
+    if os.environ.get('COMMA_CACHE', False):
+      return os.environ['COMMA_CACHE']
+    return "/tmp/comma_download_cache" + os.environ.get("OPENPILOT_PREFIX", "") + "/"
diff --git a/system/hardware/pc/hardware.h b/system/hardware/pc/hardware.h
index 529b4bfe9d..7a7ddd60b7 100644
--- a/system/hardware/pc/hardware.h
+++ b/system/hardware/pc/hardware.h
@@ -1,12 +1,14 @@
 #pragma once
 
+#include <string>
+
 #include "system/hardware/base.h"
 
 class HardwarePC : public HardwareNone {
 public:
   static std::string get_os_version() { return "openpilot for PC"; }
-  static std::string get_name() { return "pc"; };
-  static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::PC; };
+  static std::string get_name() { return "pc"; }
+  static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::PC; }
   static bool PC() { return true; }
   static bool TICI() { return util::getenv("TICI", 0) == 1; }
   static bool AGNOS() { return util::getenv("TICI", 0) == 1; }
@@ -18,4 +20,10 @@ public:
     snprintf(volume_str, sizeof(volume_str), "%.3f", volume);
     std::system(("pactl set-sink-volume @DEFAULT_SINK@ " + std::string(volume_str)).c_str());
   }
+
+  static void config_cpu_rendering() {
+    setenv("QT_QPA_PLATFORM", "offscreen", 1);
+    setenv("__GLX_VENDOR_LIBRARY_NAME", "mesa", 1);
+    setenv("LP_NUM_THREADS", "0", 1); // disable threading so we stay on our assigned CPU
+  }
 };
diff --git a/system/hardware/pc/hardware.py b/system/hardware/pc/hardware.py
index 564f9e483a..27c05f5904 100644
--- a/system/hardware/pc/hardware.py
+++ b/system/hardware/pc/hardware.py
@@ -1,7 +1,7 @@
 import random
 
 from cereal import log
-from system.hardware.base import HardwareBase, ThermalConfig
+from openpilot.system.hardware.base import HardwareBase, ThermalConfig
 
 NetworkType = log.DeviceState.NetworkType
 NetworkStrength = log.DeviceState.NetworkStrength
diff --git a/system/hardware/tici/agnos.json b/system/hardware/tici/agnos.json
index 90933e8fef..7e8cd480be 100644
--- a/system/hardware/tici/agnos.json
+++ b/system/hardware/tici/agnos.json
@@ -67,6 +67,10 @@
     "size": 10737418240,
     "sparse": true,
     "full_check": false,
-    "has_ab": true
+    "has_ab": true,
+    "alt": {
+      "hash": "256442a55fcb9e8f72969f003a4db91598dee1136f8dda85b553a557d36b93d8",
+      "url": "https://commadist.azureedge.net/agnosupdate/system-skip-chunks-e1fa3018bce9bad01c6967e5e21f1141cf5c8f02d2edfaed51c738f74a32a432.img.xz"
+    }
   }
 ]
diff --git a/system/hardware/tici/agnos.py b/system/hardware/tici/agnos.py
index 5f446a8e90..ef7d9adb79 100755
--- a/system/hardware/tici/agnos.py
+++ b/system/hardware/tici/agnos.py
@@ -10,7 +10,7 @@ from typing import Dict, Generator, List, Tuple, Union
 
 import requests
 
-import system.hardware.tici.casync as casync
+import openpilot.system.hardware.tici.casync as casync
 
 SPARSE_CHUNK_FMT = struct.Struct('H2xI4x')
 CAIBX_URL = "https://commadist.azureedge.net/agnosupdate/"
diff --git a/system/hardware/tici/amplifier.py b/system/hardware/tici/amplifier.py
index 5b656a40fa..e003f131cc 100755
--- a/system/hardware/tici/amplifier.py
+++ b/system/hardware/tici/amplifier.py
@@ -1,3 +1,4 @@
+#!/usr/bin/env python3
 import time
 from smbus2 import SMBus
 from collections import namedtuple
diff --git a/system/hardware/tici/esim.nmconnection b/system/hardware/tici/esim.nmconnection
new file mode 100644
index 0000000000..74f6f8e82c
--- /dev/null
+++ b/system/hardware/tici/esim.nmconnection
@@ -0,0 +1,30 @@
+[connection]
+id=esim
+uuid=fff6553c-3284-4707-a6b1-acc021caaafb
+type=gsm
+permissions=
+autoconnect=true
+autoconnect-retries=100
+autoconnect-priority=2
+metered=1
+
+[gsm]
+apn=
+home-only=false
+auto-config=true
+sim-id=
+
+[ipv4]
+route-metric=1000
+dns-priority=1000
+dns-search=
+method=auto
+
+[ipv6]
+ddr-gen-mode=stable-privacy
+dns-search=
+route-metric=1000
+dns-priority=1000
+method=auto
+
+[proxy]
diff --git a/system/hardware/tici/hardware.h b/system/hardware/tici/hardware.h
index 580dc83eec..fe0a20ae58 100644
--- a/system/hardware/tici/hardware.h
+++ b/system/hardware/tici/hardware.h
@@ -2,6 +2,8 @@
 
 #include <cstdlib>
 #include <fstream>
+#include <map>
+#include <string>
 
 #include "common/params.h"
 #include "common/util.h"
@@ -15,19 +17,19 @@ public:
   static bool AGNOS() { return true; }
   static std::string get_os_version() {
     return "AGNOS " + util::read_file("/VERSION");
-  };
+  }
 
   static std::string get_name() {
     std::string devicetree_model = util::read_file("/sys/firmware/devicetree/base/model");
     return (devicetree_model.find("tizi") != std::string::npos) ? "tizi" : "tici";
-  };
+  }
 
   static cereal::InitData::DeviceType get_device_type() {
     return (get_name() == "tizi") ? cereal::InitData::DeviceType::TIZI : cereal::InitData::DeviceType::TICI;
-  };
+  }
 
-  static int get_voltage() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str()); };
-  static int get_current() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str()); };
+  static int get_voltage() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str()); }
+  static int get_current() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str()); }
 
   static std::string get_serial() {
     static std::string serial("");
@@ -47,8 +49,8 @@ public:
     return serial;
   }
 
-  static void reboot() { std::system("sudo reboot"); };
-  static void poweroff() { std::system("sudo poweroff"); };
+  static void reboot() { std::system("sudo reboot"); }
+  static void poweroff() { std::system("sudo poweroff"); }
   static void set_brightness(int percent) {
     std::string max = util::read_file("/sys/class/backlight/panel0-backlight/max_brightness");
 
@@ -57,14 +59,14 @@ public:
       brightness_control << (int)(percent * (std::stof(max)/100.)) << "\n";
       brightness_control.close();
     }
-  };
+  }
   static void set_display_power(bool on) {
     std::ofstream bl_power_control("/sys/class/backlight/panel0-backlight/bl_power");
     if (bl_power_control.is_open()) {
       bl_power_control << (on ? "0" : "4") << "\n";
       bl_power_control.close();
     }
-  };
+  }
   static void set_volume(float volume) {
     volume = util::map_val(volume, 0.f, 1.f, MIN_VOLUME, MAX_VOLUME);
 
@@ -99,6 +101,11 @@ public:
     return ret;
   }
 
-  static bool get_ssh_enabled() { return Params().getBool("SshEnabled"); };
-  static void set_ssh_enabled(bool enabled) { Params().putBool("SshEnabled", enabled); };
+  static bool get_ssh_enabled() { return Params().getBool("SshEnabled"); }
+  static void set_ssh_enabled(bool enabled) { Params().putBool("SshEnabled", enabled); }
+
+  static void config_cpu_rendering() {
+    setenv("QT_QPA_PLATFORM", "eglfs", 1); // offscreen doesn't work with EGL/GLES
+    setenv("LP_NUM_THREADS", "0", 1); // disable threading so we stay on our assigned CPU
+  }
 };
diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py
index 018bc30004..5f8267a461 100644
--- a/system/hardware/tici/hardware.py
+++ b/system/hardware/tici/hardware.py
@@ -3,16 +3,17 @@ import math
 import os
 import subprocess
 import time
+import tempfile
 from enum import IntEnum
 from functools import cached_property, lru_cache
 from pathlib import Path
 
 from cereal import log
-from common.gpio import gpio_set, gpio_init, get_irqs_for_action
-from system.hardware.base import HardwareBase, ThermalConfig
-from system.hardware.tici import iwlist
-from system.hardware.tici.pins import GPIO
-from system.hardware.tici.amplifier import Amplifier
+from openpilot.common.gpio import gpio_set, gpio_init, get_irqs_for_action
+from openpilot.system.hardware.base import HardwareBase, ThermalConfig
+from openpilot.system.hardware.tici import iwlist
+from openpilot.system.hardware.tici.pins import GPIO
+from openpilot.system.hardware.tici.amplifier import Amplifier
 
 NM = 'org.freedesktop.NetworkManager'
 NM_CON_ACT = NM + '.Connection.Active'
@@ -83,17 +84,28 @@ def affine_irq(val, action):
   for i in irqs:
     sudo_write(str(val), f"/proc/irq/{i}/smp_affinity_list")
 
+@lru_cache
+def get_device_type():
+  # lru_cache and cache can cause memory leaks when used in classes
+  with open("/sys/firmware/devicetree/base/model") as f:
+    model = f.read().strip('\x00')
+  model = model.split('comma ')[-1]
+  # TODO: remove this with AGNOS 7+
+  if model.startswith('Qualcomm'):
+    model = 'tici'
+  return model
+
 class Tici(HardwareBase):
   @cached_property
   def bus(self):
-    import dbus  # pylint: disable=import-error
+    import dbus
     return dbus.SystemBus()
 
   @cached_property
   def nm(self):
     return self.bus.get_object(NM, '/org/freedesktop/NetworkManager')
 
-  @cached_property
+  @property # this should not be cached, in case the modemmanager restarts
   def mm(self):
     return self.bus.get_object(MM, '/org/freedesktop/ModemManager1')
 
@@ -105,15 +117,8 @@ class Tici(HardwareBase):
     with open("/VERSION") as f:
       return f.read().strip()
 
-  @lru_cache
   def get_device_type(self):
-    with open("/sys/firmware/devicetree/base/model") as f:
-      model = f.read().strip('\x00')
-    model = model.split('comma ')[-1]
-    # TODO: remove this with AGNOS 7+
-    if model.startswith('Qualcomm'):
-      model = 'tici'
-    return model
+    return get_device_type()
 
   def get_sound_card_online(self):
     if os.path.isfile('/proc/asound/card0/state'):
@@ -206,8 +211,8 @@ class Tici(HardwareBase):
     return str(self.get_modem().Get(MM_MODEM, 'EquipmentIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT))
 
   def get_network_info(self):
-    modem = self.get_modem()
     try:
+      modem = self.get_modem()
       info = modem.Command("AT+QNWINFO", math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
       extra = modem.Command('AT+QENG="servingcell"', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
       state = modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
@@ -319,7 +324,8 @@ class Tici(HardwareBase):
       (True, tc + ["class", "add", "dev", adapter, "parent", "1:", "classid", "1:20", "htb", "rate", f"{upload_speed_kbps}kbit"]),
 
       # Create universal 32 bit filter on adapter that sends all outbound ip traffic through the class
-      (True, tc + ["filter", "add", "dev", adapter, "parent", "1:", "protocol", "ip", "prio", "10", "u32", "match", "ip", "dst", "0.0.0.0/0", "flowid", "1:20"]),
+      (True, tc + ["filter", "add", "dev", adapter, "parent", "1:", "protocol", "ip", "prio", \
+                   "10", "u32", "match", "ip", "dst", "0.0.0.0/0", "flowid", "1:20"]),
     ]
 
     download = [
@@ -328,7 +334,8 @@ class Tici(HardwareBase):
 
       # Redirect ingress (incoming) to egress ifb0
       (True, tc + ["qdisc", "add", "dev", adapter, "handle", "ffff:", "ingress"]),
-      (True, tc + ["filter", "add", "dev", adapter, "parent", "ffff:", "protocol", "ip", "u32", "match", "u32", "0", "0", "action", "mirred", "egress", "redirect", "dev", ifb]),
+      (True, tc + ["filter", "add", "dev", adapter, "parent", "ffff:", "protocol", "ip", "u32", \
+                   "match", "u32", "0", "0", "action", "mirred", "egress", "redirect", "dev", ifb]),
 
       # Add class and rules for virtual interface
       (True, tc + ["qdisc", "add", "dev", ifb, "root", "handle", "2:", "htb"]),
@@ -499,7 +506,7 @@ class Tici(HardwareBase):
     sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_rail_on")
     sudo_write("1000", "/sys/class/kgsl/kgsl-3d0/idle_timer")
     sudo_write("performance", "/sys/class/kgsl/kgsl-3d0/devfreq/governor")
-    sudo_write("596", "/sys/class/kgsl/kgsl-3d0/max_clock_mhz")
+    sudo_write("710", "/sys/class/kgsl/kgsl-3d0/max_clock_mhz")
 
     # setup governors
     sudo_write("performance", "/sys/class/devfreq/soc:qcom,cpubw/governor")
@@ -526,9 +533,23 @@ class Tici(HardwareBase):
       except Exception:
         pass
 
-    # blue prime config
-    if sim_id.startswith('8901410'):
-      os.system('mmcli -m any --3gpp-set-initial-eps-bearer-settings="apn=Broadband"')
+    # blue prime
+    blue_prime = sim_id.startswith('8901410')
+    initial_apn = "Broadband" if blue_prime else ""
+    os.system(f'mmcli -m any --3gpp-set-initial-eps-bearer-settings="apn={initial_apn}"')
+
+    # eSIM prime
+    if sim_id.startswith('8985235'):
+      dest = "/etc/NetworkManager/system-connections/esim.nmconnection"
+      with open(Path(__file__).parent/'esim.nmconnection') as f, tempfile.NamedTemporaryFile(mode='w') as tf:
+        dat = f.read()
+        dat = dat.replace("sim-id=", f"sim-id={sim_id}")
+        tf.write(dat)
+        tf.flush()
+
+        # needs to be root
+        os.system(f"sudo cp {tf.name} {dest}")
+      os.system(f"sudo nmcli con load {dest}")
 
   def get_networks(self):
     r = {}
@@ -580,7 +601,7 @@ class Tici(HardwareBase):
     gpio_init(GPIO.STM_RST_N, True)
 
     gpio_set(GPIO.STM_RST_N, 1)
-    time.sleep(2)
+    time.sleep(1)
     gpio_set(GPIO.STM_RST_N, 0)
 
   def recover_internal_panda(self):
@@ -589,13 +610,14 @@ class Tici(HardwareBase):
 
     gpio_set(GPIO.STM_RST_N, 1)
     gpio_set(GPIO.STM_BOOT0, 1)
-    time.sleep(1)
+    time.sleep(0.5)
     gpio_set(GPIO.STM_RST_N, 0)
-    time.sleep(1)
+    time.sleep(0.5)
     gpio_set(GPIO.STM_BOOT0, 0)
 
 
 if __name__ == "__main__":
   t = Tici()
+  t.configure_modem()
   t.initialize_hardware()
   t.set_power_save(False)
diff --git a/system/hardware/tici/pins.py b/system/hardware/tici/pins.py
index 5ac0158082..4388351e1f 100644
--- a/system/hardware/tici/pins.py
+++ b/system/hardware/tici/pins.py
@@ -6,7 +6,7 @@ class GPIO:
   HUB_RST_N = 30
   UBLOX_RST_N = 32
   UBLOX_SAFEBOOT_N = 33
-  UBLOX_PWR_EN = 34
+  GNSS_PWR_EN = 34 # SCHEMATIC LABEL: GPIO_UBLOX_PWR_EN
   STM_RST_N = 124
   STM_BOOT0 = 134
 
diff --git a/system/logcatd/logcatd_systemd.cc b/system/logcatd/logcatd_systemd.cc
index 70467a9c15..54b3782132 100644
--- a/system/logcatd/logcatd_systemd.cc
+++ b/system/logcatd/logcatd_systemd.cc
@@ -5,7 +5,7 @@
 #include <map>
 #include <string>
 
-#include "json11.hpp"
+#include "third_party/json11/json11.hpp"
 
 #include "cereal/messaging/messaging.h"
 #include "common/timing.h"
@@ -35,7 +35,7 @@ int main(int argc, char *argv[]) {
     // Wait for new message if we didn't receive anything
     if (err == 0) {
       err = sd_journal_wait(journal, 1000 * 1000);
-      assert (err >= 0);
+      assert(err >= 0);
       continue; // Try again
     }
 
diff --git a/system/loggerd/SConscript b/system/loggerd/SConscript
index 3b961bce6e..d4f52fb5f1 100644
--- a/system/loggerd/SConscript
+++ b/system/loggerd/SConscript
@@ -23,5 +23,5 @@ env.Program('loggerd', ['loggerd.cc'], LIBS=libs)
 env.Program('encoderd', ['encoderd.cc'], LIBS=libs)
 env.Program('bootlog.cc', LIBS=libs)
 
-if GetOption('test'):
+if GetOption('extras'):
   env.Program('tests/test_logger', ['tests/test_runner.cc', 'tests/test_logger.cc'], LIBS=libs + ['curl', 'crypto'])
diff --git a/system/loggerd/bootlog.cc b/system/loggerd/bootlog.cc
index becd293c02..771594d20c 100644
--- a/system/loggerd/bootlog.cc
+++ b/system/loggerd/bootlog.cc
@@ -50,11 +50,11 @@ static kj::Array<capnp::word> build_boot_log() {
 
 int main(int argc, char** argv) {
   const std::string timestr = logger_get_route_name();
-  const std::string path = LOG_ROOT + "/boot/" + timestr;
+  const std::string path = Path::log_root() + "/boot/" + timestr;
   LOGW("bootlog to %s", path.c_str());
 
   // Open bootlog
-  bool r = util::create_directories(LOG_ROOT + "/boot/", 0775);
+  bool r = util::create_directories(Path::log_root() + "/boot/", 0775);
   assert(r);
 
   RawFile file(path.c_str());
diff --git a/system/loggerd/config.py b/system/loggerd/config.py
index df187a48c0..664e78b378 100644
--- a/system/loggerd/config.py
+++ b/system/loggerd/config.py
@@ -1,13 +1,7 @@
 import os
 from pathlib import Path
-from system.hardware import PC
-
-if os.environ.get('LOG_ROOT', False):
-  ROOT = os.environ['LOG_ROOT']
-elif PC:
-  ROOT = str(Path.home() / ".comma" / "media" / "0" / "realdata")
-else:
-  ROOT = '/data/media/0/realdata/'
+from openpilot.system.hardware import PC
+from openpilot.system.hardware.hw import Paths
 
 
 CAMERA_FPS = 20
@@ -23,7 +17,7 @@ STATS_FLUSH_TIME_S = 60
 
 def get_available_percent(default=None):
   try:
-    statvfs = os.statvfs(ROOT)
+    statvfs = os.statvfs(Paths.log_root())
     available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks
   except OSError:
     available_percent = default
@@ -33,7 +27,7 @@ def get_available_percent(default=None):
 
 def get_available_bytes(default=None):
   try:
-    statvfs = os.statvfs(ROOT)
+    statvfs = os.statvfs(Paths.log_root())
     available_bytes = statvfs.f_bavail * statvfs.f_frsize
   except OSError:
     available_bytes = default
diff --git a/system/loggerd/deleter.py b/system/loggerd/deleter.py
old mode 100644
new mode 100755
index 5e7b31f583..b060232b6e
--- a/system/loggerd/deleter.py
+++ b/system/loggerd/deleter.py
@@ -3,11 +3,11 @@ import os
 import shutil
 import threading
 from typing import List
-
-from system.swaglog import cloudlog
-from system.loggerd.config import ROOT, get_available_bytes, get_available_percent
-from system.loggerd.uploader import listdir_by_creation
-from system.loggerd.xattr_cache import getxattr
+from openpilot.system.hardware.hw import Paths
+from openpilot.system.swaglog import cloudlog
+from openpilot.system.loggerd.config import get_available_bytes, get_available_percent
+from openpilot.system.loggerd.uploader import listdir_by_creation
+from openpilot.system.loggerd.xattr_cache import getxattr
 
 MIN_BYTES = 5 * 1024 * 1024 * 1024
 MIN_PERCENT = 10
@@ -20,7 +20,7 @@ PRESERVE_COUNT = 5
 
 
 def has_preserve_xattr(d: str) -> bool:
-  return getxattr(os.path.join(ROOT, d), PRESERVE_ATTR_NAME) == PRESERVE_ATTR_VALUE
+  return getxattr(os.path.join(Paths.log_root(), d), PRESERVE_ATTR_NAME) == PRESERVE_ATTR_VALUE
 
 
 def get_preserved_segments(dirs_by_creation: List[str]) -> List[str]:
@@ -51,14 +51,14 @@ def deleter_thread(exit_event):
     out_of_percent = get_available_percent(default=MIN_PERCENT + 1) < MIN_PERCENT
 
     if out_of_percent or out_of_bytes:
-      dirs = listdir_by_creation(ROOT)
+      dirs = listdir_by_creation(Paths.log_root())
 
       # skip deleting most recent N preserved segments (and their prior segment)
       preserved_dirs = get_preserved_segments(dirs)
 
       # remove the earliest directory we can
       for delete_dir in sorted(dirs, key=lambda d: (d in DELETE_LAST, d in preserved_dirs)):
-        delete_path = os.path.join(ROOT, delete_dir)
+        delete_path = os.path.join(Paths.log_root(), delete_dir)
 
         if any(name.endswith(".lock") for name in os.listdir(delete_path)):
           continue
diff --git a/system/loggerd/encoder/encoder.h b/system/loggerd/encoder/encoder.h
index 59ec4357ae..a8bfd5c054 100644
--- a/system/loggerd/encoder/encoder.h
+++ b/system/loggerd/encoder/encoder.h
@@ -2,6 +2,7 @@
 
 #include <cassert>
 #include <cstdint>
+#include <memory>
 #include <thread>
 
 #include "cereal/messaging/messaging.h"
@@ -15,7 +16,7 @@
 class VideoEncoder {
 public:
   VideoEncoder(const EncoderInfo &encoder_info, int in_width, int in_height);
-  virtual ~VideoEncoder() {};
+  virtual ~VideoEncoder() {}
   virtual int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) = 0;
   virtual void encoder_open(const char* path) = 0;
   virtual void encoder_close() = 0;
diff --git a/system/loggerd/encoder/ffmpeg_encoder.cc b/system/loggerd/encoder/ffmpeg_encoder.cc
index b73f4e8f5d..f44f2fbed7 100644
--- a/system/loggerd/encoder/ffmpeg_encoder.cc
+++ b/system/loggerd/encoder/ffmpeg_encoder.cc
@@ -11,7 +11,7 @@
 
 #define __STDC_CONSTANT_MACROS
 
-#include "libyuv.h"
+#include "third_party/libyuv/include/libyuv.h"
 
 extern "C" {
 #include <libavcodec/avcodec.h>
@@ -135,7 +135,7 @@ int FfmpegEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) {
     }
 
     if (env_debug_encoder) {
-      printf("%20s got %8d bytes flags %8x idx %4d id %8d\n", encoder_info.filename, pkt.size, pkt.flags, counter, extra->frame_id);
+      printf("%20s got %8d bytes flags %8x idx %4d id %8d\n", encoder_info.publish_name, pkt.size, pkt.flags, counter, extra->frame_id);
     }
 
     publisher_publish(this, segment_num, counter, *extra,
diff --git a/system/loggerd/encoder/v4l_encoder.cc b/system/loggerd/encoder/v4l_encoder.cc
index a319d414ca..571f5979e2 100644
--- a/system/loggerd/encoder/v4l_encoder.cc
+++ b/system/loggerd/encoder/v4l_encoder.cc
@@ -1,4 +1,5 @@
 #include <cassert>
+#include <string>
 #include <sys/ioctl.h>
 #include <poll.h>
 
@@ -6,11 +7,11 @@
 #include "common/util.h"
 #include "common/timing.h"
 
-#include "libyuv.h"
-#include "msm_media_info.h"
+#include "third_party/libyuv/include/libyuv.h"
+#include "third_party/linux/include/msm_media_info.h"
 
 // has to be in this order
-#include "v4l2-controls.h"
+#include "third_party/linux/include/v4l2-controls.h"
 #include <linux/videodev2.h>
 #define V4L2_QCOM_BUF_FLAG_CODECCONFIG 0x00020000
 #define V4L2_QCOM_BUF_FLAG_EOS 0x02000000
@@ -18,7 +19,13 @@
 // echo 0x7fffffff > /sys/kernel/debug/msm_vidc/debug_level
 const int env_debug_encoder = (getenv("DEBUG_ENCODER") != NULL) ? atoi(getenv("DEBUG_ENCODER")) : 0;
 
-#define checked_ioctl(x,y,z) { int _ret = HANDLE_EINTR(ioctl(x,y,z)); if (_ret!=0) { LOGE("checked_ioctl failed %d %lx %p", x, y, z); } assert(_ret==0); }
+static void checked_ioctl(int fd, unsigned long request, void *argp) {
+  int ret = util::safe_ioctl(fd, request, argp);
+  if (ret != 0) {
+    LOGE("checked_ioctl failed with error %d (%d %lx %p)", errno, fd, request, argp);
+    assert(0);
+  }
+}
 
 static void dequeue_buffer(int fd, v4l2_buf_type buf_type, unsigned int *index=NULL, unsigned int *bytesused=NULL, unsigned int *flags=NULL, struct timeval *timestamp=NULL) {
   v4l2_plane plane = {0};
@@ -68,7 +75,7 @@ static void request_buffers(int fd, v4l2_buf_type buf_type, unsigned int count)
 }
 
 void V4LEncoder::dequeue_handler(V4LEncoder *e) {
-  std::string dequeue_thread_name = "dq-"+std::string(e->encoder_info.filename);
+  std::string dequeue_thread_name = "dq-"+std::string(e->encoder_info.publish_name);
   util::set_thread_name(dequeue_thread_name.c_str());
 
   e->segment_num++;
@@ -85,10 +92,20 @@ void V4LEncoder::dequeue_handler(V4LEncoder *e) {
 
   while (!exit) {
     int rc = poll(&pfd, 1, 1000);
-    if (!rc) { LOGE("encoder dequeue poll timeout"); continue; }
+    if (rc < 0) {
+      if (errno != EINTR) {
+        // TODO: exit encoder?
+        // ignore the error and keep going
+        LOGE("poll failed (%d - %d)", rc, errno);
+      }
+      continue;
+    } else if (rc == 0) {
+      LOGE("encoder dequeue poll timeout");
+      continue;
+    }
 
     if (env_debug_encoder >= 2) {
-      printf("%20s poll %x at %.2f ms\n", e->encoder_info.filename, pfd.revents, millis_since_boot());
+      printf("%20s poll %x at %.2f ms\n", e->encoder_info.publish_name, pfd.revents, millis_since_boot());
     }
 
     int frame_id = -1;
@@ -116,7 +133,7 @@ void V4LEncoder::dequeue_handler(V4LEncoder *e) {
 
       if (env_debug_encoder) {
         printf("%20s got(%d) %6d bytes flags %8x idx %3d/%4d id %8d ts %ld lat %.2f ms (%lu frames free)\n",
-          e->encoder_info.filename, index, bytesused, flags, e->segment_num, idx, frame_id, ts, millis_since_boot()-(ts/1000.), e->free_buf_in.size());
+          e->encoder_info.publish_name, index, bytesused, flags, e->segment_num, idx, frame_id, ts, millis_since_boot()-(ts/1000.), e->free_buf_in.size());
       }
 
       // requeue the buffer
diff --git a/system/loggerd/encoderd.cc b/system/loggerd/encoderd.cc
index 2cb26dc8d5..1b45df6827 100644
--- a/system/loggerd/encoderd.cc
+++ b/system/loggerd/encoderd.cc
@@ -47,7 +47,7 @@ bool sync_encoders(EncoderdState *s, CameraType cam_type, uint32_t frame_id) {
 void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) {
   util::set_thread_name(cam_info.thread_name);
 
-  std::vector<Encoder *> encoders;
+  std::vector<std::unique_ptr<Encoder>> encoders;
   VisionIpcClient vipc_client = VisionIpcClient("camerad", cam_info.stream_type, false);
 
   int cur_seg = 0;
@@ -60,18 +60,15 @@ void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) {
     // init encoders
     if (encoders.empty()) {
       VisionBuf buf_info = vipc_client.buffers[0];
-      LOGW("encoder %s init %dx%d", cam_info.thread_name, buf_info.width, buf_info.height);
+      LOGW("encoder %s init %zux%zu", cam_info.thread_name, buf_info.width, buf_info.height);
       assert(buf_info.width > 0 && buf_info.height > 0);
 
       for (const auto &encoder_info : cam_info.encoder_infos) {
-        encoders.push_back(new Encoder(encoder_info, buf_info.width, buf_info.height));
+        auto &e = encoders.emplace_back(new Encoder(encoder_info, buf_info.width, buf_info.height));
+        e->encoder_open(nullptr);
       }
     }
 
-    for (int i = 0; i < encoders.size(); ++i) {
-      encoders[i]->encoder_open(NULL);
-    }
-
     bool lagging = false;
     while (!do_exit) {
       VisionIpcBufExtra extra;
@@ -81,7 +78,7 @@ void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) {
       // detect loop around and drop the frames
       if (buf->get_frame_id() != extra.frame_id) {
         if (!lagging) {
-          LOGE("encoder %s lag  buffer id: %d  extra id: %d", cam_info.thread_name, buf->get_frame_id(), extra.frame_id);
+          LOGE("encoder %s lag  buffer id: %" PRIu64 " extra id: %d", cam_info.thread_name, buf->get_frame_id(), extra.frame_id);
           lagging = true;
         }
         continue;
@@ -113,15 +110,10 @@ void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) {
       }
     }
   }
-
-  LOG("encoder destroy");
-  for(auto &e : encoders) {
-    e->encoder_close();
-    delete e;
-  }
 }
 
-void encoderd_thread() {
+template <size_t N>
+void encoderd_thread(const LogCameraInfo (&cameras)[N]) {
   EncoderdState s;
 
   std::set<VisionStreamType> streams;
@@ -136,9 +128,9 @@ void encoderd_thread() {
   if (!streams.empty()) {
     std::vector<std::thread> encoder_threads;
     for (auto stream : streams) {
-      auto it = std::find_if(std::begin(cameras_logged), std::end(cameras_logged),
+      auto it = std::find_if(std::begin(cameras), std::end(cameras),
                              [stream](auto &cam) { return cam.stream_type == stream; });
-      assert(it != std::end(cameras_logged));
+      assert(it != std::end(cameras));
       ++s.max_waiting;
       encoder_threads.push_back(std::thread(encoder_thread, &s, *it));
     }
@@ -147,7 +139,7 @@ void encoderd_thread() {
   }
 }
 
-int main() {
+int main(int argc, char* argv[]) {
   if (!Hardware::PC()) {
     int ret;
     ret = util::set_realtime_priority(52);
@@ -155,6 +147,15 @@ int main() {
     ret = util::set_core_affinity({3});
     assert(ret == 0);
   }
-  encoderd_thread();
+  if (argc > 1) {
+    std::string arg1(argv[1]);
+    if (arg1 == "--stream") {
+      encoderd_thread(stream_cameras_logged);
+    } else {
+      LOGE("Argument '%s' is not supported", arg1.c_str());
+    }
+  } else {
+    encoderd_thread(cameras_logged);
+  }
   return 0;
 }
diff --git a/system/loggerd/logger.cc b/system/loggerd/logger.cc
index 1e710759e6..1474552a13 100644
--- a/system/loggerd/logger.cc
+++ b/system/loggerd/logger.cc
@@ -13,7 +13,10 @@
 #include <ctime>
 #include <fstream>
 #include <iostream>
+#include <map>
 #include <streambuf>
+#include <string>
+#include <vector>
 
 #include "common/params.h"
 #include "common/swaglog.h"
@@ -21,9 +24,12 @@
 
 // ***** log metadata *****
 kj::Array<capnp::word> logger_build_init_data() {
+  uint64_t wall_time = nanos_since_epoch();
+
   MessageBuilder msg;
   auto init = msg.initEvent().initInitData();
 
+  init.setWallTimeNanos(wall_time);
   init.setVersion(COMMA_VERSION);
   init.setDirty(!getenv("CLEAN"));
   init.setDeviceType(Hardware::get_device_type());
diff --git a/system/loggerd/logger.h b/system/loggerd/logger.h
index e7594cee88..06b11e72f9 100644
--- a/system/loggerd/logger.h
+++ b/system/loggerd/logger.h
@@ -1,11 +1,12 @@
 #pragma once
 
-#include <cassert>
 #include <pthread.h>
 
+#include <cassert>
 #include <cstdint>
 #include <cstdio>
 #include <memory>
+#include <string>
 
 #include <capnp/serialize.h>
 #include <kj/array.h>
@@ -15,8 +16,6 @@
 #include "common/swaglog.h"
 #include "system/hardware/hw.h"
 
-const std::string LOG_ROOT = Path::log_root();
-
 #define LOGGER_MAX_HANDLES 16
 
 class RawFile {
diff --git a/system/loggerd/loggerd.cc b/system/loggerd/loggerd.cc
index 9cad9d7f2e..adb24c913c 100644
--- a/system/loggerd/loggerd.cc
+++ b/system/loggerd/loggerd.cc
@@ -1,6 +1,10 @@
 #include <sys/xattr.h>
 
+#include <map>
+#include <memory>
+#include <string>
 #include <unordered_map>
+#include <vector>
 
 #include "system/loggerd/encoder/encoder.h"
 #include "system/loggerd/loggerd.h"
@@ -20,7 +24,7 @@ struct LoggerdState {
 
 void logger_rotate(LoggerdState *s) {
   int segment = -1;
-  int err = logger_next(&s->logger, LOG_ROOT.c_str(), s->segment_path, sizeof(s->segment_path), &segment);
+  int err = logger_next(&s->logger, Path::log_root().c_str(), s->segment_path, sizeof(s->segment_path), &segment);
   assert(err == 0);
   s->rotate_segment = segment;
   s->ready_to_rotate = 0;
@@ -94,7 +98,7 @@ int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct
       re.marked_ready_to_rotate = false;
       // we are in this segment now, process any queued messages before this one
       if (!re.q.empty()) {
-        for (auto &qmsg: re.q) {
+        for (auto &qmsg : re.q) {
           bytes_count += handle_encoder_msg(s, qmsg, name, re, encoder_info);
         }
         re.q.clear();
@@ -112,6 +116,7 @@ int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct
         }
         // if we aren't actually recording, don't create the writer
         if (encoder_info.record) {
+          assert(encoder_info.filename != NULL);
           re.writer.reset(new VideoWriter(s->segment_path,
             encoder_info.filename, idx.getType() != cereal::EncodeIndex::Type::FULL_H_E_V_C,
             encoder_info.frame_width, encoder_info.frame_height, encoder_info.fps, idx.getType()));
@@ -203,10 +208,11 @@ void loggerd_thread() {
   std::unique_ptr<Poller> poller(Poller::create());
 
   // subscribe to all socks
-  for (const auto& it : services) {
-    const bool encoder = strcmp(it.name+strlen(it.name)-strlen("EncodeData"), "EncodeData") == 0;
-    if (!it.should_log && !encoder) continue;
-    LOGD("logging %s (on port %d)", it.name, it.port);
+  for (const auto& [_, it] : services) {
+    const bool encoder = util::ends_with(it.name, "EncodeData");
+    const bool livestream_encoder = util::starts_with(it.name, "livestream");
+    if (!it.should_log && (!encoder || livestream_encoder)) continue;
+    LOGD("logging %s (on port %d)", it.name.c_str(), it.port);
 
     SubSocket * sock = SubSocket::create(ctx.get(), it.name);
     assert(sock != NULL);
@@ -216,7 +222,7 @@ void loggerd_thread() {
       .counter = 0,
       .freq = it.decimation,
       .encoder = encoder,
-      .user_flag = (strcmp(it.name, "userFlag") == 0),
+      .user_flag = it.name == "userFlag",
     };
   }
 
@@ -228,7 +234,7 @@ void loggerd_thread() {
 
   std::map<std::string, EncoderInfo> encoder_infos_dict;
   for (const auto &cam : cameras_logged) {
-    for (const auto &encoder_info: cam.encoder_infos) {
+    for (const auto &encoder_info : cam.encoder_infos) {
       encoder_infos_dict[encoder_info.publish_name] = encoder_info;
       s.max_waiting++;
     }
@@ -264,7 +270,7 @@ void loggerd_thread() {
 
         if ((++msg_count % 1000) == 0) {
           double seconds = (millis_since_boot() - start_ts) / 1000.0;
-          LOGD("%lu messages, %.2f msg/sec, %.2f KB/sec", msg_count, msg_count / seconds, bytes_count * 0.001 / seconds);
+          LOGD("%" PRIu64 " messages, %.2f msg/sec, %.2f KB/sec", msg_count, msg_count / seconds, bytes_count * 0.001 / seconds);
         }
 
         count++;
diff --git a/system/loggerd/loggerd.h b/system/loggerd/loggerd.h
index 4100f12f8d..cfc06c28d3 100644
--- a/system/loggerd/loggerd.h
+++ b/system/loggerd/loggerd.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <vector>
+
 #include "cereal/messaging/messaging.h"
 #include "cereal/services.h"
 #include "cereal/visionipc/visionipc_client.h"
@@ -12,7 +14,9 @@
 #include "system/loggerd/logger.h"
 
 constexpr int MAIN_FPS = 20;
-const int MAIN_BITRATE = 10000000;
+const int MAIN_BITRATE = 1e7;
+const int LIVESTREAM_BITRATE = 1e6;
+const int QCAM_BITRATE = 256000;
 
 #define NO_CAMERA_PATIENCE 500  // fall back to time-based rotation if all cameras are dead
 
@@ -26,11 +30,10 @@ const int SEGMENT_LENGTH = LOGGERD_TEST ? atoi(getenv("LOGGERD_SEGMENT_LENGTH"))
 
 constexpr char PRESERVE_ATTR_NAME[] = "user.preserve";
 constexpr char PRESERVE_ATTR_VALUE = '1';
-
 class EncoderInfo {
 public:
   const char *publish_name;
-  const char *filename;
+  const char *filename = NULL;
   bool record = true;
   int frame_width = 1928;
   int frame_height = 1208;
@@ -57,11 +60,13 @@ const EncoderInfo main_road_encoder_info = {
   .filename = "fcamera.hevc",
   INIT_ENCODE_FUNCTIONS(RoadEncode),
 };
+
 const EncoderInfo main_wide_road_encoder_info = {
   .publish_name = "wideRoadEncodeData",
   .filename = "ecamera.hevc",
   INIT_ENCODE_FUNCTIONS(WideRoadEncode),
 };
+
 const EncoderInfo main_driver_encoder_info = {
   .publish_name = "driverEncodeData",
   .filename = "dcamera.hevc",
@@ -69,17 +74,40 @@ const EncoderInfo main_driver_encoder_info = {
   INIT_ENCODE_FUNCTIONS(DriverEncode),
 };
 
+const EncoderInfo stream_road_encoder_info = {
+  .publish_name = "livestreamRoadEncodeData",
+  .encode_type = cereal::EncodeIndex::Type::QCAMERA_H264,
+  .record = false,
+  .bitrate = LIVESTREAM_BITRATE,
+  INIT_ENCODE_FUNCTIONS(LivestreamRoadEncode),
+};
+
+const EncoderInfo stream_wide_road_encoder_info = {
+  .publish_name = "livestreamWideRoadEncodeData",
+  .encode_type = cereal::EncodeIndex::Type::QCAMERA_H264,
+  .record = false,
+  .bitrate = LIVESTREAM_BITRATE,
+  INIT_ENCODE_FUNCTIONS(LivestreamWideRoadEncode),
+};
+
+const EncoderInfo stream_driver_encoder_info = {
+  .publish_name = "livestreamDriverEncodeData",
+  .encode_type = cereal::EncodeIndex::Type::QCAMERA_H264,
+  .record = false,
+  .bitrate = LIVESTREAM_BITRATE,
+  INIT_ENCODE_FUNCTIONS(LivestreamDriverEncode),
+};
+
 const EncoderInfo qcam_encoder_info = {
   .publish_name = "qRoadEncodeData",
   .filename = "qcamera.ts",
-  .bitrate = 256000,
+  .bitrate = QCAM_BITRATE,
   .encode_type = cereal::EncodeIndex::Type::QCAMERA_H264,
   .frame_width = 526,
   .frame_height = 330,
   INIT_ENCODE_FUNCTIONS(QRoadEncode),
 };
 
-
 const LogCameraInfo road_camera_info{
   .thread_name = "road_cam_encoder",
   .type = RoadCam,
@@ -101,4 +129,26 @@ const LogCameraInfo driver_camera_info{
   .encoder_infos = {main_driver_encoder_info}
 };
 
+const LogCameraInfo stream_road_camera_info{
+  .thread_name = "road_cam_encoder",
+  .type = RoadCam,
+  .stream_type = VISION_STREAM_ROAD,
+  .encoder_infos = {stream_road_encoder_info}
+};
+
+const LogCameraInfo stream_wide_road_camera_info{
+  .thread_name = "wide_road_cam_encoder",
+  .type = WideRoadCam,
+  .stream_type = VISION_STREAM_WIDE_ROAD,
+  .encoder_infos = {stream_wide_road_encoder_info}
+};
+
+const LogCameraInfo stream_driver_camera_info{
+  .thread_name = "driver_cam_encoder",
+  .type = DriverCam,
+  .stream_type = VISION_STREAM_DRIVER,
+  .encoder_infos = {stream_driver_encoder_info}
+};
+
 const LogCameraInfo cameras_logged[] = {road_camera_info, wide_road_camera_info, driver_camera_info};
+const LogCameraInfo stream_cameras_logged[] = {stream_road_camera_info, stream_wide_road_camera_info, stream_driver_camera_info};
diff --git a/system/loggerd/uploader.py b/system/loggerd/uploader.py
old mode 100644
new mode 100755
index 245e5cbcf9..12c6ecdca9
--- a/system/loggerd/uploader.py
+++ b/system/loggerd/uploader.py
@@ -13,13 +13,13 @@ from typing import BinaryIO, Iterator, List, Optional, Tuple, Union
 
 from cereal import log
 import cereal.messaging as messaging
-from common.api import Api
-from common.params import Params
-from common.realtime import set_core_affinity
-from system.hardware import TICI
-from system.loggerd.xattr_cache import getxattr, setxattr
-from system.loggerd.config import ROOT
-from system.swaglog import cloudlog
+from openpilot.common.api import Api
+from openpilot.common.params import Params
+from openpilot.common.realtime import set_core_affinity
+from openpilot.system.hardware import TICI
+from openpilot.system.hardware.hw import Paths
+from openpilot.system.loggerd.xattr_cache import getxattr, setxattr
+from openpilot.system.swaglog import cloudlog
 
 NetworkType = log.DeviceState.NetworkType
 UPLOAD_ATTR_NAME = 'user.upload'
@@ -46,7 +46,7 @@ class FakeResponse:
 UploadResponse = Union[requests.Response, FakeResponse]
 
 def get_directory_sort(d: str) -> List[str]:
-  return list(map(lambda s: s.rjust(10, '0'), d.rsplit('--', 1)))
+  return [s.rjust(10, '0') for s in d.rsplit('--', 1)]
 
 def listdir_by_creation(d: str) -> List[str]:
   try:
@@ -211,7 +211,8 @@ class Uploader:
         else:
           content_length = int(stat.request.headers.get("Content-Length", 0))
           self.last_speed = (content_length / 1e6) / self.last_time
-          cloudlog.event("upload_success", key=key, fn=fn, sz=sz, content_length=content_length, network_type=network_type, metered=metered, speed=self.last_speed)
+          cloudlog.event("upload_success", key=key, fn=fn, sz=sz, content_length=content_length,
+                         network_type=network_type, metered=metered, speed=self.last_speed)
         success = True
       else:
         success = False
@@ -243,7 +244,7 @@ def uploader_fn(exit_event: threading.Event) -> None:
   except Exception:
     cloudlog.exception("failed to set core affinity")
 
-  clear_locks(ROOT)
+  clear_locks(Paths.log_root())
 
   params = Params()
   dongle_id = params.get("DongleId", encoding='utf8')
@@ -257,7 +258,7 @@ def uploader_fn(exit_event: threading.Event) -> None:
 
   sm = messaging.SubMaster(['deviceState'])
   pm = messaging.PubMaster(['uploaderState'])
-  uploader = Uploader(dongle_id, ROOT)
+  uploader = Uploader(dongle_id, Paths.log_root())
 
   backoff = 0.1
   while not exit_event.is_set():
diff --git a/system/loggerd/video_writer.cc b/system/loggerd/video_writer.cc
index 91bf09355f..90b5f1af3d 100644
--- a/system/loggerd/video_writer.cc
+++ b/system/loggerd/video_writer.cc
@@ -1,6 +1,5 @@
 #pragma clang diagnostic ignored "-Wdeprecated-declarations"
 #include <cassert>
-#include <cstdlib>
 
 #include "system/loggerd/video_writer.h"
 #include "common/swaglog.h"
@@ -8,7 +7,6 @@
 
 VideoWriter::VideoWriter(const char *path, const char *filename, bool remuxing, int width, int height, int fps, cereal::EncodeIndex::Type codec)
   : remuxing(remuxing) {
-  raw = codec == cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS;
   vid_path = util::string_format("%s/%s", path, filename);
   lock_path = util::string_format("%s/%s.lock", path, filename);
 
@@ -18,6 +16,7 @@ VideoWriter::VideoWriter(const char *path, const char *filename, bool remuxing,
 
   LOGD("encoder_open %s remuxing:%d", this->vid_path.c_str(), this->remuxing);
   if (this->remuxing) {
+    bool raw = (codec == cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS);
     avformat_alloc_output_context2(&this->ofmt_ctx, NULL, raw ? "matroska" : NULL, this->vid_path.c_str());
     assert(this->ofmt_ctx);
 
@@ -89,7 +88,7 @@ void VideoWriter::write(uint8_t *data, int len, long long timestamp, bool codecc
 
       // TODO: can use av_write_frame for non raw?
       int err = av_interleaved_write_frame(ofmt_ctx, &pkt);
-      if (err < 0) { LOGW("ts encoder write issue len: %d ts: %lu", len, timestamp); }
+      if (err < 0) { LOGW("ts encoder write issue len: %d ts: %lld", len, timestamp); }
 
       av_packet_unref(&pkt);
     }
@@ -98,7 +97,6 @@ void VideoWriter::write(uint8_t *data, int len, long long timestamp, bool codecc
 
 VideoWriter::~VideoWriter() {
   if (this->remuxing) {
-    if (this->raw) { avcodec_close(this->codec_ctx); }
     int err = av_write_trailer(this->ofmt_ctx);
     if (err != 0) LOGE("av_write_trailer failed %d", err);
     avcodec_free_context(&this->codec_ctx);
diff --git a/system/loggerd/video_writer.h b/system/loggerd/video_writer.h
index 01a243904c..1aa758b42b 100644
--- a/system/loggerd/video_writer.h
+++ b/system/loggerd/video_writer.h
@@ -16,11 +16,10 @@ public:
   ~VideoWriter();
 private:
   std::string vid_path, lock_path;
-
   FILE *of = nullptr;
 
   AVCodecContext *codec_ctx;
   AVFormatContext *ofmt_ctx;
   AVStream *out_stream;
-  bool remuxing, raw;
-};
\ No newline at end of file
+  bool remuxing;
+};
diff --git a/system/logmessaged.py b/system/logmessaged.py
index 04101d042b..c53e20e483 100755
--- a/system/logmessaged.py
+++ b/system/logmessaged.py
@@ -3,8 +3,9 @@ import zmq
 from typing import NoReturn
 
 import cereal.messaging as messaging
-from common.logging_extra import SwagLogFileFormatter
-from system.swaglog import get_file_handler
+from openpilot.common.logging_extra import SwagLogFileFormatter
+from openpilot.system.hardware.hw import Paths
+from openpilot.system.swaglog import get_file_handler
 
 
 def main() -> NoReturn:
@@ -14,7 +15,7 @@ def main() -> NoReturn:
 
   ctx = zmq.Context.instance()
   sock = ctx.socket(zmq.PULL)
-  sock.bind("ipc:///tmp/logmessage")
+  sock.bind(Paths.swaglog_ipc())
 
   # and we publish them
   log_message_sock = messaging.pub_sock('logMessage')
diff --git a/system/micd.py b/system/micd.py
index 97ba0c262e..72f3b8b490 100755
--- a/system/micd.py
+++ b/system/micd.py
@@ -2,9 +2,9 @@
 import numpy as np
 
 from cereal import messaging
-from common.filter_simple import FirstOrderFilter
-from common.realtime import Ratekeeper
-from system.swaglog import cloudlog
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.realtime import Ratekeeper
+from openpilot.system.swaglog import cloudlog
 
 RATE = 10
 FFT_SAMPLES = 4096
@@ -40,9 +40,9 @@ def apply_a_weighting(measurements: np.ndarray) -> np.ndarray:
 
 
 class Mic:
-  def __init__(self, pm):
-    self.pm = pm
+  def __init__(self):
     self.rk = Ratekeeper(RATE)
+    self.pm = messaging.PubMaster(['microphone'])
 
     self.measurements = np.empty(0)
 
@@ -85,7 +85,7 @@ class Mic:
 
   def micd_thread(self):
     # sounddevice must be imported after forking processes
-    import sounddevice as sd  # pylint: disable=import-outside-toplevel
+    import sounddevice as sd
 
     with sd.InputStream(channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream:
       cloudlog.info(f"micd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}")
@@ -93,11 +93,8 @@ class Mic:
         self.update()
 
 
-def main(pm=None):
-  if pm is None:
-    pm = messaging.PubMaster(['microphone'])
-
-  mic = Mic(pm)
+def main():
+  mic = Mic()
   mic.micd_thread()
 
 
diff --git a/system/proclogd/SConscript b/system/proclogd/SConscript
index 1b94a32f1b..1f4b767011 100644
--- a/system/proclogd/SConscript
+++ b/system/proclogd/SConscript
@@ -2,5 +2,5 @@ Import('env', 'cereal', 'messaging', 'common')
 libs = [cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj', 'common', 'zmq', 'json11']
 env.Program('proclogd', ['main.cc', 'proclog.cc'], LIBS=libs)
 
-if GetOption('test'):
+if GetOption('extras'):
   env.Program('tests/test_proclog', ['tests/test_proclog.cc', 'proclog.cc'], LIBS=libs)
diff --git a/system/proclogd/main.cc b/system/proclogd/main.cc
index c4faa916d9..3f8a889eea 100644
--- a/system/proclogd/main.cc
+++ b/system/proclogd/main.cc
@@ -1,6 +1,7 @@
 
 #include <sys/resource.h>
 
+#include "common/ratekeeper.h"
 #include "common/util.h"
 #include "system/proclogd/proclog.h"
 
@@ -9,13 +10,15 @@ ExitHandler do_exit;
 int main(int argc, char **argv) {
   setpriority(PRIO_PROCESS, 0, -15);
 
+  RateKeeper rk("proclogd", 0.5);
   PubMaster publisher({"procLog"});
+
   while (!do_exit) {
     MessageBuilder msg;
     buildProcLogMessage(msg);
     publisher.send("procLog", msg);
 
-    util::sleep_for(2000);  // 2 secs
+    rk.keepTime();
   }
 
   return 0;
diff --git a/system/sensord/.gitignore b/system/sensord/.gitignore
index e9b8071b4b..e17675e254 100644
--- a/system/sensord/.gitignore
+++ b/system/sensord/.gitignore
@@ -1 +1 @@
-_sensord
+sensord
diff --git a/system/sensord/SConscript b/system/sensord/SConscript
index 8f26c00853..7974eb07ae 100644
--- a/system/sensord/SConscript
+++ b/system/sensord/SConscript
@@ -1,9 +1,7 @@
 Import('env', 'arch', 'common', 'cereal', 'messaging')
 
 sensors = [
-  'sensors/file_sensor.cc',
   'sensors/i2c_sensor.cc',
-  'sensors/light_sensor.cc',
   'sensors/bmx055_accel.cc',
   'sensors/bmx055_gyro.cc',
   'sensors/bmx055_magn.cc',
@@ -16,4 +14,4 @@ sensors = [
 libs = [common, cereal, messaging, 'capnp', 'zmq', 'kj', 'pthread']
 if arch == "larch64":
   libs.append('i2c')
-env.Program('_sensord', ['sensors_qcom2.cc'] + sensors, LIBS=libs)
+env.Program('sensord', ['sensors_qcom2.cc'] + sensors, LIBS=libs)
diff --git a/system/sensord/pigeond.py b/system/sensord/pigeond.py
index c9ad7ff22a..2e8f151d17 100755
--- a/system/sensord/pigeond.py
+++ b/system/sensord/pigeond.py
@@ -10,11 +10,11 @@ from datetime import datetime
 from typing import List, Optional, Tuple
 
 from cereal import messaging
-from common.params import Params
-from system.swaglog import cloudlog
-from system.hardware import TICI
-from common.gpio import gpio_init, gpio_set
-from system.hardware.tici.pins import GPIO
+from openpilot.common.params import Params
+from openpilot.system.swaglog import cloudlog
+from openpilot.system.hardware import TICI
+from openpilot.common.gpio import gpio_init, gpio_set
+from openpilot.system.hardware.tici.pins import GPIO
 
 UBLOX_TTY = "/dev/ttyHS0"
 
@@ -27,11 +27,11 @@ UBLOX_ASSIST_ACK = b"\xb5\x62\x13\x60\x08\x00"
 
 def set_power(enabled: bool) -> None:
   gpio_init(GPIO.UBLOX_SAFEBOOT_N, True)
-  gpio_init(GPIO.UBLOX_PWR_EN, True)
+  gpio_init(GPIO.GNSS_PWR_EN, True)
   gpio_init(GPIO.UBLOX_RST_N, True)
 
   gpio_set(GPIO.UBLOX_SAFEBOOT_N, True)
-  gpio_set(GPIO.UBLOX_PWR_EN, enabled)
+  gpio_set(GPIO.GNSS_PWR_EN, enabled)
   gpio_set(GPIO.UBLOX_RST_N, enabled)
 
 def add_ubx_checksum(msg: bytes) -> bytes:
diff --git a/system/sensord/rawgps/compare.py b/system/sensord/rawgps/compare.py
deleted file mode 100755
index b2f4259e64..0000000000
--- a/system/sensord/rawgps/compare.py
+++ /dev/null
@@ -1,66 +0,0 @@
-#!/usr/bin/env python3
-import cereal.messaging as messaging
-from laika import constants
-
-if __name__ == "__main__":
-  sm = messaging.SubMaster(['ubloxGnss', 'qcomGnss'])
-
-  meas = None
-  while 1:
-    sm.update()
-    if sm['ubloxGnss'].which() == "measurementReport":
-      meas = sm['ubloxGnss'].measurementReport.measurements
-    if not sm.updated['qcomGnss'] or meas is None:
-      continue
-    report = sm['qcomGnss'].measurementReport
-    if report.source not in [0, 1]:
-      continue
-    GLONASS = report.source == 1
-    recv_time = report.milliseconds / 1000
-
-    car = []
-    print("qcom has ", list(sorted([x.svId for x in report.sv])))
-    print("ublox has", list(sorted([x.svId for x in meas if x.gnssId == (6 if GLONASS else 0)])))
-    for i in report.sv:
-      # match to ublox
-      tm = None
-      for m in meas:
-        if i.svId == m.svId and m.gnssId == 0 and m.sigId == 0 and not GLONASS:
-          tm = m
-        if (i.svId-64) == m.svId and m.gnssId == 6 and m.sigId == 0 and GLONASS:
-          tm = m
-      if tm is None:
-        continue
-
-      if not i.measurementStatus.measurementNotUsable and i.measurementStatus.satelliteTimeIsKnown:
-        sat_time = (i.unfilteredMeasurementIntegral + i.unfilteredMeasurementFraction + i.latency) / 1000
-        ublox_psuedorange = tm.pseudorange
-        qcom_psuedorange = (recv_time - sat_time)*constants.SPEED_OF_LIGHT
-        if GLONASS:
-          glonass_freq = tm.glonassFrequencyIndex - 7
-          ublox_speed = -(constants.SPEED_OF_LIGHT / (constants.GLONASS_L1 + glonass_freq*constants.GLONASS_L1_DELTA)) * (tm.doppler)
-        else:
-          ublox_speed = -(constants.SPEED_OF_LIGHT / constants.GPS_L1) * tm.doppler
-        qcom_speed = i.unfilteredSpeed
-        car.append((i.svId, tm.pseudorange, ublox_speed, qcom_psuedorange, qcom_speed, tm.cno))
-
-    if len(car) == 0:
-      print("nothing to compare")
-      continue
-
-    pr_err, speed_err = 0., 0.
-    for c in car:
-      ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed = c[1:5]
-      pr_err += ublox_psuedorange - qcom_psuedorange
-      speed_err += ublox_speed - qcom_speed
-    pr_err /= len(car)
-    speed_err /= len(car)
-    print("avg psuedorange err %f avg speed err %f" % (pr_err, speed_err))
-    for c in sorted(car, key=lambda x: abs(x[1] - x[3] - pr_err)):
-      svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed, cno = c
-      print("svid: %3d  pseudorange: %10.2f m  speed: %8.2f m/s   meas: %12.2f  speed: %10.2f   meas_err: %10.3f speed_err: %8.3f cno: %d" %
-        (svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed,
-        ublox_psuedorange - qcom_psuedorange - pr_err, ublox_speed - qcom_speed - speed_err, cno))
-
-
-
diff --git a/system/sensord/rawgps/nmeaport.py b/system/sensord/rawgps/nmeaport.py
new file mode 100644
index 0000000000..01b9b179b9
--- /dev/null
+++ b/system/sensord/rawgps/nmeaport.py
@@ -0,0 +1,169 @@
+import os
+import sys
+from dataclasses import dataclass, fields
+from subprocess import check_output, CalledProcessError
+from time import sleep
+from typing import NoReturn
+
+DEBUG = int(os.environ.get("DEBUG", "0"))
+
+@dataclass
+class GnssClockNmeaPort:
+  # flags bit mask:
+  # 0x01 = leap_seconds valid
+  # 0x02 = time_uncertainty_ns valid
+  # 0x04 = full_bias_ns valid
+  # 0x08 = bias_ns valid
+  # 0x10 = bias_uncertainty_ns valid
+  # 0x20 = drift_nsps valid
+  # 0x40 = drift_uncertainty_nsps valid
+  flags: int
+  leap_seconds: int
+  time_ns: int
+  time_uncertainty_ns: int # 1-sigma
+  full_bias_ns: int
+  bias_ns: float
+  bias_uncertainty_ns: float # 1-sigma
+  drift_nsps: float
+  drift_uncertainty_nsps: float # 1-sigma
+
+  def __post_init__(self):
+    for field in fields(self):
+      val = getattr(self, field.name)
+      setattr(self, field.name, field.type(val) if val else None)
+
+@dataclass
+class GnssMeasNmeaPort:
+  messageCount: int
+  messageNum: int
+  svCount: int
+  # constellation enum:
+  # 1 = GPS
+  # 2 = SBAS
+  # 3 = GLONASS
+  # 4 = QZSS
+  # 5 = BEIDOU
+  # 6 = GALILEO
+  constellation: int
+  svId: int
+  flags: int # always zero
+  time_offset_ns: int
+  # state bit mask:
+  # 0x0001 = CODE LOCK
+  # 0x0002 = BIT SYNC
+  # 0x0004 = SUBFRAME SYNC
+  # 0x0008 = TIME OF WEEK DECODED
+  # 0x0010 = MSEC AMBIGUOUS
+  # 0x0020 = SYMBOL SYNC
+  # 0x0040 = GLONASS STRING SYNC
+  # 0x0080 = GLONASS TIME OF DAY DECODED
+  # 0x0100 = BEIDOU D2 BIT SYNC
+  # 0x0200 = BEIDOU D2 SUBFRAME SYNC
+  # 0x0400 = GALILEO E1BC CODE LOCK
+  # 0x0800 = GALILEO E1C 2ND CODE LOCK
+  # 0x1000 = GALILEO E1B PAGE SYNC
+  # 0x2000 = GALILEO E1B PAGE SYNC
+  state: int
+  time_of_week_ns: int
+  time_of_week_uncertainty_ns: int # 1-sigma
+  carrier_to_noise_ratio: float
+  pseudorange_rate: float
+  pseudorange_rate_uncertainty: float # 1-sigma
+
+  def __post_init__(self):
+    for field in fields(self):
+      val = getattr(self, field.name)
+      setattr(self, field.name, field.type(val) if val else None)
+
+def nmea_checksum_ok(s):
+  checksum = 0
+  for i, c in enumerate(s[1:]):
+    if c == "*":
+      if i != len(s) - 4: # should be 3rd to last character
+        print("ERROR: NMEA string does not have checksum delimiter in correct location:", s)
+        return False
+      break
+    checksum ^= ord(c)
+  else:
+    print("ERROR: NMEA string does not have checksum delimiter:", s)
+    return False
+
+  return True
+
+def process_nmea_port_messages(device:str="/dev/ttyUSB1") -> NoReturn:
+  while True:
+    try:
+      with open(device, "r") as nmeaport:
+        for line in nmeaport:
+          line = line.strip()
+          if DEBUG:
+            print(line)
+          if not line.startswith("$"): # all NMEA messages start with $
+            continue
+          if not nmea_checksum_ok(line):
+            continue
+
+          fields = line.split(",")
+          match fields[0]:
+            case "$GNCLK":
+              # fields at end are reserved (not used)
+              gnss_clock = GnssClockNmeaPort(*fields[1:10]) # type: ignore[arg-type]
+              print(gnss_clock)
+            case "$GNMEAS":
+              # fields at end are reserved (not used)
+              gnss_meas = GnssMeasNmeaPort(*fields[1:14]) # type: ignore[arg-type]
+              print(gnss_meas)
+    except Exception as e:
+      print(e)
+      sleep(1)
+
+def main() -> NoReturn:
+  from openpilot.common.gpio import gpio_init, gpio_set
+  from openpilot.system.hardware.tici.pins import GPIO
+  from openpilot.system.sensord.rawgps.rawgpsd import at_cmd
+
+  try:
+    check_output(["pidof", "rawgpsd"])
+    print("rawgpsd is running, please kill openpilot before running this script! (aborted)")
+    sys.exit(1)
+  except CalledProcessError as e:
+    if e.returncode != 1: # 1 == no process found (boardd not running)
+      raise e
+
+  print("power up antenna ...")
+  gpio_init(GPIO.GNSS_PWR_EN, True)
+  gpio_set(GPIO.GNSS_PWR_EN, True)
+
+  if b"+QGPS: 0" not in (at_cmd("AT+QGPS?") or b""):
+    print("stop location tracking ...")
+    at_cmd("AT+QGPSEND")
+
+  if b'+QGPSCFG: "outport",usbnmea' not in (at_cmd('AT+QGPSCFG="outport"') or b""):
+    print("configure outport ...")
+    at_cmd('AT+QGPSCFG="outport","usbnmea"') # usbnmea = /dev/ttyUSB1
+
+  if b'+QGPSCFG: "gnssrawdata",3,0' not in (at_cmd('AT+QGPSCFG="gnssrawdata"') or b""):
+    print("configure gnssrawdata ...")
+    # AT+QGPSCFG="gnssrawdata",<constellation-mask>,<port>'
+    # <constellation-mask> values:
+    # 0x01 = GPS
+    # 0x02 = GLONASS
+    # 0x04 = BEIDOU
+    # 0x08 = GALILEO
+    # 0x10 = QZSS
+    # <port> values:
+    # 0 = NMEA port
+    # 1 = AT port
+    at_cmd('AT+QGPSCFG="gnssrawdata",3,0') # enable all constellations, output data to NMEA port
+    print("rebooting ...")
+    at_cmd('AT+CFUN=1,1')
+    print("re-run this script when it is back up")
+    sys.exit(2)
+
+  print("starting location tracking ...")
+  at_cmd("AT+QGPS=1")
+
+  process_nmea_port_messages()
+
+if __name__ == "__main__":
+  main()
diff --git a/system/sensord/rawgps/rawgpsd.py b/system/sensord/rawgps/rawgpsd.py
index 5db6ad1cf5..787a9316d3 100755
--- a/system/sensord/rawgps/rawgpsd.py
+++ b/system/sensord/rawgps/rawgpsd.py
@@ -6,22 +6,20 @@ import itertools
 import math
 import time
 import pycurl
+import shutil
 import subprocess
-from datetime import datetime
+import datetime
 from multiprocessing import Process, Event
 from typing import NoReturn, Optional
 from struct import unpack_from, calcsize, pack
 
 from cereal import log
 import cereal.messaging as messaging
-from common.gpio import gpio_init, gpio_set
-from laika.gps_time import GPSTime, utc_to_gpst, get_leap_seconds
-from laika.helpers import get_prn_from_nmea_id
-from laika.constants import SECS_IN_HR, SECS_IN_DAY, SECS_IN_WEEK
-from system.hardware.tici.pins import GPIO
-from system.swaglog import cloudlog
-from system.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
-from system.sensord.rawgps.structs import (dict_unpacker, position_report, relist,
+from openpilot.common.gpio import gpio_init, gpio_set
+from openpilot.system.hardware.tici.pins import GPIO
+from openpilot.system.swaglog import cloudlog
+from openpilot.system.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
+from openpilot.system.sensord.rawgps.structs import (dict_unpacker, position_report, relist,
                                               gps_measurement_report, gps_measurement_report_sv,
                                               glonass_measurement_report, glonass_measurement_report_sv,
                                               oemdre_measurement_report, oemdre_measurement_report_sv, oemdre_svpoly_report,
@@ -144,9 +142,17 @@ def download_assistance():
 def downloader_loop(event):
   if os.path.exists(ASSIST_DATA_FILE):
     os.remove(ASSIST_DATA_FILE)
-  while not os.path.exists(ASSIST_DATA_FILE) and not event.is_set():
-    download_assistance()
-    time.sleep(10)
+
+  alt_path = os.getenv("QCOM_ALT_ASSISTANCE_PATH", None)
+  if alt_path is not None and os.path.exists(alt_path):
+    shutil.copyfile(alt_path, ASSIST_DATA_FILE)
+
+  try:
+    while not os.path.exists(ASSIST_DATA_FILE) and not event.is_set():
+      download_assistance()
+      event.wait(timeout=10)
+  except KeyboardInterrupt:
+    pass
 
 def inject_assistance():
   for _ in range(5):
@@ -181,7 +187,13 @@ def setup_quectel(diag: ModemDiag) -> bool:
 
   if gps_enabled():
     at_cmd("AT+QGPSEND")
-  #at_cmd("AT+QGPSDEL=0")
+
+  if "GPS_COLD_START" in os.environ:
+    # deletes all assistance
+    at_cmd("AT+QGPSDEL=0")
+  else:
+    # allow module to perform hot start
+    at_cmd("AT+QGPSDEL=1")
 
   # disable DPO power savings for more accuracy
   at_cmd("AT+QGPSCFG=\"dpoenable\",0")
@@ -196,7 +208,7 @@ def setup_quectel(diag: ModemDiag) -> bool:
     inject_assistance()
     os.remove(ASSIST_DATA_FILE)
   #at_cmd("AT+QGPSXTRADATA?")
-  time_str = datetime.utcnow().strftime("%Y/%m/%d,%H:%M:%S")
+  time_str = datetime.datetime.utcnow().strftime("%Y/%m/%d,%H:%M:%S")
   at_cmd(f"AT+QGPSXTRATIME=0,\"{time_str}\",1,1,1000")
 
   at_cmd("AT+QGPSCFG=\"outport\",\"usbnmea\"")
@@ -259,23 +271,28 @@ def main() -> NoReturn:
   stop_download_event = Event()
   assist_fetch_proc = Process(target=downloader_loop, args=(stop_download_event,))
   assist_fetch_proc.start()
-  def cleanup(proc):
+  def cleanup(sig, frame):
     cloudlog.warning("caught sig disabling quectel gps")
-    gpio_set(GPIO.UBLOX_PWR_EN, False)
+
+    gpio_set(GPIO.GNSS_PWR_EN, False)
     teardown_quectel(diag)
     cloudlog.warning("quectel cleanup done")
+
+    stop_download_event.set()
+    assist_fetch_proc.kill()
+    assist_fetch_proc.join()
+
     sys.exit(0)
-  signal.signal(signal.SIGINT, lambda sig, frame: cleanup(assist_fetch_proc))
-  signal.signal(signal.SIGTERM, lambda sig, frame: cleanup(assist_fetch_proc))
+  signal.signal(signal.SIGINT, cleanup)
+  signal.signal(signal.SIGTERM, cleanup)
 
   # connect to modem
   diag = ModemDiag()
   r = setup_quectel(diag)
   want_assistance = not r
-  current_gps_time = utc_to_gpst(GPSTime.from_datetime(datetime.utcnow()))
   cloudlog.warning("quectel setup done")
-  gpio_init(GPIO.UBLOX_PWR_EN, True)
-  gpio_set(GPIO.UBLOX_PWR_EN, True)
+  gpio_init(GPIO.GNSS_PWR_EN, True)
+  gpio_set(GPIO.GNSS_PWR_EN, True)
 
   pm = messaging.PubMaster(['qcomGnss', 'gpsLocation'])
 
@@ -345,8 +362,6 @@ def main() -> NoReturn:
               setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
           else:
             setattr(sv, k, v)
-      if report.source == log.QcomGnss.MeasurementSource.gps:
-        current_gps_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0)
       pm.send('qcomGnss', msg)
     elif log_type == LOG_GNSS_POSITION_REPORT:
       report = unpack_position(log_payload)
@@ -362,8 +377,12 @@ def main() -> NoReturn:
       gps.altitude = report["q_FltFinalPosAlt"]
       gps.speed = math.sqrt(sum([x**2 for x in vNED]))
       gps.bearingDeg = report["q_FltHeadingRad"] * 180/math.pi
-      gps.unixTimestampMillis = GPSTime(report['w_GpsWeekNumber'],
-                                        1e-3*report['q_GpsFixTimeMs']).as_unix_timestamp()*1e3
+
+      # TODO needs update if there is another leap second, after june 2024?
+      dt_timestamp = (datetime.datetime(1980, 1, 6, 0, 0, 0, 0, None) +
+                      datetime.timedelta(weeks=report['w_GpsWeekNumber']) +
+                      datetime.timedelta(seconds=(1e-3*report['q_GpsFixTimeMs'] - 18)))
+      gps.unixTimestampMillis = dt_timestamp.timestamp()*1e3
       gps.source = log.GpsLocationData.SensorSource.qcomdiag
       gps.vNED = vNED
       gps.verticalAccuracy = report["q_FltVdop"]
@@ -374,8 +393,6 @@ def main() -> NoReturn:
       if gps.flags:
         want_assistance = False
         stop_download_event.set()
-
-
       pm.send('gpsLocation', msg)
 
     elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT:
@@ -394,6 +411,10 @@ def main() -> NoReturn:
         else:
           setattr(poly, k, v)
 
+      '''
+      # Timestamp glonass polys with GPSTime
+      from laika.gps_time import GPSTime, utc_to_gpst, get_leap_seconds
+      from laika.helpers import get_prn_from_nmea_id
       prn = get_prn_from_nmea_id(poly.svId)
       if prn[0] == 'R':
         epoch = GPSTime(current_gps_time.week, (poly.t0 - 3*SECS_IN_HR + SECS_IN_DAY) % (SECS_IN_WEEK) + get_leap_seconds(current_gps_time))
@@ -408,6 +429,7 @@ def main() -> NoReturn:
 
       poly.gpsWeek = epoch.week
       poly.gpsTow = epoch.tow
+      '''
       pm.send('qcomGnss', msg)
 
     elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]:
@@ -431,7 +453,7 @@ def main() -> NoReturn:
         report.source = 1  # glonass
         measurement_status_fields = (measurementStatusFields.items(), measurementStatusGlonassFields.items())
       else:
-        assert False
+        raise RuntimeError(f"invalid log_type: {log_type}")
 
       for k,v in dat.items():
         if k == "version":
diff --git a/system/sensord/rawgps/structs.py b/system/sensord/rawgps/structs.py
index 97e3d3d605..bde4e0049c 100644
--- a/system/sensord/rawgps/structs.py
+++ b/system/sensord/rawgps/structs.py
@@ -255,7 +255,7 @@ position_report = """
   uint32      q_FltRawAlt;              /* Raw height-above-ellipsoid altitude in meters as computed by WLS */
   uint32      q_FltRawAltSigma;         /* Gaussian 1-sigma value for raw height-above-ellipsoid altitude in meters */
   uint32   align_Flt[14];
-  uint32      q_FltPdop;                /* 3D position dilution of precision as computed from the unweighted 
+  uint32      q_FltPdop;                /* 3D position dilution of precision as computed from the unweighted
   uint32      q_FltHdop;                /* Horizontal position dilution of precision as computed from the unweighted least-squares covariance matrix */
   uint32      q_FltVdop;                /* Vertical position dilution of precision as computed from the unweighted least-squares covariance matrix */
   uint8       u_EllipseConfidence;      /* Statistical measure of the confidence (percentage) associated with the uncertainty ellipse values */
@@ -276,7 +276,7 @@ position_report = """
   uint8       u_TotalGloSvs;            /* Total number of Glonass SVs detected by searcher, including ones not used in position calculation */
   uint8       u_NumBdsSvsUsed;          /* The number of BeiDou SVs used in the fix */
   uint8       u_TotalBdsSvs;            /* Total number of BeiDou SVs detected by searcher, including ones not used in position calculation */
-"""
+""" # noqa: E501
 
 def name_to_camelcase(nam):
   ret = []
@@ -317,8 +317,7 @@ def parse_struct(ss):
     elif typ in ["uint64", "uint64_t"]:
       st += "Q"
     else:
-      print("unknown type", typ)
-      assert False
+      raise RuntimeError(f"unknown type {typ}")
     if '[' in nam:
       cnt = int(nam.split("[")[1].split("]")[0])
       st += st[-1]*(cnt-1)
@@ -333,7 +332,7 @@ def dict_unpacker(ss, camelcase = False):
   if camelcase:
     nams = [name_to_camelcase(x) for x in nams]
   sz = calcsize(st)
-  return lambda x: dict(zip(nams, unpack_from(st, x))), sz
+  return lambda x: dict(zip(nams, unpack_from(st, x), strict=True)), sz
 
 def relist(dat):
   list_keys = set()
diff --git a/system/sensord/rawgps/test_rawgps.py b/system/sensord/rawgps/test_rawgps.py
index 8c2e246764..3642438132 100755
--- a/system/sensord/rawgps/test_rawgps.py
+++ b/system/sensord/rawgps/test_rawgps.py
@@ -1,27 +1,23 @@
 #!/usr/bin/env python3
 import os
+import pytest
 import json
 import time
 import datetime
 import unittest
 import subprocess
-import numpy as np
 
 import cereal.messaging as messaging
-from system.hardware import TICI
-from system.sensord.rawgps.rawgpsd import at_cmd, wait_for_modem
-from selfdrive.manager.process_config import managed_processes
-from common.transformations.coordinates import ecef_from_geodetic
+from openpilot.system.sensord.rawgps.rawgpsd import at_cmd, wait_for_modem
+from openpilot.selfdrive.manager.process_config import managed_processes
 
 GOOD_SIGNAL = bool(int(os.getenv("GOOD_SIGNAL", '0')))
 
 
+@pytest.mark.tici
 class TestRawgpsd(unittest.TestCase):
   @classmethod
   def setUpClass(cls):
-    if not TICI:
-      raise unittest.SkipTest
-
     os.system("sudo systemctl start systemd-resolved")
     os.system("sudo systemctl restart ModemManager lte")
     wait_for_modem()
@@ -123,22 +119,5 @@ class TestRawgpsd(unittest.TestCase):
     managed_processes['rawgpsd'].stop()
     self.check_assistance(True)
 
-  @unittest.skipIf(not GOOD_SIGNAL, "No good GPS signal")
-  def test_fix(self):
-    managed_processes['rawgpsd'].start()
-    managed_processes['laikad'].start()
-    assert self._wait_for_output(60)
-    assert self.sm.updated['qcomGnss']
-    assert self.sm.updated['gpsLocation']
-    assert self.sm['gpsLocation'].flags == 1
-    module_fix = ecef_from_geodetic([self.sm['gpsLocation'].latitude,
-                                     self.sm['gpsLocation'].longitude,
-                                     self.sm['gpsLocation'].altitude])
-    assert self.sm['gnssMeasurements'].positionECEF.valid
-    total_diff = np.array(self.sm['gnssMeasurements'].positionECEF.value) - module_fix
-    self.assertLess(np.linalg.norm(total_diff), 100)
-    managed_processes['laikad'].stop()
-    managed_processes['rawgpsd'].stop()
-
 if __name__ == "__main__":
   unittest.main(failfast=True)
diff --git a/system/sensord/sensord b/system/sensord/sensord
deleted file mode 100755
index 9ea848d577..0000000000
--- a/system/sensord/sensord
+++ /dev/null
@@ -1,4 +0,0 @@
-#!/bin/sh
-cd "$(dirname "$0")"
-export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH"
-exec ./_sensord
diff --git a/system/sensord/sensors/bmx055_accel.cc b/system/sensord/sensors/bmx055_accel.cc
index 0c48d1e3ba..bdb0113de3 100644
--- a/system/sensord/sensors/bmx055_accel.cc
+++ b/system/sensord/sensors/bmx055_accel.cc
@@ -1,4 +1,4 @@
-#include "bmx055_accel.h"
+#include "system/sensord/sensors/bmx055_accel.h"
 
 #include <cassert>
 
diff --git a/system/sensord/sensors/bmx055_gyro.cc b/system/sensord/sensors/bmx055_gyro.cc
index ba41f3b47c..411b2f445e 100644
--- a/system/sensord/sensors/bmx055_gyro.cc
+++ b/system/sensord/sensors/bmx055_gyro.cc
@@ -1,4 +1,4 @@
-#include "bmx055_gyro.h"
+#include "system/sensord/sensors/bmx055_gyro.h"
 
 #include <cassert>
 #include <cmath>
diff --git a/system/sensord/sensors/bmx055_magn.cc b/system/sensord/sensors/bmx055_magn.cc
index 7716ce25c0..3d0d3d2fc6 100644
--- a/system/sensord/sensors/bmx055_magn.cc
+++ b/system/sensord/sensors/bmx055_magn.cc
@@ -1,4 +1,4 @@
-#include "bmx055_magn.h"
+#include "system/sensord/sensors/bmx055_magn.h"
 
 #include <unistd.h>
 
@@ -77,7 +77,7 @@ int BMX055_Magn::init() {
 
   // suspend -> sleep
   int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01);
-  if(ret < 0) {
+  if (ret < 0) {
     LOGE("Enabling power failed: %d", ret);
     goto fail;
   }
@@ -90,21 +90,21 @@ int BMX055_Magn::init() {
 
   // Load magnetometer trim
   ret = read_register(BMX055_MAGN_I2C_REG_DIG_X1, trim_x1y1, 2);
-  if(ret < 0) goto fail;
+  if (ret < 0) goto fail;
   ret = read_register(BMX055_MAGN_I2C_REG_DIG_X2, trim_x2y2, 2);
-  if(ret < 0) goto fail;
+  if (ret < 0) goto fail;
   ret = read_register(BMX055_MAGN_I2C_REG_DIG_XY2, trim_xy1xy2, 2);
-  if(ret < 0) goto fail;
+  if (ret < 0) goto fail;
   ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z1_LSB, trim_z1, 2);
-  if(ret < 0) goto fail;
+  if (ret < 0) goto fail;
   ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z2_LSB, trim_z2, 2);
-  if(ret < 0) goto fail;
+  if (ret < 0) goto fail;
   ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z3_LSB, trim_z3, 2);
-  if(ret < 0) goto fail;
+  if (ret < 0) goto fail;
   ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z4_LSB, trim_z4, 2);
-  if(ret < 0) goto fail;
+  if (ret < 0) goto fail;
   ret = read_register(BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB, trim_xyz1, 2);
-  if(ret < 0) goto fail;
+  if (ret < 0) goto fail;
 
   // Read trim data
   trim_data.dig_x1 = trim_x1y1[0];
@@ -171,17 +171,17 @@ bool BMX055_Magn::perform_self_test() {
   uint8_t forced = BMX055_MAGN_FORCED;
 
   // Negative current
-	set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b10) << 6));
-	util::sleep_for(100);
+  set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b10) << 6));
+  util::sleep_for(100);
 
-	read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
-	parse_xyz(buffer, &x, &y, &neg_z);
+  read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
+  parse_xyz(buffer, &x, &y, &neg_z);
 
   // Positive current
-	set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b11) << 6));
-	util::sleep_for(100);
+  set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b11) << 6));
+  util::sleep_for(100);
 
-	read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
+  read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
   parse_xyz(buffer, &x, &y, &pos_z);
 
   // Put back in normal mode
@@ -206,9 +206,9 @@ bool BMX055_Magn::parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *
     uint16_t data_r = (uint16_t) (((uint16_t)buffer[7] << 8) | buffer[6]) >> 2;
     assert(data_r != 0);
 
-		*x = compensate_x(trim_data, mdata_x, data_r);
-		*y = compensate_y(trim_data, mdata_y, data_r);
-		*z = compensate_z(trim_data, mdata_z, data_r);
+    *x = compensate_x(trim_data, mdata_x, data_r);
+    *y = compensate_y(trim_data, mdata_y, data_r);
+    *z = compensate_z(trim_data, mdata_z, data_r);
   }
   return ready;
 }
diff --git a/system/sensord/sensors/bmx055_temp.cc b/system/sensord/sensors/bmx055_temp.cc
index 68ee0da1d6..da7b86476c 100644
--- a/system/sensord/sensors/bmx055_temp.cc
+++ b/system/sensord/sensors/bmx055_temp.cc
@@ -1,4 +1,4 @@
-#include "bmx055_temp.h"
+#include "system/sensord/sensors/bmx055_temp.h"
 
 #include <cassert>
 
diff --git a/system/sensord/sensors/file_sensor.cc b/system/sensord/sensors/file_sensor.cc
deleted file mode 100644
index a74ae1ae1e..0000000000
--- a/system/sensord/sensors/file_sensor.cc
+++ /dev/null
@@ -1,17 +0,0 @@
-#include "file_sensor.h"
-
-#include <string>
-
-FileSensor::FileSensor(std::string filename) : file(filename) {}
-
-int FileSensor::init() {
-  return file.is_open() ? 0 : 1;
-}
-
-FileSensor::~FileSensor() {
-  file.close();
-}
-
-bool FileSensor::has_interrupt_enabled() {
-  return false;
-}
\ No newline at end of file
diff --git a/system/sensord/sensors/file_sensor.h b/system/sensord/sensors/file_sensor.h
deleted file mode 100644
index 07d7e8f946..0000000000
--- a/system/sensord/sensors/file_sensor.h
+++ /dev/null
@@ -1,19 +0,0 @@
-#pragma once
-
-#include <fstream>
-#include <string>
-
-#include "cereal/gen/cpp/log.capnp.h"
-#include "system/sensord/sensors/sensor.h"
-
-class FileSensor : public Sensor {
-protected:
-  std::ifstream file;
-
-public:
-  FileSensor(std::string filename);
-  ~FileSensor();
-  int init();
-  bool has_interrupt_enabled();
-  virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
-};
diff --git a/system/sensord/sensors/i2c_sensor.cc b/system/sensord/sensors/i2c_sensor.cc
index f563f93d2b..90220f551d 100644
--- a/system/sensord/sensors/i2c_sensor.cc
+++ b/system/sensord/sensors/i2c_sensor.cc
@@ -1,4 +1,4 @@
-#include "i2c_sensor.h"
+#include "system/sensord/sensors/i2c_sensor.h"
 
 int16_t read_12_bit(uint8_t lsb, uint8_t msb) {
   uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb & 0xF0);
diff --git a/system/sensord/sensors/i2c_sensor.h b/system/sensord/sensors/i2c_sensor.h
index ccac526c12..ba100c3b01 100644
--- a/system/sensord/sensors/i2c_sensor.h
+++ b/system/sensord/sensors/i2c_sensor.h
@@ -2,6 +2,7 @@
 
 #include <cstdint>
 #include <unistd.h>
+#include <vector>
 #include "cereal/gen/cpp/log.capnp.h"
 
 #include "common/i2c.h"
diff --git a/system/sensord/sensors/light_sensor.cc b/system/sensord/sensors/light_sensor.cc
deleted file mode 100644
index 99e321b47d..0000000000
--- a/system/sensord/sensors/light_sensor.cc
+++ /dev/null
@@ -1,27 +0,0 @@
-#include "light_sensor.h"
-
-#include <string>
-
-#include "common/timing.h"
-#include "system/sensord/sensors/constants.h"
-
-LightSensor::LightSensor(std::string filename) : FileSensor(filename) {}
-
-bool LightSensor::get_event(MessageBuilder &msg, uint64_t ts) {
-  uint64_t start_time = nanos_since_boot();
-  file.clear();
-  file.seekg(0);
-
-  int value;
-  file >> value;
-
-  auto event = msg.initEvent().initLightSensor();
-  event.setSource(cereal::SensorEventData::SensorSource::RPR0521);
-  event.setVersion(1);
-  event.setSensor(SENSOR_LIGHT);
-  event.setType(SENSOR_TYPE_LIGHT);
-  event.setTimestamp(start_time);
-  event.setLight(value);
-
-  return true;
-}
diff --git a/system/sensord/sensors/light_sensor.h b/system/sensord/sensors/light_sensor.h
deleted file mode 100644
index 7ed1c1f70c..0000000000
--- a/system/sensord/sensors/light_sensor.h
+++ /dev/null
@@ -1,9 +0,0 @@
-#pragma once
-#include "file_sensor.h"
-
-class LightSensor : public FileSensor {
-public:
-  LightSensor(std::string filename);
-  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
-  int shutdown() { return 0; }
-};
diff --git a/system/sensord/sensors/lsm6ds3_accel.cc b/system/sensord/sensors/lsm6ds3_accel.cc
index 2a09702c96..03533e0657 100644
--- a/system/sensord/sensors/lsm6ds3_accel.cc
+++ b/system/sensord/sensors/lsm6ds3_accel.cc
@@ -1,4 +1,4 @@
-#include "lsm6ds3_accel.h"
+#include "system/sensord/sensors/lsm6ds3_accel.h"
 
 #include <cassert>
 #include <cmath>
@@ -121,7 +121,7 @@ int LSM6DS3_Accel::init() {
   uint8_t value = 0;
   bool do_self_test = false;
 
-  const char* env_lsm_selftest =env_lsm_selftest = std::getenv("LSM_SELF_TEST");
+  const char* env_lsm_selftest = std::getenv("LSM_SELF_TEST");
   if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) {
     do_self_test = true;
   }
diff --git a/system/sensord/sensors/lsm6ds3_gyro.cc b/system/sensord/sensors/lsm6ds3_gyro.cc
index 9bc43485af..0459b6ad64 100644
--- a/system/sensord/sensors/lsm6ds3_gyro.cc
+++ b/system/sensord/sensors/lsm6ds3_gyro.cc
@@ -1,4 +1,4 @@
-#include "lsm6ds3_gyro.h"
+#include "system/sensord/sensors/lsm6ds3_gyro.h"
 
 #include <cassert>
 #include <cmath>
@@ -128,7 +128,7 @@ int LSM6DS3_Gyro::init() {
   }
 
   ret = self_test(LSM6DS3_GYRO_POSITIVE_TEST);
-  if (ret < 0 ) {
+  if (ret < 0) {
     LOGE("LSM6DS3 gyro positive self-test failed!");
     if (do_self_test) goto fail;
   }
diff --git a/system/sensord/sensors/lsm6ds3_temp.cc b/system/sensord/sensors/lsm6ds3_temp.cc
index c2e2c83c1d..f481614154 100644
--- a/system/sensord/sensors/lsm6ds3_temp.cc
+++ b/system/sensord/sensors/lsm6ds3_temp.cc
@@ -1,4 +1,4 @@
-#include "lsm6ds3_temp.h"
+#include "system/sensord/sensors/lsm6ds3_temp.h"
 
 #include <cassert>
 
diff --git a/system/sensord/sensors/mmc5603nj_magn.cc b/system/sensord/sensors/mmc5603nj_magn.cc
index 048095786e..0e8ba967e3 100644
--- a/system/sensord/sensors/mmc5603nj_magn.cc
+++ b/system/sensord/sensors/mmc5603nj_magn.cc
@@ -1,9 +1,12 @@
-#include "mmc5603nj_magn.h"
+#include "system/sensord/sensors/mmc5603nj_magn.h"
 
+#include <algorithm>
 #include <cassert>
+#include <vector>
 
 #include "common/swaglog.h"
 #include "common/timing.h"
+#include "common/util.h"
 
 MMC5603NJ_Magn::MMC5603NJ_Magn(I2CBus *bus) : I2CSensor(bus) {}
 
@@ -11,8 +14,8 @@ int MMC5603NJ_Magn::init() {
   int ret = verify_chip_id(MMC5603NJ_I2C_REG_ID, {MMC5603NJ_CHIP_ID});
   if (ret == -1) return -1;
 
-  // Set 100 Hz
-  ret = set_register(MMC5603NJ_I2C_REG_ODR, 100);
+  // Set ODR to 0
+  ret = set_register(MMC5603NJ_I2C_REG_ODR, 0);
   if (ret < 0) {
     goto fail;
   }
@@ -23,18 +26,6 @@ int MMC5603NJ_Magn::init() {
     goto fail;
   }
 
-  // Set compute measurement rate
-  ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_CMM_FREQ_EN | MMC5603NJ_AUTO_SR_EN);
-  if (ret < 0) {
-    goto fail;
-  }
-
-  // Enable continuous mode, set every 100 measurements
-  ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_2, MMC5603NJ_CMM_EN | MMC5603NJ_EN_PRD_SET | 0b11);
-  if (ret < 0) {
-    goto fail;
-  }
-
 fail:
   return ret;
 }
@@ -67,16 +58,36 @@ fail:
   return ret;
 }
 
-bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
-  uint64_t start_time = nanos_since_boot();
+void MMC5603NJ_Magn::start_measurement() {
+  set_register(MMC5603NJ_I2C_REG_INTERNAL_0, 0b01);
+  util::sleep_for(5);
+}
+
+std::vector<float> MMC5603NJ_Magn::read_measurement() {
+  int len;
   uint8_t buffer[9];
-  int len = read_register(MMC5603NJ_I2C_REG_XOUT0, buffer, sizeof(buffer));
+  len = read_register(MMC5603NJ_I2C_REG_XOUT0, buffer, sizeof(buffer));
   assert(len == sizeof(buffer));
-
   float scale = 1.0 / 16384.0;
-  float x = read_20_bit(buffer[6], buffer[1], buffer[0]) * scale;
-  float y = read_20_bit(buffer[7], buffer[3], buffer[2]) * scale;
-  float z = read_20_bit(buffer[8], buffer[5], buffer[4]) * scale;
+  float x = (read_20_bit(buffer[6], buffer[1], buffer[0]) * scale) - 32.0;
+  float y = (read_20_bit(buffer[7], buffer[3], buffer[2]) * scale) - 32.0;
+  float z = (read_20_bit(buffer[8], buffer[5], buffer[4]) * scale) - 32.0;
+  std::vector<float> xyz = {x, y, z};
+  return xyz;
+}
+
+bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
+  uint64_t start_time = nanos_since_boot();
+  // SET - RESET cycle
+  set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_SET);
+  util::sleep_for(5);
+  MMC5603NJ_Magn::start_measurement();
+  std::vector<float> xyz = MMC5603NJ_Magn::read_measurement();
+
+  set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_RESET);
+  util::sleep_for(5);
+  MMC5603NJ_Magn::start_measurement();
+  std::vector<float> reset_xyz = MMC5603NJ_Magn::read_measurement();
 
   auto event = msg.initEvent().initMagnetometer();
   event.setSource(cereal::SensorEventData::SensorSource::MMC5603NJ);
@@ -85,10 +96,13 @@ bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
   event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
   event.setTimestamp(start_time);
 
-  float xyz[] = {x, y, z};
+  float vals[] = {xyz[0], xyz[1], xyz[2], reset_xyz[0], reset_xyz[1], reset_xyz[2]};
+  bool valid = true;
+  if (std::any_of(std::begin(vals), std::end(vals), [](float val) { return val == -32.0; })) {
+    valid = false;
+  }
   auto svec = event.initMagneticUncalibrated();
-  svec.setV(xyz);
-  svec.setStatus(true);
-
+  svec.setV(vals);
+  svec.setStatus(valid);
   return true;
 }
diff --git a/system/sensord/sensors/mmc5603nj_magn.h b/system/sensord/sensors/mmc5603nj_magn.h
index fce3f3fecb..9c0fbd2521 100644
--- a/system/sensord/sensors/mmc5603nj_magn.h
+++ b/system/sensord/sensors/mmc5603nj_magn.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <vector>
+
 #include "system/sensord/sensors/i2c_sensor.h"
 
 // Address of the chip on the bus
@@ -19,9 +21,14 @@
 #define MMC5603NJ_AUTO_SR_EN     (1 << 5)
 #define MMC5603NJ_CMM_EN         (1 << 4)
 #define MMC5603NJ_EN_PRD_SET     (1 << 3)
+#define MMC5603NJ_SET            (1 << 3)
+#define MMC5603NJ_RESET          (1 << 4)
 
 class MMC5603NJ_Magn : public I2CSensor {
+private:
   uint8_t get_device_address() {return MMC5603NJ_I2C_ADDR;}
+  void start_measurement();
+  std::vector<float> read_measurement();
 public:
   MMC5603NJ_Magn(I2CBus *bus);
   int init();
diff --git a/system/sensord/sensors/sensor.h b/system/sensord/sensors/sensor.h
index 603aa3586e..1b0e3be0dc 100644
--- a/system/sensord/sensors/sensor.h
+++ b/system/sensord/sensors/sensor.h
@@ -5,14 +5,18 @@
 class Sensor {
 public:
   int gpio_fd = -1;
+  uint64_t start_ts = 0;
   uint64_t init_delay = 500e6; // default dealy 500ms
-  virtual ~Sensor() {};
+  virtual ~Sensor() {}
   virtual int init() = 0;
   virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
   virtual bool has_interrupt_enabled() = 0;
   virtual int shutdown() = 0;
 
-  virtual bool is_data_valid(uint64_t st, uint64_t ct) {
-    return (ct - st) > init_delay;
+  virtual bool is_data_valid(uint64_t current_ts) {
+    if (start_ts == 0) {
+      start_ts = current_ts;
+    }
+    return (current_ts - start_ts) > init_delay;
   }
 };
diff --git a/system/sensord/sensors_qcom2.cc b/system/sensord/sensors_qcom2.cc
index 349c67f498..36d9b4a13e 100644
--- a/system/sensord/sensors_qcom2.cc
+++ b/system/sensord/sensors_qcom2.cc
@@ -7,8 +7,10 @@
 #include <poll.h>
 #include <linux/gpio.h>
 
+#include "cereal/services.h"
 #include "cereal/messaging/messaging.h"
 #include "common/i2c.h"
+#include "common/ratekeeper.h"
 #include "common/swaglog.h"
 #include "common/timing.h"
 #include "common/util.h"
@@ -17,24 +19,26 @@
 #include "system/sensord/sensors/bmx055_magn.h"
 #include "system/sensord/sensors/bmx055_temp.h"
 #include "system/sensord/sensors/constants.h"
-#include "system/sensord/sensors/light_sensor.h"
 #include "system/sensord/sensors/lsm6ds3_accel.h"
 #include "system/sensord/sensors/lsm6ds3_gyro.h"
 #include "system/sensord/sensors/lsm6ds3_temp.h"
 #include "system/sensord/sensors/mmc5603nj_magn.h"
-#include "system/sensord/sensors/sensor.h"
 
 #define I2C_BUS_IMU 1
 
 ExitHandler do_exit;
-uint64_t init_ts = 0;
 
-void interrupt_loop(std::vector<Sensor *>& sensors,
-                    std::map<Sensor*, std::string>& sensor_service)
-{
-  PubMaster pm_int({"gyroscope", "accelerometer"});
+void interrupt_loop(std::vector<std::tuple<Sensor *, std::string>> sensors) {
+  PubMaster pm({"gyroscope", "accelerometer"});
+
+  int fd = -1;
+  for (auto &[sensor, msg_name] : sensors) {
+    if (sensor->has_interrupt_enabled()) {
+      fd = sensor->gpio_fd;
+      break;
+    }
+  }
 
-  int fd = sensors[0]->gpio_fd;
   struct pollfd fd_list[1] = {0};
   fd_list[0].fd = fd;
   fd_list[0].events = POLLIN | POLLPRI;
@@ -68,95 +72,63 @@ void interrupt_loop(std::vector<Sensor *>& sensors,
     uint64_t offset = nanos_since_epoch() - nanos_since_boot();
     uint64_t ts = evdata[num_events - 1].timestamp - offset;
 
-    for (Sensor *sensor : sensors) {
+    for (auto &[sensor, msg_name] : sensors) {
+      if (!sensor->has_interrupt_enabled()) {
+        continue;
+      }
+
       MessageBuilder msg;
       if (!sensor->get_event(msg, ts)) {
         continue;
       }
 
-      if (!sensor->is_data_valid(init_ts, ts)) {
+      if (!sensor->is_data_valid(ts)) {
         continue;
       }
 
-      pm_int.send(sensor_service[sensor].c_str(), msg);
+      pm.send(msg_name.c_str(), msg);
     }
   }
+}
 
-  // poweroff sensors, disable interrupts
-  for (Sensor *sensor : sensors) {
-    sensor->shutdown();
+void polling_loop(Sensor *sensor, std::string msg_name) {
+  PubMaster pm({msg_name.c_str()});
+  RateKeeper rk(msg_name, services.at(msg_name).frequency);
+  while (!do_exit) {
+    MessageBuilder msg;
+    if (sensor->get_event(msg) && sensor->is_data_valid(nanos_since_boot())) {
+      pm.send(msg_name.c_str(), msg);
+    }
+    rk.keepTime();
   }
 }
 
 int sensor_loop(I2CBus *i2c_bus_imu) {
-  BMX055_Accel bmx055_accel(i2c_bus_imu);
-  BMX055_Gyro bmx055_gyro(i2c_bus_imu);
-  BMX055_Magn bmx055_magn(i2c_bus_imu);
-  BMX055_Temp bmx055_temp(i2c_bus_imu);
-
-  LSM6DS3_Accel lsm6ds3_accel(i2c_bus_imu, GPIO_LSM_INT);
-  LSM6DS3_Gyro lsm6ds3_gyro(i2c_bus_imu, GPIO_LSM_INT, true); // GPIO shared with accel
-  LSM6DS3_Temp lsm6ds3_temp(i2c_bus_imu);
-
-  MMC5603NJ_Magn mmc5603nj_magn(i2c_bus_imu);
-
-  LightSensor light("/sys/class/i2c-adapter/i2c-2/2-0038/iio:device1/in_intensity_both_raw");
-
-  std::map<Sensor*, std::string> sensor_service = {
-    {&bmx055_accel, "accelerometer2"},
-    {&bmx055_gyro, "gyroscope2"},
-    {&bmx055_magn, "magnetometer"},
-    {&bmx055_temp, "temperatureSensor"},
-
-    {&lsm6ds3_accel, "accelerometer"},
-    {&lsm6ds3_gyro, "gyroscope"},
-    {&lsm6ds3_temp, "temperatureSensor"},
-
-    {&mmc5603nj_magn, "magnetometer"},
-    {&light, "lightSensor"}
-  };
-
   // Sensor init
-  std::vector<std::pair<Sensor *, bool>> sensors_init; // Sensor, required
-  sensors_init.push_back({&bmx055_accel, false});
-  sensors_init.push_back({&bmx055_gyro, false});
-  sensors_init.push_back({&bmx055_magn, false});
-  sensors_init.push_back({&bmx055_temp, false});
-
-  sensors_init.push_back({&lsm6ds3_accel, true});
-  sensors_init.push_back({&lsm6ds3_gyro, true});
-  sensors_init.push_back({&lsm6ds3_temp, true});
-
-  sensors_init.push_back({&mmc5603nj_magn, false});
+  std::vector<std::tuple<Sensor *, std::string>> sensors_init = {
+    {new BMX055_Accel(i2c_bus_imu), "accelerometer2"},
+    {new BMX055_Gyro(i2c_bus_imu), "gyroscope2"},
+    {new BMX055_Magn(i2c_bus_imu), "magnetometer"},
+    {new BMX055_Temp(i2c_bus_imu), "temperatureSensor2"},
 
-  sensors_init.push_back({&light, true});
+    {new LSM6DS3_Accel(i2c_bus_imu, GPIO_LSM_INT), "accelerometer"},
+    {new LSM6DS3_Gyro(i2c_bus_imu, GPIO_LSM_INT, true), "gyroscope"},
+    {new LSM6DS3_Temp(i2c_bus_imu), "temperatureSensor"},
 
-  bool has_magnetometer = false;
+    {new MMC5603NJ_Magn(i2c_bus_imu), "magnetometer"},
+  };
 
   // Initialize sensors
-  std::vector<Sensor *> sensors;
-  for (auto &[sensor, required] : sensors_init) {
+  std::vector<std::thread> threads;
+  for (auto &[sensor, msg_name] : sensors_init) {
     int err = sensor->init();
     if (err < 0) {
-      if (required) {
-        LOGE("Error initializing sensors");
-        return -1;
-      }
-    } else {
-
-      if (sensor == &bmx055_magn || sensor == &mmc5603nj_magn) {
-        has_magnetometer = true;
-      }
-
-      if (!sensor->has_interrupt_enabled()) {
-        sensors.push_back(sensor);
-      }
+      continue;
     }
-  }
 
-  if (!has_magnetometer) {
-    LOGE("No magnetometer present");
-    return -1;
+    if (!sensor->has_interrupt_enabled()) {
+      threads.emplace_back(polling_loop, sensor, msg_name);
+    }
   }
 
   // increase interrupt quality by pinning interrupt and process to core 1
@@ -164,41 +136,18 @@ int sensor_loop(I2CBus *i2c_bus_imu) {
   util::set_core_affinity({1});
   std::system("sudo su -c 'echo 1 > /proc/irq/336/smp_affinity_list'");
 
-  PubMaster pm_non_int({"gyroscope2", "accelerometer2", "temperatureSensor",
-                        "lightSensor", "magnetometer"});
-  init_ts = nanos_since_boot();
-
   // thread for reading events via interrupts
-  std::vector<Sensor *> lsm_interrupt_sensors = {&lsm6ds3_accel, &lsm6ds3_gyro};
-  std::thread lsm_interrupt_thread(&interrupt_loop, std::ref(lsm_interrupt_sensors),
-                                   std::ref(sensor_service));
-
-  // polling loop for non interrupt handled sensors
-  while (!do_exit) {
-    std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
-
-    for (Sensor *sensor : sensors) {
-      MessageBuilder msg;
-      if (!sensor->get_event(msg)) {
-        continue;
-      }
+  threads.emplace_back(&interrupt_loop, std::ref(sensors_init));
 
-      if (!sensor->is_data_valid(init_ts, nanos_since_boot())) {
-        continue;
-      }
-
-      pm_non_int.send(sensor_service[sensor].c_str(), msg);
-    }
-
-    std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
-    std::this_thread::sleep_for(std::chrono::milliseconds(10) - (end - begin));
+  // wait for all threads to finish
+  for (auto &t : threads) {
+    t.join();
   }
 
-  for (Sensor *sensor : sensors) {
+  for (auto &[sensor, msg_name] : sensors_init) {
     sensor->shutdown();
+    delete sensor;
   }
-
-  lsm_interrupt_thread.join();
   return 0;
 }
 
diff --git a/system/swaglog.py b/system/swaglog.py
index 28beb5a4d1..ba81318234 100644
--- a/system/swaglog.py
+++ b/system/swaglog.py
@@ -7,17 +7,13 @@ from logging.handlers import BaseRotatingHandler
 
 import zmq
 
-from common.logging_extra import SwagLogger, SwagFormatter, SwagLogFileFormatter
-from system.hardware import PC
+from openpilot.common.logging_extra import SwagLogger, SwagFormatter, SwagLogFileFormatter
+from openpilot.system.hardware.hw import Paths
 
-if PC:
-  SWAGLOG_DIR = os.path.join(str(Path.home()), ".comma", "log")
-else:
-  SWAGLOG_DIR = "/data/log/"
 
 def get_file_handler():
-  Path(SWAGLOG_DIR).mkdir(parents=True, exist_ok=True)
-  base_filename = os.path.join(SWAGLOG_DIR, "swaglog")
+  Path(Paths.swaglog_root()).mkdir(parents=True, exist_ok=True)
+  base_filename = os.path.join(Paths.swaglog_root(), "swaglog")
   handler = SwaglogRotatingFileHandler(base_filename)
   return handler
 
@@ -77,6 +73,9 @@ class UnixDomainSocketHandler(logging.Handler):
     self.sock = None
 
   def __del__(self):
+    self.close()
+
+  def close(self):
     if self.sock is not None:
       self.sock.close()
     if self.zctx is not None:
@@ -86,7 +85,7 @@ class UnixDomainSocketHandler(logging.Handler):
     self.zctx = zmq.Context()
     self.sock = self.zctx.socket(zmq.PUSH)
     self.sock.setsockopt(zmq.LINGER, 10)
-    self.sock.connect("ipc:///tmp/logmessage")
+    self.sock.connect(Paths.swaglog_ipc())
     self.pid = os.getpid()
 
   def emit(self, record):
@@ -129,6 +128,8 @@ elif print_level == 'info':
 elif print_level == 'warning':
   outhandler.setLevel(logging.WARNING)
 
+ipchandler = UnixDomainSocketHandler(SwagFormatter(log))
+
 log.addHandler(outhandler)
 # logs are sent through IPC before writing to disk to prevent disk I/O blocking
-log.addHandler(UnixDomainSocketHandler(SwagFormatter(log)))
+log.addHandler(ipchandler)
diff --git a/system/timezoned.py b/system/timezoned.py
index 884a5c3812..91424d33b5 100755
--- a/system/timezoned.py
+++ b/system/timezoned.py
@@ -8,9 +8,12 @@ from typing import NoReturn
 import requests
 from timezonefinder import TimezoneFinder
 
-from common.params import Params
-from system.hardware import AGNOS
-from system.swaglog import cloudlog
+from openpilot.common.params import Params
+from openpilot.system.hardware import AGNOS
+from openpilot.system.swaglog import cloudlog
+from openpilot.system.version import get_version
+
+REQUEST_HEADERS = {'User-Agent': "openpilot-" + get_version()}
 
 
 def set_timezone(valid_timezones, timezone):
@@ -18,7 +21,7 @@ def set_timezone(valid_timezones, timezone):
     cloudlog.error(f"Timezone not supported {timezone}")
     return
 
-  cloudlog.debug(f"Setting timezone to {timezone}")
+  cloudlog.info(f"Setting timezone to {timezone}")
   try:
     if AGNOS:
       tzpath = os.path.join("/usr/share/zoneinfo/", timezone)
@@ -58,7 +61,7 @@ def main() -> NoReturn:
     if location is None:
       cloudlog.debug("Setting timezone based on IP lookup")
       try:
-        r = requests.get("https://ipapi.co/timezone", timeout=10)
+        r = requests.get("https://ipapi.co/timezone", headers=REQUEST_HEADERS, timeout=10)
         if r.status_code == 200:
           set_timezone(valid_timezones, r.text)
         else:
diff --git a/system/ubloxd/SConscript b/system/ubloxd/SConscript
index fff0986efd..67d9856dad 100644
--- a/system/ubloxd/SConscript
+++ b/system/ubloxd/SConscript
@@ -16,5 +16,5 @@ if GetOption('kaitai'):
 glonass_obj = env.Object('generated/glonass.cpp')
 env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp",  glonass_obj], LIBS=loc_libs)
 
-if GetOption('test'):
+if GetOption('extras'):
   env.Program("tests/test_glonass_runner", ['tests/test_glonass_runner.cc', 'tests/test_glonass_kaitai.cc', glonass_obj], LIBS=[loc_libs])
\ No newline at end of file
diff --git a/system/ubloxd/ublox_msg.cc b/system/ubloxd/ublox_msg.cc
index ee8447fa64..7b4505dc81 100644
--- a/system/ubloxd/ublox_msg.cc
+++ b/system/ubloxd/ublox_msg.cc
@@ -1,7 +1,8 @@
-#include "ublox_msg.h"
+#include "system/ubloxd/ublox_msg.h"
 
 #include <unistd.h>
 
+#include <algorithm>
 #include <cassert>
 #include <chrono>
 #include <cmath>
@@ -9,6 +10,7 @@
 #include <cstdlib>
 #include <ctime>
 #include <unordered_map>
+#include <utility>
 
 #include "common/swaglog.h"
 
@@ -21,26 +23,26 @@ inline static bool bit_to_bool(uint8_t val, int shifts) {
 
 inline int UbloxMsgParser::needed_bytes() {
   // Msg header incomplete?
-  if(bytes_in_parse_buf < ublox::UBLOX_HEADER_SIZE)
+  if (bytes_in_parse_buf < ublox::UBLOX_HEADER_SIZE)
     return ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE - bytes_in_parse_buf;
   uint16_t needed = UBLOX_MSG_SIZE(msg_parse_buf) + ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE;
   // too much data
-  if(needed < (uint16_t)bytes_in_parse_buf)
+  if (needed < (uint16_t)bytes_in_parse_buf)
     return -1;
   return needed - (uint16_t)bytes_in_parse_buf;
 }
 
 inline bool UbloxMsgParser::valid_cheksum() {
   uint8_t ck_a = 0, ck_b = 0;
-  for(int i = 2; i < bytes_in_parse_buf - ublox::UBLOX_CHECKSUM_SIZE;i++) {
+  for (int i = 2; i < bytes_in_parse_buf - ublox::UBLOX_CHECKSUM_SIZE; i++) {
     ck_a = (ck_a + msg_parse_buf[i]) & 0xFF;
     ck_b = (ck_b + ck_a) & 0xFF;
   }
-  if(ck_a != msg_parse_buf[bytes_in_parse_buf - 2]) {
+  if (ck_a != msg_parse_buf[bytes_in_parse_buf - 2]) {
     LOGD("Checksum a mismatch: %02X, %02X", ck_a, msg_parse_buf[6]);
     return false;
   }
-  if(ck_b != msg_parse_buf[bytes_in_parse_buf - 1]) {
+  if (ck_b != msg_parse_buf[bytes_in_parse_buf - 1]) {
     LOGD("Checksum b mismatch: %02X, %02X", ck_b, msg_parse_buf[7]);
     return false;
   }
@@ -53,13 +55,13 @@ inline bool UbloxMsgParser::valid() {
 }
 
 inline bool UbloxMsgParser::valid_so_far() {
-  if(bytes_in_parse_buf > 0 && msg_parse_buf[0] != ublox::PREAMBLE1) {
+  if (bytes_in_parse_buf > 0 && msg_parse_buf[0] != ublox::PREAMBLE1) {
     return false;
   }
-  if(bytes_in_parse_buf > 1 && msg_parse_buf[1] != ublox::PREAMBLE2) {
+  if (bytes_in_parse_buf > 1 && msg_parse_buf[1] != ublox::PREAMBLE2) {
     return false;
   }
-  if(needed_bytes() == 0 && !valid()) {
+  if (needed_bytes() == 0 && !valid()) {
     return false;
   }
   return true;
@@ -68,8 +70,8 @@ inline bool UbloxMsgParser::valid_so_far() {
 bool UbloxMsgParser::add_data(float log_time, const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) {
   last_log_time = log_time;
   int needed = needed_bytes();
-  if(needed > 0) {
-    bytes_consumed = std::min((uint32_t)needed, incoming_data_len );
+  if (needed > 0) {
+    bytes_consumed = std::min((uint32_t)needed, incoming_data_len);
     // Add data to buffer
     memcpy(msg_parse_buf + bytes_in_parse_buf, incoming_data, bytes_consumed);
     bytes_in_parse_buf += bytes_consumed;
@@ -78,15 +80,15 @@ bool UbloxMsgParser::add_data(float log_time, const uint8_t *incoming_data, uint
   }
 
   // Validate msg format, detect invalid header and invalid checksum.
-  while(!valid_so_far() && bytes_in_parse_buf != 0) {
+  while (!valid_so_far() && bytes_in_parse_buf != 0) {
     // Corrupted msg, drop a byte.
     bytes_in_parse_buf -= 1;
-    if(bytes_in_parse_buf > 0)
+    if (bytes_in_parse_buf > 0)
       memmove(&msg_parse_buf[0], &msg_parse_buf[1], bytes_in_parse_buf);
   }
 
   // There is redundant data at the end of buffer, reset the buffer.
-  if(needed_bytes() == -1) {
+  if (needed_bytes() == -1) {
     bytes_in_parse_buf = 0;
   }
   return valid();
@@ -295,8 +297,7 @@ kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_
         continue;
       if (glonass_string_superframes[msg->freq_id()][i] == 0 || gl_string.superframe_number() == 0) {
         superframe_unknown = true;
-      }
-      else if (glonass_string_superframes[msg->freq_id()][i] != gl_string.superframe_number()) {
+      } else if (glonass_string_superframes[msg->freq_id()][i] != gl_string.superframe_number()) {
         needs_clear = true;
       }
       // Check if string times add up to being from the same frame
@@ -390,7 +391,7 @@ kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_
     eph.setP4(data->p4());
     eph.setSvURA(glonass_URA_lookup.at(data->f_t()));
     if (msg->sv_id() != data->n()) {
-      LOGE("SV_ID != SLOT_NUMBER: %d %d", msg->sv_id(), data->n());
+      LOGE("SV_ID != SLOT_NUMBER: %d %" PRIu64, msg->sv_id(), data->n());
     }
     eph.setSvType(data->m());
   }
@@ -434,7 +435,7 @@ kj::Array<capnp::word> UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) {
 
   auto mb = mr.initMeasurements(msg->num_meas());
   auto measurements = *msg->meas();
-  for(int8_t i = 0; i < msg->num_meas(); i++) {
+  for (int8_t i = 0; i < msg->num_meas(); i++) {
     mb[i].setSvId(measurements[i]->sv_id());
     mb[i].setPseudorange(measurements[i]->pr_mes());
     mb[i].setCarrierCycles(measurements[i]->cp_mes());
@@ -469,7 +470,7 @@ kj::Array<capnp::word> UbloxMsgParser::gen_nav_sat(ubx_t::nav_sat_t *msg) {
 
   auto svs = sr.initSvs(msg->num_svs());
   auto svs_data = *msg->svs();
-  for(int8_t i = 0; i < msg->num_svs(); i++) {
+  for (int8_t i = 0; i < msg->num_svs(); i++) {
     svs[i].setSvId(svs_data[i]->sv_id());
     svs[i].setGnssId(svs_data[i]->gnss_id());
     svs[i].setFlagsBitfield(svs_data[i]->flags());
diff --git a/system/ubloxd/ublox_msg.h b/system/ubloxd/ublox_msg.h
index a52a7db3e5..d21760edc2 100644
--- a/system/ubloxd/ublox_msg.h
+++ b/system/ubloxd/ublox_msg.h
@@ -2,10 +2,11 @@
 
 #include <cassert>
 #include <cstdint>
+#include <ctime>
 #include <memory>
 #include <string>
 #include <unordered_map>
-#include <ctime>
+#include <utility>
 
 #include "cereal/messaging/messaging.h"
 #include "common/util.h"
@@ -51,7 +52,7 @@ namespace ublox {
     assert(msg.size() > 2);
 
     uint8_t ck_a = 0, ck_b = 0;
-    for(int i = 2; i < msg.size(); i++) {
+    for (int i = 2; i < msg.size(); i++) {
       ck_a = (ck_a + msg[i]) & 0xFF;
       ck_b = (ck_b + ck_a) & 0xFF;
     }
diff --git a/system/ubloxd/ubloxd.cc b/system/ubloxd/ubloxd.cc
index 1dae6dc866..668c1a7ec0 100644
--- a/system/ubloxd/ubloxd.cc
+++ b/system/ubloxd/ubloxd.cc
@@ -41,9 +41,9 @@ int main() {
     size_t len = ubloxRaw.size();
     size_t bytes_consumed = 0;
 
-    while(bytes_consumed < len && !do_exit) {
+    while (bytes_consumed < len && !do_exit) {
       size_t bytes_consumed_this_time = 0U;
-      if(parser.add_data(log_time, data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) {
+      if (parser.add_data(log_time, data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) {
 
         try {
           auto ublox_msg = parser.gen_msg();
diff --git a/system/version.py b/system/version.py
old mode 100644
new mode 100755
index 55f80afa35..82e99fd416
--- a/system/version.py
+++ b/system/version.py
@@ -4,8 +4,8 @@ import subprocess
 from typing import List, Optional
 from functools import lru_cache
 
-from common.basedir import BASEDIR
-from system.swaglog import cloudlog
+from openpilot.common.basedir import BASEDIR
+from openpilot.system.swaglog import cloudlog
 
 RELEASE_BRANCHES = ['release3-staging', 'dashcam3-staging', 'release3', 'dashcam3', 'nightly']
 TESTED_BRANCHES = RELEASE_BRANCHES + ['devel', 'devel-staging']
@@ -90,7 +90,7 @@ def is_comma_remote() -> bool:
   if origin is None:
     return False
 
-  return origin.startswith('git@github.com:commaai') or origin.startswith('https://github.com/commaai')
+  return origin.startswith(('git@github.com:commaai', 'https://github.com/commaai'))
 
 
 @cache
@@ -127,7 +127,7 @@ def is_dirty() -> bool:
 
 
 if __name__ == "__main__":
-  from common.params import Params
+  from openpilot.common.params import Params
 
   params = Params()
   params.put("TermsVersion", terms_version)
diff --git a/third_party/SConscript b/third_party/SConscript
index e8d1789ee0..e5bbfaa07a 100644
--- a/third_party/SConscript
+++ b/third_party/SConscript
@@ -4,5 +4,3 @@ env.Library('json11', ['json11/json11.cpp'], CCFLAGS=env['CCFLAGS'] + ['-Wno-unq
 env.Append(CPPPATH=[Dir('json11')])
 
 env.Library('kaitai', ['kaitai/kaitaistream.cpp'], CPPDEFINES=['KS_STR_ENCODING_NONE'])
-
-SConscript(['cluster/SConscript'])
diff --git a/third_party/acados/acados_template/__init__.py b/third_party/acados/acados_template/__init__.py
index f33b75bb7b..bfbe907990 100644
--- a/third_party/acados/acados_template/__init__.py
+++ b/third_party/acados/acados_template/__init__.py
@@ -1,8 +1,5 @@
 #
-# 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
+# Copyright (c) The acados authors.
 #
 # This file is part of acados.
 #
@@ -31,13 +28,13 @@
 # POSSIBILITY OF SUCH DAMAGE.;
 #
 
-from .acados_model import *
-from .generate_c_code_explicit_ode import *
-from .generate_c_code_implicit_ode import *
-from .generate_c_code_constraint import *
-from .generate_c_code_nls_cost import *
-from .acados_ocp import *
-from .acados_sim import *
-from .acados_ocp_solver import *
-from .acados_sim_solver import *
-from .utils import *
+from .acados_model import AcadosModel
+from .acados_ocp import AcadosOcp, AcadosOcpConstraints, AcadosOcpCost, AcadosOcpDims, AcadosOcpOptions
+from .acados_sim import AcadosSim, AcadosSimDims, AcadosSimOpts
+from .acados_ocp_solver import AcadosOcpSolver, get_simulink_default_opts, ocp_get_default_cmake_builder
+from .acados_sim_solver import AcadosSimSolver, sim_get_default_cmake_builder
+from .utils import print_casadi_expression, get_acados_path, get_python_interface_path, \
+    get_tera_exec_path, get_tera, check_casadi_version, acados_dae_model_json_dump, \
+    casadi_length, make_object_json_dumpable, J_to_idx, get_default_simulink_opts
+
+from .zoro_description import ZoroDescription, process_zoro_description
diff --git a/third_party/acados/acados_template/acados_layout.json b/third_party/acados/acados_template/acados_layout.json
index c9f0b90c73..a1cc5bbdf3 100644
--- a/third_party/acados/acados_template/acados_layout.json
+++ b/third_party/acados/acados_template/acados_layout.json
@@ -6,6 +6,12 @@
         "str"
     ],
     "cython_include_dirs": [
+        "list"
+    ],
+    "json_file": [
+        "str"
+    ],
+    "shared_lib_ext": [
         "str"
     ],
     "model": {
@@ -15,7 +21,16 @@
         "dyn_ext_fun_type" : [
             "str"
         ],
-        "dyn_source_discrete" : [
+        "dyn_generic_source" : [
+            "str"
+        ],
+        "dyn_impl_dae_fun" : [
+            "str"
+        ],
+        "dyn_impl_dae_fun_jac" : [
+            "str"
+        ],
+        "dyn_impl_dae_jac" : [
             "str"
         ],
         "dyn_disc_fun_jac_hess" : [
@@ -734,6 +749,9 @@
         "sim_method_newton_iter": [
             "int"
         ],
+        "sim_method_newton_tol": [
+            "float"
+        ],
         "sim_method_jac_reuse": [
             "ndarray",
             [
@@ -761,6 +779,12 @@
         "qp_solver_iter_max": [
             "int"
         ],
+        "qp_solver_cond_ric_alg": [
+            "int"
+        ],
+        "qp_solver_ric_alg": [
+            "int"
+        ],
         "nlp_solver_tol_stat": [
             "float"
         ],
@@ -776,6 +800,9 @@
         "nlp_solver_max_iter": [
             "int"
         ],
+        "nlp_solver_ext_qp_res": [
+            "int"
+        ],
         "print_level": [
             "int"
         ],
@@ -794,6 +821,9 @@
         "ext_cost_num_hess": [
             "int"
         ],
+        "ext_fun_compile_flags": [
+            "str"
+        ],
         "model_external_shared_lib_dir": [
             "str"
         ],
diff --git a/third_party/acados/acados_template/acados_model.py b/third_party/acados/acados_template/acados_model.py
index e292cc7477..b7c6945442 100644
--- a/third_party/acados/acados_template/acados_model.py
+++ b/third_party/acados/acados_template/acados_model.py
@@ -1,8 +1,5 @@
 #
-# 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
+# Copyright (c) The acados authors.
 #
 # This file is part of acados.
 #
@@ -37,7 +34,7 @@ class AcadosModel():
     Class containing all the information to code generate the external CasADi functions
     that are needed when creating an acados ocp solver or acados integrator.
     Thus, this class contains:
-    
+
     a) the :py:attr:`name` of the model,
     b) all CasADi variables/expressions needed in the CasADi function generation process.
     """
@@ -73,7 +70,7 @@ class AcadosModel():
         """
 
         self.dyn_ext_fun_type = 'casadi'  #: type of external functions for dynamics module; 'casadi' or 'generic'; Default: 'casadi'
-        self.dyn_source_discrete = None  #: name of source file for discrete dyanamics; Default: :code:`None`
+        self.dyn_generic_source = None  #: name of source file for discrete dyanamics; Default: :code:`None`
         self.dyn_disc_fun_jac_hess = None  #: name of function discrete dyanamics + jacobian and hessian; Default: :code:`None`
         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`
@@ -87,7 +84,9 @@ class AcadosModel():
 
         ## for OCP
         # constraints
+        # BGH(default): lh <= h(x, u) <= uh
         self.con_h_expr = None  #: CasADi expression for the constraint :math:`h`; Default: :code:`None`
+        # BGP(convex over nonlinear): lphi <= phi(r(x, u)) <= uphi
         self.con_phi_expr = None  #: CasADi expression for the constraint phi; Default: :code:`None`
         self.con_r_expr = None  #: CasADi expression for the constraint phi(r); Default: :code:`None`
         self.con_r_in_phi = None
@@ -107,61 +106,49 @@ class AcadosModel():
         self.cost_expr_ext_cost_custom_hess_e = None  #: CasADi expression for custom hessian (only for external cost), terminal; Default: :code:`None`
         self.cost_expr_ext_cost_custom_hess_0 = None  #: CasADi expression for custom hessian (only for external cost), initial; Default: :code:`None`
 
-
-def acados_model_strip_casadi_symbolics(model):
-    out = model
-    if 'f_impl_expr' in out.keys():
-        del out['f_impl_expr']
-    if 'f_expl_expr' in out.keys():
-        del out['f_expl_expr']
-    if 'disc_dyn_expr' in out.keys():
-        del out['disc_dyn_expr']
-    if 'x' in out.keys():
-        del out['x']
-    if 'xdot' in out.keys():
-        del out['xdot']
-    if 'u' in out.keys():
-        del out['u']
-    if 'z' in out.keys():
-        del out['z']
-    if 'p' in out.keys():
-        del out['p']
-    # constraints
-    if 'con_phi_expr' in out.keys():
-        del out['con_phi_expr']
-    if 'con_h_expr' in out.keys():
-        del out['con_h_expr']
-    if 'con_r_expr' in out.keys():
-        del out['con_r_expr']
-    if 'con_r_in_phi' in out.keys():
-        del out['con_r_in_phi']
-    # terminal
-    if 'con_phi_expr_e' in out.keys():
-        del out['con_phi_expr_e']
-    if 'con_h_expr_e' in out.keys():
-        del out['con_h_expr_e']
-    if 'con_r_expr_e' in out.keys():
-        del out['con_r_expr_e']
-    if 'con_r_in_phi_e' in out.keys():
-        del out['con_r_in_phi_e']
-    # cost
-    if 'cost_y_expr' in out.keys():
-        del out['cost_y_expr']
-    if 'cost_y_expr_e' in out.keys():
-        del out['cost_y_expr_e']
-    if 'cost_y_expr_0' in out.keys():
-        del out['cost_y_expr_0']
-    if 'cost_expr_ext_cost' in out.keys():
-        del out['cost_expr_ext_cost']
-    if 'cost_expr_ext_cost_e' in out.keys():
-        del out['cost_expr_ext_cost_e']
-    if 'cost_expr_ext_cost_0' in out.keys():
-        del out['cost_expr_ext_cost_0']
-    if 'cost_expr_ext_cost_custom_hess' in out.keys():
-        del out['cost_expr_ext_cost_custom_hess']
-    if 'cost_expr_ext_cost_custom_hess_e' in out.keys():
-        del out['cost_expr_ext_cost_custom_hess_e']
-    if 'cost_expr_ext_cost_custom_hess_0' in out.keys():
-        del out['cost_expr_ext_cost_custom_hess_0']
-
-    return out
+        ## CONVEX_OVER_NONLINEAR convex-over-nonlinear cost: psi(y(x, u, p) - y_ref; p)
+        self.cost_psi_expr_0 = None
+        """
+        CasADi expression for the outer loss function :math:`\psi(r, p)`, initial; Default: :code:`None`
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_0` == 'CONVEX_OVER_NONLINEAR'.
+        """
+        self.cost_psi_expr = None
+        """
+        CasADi expression for the outer loss function :math:`\psi(r, p)`; Default: :code:`None`
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type` == 'CONVEX_OVER_NONLINEAR'.
+        """
+        self.cost_psi_expr_e = None
+        """
+        CasADi expression for the outer loss function :math:`\psi(r, p)`, terminal; Default: :code:`None`
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_e` == 'CONVEX_OVER_NONLINEAR'.
+        """
+        self.cost_r_in_psi_expr_0 = None
+        """
+        CasADi expression for the argument :math:`r`; to the outer loss function :math:`\psi(r, p)`, initial; Default: :code:`None`
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_0` == 'CONVEX_OVER_NONLINEAR'.
+        """
+        self.cost_r_in_psi_expr = None
+        """
+        CasADi expression for the argument :math:`r`; to the outer loss function :math:`\psi(r, p)`; Default: :code:`None`
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type` == 'CONVEX_OVER_NONLINEAR'.
+        """
+        self.cost_r_in_psi_expr_e = None
+        """
+        CasADi expression for the argument :math:`r`; to the outer loss function :math:`\psi(r, p)`, terminal; Default: :code:`None`
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_e` == 'CONVEX_OVER_NONLINEAR'.
+        """
+        self.cost_conl_custom_outer_hess_0 = None
+        """
+        CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost), initial; Default: :code:`None`
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_0` == 'CONVEX_OVER_NONLINEAR'.
+        """
+        self.cost_conl_custom_outer_hess = None
+        """
+        CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost); Default: :code:`None`
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type` == 'CONVEX_OVER_NONLINEAR'.
+        """
+        self.cost_conl_custom_outer_hess_e = None
+        """
+        CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost), terminal; Default: :code:`None`
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_e` == 'CONVEX_OVER_NONLINEAR'.
+        """
diff --git a/third_party/acados/acados_template/acados_ocp.py b/third_party/acados/acados_template/acados_ocp.py
index 80970239eb..ec02822ceb 100644
--- a/third_party/acados/acados_template/acados_ocp.py
+++ b/third_party/acados/acados_template/acados_ocp.py
@@ -1,9 +1,6 @@
 # -*- coding: future_fstrings -*-
 #
-# 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
+# Copyright (c) The acados authors.
 #
 # This file is part of acados.
 #
@@ -35,7 +32,7 @@
 import numpy as np
 import os
 from .acados_model import AcadosModel
-from .utils import get_acados_path, J_to_idx, J_to_idx_slack
+from .utils import get_acados_path, J_to_idx, J_to_idx_slack, get_lib_ext
 
 class AcadosOcpDims:
     """
@@ -273,224 +270,224 @@ class AcadosOcpDims:
         if isinstance(nx, int) and nx > 0:
             self.__nx = nx
         else:
-            raise Exception('Invalid nx value, expected positive integer. Exiting.')
+            raise Exception('Invalid nx value, expected positive integer.')
 
     @nz.setter
     def nz(self, nz):
         if isinstance(nz, int) and nz > -1:
             self.__nz = nz
         else:
-            raise Exception('Invalid nz value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid nz value, expected nonnegative integer.')
 
     @nu.setter
     def nu(self, nu):
         if isinstance(nu, int) and nu > -1:
             self.__nu = nu
         else:
-            raise Exception('Invalid nu value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid nu value, expected nonnegative integer.')
 
     @np.setter
     def np(self, np):
         if isinstance(np, int) and np > -1:
             self.__np = np
         else:
-            raise Exception('Invalid np value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid np value, expected nonnegative integer.')
 
     @ny_0.setter
     def ny_0(self, ny_0):
         if isinstance(ny_0, int) and ny_0 > -1:
             self.__ny_0 = ny_0
         else:
-            raise Exception('Invalid ny_0 value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid ny_0 value, expected nonnegative integer.')
 
     @ny.setter
     def ny(self, ny):
         if isinstance(ny, int) and ny > -1:
             self.__ny = ny
         else:
-            raise Exception('Invalid ny value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid ny value, expected nonnegative integer.')
 
     @ny_e.setter
     def ny_e(self, ny_e):
         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.')
+            raise Exception('Invalid ny_e value, expected nonnegative integer.')
 
     @nr.setter
     def nr(self, nr):
         if isinstance(nr, int) and nr > -1:
             self.__nr = nr
         else:
-            raise Exception('Invalid nr value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid nr value, expected nonnegative integer.')
 
     @nr_e.setter
     def nr_e(self, nr_e):
         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.')
+            raise Exception('Invalid nr_e value, expected nonnegative integer.')
 
     @nh.setter
     def nh(self, nh):
         if isinstance(nh, int) and nh > -1:
             self.__nh = nh
         else:
-            raise Exception('Invalid nh value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid nh value, expected nonnegative integer.')
 
     @nh_e.setter
     def nh_e(self, nh_e):
         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.')
+            raise Exception('Invalid nh_e value, expected nonnegative integer.')
 
     @nphi.setter
     def nphi(self, nphi):
         if isinstance(nphi, int) and nphi > -1:
             self.__nphi = nphi
         else:
-            raise Exception('Invalid nphi value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid nphi value, expected nonnegative integer.')
 
     @nphi_e.setter
     def nphi_e(self, nphi_e):
         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.')
+            raise Exception('Invalid nphi_e value, expected nonnegative integer.')
 
     @nbx.setter
     def nbx(self, nbx):
         if isinstance(nbx, int) and nbx > -1:
             self.__nbx = nbx
         else:
-            raise Exception('Invalid nbx value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid nbx value, expected nonnegative integer.')
 
     @nbxe_0.setter
     def nbxe_0(self, nbxe_0):
         if isinstance(nbxe_0, int) and nbxe_0 > -1:
             self.__nbxe_0 = nbxe_0
         else:
-            raise Exception('Invalid nbxe_0 value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid nbxe_0 value, expected nonnegative integer.')
 
     @nbx_0.setter
     def nbx_0(self, nbx_0):
         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.')
+            raise Exception('Invalid nbx_0 value, expected nonnegative integer.')
 
     @nbx_e.setter
     def nbx_e(self, nbx_e):
         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.')
+            raise Exception('Invalid nbx_e value, expected nonnegative integer.')
 
     @nbu.setter
     def nbu(self, nbu):
         if isinstance(nbu, int) and nbu > -1:
             self.__nbu = nbu
         else:
-            raise Exception('Invalid nbu value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid nbu value, expected nonnegative integer.')
 
     @nsbx.setter
     def nsbx(self, nsbx):
         if isinstance(nsbx, int) and nsbx > -1:
             self.__nsbx = nsbx
         else:
-            raise Exception('Invalid nsbx value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid nsbx value, expected nonnegative integer.')
 
     @nsbx_e.setter
     def nsbx_e(self, nsbx_e):
         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.')
+            raise Exception('Invalid nsbx_e value, expected nonnegative integer.')
 
     @nsbu.setter
     def nsbu(self, nsbu):
         if isinstance(nsbu, int) and nsbu > -1:
             self.__nsbu = nsbu
         else:
-            raise Exception('Invalid nsbu value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid nsbu value, expected nonnegative integer.')
 
     @nsg.setter
     def nsg(self, nsg):
         if isinstance(nsg, int) and nsg > -1:
             self.__nsg = nsg
         else:
-            raise Exception('Invalid nsg value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid nsg value, expected nonnegative integer.')
 
     @nsg_e.setter
     def nsg_e(self, nsg_e):
         if isinstance(nsg_e, int) and nsg_e > -1:
             self.__nsg_e = nsg_e
         else:
-            raise Exception('Invalid nsg_e value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid nsg_e value, expected nonnegative integer.')
 
     @nsh.setter
     def nsh(self, nsh):
         if isinstance(nsh, int) and nsh > -1:
             self.__nsh = nsh
         else:
-            raise Exception('Invalid nsh value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid nsh value, expected nonnegative integer.')
 
     @nsh_e.setter
     def nsh_e(self, nsh_e):
         if isinstance(nsh_e, int) and nsh_e > -1:
             self.__nsh_e = nsh_e
         else:
-            raise Exception('Invalid nsh_e value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid nsh_e value, expected nonnegative integer.')
 
     @nsphi.setter
     def nsphi(self, nsphi):
         if isinstance(nsphi, int) and nsphi > -1:
             self.__nsphi = nsphi
         else:
-            raise Exception('Invalid nsphi value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid nsphi value, expected nonnegative integer.')
 
     @nsphi_e.setter
     def nsphi_e(self, nsphi_e):
         if isinstance(nsphi_e, int) and nsphi_e > -1:
             self.__nsphi_e = nsphi_e
         else:
-            raise Exception('Invalid nsphi_e value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid nsphi_e value, expected nonnegative integer.')
 
     @ns.setter
     def ns(self, ns):
         if isinstance(ns, int) and ns > -1:
             self.__ns = ns
         else:
-            raise Exception('Invalid ns value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid ns value, expected nonnegative integer.')
 
     @ns_e.setter
     def ns_e(self, ns_e):
         if isinstance(ns_e, int) and ns_e > -1:
             self.__ns_e = ns_e
         else:
-            raise Exception('Invalid ns_e value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid ns_e value, expected nonnegative integer.')
 
     @ng.setter
     def ng(self, ng):
         if isinstance(ng, int) and ng > -1:
             self.__ng = ng
         else:
-            raise Exception('Invalid ng value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid ng value, expected nonnegative integer.')
 
     @ng_e.setter
     def ng_e(self, ng_e):
         if isinstance(ng_e, int) and ng_e > -1:
             self.__ng_e = ng_e
         else:
-            raise Exception('Invalid ng_e value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid ng_e value, expected nonnegative integer.')
 
     @N.setter
     def N(self, N):
         if isinstance(N, int) and N > 0:
             self.__N = N
         else:
-            raise Exception('Invalid N value, expected positive integer. Exiting.')
+            raise Exception('Invalid N value, expected positive integer.')
 
     def set(self, attr, value):
         setattr(self, attr, value)
@@ -500,6 +497,12 @@ class AcadosOcpCost:
     """
     Class containing the numerical data of the cost:
 
+    NOTE: all cost terms, except for the terminal one are weighted with the corresponding time step.
+    This means given the time steps are :math:`\Delta t_0,..., \Delta t_N`, the total cost is given by:
+    :math:`c_\\text{total} = \Delta t_0 \cdot c_0(x_0, u_0, p_0, z_0) + ... + \Delta t_{N-1} \cdot c_{N-1}(x_0, u_0, p_0, z_0) + c_N(x_N, p_N)`.
+
+    This means the Lagrange cost term is given in continuous time, this makes up for a seeminglessly OCP discretization with a nonuniform time grid.
+
     In case of LINEAR_LS:
     stage cost is
     :math:`l(x,u,z) = || V_x \, x + V_u \, u + V_z \, z - y_\\text{ref}||^2_W`,
@@ -508,9 +511,15 @@ class AcadosOcpCost:
 
     In case of NONLINEAR_LS:
     stage cost is
-    :math:`l(x,u,z) = || y(x,u,z) - y_\\text{ref}||^2_W`,
+    :math:`l(x,u,z,p) = || y(x,u,z,p) - y_\\text{ref}||^2_W`,
+    terminal cost is
+    :math:`m(x,p) = || y^e(x,p) - y_\\text{ref}^e||^2_{W^e}`
+
+    In case of CONVEX_OVER_NONLINEAR:
+    stage cost is
+    :math:`l(x,u,p) = \psi(y(x,u,p) - y_\\text{ref}, p)`,
     terminal cost is
-    :math:`m(x) = || y^e(x) - y_\\text{ref}^e||^2_{W^e}`
+    :math:`m(x, p) = \psi^e (y^e(x,p) - y_\\text{ref}^e, p)`
     """
     def __init__(self):
         # initial stage
@@ -548,7 +557,7 @@ class AcadosOcpCost:
     @property
     def cost_type_0(self):
         """Cost type at initial shooting node (0)
-        -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS} or :code:`None`.
+        -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS, CONVEX_OVER_NONLINEAR} or :code:`None`.
         Default: :code:`None`.
 
             .. note:: Cost at initial stage is the same as for intermediate shooting nodes if not set differently explicitly.
@@ -604,10 +613,10 @@ class AcadosOcpCost:
 
     @yref_0.setter
     def yref_0(self, yref_0):
-        if isinstance(yref_0, np.ndarray):
+        if isinstance(yref_0, np.ndarray) and len(yref_0.shape) == 1:
             self.__yref_0 = yref_0
         else:
-            raise Exception('Invalid yref_0 value, expected numpy array. Exiting.')
+            raise Exception('Invalid yref_0 value, expected 1-dimensional numpy array.')
 
     @W_0.setter
     def W_0(self, W_0):
@@ -615,7 +624,7 @@ class AcadosOcpCost:
             self.__W_0 = W_0
         else:
             raise Exception('Invalid cost W_0 value. ' \
-                + 'Should be 2 dimensional numpy array. Exiting.')
+                + 'Should be 2 dimensional numpy array.')
 
     @Vx_0.setter
     def Vx_0(self, Vx_0):
@@ -623,7 +632,7 @@ class AcadosOcpCost:
             self.__Vx_0 = Vx_0
         else:
             raise Exception('Invalid cost Vx_0 value. ' \
-                + 'Should be 2 dimensional numpy array. Exiting.')
+                + 'Should be 2 dimensional numpy array.')
 
     @Vu_0.setter
     def Vu_0(self, Vu_0):
@@ -631,7 +640,7 @@ class AcadosOcpCost:
             self.__Vu_0 = Vu_0
         else:
             raise Exception('Invalid cost Vu_0 value. ' \
-                + 'Should be 2 dimensional numpy array. Exiting.')
+                + 'Should be 2 dimensional numpy array.')
 
     @Vz_0.setter
     def Vz_0(self, Vz_0):
@@ -639,21 +648,21 @@ class AcadosOcpCost:
             self.__Vz_0 = Vz_0
         else:
             raise Exception('Invalid cost Vz_0 value. ' \
-                + 'Should be 2 dimensional numpy array. Exiting.')
+                + 'Should be 2 dimensional numpy array.')
 
     @cost_ext_fun_type_0.setter
     def cost_ext_fun_type_0(self, cost_ext_fun_type_0):
         if cost_ext_fun_type_0 in ['casadi', 'generic']:
             self.__cost_ext_fun_type_0 = cost_ext_fun_type_0
         else:
-            raise Exception('Invalid cost_ext_fun_type_0 value, expected numpy array. Exiting.')
+            raise Exception('Invalid cost_ext_fun_type_0 value, expected numpy array.')
 
     # Lagrange term
     @property
     def cost_type(self):
         """
         Cost type at intermediate shooting nodes (1 to N-1)
-        -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS}.
+        -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS, CONVEX_OVER_NONLINEAR}.
         Default: 'LINEAR_LS'.
         """
         return self.__cost_type
@@ -695,28 +704,28 @@ class AcadosOcpCost:
 
     @property
     def Zl(self):
-        """:math:`Z_l` - diagonal of Hessian wrt lower slack at intermediate shooting nodes (1 to N-1).
+        """:math:`Z_l` - diagonal of Hessian wrt lower slack at intermediate shooting nodes (0 to N-1).
         Default: :code:`np.array([])`.
         """
         return self.__Zl
 
     @property
     def Zu(self):
-        """:math:`Z_u` - diagonal of Hessian wrt upper slack at intermediate shooting nodes (1 to N-1).
+        """:math:`Z_u` - diagonal of Hessian wrt upper slack at intermediate shooting nodes (0 to N-1).
         Default: :code:`np.array([])`.
         """
         return self.__Zu
 
     @property
     def zl(self):
-        """:math:`z_l` - gradient wrt lower slack at intermediate shooting nodes (1 to N-1).
+        """:math:`z_l` - gradient wrt lower slack at intermediate shooting nodes (0 to N-1).
         Default: :code:`np.array([])`.
         """
         return self.__zl
 
     @property
     def zu(self):
-        """:math:`z_u` - gradient wrt upper slack at intermediate shooting nodes (1 to N-1).
+        """:math:`z_u` - gradient wrt upper slack at intermediate shooting nodes (0 to N-1).
         Default: :code:`np.array([])`.
         """
         return self.__zu
@@ -731,19 +740,19 @@ class AcadosOcpCost:
 
     @cost_type.setter
     def cost_type(self, cost_type):
-        cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL')
+        cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL', 'CONVEX_OVER_NONLINEAR')
         if cost_type in cost_types:
             self.__cost_type = cost_type
         else:
-            raise Exception('Invalid cost_type value. Exiting.')
+            raise Exception('Invalid cost_type value.')
 
     @cost_type_0.setter
     def cost_type_0(self, cost_type_0):
-        cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL')
+        cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL', 'CONVEX_OVER_NONLINEAR')
         if cost_type_0 in cost_types:
             self.__cost_type_0 = cost_type_0
         else:
-            raise Exception('Invalid cost_type_0 value. Exiting.')
+            raise Exception('Invalid cost_type_0 value.')
 
     @W.setter
     def W(self, W):
@@ -751,7 +760,7 @@ class AcadosOcpCost:
             self.__W = W
         else:
             raise Exception('Invalid cost W value. ' \
-                + 'Should be 2 dimensional numpy array. Exiting.')
+                + 'Should be 2 dimensional numpy array.')
 
 
     @Vx.setter
@@ -760,7 +769,7 @@ class AcadosOcpCost:
             self.__Vx = Vx
         else:
             raise Exception('Invalid cost Vx value. ' \
-                + 'Should be 2 dimensional numpy array. Exiting.')
+                + 'Should be 2 dimensional numpy array.')
 
     @Vu.setter
     def Vu(self, Vu):
@@ -768,7 +777,7 @@ class AcadosOcpCost:
             self.__Vu = Vu
         else:
             raise Exception('Invalid cost Vu value. ' \
-                + 'Should be 2 dimensional numpy array. Exiting.')
+                + 'Should be 2 dimensional numpy array.')
 
     @Vz.setter
     def Vz(self, Vz):
@@ -776,56 +785,56 @@ class AcadosOcpCost:
             self.__Vz = Vz
         else:
             raise Exception('Invalid cost Vz value. ' \
-                + 'Should be 2 dimensional numpy array. Exiting.')
+                + 'Should be 2 dimensional numpy array.')
 
     @yref.setter
     def yref(self, yref):
-        if isinstance(yref, np.ndarray):
+        if isinstance(yref, np.ndarray) and len(yref.shape) == 1:
             self.__yref = yref
         else:
-            raise Exception('Invalid yref value, expected numpy array. Exiting.')
+            raise Exception('Invalid yref value, expected 1-dimensional numpy array.')
 
     @Zl.setter
     def Zl(self, Zl):
         if isinstance(Zl, np.ndarray):
             self.__Zl = Zl
         else:
-            raise Exception('Invalid Zl value, expected numpy array. Exiting.')
+            raise Exception('Invalid Zl value, expected numpy array.')
 
     @Zu.setter
     def Zu(self, Zu):
         if isinstance(Zu, np.ndarray):
             self.__Zu = Zu
         else:
-            raise Exception('Invalid Zu value, expected numpy array. Exiting.')
+            raise Exception('Invalid Zu value, expected numpy array.')
 
     @zl.setter
     def zl(self, zl):
         if isinstance(zl, np.ndarray):
             self.__zl = zl
         else:
-            raise Exception('Invalid zl value, expected numpy array. Exiting.')
+            raise Exception('Invalid zl value, expected numpy array.')
 
     @zu.setter
     def zu(self, zu):
         if isinstance(zu, np.ndarray):
             self.__zu = zu
         else:
-            raise Exception('Invalid zu value, expected numpy array. Exiting.')
+            raise Exception('Invalid zu value, expected numpy array.')
 
     @cost_ext_fun_type.setter
     def cost_ext_fun_type(self, cost_ext_fun_type):
         if cost_ext_fun_type in ['casadi', 'generic']:
             self.__cost_ext_fun_type = cost_ext_fun_type
         else:
-            raise Exception('Invalid cost_ext_fun_type value, expected numpy array. Exiting.')
+            raise Exception("Invalid cost_ext_fun_type value, expected one in ['casadi', 'generic'].")
 
     # Mayer term
     @property
     def cost_type_e(self):
         """
         Cost type at terminal shooting node (N)
-        -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS}.
+        -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS, CONVEX_OVER_NONLINEAR}.
         Default: 'LINEAR_LS'.
         """
         return self.__cost_type_e
@@ -881,7 +890,7 @@ class AcadosOcpCost:
 
     @property
     def cost_ext_fun_type_e(self):
-        """Type of external function for cost at intermediate shooting nodes (1 to N-1).
+        """Type of external function for cost at terminal shooting node (N).
         -- string in {casadi, generic}
         Default: :code:'casadi'.
         """
@@ -889,12 +898,12 @@ class AcadosOcpCost:
 
     @cost_type_e.setter
     def cost_type_e(self, cost_type_e):
-        cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL')
+        cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL', 'CONVEX_OVER_NONLINEAR')
 
         if cost_type_e in cost_types:
             self.__cost_type_e = cost_type_e
         else:
-            raise Exception('Invalid cost_type_e value. Exiting.')
+            raise Exception('Invalid cost_type_e value.')
 
     @W_e.setter
     def W_e(self, W_e):
@@ -902,7 +911,7 @@ class AcadosOcpCost:
             self.__W_e = W_e
         else:
             raise Exception('Invalid cost W_e value. ' \
-                + 'Should be 2 dimensional numpy array. Exiting.')
+                + 'Should be 2 dimensional numpy array.')
 
     @Vx_e.setter
     def Vx_e(self, Vx_e):
@@ -910,49 +919,49 @@ class AcadosOcpCost:
             self.__Vx_e = Vx_e
         else:
             raise Exception('Invalid cost Vx_e value. ' \
-                + 'Should be 2 dimensional numpy array. Exiting.')
+                + 'Should be 2 dimensional numpy array.')
 
     @yref_e.setter
     def yref_e(self, yref_e):
-        if isinstance(yref_e, np.ndarray):
+        if isinstance(yref_e, np.ndarray) and len(yref_e.shape) == 1:
             self.__yref_e = yref_e
         else:
-            raise Exception('Invalid yref_e value, expected numpy array. Exiting.')
+            raise Exception('Invalid yref_e value, expected 1-dimensional numpy array.')
 
     @Zl_e.setter
     def Zl_e(self, Zl_e):
         if isinstance(Zl_e, np.ndarray):
             self.__Zl_e = Zl_e
         else:
-            raise Exception('Invalid Zl_e value, expected numpy array. Exiting.')
+            raise Exception('Invalid Zl_e value, expected numpy array.')
 
     @Zu_e.setter
     def Zu_e(self, Zu_e):
         if isinstance(Zu_e, np.ndarray):
             self.__Zu_e = Zu_e
         else:
-            raise Exception('Invalid Zu_e value, expected numpy array. Exiting.')
+            raise Exception('Invalid Zu_e value, expected numpy array.')
 
     @zl_e.setter
     def zl_e(self, zl_e):
         if isinstance(zl_e, np.ndarray):
             self.__zl_e = zl_e
         else:
-            raise Exception('Invalid zl_e value, expected numpy array. Exiting.')
+            raise Exception('Invalid zl_e value, expected numpy array.')
 
     @zu_e.setter
     def zu_e(self, zu_e):
         if isinstance(zu_e, np.ndarray):
             self.__zu_e = zu_e
         else:
-            raise Exception('Invalid zu_e value, expected numpy array. Exiting.')
+            raise Exception('Invalid zu_e value, expected numpy array.')
 
     @cost_ext_fun_type_e.setter
     def cost_ext_fun_type_e(self, cost_ext_fun_type_e):
         if cost_ext_fun_type_e in ['casadi', 'generic']:
             self.__cost_ext_fun_type_e = cost_ext_fun_type_e
         else:
-            raise Exception('Invalid cost_ext_fun_type_e value, expected numpy array. Exiting.')
+            raise Exception("Invalid cost_ext_fun_type_e value, expected one in ['casadi', 'generic'].")
 
     def set(self, attr, value):
         setattr(self, attr, value)
@@ -1578,7 +1587,7 @@ class AcadosOcpConstraints:
             self.__constr_type = constr_type
         else:
             raise Exception('Invalid constr_type value. Possible values are:\n\n' \
-                    + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type + '.\n\nExiting.')
+                    + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type + '.\n\n')
 
     @constr_type_e.setter
     def constr_type_e(self, constr_type_e):
@@ -1587,7 +1596,7 @@ class AcadosOcpConstraints:
             self.__constr_type_e = constr_type_e
         else:
             raise Exception('Invalid constr_type_e value. Possible values are:\n\n' \
-                    + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type_e + '.\n\nExiting.')
+                    + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type_e + '.\n\n')
 
     # initial x
     @lbx_0.setter
@@ -1595,35 +1604,35 @@ class AcadosOcpConstraints:
         if isinstance(lbx_0, np.ndarray):
             self.__lbx_0 = lbx_0
         else:
-            raise Exception('Invalid lbx_0 value. Exiting.')
+            raise Exception('Invalid lbx_0 value.')
 
     @ubx_0.setter
     def ubx_0(self, ubx_0):
         if isinstance(ubx_0, np.ndarray):
             self.__ubx_0 = ubx_0
         else:
-            raise Exception('Invalid ubx_0 value. Exiting.')
+            raise Exception('Invalid ubx_0 value.')
 
     @idxbx_0.setter
     def idxbx_0(self, idxbx_0):
         if isinstance(idxbx_0, np.ndarray):
             self.__idxbx_0 = idxbx_0
         else:
-            raise Exception('Invalid idxbx_0 value. Exiting.')
+            raise Exception('Invalid idxbx_0 value.')
 
     @Jbx_0.setter
     def Jbx_0(self, Jbx_0):
         if isinstance(Jbx_0, np.ndarray):
             self.__idxbx_0 = J_to_idx(Jbx_0)
         else:
-            raise Exception('Invalid Jbx_0 value. Exiting.')
+            raise Exception('Invalid Jbx_0 value.')
 
     @idxbxe_0.setter
     def idxbxe_0(self, idxbxe_0):
         if isinstance(idxbxe_0, np.ndarray):
             self.__idxbxe_0 = idxbxe_0
         else:
-            raise Exception('Invalid idxbxe_0 value. Exiting.')
+            raise Exception('Invalid idxbxe_0 value.')
 
 
     @x0.setter
@@ -1634,7 +1643,7 @@ class AcadosOcpConstraints:
             self.__idxbx_0 = np.arange(x0.size)
             self.__idxbxe_0 = np.arange(x0.size)
         else:
-            raise Exception('Invalid x0 value. Exiting.')
+            raise Exception('Invalid x0 value.')
 
     # bounds on x
     @lbx.setter
@@ -1642,28 +1651,28 @@ class AcadosOcpConstraints:
         if isinstance(lbx, np.ndarray):
             self.__lbx = lbx
         else:
-            raise Exception('Invalid lbx value. Exiting.')
+            raise Exception('Invalid lbx value.')
 
     @ubx.setter
     def ubx(self, ubx):
         if isinstance(ubx, np.ndarray):
             self.__ubx = ubx
         else:
-            raise Exception('Invalid ubx value. Exiting.')
+            raise Exception('Invalid ubx value.')
 
     @idxbx.setter
     def idxbx(self, idxbx):
         if isinstance(idxbx, np.ndarray):
             self.__idxbx = idxbx
         else:
-            raise Exception('Invalid idxbx value. Exiting.')
+            raise Exception('Invalid idxbx value.')
 
     @Jbx.setter
     def Jbx(self, Jbx):
         if isinstance(Jbx, np.ndarray):
             self.__idxbx = J_to_idx(Jbx)
         else:
-            raise Exception('Invalid Jbx value. Exiting.')
+            raise Exception('Invalid Jbx value.')
 
     # bounds on u
     @lbu.setter
@@ -1671,28 +1680,28 @@ class AcadosOcpConstraints:
         if isinstance(lbu, np.ndarray):
             self.__lbu = lbu
         else:
-            raise Exception('Invalid lbu value. Exiting.')
+            raise Exception('Invalid lbu value.')
 
     @ubu.setter
     def ubu(self, ubu):
         if isinstance(ubu, np.ndarray):
             self.__ubu = ubu
         else:
-            raise Exception('Invalid ubu value. Exiting.')
+            raise Exception('Invalid ubu value.')
 
     @idxbu.setter
     def idxbu(self, idxbu):
         if isinstance(idxbu, np.ndarray):
             self.__idxbu = idxbu
         else:
-            raise Exception('Invalid idxbu value. Exiting.')
+            raise Exception('Invalid idxbu value.')
 
     @Jbu.setter
     def Jbu(self, Jbu):
         if isinstance(Jbu, np.ndarray):
             self.__idxbu = J_to_idx(Jbu)
         else:
-            raise Exception('Invalid Jbu value. Exiting.')
+            raise Exception('Invalid Jbu value.')
 
     # bounds on x at shooting node N
     @lbx_e.setter
@@ -1700,28 +1709,28 @@ class AcadosOcpConstraints:
         if isinstance(lbx_e, np.ndarray):
             self.__lbx_e = lbx_e
         else:
-            raise Exception('Invalid lbx_e value. Exiting.')
+            raise Exception('Invalid lbx_e value.')
 
     @ubx_e.setter
     def ubx_e(self, ubx_e):
         if isinstance(ubx_e, np.ndarray):
             self.__ubx_e = ubx_e
         else:
-            raise Exception('Invalid ubx_e value. Exiting.')
+            raise Exception('Invalid ubx_e value.')
 
     @idxbx_e.setter
     def idxbx_e(self, idxbx_e):
         if isinstance(idxbx_e, np.ndarray):
             self.__idxbx_e = idxbx_e
         else:
-            raise Exception('Invalid idxbx_e value. Exiting.')
+            raise Exception('Invalid idxbx_e value.')
 
     @Jbx_e.setter
     def Jbx_e(self, Jbx_e):
         if isinstance(Jbx_e, np.ndarray):
             self.__idxbx_e = J_to_idx(Jbx_e)
         else:
-            raise Exception('Invalid Jbx_e value. Exiting.')
+            raise Exception('Invalid Jbx_e value.')
 
     # polytopic constraints
     @D.setter
@@ -1730,7 +1739,7 @@ class AcadosOcpConstraints:
             self.__D = D
         else:
             raise Exception('Invalid constraint D value.' \
-                + 'Should be 2 dimensional numpy array. Exiting.')
+                + 'Should be 2 dimensional numpy array.')
 
     @C.setter
     def C(self, C):
@@ -1738,21 +1747,21 @@ class AcadosOcpConstraints:
             self.__C = C
         else:
             raise Exception('Invalid constraint C value.' \
-                + 'Should be 2 dimensional numpy array. Exiting.')
+                + 'Should be 2 dimensional numpy array.')
 
     @lg.setter
     def lg(self, lg):
         if isinstance(lg, np.ndarray):
             self.__lg = lg
         else:
-            raise Exception('Invalid lg value. Exiting.')
+            raise Exception('Invalid lg value.')
 
     @ug.setter
     def ug(self, ug):
         if isinstance(ug, np.ndarray):
             self.__ug = ug
         else:
-            raise Exception('Invalid ug value. Exiting.')
+            raise Exception('Invalid ug value.')
 
     # polytopic constraints at shooting node N
     @C_e.setter
@@ -1761,21 +1770,21 @@ class AcadosOcpConstraints:
             self.__C_e = C_e
         else:
             raise Exception('Invalid constraint C_e value.' \
-                + 'Should be 2 dimensional numpy array. Exiting.')
+                + 'Should be 2 dimensional numpy array.')
 
     @lg_e.setter
     def lg_e(self, lg_e):
         if isinstance(lg_e, np.ndarray):
             self.__lg_e = lg_e
         else:
-            raise Exception('Invalid lg_e value. Exiting.')
+            raise Exception('Invalid lg_e value.')
 
     @ug_e.setter
     def ug_e(self, ug_e):
         if isinstance(ug_e, np.ndarray):
             self.__ug_e = ug_e
         else:
-            raise Exception('Invalid ug_e value. Exiting.')
+            raise Exception('Invalid ug_e value.')
 
     # nonlinear constraints
     @lh.setter
@@ -1783,14 +1792,14 @@ class AcadosOcpConstraints:
         if isinstance(lh, np.ndarray):
             self.__lh = lh
         else:
-            raise Exception('Invalid lh value. Exiting.')
+            raise Exception('Invalid lh value.')
 
     @uh.setter
     def uh(self, uh):
         if isinstance(uh, np.ndarray):
             self.__uh = uh
         else:
-            raise Exception('Invalid uh value. Exiting.')
+            raise Exception('Invalid uh value.')
 
     # convex-over-nonlinear constraints
     @lphi.setter
@@ -1798,14 +1807,14 @@ class AcadosOcpConstraints:
         if isinstance(lphi, np.ndarray):
             self.__lphi = lphi
         else:
-            raise Exception('Invalid lphi value. Exiting.')
+            raise Exception('Invalid lphi value.')
 
     @uphi.setter
     def uphi(self, uphi):
         if isinstance(uphi, np.ndarray):
             self.__uphi = uphi
         else:
-            raise Exception('Invalid uphi value. Exiting.')
+            raise Exception('Invalid uphi value.')
 
     # nonlinear constraints at shooting node N
     @lh_e.setter
@@ -1813,14 +1822,14 @@ class AcadosOcpConstraints:
         if isinstance(lh_e, np.ndarray):
             self.__lh_e = lh_e
         else:
-            raise Exception('Invalid lh_e value. Exiting.')
+            raise Exception('Invalid lh_e value.')
 
     @uh_e.setter
     def uh_e(self, uh_e):
         if isinstance(uh_e, np.ndarray):
             self.__uh_e = uh_e
         else:
-            raise Exception('Invalid uh_e value. Exiting.')
+            raise Exception('Invalid uh_e value.')
 
     # convex-over-nonlinear constraints at shooting node N
     @lphi_e.setter
@@ -1828,14 +1837,14 @@ class AcadosOcpConstraints:
         if isinstance(lphi_e, np.ndarray):
             self.__lphi_e = lphi_e
         else:
-            raise Exception('Invalid lphi_e value. Exiting.')
+            raise Exception('Invalid lphi_e value.')
 
     @uphi_e.setter
     def uphi_e(self, uphi_e):
         if isinstance(uphi_e, np.ndarray):
             self.__uphi_e = uphi_e
         else:
-            raise Exception('Invalid uphi_e value. Exiting.')
+            raise Exception('Invalid uphi_e value.')
 
     # SLACK bounds
     # soft bounds on x
@@ -1844,28 +1853,28 @@ class AcadosOcpConstraints:
         if isinstance(lsbx, np.ndarray):
             self.__lsbx = lsbx
         else:
-            raise Exception('Invalid lsbx value. Exiting.')
+            raise Exception('Invalid lsbx value.')
 
     @usbx.setter
     def usbx(self, usbx):
         if isinstance(usbx, np.ndarray):
             self.__usbx = usbx
         else:
-            raise Exception('Invalid usbx value. Exiting.')
+            raise Exception('Invalid usbx value.')
 
     @idxsbx.setter
     def idxsbx(self, idxsbx):
         if isinstance(idxsbx, np.ndarray):
             self.__idxsbx = idxsbx
         else:
-            raise Exception('Invalid idxsbx value. Exiting.')
+            raise Exception('Invalid idxsbx value.')
 
     @Jsbx.setter
     def Jsbx(self, Jsbx):
         if isinstance(Jsbx, np.ndarray):
             self.__idxsbx = J_to_idx_slack(Jsbx)
         else:
-            raise Exception('Invalid Jsbx value, expected numpy array. Exiting.')
+            raise Exception('Invalid Jsbx value, expected numpy array.')
 
     # soft bounds on u
     @lsbu.setter
@@ -1873,28 +1882,28 @@ class AcadosOcpConstraints:
         if isinstance(lsbu, np.ndarray):
             self.__lsbu = lsbu
         else:
-            raise Exception('Invalid lsbu value. Exiting.')
+            raise Exception('Invalid lsbu value.')
 
     @usbu.setter
     def usbu(self, usbu):
         if isinstance(usbu, np.ndarray):
             self.__usbu = usbu
         else:
-            raise Exception('Invalid usbu value. Exiting.')
+            raise Exception('Invalid usbu value.')
 
     @idxsbu.setter
     def idxsbu(self, idxsbu):
         if isinstance(idxsbu, np.ndarray):
             self.__idxsbu = idxsbu
         else:
-            raise Exception('Invalid idxsbu value. Exiting.')
+            raise Exception('Invalid idxsbu value.')
 
     @Jsbu.setter
     def Jsbu(self, Jsbu):
         if isinstance(Jsbu, np.ndarray):
             self.__idxsbu = J_to_idx_slack(Jsbu)
         else:
-            raise Exception('Invalid Jsbu value. Exiting.')
+            raise Exception('Invalid Jsbu value.')
 
     # soft bounds on x at shooting node N
     @lsbx_e.setter
@@ -1902,28 +1911,28 @@ class AcadosOcpConstraints:
         if isinstance(lsbx_e, np.ndarray):
             self.__lsbx_e = lsbx_e
         else:
-            raise Exception('Invalid lsbx_e value. Exiting.')
+            raise Exception('Invalid lsbx_e value.')
 
     @usbx_e.setter
     def usbx_e(self, usbx_e):
         if isinstance(usbx_e, np.ndarray):
             self.__usbx_e = usbx_e
         else:
-            raise Exception('Invalid usbx_e value. Exiting.')
+            raise Exception('Invalid usbx_e value.')
 
     @idxsbx_e.setter
     def idxsbx_e(self, idxsbx_e):
         if isinstance(idxsbx_e, np.ndarray):
             self.__idxsbx_e = idxsbx_e
         else:
-            raise Exception('Invalid idxsbx_e value. Exiting.')
+            raise Exception('Invalid idxsbx_e value.')
 
     @Jsbx_e.setter
     def Jsbx_e(self, Jsbx_e):
         if isinstance(Jsbx_e, np.ndarray):
             self.__idxsbx_e = J_to_idx_slack(Jsbx_e)
         else:
-            raise Exception('Invalid Jsbx_e value. Exiting.')
+            raise Exception('Invalid Jsbx_e value.')
 
 
     # soft bounds on general linear constraints
@@ -1932,28 +1941,28 @@ class AcadosOcpConstraints:
         if isinstance(lsg, np.ndarray):
             self.__lsg = lsg
         else:
-            raise Exception('Invalid lsg value. Exiting.')
+            raise Exception('Invalid lsg value.')
 
     @usg.setter
     def usg(self, usg):
         if isinstance(usg, np.ndarray):
             self.__usg = usg
         else:
-            raise Exception('Invalid usg value. Exiting.')
+            raise Exception('Invalid usg value.')
 
     @idxsg.setter
     def idxsg(self, idxsg):
         if isinstance(idxsg, np.ndarray):
             self.__idxsg = idxsg
         else:
-            raise Exception('Invalid idxsg value. Exiting.')
+            raise Exception('Invalid idxsg value.')
 
     @Jsg.setter
     def Jsg(self, Jsg):
         if isinstance(Jsg, np.ndarray):
             self.__idxsg = J_to_idx_slack(Jsg)
         else:
-            raise Exception('Invalid Jsg value, expected numpy array. Exiting.')
+            raise Exception('Invalid Jsg value, expected numpy array.')
 
 
     # soft bounds on nonlinear constraints
@@ -1962,21 +1971,21 @@ class AcadosOcpConstraints:
         if isinstance(lsh, np.ndarray):
             self.__lsh = lsh
         else:
-            raise Exception('Invalid lsh value. Exiting.')
+            raise Exception('Invalid lsh value.')
 
     @ush.setter
     def ush(self, ush):
         if isinstance(ush, np.ndarray):
             self.__ush = ush
         else:
-            raise Exception('Invalid ush value. Exiting.')
+            raise Exception('Invalid ush value.')
 
     @idxsh.setter
     def idxsh(self, idxsh):
         if isinstance(idxsh, np.ndarray):
             self.__idxsh = idxsh
         else:
-            raise Exception('Invalid idxsh value. Exiting.')
+            raise Exception('Invalid idxsh value.')
 
 
     @Jsh.setter
@@ -1984,7 +1993,7 @@ class AcadosOcpConstraints:
         if isinstance(Jsh, np.ndarray):
             self.__idxsh = J_to_idx_slack(Jsh)
         else:
-            raise Exception('Invalid Jsh value, expected numpy array. Exiting.')
+            raise Exception('Invalid Jsh value, expected numpy array.')
 
     # soft bounds on convex-over-nonlinear constraints
     @lsphi.setter
@@ -1992,28 +2001,28 @@ class AcadosOcpConstraints:
         if isinstance(lsphi, np.ndarray):
             self.__lsphi = lsphi
         else:
-            raise Exception('Invalid lsphi value. Exiting.')
+            raise Exception('Invalid lsphi value.')
 
     @usphi.setter
     def usphi(self, usphi):
         if isinstance(usphi, np.ndarray):
             self.__usphi = usphi
         else:
-            raise Exception('Invalid usphi value. Exiting.')
+            raise Exception('Invalid usphi value.')
 
     @idxsphi.setter
     def idxsphi(self, idxsphi):
         if isinstance(idxsphi, np.ndarray):
             self.__idxsphi = idxsphi
         else:
-            raise Exception('Invalid idxsphi value. Exiting.')
+            raise Exception('Invalid idxsphi value.')
 
     @Jsphi.setter
     def Jsphi(self, Jsphi):
         if isinstance(Jsphi, np.ndarray):
             self.__idxsphi = J_to_idx_slack(Jsphi)
         else:
-            raise Exception('Invalid Jsphi value, expected numpy array. Exiting.')
+            raise Exception('Invalid Jsphi value, expected numpy array.')
 
     # soft bounds on general linear constraints at shooting node N
     @lsg_e.setter
@@ -2021,28 +2030,28 @@ class AcadosOcpConstraints:
         if isinstance(lsg_e, np.ndarray):
             self.__lsg_e = lsg_e
         else:
-            raise Exception('Invalid lsg_e value. Exiting.')
+            raise Exception('Invalid lsg_e value.')
 
     @usg_e.setter
     def usg_e(self, usg_e):
         if isinstance(usg_e, np.ndarray):
             self.__usg_e = usg_e
         else:
-            raise Exception('Invalid usg_e value. Exiting.')
+            raise Exception('Invalid usg_e value.')
 
     @idxsg_e.setter
     def idxsg_e(self, idxsg_e):
         if isinstance(idxsg_e, np.ndarray):
             self.__idxsg_e = idxsg_e
         else:
-            raise Exception('Invalid idxsg_e value. Exiting.')
+            raise Exception('Invalid idxsg_e value.')
 
     @Jsg_e.setter
     def Jsg_e(self, Jsg_e):
         if isinstance(Jsg_e, np.ndarray):
             self.__idxsg_e = J_to_idx_slack(Jsg_e)
         else:
-            raise Exception('Invalid Jsg_e value, expected numpy array. Exiting.')
+            raise Exception('Invalid Jsg_e value, expected numpy array.')
 
     # soft bounds on nonlinear constraints at shooting node N
     @lsh_e.setter
@@ -2050,28 +2059,28 @@ class AcadosOcpConstraints:
         if isinstance(lsh_e, np.ndarray):
             self.__lsh_e = lsh_e
         else:
-            raise Exception('Invalid lsh_e value. Exiting.')
+            raise Exception('Invalid lsh_e value.')
 
     @ush_e.setter
     def ush_e(self, ush_e):
         if isinstance(ush_e, np.ndarray):
             self.__ush_e = ush_e
         else:
-            raise Exception('Invalid ush_e value. Exiting.')
+            raise Exception('Invalid ush_e value.')
 
     @idxsh_e.setter
     def idxsh_e(self, idxsh_e):
         if isinstance(idxsh_e, np.ndarray):
             self.__idxsh_e = idxsh_e
         else:
-            raise Exception('Invalid idxsh_e value. Exiting.')
+            raise Exception('Invalid idxsh_e value.')
 
     @Jsh_e.setter
     def Jsh_e(self, Jsh_e):
         if isinstance(Jsh_e, np.ndarray):
             self.__idxsh_e = J_to_idx_slack(Jsh_e)
         else:
-            raise Exception('Invalid Jsh_e value, expected numpy array. Exiting.')
+            raise Exception('Invalid Jsh_e value, expected numpy array.')
 
 
     # soft bounds on convex-over-nonlinear constraints at shooting node N
@@ -2080,28 +2089,28 @@ class AcadosOcpConstraints:
         if isinstance(lsphi_e, np.ndarray):
             self.__lsphi_e = lsphi_e
         else:
-            raise Exception('Invalid lsphi_e value. Exiting.')
+            raise Exception('Invalid lsphi_e value.')
 
     @usphi_e.setter
     def usphi_e(self, usphi_e):
         if isinstance(usphi_e, np.ndarray):
             self.__usphi_e = usphi_e
         else:
-            raise Exception('Invalid usphi_e value. Exiting.')
+            raise Exception('Invalid usphi_e value.')
 
     @idxsphi_e.setter
     def idxsphi_e(self, idxsphi_e):
         if isinstance(idxsphi_e, np.ndarray):
             self.__idxsphi_e = idxsphi_e
         else:
-            raise Exception('Invalid idxsphi_e value. Exiting.')
+            raise Exception('Invalid idxsphi_e value.')
 
     @Jsphi_e.setter
     def Jsphi_e(self, Jsphi_e):
         if isinstance(Jsphi_e, np.ndarray):
             self.__idxsphi_e = J_to_idx_slack(Jsphi_e)
         else:
-            raise Exception('Invalid Jsphi_e value. Exiting.')
+            raise Exception('Invalid Jsphi_e value.')
 
     def set(self, attr, value):
         setattr(self, attr, value)
@@ -2124,6 +2133,7 @@ class AcadosOcpOptions:
         self.__sim_method_num_stages  = 4                     # number of stages in the integrator
         self.__sim_method_num_steps   = 1                     # number of steps in the integrator
         self.__sim_method_newton_iter = 3                     # number of Newton iterations in simulation method
+        self.__sim_method_newton_tol = 0.0
         self.__sim_method_jac_reuse = 0
         self.__qp_solver_tol_stat = None                      # QP solver stationarity tolerance
         self.__qp_solver_tol_eq   = None                      # QP solver equality tolerance
@@ -2132,16 +2142,17 @@ class AcadosOcpOptions:
         self.__qp_solver_iter_max = 50                        # QP solver max iter
         self.__qp_solver_cond_N = None                        # QP solver: new horizon after partial condensing
         self.__qp_solver_warm_start = 0
+        self.__qp_solver_cond_ric_alg = 1
+        self.__qp_solver_ric_alg = 1
         self.__nlp_solver_tol_stat = 1e-6                     # NLP solver stationarity tolerance
         self.__nlp_solver_tol_eq   = 1e-6                     # NLP solver equality tolerance
         self.__nlp_solver_tol_ineq = 1e-6                     # NLP solver inequality
         self.__nlp_solver_tol_comp = 1e-6                     # NLP solver complementarity
         self.__nlp_solver_max_iter = 100                      # NLP solver maximum number of iterations
+        self.__nlp_solver_ext_qp_res = 0
         self.__Tsim = None                                    # automatically calculated as tf/N
         self.__print_level = 0                                # print level
         self.__initialize_t_slacks = 0                        # possible values: 0, 1
-        self.__model_external_shared_lib_dir   = None         # path to the the .so lib
-        self.__model_external_shared_lib_name  = None         # name of the the .so lib
         self.__regularize_method = None
         self.__time_steps = None
         self.__shooting_nodes = None
@@ -2156,16 +2167,93 @@ class AcadosOcpOptions:
         self.__full_step_dual = 0
         self.__eps_sufficient_descent = 1e-4
         self.__hpipm_mode = 'BALANCE'
-
+        # TODO: move those out? they are more about generation than about the acados OCP solver.
+        self.__ext_fun_compile_flags = '-O2'
+        self.__model_external_shared_lib_dir   = None         # path to the the .so lib
+        self.__model_external_shared_lib_name  = None         # name of the the .so lib
+        self.__custom_update_filename = ''
+        self.__custom_update_header_filename = ''
+        self.__custom_templates = []
+        self.__custom_update_copy = True
 
     @property
     def qp_solver(self):
         """QP solver to be used in the NLP solver.
-        String in ('PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP').
+        String in ('PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', 'FULL_CONDENSING_DAQP').
         Default: 'PARTIAL_CONDENSING_HPIPM'.
         """
         return self.__qp_solver
 
+    @property
+    def ext_fun_compile_flags(self):
+        """
+        String with compiler flags for external function compilation.
+        Default: '-O2'.
+        """
+        return self.__ext_fun_compile_flags
+
+
+    @property
+    def custom_update_filename(self):
+        """
+        Filename of the custom C function to update solver data and parameters in between solver calls
+
+        This file has to implement the functions
+        int custom_update_init_function([model.name]_solver_capsule* capsule);
+        int custom_update_function([model.name]_solver_capsule* capsule);
+        int custom_update_terminate_function([model.name]_solver_capsule* capsule);
+
+
+        Default: ''.
+        """
+        return self.__custom_update_filename
+
+
+    @property
+    def custom_templates(self):
+        """
+        List of tuples of the form:
+        (input_filename, output_filename)
+
+        Custom templates are render in OCP solver generation.
+
+        Default: [].
+        """
+        return self.__custom_templates
+
+
+    @property
+    def custom_update_header_filename(self):
+        """
+        Header filename of the custom C function to update solver data and parameters in between solver calls
+
+        This file has to declare the custom_update functions and look as follows:
+
+        ```
+        // Called at the end of solver creation.
+        // This is allowed to allocate memory and store the pointer to it into capsule->custom_update_memory.
+        int custom_update_init_function([model.name]_solver_capsule* capsule);
+
+        // Custom update function that can be called between solver calls
+        int custom_update_function([model.name]_solver_capsule* capsule, double* data, int data_len);
+
+        // Called just before destroying the solver.
+        // Responsible to free allocated memory, stored at capsule->custom_update_memory.
+        int custom_update_terminate_function([model.name]_solver_capsule* capsule);
+
+        Default: ''.
+        """
+        return self.__custom_update_header_filename
+
+    @property
+    def custom_update_copy(self):
+        """
+        Boolean;
+        If True, the custom update function files are copied into the `code_export_directory`.
+        """
+        return self.__custom_update_copy
+
+
     @property
     def hpipm_mode(self):
         """
@@ -2230,6 +2318,13 @@ class AcadosOcpOptions:
         """Regularization method for the Hessian.
         String in ('NO_REGULARIZE', 'MIRROR', 'PROJECT', 'PROJECT_REDUC_HESS', 'CONVEXIFY') or :code:`None`.
 
+        - MIRROR: performs eigenvalue decomposition H = V^T D V and sets D_ii = max(eps, abs(D_ii))
+        - PROJECT: performs eigenvalue decomposition H = V^T D V and sets D_ii = max(eps, D_ii)
+        - CONVEXIFY: Algorithm 6 from Verschueren2017, https://cdn.syscop.de/publications/Verschueren2017.pdf
+        - PROJECT_REDUC_HESS: experimental
+
+        Note: default eps = 1e-4
+
         Default: :code:`None`.
         """
         return self.__regularize_method
@@ -2279,6 +2374,15 @@ class AcadosOcpOptions:
         """
         return self.__sim_method_newton_iter
 
+    @property
+    def sim_method_newton_tol(self):
+        """
+        Tolerance of Newton system in simulation method.
+        Type: float: 0.0 means not used
+        Default: 0.0
+        """
+        return self.__sim_method_newton_tol
+
     @property
     def sim_method_jac_reuse(self):
         """
@@ -2328,10 +2432,42 @@ class AcadosOcpOptions:
 
     @property
     def qp_solver_warm_start(self):
-        """QP solver: Warm starting.
-        0: no warm start; 1: warm start; 2: hot start."""
+        """
+        QP solver: Warm starting.
+        0: no warm start; 1: warm start; 2: hot start.
+        Default: 0
+        """
         return self.__qp_solver_warm_start
 
+    @property
+    def qp_solver_cond_ric_alg(self):
+        """
+        QP solver: Determines which algorithm is used in HPIPM condensing.
+        0: dont factorize hessian in the condensing; 1: factorize.
+        Default: 1
+        """
+        return self.__qp_solver_cond_ric_alg
+
+    @property
+    def qp_solver_ric_alg(self):
+        """
+        QP solver: Determines which algorithm is used in HPIPM OCP QP solver.
+        0 classical Riccati, 1 square-root Riccati.
+
+        Note: taken from [HPIPM paper]:
+
+        (a) the classical implementation requires the reduced Hessian with respect to the dynamics
+            equality constraints to be positive definite, but allows the full-space Hessian to be indefinite)
+        (b) the square-root implementation, which in order to reduce the flop count employs the Cholesky
+            factorization of the Riccati recursion matrix, and therefore requires the full-space Hessian to be positive definite
+
+        [HPIPM paper]: HPIPM: a high-performance quadratic programming framework for model predictive control, Frison and Diehl, 2020
+        https://cdn.syscop.de/publications/Frison2020a.pdf
+
+        Default: 1
+        """
+        return self.__qp_solver_ric_alg
+
     @property
     def qp_solver_iter_max(self):
         """
@@ -2429,6 +2565,15 @@ class AcadosOcpOptions:
         """NLP solver inequality tolerance"""
         return self.__nlp_solver_tol_ineq
 
+    @property
+    def nlp_solver_ext_qp_res(self):
+        """Determines if residuals of QP are computed externally within NLP solver (for debugging)
+
+        Type: int; 0 or 1;
+        Default: 0.
+        """
+        return self.__nlp_solver_ext_qp_res
+
     @property
     def nlp_solver_tol_comp(self):
         """NLP solver complementarity tolerance"""
@@ -2531,12 +2676,13 @@ class AcadosOcpOptions:
     def qp_solver(self, qp_solver):
         qp_solvers = ('PARTIAL_CONDENSING_HPIPM', \
                 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', \
-                'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP')
+                'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', \
+                'FULL_CONDENSING_DAQP')
         if qp_solver in qp_solvers:
             self.__qp_solver = qp_solver
         else:
             raise Exception('Invalid qp_solver value. Possible values are:\n\n' \
-                    + ',\n'.join(qp_solvers) + '.\n\nYou have: ' + qp_solver + '.\n\nExiting.')
+                    + ',\n'.join(qp_solvers) + '.\n\nYou have: ' + qp_solver + '.\n\n')
 
     @regularize_method.setter
     def regularize_method(self, regularize_method):
@@ -2546,7 +2692,7 @@ class AcadosOcpOptions:
             self.__regularize_method = regularize_method
         else:
             raise Exception('Invalid regularize_method value. Possible values are:\n\n' \
-                    + ',\n'.join(regularize_methods) + '.\n\nYou have: ' + regularize_method + '.\n\nExiting.')
+                    + ',\n'.join(regularize_methods) + '.\n\nYou have: ' + regularize_method + '.\n\n')
 
     @collocation_type.setter
     def collocation_type(self, collocation_type):
@@ -2555,7 +2701,7 @@ class AcadosOcpOptions:
             self.__collocation_type = collocation_type
         else:
             raise Exception('Invalid collocation_type value. Possible values are:\n\n' \
-                    + ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\nExiting.')
+                    + ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\n')
 
     @hpipm_mode.setter
     def hpipm_mode(self, hpipm_mode):
@@ -2564,7 +2710,48 @@ class AcadosOcpOptions:
             self.__hpipm_mode = hpipm_mode
         else:
             raise Exception('Invalid hpipm_mode value. Possible values are:\n\n' \
-                    + ',\n'.join(hpipm_modes) + '.\n\nYou have: ' + hpipm_mode + '.\n\nExiting.')
+                    + ',\n'.join(hpipm_modes) + '.\n\nYou have: ' + hpipm_mode + '.\n\n')
+
+    @ext_fun_compile_flags.setter
+    def ext_fun_compile_flags(self, ext_fun_compile_flags):
+        if isinstance(ext_fun_compile_flags, str):
+            self.__ext_fun_compile_flags = ext_fun_compile_flags
+        else:
+            raise Exception('Invalid ext_fun_compile_flags, expected a string.\n')
+
+
+    @custom_update_filename.setter
+    def custom_update_filename(self, custom_update_filename):
+        if isinstance(custom_update_filename, str):
+            self.__custom_update_filename = custom_update_filename
+        else:
+            raise Exception('Invalid custom_update_filename, expected a string.\n')
+
+    @custom_templates.setter
+    def custom_templates(self, custom_templates):
+        if not isinstance(custom_templates, list):
+            raise Exception('Invalid custom_templates, expected a list.\n')
+        for tup in custom_templates:
+            if not isinstance(tup, tuple):
+                raise Exception('Invalid custom_templates, shoubld be list of tuples.\n')
+            for s in tup:
+                if not isinstance(s, str):
+                    raise Exception('Invalid custom_templates, shoubld be list of tuples of strings.\n')
+        self.__custom_templates = custom_templates
+
+    @custom_update_header_filename.setter
+    def custom_update_header_filename(self, custom_update_header_filename):
+        if isinstance(custom_update_header_filename, str):
+            self.__custom_update_header_filename = custom_update_header_filename
+        else:
+            raise Exception('Invalid custom_update_header_filename, expected a string.\n')
+
+    @custom_update_copy.setter
+    def custom_update_copy(self, custom_update_copy):
+        if isinstance(custom_update_copy, bool):
+            self.__custom_update_copy = custom_update_copy
+        else:
+            raise Exception('Invalid custom_update_copy, expected a bool.\n')
 
     @hessian_approx.setter
     def hessian_approx(self, hessian_approx):
@@ -2573,7 +2760,7 @@ class AcadosOcpOptions:
             self.__hessian_approx = hessian_approx
         else:
             raise Exception('Invalid hessian_approx value. Possible values are:\n\n' \
-                    + ',\n'.join(hessian_approxs) + '.\n\nYou have: ' + hessian_approx + '.\n\nExiting.')
+                    + ',\n'.join(hessian_approxs) + '.\n\nYou have: ' + hessian_approx + '.\n\n')
 
     @integrator_type.setter
     def integrator_type(self, integrator_type):
@@ -2582,7 +2769,7 @@ class AcadosOcpOptions:
             self.__integrator_type = integrator_type
         else:
             raise Exception('Invalid integrator_type value. Possible values are:\n\n' \
-                    + ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\nExiting.')
+                    + ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\n')
 
     @tf.setter
     def tf(self, tf):
@@ -2619,7 +2806,7 @@ class AcadosOcpOptions:
             self.__globalization = globalization
         else:
             raise Exception('Invalid globalization value. Possible values are:\n\n' \
-                    + ',\n'.join(globalization_types) + '.\n\nYou have: ' + globalization + '.\n\nExiting.')
+                    + ',\n'.join(globalization_types) + '.\n\nYou have: ' + globalization + '.\n\n')
 
     @alpha_min.setter
     def alpha_min(self, alpha_min):
@@ -2655,7 +2842,7 @@ class AcadosOcpOptions:
         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')
+            raise Exception('Invalid eps_sufficient_descent value. eps_sufficient_descent must be a positive float.')
 
     @sim_method_num_stages.setter
     def sim_method_num_stages(self, sim_method_num_stages):
@@ -2663,7 +2850,7 @@ class AcadosOcpOptions:
         # 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.')
+        #     raise Exception('Invalid sim_method_num_stages value. sim_method_num_stages must be an integer.')
 
         self.__sim_method_num_stages = sim_method_num_stages
 
@@ -2673,7 +2860,7 @@ class AcadosOcpOptions:
         # 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.')
+        #     raise Exception('Invalid sim_method_num_steps value. sim_method_num_steps must be an integer.')
         self.__sim_method_num_steps = sim_method_num_steps
 
 
@@ -2683,7 +2870,7 @@ class AcadosOcpOptions:
         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.')
+            raise Exception('Invalid sim_method_newton_iter value. sim_method_newton_iter must be an integer.')
 
     @sim_method_jac_reuse.setter
     def sim_method_jac_reuse(self, sim_method_jac_reuse):
@@ -2699,42 +2886,57 @@ class AcadosOcpOptions:
             self.__nlp_solver_type = nlp_solver_type
         else:
             raise Exception('Invalid nlp_solver_type value. Possible values are:\n\n' \
-                    + ',\n'.join(nlp_solver_types) + '.\n\nYou have: ' + nlp_solver_type + '.\n\nExiting.')
+                    + ',\n'.join(nlp_solver_types) + '.\n\nYou have: ' + nlp_solver_type + '.\n\n')
 
     @nlp_solver_step_length.setter
     def nlp_solver_step_length(self, nlp_solver_step_length):
         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')
+            raise Exception('Invalid nlp_solver_step_length value. nlp_solver_step_length must be a positive float.')
 
     @levenberg_marquardt.setter
     def levenberg_marquardt(self, levenberg_marquardt):
         if isinstance(levenberg_marquardt, float) and levenberg_marquardt >= 0:
             self.__levenberg_marquardt = levenberg_marquardt
         else:
-            raise Exception('Invalid levenberg_marquardt value. levenberg_marquardt must be a positive float. Exiting')
+            raise Exception('Invalid levenberg_marquardt value. levenberg_marquardt must be a positive float.')
 
     @qp_solver_iter_max.setter
     def qp_solver_iter_max(self, qp_solver_iter_max):
         if isinstance(qp_solver_iter_max, int) and qp_solver_iter_max > 0:
             self.__qp_solver_iter_max = qp_solver_iter_max
         else:
-            raise Exception('Invalid qp_solver_iter_max value. qp_solver_iter_max must be a positive int. Exiting')
+            raise Exception('Invalid qp_solver_iter_max value. qp_solver_iter_max must be a positive int.')
+
+    @qp_solver_ric_alg.setter
+    def qp_solver_ric_alg(self, qp_solver_ric_alg):
+        if qp_solver_ric_alg in [0, 1]:
+            self.__qp_solver_ric_alg = qp_solver_ric_alg
+        else:
+            raise Exception(f'Invalid qp_solver_ric_alg value. qp_solver_ric_alg must be in [0, 1], got {qp_solver_ric_alg}.')
+
+    @qp_solver_cond_ric_alg.setter
+    def qp_solver_cond_ric_alg(self, qp_solver_cond_ric_alg):
+        if qp_solver_cond_ric_alg in [0, 1]:
+            self.__qp_solver_cond_ric_alg = qp_solver_cond_ric_alg
+        else:
+            raise Exception(f'Invalid qp_solver_cond_ric_alg value. qp_solver_cond_ric_alg must be in [0, 1], got {qp_solver_cond_ric_alg}.')
+
 
     @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:
             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')
+            raise Exception('Invalid qp_solver_cond_N value. qp_solver_cond_N must be a positive int.')
 
     @qp_solver_warm_start.setter
     def qp_solver_warm_start(self, qp_solver_warm_start):
         if qp_solver_warm_start in [0, 1, 2]:
             self.__qp_solver_warm_start = qp_solver_warm_start
         else:
-            raise Exception('Invalid qp_solver_warm_start value. qp_solver_warm_start must be 0 or 1 or 2. Exiting')
+            raise Exception('Invalid qp_solver_warm_start value. qp_solver_warm_start must be 0 or 1 or 2.')
 
     @qp_tol.setter
     def qp_tol(self, qp_tol):
@@ -2744,35 +2946,35 @@ class AcadosOcpOptions:
             self.__qp_solver_tol_stat = qp_tol
             self.__qp_solver_tol_comp = qp_tol
         else:
-            raise Exception('Invalid qp_tol value. qp_tol must be a positive float. Exiting')
+            raise Exception('Invalid qp_tol value. qp_tol must be a positive float.')
 
     @qp_solver_tol_stat.setter
     def qp_solver_tol_stat(self, qp_solver_tol_stat):
         if isinstance(qp_solver_tol_stat, float) and qp_solver_tol_stat > 0:
             self.__qp_solver_tol_stat = qp_solver_tol_stat
         else:
-            raise Exception('Invalid qp_solver_tol_stat value. qp_solver_tol_stat must be a positive float. Exiting')
+            raise Exception('Invalid qp_solver_tol_stat value. qp_solver_tol_stat must be a positive float.')
 
     @qp_solver_tol_eq.setter
     def qp_solver_tol_eq(self, qp_solver_tol_eq):
         if isinstance(qp_solver_tol_eq, float) and qp_solver_tol_eq > 0:
             self.__qp_solver_tol_eq = qp_solver_tol_eq
         else:
-            raise Exception('Invalid qp_solver_tol_eq value. qp_solver_tol_eq must be a positive float. Exiting')
+            raise Exception('Invalid qp_solver_tol_eq value. qp_solver_tol_eq must be a positive float.')
 
     @qp_solver_tol_ineq.setter
     def qp_solver_tol_ineq(self, qp_solver_tol_ineq):
         if isinstance(qp_solver_tol_ineq, float) and qp_solver_tol_ineq > 0:
             self.__qp_solver_tol_ineq = qp_solver_tol_ineq
         else:
-            raise Exception('Invalid qp_solver_tol_ineq value. qp_solver_tol_ineq must be a positive float. Exiting')
+            raise Exception('Invalid qp_solver_tol_ineq value. qp_solver_tol_ineq must be a positive float.')
 
     @qp_solver_tol_comp.setter
     def qp_solver_tol_comp(self, qp_solver_tol_comp):
         if isinstance(qp_solver_tol_comp, float) and qp_solver_tol_comp > 0:
             self.__qp_solver_tol_comp = qp_solver_tol_comp
         else:
-            raise Exception('Invalid qp_solver_tol_comp value. qp_solver_tol_comp must be a positive float. Exiting')
+            raise Exception('Invalid qp_solver_tol_comp value. qp_solver_tol_comp must be a positive float.')
 
     @tol.setter
     def tol(self, tol):
@@ -2782,35 +2984,42 @@ class AcadosOcpOptions:
             self.__nlp_solver_tol_stat = tol
             self.__nlp_solver_tol_comp = tol
         else:
-            raise Exception('Invalid tol value. tol must be a positive float. Exiting')
+            raise Exception('Invalid tol value. tol must be a positive float.')
 
     @nlp_solver_tol_stat.setter
     def nlp_solver_tol_stat(self, nlp_solver_tol_stat):
         if isinstance(nlp_solver_tol_stat, float) and nlp_solver_tol_stat > 0:
             self.__nlp_solver_tol_stat = nlp_solver_tol_stat
         else:
-            raise Exception('Invalid nlp_solver_tol_stat value. nlp_solver_tol_stat must be a positive float. Exiting')
+            raise Exception('Invalid nlp_solver_tol_stat value. nlp_solver_tol_stat must be a positive float.')
 
     @nlp_solver_tol_eq.setter
     def nlp_solver_tol_eq(self, nlp_solver_tol_eq):
         if isinstance(nlp_solver_tol_eq, float) and nlp_solver_tol_eq > 0:
             self.__nlp_solver_tol_eq = nlp_solver_tol_eq
         else:
-            raise Exception('Invalid nlp_solver_tol_eq value. nlp_solver_tol_eq must be a positive float. Exiting')
+            raise Exception('Invalid nlp_solver_tol_eq value. nlp_solver_tol_eq must be a positive float.')
 
     @nlp_solver_tol_ineq.setter
     def nlp_solver_tol_ineq(self, nlp_solver_tol_ineq):
         if isinstance(nlp_solver_tol_ineq, float) and nlp_solver_tol_ineq > 0:
             self.__nlp_solver_tol_ineq = nlp_solver_tol_ineq
         else:
-            raise Exception('Invalid nlp_solver_tol_ineq value. nlp_solver_tol_ineq must be a positive float. Exiting')
+            raise Exception('Invalid nlp_solver_tol_ineq value. nlp_solver_tol_ineq must be a positive float.')
+
+    @nlp_solver_ext_qp_res.setter
+    def nlp_solver_ext_qp_res(self, nlp_solver_ext_qp_res):
+        if nlp_solver_ext_qp_res in [0, 1]:
+            self.__nlp_solver_ext_qp_res = nlp_solver_ext_qp_res
+        else:
+            raise Exception('Invalid nlp_solver_ext_qp_res value. nlp_solver_ext_qp_res must be in [0, 1].')
 
     @nlp_solver_tol_comp.setter
     def nlp_solver_tol_comp(self, nlp_solver_tol_comp):
         if isinstance(nlp_solver_tol_comp, float) and nlp_solver_tol_comp > 0:
             self.__nlp_solver_tol_comp = nlp_solver_tol_comp
         else:
-            raise Exception('Invalid nlp_solver_tol_comp value. nlp_solver_tol_comp must be a positive float. Exiting')
+            raise Exception('Invalid nlp_solver_tol_comp value. nlp_solver_tol_comp must be a positive float.')
 
     @nlp_solver_max_iter.setter
     def nlp_solver_max_iter(self, nlp_solver_max_iter):
@@ -2818,14 +3027,14 @@ class AcadosOcpOptions:
         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')
+            raise Exception('Invalid nlp_solver_max_iter value. nlp_solver_max_iter must be a positive int.')
 
     @print_level.setter
     def print_level(self, print_level):
         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')
+            raise Exception('Invalid print_level value. print_level takes one of the values >=0.')
 
     @model_external_shared_lib_dir.setter
     def model_external_shared_lib_dir(self, model_external_shared_lib_dir):
@@ -2833,47 +3042,47 @@ class AcadosOcpOptions:
             self.__model_external_shared_lib_dir = model_external_shared_lib_dir
         else:
             raise Exception('Invalid model_external_shared_lib_dir value. Str expected.' \
-            + '.\n\nYou have: ' + type(model_external_shared_lib_dir) + '.\n\nExiting.')
+            + '.\n\nYou have: ' + type(model_external_shared_lib_dir) + '.\n\n')
 
     @model_external_shared_lib_name.setter
     def model_external_shared_lib_name(self, model_external_shared_lib_name):
         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.')
+            + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\n')
             else :
                 self.__model_external_shared_lib_name = model_external_shared_lib_name
         else:
             raise Exception('Invalid model_external_shared_lib_name value. Str expected.' \
-            + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\nExiting.')
+            + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\n')
 
     @exact_hess_constr.setter
     def exact_hess_constr(self, exact_hess_constr):
         if exact_hess_constr in [0, 1]:
             self.__exact_hess_constr = exact_hess_constr
         else:
-            raise Exception('Invalid exact_hess_constr value. exact_hess_constr takes one of the values 0, 1. Exiting')
+            raise Exception('Invalid exact_hess_constr value. exact_hess_constr takes one of the values 0, 1.')
 
     @exact_hess_cost.setter
     def exact_hess_cost(self, exact_hess_cost):
         if exact_hess_cost in [0, 1]:
             self.__exact_hess_cost = exact_hess_cost
         else:
-            raise Exception('Invalid exact_hess_cost value. exact_hess_cost takes one of the values 0, 1. Exiting')
+            raise Exception('Invalid exact_hess_cost value. exact_hess_cost takes one of the values 0, 1.')
 
     @exact_hess_dyn.setter
     def exact_hess_dyn(self, exact_hess_dyn):
         if exact_hess_dyn in [0, 1]:
             self.__exact_hess_dyn = exact_hess_dyn
         else:
-            raise Exception('Invalid exact_hess_dyn value. exact_hess_dyn takes one of the values 0, 1. Exiting')
+            raise Exception('Invalid exact_hess_dyn value. exact_hess_dyn takes one of the values 0, 1.')
 
     @ext_cost_num_hess.setter
     def ext_cost_num_hess(self, ext_cost_num_hess):
         if ext_cost_num_hess in [0, 1]:
             self.__ext_cost_num_hess = ext_cost_num_hess
         else:
-            raise Exception('Invalid ext_cost_num_hess value. ext_cost_num_hess takes one of the values 0, 1. Exiting')
+            raise Exception('Invalid ext_cost_num_hess value. ext_cost_num_hess takes one of the values 0, 1.')
 
     def set(self, attr, value):
         setattr(self, attr, value)
@@ -2893,6 +3102,7 @@ class AcadosOcp:
         - :py:attr:`solver_options` of type :py:class:`acados_template.acados_ocp.AcadosOcpOptions`
 
         - :py:attr:`acados_include_path` (set automatically)
+        - :py:attr:`shared_lib_ext` (set automatically)
         - :py:attr:`acados_lib_path` (set automatically)
         - :py:attr:`parameter_values` - used to initialize the parameters (can be changed)
     """
@@ -2914,14 +3124,16 @@ class AcadosOcp:
         """Constraints definitions, type :py:class:`acados_template.acados_ocp.AcadosOcpConstraints`"""
         self.solver_options = AcadosOcpOptions()
         """Solver Options, type :py:class:`acados_template.acados_ocp.AcadosOcpOptions`"""
-		
+
         self.acados_include_path = os.path.join(acados_path, 'include').replace(os.sep, '/') # the replace part is important on Windows for CMake
         """Path to acados include directory (set automatically), type: `string`"""
         self.acados_lib_path = os.path.join(acados_path, 'lib').replace(os.sep, '/') # the replace part is important on Windows for CMake
         """Path to where acados library is located, type: `string`"""
+        self.shared_lib_ext = get_lib_ext()
 
-        import numpy
-        self.cython_include_dirs = numpy.get_include()
+        # get cython paths
+        from sysconfig import get_paths
+        self.cython_include_dirs = [np.get_include(), get_paths()['include']]
 
         self.__parameter_values = np.array([])
         self.__problem_class = 'OCP'
diff --git a/third_party/acados/acados_template/acados_ocp_solver.py b/third_party/acados/acados_template/acados_ocp_solver.py
index beeda2cb0c..ffc9cf4b0e 100644
--- a/third_party/acados/acados_template/acados_ocp_solver.py
+++ b/third_party/acados/acados_template/acados_ocp_solver.py
@@ -1,9 +1,6 @@
 # -*- coding: future_fstrings -*-
 #
-# 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
+# Copyright (c) The acados authors.
 #
 # This file is part of acados.
 #
@@ -38,26 +35,29 @@ import json
 import numpy as np
 from datetime import datetime
 import importlib
+import shutil
+
+from subprocess import DEVNULL, call, STDOUT
+
 from ctypes import POINTER, cast, CDLL, c_void_p, c_char_p, c_double, c_int, c_int64, byref
 
 from copy import deepcopy
+from pathlib import Path
 
-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 .casadi_function_generation import generate_c_code_explicit_ode, \
+    generate_c_code_implicit_ode, generate_c_code_gnsf, generate_c_code_discrete_dynamics, \
+    generate_c_code_constraint, generate_c_code_nls_cost, generate_c_code_conl_cost, \
+    generate_c_code_external_cost
+from .gnsf.detect_gnsf_structure import detect_gnsf_structure
 from .acados_ocp import AcadosOcp
-from .acados_model import acados_model_strip_casadi_symbolics
+from .acados_model import AcadosModel
 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_ocp_nlp_layout, get_python_interface_path
+     format_class_dict, make_object_json_dumpable, make_model_consistent,\
+     set_up_imported_gnsf_model, get_ocp_nlp_layout, get_python_interface_path, get_lib_ext, check_casadi_version
 from .builders import CMakeBuilder
 
 
-def make_ocp_dims_consistent(acados_ocp):
+def make_ocp_dims_consistent(acados_ocp: AcadosOcp):
     dims = acados_ocp.dims
     cost = acados_ocp.cost
     constraints = acados_ocp.constraints
@@ -105,6 +105,9 @@ def make_ocp_dims_consistent(acados_ocp):
         model.cost_expr_ext_cost_0 = model.cost_expr_ext_cost
         model.cost_expr_ext_cost_custom_hess_0 = model.cost_expr_ext_cost_custom_hess
 
+        model.cost_psi_expr_0 = model.cost_psi_expr
+        model.cost_r_in_psi_expr_0 = model.cost_r_in_psi_expr
+
     if cost.cost_type_0 == 'LINEAR_LS':
         ny_0 = cost.W_0.shape[0]
         if cost.Vx_0.shape[0] != ny_0 or cost.Vu_0.shape[0] != ny_0:
@@ -133,6 +136,22 @@ def make_ocp_dims_consistent(acados_ocp):
                             f'\nGot W_0[{cost.W.shape}], yref_0[{cost.yref_0.shape}]\n')
         dims.ny_0 = ny_0
 
+    elif cost.cost_type_0 == 'CONVEX_OVER_NONLINEAR':
+        if is_empty(model.cost_y_expr_0):
+            raise Exception('cost_y_expr_0 and/or cost_y_expr not provided.')
+        ny_0 = casadi_length(model.cost_y_expr_0)
+        if is_empty(model.cost_r_in_psi_expr_0) or casadi_length(model.cost_r_in_psi_expr_0) != ny_0:
+            raise Exception('inconsistent dimension ny_0: regarding cost_y_expr_0 and cost_r_in_psi_0.')
+        if is_empty(model.cost_psi_expr_0) or casadi_length(model.cost_psi_expr_0) != 1:
+            raise Exception('cost_psi_expr_0 not provided or not scalar-valued.')
+        if cost.yref_0.shape[0] != ny_0:
+            raise Exception('inconsistent dimension: regarding yref_0 and cost_y_expr_0, cost_r_in_psi_0.')
+        dims.ny_0 = ny_0
+
+        if not (opts.hessian_approx=='EXACT' and opts.exact_hess_cost==False) and opts.hessian_approx != 'GAUSS_NEWTON':
+            raise Exception("\nWith CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:\n"
+            "GAUSS_NEWTON or EXACT with 'exact_hess_cost' == False.\n")
+
     elif cost.cost_type_0 == 'EXTERNAL':
         if opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and model.cost_expr_ext_cost_custom_hess_0 is None:
             print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not possible!\n"
@@ -171,6 +190,23 @@ def make_ocp_dims_consistent(acados_ocp):
                             f'\nGot W[{cost.W.shape}], yref[{cost.yref.shape}]\n')
         dims.ny = ny
 
+    elif cost.cost_type == 'CONVEX_OVER_NONLINEAR':
+        if is_empty(model.cost_y_expr):
+            raise Exception('cost_y_expr and/or cost_y_expr not provided.')
+        ny = casadi_length(model.cost_y_expr)
+        if is_empty(model.cost_r_in_psi_expr) or casadi_length(model.cost_r_in_psi_expr) != ny:
+            raise Exception('inconsistent dimension ny: regarding cost_y_expr and cost_r_in_psi.')
+        if is_empty(model.cost_psi_expr) or casadi_length(model.cost_psi_expr) != 1:
+            raise Exception('cost_psi_expr not provided or not scalar-valued.')
+        if cost.yref.shape[0] != ny:
+            raise Exception('inconsistent dimension: regarding yref and cost_y_expr, cost_r_in_psi.')
+        dims.ny = ny
+
+        if not (opts.hessian_approx=='EXACT' and opts.exact_hess_cost==False) and opts.hessian_approx != 'GAUSS_NEWTON':
+            raise Exception("\nWith CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:\n"
+            "GAUSS_NEWTON or EXACT with 'exact_hess_cost' == False.\n")
+
+
     elif cost.cost_type == 'EXTERNAL':
         if opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and model.cost_expr_ext_cost_custom_hess is None:
             print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not possible!\n"
@@ -202,6 +238,24 @@ def make_ocp_dims_consistent(acados_ocp):
             raise Exception('inconsistent dimension: regarding W_e, yref_e.')
         dims.ny_e = ny_e
 
+    elif cost.cost_type_e == 'CONVEX_OVER_NONLINEAR':
+        if is_empty(model.cost_y_expr_e):
+            raise Exception('cost_y_expr_e not provided.')
+        ny_e = casadi_length(model.cost_y_expr_e)
+        if is_empty(model.cost_r_in_psi_expr_e) or casadi_length(model.cost_r_in_psi_expr_e) != ny_e:
+            raise Exception('inconsistent dimension ny_e: regarding cost_y_expr_e and cost_r_in_psi_e.')
+        if is_empty(model.cost_psi_expr_e) or casadi_length(model.cost_psi_expr_e) != 1:
+            raise Exception('cost_psi_expr_e not provided or not scalar-valued.')
+        if cost.yref_e.shape[0] != ny_e:
+            raise Exception('inconsistent dimension: regarding yref_e and cost_y_expr_e, cost_r_in_psi_e.')
+        dims.ny_e = ny_e
+
+        if not (opts.hessian_approx=='EXACT' and opts.exact_hess_cost==False) and opts.hessian_approx != 'GAUSS_NEWTON':
+            raise Exception("\nWith CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:\n"
+            "GAUSS_NEWTON or EXACT with 'exact_hess_cost' == False.\n")
+
+
+
     elif cost.cost_type_e == 'EXTERNAL':
         if opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and model.cost_expr_ext_cost_custom_hess_e is None:
             print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not possible!\n"
@@ -213,16 +267,13 @@ def make_ocp_dims_consistent(acados_ocp):
 
     ## constraints
     # initial
-    if (constraints.lbx_0 == [] and constraints.ubx_0 == []):
-        dims.nbx_0 = 0
-    else:
-        this_shape = constraints.lbx_0.shape
-        other_shape = constraints.ubx_0.shape
-        if not this_shape == other_shape:
-            raise Exception('lbx_0, ubx_0 have different shapes!')
-        if not is_column(constraints.lbx_0):
-            raise Exception('lbx_0, ubx_0 must be column vectors!')
-        dims.nbx_0 = constraints.lbx_0.size
+    this_shape = constraints.lbx_0.shape
+    other_shape = constraints.ubx_0.shape
+    if not this_shape == other_shape:
+        raise Exception('lbx_0, ubx_0 have different shapes!')
+    if not is_column(constraints.lbx_0):
+        raise Exception('lbx_0, ubx_0 must be column vectors!')
+    dims.nbx_0 = constraints.lbx_0.size
 
     if all(constraints.lbx_0 == constraints.ubx_0) and dims.nbx_0 == dims.nx \
         and dims.nbxe_0 is None \
@@ -230,8 +281,10 @@ def make_ocp_dims_consistent(acados_ocp):
             and all(constraints.idxbxe_0 == constraints.idxbx_0):
         # case: x0 was set: nbx0 are all equlities.
         dims.nbxe_0 = dims.nbx_0
+    elif constraints.idxbxe_0 is not None:
+        dims.nbxe_0 = constraints.idxbxe_0.shape[0]
     elif dims.nbxe_0 is None:
-        # case: x0 was not set -> dont assume nbx0 to be equality constraints.
+        # case: x0 and idxbxe_0 were not set -> dont assume nbx0 to be equality constraints.
         dims.nbxe_0 = 0
 
     # path
@@ -309,6 +362,8 @@ def make_ocp_dims_consistent(acados_ocp):
 
     # Slack dimensions
     nsbx = constraints.idxsbx.shape[0]
+    if nsbx > nbx:
+        raise Exception(f'inconsistent dimension nsbx = {nsbx}. Is greater than nbx = {nbx}.')
     if is_empty(constraints.lsbx):
         constraints.lsbx = np.zeros((nsbx,))
     elif constraints.lsbx.shape[0] != nsbx:
@@ -320,6 +375,8 @@ def make_ocp_dims_consistent(acados_ocp):
     dims.nsbx = nsbx
 
     nsbu = constraints.idxsbu.shape[0]
+    if nsbu > nbu:
+        raise Exception(f'inconsistent dimension nsbu = {nsbu}. Is greater than nbu = {nbu}.')
     if is_empty(constraints.lsbu):
         constraints.lsbu = np.zeros((nsbu,))
     elif constraints.lsbu.shape[0] != nsbu:
@@ -331,6 +388,8 @@ def make_ocp_dims_consistent(acados_ocp):
     dims.nsbu = nsbu
 
     nsh = constraints.idxsh.shape[0]
+    if nsh > nh:
+        raise Exception(f'inconsistent dimension nsh = {nsh}. Is greater than nh = {nh}.')
     if is_empty(constraints.lsh):
         constraints.lsh = np.zeros((nsh,))
     elif constraints.lsh.shape[0] != nsh:
@@ -342,6 +401,8 @@ def make_ocp_dims_consistent(acados_ocp):
     dims.nsh = nsh
 
     nsphi = constraints.idxsphi.shape[0]
+    if nsphi > dims.nphi:
+        raise Exception(f'inconsistent dimension nsphi = {nsphi}. Is greater than nphi = {dims.nphi}.')
     if is_empty(constraints.lsphi):
         constraints.lsphi = np.zeros((nsphi,))
     elif constraints.lsphi.shape[0] != nsphi:
@@ -353,6 +414,8 @@ def make_ocp_dims_consistent(acados_ocp):
     dims.nsphi = nsphi
 
     nsg = constraints.idxsg.shape[0]
+    if nsg > ng:
+        raise Exception(f'inconsistent dimension nsg = {nsg}. Is greater than ng = {ng}.')
     if is_empty(constraints.lsg):
         constraints.lsg = np.zeros((nsg,))
     elif constraints.lsg.shape[0] != nsg:
@@ -386,6 +449,8 @@ def make_ocp_dims_consistent(acados_ocp):
     dims.ns = ns
 
     nsbx_e = constraints.idxsbx_e.shape[0]
+    if nsbx_e > nbx_e:
+        raise Exception(f'inconsistent dimension nsbx_e = {nsbx_e}. Is greater than nbx_e = {nbx_e}.')
     if is_empty(constraints.lsbx_e):
         constraints.lsbx_e = np.zeros((nsbx_e,))
     elif constraints.lsbx_e.shape[0] != nsbx_e:
@@ -397,6 +462,8 @@ def make_ocp_dims_consistent(acados_ocp):
     dims.nsbx_e = nsbx_e
 
     nsh_e = constraints.idxsh_e.shape[0]
+    if nsh_e > nh_e:
+        raise Exception(f'inconsistent dimension nsh_e = {nsh_e}. Is greater than nh_e = {nh_e}.')
     if is_empty(constraints.lsh_e):
         constraints.lsh_e = np.zeros((nsh_e,))
     elif constraints.lsh_e.shape[0] != nsh_e:
@@ -408,6 +475,8 @@ def make_ocp_dims_consistent(acados_ocp):
     dims.nsh_e = nsh_e
 
     nsg_e = constraints.idxsg_e.shape[0]
+    if nsg_e > ng_e:
+        raise Exception(f'inconsistent dimension nsg_e = {nsg_e}. Is greater than ng_e = {ng_e}.')
     if is_empty(constraints.lsg_e):
         constraints.lsg_e = np.zeros((nsg_e,))
     elif constraints.lsg_e.shape[0] != nsg_e:
@@ -419,6 +488,8 @@ def make_ocp_dims_consistent(acados_ocp):
     dims.nsg_e = nsg_e
 
     nsphi_e = constraints.idxsphi_e.shape[0]
+    if nsphi_e > dims.nphi_e:
+        raise Exception(f'inconsistent dimension nsphi_e = {nsphi_e}. Is greater than nphi_e = {dims.nphi_e}.')
     if is_empty(constraints.lsphi_e):
         constraints.lsphi_e = np.zeros((nsphi_e,))
     elif constraints.lsphi_e.shape[0] != nsphi_e:
@@ -525,7 +596,7 @@ def get_simulink_default_opts():
     return simulink_default_opts
 
 
-def ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file='acados_ocp_nlp.json'):
+def ocp_formulation_json_dump(acados_ocp, simulink_opts=None, json_file='acados_ocp_nlp.json'):
     # Load acados_ocp_nlp structure description
     ocp_layout = get_ocp_nlp_layout()
 
@@ -543,20 +614,11 @@ def ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file='acados_ocp_n
 
     ocp_nlp_dict = format_class_dict(ocp_nlp_dict)
 
-    # strip symbolics
-    ocp_nlp_dict['model'] = acados_model_strip_casadi_symbolics(ocp_nlp_dict['model'])
-
-    # strip shooting_nodes
-    ocp_nlp_dict['solver_options'].pop('shooting_nodes', None)
-    dims_dict = format_class_dict(acados_ocp.dims.__dict__)
-
-    ocp_check_against_layout(ocp_nlp_dict, dims_dict)
-
-    # add simulink options
-    ocp_nlp_dict['simulink_opts'] = simulink_opts
+    if simulink_opts is not None:
+        ocp_nlp_dict['simulink_opts'] = simulink_opts
 
     with open(json_file, 'w') as f:
-        json.dump(ocp_nlp_dict, f, default=np_array_to_list, indent=4, sort_keys=True)
+        json.dump(ocp_nlp_dict, f, default=make_object_json_dumpable, indent=4, sort_keys=True)
 
 
 
@@ -587,7 +649,7 @@ def ocp_formulation_json_load(json_file='acados_ocp_nlp.json'):
     return acados_ocp
 
 
-def ocp_generate_external_functions(acados_ocp, model):
+def ocp_generate_external_functions(acados_ocp: AcadosOcp, model: AcadosModel):
 
     model = make_model_consistent(model)
 
@@ -595,27 +657,32 @@ def ocp_generate_external_functions(acados_ocp, model):
         opts = dict(generate_hess=1)
     else:
         opts = dict(generate_hess=0)
+
+    # create code_export_dir, model_dir
     code_export_dir = acados_ocp.code_export_directory
     opts['code_export_directory'] = code_export_dir
-
-    if acados_ocp.model.dyn_ext_fun_type != 'casadi':
-        raise Exception("ocp_generate_external_functions: dyn_ext_fun_type only supports 'casadi' for now.\
-            Extending the Python interface with generic function support is welcome.")
-
-    if acados_ocp.solver_options.integrator_type == 'ERK':
-        # explicit model -- generate C code
-        generate_c_code_explicit_ode(model, opts)
-    elif acados_ocp.solver_options.integrator_type == 'IRK':
-        # implicit model -- generate C code
-        generate_c_code_implicit_ode(model, opts)
-    elif acados_ocp.solver_options.integrator_type == 'LIFTED_IRK':
-        generate_c_code_implicit_ode(model, opts)
-    elif acados_ocp.solver_options.integrator_type == 'GNSF':
-        generate_c_code_gnsf(model, opts)
-    elif acados_ocp.solver_options.integrator_type == 'DISCRETE':
-        generate_c_code_discrete_dynamics(model, opts)
+    model_dir = os.path.join(code_export_dir, model.name + '_model')
+    if not os.path.exists(model_dir):
+        os.makedirs(model_dir)
+
+    check_casadi_version()
+    # TODO: remove dir gen from all the generate_c_* functions
+    if acados_ocp.model.dyn_ext_fun_type == 'casadi':
+        if acados_ocp.solver_options.integrator_type == 'ERK':
+            generate_c_code_explicit_ode(model, opts)
+        elif acados_ocp.solver_options.integrator_type == 'IRK':
+            generate_c_code_implicit_ode(model, opts)
+        elif acados_ocp.solver_options.integrator_type == 'LIFTED_IRK':
+            generate_c_code_implicit_ode(model, opts)
+        elif acados_ocp.solver_options.integrator_type == 'GNSF':
+            generate_c_code_gnsf(model, opts)
+        elif acados_ocp.solver_options.integrator_type == 'DISCRETE':
+            generate_c_code_discrete_dynamics(model, opts)
+        else:
+            raise Exception("ocp_generate_external_functions: unknown integrator type.")
     else:
-        raise Exception("ocp_generate_external_functions: unknown integrator type.")
+        target_location = os.path.join(code_export_dir, model_dir, model.dyn_generic_source)
+        shutil.copyfile(model.dyn_generic_source, target_location)
 
     if acados_ocp.dims.nphi > 0 or acados_ocp.dims.nh > 0:
         generate_c_code_constraint(model, model.name, False, opts)
@@ -623,28 +690,24 @@ def ocp_generate_external_functions(acados_ocp, model):
     if acados_ocp.dims.nphi_e > 0 or acados_ocp.dims.nh_e > 0:
         generate_c_code_constraint(model, model.name, True, opts)
 
-    # dummy matrices
-    if not acados_ocp.cost.cost_type_0 == 'LINEAR_LS':
-        acados_ocp.cost.Vx_0 = np.zeros((acados_ocp.dims.ny_0, acados_ocp.dims.nx))
-        acados_ocp.cost.Vu_0 = np.zeros((acados_ocp.dims.ny_0, acados_ocp.dims.nu))
-    if not acados_ocp.cost.cost_type == 'LINEAR_LS':
-        acados_ocp.cost.Vx = np.zeros((acados_ocp.dims.ny, acados_ocp.dims.nx))
-        acados_ocp.cost.Vu = np.zeros((acados_ocp.dims.ny, acados_ocp.dims.nu))
-    if not acados_ocp.cost.cost_type_e == 'LINEAR_LS':
-        acados_ocp.cost.Vx_e = np.zeros((acados_ocp.dims.ny_e, acados_ocp.dims.nx))
-
     if acados_ocp.cost.cost_type_0 == 'NONLINEAR_LS':
         generate_c_code_nls_cost(model, model.name, 'initial', opts)
+    elif acados_ocp.cost.cost_type_0 == 'CONVEX_OVER_NONLINEAR':
+        generate_c_code_conl_cost(model, model.name, 'initial', opts)
     elif acados_ocp.cost.cost_type_0 == 'EXTERNAL':
         generate_c_code_external_cost(model, 'initial', opts)
 
     if acados_ocp.cost.cost_type == 'NONLINEAR_LS':
         generate_c_code_nls_cost(model, model.name, 'path', opts)
+    elif acados_ocp.cost.cost_type == 'CONVEX_OVER_NONLINEAR':
+        generate_c_code_conl_cost(model, model.name, 'path', opts)
     elif acados_ocp.cost.cost_type == 'EXTERNAL':
         generate_c_code_external_cost(model, 'path', opts)
 
     if acados_ocp.cost.cost_type_e == 'NONLINEAR_LS':
         generate_c_code_nls_cost(model, model.name, 'terminal', opts)
+    elif acados_ocp.cost.cost_type_e == 'CONVEX_OVER_NONLINEAR':
+        generate_c_code_conl_cost(model, model.name, 'terminal', opts)
     elif acados_ocp.cost.cost_type_e == 'EXTERNAL':
         generate_c_code_external_cost(model, 'terminal', opts)
 
@@ -659,9 +722,8 @@ def ocp_get_default_cmake_builder() -> CMakeBuilder:
     return cmake_builder
 
 
-def ocp_render_templates(acados_ocp, json_file, cmake_builder=None):
 
-    name = acados_ocp.model.name
+def ocp_render_templates(acados_ocp: AcadosOcp, json_file, cmake_builder=None, simulink_opts=None):
 
     # setting up loader and environment
     json_path = os.path.abspath(json_file)
@@ -669,132 +731,69 @@ def ocp_render_templates(acados_ocp, json_file, cmake_builder=None):
     if not os.path.exists(json_path):
         raise Exception(f'Path "{json_path}" not found!')
 
-    code_export_dir = acados_ocp.code_export_directory
-    template_dir = code_export_dir
+    # Render templates
+    template_list = __ocp_get_template_list(acados_ocp, cmake_builder=cmake_builder, simulink_opts=simulink_opts)
+    for tup in template_list:
+        if len(tup) > 2:
+            output_dir = tup[2]
+        else:
+            output_dir = acados_ocp.code_export_directory
+        render_template(tup[0], tup[1], output_dir, json_path)
 
-    ## Render templates
-    in_file = 'main.in.c'
-    out_file = f'main_{name}.c'
-    render_template(in_file, out_file, template_dir, json_path)
+    # Custom templates
+    acados_template_path = os.path.dirname(os.path.abspath(__file__))
+    custom_template_glob = os.path.join(acados_template_path, 'custom_update_templates', '*')
+    for tup in acados_ocp.solver_options.custom_templates:
+        render_template(tup[0], tup[1], acados_ocp.code_export_directory, json_path, template_glob=custom_template_glob)
 
-    in_file = 'acados_solver.in.c'
-    out_file = f'acados_solver_{name}.c'
-    render_template(in_file, out_file, template_dir, json_path)
+    return
 
-    in_file = 'acados_solver.in.h'
-    out_file = f'acados_solver_{name}.h'
-    render_template(in_file, out_file, template_dir, json_path)
 
-    in_file = 'acados_solver.in.pxd'
-    out_file = f'acados_solver.pxd'
-    render_template(in_file, out_file, template_dir, json_path)
 
+def __ocp_get_template_list(acados_ocp: AcadosOcp, cmake_builder=None, simulink_opts=None) -> list:
+    """
+    returns a list of tuples in the form:
+    (input_filename, output_filname)
+    or
+    (input_filename, output_filname, output_directory)
+    """
+    name = acados_ocp.model.name
+    code_export_directory = acados_ocp.code_export_directory
+    template_list = []
+
+    template_list.append(('main.in.c', f'main_{name}.c'))
+    template_list.append(('acados_solver.in.c', f'acados_solver_{name}.c'))
+    template_list.append(('acados_solver.in.h', f'acados_solver_{name}.h'))
+    template_list.append(('acados_solver.in.pxd', f'acados_solver.pxd'))
     if cmake_builder is not None:
-        in_file = 'CMakeLists.in.txt'
-        out_file = 'CMakeLists.txt'
-        render_template(in_file, out_file, template_dir, json_path)
+        template_list.append(('CMakeLists.in.txt', 'CMakeLists.txt'))
     else:
-        in_file = 'Makefile.in'
-        out_file = 'Makefile'
-        render_template(in_file, out_file, template_dir, json_path)
-
-    in_file = 'acados_solver_sfun.in.c'
-    out_file = f'acados_solver_sfunction_{name}.c'
-    render_template(in_file, out_file, template_dir, json_path)
+        template_list.append(('Makefile.in', 'Makefile'))
 
-    in_file = 'make_sfun.in.m'
-    out_file = f'make_sfun_{name}.m'
-    render_template(in_file, out_file, template_dir, json_path)
 
     # sim
-    in_file = 'acados_sim_solver.in.c'
-    out_file = f'acados_sim_solver_{name}.c'
-    render_template(in_file, out_file, template_dir, json_path)
-
-    in_file = 'acados_sim_solver.in.h'
-    out_file = f'acados_sim_solver_{name}.h'
-    render_template(in_file, out_file, template_dir, json_path)
-
-    in_file = 'main_sim.in.c'
-    out_file = f'main_sim_{name}.c'
-    render_template(in_file, out_file, template_dir, json_path)
-
-    ## folder model
-    template_dir = os.path.join(code_export_dir, name + '_model')
-    in_file = 'model.in.h'
-    out_file = f'{name}_model.h'
-    render_template(in_file, out_file, template_dir, json_path)
-
-    # constraints on convex over nonlinear function
-    if acados_ocp.constraints.constr_type == 'BGP' and acados_ocp.dims.nphi > 0:
-        # constraints on outer function
-        template_dir = os.path.join(code_export_dir, name + '_constraints')
-        in_file = 'phi_constraint.in.h'
-        out_file = f'{name}_phi_constraint.h'
-        render_template(in_file, out_file, template_dir, json_path)
-
-    # terminal constraints on convex over nonlinear function
-    if acados_ocp.constraints.constr_type_e == 'BGP' and acados_ocp.dims.nphi_e > 0:
-        # terminal constraints on outer function
-        template_dir = os.path.join(code_export_dir, name + '_constraints')
-        in_file = 'phi_e_constraint.in.h'
-        out_file = f'{name}_phi_e_constraint.h'
-        render_template(in_file, out_file, template_dir, json_path)
-
-    # nonlinear constraints
-    if acados_ocp.constraints.constr_type == 'BGH' and acados_ocp.dims.nh > 0:
-        template_dir = os.path.join(code_export_dir, name + '_constraints')
-        in_file = 'h_constraint.in.h'
-        out_file = f'{name}_h_constraint.h'
-        render_template(in_file, out_file, template_dir, json_path)
-
-    # terminal nonlinear constraints
-    if acados_ocp.constraints.constr_type_e == 'BGH' and acados_ocp.dims.nh_e > 0:
-        template_dir = os.path.join(code_export_dir, name + '_constraints')
-        in_file = 'h_e_constraint.in.h'
-        out_file = f'{name}_h_e_constraint.h'
-        render_template(in_file, out_file, template_dir, json_path)
-
-    # initial stage Nonlinear LS cost function
-    if acados_ocp.cost.cost_type_0 == 'NONLINEAR_LS':
-        template_dir = os.path.join(code_export_dir, name + '_cost')
-        in_file = 'cost_y_0_fun.in.h'
-        out_file = f'{name}_cost_y_0_fun.h'
-        render_template(in_file, out_file, template_dir, json_path)
-    # external cost - terminal
-    elif acados_ocp.cost.cost_type_0 == 'EXTERNAL':
-        template_dir = os.path.join(code_export_dir, name + '_cost')
-        in_file = 'external_cost_0.in.h'
-        out_file = f'{name}_external_cost_0.h'
-        render_template(in_file, out_file, template_dir, json_path)
-
-    # path Nonlinear LS cost function
-    if acados_ocp.cost.cost_type == 'NONLINEAR_LS':
-        template_dir = os.path.join(code_export_dir, name + '_cost')
-        in_file = 'cost_y_fun.in.h'
-        out_file = f'{name}_cost_y_fun.h'
-        render_template(in_file, out_file, template_dir, json_path)
-
-    # terminal Nonlinear LS cost function
-    if acados_ocp.cost.cost_type_e == 'NONLINEAR_LS':
-        template_dir = os.path.join(code_export_dir, name + '_cost')
-        in_file = 'cost_y_e_fun.in.h'
-        out_file = f'{name}_cost_y_e_fun.h'
-        render_template(in_file, out_file, template_dir, json_path)
-
-    # external cost
-    if acados_ocp.cost.cost_type == 'EXTERNAL':
-        template_dir = os.path.join(code_export_dir, name + '_cost')
-        in_file = 'external_cost.in.h'
-        out_file = f'{name}_external_cost.h'
-        render_template(in_file, out_file, template_dir, json_path)
-
-    # external cost - terminal
-    if acados_ocp.cost.cost_type_e == 'EXTERNAL':
-        template_dir = os.path.join(code_export_dir, name + '_cost')
-        in_file = 'external_cost_e.in.h'
-        out_file = f'{name}_external_cost_e.h'
-        render_template(in_file, out_file, template_dir, json_path)
+    template_list.append(('acados_sim_solver.in.c', f'acados_sim_solver_{name}.c'))
+    template_list.append(('acados_sim_solver.in.h', f'acados_sim_solver_{name}.h'))
+    template_list.append(('main_sim.in.c', f'main_sim_{name}.c'))
+
+    # model
+    model_dir = os.path.join(code_export_directory, f'{name}_model')
+    template_list.append(('model.in.h', f'{name}_model.h', model_dir))
+    # constraints
+    constraints_dir = os.path.join(code_export_directory, f'{name}_constraints')
+    template_list.append(('constraints.in.h', f'{name}_constraints.h', constraints_dir))
+    # cost
+    cost_dir = os.path.join(code_export_directory, f'{name}_cost')
+    template_list.append(('cost.in.h', f'{name}_cost.h', cost_dir))
+
+    # Simulink
+    if simulink_opts is not None:
+        template_file = os.path.join('matlab_templates', 'acados_solver_sfun.in.c')
+        template_list.append((template_file, f'acados_solver_sfunction_{name}.c'))
+        template_file = os.path.join('matlab_templates', 'acados_solver_sfun.in.c')
+        template_list.append((template_file, f'make_sfun_{name}.m'))
+
+    return template_list
 
 
 def remove_x0_elimination(acados_ocp):
@@ -820,7 +819,7 @@ class AcadosOcpSolver:
         dlclose.argtypes = [c_void_p]
 
     @classmethod
-    def generate(cls, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, cmake_builder: CMakeBuilder = None):
+    def generate(cls, acados_ocp: AcadosOcp, json_file='acados_ocp_nlp.json', simulink_opts=None, cmake_builder: CMakeBuilder = 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
@@ -834,15 +833,15 @@ class AcadosOcpSolver:
         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()
-
         # make dims consistent
         make_ocp_dims_consistent(acados_ocp)
 
         # module dependent post processing
         if acados_ocp.solver_options.integrator_type == 'GNSF':
-            set_up_imported_gnsf_model(acados_ocp)
+            if 'gnsf_model' in acados_ocp.__dict__:
+                set_up_imported_gnsf_model(acados_ocp)
+            else:
+                detect_gnsf_structure(acados_ocp)
 
         if acados_ocp.solver_options.qp_solver == 'PARTIAL_CONDENSING_QPDUNES':
             remove_x0_elimination(acados_ocp)
@@ -854,15 +853,23 @@ class AcadosOcpSolver:
         ocp_generate_external_functions(acados_ocp, model)
 
         # dump to json
-        ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file)
+        acados_ocp.json_file = json_file
+        ocp_formulation_json_dump(acados_ocp, simulink_opts=simulink_opts, json_file=json_file)
 
         # render templates
-        ocp_render_templates(acados_ocp, json_file, cmake_builder=cmake_builder)
-        acados_ocp.json_file = json_file
+        ocp_render_templates(acados_ocp, json_file, cmake_builder=cmake_builder, simulink_opts=simulink_opts)
+
+        # copy custom update function
+        if acados_ocp.solver_options.custom_update_filename != "" and acados_ocp.solver_options.custom_update_copy:
+            target_location = os.path.join(acados_ocp.code_export_directory, acados_ocp.solver_options.custom_update_filename)
+            shutil.copyfile(acados_ocp.solver_options.custom_update_filename, target_location)
+        if acados_ocp.solver_options.custom_update_header_filename != "" and acados_ocp.solver_options.custom_update_copy:
+            target_location = os.path.join(acados_ocp.code_export_directory, acados_ocp.solver_options.custom_update_header_filename)
+            shutil.copyfile(acados_ocp.solver_options.custom_update_header_filename, target_location)
 
 
     @classmethod
-    def build(cls, code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = None):
+    def build(cls, code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = None, verbose: bool = True):
         """
         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()
@@ -870,19 +877,36 @@ class AcadosOcpSolver:
             :param cmake_builder: type :py:class:`~acados_template.builders.CMakeBuilder` generate a `CMakeLists.txt` and use
                    the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with
                    `MS Visual Studio`); default: `None`
+            :param verbose: indicating if build command is printed
         """
         code_export_dir = os.path.abspath(code_export_dir)
-        cwd=os.getcwd()
+        cwd = os.getcwd()
         os.chdir(code_export_dir)
         if with_cython:
-            os.system('make clean_ocp_cython')
-            os.system('make ocp_cython')
+            call(
+                ['make', 'clean_all'],
+                stdout=None if verbose else DEVNULL,
+                stderr=None if verbose else STDOUT
+            )
+            call(
+                ['make', 'ocp_cython'],
+                stdout=None if verbose else DEVNULL,
+                stderr=None if verbose else STDOUT
+            )
         else:
             if cmake_builder is not None:
                 cmake_builder.exec(code_export_dir)
             else:
-                os.system('make clean_ocp_shared_lib')
-                os.system('make ocp_shared_lib')
+                call(
+                    ['make', 'clean_ocp_shared_lib'],
+                    stdout=None if verbose else DEVNULL,
+                    stderr=None if verbose else STDOUT
+                )
+                call(
+                    ['make', 'ocp_shared_lib'],
+                    stdout=None if verbose else DEVNULL,
+                    stderr=None if verbose else STDOUT
+                )
         os.chdir(cwd)
 
 
@@ -910,7 +934,7 @@ class AcadosOcpSolver:
                     acados_ocp_json['dims']['N'])
 
 
-    def __init__(self, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True, generate=True, cmake_builder: CMakeBuilder = None):
+    def __init__(self, acados_ocp: AcadosOcp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True, generate=True, cmake_builder: CMakeBuilder = None, verbose=True):
 
         self.solver_created = False
         if generate:
@@ -927,15 +951,13 @@ class AcadosOcpSolver:
         code_export_directory = acados_ocp_json['code_export_directory']
 
         if build:
-            self.build(code_export_directory, with_cython=False, cmake_builder=cmake_builder)
+            self.build(code_export_directory, with_cython=False, cmake_builder=cmake_builder, verbose=verbose)
 
         # prepare library loading
         lib_prefix = 'lib'
-        lib_ext = '.so'
+        lib_ext = get_lib_ext()
         if os.name == 'nt':
             lib_prefix = ''
-            lib_ext = ''
-        # ToDo: check for mac
 
         # 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.
@@ -970,16 +992,23 @@ class AcadosOcpSolver:
         assert getattr(self.shared_lib, f"{self.model_name}_acados_create")(self.capsule)==0
         self.solver_created = True
 
+        self.acados_ocp = acados_ocp
+
         # get pointers solver
         self.__get_pointers_solver()
 
         self.status = 0
 
+        # gettable fields
+        self.__qp_dynamics_fields = ['A', 'B', 'b']
+        self.__qp_cost_fields = ['Q', 'R', 'S', 'q', 'r']
+        self.__qp_constraint_fields = ['C', 'D', 'lg', 'ug', 'lbx', 'ubx', 'lbu', 'ubu']
+
 
     def __get_pointers_solver(self):
-        """
-        Private function to get the pointers for solver
-        """
+        # """
+        # Private function to get the pointers for solver
+        # """
         # get pointers solver
         getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts").argtypes = [c_void_p]
         getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts").restype = c_void_p
@@ -1010,6 +1039,25 @@ class AcadosOcpSolver:
         self.nlp_solver = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver")(self.capsule)
 
 
+
+    def solve_for_x0(self, x0_bar):
+        """
+        Wrapper around `solve()` which sets initial state constraint, solves the OCP, and returns u0.
+        """
+        self.set(0, "lbx", x0_bar)
+        self.set(0, "ubx", x0_bar)
+
+        status = self.solve()
+
+        if status == 2:
+            print("Warning: acados_ocp_solver reached maximum iterations.")
+        elif status != 0:
+            raise Exception(f'acados acados_ocp_solver returned status {status}')
+
+        u0 = self.get(0, "u")
+        return u0
+
+
     def solve(self):
         """
         Solve the ocp with current input.
@@ -1021,13 +1069,31 @@ class AcadosOcpSolver:
         return self.status
 
 
-    def reset(self):
+    def custom_update(self, data_: np.ndarray):
+        """
+        A custom function that can be implemented by a user to be called between solver calls.
+        By default this does nothing.
+        The idea is to have a convenient wrapper to do complex updates of parameters and numerical data efficiently in C,
+        in a function that is compiled into the solver library and can be conveniently used in the Python environment.
+        """
+        data = np.ascontiguousarray(data_, dtype=np.float64)
+        c_data = cast(data.ctypes.data, POINTER(c_double))
+        data_len = len(data)
+
+        getattr(self.shared_lib, f"{self.model_name}_acados_custom_update").argtypes = [c_void_p, POINTER(c_double), c_int]
+        getattr(self.shared_lib, f"{self.model_name}_acados_custom_update").restype = c_int
+        status = getattr(self.shared_lib, f"{self.model_name}_acados_custom_update")(self.capsule, c_data, data_len)
+
+        return status
+
+
+    def reset(self, reset_qp_solver_mem=1):
         """
         Sets current iterate to all zeros.
         """
-        getattr(self.shared_lib, f"{self.model_name}_acados_reset").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{self.model_name}_acados_reset").argtypes = [c_void_p, c_int]
         getattr(self.shared_lib, f"{self.model_name}_acados_reset").restype = c_int
-        getattr(self.shared_lib, f"{self.model_name}_acados_reset")(self.capsule)
+        getattr(self.shared_lib, f"{self.model_name}_acados_reset")(self.capsule, reset_qp_solver_mem)
 
         return
 
@@ -1175,18 +1241,17 @@ class AcadosOcpSolver:
         field = field_
 
         if (field_ not in all_fields):
-            raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\
-                    \n Possible values are {}. Exiting.'.format(field_, all_fields))
+            raise Exception(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): \'{field_}\' is an invalid argument.\
+                    \n Possible values are {all_fields}.')
 
         if not isinstance(stage_, int):
-            raise Exception('AcadosOcpSolver.get(): stage index must be Integer.')
+            raise Exception(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): stage index must be an integer, got type {type(stage_)}.')
 
         if stage_ < 0 or stage_ > self.N:
-            raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(stage_))
+            raise Exception(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): stage index must be in [0, {self.N}], got: {stage_}.')
 
         if stage_ == self.N and field_ == 'pi':
-            raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\
-                .format(field_, stage_))
+            raise Exception(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): field \'{field_}\' does not exist at final stage {stage_}.')
 
         if field_ in sens_fields:
             field = field_.replace('sens_', '')
@@ -1265,15 +1330,15 @@ class AcadosOcpSolver:
         return
 
 
-    def store_iterate(self, filename='', overwrite=False):
+    def store_iterate(self, filename: str = '', overwrite=False):
         """
         Stores the current iterate of the ocp solver in a json file.
 
-            :param filename: if not set, use model_name + timestamp + '.json'
+            :param filename: if not set, use f'{self.model_name}_iterate.json'
             :param overwrite: if false and filename exists add timestamp to filename
         """
         if filename == '':
-            filename += self.model_name + '_' + 'iterate' + '.json'
+            filename = f'{self.model_name}_iterate.json'
 
         if not overwrite:
             # append timestamp
@@ -1284,23 +1349,70 @@ class AcadosOcpSolver:
         # get iterate:
         solution = dict()
 
+        lN = len(str(self.N+1))
         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')
+            i_string = f'{i:0{lN}d}'
+            solution['x_'+i_string] = self.get(i,'x')
+            solution['u_'+i_string] = self.get(i,'u')
+            solution['z_'+i_string] = self.get(i,'z')
+            solution['lam_'+i_string] = self.get(i,'lam')
+            solution['t_'+i_string] = self.get(i, 't')
+            solution['sl_'+i_string] = self.get(i, 'sl')
+            solution['su_'+i_string] = self.get(i, 'su')
+            if i < self.N:
+                solution['pi_'+i_string] = self.get(i,'pi')
+
+        for k in list(solution.keys()):
+            if len(solution[k]) == 0:
+                del solution[k]
 
         # save
         with open(filename, 'w') as f:
-            json.dump(solution, f, default=np_array_to_list, indent=4, sort_keys=True)
+            json.dump(solution, f, default=make_object_json_dumpable, indent=4, sort_keys=True)
         print("stored current iterate in ", os.path.join(os.getcwd(), filename))
 
 
+
+    def dump_last_qp_to_json(self, filename: str = '', overwrite=False):
+        """
+        Dumps the latest QP data into a json file
+
+            :param filename: if not set, use model_name + timestamp + '.json'
+            :param overwrite: if false and filename exists add timestamp to filename
+        """
+        if filename == '':
+            filename = f'{self.model_name}_QP.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 QP data:
+        qp_data = dict()
+
+        lN = len(str(self.N+1))
+        for field in self.__qp_dynamics_fields:
+            for i in range(self.N):
+                qp_data[f'{field}_{i:0{lN}d}'] = self.get_from_qp_in(i,field)
+
+        for field in self.__qp_constraint_fields + self.__qp_cost_fields:
+            for i in range(self.N+1):
+                qp_data[f'{field}_{i:0{lN}d}'] = self.get_from_qp_in(i,field)
+
+        # remove empty fields
+        for k in list(qp_data.keys()):
+            if len(qp_data[k]) == 0:
+                del qp_data[k]
+
+        # save
+        with open(filename, 'w') as f:
+            json.dump(qp_data, f, default=make_object_json_dumpable, indent=4, sort_keys=True)
+        print("stored qp from solver memory 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.
@@ -1412,7 +1524,7 @@ class AcadosOcpSolver:
             return self.get_residuals()
 
         else:
-            raise Exception(f'AcadosOcpSolver.get_stats(): {field} is not a valid argument.'
+            raise Exception(f'AcadosOcpSolver.get_stats(): \'{field}\' is not a valid argument.'
                     + f'\n Possible values are {fields}.')
 
 
@@ -1440,6 +1552,12 @@ class AcadosOcpSolver:
     def get_residuals(self, recompute=False):
         """
         Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
+        This residual has to be computed for SQP_RTI solver, since it is not available by default.
+
+        - res_stat: stationarity residual
+        - res_eq: residual wrt equality constraints (dynamics)
+        - res_ineq: residual wrt inequality constraints (constraints)
+        - res_comp: residual wrt complementarity conditions
         """
         # compute residuals if RTI
         if self.solver_options['nlp_solver_type'] == 'SQP_RTI' or recompute:
@@ -1499,24 +1617,22 @@ class AcadosOcpSolver:
             value_ = np.array([value_])
         value_ = value_.astype(float)
 
-        field = field_
-        field = field.encode('utf-8')
+        field = field_.encode('utf-8')
 
         stage = c_int(stage_)
 
         # treat parameters separately
         if field_ == 'p':
-            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").argtypes = [c_void_p, c_int, POINTER(c_double), c_int]
             getattr(self.shared_lib, f"{self.model_name}_acados_update_params").restype = c_int
 
             value_data = cast(value_.ctypes.data, POINTER(c_double))
 
             assert getattr(self.shared_lib, f"{self.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:
-                raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
-                    \nPossible values are {}. Exiting.".format(field, \
-                    constraints_fields + cost_fields + out_fields + ['p']))
+            if field_ not in constraints_fields + cost_fields + out_fields + mem_fields:
+                raise Exception(f"AcadosOcpSolver.set(): '{field}' is not a valid argument.\n"
+                    f" Possible values are {constraints_fields + cost_fields + out_fields + mem_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]
@@ -1526,8 +1642,8 @@ class AcadosOcpSolver:
                 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[0])
+                msg = f'AcadosOcpSolver.set(): mismatching dimension for field "{field_}" '
+                msg += f'with dimension {dims} (you have {value_.shape[0]})'
                 raise Exception(msg)
 
             value_data = cast(value_.ctypes.data, POINTER(c_double))
@@ -1553,6 +1669,13 @@ class AcadosOcpSolver:
                     [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)
+            # also set z_guess, when setting z.
+            if field_ == 'z':
+                field = 'z_guess'.encode('utf-8')
+                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
 
 
@@ -1561,7 +1684,7 @@ class AcadosOcpSolver:
         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 field: string, e.g. 'yref', 'W', 'ext_cost_num_hess', 'zl', 'zu', 'Zl', 'Zu'
             :param value: of appropriate size
         """
         # cast value_ to avoid conversion issues
@@ -1595,7 +1718,7 @@ class AcadosOcpSolver:
                     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_) +
+                                    f"  You are relying on a now-fixed bug in cost_set for field '{field_}'.\n" +
                                     "  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" +
@@ -1663,7 +1786,7 @@ class AcadosOcpSolver:
                     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_) +
+                                    f"  You are relying on a now-fixed bug in constraints_set for field '{field}'.\n" +
                                     "  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" +
@@ -1676,7 +1799,7 @@ class AcadosOcpSolver:
                 # Get elements in column major order
                 value_ = np.ravel(value_, order='F')
             else:
-                raise Exception("Unknown api: '{}'".format(api))
+                raise Exception(f"Unknown api: '{api}'")
 
         if value_shape != tuple(dims):
             raise Exception(f'AcadosOcpSolver.constraints_set(): mismatching dimension' +
@@ -1693,27 +1816,35 @@ class AcadosOcpSolver:
         return
 
 
-    def dynamics_get(self, stage_, field_):
+    def get_from_qp_in(self, stage_: int, field_: str):
         """
-        Get numerical data from the dynamics module of the solver:
+        Get numerical data from the current QP.
 
             :param stage: integer corresponding to shooting node
-            :param field: string, e.g. 'A'
+            :param field: string in ['A', 'B', 'b', 'Q', 'R', 'S', 'q', 'r', 'C', 'D', 'lg', 'ug', 'lbx', 'ubx', 'lbu', 'ubu']
         """
+        # idx* should be added too..
+        if not isinstance(stage_, int):
+            raise TypeError("stage should be int")
+        if stage_ > self.N:
+            raise Exception("stage should be <= self.N")
+        if field_ in self.__qp_dynamics_fields and stage_ >= self.N:
+            raise ValueError(f"dynamics field {field_} not available at terminal stage")
+        if field_ not in self.__qp_dynamics_fields + self.__qp_cost_fields + self.__qp_constraint_fields:
+            raise Exception(f"field {field_} not supported.")
 
-        field = field_
-        field = field.encode('utf-8')
+        field = field_.encode('utf-8')
         stage = c_int(stage_)
 
         # get dims
-        self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr.argtypes = \
+        self.shared_lib.ocp_nlp_qp_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_dynamics_dims_get_from_attr.restype = c_int
+        self.shared_lib.ocp_nlp_qp_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_dynamics_dims_get_from_attr(self.nlp_config, \
+        self.shared_lib.ocp_nlp_qp_dims_get_from_attr(self.nlp_config, \
             self.nlp_dims, self.nlp_out, stage_, field, dims_data)
 
         # create output data
@@ -1748,32 +1879,34 @@ class AcadosOcpSolver:
             - qp_mu0: for HPIPM QP solvers: initial value for complementarity slackness
             - warm_start_first_qp: indicates if first QP in SQP is warm_started
         """
-        int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'warm_start_first_qp']
-        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent',
-        'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', 'qp_mu0']
+        int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start',
+                      'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'warm_start_first_qp']
+        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction',
+                         'eps_sufficient_descent', 'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', 'qp_mu0']
         string_fields = ['globalization']
 
         # check field availability and type
         if field_ in int_fields:
             if not isinstance(value_, int):
-                raise Exception('solver option {} must be of type int. You have {}.'.format(field_, type(value_)))
+                raise Exception(f'solver option \'{field_}\' must be of type int. You have {type(value_)}.')
             else:
                 value_ctypes = c_int(value_)
 
         elif field_ in double_fields:
             if not isinstance(value_, float):
-                raise Exception('solver option {} must be of type float. You have {}.'.format(field_, type(value_)))
+                raise Exception(f'solver option \'{field_}\' must be of type float. You have {type(value_)}.')
             else:
                 value_ctypes = c_double(value_)
 
         elif field_ in string_fields:
             if not isinstance(value_, str):
-                raise Exception('solver option {} must be of type str. You have {}.'.format(field_, type(value_)))
+                raise Exception(f'solver option \'{field_}\' must be of type str. You have {type(value_)}.')
             else:
                 value_ctypes = value_.encode('utf-8')
         else:
-            raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\
-                '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
+            fields = ', '.join(int_fields + double_fields + string_fields)
+            raise Exception(f'AcadosOcpSolver.options_set() does not support field \'{field_}\'.\n'\
+                f' Possible values are {fields}.')
 
 
         if field_ == 'rti_phase':
@@ -1802,6 +1935,44 @@ class AcadosOcpSolver:
         return
 
 
+    def set_params_sparse(self, stage_, idx_values_, param_values_):
+        """
+        set parameters of the solvers external function partially:
+        Pseudo: solver.param[idx_values_] = param_values_;
+        Parameters:
+            :param stage_: integer corresponding to shooting node
+            :param idx_values_: 0 based np array (or iterable) of integers: indices of parameter to be set
+            :param param_values_: new parameter values as numpy array
+        """
+
+        # if not isinstance(idx_values_, np.ndarray) or not issubclass(type(idx_values_[0]), np.integer):
+        #     raise Exception('idx_values_ must be np.array of integers.')
+
+        if not isinstance(param_values_, np.ndarray):
+            raise Exception('param_values_ must be np.array.')
+        elif np.float64 != param_values_.dtype:
+            raise TypeError('param_values_ must be np.array of float64.')
+
+        if param_values_.shape[0] != len(idx_values_):
+            raise Exception(f'param_values_ and idx_values_ must be of the same size.' +
+                 f' Got sizes idx {param_values_.shape[0]}, param_values {len(idx_values_)}.')
+
+        if any(idx_values_ >= self.acados_ocp.dims.np):
+            raise Exception(f'idx_values_ contains value >= np = {self.acados_ocp.dims.np}')
+
+        stage = c_int(stage_)
+        n_update = c_int(len(param_values_))
+
+        param_data = cast(param_values_.ctypes.data, POINTER(c_double))
+        c_idx_values = np.ascontiguousarray(idx_values_, dtype=np.intc)
+        idx_data = cast(c_idx_values.ctypes.data, POINTER(c_int))
+
+        getattr(self.shared_lib, f"{self.model_name}_acados_update_params_sparse").argtypes = \
+                        [c_void_p, c_int, POINTER(c_int), POINTER(c_double), c_int]
+        getattr(self.shared_lib, f"{self.model_name}_acados_update_params_sparse").restype = c_int
+        getattr(self.shared_lib, f"{self.model_name}_acados_update_params_sparse") \
+                                    (self.capsule, stage, idx_data, param_data, n_update)
+
     def __del__(self):
         if self.solver_created:
             getattr(self.shared_lib, f"{self.model_name}_acados_free").argtypes = [c_void_p]
@@ -1815,4 +1986,6 @@ class AcadosOcpSolver:
             try:
                 self.dlclose(self.shared_lib._handle)
             except:
+                print(f"WARNING: acados Python interface could not close shared_lib handle of AcadosOcpSolver {self.model_name}.\n",
+                     "Attempting to create a new one with the same name will likely result in the old one being used!")
                 pass
diff --git a/third_party/acados/acados_template/acados_ocp_solver_pyx.pyx b/third_party/acados/acados_template/acados_ocp_solver_pyx.pyx
index fe7fa8425a..acd7f02d0a 100644
--- a/third_party/acados/acados_template/acados_ocp_solver_pyx.pyx
+++ b/third_party/acados/acados_template/acados_ocp_solver_pyx.pyx
@@ -1,9 +1,6 @@
 # -*- coding: future_fstrings -*-
 #
-# 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
+# Copyright (c) The acados authors.
 #
 # This file is part of acados.
 #
@@ -63,7 +60,6 @@ cdef class AcadosOcpSolverCython:
     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
@@ -88,7 +84,6 @@ cdef class AcadosOcpSolverCython:
 
         # get pointers solver
         self.__get_pointers_solver()
-        self.status = 0
 
 
     def __get_pointers_solver(self):
@@ -105,6 +100,24 @@ cdef class AcadosOcpSolverCython:
         self.nlp_solver = acados_solver.acados_get_nlp_solver(self.capsule)
 
 
+    def solve_for_x0(self, x0_bar):
+        """
+        Wrapper around `solve()` which sets initial state constraint, solves the OCP, and returns u0.
+        """
+        self.set(0, "lbx", x0_bar)
+        self.set(0, "ubx", x0_bar)
+
+        status = self.solve()
+
+        if status == 2:
+            print("Warning: acados_ocp_solver reached maximum iterations.")
+        elif status != 0:
+            raise Exception(f'acados acados_ocp_solver returned status {status}')
+
+        u0 = self.get(0, "u")
+        return u0
+
+
     def solve(self):
         """
         Solve the ocp with current input.
@@ -112,11 +125,24 @@ cdef class AcadosOcpSolverCython:
         return acados_solver.acados_solve(self.capsule)
 
 
-    def reset(self):
+    def reset(self, reset_qp_solver_mem=1):
         """
         Sets current iterate to all zeros.
         """
-        return acados_solver.acados_reset(self.capsule)
+        return acados_solver.acados_reset(self.capsule, reset_qp_solver_mem)
+
+
+    def custom_update(self, data_):
+        """
+        A custom function that can be implemented by a user to be called between solver calls.
+        By default this does nothing.
+        The idea is to have a convenient wrapper to do complex updates of parameters and numerical data efficiently in C,
+        in a function that is compiled into the solver library and can be conveniently used in the Python environment.
+        """
+        data_len = len(data_)
+        cdef cnp.ndarray[cnp.float64_t, ndim=1] data = np.ascontiguousarray(data_, dtype=np.float64)
+
+        return acados_solver.acados_custom_update(self.capsule, <double *> data.data, data_len)
 
 
     def set_new_time_steps(self, new_time_steps):
@@ -253,7 +279,7 @@ cdef class AcadosOcpSolverCython:
 
         if field_ not in out_fields:
             raise Exception('AcadosOcpSolverCython.get(): {} is an invalid argument.\
-                    \n Possible values are {}. Exiting.'.format(field_, out_fields))
+                    \n Possible values are {}.'.format(field_, out_fields))
 
         if stage < 0 or stage > self.N:
             raise Exception('AcadosOcpSolverCython.get(): stage index must be in [0, N], got: {}.'.format(self.N))
@@ -310,16 +336,22 @@ cdef class AcadosOcpSolverCython:
         # get iterate:
         solution = dict()
 
+        lN = len(str(self.N+1))
         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')
+            i_string = f'{i:0{lN}d}'
+            solution['x_'+i_string] = self.get(i,'x')
+            solution['u_'+i_string] = self.get(i,'u')
+            solution['z_'+i_string] = self.get(i,'z')
+            solution['lam_'+i_string] = self.get(i,'lam')
+            solution['t_'+i_string] = self.get(i, 't')
+            solution['sl_'+i_string] = self.get(i, 'sl')
+            solution['su_'+i_string] = self.get(i, 'su')
+            if i < self.N:
+                solution['pi_'+i_string] = self.get(i,'pi')
+
+        for k in list(solution.keys()):
+            if len(solution[k]) == 0:
+                del solution[k]
 
         # save
         with open(filename, 'w') as f:
@@ -508,6 +540,8 @@ cdef class AcadosOcpSolverCython:
                       sl: slack variables of soft lower inequality constraints \n
                       su: slack variables of soft upper inequality constraints \n
         """
+        if not isinstance(value_, np.ndarray):
+            raise Exception(f"set: value must be numpy array, got {type(value_)}.")
         cost_fields = ['y_ref', 'yref']
         constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
         out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su']
@@ -523,7 +557,7 @@ cdef class AcadosOcpSolverCython:
         else:
             if field_ not in constraints_fields + cost_fields + out_fields:
                 raise Exception("AcadosOcpSolverCython.set(): {} is not a valid argument.\
-                    \nPossible values are {}. Exiting.".format(field, \
+                    \nPossible values are {}.".format(field, \
                     constraints_fields + cost_fields + out_fields + ['p']))
 
             dims = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config,
@@ -547,6 +581,11 @@ cdef class AcadosOcpSolverCython:
                 acados_solver_common.ocp_nlp_set(self.nlp_config, \
                     self.nlp_solver, stage, field, <void *> value.data)
 
+            if field_ == 'z':
+                field = 'z_guess'.encode('utf-8')
+                acados_solver_common.ocp_nlp_set(self.nlp_config, \
+                    self.nlp_solver, stage, field, <void *> value.data)
+        return
 
     def cost_set(self, int stage, str field_, value_):
         """
@@ -556,6 +595,8 @@ cdef class AcadosOcpSolverCython:
             :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess'
             :param value: of appropriate size
         """
+        if not isinstance(value_, np.ndarray):
+            raise Exception(f"cost_set: value must be numpy array, got {type(value_)}.")
         field = field_.encode('utf-8')
 
         cdef int dims[2]
@@ -589,6 +630,9 @@ cdef class AcadosOcpSolverCython:
             :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi', 'C', 'D']
             :param value: of appropriate size
         """
+        if not isinstance(value_, np.ndarray):
+            raise Exception(f"constraints_set: value must be numpy array, got {type(value_)}.")
+
         field = field_.encode('utf-8')
 
         cdef int dims[2]
@@ -606,7 +650,7 @@ cdef class AcadosOcpSolverCython:
             # Get elements in column major order
             value = np.asfortranarray(value_)
 
-        if value_shape[0] != dims[0] or value_shape[1] != dims[1]:
+        if value_shape != tuple(dims):
             raise Exception(f'AcadosOcpSolverCython.constraints_set(): mismatching dimension' +
                 f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
 
@@ -616,7 +660,7 @@ cdef class AcadosOcpSolverCython:
         return
 
 
-    def dynamics_get(self, int stage, str field_):
+    def get_from_qp_in(self, int stage, str field_):
         """
         Get numerical data from the dynamics module of the solver:
 
@@ -627,7 +671,7 @@ cdef class AcadosOcpSolverCython:
 
         # get dims
         cdef int[2] dims
-        acados_solver_common.ocp_nlp_dynamics_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field, &dims[0])
+        acados_solver_common.ocp_nlp_qp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field, &dims[0])
 
         # create output data
         cdef cnp.ndarray[cnp.float64_t, ndim=2] out = np.zeros((dims[0], dims[1]), order='F')
@@ -701,6 +745,50 @@ cdef class AcadosOcpSolverCython:
                 '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
 
 
+    def set_params_sparse(self, int stage, idx_values_, param_values_):
+        """
+        set parameters of the solvers external function partially:
+        Pseudo: solver.param[idx_values_] = param_values_;
+        Parameters:
+            :param stage_: integer corresponding to shooting node
+            :param idx_values_: 0 based integer array corresponding to parameter indices to be set
+            :param param_values_: new parameter values as numpy array
+        """
+
+        if not isinstance(param_values_, np.ndarray):
+            raise Exception('param_values_ must be np.array.')
+
+        if param_values_.shape[0] != len(idx_values_):
+            raise Exception(f'param_values_ and idx_values_ must be of the same size.' +
+                 f' Got sizes idx {param_values_.shape[0]}, param_values {len(idx_values_)}.')
+
+        # n_update = c_int(len(param_values_))
+
+        # param_data = cast(param_values_.ctypes.data, POINTER(c_double))
+        # c_idx_values = np.ascontiguousarray(idx_values_, dtype=np.intc)
+        # idx_data = cast(c_idx_values.ctypes.data, POINTER(c_int))
+
+        # getattr(self.shared_lib, f"{self.model_name}_acados_update_params_sparse").argtypes = \
+        #                 [c_void_p, c_int, POINTER(c_int), POINTER(c_double), c_int]
+        # getattr(self.shared_lib, f"{self.model_name}_acados_update_params_sparse").restype = c_int
+        # getattr(self.shared_lib, f"{self.model_name}_acados_update_params_sparse") \
+        #                             (self.capsule, stage, idx_data, param_data, n_update)
+
+        cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(param_values_, dtype=np.float64)
+        # cdef cnp.ndarray[cnp.intc, ndim=1] idx = np.ascontiguousarray(idx_values_, dtype=np.intc)
+
+        # NOTE: this does throw an error somehow:
+        # ValueError: Buffer dtype mismatch, expected 'int object' but got 'int'
+        # cdef cnp.ndarray[cnp.int, ndim=1] idx = np.ascontiguousarray(idx_values_, dtype=np.intc)
+
+        cdef cnp.ndarray[cnp.int32_t, ndim=1] idx = np.ascontiguousarray(idx_values_, dtype=np.int32)
+        cdef int n_update = value.shape[0]
+        # print(f"in set_params_sparse Cython n_update {n_update}")
+
+        assert acados_solver.acados_update_params_sparse(self.capsule, stage, <int *> idx.data, <double *> value.data, n_update) == 0
+        return
+
+
     def __del__(self):
         if self.solver_created:
             acados_solver.acados_free(self.capsule)
diff --git a/third_party/acados/acados_template/acados_sim.py b/third_party/acados/acados_template/acados_sim.py
index 93d5f298db..c0d6937a49 100644
--- a/third_party/acados/acados_template/acados_sim.py
+++ b/third_party/acados/acados_template/acados_sim.py
@@ -1,9 +1,6 @@
 # -*- coding: future_fstrings -*-
 #
-# 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
+# Copyright (c) The acados authors.
 #
 # This file is part of acados.
 #
@@ -33,10 +30,9 @@
 #
 
 import numpy as np
-import casadi as ca
 import os
 from .acados_model import AcadosModel
-from .utils import get_acados_path
+from .utils import get_acados_path, get_lib_ext
 
 class AcadosSimDims:
     """
@@ -73,28 +69,28 @@ class AcadosSimDims:
         if isinstance(nx, int) and nx > 0:
             self.__nx = nx
         else:
-            raise Exception('Invalid nx value, expected positive integer. Exiting.')
+            raise Exception('Invalid nx value, expected positive integer.')
 
     @nz.setter
     def nz(self, nz):
         if isinstance(nz, int) and nz > -1:
             self.__nz = nz
         else:
-            raise Exception('Invalid nz value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid nz value, expected nonnegative integer.')
 
     @nu.setter
     def nu(self, nu):
         if isinstance(nu, int) and nu > -1:
             self.__nu = nu
         else:
-            raise Exception('Invalid nu value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid nu value, expected nonnegative integer.')
 
     @np.setter
     def np(self, np):
         if isinstance(np, int) and np > -1:
             self.__np = np
         else:
-            raise Exception('Invalid np value, expected nonnegative integer. Exiting.')
+            raise Exception('Invalid np value, expected nonnegative integer.')
 
     def set(self, attr, value):
         setattr(self, attr, value)
@@ -112,13 +108,16 @@ class AcadosSimOpts:
         self.__sim_method_num_stages = 1
         self.__sim_method_num_steps = 1
         self.__sim_method_newton_iter = 3
+        # doubles
+        self.__sim_method_newton_tol = 0.0
         # bools
         self.__sens_forw = True
         self.__sens_adj = False
         self.__sens_algebraic = False
         self.__sens_hess = False
-        self.__output_z = False
+        self.__output_z = True
         self.__sim_method_jac_reuse = 0
+        self.__ext_fun_compile_flags = '-O2'
 
     @property
     def integrator_type(self):
@@ -140,6 +139,15 @@ class AcadosSimOpts:
         """Number of Newton iterations in simulation method. Default: 3"""
         return self.__sim_method_newton_iter
 
+    @property
+    def newton_tol(self):
+        """
+        Tolerance for Newton system solved in implicit integrator (IRK, GNSF).
+        0.0 means this is not used and exactly newton_iter iterations are carried out.
+        Default: 0.0
+        """
+        return self.__sim_method_newton_tol
+
     @property
     def sens_forw(self):
         """Boolean determining if forward sensitivities are computed. Default: True"""
@@ -162,7 +170,7 @@ class AcadosSimOpts:
 
     @property
     def output_z(self):
-        """Boolean determining if values for algebraic variables (corresponding to start of simulation interval) are computed. Default: False"""
+        """Boolean determining if values for algebraic variables (corresponding to start of simulation interval) are computed. Default: True"""
         return self.__output_z
 
     @property
@@ -184,6 +192,21 @@ class AcadosSimOpts:
         """
         return self.__collocation_type
 
+    @property
+    def ext_fun_compile_flags(self):
+        """
+        String with compiler flags for external function compilation.
+        Default: '-O2'.
+        """
+        return self.__ext_fun_compile_flags
+
+    @ext_fun_compile_flags.setter
+    def ext_fun_compile_flags(self, ext_fun_compile_flags):
+        if isinstance(ext_fun_compile_flags, str):
+            self.__ext_fun_compile_flags = ext_fun_compile_flags
+        else:
+            raise Exception('Invalid ext_fun_compile_flags, expected a string.\n')
+
     @integrator_type.setter
     def integrator_type(self, integrator_type):
         integrator_types = ('ERK', 'IRK', 'GNSF')
@@ -191,7 +214,7 @@ class AcadosSimOpts:
             self.__integrator_type = integrator_type
         else:
             raise Exception('Invalid integrator_type value. Possible values are:\n\n' \
-                    + ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\nExiting.')
+                    + ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\n')
 
     @collocation_type.setter
     def collocation_type(self, collocation_type):
@@ -200,7 +223,7 @@ class AcadosSimOpts:
             self.__collocation_type = collocation_type
         else:
             raise Exception('Invalid collocation_type value. Possible values are:\n\n' \
-                    + ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\nExiting.')
+                    + ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\n')
 
     @T.setter
     def T(self, T):
@@ -227,6 +250,13 @@ class AcadosSimOpts:
         else:
             raise Exception('Invalid newton_iter value. newton_iter must be an integer.')
 
+    @newton_tol.setter
+    def newton_tol(self, newton_tol):
+        if isinstance(newton_tol, float):
+            self.__sim_method_newton_tol = newton_tol
+        else:
+            raise Exception('Invalid newton_tol value. newton_tol must be an float.')
+
     @sens_forw.setter
     def sens_forw(self, sens_forw):
         if sens_forw in (True, False):
@@ -280,6 +310,7 @@ class AcadosSim:
     - :py:attr:`solver_options` of type :py:class:`acados_template.acados_sim.AcadosSimOpts`
 
     - :py:attr:`acados_include_path` (set automatically)
+    - :py:attr:`shared_lib_ext` (set automatically)
     - :py:attr:`acados_lib_path` (set automatically)
     - :py:attr:`parameter_values` - used to initialize the parameters (can be changed)
 
@@ -301,9 +332,14 @@ class AcadosSim:
 
         self.code_export_directory = 'c_generated_code'
         """Path to where code will be exported. Default: `c_generated_code`."""
+        self.shared_lib_ext = get_lib_ext()
+
+        # get cython paths
+        from sysconfig import get_paths
+        self.cython_include_dirs = [np.get_include(), get_paths()['include']]
 
-        self.cython_include_dirs = ''
         self.__parameter_values = np.array([])
+        self.__problem_class = 'SIM'
 
     @property
     def parameter_values(self):
diff --git a/third_party/acados/acados_template/acados_sim_layout.json b/third_party/acados/acados_template/acados_sim_layout.json
index 25b149613b..e3ca4b575b 100644
--- a/third_party/acados/acados_template/acados_sim_layout.json
+++ b/third_party/acados/acados_template/acados_sim_layout.json
@@ -42,6 +42,12 @@
         ],
         "sim_method_newton_iter": [
             "int"
+        ],
+        "sim_method_newton_tol": [
+            "float"
+        ],
+        "ext_fun_compile_flags": [
+            "str"
         ]
     }
 }
diff --git a/third_party/acados/acados_template/acados_sim_solver.py b/third_party/acados/acados_template/acados_sim_solver.py
index 3588dd38cd..612f439eaf 100644
--- a/third_party/acados/acados_template/acados_sim_solver.py
+++ b/third_party/acados/acados_template/acados_sim_solver.py
@@ -1,9 +1,6 @@
 # -*- coding: future_fstrings -*-
 #
-# 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
+# Copyright (c) The acados authors.
 #
 # This file is part of acados.
 #
@@ -32,56 +29,58 @@
 # POSSIBILITY OF SUCH DAMAGE.;
 #
 
-import sys, os, json
+import sys
+import os
+import json
+import importlib
 
 import numpy as np
 
-from ctypes import *
+from subprocess import DEVNULL, call, STDOUT
+
+from ctypes import POINTER, cast, CDLL, c_void_p, c_char_p, c_double, c_int, c_bool, byref
 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 .casadi_function_generation import generate_c_code_implicit_ode, generate_c_code_gnsf, generate_c_code_explicit_ode
 from .acados_sim import AcadosSim
 from .acados_ocp import AcadosOcp
-from .acados_model import acados_model_strip_casadi_symbolics
-from .utils import is_column, render_template, format_class_dict, np_array_to_list,\
-     make_model_consistent, set_up_imported_gnsf_model, get_python_interface_path
+from .utils import is_column, render_template, format_class_dict, make_object_json_dumpable,\
+     make_model_consistent, set_up_imported_gnsf_model, get_python_interface_path, get_lib_ext,\
+     casadi_length, is_empty, check_casadi_version
 from .builders import CMakeBuilder
+from .gnsf.detect_gnsf_structure import detect_gnsf_structure
+
 
 
-def make_sim_dims_consistent(acados_sim):
+def make_sim_dims_consistent(acados_sim: AcadosSim):
     dims = acados_sim.dims
     model = acados_sim.model
     # nx
     if is_column(model.x):
-        dims.nx = model.x.shape[0]
+        dims.nx = casadi_length(model.x)
     else:
-        raise Exception("model.x should be column vector!")
+        raise Exception('model.x should be column vector!')
 
     # nu
-    if is_column(model.u):
-        dims.nu = model.u.shape[0]
-    elif model.u == None or model.u == []:
+    if is_empty(model.u):
         dims.nu = 0
     else:
-        raise Exception("model.u should be column vector or None!")
+        dims.nu = casadi_length(model.u)
 
     # nz
-    if is_column(model.z):
-        dims.nz = model.z.shape[0]
-    elif model.z == None or model.z == []:
+    if is_empty(model.z):
         dims.nz = 0
     else:
-        raise Exception("model.z should be column vector or None!")
+        dims.nz = casadi_length(model.z)
 
     # np
-    if is_column(model.p):
-        dims.np = model.p.shape[0]
-    elif model.p == None or model.p == []:
+    if is_empty(model.p):
         dims.np = 0
     else:
-        raise Exception("model.p should be column vector or None!")
+        dims.np = casadi_length(model.p)
+    if acados_sim.parameter_values.shape[0] != dims.np:
+        raise Exception('inconsistent dimension np, regarding model.p and parameter_values.' + \
+            f'\nGot np = {dims.np}, acados_sim.parameter_values.shape = {acados_sim.parameter_values.shape[0]}\n')
 
 
 def get_sim_layout():
@@ -92,7 +91,7 @@ def get_sim_layout():
     return sim_layout
 
 
-def sim_formulation_json_dump(acados_sim, json_file='acados_sim.json'):
+def sim_formulation_json_dump(acados_sim: AcadosSim, json_file='acados_sim.json'):
     # Load acados_sim structure description
     sim_layout = get_sim_layout()
 
@@ -105,11 +104,10 @@ def sim_formulation_json_dump(acados_sim, json_file='acados_sim.json'):
         # Copy sim object attributes dictionaries
         sim_dict[key]=dict(getattr(acados_sim, key).__dict__)
 
-    sim_dict['model'] = acados_model_strip_casadi_symbolics(sim_dict['model'])
     sim_json = format_class_dict(sim_dict)
 
     with open(json_file, 'w') as f:
-        json.dump(sim_json, f, default=np_array_to_list, indent=4, sort_keys=True)
+        json.dump(sim_json, f, default=make_object_json_dumpable, indent=4, sort_keys=True)
 
 
 def sim_get_default_cmake_builder() -> CMakeBuilder:
@@ -122,47 +120,49 @@ def sim_get_default_cmake_builder() -> CMakeBuilder:
     return cmake_builder
 
 
-def sim_render_templates(json_file, model_name, code_export_dir, cmake_options: CMakeBuilder = None):
+def sim_render_templates(json_file, model_name: str, code_export_dir, cmake_options: CMakeBuilder = None):
     # setting up loader and environment
     json_path = os.path.join(os.getcwd(), json_file)
 
     if not os.path.exists(json_path):
         raise Exception(f"{json_path} not found!")
 
-    template_dir = code_export_dir
-
-    ## Render templates
+    # Render templates
     in_file = 'acados_sim_solver.in.c'
     out_file = f'acados_sim_solver_{model_name}.c'
-    render_template(in_file, out_file, template_dir, json_path)
+    render_template(in_file, out_file, code_export_dir, json_path)
 
     in_file = 'acados_sim_solver.in.h'
     out_file = f'acados_sim_solver_{model_name}.h'
-    render_template(in_file, out_file, template_dir, json_path)
+    render_template(in_file, out_file, code_export_dir, json_path)
+
+    in_file = 'acados_sim_solver.in.pxd'
+    out_file = f'acados_sim_solver.pxd'
+    render_template(in_file, out_file, code_export_dir, json_path)
 
     # Builder
     if cmake_options is not None:
         in_file = 'CMakeLists.in.txt'
         out_file = 'CMakeLists.txt'
-        render_template(in_file, out_file, template_dir, json_path)
+        render_template(in_file, out_file, code_export_dir, json_path)
     else:
         in_file = 'Makefile.in'
         out_file = 'Makefile'
-        render_template(in_file, out_file, template_dir, json_path)
+        render_template(in_file, out_file, code_export_dir, json_path)
 
     in_file = 'main_sim.in.c'
     out_file = f'main_sim_{model_name}.c'
-    render_template(in_file, out_file, template_dir, json_path)
+    render_template(in_file, out_file, code_export_dir, json_path)
 
-    ## folder model
-    template_dir = os.path.join(code_export_dir, model_name + '_model')
+    # folder model
+    model_dir = os.path.join(code_export_dir, model_name + '_model')
 
     in_file = 'model.in.h'
     out_file = f'{model_name}_model.h'
-    render_template(in_file, out_file, template_dir, json_path)
+    render_template(in_file, out_file, model_dir, json_path)
 
 
-def sim_generate_casadi_functions(acados_sim):
+def sim_generate_external_functions(acados_sim: AcadosSim):
     model = acados_sim.model
     model = make_model_consistent(model)
 
@@ -170,7 +170,16 @@ def sim_generate_casadi_functions(acados_sim):
 
     opts = dict(generate_hess = acados_sim.solver_options.sens_hess,
                 code_export_directory = acados_sim.code_export_directory)
+
+    # create code_export_dir, model_dir
+    code_export_dir = acados_sim.code_export_directory
+    opts['code_export_directory'] = code_export_dir
+    model_dir = os.path.join(code_export_dir, model.name + '_model')
+    if not os.path.exists(model_dir):
+        os.makedirs(model_dir)
+
     # generate external functions
+    check_casadi_version()
     if integrator_type == 'ERK':
         generate_c_code_explicit_ode(model, opts)
     elif integrator_type == 'IRK':
@@ -190,62 +199,117 @@ class AcadosSimSolver:
             the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with
             `MS Visual Studio`); default: `None`
     """
-    def __init__(self, acados_sim_, json_file='acados_sim.json', build=True, cmake_builder: CMakeBuilder = None):
-
-        self.solver_created = False
+    if sys.platform=="win32":
+        from ctypes import wintypes
+        from ctypes import WinDLL
+        dlclose = WinDLL('kernel32', use_last_error=True).FreeLibrary
+        dlclose.argtypes = [wintypes.HMODULE]
+    else:
+        dlclose = CDLL(None).dlclose
+        dlclose.argtypes = [c_void_p]
 
-        if isinstance(acados_sim_, AcadosOcp):
-            # set up acados_sim_
-            acados_sim = AcadosSim()
-            acados_sim.model = acados_sim_.model
-            acados_sim.dims.nx = acados_sim_.dims.nx
-            acados_sim.dims.nu = acados_sim_.dims.nu
-            acados_sim.dims.nz = acados_sim_.dims.nz
-            acados_sim.dims.np = acados_sim_.dims.np
-            acados_sim.solver_options.integrator_type = acados_sim_.solver_options.integrator_type
-            acados_sim.code_export_directory = acados_sim_.code_export_directory
 
-        elif isinstance(acados_sim_, AcadosSim):
-            acados_sim = acados_sim_
+    @classmethod
+    def generate(cls, acados_sim: AcadosSim, json_file='acados_sim.json', cmake_builder: CMakeBuilder = None):
+        """
+        Generates the code for an acados sim solver, given the description in acados_sim
+        """
 
-        acados_sim.__problem_class = 'SIM'
+        acados_sim.code_export_directory = os.path.abspath(acados_sim.code_export_directory)
 
-        model_name = acados_sim.model.name
+        # make dims consistent
         make_sim_dims_consistent(acados_sim)
 
-        # reuse existing json and casadi functions, when creating integrator from ocp
-        if isinstance(acados_sim_, AcadosSim):
-            if acados_sim.solver_options.integrator_type == 'GNSF':
+        # module dependent post processing
+        if acados_sim.solver_options.integrator_type == 'GNSF':
+            if acados_sim.solver_options.sens_hess == True:
+                raise Exception("AcadosSimSolver: GNSF does not support sens_hess = True.")
+            if 'gnsf_model' in acados_sim.__dict__:
                 set_up_imported_gnsf_model(acados_sim)
+            else:
+                detect_gnsf_structure(acados_sim)
+
+        # generate external functions
+        sim_generate_external_functions(acados_sim)
+
+        # dump to json
+        sim_formulation_json_dump(acados_sim, json_file)
+
+        # render templates
+        sim_render_templates(json_file, acados_sim.model.name, acados_sim.code_export_directory, cmake_builder)
+
+
+    @classmethod
+    def build(cls, code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = None, verbose: bool = True):
+        # Compile solver
+        cwd = os.getcwd()
+        os.chdir(code_export_dir)
+        if with_cython:
+            call(
+                ['make', 'clean_sim_cython'],
+                stdout=None if verbose else DEVNULL,
+                stderr=None if verbose else STDOUT
+            )
+            call(
+                ['make', 'sim_cython'],
+                stdout=None if verbose else DEVNULL,
+                stderr=None if verbose else STDOUT
+            )
+        else:
+            if cmake_builder is not None:
+                cmake_builder.exec(code_export_dir, verbose=verbose)
+            else:
+                call(
+                    ['make', 'sim_shared_lib'],
+                    stdout=None if verbose else DEVNULL,
+                    stderr=None if verbose else STDOUT
+                )
+        os.chdir(cwd)
 
-            sim_generate_casadi_functions(acados_sim)
-            sim_formulation_json_dump(acados_sim, json_file)
 
-        code_export_dir = acados_sim.code_export_directory
-        if build:
-            # render templates
-            sim_render_templates(json_file, model_name, code_export_dir, cmake_builder)
+    @classmethod
+    def create_cython_solver(cls, json_file):
+        """
+        """
+        with open(json_file, 'r') as f:
+            acados_sim_json = json.load(f)
+        code_export_directory = acados_sim_json['code_export_directory']
 
-            # Compile solver
-            cwd = os.getcwd()
-            code_export_dir = os.path.abspath(code_export_dir)
-            os.chdir(code_export_dir)
-            if cmake_builder is not None:
-                cmake_builder.exec(code_export_dir)
-            else:
-                os.system('make sim_shared_lib')
-            os.chdir(cwd)
+        importlib.invalidate_caches()
+        rel_code_export_directory = os.path.relpath(code_export_directory)
+        acados_sim_solver_pyx = importlib.import_module(f'{rel_code_export_directory}.acados_sim_solver_pyx')
+
+        AcadosSimSolverCython = getattr(acados_sim_solver_pyx, 'AcadosSimSolverCython')
+        return AcadosSimSolverCython(acados_sim_json['model']['name'])
 
-        self.sim_struct = acados_sim
-        model_name = self.sim_struct.model.name
+    def __init__(self, acados_sim, json_file='acados_sim.json', generate=True, build=True, cmake_builder: CMakeBuilder = None, verbose: bool = True):
+
+        self.solver_created = False
+        self.acados_sim = acados_sim
+        model_name = acados_sim.model.name
         self.model_name = model_name
 
+        code_export_dir = os.path.abspath(acados_sim.code_export_directory)
+
+        # reuse existing json and casadi functions, when creating integrator from ocp
+        if generate and not isinstance(acados_sim, AcadosOcp):
+            self.generate(acados_sim, json_file=json_file, cmake_builder=cmake_builder)
+
+        if build:
+            self.build(code_export_dir, cmake_builder=cmake_builder, verbose=True)
+
+        # prepare library loading
+        lib_prefix = 'lib'
+        lib_ext = get_lib_ext()
+        if os.name == 'nt':
+            lib_prefix = ''
+
         # 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_name = f'{lib_prefix}acados{lib_ext}'
         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
@@ -257,19 +321,12 @@ class AcadosSimSolver:
             print('acados was compiled with OpenMP.')
         else:
             print('acados was compiled without OpenMP.')
+        libacados_sim_solver_name = f'{lib_prefix}acados_sim_solver_{self.model_name}{lib_ext}'
+        self.shared_lib_name = os.path.join(code_export_dir, libacados_sim_solver_name)
 
-        # Ctypes
-        lib_prefix = 'lib'
-        lib_ext = '.so'
-        if os.name == 'nt':
-            lib_prefix = ''
-            lib_ext = ''
-        self.shared_lib_name = os.path.join(code_export_dir, f'{lib_prefix}acados_sim_solver_{model_name}{lib_ext}')
-        print(f'self.shared_lib_name = "{self.shared_lib_name}"')
-        
+        # get shared_lib
         self.shared_lib = CDLL(self.shared_lib_name)
 
-
         # create capsule
         getattr(self.shared_lib, f"{model_name}_acados_sim_solver_create_capsule").restype = c_void_p
         self.capsule = getattr(self.shared_lib, f"{model_name}_acados_sim_solver_create_capsule")()
@@ -304,23 +361,34 @@ class AcadosSimSolver:
         getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver").restype = c_void_p
         self.sim_solver = getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver")(self.capsule)
 
-        nu = self.sim_struct.dims.nu
-        nx = self.sim_struct.dims.nx
-        nz = self.sim_struct.dims.nz
-        self.gettable = {
-            'x': nx,
-            'xn': nx,
-            'u': nu,
-            'z': nz,
-            'S_forw': nx*(nx+nu),
-            'Sx': nx*nx,
-            'Su': nx*nu,
-            'S_adj': nx+nu,
-            'S_hess': (nx+nu)*(nx+nu),
-            'S_algebraic': (nz)*(nx+nu),
-        }
-
-        self.settable = ['S_adj', 'T', 'x', 'u', 'xdot', 'z', 'p'] # S_forw
+        self.gettable_vectors = ['x', 'u', 'z', 'S_adj']
+        self.gettable_matrices = ['S_forw', 'Sx', 'Su', 'S_hess', 'S_algebraic']
+        self.gettable_scalars = ['CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la']
+
+
+    def simulate(self, x=None, u=None, z=None, p=None):
+        """
+        Simulate the system forward for the given x, u, z, p and return x_next.
+        Wrapper around `solve()` taking care of setting/getting inputs/outputs.
+        """
+        if x is not None:
+            self.set('x', x)
+        if u is not None:
+            self.set('u', u)
+        if z is not None:
+            self.set('z', z)
+        if p is not None:
+            self.set('p', p)
+
+        status = self.solve()
+
+        if status == 2:
+            print("Warning: acados_sim_solver reached maximum iterations.")
+        elif status != 0:
+            raise Exception(f'acados_sim_solver for model {self.model_name} returned status {status}.')
+
+        x_next = self.get('x')
+        return x_next
 
 
     def solve(self):
@@ -338,55 +406,64 @@ class AcadosSimSolver:
         """
         Get the last solution of the solver.
 
-            :param str field: string in ['x', 'u', 'z', 'S_forw', 'Sx', 'Su', 'S_adj', 'S_hess', 'S_algebraic']
+            :param str field: string in ['x', 'u', 'z', 'S_forw', 'Sx', 'Su', 'S_adj', 'S_hess', 'S_algebraic', 'CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la']
         """
-        field = field_
-        field = field.encode('utf-8')
+        field = field_.encode('utf-8')
 
-        if field_ in self.gettable.keys():
+        if field_ in self.gettable_vectors:
+            # get dims
+            dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
+            dims_data = cast(dims.ctypes.data, POINTER(c_int))
+
+            self.shared_lib.sim_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_int)]
+            self.shared_lib.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, dims_data)
 
             # allocate array
-            dims = self.gettable[field_]
-            out = np.ascontiguousarray(np.zeros((dims,)), dtype=np.float64)
+            out = np.ascontiguousarray(np.zeros((dims[0],)), dtype=np.float64)
+            out_data = cast(out.ctypes.data, POINTER(c_double))
+
+            self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p]
+            self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, out_data)
+
+        elif field_ in self.gettable_matrices:
+            # get dims
+            dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
+            dims_data = cast(dims.ctypes.data, POINTER(c_int))
+
+            self.shared_lib.sim_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_int)]
+            self.shared_lib.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, dims_data)
+
+            out = np.zeros((dims[0], dims[1]), order='F')
             out_data = cast(out.ctypes.data, POINTER(c_double))
 
             self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p]
             self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, out_data)
 
-            if field_ == 'S_forw':
-                nu = self.sim_struct.dims.nu
-                nx = self.sim_struct.dims.nx
-                out = out.reshape(nx, nx+nu, order='F')
-            elif field_ == 'Sx':
-                nx = self.sim_struct.dims.nx
-                out = out.reshape(nx, nx, order='F')
-            elif field_ == 'Su':
-                nx = self.sim_struct.dims.nx
-                nu = self.sim_struct.dims.nu
-                out = out.reshape(nx, nu, order='F')
-            elif field_ == 'S_hess':
-                nx = self.sim_struct.dims.nx
-                nu = self.sim_struct.dims.nu
-                out = out.reshape(nx+nu, nx+nu, order='F')
-            elif field_ == 'S_algebraic':
-                nx = self.sim_struct.dims.nx
-                nu = self.sim_struct.dims.nu
-                nz = self.sim_struct.dims.nz
-                out = out.reshape(nz, nx+nu, order='F')
+        elif field_ in self.gettable_scalars:
+            scalar = c_double()
+            scalar_data = byref(scalar)
+            self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p]
+            self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, scalar_data)
+
+            out = scalar.value
         else:
             raise Exception(f'AcadosSimSolver.get(): Unknown field {field_},' \
-                f' available fields are {", ".join(self.gettable.keys())}')
+                f' available fields are {", ".join(self.gettable_vectors+self.gettable_matrices)}, {", ".join(self.gettable_scalars)}')
 
         return out
 
 
-    def set(self, field_, value_):
+
+    def set(self, field_: str, value_):
         """
         Set numerical data inside the solver.
 
-            :param field: string in ['p', 'S_adj', 'T', 'x', 'u', 'xdot', 'z']
+            :param field: string in ['x', 'u', 'p', 'xdot', 'z', 'seed_adj', 'T']
             :param value: the value with appropriate size.
         """
+        settable = ['x', 'u', 'p', 'xdot', 'z', 'seed_adj', 'T'] # S_forw
+
+        # TODO: check and throw error here. then remove checks in Cython for speed
         # cast value_ to avoid conversion issues
         if isinstance(value_, (float, int)):
             value_ = np.array([value_])
@@ -395,12 +472,11 @@ class AcadosSimSolver:
         value_data = cast(value_.ctypes.data, POINTER(c_double))
         value_data_p = cast((value_data), c_void_p)
 
-        field = field_
-        field = field.encode('utf-8')
+        field = field_.encode('utf-8')
 
         # treat parameters separately
         if field_ == 'p':
-            model_name = self.sim_struct.model.name
+            model_name = self.acados_sim.model.name
             getattr(self.shared_lib, f"{model_name}_acados_sim_update_params").argtypes = [c_void_p, POINTER(c_double), c_int]
             value_data = cast(value_.ctypes.data, POINTER(c_double))
             getattr(self.shared_lib, f"{model_name}_acados_sim_update_params")(self.capsule, value_data, value_.shape[0])
@@ -420,19 +496,46 @@ class AcadosSimSolver:
                 value_shape = (value_shape[0], 0)
 
             if value_shape != tuple(dims):
-                raise Exception('AcadosSimSolver.set(): mismatching dimension' \
-                    ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))
+                raise Exception(f'AcadosSimSolver.set(): mismatching dimension' \
+                    f' for field "{field_}" with dimension {tuple(dims)} (you have {value_shape}).')
 
         # set
         if field_ in ['xdot', 'z']:
             self.shared_lib.sim_solver_set.argtypes = [c_void_p, c_char_p, c_void_p]
             self.shared_lib.sim_solver_set(self.sim_solver, field, value_data_p)
-        elif field_ in self.settable:
+        elif field_ in settable:
             self.shared_lib.sim_in_set.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p]
             self.shared_lib.sim_in_set(self.sim_config, self.sim_dims, self.sim_in, field, value_data_p)
         else:
             raise Exception(f'AcadosSimSolver.set(): Unknown field {field_},' \
-                f' available fields are {", ".join(self.settable)}')
+                f' available fields are {", ".join(settable)}')
+
+        return
+
+
+    def options_set(self, field_: str, value_: bool):
+        """
+        Set solver options
+
+            :param field: string in ['sens_forw', 'sens_adj', 'sens_hess']
+            :param value: Boolean
+        """
+        fields = ['sens_forw', 'sens_adj', 'sens_hess']
+        if field_ not in fields:
+            raise Exception(f"field {field_} not supported. Supported values are {', '.join(fields)}.\n")
+
+        field = field_.encode('utf-8')
+        value_ctypes = c_bool(value_)
+
+        if not isinstance(value_, bool):
+            raise TypeError("options_set: expected boolean for value")
+
+        # only allow setting
+        if getattr(self.acados_sim.solver_options, field_) or value_ == False:
+            self.shared_lib.sim_opts_set.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_bool)]
+            self.shared_lib.sim_opts_set(self.sim_config, self.sim_opts, field, value_ctypes)
+        else:
+            raise RuntimeError(f"Cannot set option {field_} to True, because it was False in original solver options.\n")
 
         return
 
@@ -451,4 +554,6 @@ class AcadosSimSolver:
             try:
                 self.dlclose(self.shared_lib._handle)
             except:
+                print(f"WARNING: acados Python interface could not close shared_lib handle of AcadosSimSolver {self.model_name}.\n",
+                     "Attempting to create a new one with the same name will likely result in the old one being used!")
                 pass
diff --git a/third_party/acados/acados_template/acados_sim_solver_common.pxd b/third_party/acados/acados_template/acados_sim_solver_common.pxd
new file mode 100644
index 0000000000..cc6a58efd7
--- /dev/null
+++ b/third_party/acados/acados_template/acados_sim_solver_common.pxd
@@ -0,0 +1,64 @@
+# -*- coding: future_fstrings -*-
+#
+# Copyright (c) The acados authors.
+#
+# 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.;
+#
+
+
+cdef extern from "acados/sim/sim_common.h":
+    ctypedef struct sim_config:
+        pass
+
+    ctypedef struct sim_opts:
+        pass
+
+    ctypedef struct sim_in:
+        pass
+
+    ctypedef struct sim_out:
+        pass
+
+
+cdef extern from "acados_c/sim_interface.h":
+
+    ctypedef struct sim_plan:
+        pass
+
+    ctypedef struct sim_solver:
+        pass
+
+    # out
+    void sim_out_get(sim_config *config, void *dims, sim_out *out, const char *field, void *value)
+    int sim_dims_get_from_attr(sim_config *config, void *dims, const char *field, void *dims_data)
+
+    # opts
+    void sim_opts_set(sim_config *config, void *opts_, const char *field, void *value)
+
+    # get/set
+    void sim_in_set(sim_config *config, void *dims, sim_in *sim_in, const char *field, void *value)
+    void sim_solver_set(sim_solver *solver, const char *field, void *value)
\ No newline at end of file
diff --git a/third_party/acados/acados_template/acados_sim_solver_pyx.pyx b/third_party/acados/acados_template/acados_sim_solver_pyx.pyx
new file mode 100644
index 0000000000..be400addc7
--- /dev/null
+++ b/third_party/acados/acados_template/acados_sim_solver_pyx.pyx
@@ -0,0 +1,256 @@
+# -*- coding: future_fstrings -*-
+#
+# Copyright (c) The acados authors.
+#
+# 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.;
+#
+# cython: language_level=3
+# cython: profile=False
+# distutils: language=c
+
+cimport cython
+from libc cimport string
+# from libc cimport bool as bool_t
+
+cimport acados_sim_solver_common
+cimport acados_sim_solver
+
+cimport numpy as cnp
+
+import os
+from datetime import datetime
+import numpy as np
+
+
+cdef class AcadosSimSolverCython:
+    """
+    Class to interact with the acados sim solver C object.
+    """
+
+    cdef acados_sim_solver.sim_solver_capsule *capsule
+    cdef void *sim_dims
+    cdef acados_sim_solver_common.sim_opts *sim_opts
+    cdef acados_sim_solver_common.sim_config *sim_config
+    cdef acados_sim_solver_common.sim_out *sim_out
+    cdef acados_sim_solver_common.sim_in *sim_in
+    cdef acados_sim_solver_common.sim_solver *sim_solver
+
+    cdef bint solver_created
+
+    cdef str model_name
+
+    cdef str sim_solver_type
+
+    cdef list gettable_vectors
+    cdef list gettable_matrices
+    cdef list gettable_scalars
+
+    def __cinit__(self, model_name):
+
+        self.solver_created = False
+
+        self.model_name = model_name
+
+        # create capsule
+        self.capsule = acados_sim_solver.acados_sim_solver_create_capsule()
+
+        # create solver
+        assert acados_sim_solver.acados_sim_create(self.capsule) == 0
+        self.solver_created = True
+
+        # get pointers solver
+        self.__get_pointers_solver()
+
+        self.gettable_vectors = ['x', 'u', 'z', 'S_adj']
+        self.gettable_matrices = ['S_forw', 'Sx', 'Su', 'S_hess', 'S_algebraic']
+        self.gettable_scalars = ['CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la']
+
+    def __get_pointers_solver(self):
+        """
+        Private function to get the pointers for solver
+        """
+        # get pointers solver
+        self.sim_opts = acados_sim_solver.acados_get_sim_opts(self.capsule)
+        self.sim_dims = acados_sim_solver.acados_get_sim_dims(self.capsule)
+        self.sim_config = acados_sim_solver.acados_get_sim_config(self.capsule)
+        self.sim_out = acados_sim_solver.acados_get_sim_out(self.capsule)
+        self.sim_in = acados_sim_solver.acados_get_sim_in(self.capsule)
+        self.sim_solver = acados_sim_solver.acados_get_sim_solver(self.capsule)
+
+
+    def simulate(self, x=None, u=None, z=None, p=None):
+        """
+        Simulate the system forward for the given x, u, z, p and return x_next.
+        Wrapper around `solve()` taking care of setting/getting inputs/outputs.
+        """
+        if x is not None:
+            self.set('x', x)
+        if u is not None:
+            self.set('u', u)
+        if z is not None:
+            self.set('z', z)
+        if p is not None:
+            self.set('p', p)
+
+        status = self.solve()
+
+        if status == 2:
+            print("Warning: acados_sim_solver reached maximum iterations.")
+        elif status != 0:
+            raise Exception(f'acados_sim_solver for model {self.model_name} returned status {status}.')
+
+        x_next = self.get('x')
+        return x_next
+
+
+    def solve(self):
+        """
+        Solve the sim with current input.
+        """
+        return acados_sim_solver.acados_sim_solve(self.capsule)
+
+
+    def get(self, field_):
+        """
+        Get the last solution of the solver.
+
+            :param str field: string in ['x', 'u', 'z', 'S_forw', 'Sx', 'Su', 'S_adj', 'S_hess', 'S_algebraic', 'CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la']
+        """
+        field = field_.encode('utf-8')
+
+        if field_ in self.gettable_vectors:
+            return self.__get_vector(field)
+        elif field_ in self.gettable_matrices:
+            return self.__get_matrix(field)
+        elif field_ in self.gettable_scalars:
+            return self.__get_scalar(field)
+        else:
+            raise Exception(f'AcadosSimSolver.get(): Unknown field {field_},' \
+                f' available fields are {", ".join(self.gettable.keys())}')
+
+
+    def __get_scalar(self, field):
+        cdef double scalar
+        acados_sim_solver_common.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, <void *> &scalar)
+        return scalar
+
+
+    def __get_vector(self, field):
+        cdef int[2] dims
+        acados_sim_solver_common.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, &dims[0])
+        # cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.ascontiguousarray(np.zeros((dims[0],), dtype=np.float64))
+        cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.zeros((dims[0]),)
+        acados_sim_solver_common.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, <void *> out.data)
+        return out
+
+
+    def __get_matrix(self, field):
+        cdef int[2] dims
+        acados_sim_solver_common.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, &dims[0])
+        cdef cnp.ndarray[cnp.float64_t, ndim=2] out = np.zeros((dims[0], dims[1]), order='F', dtype=np.float64)
+        acados_sim_solver_common.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, <void *> out.data)
+        return out
+
+
+    def set(self, field_: str, value_):
+        """
+        Set numerical data inside the solver.
+
+            :param field: string in ['p', 'seed_adj', 'T', 'x', 'u', 'xdot', 'z']
+            :param value: the value with appropriate size.
+        """
+        settable = ['seed_adj', 'T', 'x', 'u', 'xdot', 'z', 'p'] # S_forw
+
+        # cast value_ to avoid conversion issues
+        if isinstance(value_, (float, int)):
+            value_ = np.array([value_])
+        # if len(value_.shape) > 1:
+            # raise RuntimeError('AcadosSimSolverCython.set(): value_ should be 1 dimensional')
+
+        cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(value_, dtype=np.float64).flatten()
+
+        field = field_.encode('utf-8')
+        cdef int[2] dims
+
+        # treat parameters separately
+        if field_ == 'p':
+            assert acados_sim_solver.acados_sim_update_params(self.capsule, <double *> value.data, value.shape[0]) == 0
+            return
+        else:
+            acados_sim_solver_common.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, &dims[0])
+
+            value_ = np.ravel(value_, order='F')
+
+            value_shape = value_.shape
+            if len(value_shape) == 1:
+                value_shape = (value_shape[0], 0)
+
+            if value_shape != tuple(dims):
+                raise Exception(f'AcadosSimSolverCython.set(): mismatching dimension' \
+                    f' for field "{field_}" with dimension {tuple(dims)} (you have {value_shape}).')
+
+        # set
+        if field_ in ['xdot', 'z']:
+            acados_sim_solver_common.sim_solver_set(self.sim_solver, field, <void *> value.data)
+        elif field_ in settable:
+            acados_sim_solver_common.sim_in_set(self.sim_config, self.sim_dims, self.sim_in, field, <void *> value.data)
+        else:
+            raise Exception(f'AcadosSimSolverCython.set(): Unknown field {field_},' \
+                f' available fields are {", ".join(settable)}')
+
+
+    def options_set(self, field_: str, value_: bool):
+        """
+        Set solver options
+
+            :param field: string in ['sens_forw', 'sens_adj', 'sens_hess']
+            :param value: Boolean
+        """
+        fields = ['sens_forw', 'sens_adj', 'sens_hess']
+        if field_ not in fields:
+            raise Exception(f"field {field_} not supported. Supported values are {', '.join(fields)}.\n")
+
+        field = field_.encode('utf-8')
+
+        if not isinstance(value_, bool):
+            raise TypeError("options_set: expected boolean for value")
+
+        cdef bint bool_value = value_
+        acados_sim_solver_common.sim_opts_set(self.sim_config, self.sim_opts, field, <void *> &bool_value)
+        # TODO: only allow setting
+        # if getattr(self.acados_sim.solver_options, field_) or value_ == False:
+        #     acados_sim_solver_common.sim_opts_set(self.sim_config, self.sim_opts, field, <void *> &bool_value)
+        # else:
+        #     raise RuntimeError(f"Cannot set option {field_} to True, because it was False in original solver options.\n")
+
+        return
+
+
+    def __del__(self):
+        if self.solver_created:
+            acados_sim_solver.acados_sim_free(self.capsule)
+            acados_sim_solver.acados_sim_solver_free_capsule(self.capsule)
diff --git a/third_party/acados/acados_template/acados_solver_common.pxd b/third_party/acados/acados_template/acados_solver_common.pxd
index fedd7190d9..c6d59d40a5 100644
--- a/third_party/acados/acados_template/acados_solver_common.pxd
+++ b/third_party/acados/acados_template/acados_solver_common.pxd
@@ -1,9 +1,6 @@
 # -*- coding: future_fstrings -*-
 #
-# 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
+# Copyright (c) The acados authors.
 #
 # This file is part of acados.
 #
@@ -87,7 +84,7 @@ cdef extern from "acados_c/ocp_nlp_interface.h":
         int stage, const char *field, int *dims_out)
     void ocp_nlp_cost_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
         int stage, const char *field, int *dims_out)
-    void ocp_nlp_dynamics_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+    void ocp_nlp_qp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
         int stage, const char *field, int *dims_out)
 
     # opts
diff --git a/third_party/acados/acados_template/builders.py b/third_party/acados/acados_template/builders.py
index f595033ceb..6f21bfe8cd 100644
--- a/third_party/acados/acados_template/builders.py
+++ b/third_party/acados/acados_template/builders.py
@@ -34,7 +34,7 @@
 
 import os
 import sys
-from subprocess import call
+from subprocess import DEVNULL, call, STDOUT
 
 
 class CMakeBuilder:
@@ -78,7 +78,7 @@ class CMakeBuilder:
     def get_cmd3_install(self):
         return f'cmake --install "{self._build_dir}"'
 
-    def exec(self, code_export_directory):
+    def exec(self, code_export_directory, verbose=True):
         """
         Execute the compilation using `CMake` with the given settings.
         :param code_export_directory: must be the absolute path to the directory where the code was exported to
@@ -96,17 +96,32 @@ class CMakeBuilder:
             os.chdir(self._build_dir)
             cmd_str = self.get_cmd1_cmake()
             print(f'call("{cmd_str})"')
-            retcode = call(cmd_str, shell=True)
+            retcode = call(
+                cmd_str,
+                shell=True,
+                stdout=None if verbose else DEVNULL,
+                stderr=None if verbose else STDOUT
+            )
             if retcode != 0:
                 raise RuntimeError(f'CMake command "{cmd_str}" was terminated by signal {retcode}')
             cmd_str = self.get_cmd2_build()
             print(f'call("{cmd_str}")')
-            retcode = call(cmd_str, shell=True)
+            retcode = call(
+                cmd_str,
+                shell=True,
+                stdout=None if verbose else DEVNULL,
+                stderr=None if verbose else STDOUT
+            )
             if retcode != 0:
                 raise RuntimeError(f'Build command "{cmd_str}" was terminated by signal {retcode}')
             cmd_str = self.get_cmd3_install()
             print(f'call("{cmd_str}")')
-            retcode = call(cmd_str, shell=True)
+            retcode = call(
+                cmd_str,
+                shell=True,
+                stdout=None if verbose else DEVNULL,
+                stderr=None if verbose else STDOUT
+            )
             if retcode != 0:
                 raise RuntimeError(f'Install command "{cmd_str}" was terminated by signal {retcode}')
         except OSError as e:
diff --git a/third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt b/third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt
index 3d6483b5d2..99bc26f750 100644
--- a/third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt
+++ b/third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt
@@ -1,8 +1,5 @@
 #
-# 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
+# Copyright (c) The acados authors.
 #
 # This file is part of acados.
 #
@@ -121,12 +118,14 @@
 	{%- set openmp_flag = " " %}
 	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
 		{%- set link_libs = "-lqpOASES_e" %}
+	{%- elif qp_solver == "FULL_CONDENSING_DAQP" %}
+		{%- set link_libs = "-ldaqp" %}
 	{%- else %}
 		{%- set link_libs = "" %}
 	{%- endif %}
 {%- endif %}
 
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.13)
 
 project({{ model.name }})
 
@@ -139,6 +138,14 @@ option(BUILD_SIM_EXAMPLE "Should the simulation example main_sim_{{ model.name }
 option(BUILD_ACADOS_SIM_SOLVER_LIB "Should the simulation solver library acados_sim_solver_{{ model.name }} be build?" OFF)
 {%- endif %}
 
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" AND CMAKE_SYSTEM_NAME MATCHES "Windows")
+    # MinGW, change to .lib such that mex recognizes it
+    set(CMAKE_SHARED_LIBRARY_SUFFIX ".lib")
+    set(CMAKE_SHARED_LIBRARY_PREFIX "")
+endif()
+
+
 # object target names
 set(MODEL_OBJ model_{{ model.name }})
 set(OCP_OBJ ocp_{{ model.name }})
@@ -146,9 +153,11 @@ set(SIM_OBJ sim_{{ model.name }})
 
 # model
 set(MODEL_SRC
+	{%- if model.dyn_ext_fun_type == "casadi" %}
 {%- if  solver_options.integrator_type == "ERK" %}
     {{ model.name }}_model/{{ model.name }}_expl_ode_fun.c
     {{ model.name }}_model/{{ model.name }}_expl_vde_forw.c
+    {{ model.name }}_model/{{ model.name }}_expl_vde_adj.c
 	{%- if hessian_approx == "EXACT" %}
     {{ model.name }}_model/{{ model.name }}_expl_ode_hess.c
 	{%- endif %}
@@ -176,16 +185,15 @@ set(MODEL_SRC
 	{%- endif %}
     {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c
 {%- elif solver_options.integrator_type == "DISCRETE" %}
-	{%- if model.dyn_ext_fun_type == "casadi" %}
     {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c
     {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c
 		{%- if hessian_approx == "EXACT" %}
     {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c
 		{%- endif %}
+{%- endif -%}
 	{%- else %}
-    {{ model.name }}_model/{{ model.dyn_source_discrete }}
+    {{ model.name }}_model/{{ model.dyn_generic_source }}
 	{%- endif %}
-{%- endif -%}
 )
 add_library(${MODEL_OBJ} OBJECT ${MODEL_SRC} )
 
@@ -219,6 +227,9 @@ if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_ACADOS_OCP_SOLVER_LIB} OR ${BUILD_EXAMP
         {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c
         {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c
         {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c
+{%- elif cost_type_0 == "CONVEX_OVER_NONLINEAR" %}
+        {{ model.name }}_cost/{{ model.name }}_conl_cost_0_fun.c
+        {{ model.name }}_cost/{{ model.name }}_conl_cost_0_fun_jac_hess.c
 {%- elif cost_type_0 == "EXTERNAL" %}
     {%- if cost.cost_ext_fun_type_0 == "casadi" %}
         {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c
@@ -232,6 +243,9 @@ if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_ACADOS_OCP_SOLVER_LIB} OR ${BUILD_EXAMP
         {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c
         {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c
         {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c
+{%- elif cost_type == "CONVEX_OVER_NONLINEAR" %}
+        {{ model.name }}_cost/{{ model.name }}_conl_cost_fun.c
+        {{ model.name }}_cost/{{ model.name }}_conl_cost_fun_jac_hess.c
 {%- elif cost_type == "EXTERNAL" %}
     {%- if cost.cost_ext_fun_type == "casadi" %}
         {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
@@ -245,6 +259,9 @@ if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_ACADOS_OCP_SOLVER_LIB} OR ${BUILD_EXAMP
         {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c
         {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c
         {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c
+{%- elif cost_type_e == "CONVEX_OVER_NONLINEAR" %}
+        {{ model.name }}_cost/{{ model.name }}_conl_cost_e_fun.c
+        {{ model.name }}_cost/{{ model.name }}_conl_cost_e_fun_jac_hess.c
 {%- elif cost_type_e == "EXTERNAL" %}
     {%- if cost.cost_ext_fun_type_e == "casadi" %}
         {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c
@@ -275,7 +292,7 @@ set(EX_SRC main_{{ model.name }}.c)
 set(EX_EXE main_{{ model.name }})
 
 {%- if model_external_shared_lib_dir and model_external_shared_lib_name %}
-set(EXTERNAL_DIR {{ model_external_shared_lib_dir  }})
+set(EXTERNAL_DIR {{ model_external_shared_lib_dir | replace(from="\", to="/")  }})
 set(EXTERNAL_LIB {{ model_external_shared_lib_name }})
 {%- else %}
 set(EXTERNAL_DIR)
@@ -283,23 +300,26 @@ set(EXTERNAL_LIB)
 {%- endif %}
 
 # set some search paths for preprocessor and linker
-set(ACADOS_INCLUDE_PATH {{ acados_include_path }} CACHE PATH "Define the path which contains the include directory for acados.")
-set(ACADOS_LIB_PATH {{ acados_lib_path }} CACHE PATH "Define the path which contains the lib directory for acados.")
+set(ACADOS_INCLUDE_PATH {{ acados_include_path | replace(from="\", to="/")  }} CACHE PATH "Define the path which contains the include directory for acados.")
+set(ACADOS_LIB_PATH {{ acados_lib_path | replace(from="\", to="/") }} CACHE PATH "Define the path which contains the lib directory for acados.")
 
 # c-compiler flags for debugging
 set(CMAKE_C_FLAGS_DEBUG "-O0 -ggdb")
 
-set(CMAKE_C_FLAGS "
+set(CMAKE_C_FLAGS "-fPIC -std=c99 {{ openmp_flag }}
 {%- if qp_solver == "FULL_CONDENSING_QPOASES" -%}
-   -DACADOS_WITH_QPOASES
+    -DACADOS_WITH_QPOASES
+{%- endif -%}
+{%- if qp_solver == "FULL_CONDENSING_DAQP" -%}
+    -DACADOS_WITH_DAQP
 {%- endif -%}
 {%- if qp_solver == "PARTIAL_CONDENSING_OSQP" -%}
-   -DACADOS_WITH_OSQP
+    -DACADOS_WITH_OSQP
 {%- endif -%}
 {%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" -%}
-   -DACADOS_WITH_QPDUNES
+    -DACADOS_WITH_QPDUNES
 {%- endif -%}
-    -fPIC -std=c99 {{ openmp_flag }}")
+")
 #-fno-diagnostics-show-line-numbers -g
 
 include_directories(
@@ -310,6 +330,9 @@ include_directories(
 {%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
    ${ACADOS_INCLUDE_PATH}/qpOASES_e/
 {%- endif %}
+{%- if qp_solver == "FULL_CONDENSING_DAQP" %}
+   ${ACADOS_INCLUDE_PATH}/daqp/include
+{%- endif %}
 )
 
 # linker flags
diff --git a/third_party/acados/acados_template/c_templates_tera/CPPLINT.cfg b/third_party/acados/acados_template/c_templates_tera/CPPLINT.cfg
deleted file mode 100644
index bbd1caf057..0000000000
--- a/third_party/acados/acados_template/c_templates_tera/CPPLINT.cfg
+++ /dev/null
@@ -1 +0,0 @@
-exclude_files=[main, acados_solver, acados_solver_sfun, Makefile, model].*\.?
diff --git a/third_party/acados/acados_template/c_templates_tera/Makefile.in b/third_party/acados/acados_template/c_templates_tera/Makefile.in
index d45be0a9c7..fbefc08e38 100644
--- a/third_party/acados/acados_template/c_templates_tera/Makefile.in
+++ b/third_party/acados/acados_template/c_templates_tera/Makefile.in
@@ -1,8 +1,5 @@
 #
-# 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
+# Copyright (c) The acados authors.
 #
 # This file is part of acados.
 #
@@ -120,6 +117,8 @@
 	{%- set openmp_flag = " " %}
 	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
 		{%- set link_libs = "-lqpOASES_e" %}
+	{%- elif qp_solver == "FULL_CONDENSING_DAQP" %}
+		{%- set link_libs = "-ldaqp" %}
 	{%- else %}
 		{%- set link_libs = "" %}
 	{%- endif %}
@@ -129,9 +128,11 @@
 
 # model
 MODEL_SRC=
-{%- if  solver_options.integrator_type == "ERK" %}
+	{%- if model.dyn_ext_fun_type == "casadi" %}
+{%- if solver_options.integrator_type == "ERK" %}
 MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.c
 MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_vde_adj.c
 	{%- if hessian_approx == "EXACT" %}
 MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.c
 	{%- endif %}
@@ -139,9 +140,9 @@ MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.c
 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" %}
+		{%- if hessian_approx == "EXACT" %}
 MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
-	{%- endif %}
+		{%- endif %}
 {%- elif solver_options.integrator_type == "LIFTED_IRK" %}
 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
@@ -159,16 +160,15 @@ MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c
 	{%- 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_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 %}
+{%- endif %}
 	{%- else %}
-MODEL_SRC+= {{ model.name }}_model/{{ model.dyn_source_discrete }}
+MODEL_SRC+= {{ model.name }}_model/{{ model.dyn_generic_source }}
 	{%- endif %}
-{%- endif %}
 MODEL_OBJ := $(MODEL_SRC:.c=.o)
 
 # optimal control problem - mostly CasADi exports
@@ -200,6 +200,9 @@ OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_z
 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 == "CONVEX_OVER_NONLINEAR" %}
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_0_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_0_fun_jac_hess.c
 {%- elif cost_type_0 == "EXTERNAL" %}
 	{%- if cost.cost_ext_fun_type_0 == "casadi" %}
 OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c
@@ -213,6 +216,9 @@ OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }}
 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 == "CONVEX_OVER_NONLINEAR" %}
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_fun_jac_hess.c
 {%- elif cost_type == "EXTERNAL" %}
 	{%- if cost.cost_ext_fun_type == "casadi" %}
 OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
@@ -226,6 +232,9 @@ OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }}
 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 == "CONVEX_OVER_NONLINEAR" %}
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_e_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_e_fun_jac_hess.c
 {%- elif cost_type_e == "EXTERNAL" %}
 	{%- if cost.cost_ext_fun_type_e == "casadi" %}
 OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c
@@ -235,6 +244,12 @@ OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c
 OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }}
 	{%- endif %}
 {%- endif %}
+{%- if solver_options.custom_update_filename %}
+	{%- if solver_options.custom_update_filename != "" %}
+OCP_SRC+= {{ solver_options.custom_update_filename }}
+	{%- endif %}
+{%- endif %}
+
 OCP_SRC+= acados_solver_{{ model.name }}.c
 OCP_OBJ := $(OCP_SRC:.c=.o)
 
@@ -275,6 +290,9 @@ LIB_PATH = {{ acados_lib_path }}
 {%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
 CPPFLAGS += -DACADOS_WITH_QPOASES
 {%- endif %}
+{%- if qp_solver == "FULL_CONDENSING_DAQP" %}
+CPPFLAGS += -DACADOS_WITH_DAQP
+{%- endif %}
 {%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %}
 CPPFLAGS += -DACADOS_WITH_OSQP
 {%- endif %}
@@ -288,10 +306,13 @@ CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include
  {%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
 CPPFLAGS+= -I $(INCLUDE_PATH)/qpOASES_e/
  {%- endif %}
+ {%- if qp_solver == "FULL_CONDENSING_DAQP" %}
+CPPFLAGS+= -I $(INCLUDE_PATH)/daqp/include
+ {%- 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
+CFLAGS = -fPIC -std=c99 {{ openmp_flag }} {{ solver_options.ext_fun_compile_flags }}#-fno-diagnostics-show-line-numbers -g
 # # Debugging
 # CFLAGS += -g3
 
@@ -306,9 +327,9 @@ LDLIBS+= -lm
 LDLIBS+= {{ link_libs }}
 
 # 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)
+LIBACADOS_SOLVER=libacados_solver_{{ model.name }}{{ shared_lib_ext }}
+LIBACADOS_OCP_SOLVER=libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }}
+LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c={{ shared_lib_ext }})
 
 # virtual targets
 .PHONY : all clean
@@ -354,38 +375,73 @@ ocp_cython_o: ocp_cython_c
 	$(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 }} \
+	{%- for path in cython_include_dirs %}
+	-I {{ path }} \
+	{%- endfor %}
 	acados_ocp_solver_pyx.c \
 
 ocp_cython: ocp_cython_o
 	$(CC) $(ACADOS_FLAGS) -shared \
-	-o acados_ocp_solver_pyx.so \
+	-o acados_ocp_solver_pyx{{ shared_lib_ext }} \
 	-Wl,-rpath=$(LIB_PATH) \
 	acados_ocp_solver_pyx.o \
-	$(abspath .)/libacados_ocp_solver_{{ model.name }}.so \
+	$(abspath .)/libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }} \
+	$(LDFLAGS) $(LDLIBS)
+
+# Sim Cython targets
+sim_cython_c: sim_shared_lib
+	cython \
+	-o acados_sim_solver_pyx.c \
+	-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
+	$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \
+	-I {{ code_export_directory }} \
+
+sim_cython_o: sim_cython_c
+	$(CC) $(ACADOS_FLAGS) -c -O2 \
+	-fPIC \
+	-o acados_sim_solver_pyx.o \
+	-I $(INCLUDE_PATH)/blasfeo/include/ \
+	-I $(INCLUDE_PATH)/hpipm/include/ \
+	-I $(INCLUDE_PATH) \
+	{%- for path in cython_include_dirs %}
+	-I {{ path }} \
+	{%- endfor %}
+	acados_sim_solver_pyx.c \
+
+sim_cython: sim_cython_o
+	$(CC) $(ACADOS_FLAGS) -shared \
+	-o acados_sim_solver_pyx{{ shared_lib_ext }} \
+	-Wl,-rpath=$(LIB_PATH) \
+	acados_sim_solver_pyx.o \
+	$(abspath .)/libacados_sim_solver_{{ model.name }}{{ shared_lib_ext }} \
 	$(LDFLAGS) $(LDLIBS)
 
 {%- if os and os == "pc" %}
 
 clean:
 	del \Q *.o 2>nul
-	del \Q *.so 2>nul
+	del \Q *{{ shared_lib_ext }} 2>nul
 	del \Q main_{{ model.name }} 2>nul
 
 clean_ocp_shared_lib:
-	del \Q libacados_ocp_solver_{{ model.name }}.so 2>nul
+	del \Q libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }} 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 libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }} 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{{ shared_lib_ext }} 2>nul
 	del \Q acados_ocp_solver_pyx.o 2>nul
 
+clean_sim_cython:
+	del \Q libacados_sim_solver_{{ model.name }}{{ shared_lib_ext }} 2>nul
+	del \Q acados_sim_solver_{{ model.name }}.o 2>nul
+	del \Q acados_sim_solver_pyx{{ shared_lib_ext }} 2>nul
+	del \Q acados_sim_solver_pyx.o 2>nul
+
 {%- else %}
 
 clean:
@@ -398,9 +454,15 @@ clean_ocp_shared_lib:
 	$(RM) $(OCP_OBJ)
 
 clean_ocp_cython:
-	$(RM) libacados_ocp_solver_{{ model.name }}.so
+	$(RM) libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }}
 	$(RM) acados_solver_{{ model.name }}.o
-	$(RM) acados_ocp_solver_pyx.so
+	$(RM) acados_ocp_solver_pyx{{ shared_lib_ext }}
 	$(RM) acados_ocp_solver_pyx.o
 
+clean_sim_cython:
+	$(RM) libacados_sim_solver_{{ model.name }}{{ shared_lib_ext }}
+	$(RM) acados_sim_solver_{{ model.name }}.o
+	$(RM) acados_sim_solver_pyx{{ shared_lib_ext }}
+	$(RM) acados_sim_solver_pyx.o
+
 {%- endif %}
diff --git a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.c b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.c
index f2e75058c1..0cd098273c 100644
--- a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.c
+++ b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.c
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -88,18 +85,19 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     double Tsim = {{ solver_options.Tsim }};
 
     {% if solver_options.integrator_type == "IRK" %}
-    capsule->sim_impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
-    capsule->sim_impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
-    capsule->sim_impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_impl_dae_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
+    capsule->sim_impl_dae_fun_jac_x_xdot_z = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
+    capsule->sim_impl_dae_jac_x_xdot_u_z = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
 
+  {%- if model.dyn_ext_fun_type == "casadi" %}
     // external functions (implicit model)
-    capsule->sim_impl_dae_fun->casadi_fun  = &{{ model.name }}_impl_dae_fun;
+    capsule->sim_impl_dae_fun->casadi_fun = &{{ model.name }}_impl_dae_fun;
     capsule->sim_impl_dae_fun->casadi_work = &{{ model.name }}_impl_dae_fun_work;
     capsule->sim_impl_dae_fun->casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_sparsity_in;
     capsule->sim_impl_dae_fun->casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_sparsity_out;
     capsule->sim_impl_dae_fun->casadi_n_in = &{{ model.name }}_impl_dae_fun_n_in;
     capsule->sim_impl_dae_fun->casadi_n_out = &{{ model.name }}_impl_dae_fun_n_out;
-    external_function_param_casadi_create(capsule->sim_impl_dae_fun, np);
+    external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_impl_dae_fun, np);
 
     capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_fun = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z;
     capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_work = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_work;
@@ -107,33 +105,39 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out;
     capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in;
     capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out;
-    external_function_param_casadi_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, np);
+    external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, np);
 
-    // external_function_param_casadi impl_dae_jac_x_xdot_u_z;
+    // external_function_param_{{ model.dyn_ext_fun_type }} impl_dae_jac_x_xdot_u_z;
     capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_fun = &{{ model.name }}_impl_dae_jac_x_xdot_u_z;
     capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_work = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_work;
     capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in;
     capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out;
     capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in;
     capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out;
-    external_function_param_casadi_create(capsule->sim_impl_dae_jac_x_xdot_u_z, np);
+    external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_impl_dae_jac_x_xdot_u_z, np);
+  {%- else %}
+    capsule->sim_impl_dae_fun->fun = &{{ model.dyn_impl_dae_fun }};
+    capsule->sim_impl_dae_fun_jac_x_xdot_z->fun = &{{ model.dyn_impl_dae_fun_jac }};
+    capsule->sim_impl_dae_jac_x_xdot_u_z->fun = &{{ model.dyn_impl_dae_jac }};
+  {%- endif %}
 
 {%- if hessian_approx == "EXACT" %}
-    capsule->sim_impl_dae_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
-    // external_function_param_casadi impl_dae_jac_x_xdot_u_z;
+    capsule->sim_impl_dae_hess = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
+    // external_function_param_{{ model.dyn_ext_fun_type }} impl_dae_jac_x_xdot_u_z;
     capsule->sim_impl_dae_hess->casadi_fun = &{{ model.name }}_impl_dae_hess;
     capsule->sim_impl_dae_hess->casadi_work = &{{ model.name }}_impl_dae_hess_work;
     capsule->sim_impl_dae_hess->casadi_sparsity_in = &{{ model.name }}_impl_dae_hess_sparsity_in;
     capsule->sim_impl_dae_hess->casadi_sparsity_out = &{{ model.name }}_impl_dae_hess_sparsity_out;
     capsule->sim_impl_dae_hess->casadi_n_in = &{{ model.name }}_impl_dae_hess_n_in;
     capsule->sim_impl_dae_hess->casadi_n_out = &{{ model.name }}_impl_dae_hess_n_out;
-    external_function_param_casadi_create(capsule->sim_impl_dae_hess, np);
+    external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_impl_dae_hess, np);
 {%- endif %}
 
     {% elif solver_options.integrator_type == "ERK" %}
     // explicit ode
-    capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
-    capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_forw_vde_casadi = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
+    capsule->sim_vde_adj_casadi = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
+    capsule->sim_expl_ode_fun_casadi = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
 
     capsule->sim_forw_vde_casadi->casadi_fun = &{{ model.name }}_expl_vde_forw;
     capsule->sim_forw_vde_casadi->casadi_n_in = &{{ model.name }}_expl_vde_forw_n_in;
@@ -141,7 +145,15 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     capsule->sim_forw_vde_casadi->casadi_sparsity_in = &{{ model.name }}_expl_vde_forw_sparsity_in;
     capsule->sim_forw_vde_casadi->casadi_sparsity_out = &{{ model.name }}_expl_vde_forw_sparsity_out;
     capsule->sim_forw_vde_casadi->casadi_work = &{{ model.name }}_expl_vde_forw_work;
-    external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
+    external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_forw_vde_casadi, np);
+
+    capsule->sim_vde_adj_casadi->casadi_fun = &{{ model.name }}_expl_vde_adj;
+    capsule->sim_vde_adj_casadi->casadi_n_in = &{{ model.name }}_expl_vde_adj_n_in;
+    capsule->sim_vde_adj_casadi->casadi_n_out = &{{ model.name }}_expl_vde_adj_n_out;
+    capsule->sim_vde_adj_casadi->casadi_sparsity_in = &{{ model.name }}_expl_vde_adj_sparsity_in;
+    capsule->sim_vde_adj_casadi->casadi_sparsity_out = &{{ model.name }}_expl_vde_adj_sparsity_out;
+    capsule->sim_vde_adj_casadi->casadi_work = &{{ model.name }}_expl_vde_adj_work;
+    external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_vde_adj_casadi, np);
 
     capsule->sim_expl_ode_fun_casadi->casadi_fun = &{{ model.name }}_expl_ode_fun;
     capsule->sim_expl_ode_fun_casadi->casadi_n_in = &{{ model.name }}_expl_ode_fun_n_in;
@@ -149,30 +161,30 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &{{ model.name }}_expl_ode_fun_sparsity_in;
     capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &{{ model.name }}_expl_ode_fun_sparsity_out;
     capsule->sim_expl_ode_fun_casadi->casadi_work = &{{ model.name }}_expl_ode_fun_work;
-    external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
+    external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_expl_ode_fun_casadi, np);
 
 {%- if hessian_approx == "EXACT" %}
-    capsule->sim_expl_ode_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
-    // external_function_param_casadi impl_dae_jac_x_xdot_u_z;
+    capsule->sim_expl_ode_hess = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
+    // external_function_param_{{ model.dyn_ext_fun_type }} impl_dae_jac_x_xdot_u_z;
     capsule->sim_expl_ode_hess->casadi_fun = &{{ model.name }}_expl_ode_hess;
     capsule->sim_expl_ode_hess->casadi_work = &{{ model.name }}_expl_ode_hess_work;
     capsule->sim_expl_ode_hess->casadi_sparsity_in = &{{ model.name }}_expl_ode_hess_sparsity_in;
     capsule->sim_expl_ode_hess->casadi_sparsity_out = &{{ model.name }}_expl_ode_hess_sparsity_out;
     capsule->sim_expl_ode_hess->casadi_n_in = &{{ model.name }}_expl_ode_hess_n_in;
     capsule->sim_expl_ode_hess->casadi_n_out = &{{ model.name }}_expl_ode_hess_n_out;
-    external_function_param_casadi_create(capsule->sim_expl_ode_hess, np);
+    external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_expl_ode_hess, np);
 {%- 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));
+    capsule->sim_gnsf_phi_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
+    capsule->sim_gnsf_phi_fun_jac_y = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
+    capsule->sim_gnsf_phi_jac_y_uhat = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
   {% 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));
+    capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
   {%- endif %}
   {%- endif %}
-    capsule->sim_gnsf_get_matrices_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_gnsf_get_matrices_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
 
   {% if model.gnsf.purely_linear != 1 %}
     capsule->sim_gnsf_phi_fun->casadi_fun = &{{ model.name }}_gnsf_phi_fun;
@@ -181,7 +193,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     capsule->sim_gnsf_phi_fun->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_sparsity_in;
     capsule->sim_gnsf_phi_fun->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_sparsity_out;
     capsule->sim_gnsf_phi_fun->casadi_work = &{{ model.name }}_gnsf_phi_fun_work;
-    external_function_param_casadi_create(capsule->sim_gnsf_phi_fun, np);
+    external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_phi_fun, np);
 
     capsule->sim_gnsf_phi_fun_jac_y->casadi_fun = &{{ model.name }}_gnsf_phi_fun_jac_y;
     capsule->sim_gnsf_phi_fun_jac_y->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_jac_y_n_in;
@@ -189,7 +201,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     capsule->sim_gnsf_phi_fun_jac_y->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in;
     capsule->sim_gnsf_phi_fun_jac_y->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out;
     capsule->sim_gnsf_phi_fun_jac_y->casadi_work = &{{ model.name }}_gnsf_phi_fun_jac_y_work;
-    external_function_param_casadi_create(capsule->sim_gnsf_phi_fun_jac_y, np);
+    external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_phi_fun_jac_y, np);
 
     capsule->sim_gnsf_phi_jac_y_uhat->casadi_fun = &{{ model.name }}_gnsf_phi_jac_y_uhat;
     capsule->sim_gnsf_phi_jac_y_uhat->casadi_n_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_in;
@@ -197,7 +209,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     capsule->sim_gnsf_phi_jac_y_uhat->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in;
     capsule->sim_gnsf_phi_jac_y_uhat->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out;
     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);
+    external_function_param_{{ model.dyn_ext_fun_type }}_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;
@@ -206,7 +218,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in;
     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);
+    external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, np);
   {%- endif %}
   {%- endif %}
 
@@ -216,7 +228,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     capsule->sim_gnsf_get_matrices_fun->casadi_sparsity_in = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_in;
     capsule->sim_gnsf_get_matrices_fun->casadi_sparsity_out = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_out;
     capsule->sim_gnsf_get_matrices_fun->casadi_work = &{{ model.name }}_gnsf_get_matrices_fun_work;
-    external_function_param_casadi_create(capsule->sim_gnsf_get_matrices_fun, np);
+    external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_get_matrices_fun, np);
     {% endif %}
 
     // sim plan & config
@@ -252,6 +264,8 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     capsule->acados_sim_opts = {{ model.name }}_sim_opts;
     int tmp_int = {{ solver_options.sim_method_newton_iter }};
     sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "newton_iter", &tmp_int);
+    double tmp_double = {{ solver_options.sim_method_newton_tol }};
+    sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "newton_tol", &tmp_double);
     sim_collocation_type collocation_type = {{ solver_options.collocation_type }};
     sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "collocation_type", &collocation_type);
 
@@ -307,7 +321,9 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
 
 {%- elif solver_options.integrator_type == "ERK" %}
     {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
-                 "expl_vde_for", capsule->sim_forw_vde_casadi);
+                 "expl_vde_forw", capsule->sim_forw_vde_casadi);
+    {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
+                 "expl_vde_adj", capsule->sim_vde_adj_casadi);
     {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
                  "expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
 {%- if hessian_approx == "EXACT" %}
@@ -408,28 +424,29 @@ int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule)
 
     // free external function
 {%- if solver_options.integrator_type == "IRK" %}
-    external_function_param_casadi_free(capsule->sim_impl_dae_fun);
-    external_function_param_casadi_free(capsule->sim_impl_dae_fun_jac_x_xdot_z);
-    external_function_param_casadi_free(capsule->sim_impl_dae_jac_x_xdot_u_z);
+    external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_impl_dae_fun);
+    external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_impl_dae_fun_jac_x_xdot_z);
+    external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_impl_dae_jac_x_xdot_u_z);
 {%- if hessian_approx == "EXACT" %}
-    external_function_param_casadi_free(capsule->sim_impl_dae_hess);
+    external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_impl_dae_hess);
 {%- endif %}
 {%- elif solver_options.integrator_type == "ERK" %}
-    external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
-    external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
+    external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_forw_vde_casadi);
+    external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_vde_adj_casadi);
+    external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_expl_ode_fun_casadi);
 {%- if hessian_approx == "EXACT" %}
-    external_function_param_casadi_free(capsule->sim_expl_ode_hess);
+    external_function_param_{{ model.dyn_ext_fun_type }}_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);
+    external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_phi_fun);
+    external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_phi_fun_jac_y);
+    external_function_param_{{ model.dyn_ext_fun_type }}_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);
+    external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
   {%- endif %}
   {%- endif %}
-    external_function_param_casadi_free(capsule->sim_gnsf_get_matrices_fun);
+    external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_get_matrices_fun);
 {% endif %}
 
     return 0;
@@ -449,6 +466,7 @@ int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, doubl
 
 {%- if solver_options.integrator_type == "ERK" %}
     capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
+    capsule->sim_vde_adj_casadi[0].set_param(capsule->sim_vde_adj_casadi, p);
     capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
 {%- if hessian_approx == "EXACT" %}
     capsule->sim_expl_ode_hess[0].set_param(capsule->sim_expl_ode_hess, p);
diff --git a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h
index 7306491baf..59aee62f49 100644
--- a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h
+++ b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -60,22 +57,23 @@ typedef struct sim_solver_capsule
 
     /* external functions */
     // ERK
-    external_function_param_casadi * sim_forw_vde_casadi;
-    external_function_param_casadi * sim_expl_ode_fun_casadi;
-    external_function_param_casadi * sim_expl_ode_hess;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_forw_vde_casadi;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_vde_adj_casadi;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_expl_ode_fun_casadi;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_expl_ode_hess;
 
     // IRK
-    external_function_param_casadi * sim_impl_dae_fun;
-    external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
-    external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
-    external_function_param_casadi * sim_impl_dae_hess;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_fun;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_fun_jac_x_xdot_z;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_jac_x_xdot_u_z;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_hess;
 
     // GNSF
-    external_function_param_casadi * sim_gnsf_phi_fun;
-    external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
-    external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
-    external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
-    external_function_param_casadi * sim_gnsf_get_matrices_fun;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_phi_fun;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_phi_fun_jac_y;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_phi_jac_y_uhat;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_get_matrices_fun;
 
 } sim_solver_capsule;
 
diff --git a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.pxd b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.pxd
new file mode 100644
index 0000000000..153f98d13a
--- /dev/null
+++ b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.pxd
@@ -0,0 +1,51 @@
+#
+# Copyright (c) The acados authors.
+#
+# 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_sim_solver_common
+
+cdef extern from "acados_sim_solver_{{ model.name }}.h":
+    ctypedef struct sim_solver_capsule "sim_solver_capsule":
+        pass
+
+    sim_solver_capsule * acados_sim_solver_create_capsule "{{ model.name }}_acados_sim_solver_create_capsule"()
+    int acados_sim_solver_free_capsule "{{ model.name }}_acados_sim_solver_free_capsule"(sim_solver_capsule *capsule)
+
+    int acados_sim_create "{{ model.name }}_acados_sim_create"(sim_solver_capsule * capsule)
+    int acados_sim_solve "{{ model.name }}_acados_sim_solve"(sim_solver_capsule * capsule)
+    int acados_sim_free "{{ model.name }}_acados_sim_free"(sim_solver_capsule * capsule)
+    int acados_sim_update_params "{{ model.name }}_acados_sim_update_params"(sim_solver_capsule * capsule, double *value, int np_)
+    # int acados_sim_update_params_sparse "{{ model.name }}_acados_sim_update_params_sparse"(sim_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
+
+    acados_sim_solver_common.sim_in *acados_get_sim_in "{{ model.name }}_acados_get_sim_in"(sim_solver_capsule * capsule)
+    acados_sim_solver_common.sim_out *acados_get_sim_out "{{ model.name }}_acados_get_sim_out"(sim_solver_capsule * capsule)
+    acados_sim_solver_common.sim_solver *acados_get_sim_solver "{{ model.name }}_acados_get_sim_solver"(sim_solver_capsule * capsule)
+    acados_sim_solver_common.sim_config *acados_get_sim_config "{{ model.name }}_acados_get_sim_config"(sim_solver_capsule * capsule)
+    acados_sim_solver_common.sim_opts *acados_get_sim_opts "{{ model.name }}_acados_get_sim_opts"(sim_solver_capsule * capsule)
+    void *acados_get_sim_dims "{{ model.name }}_acados_get_sim_dims"(sim_solver_capsule * capsule)
diff --git a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.c b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.c
index 0af8127709..5e36a53d10 100644
--- a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.c
+++ b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.c
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -42,34 +39,27 @@
 
 // example specific
 #include "{{ model.name }}_model/{{ model.name }}_model.h"
-{% if constraints.constr_type == "BGP" and dims.nphi %}
-#include "{{ model.name }}_constraints/{{ model.name }}_phi_constraint.h"
-{% endif %}
-{% if constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %}
-#include "{{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.h"
-{% endif %}
-{% if constraints.constr_type == "BGH" and dims.nh > 0 %}
-#include "{{ model.name }}_constraints/{{ model.name }}_h_constraint.h"
-{% endif %}
-{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
-#include "{{ model.name }}_constraints/{{ model.name }}_h_e_constraint.h"
-{% endif %}
-{%- if cost.cost_type == "NONLINEAR_LS" %}
-#include "{{ model.name }}_cost/{{ model.name }}_cost_y_fun.h"
-{%- elif cost.cost_type == "EXTERNAL" %}
-#include "{{ model.name }}_cost/{{ model.name }}_external_cost.h"
+#include "{{ model.name }}_constraints/{{ model.name }}_constraints.h"
+
+{%- if cost.cost_type != "LINEAR_LS" or cost.cost_type_e != "LINEAR_LS" or cost.cost_type_0 != "LINEAR_LS" %}
+#include "{{ model.name }}_cost/{{ model.name }}_cost.h"
 {%- endif %}
-{%- if cost.cost_type_0 == "NONLINEAR_LS" %}
-#include "{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.h"
-{%- elif cost.cost_type_0 == "EXTERNAL" %}
-#include "{{ model.name }}_cost/{{ model.name }}_external_cost_0.h"
+
+{%- if not solver_options.custom_update_filename %}
+    {%- set custom_update_filename = "" %}
+{% else %}
+    {%- set custom_update_filename = solver_options.custom_update_filename %}
 {%- endif %}
-{%- if cost.cost_type_e == "NONLINEAR_LS" %}
-#include "{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.h"
-{%- elif cost.cost_type_e == "EXTERNAL" %}
-#include "{{ model.name }}_cost/{{ model.name }}_external_cost_e.h"
+{%- if not solver_options.custom_update_header_filename %}
+    {%- set custom_update_header_filename = "" %}
+{% else %}
+    {%- set custom_update_header_filename = solver_options.custom_update_header_filename %}
+{%- endif %}
+{%- if custom_update_header_filename != "" %}
+#include "{{ custom_update_header_filename }}"
 {%- endif %}
 
+
 #include "acados_solver_{{ model.name }}.h"
 
 #define NX     {{ model.name | upper }}_NX
@@ -205,7 +195,6 @@ void {{ model.name }}_acados_create_1_set_plan(ocp_nlp_plan_t* nlp_solver_plan,
     nlp_solver_plan->nlp_constraints[N] = BGH;
     {%- endif %}
 
-{%- if solver_options.hessian_approx == "EXACT" %}
     {%- if solver_options.regularize_method == "NO_REGULARIZE" %}
     nlp_solver_plan->regularization = NO_REGULARIZE;
     {%- elif solver_options.regularize_method == "MIRROR" %}
@@ -217,7 +206,6 @@ void {{ model.name }}_acados_create_1_set_plan(ocp_nlp_plan_t* nlp_solver_plan,
     {%- elif solver_options.regularize_method == "CONVEXIFY" %}
     nlp_solver_plan->regularization = CONVEXIFY;
     {%- endif %}
-{%- endif %}
 }
 
 
@@ -324,11 +312,11 @@ ocp_nlp_dims* {{ model.name }}_acados_create_2_create_and_set_dimensions({{ mode
         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" or cost.cost_type_0 == "CONVEX_OVER_NONLINEAR"%}
     ocp_nlp_dims_set_cost(nlp_config, nlp_dims, 0, "ny", &ny[0]);
 {%- 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" or cost.cost_type == "CONVEX_OVER_NONLINEAR"%}
     for (int i = 1; i < N; i++)
         ocp_nlp_dims_set_cost(nlp_config, nlp_dims, i, "ny", &ny[i]);
 {%- endif %}
@@ -353,7 +341,7 @@ ocp_nlp_dims* {{ model.name }}_acados_create_2_create_and_set_dimensions({{ mode
     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" %}
+{%- if cost.cost_type_e == "NONLINEAR_LS" or cost.cost_type_e == "LINEAR_LS" or cost.cost_type_e == "CONVEX_OVER_NONLINEAR"%}
     ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]);
 {%- endif %}
     free(intNp1mem);
@@ -388,7 +376,6 @@ return nlp_dims;
 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
@@ -468,35 +455,50 @@ void {{ model.name }}_acados_create_3_create_and_set_functions({{ model.name }}_
 
 {% elif solver_options.integrator_type == "IRK" %}
     // implicit dae
-    capsule->impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    capsule->impl_dae_fun = (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" %}
         MAP_CASADI_FNC(impl_dae_fun[i], {{ model.name }}_impl_dae_fun);
+    {%- else %}
+        capsule->impl_dae_fun[i].fun = &{{ model.dyn_impl_dae_fun }};
+        external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->impl_dae_fun[i], {{ dims.np }});
+    {%- endif %}
     }
 
-    capsule->impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    capsule->impl_dae_fun_jac_x_xdot_z = (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" %}
         MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_z[i], {{ model.name }}_impl_dae_fun_jac_x_xdot_z);
+    {%- else %}
+        capsule->impl_dae_fun_jac_x_xdot_z[i].fun = &{{ model.dyn_impl_dae_fun_jac }};
+        external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->impl_dae_fun_jac_x_xdot_z[i], {{ dims.np }});
+    {%- endif %}
     }
 
-    capsule->impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    capsule->impl_dae_jac_x_xdot_u_z = (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" %}
         MAP_CASADI_FNC(impl_dae_jac_x_xdot_u_z[i], {{ model.name }}_impl_dae_jac_x_xdot_u_z);
+    {%- else %}
+        capsule->impl_dae_jac_x_xdot_u_z[i].fun = &{{ model.dyn_impl_dae_jac }};
+        external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->impl_dae_jac_x_xdot_u_z[i], {{ dims.np }});
+    {%- endif %}
     }
 
     {%- if solver_options.hessian_approx == "EXACT" %}
-    capsule->impl_dae_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    capsule->impl_dae_hess = (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++) {
         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);
+    capsule->impl_dae_fun = (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++) {
         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);
+    capsule->impl_dae_fun_jac_x_xdot_u = (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++) {
         MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_u[i], {{ model.name }}_impl_dae_fun_jac_x_xdot_u);
     }
@@ -574,6 +576,11 @@ void {{ model.name }}_acados_create_3_create_and_set_functions({{ model.name }}_
     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 == "CONVEX_OVER_NONLINEAR" %}
+    // convex-over-nonlinear cost
+    MAP_CASADI_FNC(conl_cost_0_fun, {{ model.name }}_conl_cost_0_fun);
+    MAP_CASADI_FNC(conl_cost_0_fun_jac_hess, {{ model.name }}_conl_cost_0_fun_jac_hess);
+
 {%- elif cost.cost_type_0 == "EXTERNAL" %}
     // external cost
     {%- if cost.cost_ext_fun_type_0 == "casadi" %}
@@ -619,6 +626,20 @@ void {{ model.name }}_acados_create_3_create_and_set_functions({{ model.name }}_
     {
         MAP_CASADI_FNC(cost_y_hess[i], {{ model.name }}_cost_y_hess);
     }
+
+{%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %}
+    // convex-over-nonlinear cost
+    capsule->conl_cost_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N-1; i++)
+    {
+        MAP_CASADI_FNC(conl_cost_fun[i], {{ model.name }}_conl_cost_fun);
+    }
+    capsule->conl_cost_fun_jac_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N-1; i++)
+    {
+        MAP_CASADI_FNC(conl_cost_fun_jac_hess[i], {{ model.name }}_conl_cost_fun_jac_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);
@@ -660,6 +681,12 @@ void {{ model.name }}_acados_create_3_create_and_set_functions({{ model.name }}_
     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 == "CONVEX_OVER_NONLINEAR" %}
+    // convex-over-nonlinear cost
+    MAP_CASADI_FNC(conl_cost_e_fun, {{ model.name }}_conl_cost_e_fun);
+    MAP_CASADI_FNC(conl_cost_e_fun_jac_hess, {{ model.name }}_conl_cost_e_fun_jac_hess);
+
 {%- elif cost.cost_type_e == "EXTERNAL" %}
     // external cost - function
     {%- if cost.cost_ext_fun_type_e == "casadi" %}
@@ -808,20 +835,9 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule
     }
 
     /**** Cost ****/
-{%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %}
-    {%- 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) %}
-        {%- for k in range(end=dims.ny_0) %}
-            {%- if cost.W_0[j][k] != 0 %}
-    W_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.W_0[j][k] }};
-            {%- endif %}
-        {%- endfor %}
-    {%- endfor %}
-    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0);
-    free(W_0);
 
+{%- if dims.ny_0 == 0 %}
+{%- elif cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" or cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %}
     double* yref_0 = calloc(NY0, sizeof(double));
     // change only the non-zero elements:
     {%- for j in range(end=dims.ny_0) %}
@@ -831,40 +847,91 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule
     {%- endfor %}
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0);
     free(yref_0);
-    {%- endif %}
 {%- endif %}
 
-{%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %}
-    {%- if dims.ny > 0 %}
-    double* W = calloc(NY*NY, sizeof(double));
+
+{%- if dims.ny == 0 %}
+{%- elif cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" or cost.cost_type == "CONVEX_OVER_NONLINEAR" %}
+    double* yref = calloc(NY, sizeof(double));
     // change only the non-zero elements:
     {%- 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] }};
+        {%- if cost.yref[j] != 0 %}
+    yref[{{ j }}] = {{ cost.yref[j] }};
+        {%- endif %}
+    {%- endfor %}
+
+    for (int i = 1; i < N; i++)
+    {
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref);
+    }
+    free(yref);
+{%- endif %}
+
+
+{%- if dims.ny_e == 0 %}
+{%- elif cost.cost_type_e == "NONLINEAR_LS" or cost.cost_type_e == "LINEAR_LS" or cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %}
+    double* yref_e = calloc(NYN, sizeof(double));
+    // change only the non-zero elements:
+    {%- for j in range(end=dims.ny_e) %}
+        {%- if cost.yref_e[j] != 0 %}
+    yref_e[{{ j }}] = {{ cost.yref_e[j] }};
+        {%- endif %}
+    {%- endfor %}
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e);
+    free(yref_e);
+{%- endif %}
+
+{%- if dims.ny_0 == 0 %}
+{%- elif cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %}
+   double* W_0 = calloc(NY0*NY0, sizeof(double));
+    // change only the non-zero elements:
+    {%- for j in range(end=dims.ny_0) %}
+        {%- for k in range(end=dims.ny_0) %}
+            {%- if cost.W_0[j][k] != 0 %}
+    W_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.W_0[j][k] }};
             {%- endif %}
         {%- endfor %}
     {%- endfor %}
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0);
+    free(W_0);
+{%- endif %}
 
-    double* yref = calloc(NY, sizeof(double));
+{%- if dims.ny == 0 %}
+{%- elif cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %}
+    double* W = calloc(NY*NY, sizeof(double));
     // change only the non-zero elements:
     {%- for j in range(end=dims.ny) %}
-        {%- if cost.yref[j] != 0 %}
-    yref[{{ j }}] = {{ cost.yref[j] }};
-        {%- endif %}
+        {%- for k in range(end=dims.ny) %}
+            {%- if cost.W[j][k] != 0 %}
+    W[{{ j }}+(NY) * {{ k }}] = {{ cost.W[j][k] }};
+            {%- endif %}
+        {%- endfor %}
     {%- endfor %}
 
     for (int i = 1; i < N; i++)
     {
         ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W);
-        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref);
     }
     free(W);
-    free(yref);
-    {%- endif %}
 {%- endif %}
 
-{%- if cost.cost_type_0 == "LINEAR_LS" %}
+{%- if dims.ny_e == 0 %}
+{%- elif cost.cost_type_e == "NONLINEAR_LS" or cost.cost_type_e == "LINEAR_LS" %}
+    double* W_e = calloc(NYN*NYN, sizeof(double));
+    // change only the non-zero elements:
+    {%- 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] }};
+            {%- endif %}
+        {%- endfor %}
+    {%- endfor %}
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e);
+    free(W_e);
+{%- endif %}
+
+{%- if dims.ny_0 == 0 %}
+{%- elif 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) %}
@@ -877,7 +944,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule
     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.nu > 0 %}
     double* Vu_0 = calloc(NY0*NU, sizeof(double));
     // change only the non-zero elements:
     {%- for j in range(end=dims.ny_0) %}
@@ -891,7 +958,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule
     free(Vu_0);
     {%- endif %}
 
-    {%- if dims.ny_0 > 0 and dims.nz > 0 %}
+    {%- if 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) %}
@@ -904,10 +971,10 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vz", Vz_0);
     free(Vz_0);
     {%- endif %}
-{%- endif %}{# LINEAR LS #}
-
+{%- endif %}
 
-{%- if cost.cost_type == "LINEAR_LS" %}
+{%- if dims.ny == 0 %}
+{%- elif 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) %}
@@ -923,7 +990,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule
     }
     free(Vx);
 
-    {% if dims.ny > 0 and dims.nu > 0 %}
+    {% if dims.nu > 0 %}
     double* Vu = calloc(NY*NU, sizeof(double));
     // change only the non-zero elements:
     {% for j in range(end=dims.ny) %}
@@ -941,7 +1008,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule
     free(Vu);
     {%- endif %}
 
-    {%- if dims.ny > 0 and dims.nz > 0 %}
+    {%- if dims.nz > 0 %}
     double* Vz = calloc(NY*NZ, sizeof(double));
     // change only the non-zero elements:
     {% for j in range(end=dims.ny) %}
@@ -960,11 +1027,28 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule
     {%- endif %}
 {%- endif %}{# LINEAR LS #}
 
+{%- if dims.ny_e == 0 %}
+{%- elif 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) %}
+        {%- for k in range(end=dims.nx) %}
+            {%- if cost.Vx_e[j][k] != 0 %}
+    Vx_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.Vx_e[j][k] }};
+            {%- endif %}
+        {%- endfor %}
+    {%- endfor %}
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e);
+    free(Vx_e);
+{%- endif %}
 
 {%- if cost.cost_type_0 == "NONLINEAR_LS" %}
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun", &capsule->cost_y_0_fun);
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun_jac", &capsule->cost_y_0_fun_jac_ut_xt);
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_hess", &capsule->cost_y_0_hess);
+{%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %}
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "conl_cost_fun", &capsule->conl_cost_0_fun);
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "conl_cost_fun_jac_hess", &capsule->conl_cost_0_fun_jac_hess);
 {%- elif cost.cost_type_0 == "EXTERNAL" %}
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "ext_cost_fun", &capsule->ext_cost_0_fun);
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "ext_cost_fun_jac", &capsule->ext_cost_0_fun_jac);
@@ -978,6 +1062,12 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule
         ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_fun_jac", &capsule->cost_y_fun_jac_ut_xt[i-1]);
         ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_hess", &capsule->cost_y_hess[i-1]);
     }
+{%- elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %}
+    for (int i = 1; i < N; i++)
+    {
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "conl_cost_fun", &capsule->conl_cost_fun[i-1]);
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "conl_cost_fun_jac_hess", &capsule->conl_cost_fun_jac_hess[i-1]);
+    }
 {%- elif cost.cost_type == "EXTERNAL" %}
     for (int i = 1; i < N; i++)
     {
@@ -987,7 +1077,25 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule
     }
 {%- endif %}
 
+{%- 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);
+
+{%- elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %}
+
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "conl_cost_fun", &capsule->conl_cost_e_fun);
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "conl_cost_fun_jac_hess", &capsule->conl_cost_e_fun_jac_hess);
+
+{%- 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);
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac", &capsule->ext_cost_e_fun_jac);
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac_hess", &capsule->ext_cost_e_fun_jac_hess);
+{%- endif %}
+
+
 {%- if dims.ns > 0 %}
+    // slacks
     double* zlumem = calloc(4*NS, sizeof(double));
     double* Zl = zlumem+NS*0;
     double* Zu = zlumem+NS*1;
@@ -1028,59 +1136,8 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule
     free(zlumem);
 {%- endif %}
 
-    // terminal cost
-{%- 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) %}
-        {%- if cost.yref_e[j] != 0 %}
-    yref_e[{{ j }}] = {{ cost.yref_e[j] }};
-        {%- endif %}
-    {%- endfor %}
-    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e);
-    free(yref_e);
-
-    double* W_e = calloc(NYN*NYN, sizeof(double));
-    // change only the non-zero elements:
-    {%- 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] }};
-            {%- endif %}
-        {%- endfor %}
-    {%- endfor %}
-    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" %}
-    double* Vx_e = calloc(NYN*NX, sizeof(double));
-    // change only the non-zero elements:
-    {% for j in range(end=dims.ny_e) %}
-        {%- for k in range(end=dims.nx) %}
-            {%- if cost.Vx_e[j][k] != 0 %}
-    Vx_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.Vx_e[j][k] }};
-            {%- endif %}
-        {%- endfor %}
-    {%- endfor %}
-    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e);
-    free(Vx_e);
-        {%- endif %}
-
-        {%- 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 #}
-
-{%- 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);
-    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac", &capsule->ext_cost_e_fun_jac);
-    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac_hess", &capsule->ext_cost_e_fun_jac_hess);
-{%- endif %}
-
 {% if dims.ns_e > 0 %}
+    // slacks terminal
     double* zluemem = calloc(4*NSN, sizeof(double));
     double* Zl_e = zluemem+NSN*0;
     double* Zu_e = zluemem+NSN*1;
@@ -1185,7 +1242,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule
     {%- endfor %}
 
     for (int i = 1; i < N; i++)
-    {       
+    {
         ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsbx", idxsbx);
         ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbx", lsbx);
         ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbx", usbx);
@@ -1363,7 +1420,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule
 {%- endif %}
 
 {% if dims.ng > 0 %}
-    // set up general constraints for stage 0 to N-1 
+    // set up general constraints for stage 0 to N-1
     double* D = calloc(NG*NU, sizeof(double));
     double* C = calloc(NG*NX, sizeof(double));
     double* lug = calloc(2*NG, sizeof(double));
@@ -1427,7 +1484,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule
     uh[{{ i }}] = {{ constraints.uh[i] }};
         {%- endif %}
     {%- endfor %}
-    
+
     for (int i = 0; i < N; i++)
     {
         // nonlinear constraints for stages 0 to N-1
@@ -1599,16 +1656,16 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule
 {% endif %}
 
 {% if dims.ng_e > 0 %}
-    // set up general constraints for last stage 
+    // set up general constraints for last stage
     double* C_e = calloc(NGN*NX, sizeof(double));
     double* lug_e = calloc(2*NGN, sizeof(double));
     double* lg_e = lug_e;
     double* ug_e = lug_e + NGN;
 
-    {% for j in range(end=dims.ng) %}
+    {% for j in range(end=dims.ng_e) %}
         {%- for k in range(end=dims.nx) %}
             {%- if constraints.C_e[j][k] != 0 %}
-    C_e[{{ j }}+NG * {{ k }}] = {{ constraints.C_e[j][k] }};
+    C_e[{{ j }}+NGN * {{ k }}] = {{ constraints.C_e[j][k] }};
             {%- endif %}
         {%- endfor %}
     {%- endfor %}
@@ -1658,7 +1715,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule
 {%- endif %}
 
 {% if dims.nphi_e > 0 and constraints.constr_type_e == "BGP" %}
-    // set up convex-over-nonlinear constraints for last stage 
+    // set up convex-over-nonlinear constraints for last stage
     double* luphi_e = calloc(2*NPHIN, sizeof(double));
     double* lphi_e = luphi_e;
     double* uphi_e = luphi_e + NPHIN;
@@ -1687,7 +1744,6 @@ void {{ model.name }}_acados_create_6_set_opts({{ model.name }}_solver_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;
 
     /************************************************
@@ -1695,12 +1751,9 @@ void {{ model.name }}_acados_create_6_set_opts({{ model.name }}_solver_capsule*
     ************************************************/
 
 {% 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, nlp_opts, "exact_hess", &nlp_solver_exact_hessian);
-    }
+    int nlp_solver_exact_hessian = 1;
+    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, nlp_opts, "exact_hess_dyn", &exact_hess_dyn);
 
@@ -1861,6 +1914,8 @@ void {{ model.name }}_acados_create_6_set_opts({{ model.name }}_solver_capsule*
     ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_N", &qp_solver_cond_N);
 {%- endif %}
 
+    int nlp_solver_ext_qp_res = {{ solver_options.nlp_solver_ext_qp_res }};
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "ext_qp_res", &nlp_solver_ext_qp_res);
 
 {%- if solver_options.qp_solver is containing("HPIPM") %}
     // set HPIPM mode: should be done before setting other QP solver options
@@ -1920,6 +1975,15 @@ void {{ model.name }}_acados_create_6_set_opts({{ model.name }}_solver_capsule*
     int print_level = {{ solver_options.print_level }};
     ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "print_level", &print_level);
 
+{%- if solver_options.qp_solver is containing('PARTIAL_CONDENSING') %}
+    int qp_solver_cond_ric_alg = {{ solver_options.qp_solver_cond_ric_alg }};
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_ric_alg", &qp_solver_cond_ric_alg);
+{% endif %}
+
+{%- if solver_options.qp_solver  == 'PARTIAL_CONDENSING_HPIPM' %}
+    int qp_solver_ric_alg = {{ solver_options.qp_solver_ric_alg }};
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_ric_alg", &qp_solver_ric_alg);
+{% endif %}
 
     int ext_cost_num_hess = {{ solver_options.ext_cost_num_hess }};
 {%- if cost.cost_type == "EXTERNAL" %}
@@ -2042,6 +2106,12 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
 
     // 9) do precomputations
     int status = {{ model.name }}_acados_create_9_precompute(capsule);
+
+    {%- if custom_update_filename != "" %}
+    // Initialize custom update function
+    custom_update_init_function(capsule);
+    {%- endif %}
+
     return status;
 }
 
@@ -2076,7 +2146,7 @@ int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_caps
 }
 
 
-int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule)
+int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule, int reset_qp_solver_mem)
 {
 
     // set initialization to all zeros
@@ -2088,8 +2158,6 @@ int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule)
     ocp_nlp_in* nlp_in = capsule->nlp_in;
     ocp_nlp_solver* nlp_solver = capsule->nlp_solver;
 
-    int nx, nu, nv, ns, nz, ni, dim;
-
     double* buffer = calloc(NX+NU+NZ+2*NS+2*NSN+NBX+NBU+NG+NH+NPHI+NBX0+NBXN+NHN+NPHIN+NGN, sizeof(double));
 
     for(int i=0; i<N+1; i++)
@@ -2119,7 +2187,7 @@ int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule)
     // get qp_status: if NaN -> reset memory
     int qp_status;
     ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "qp_status", &qp_status);
-    if (qp_status == 3)
+    if (reset_qp_solver_mem || (qp_status == 3))
     {
         // printf("\nin reset qp_status %d -> resetting QP memory\n", qp_status);
         ocp_nlp_solver_reset_qp_memory(nlp_solver, nlp_in, nlp_out);
@@ -2144,7 +2212,6 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsu
         exit(1);
     }
 
-{%- if dims.np > 0 %}
     const int N = capsule->nlp_solver_plan->N;
     if (stage < N && stage >= 0)
     {
@@ -2201,6 +2268,9 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsu
             capsule->cost_y_0_fun.set_param(&capsule->cost_y_0_fun, p);
             capsule->cost_y_0_fun_jac_ut_xt.set_param(&capsule->cost_y_0_fun_jac_ut_xt, p);
             capsule->cost_y_0_hess.set_param(&capsule->cost_y_0_hess, p);
+        {%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %}
+            capsule->conl_cost_0_fun.set_param(&capsule->conl_cost_0_fun, p);
+            capsule->conl_cost_0_fun_jac_hess.set_param(&capsule->conl_cost_0_fun_jac_hess, p);
         {%- elif cost.cost_type_0 == "EXTERNAL" %}
             capsule->ext_cost_0_fun.set_param(&capsule->ext_cost_0_fun, p);
             capsule->ext_cost_0_fun_jac.set_param(&capsule->ext_cost_0_fun_jac, p);
@@ -2213,6 +2283,9 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsu
             capsule->cost_y_fun[stage-1].set_param(capsule->cost_y_fun+stage-1, p);
             capsule->cost_y_fun_jac_ut_xt[stage-1].set_param(capsule->cost_y_fun_jac_ut_xt+stage-1, p);
             capsule->cost_y_hess[stage-1].set_param(capsule->cost_y_hess+stage-1, p);
+        {%- elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %}
+            capsule->conl_cost_fun[stage-1].set_param(capsule->conl_cost_fun+stage-1, p);
+            capsule->conl_cost_fun_jac_hess[stage-1].set_param(capsule->conl_cost_fun_jac_hess+stage-1, p);
         {%- elif cost.cost_type == "EXTERNAL" %}
             capsule->ext_cost_fun[stage-1].set_param(capsule->ext_cost_fun+stage-1, p);
             capsule->ext_cost_fun_jac[stage-1].set_param(capsule->ext_cost_fun_jac+stage-1, p);
@@ -2229,6 +2302,9 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsu
         capsule->cost_y_e_fun.set_param(&capsule->cost_y_e_fun, p);
         capsule->cost_y_e_fun_jac_ut_xt.set_param(&capsule->cost_y_e_fun_jac_ut_xt, p);
         capsule->cost_y_e_hess.set_param(&capsule->cost_y_e_hess, p);
+    {%- elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %}
+        capsule->conl_cost_e_fun.set_param(&capsule->conl_cost_e_fun, p);
+        capsule->conl_cost_e_fun_jac_hess.set_param(&capsule->conl_cost_e_fun_jac_hess, p);
     {%- elif cost.cost_type_e == "EXTERNAL" %}
         capsule->ext_cost_e_fun.set_param(&capsule->ext_cost_e_fun, p);
         capsule->ext_cost_e_fun_jac.set_param(&capsule->ext_cost_e_fun_jac, p);
@@ -2245,16 +2321,149 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsu
     {%- endif %}
     {% endif %}
     }
-{% endif %}{# if dims.np #}
 
     return solver_status;
 }
 
 
+int {{ model.name }}_acados_update_params_sparse({{ model.name }}_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
+{
+    int solver_status = 0;
+
+    int casadi_np = {{ dims.np }};
+    if (casadi_np < n_update) {
+        printf("{{ model.name }}_acados_update_params_sparse: trying to set %d parameters for external functions."
+            " External function has %d parameters. Exiting.\n", n_update, casadi_np);
+        exit(1);
+    }
+    // for (int i = 0; i < n_update; i++)
+    // {
+    //     if (idx[i] > casadi_np) {
+    //         printf("{{ model.name }}_acados_update_params_sparse: attempt to set parameters with index %d, while"
+    //             " external functions only has %d parameters. Exiting.\n", idx[i], casadi_np);
+    //         exit(1);
+    //     }
+    //     printf("param %d value %e\n", idx[i], p[i]);
+    // }
+
+{%- if dims.np > 0 %}
+    const int N = capsule->nlp_solver_plan->N;
+    if (stage < N && stage >= 0)
+    {
+    {%- if solver_options.integrator_type == "IRK" %}
+        capsule->impl_dae_fun[stage].set_param_sparse(capsule->impl_dae_fun+stage, n_update, idx, p);
+        capsule->impl_dae_fun_jac_x_xdot_z[stage].set_param_sparse(capsule->impl_dae_fun_jac_x_xdot_z+stage, n_update, idx, p);
+        capsule->impl_dae_jac_x_xdot_u_z[stage].set_param_sparse(capsule->impl_dae_jac_x_xdot_u_z+stage, n_update, idx, p);
+
+        {%- if solver_options.hessian_approx == "EXACT" %}
+        capsule->impl_dae_hess[stage].set_param_sparse(capsule->impl_dae_hess+stage, n_update, idx, p);
+        {%- endif %}
+    {% elif solver_options.integrator_type == "LIFTED_IRK" %}
+        capsule->impl_dae_fun[stage].set_param_sparse(capsule->impl_dae_fun+stage, n_update, idx, p);
+        capsule->impl_dae_fun_jac_x_xdot_u[stage].set_param_sparse(capsule->impl_dae_fun_jac_x_xdot_u+stage, n_update, idx, p);
+    {% elif solver_options.integrator_type == "ERK" %}
+        capsule->forw_vde_casadi[stage].set_param_sparse(capsule->forw_vde_casadi+stage, n_update, idx, p);
+        capsule->expl_ode_fun[stage].set_param_sparse(capsule->expl_ode_fun+stage, n_update, idx, p);
+
+        {%- if solver_options.hessian_approx == "EXACT" %}
+        capsule->hess_vde_casadi[stage].set_param_sparse(capsule->hess_vde_casadi+stage, n_update, idx, p);
+        {%- endif %}
+    {% elif solver_options.integrator_type == "GNSF" %}
+    {% if model.gnsf.purely_linear != 1 %}
+        capsule->gnsf_phi_fun[stage].set_param_sparse(capsule->gnsf_phi_fun+stage, n_update, idx, p);
+        capsule->gnsf_phi_fun_jac_y[stage].set_param_sparse(capsule->gnsf_phi_fun_jac_y+stage, n_update, idx, p);
+        capsule->gnsf_phi_jac_y_uhat[stage].set_param_sparse(capsule->gnsf_phi_jac_y_uhat+stage, n_update, idx, p);
+        {% if model.gnsf.nontrivial_f_LO == 1 %}
+            capsule->gnsf_f_lo_jac_x1_x1dot_u_z[stage].set_param_sparse(capsule->gnsf_f_lo_jac_x1_x1dot_u_z+stage, n_update, idx, p);
+        {%- endif %}
+    {%- endif %}
+    {% elif solver_options.integrator_type == "DISCRETE" %}
+        capsule->discr_dyn_phi_fun[stage].set_param_sparse(capsule->discr_dyn_phi_fun+stage, n_update, idx, p);
+        capsule->discr_dyn_phi_fun_jac_ut_xt[stage].set_param_sparse(capsule->discr_dyn_phi_fun_jac_ut_xt+stage, n_update, idx, p);
+    {%- if solver_options.hessian_approx == "EXACT" %}
+        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[stage].set_param_sparse(capsule->discr_dyn_phi_fun_jac_ut_xt_hess+stage, n_update, idx, p);
+    {% endif %}
+    {%- endif %}{# integrator_type #}
+
+        // constraints
+    {% if constraints.constr_type == "BGP" %}
+        capsule->phi_constraint[stage].set_param_sparse(capsule->phi_constraint+stage, n_update, idx, p);
+    {% elif constraints.constr_type == "BGH" and dims.nh > 0 %}
+        capsule->nl_constr_h_fun_jac[stage].set_param_sparse(capsule->nl_constr_h_fun_jac+stage, n_update, idx, p);
+        capsule->nl_constr_h_fun[stage].set_param_sparse(capsule->nl_constr_h_fun+stage, n_update, idx, p);
+    {%- if solver_options.hessian_approx == "EXACT" %}
+        capsule->nl_constr_h_fun_jac_hess[stage].set_param_sparse(capsule->nl_constr_h_fun_jac_hess+stage, n_update, idx, p);
+    {%- endif %}
+    {%- endif %}
+
+        // cost
+        if (stage == 0)
+        {
+        {%- if cost.cost_type_0 == "NONLINEAR_LS" %}
+            capsule->cost_y_0_fun.set_param_sparse(&capsule->cost_y_0_fun, n_update, idx, p);
+            capsule->cost_y_0_fun_jac_ut_xt.set_param_sparse(&capsule->cost_y_0_fun_jac_ut_xt, n_update, idx, p);
+            capsule->cost_y_0_hess.set_param_sparse(&capsule->cost_y_0_hess, n_update, idx, p);
+        {%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %}
+            capsule->conl_cost_0_fun.set_param_sparse(&capsule->conl_cost_0_fun, n_update, idx, p);
+            capsule->conl_cost_0_fun_jac_hess.set_param_sparse(&capsule->conl_cost_0_fun_jac_hess, n_update, idx, p);
+        {%- elif cost.cost_type_0 == "EXTERNAL" %}
+            capsule->ext_cost_0_fun.set_param_sparse(&capsule->ext_cost_0_fun, n_update, idx, p);
+            capsule->ext_cost_0_fun_jac.set_param_sparse(&capsule->ext_cost_0_fun_jac, n_update, idx, p);
+            capsule->ext_cost_0_fun_jac_hess.set_param_sparse(&capsule->ext_cost_0_fun_jac_hess, n_update, idx, p);
+        {% endif %}
+        }
+        else // 0 < stage < N
+        {
+        {%- if cost.cost_type == "NONLINEAR_LS" %}
+            capsule->cost_y_fun[stage-1].set_param_sparse(capsule->cost_y_fun+stage-1, n_update, idx, p);
+            capsule->cost_y_fun_jac_ut_xt[stage-1].set_param_sparse(capsule->cost_y_fun_jac_ut_xt+stage-1, n_update, idx, p);
+            capsule->cost_y_hess[stage-1].set_param_sparse(capsule->cost_y_hess+stage-1, n_update, idx, p);
+        {%- elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %}
+            capsule->conl_cost_fun[stage-1].set_param_sparse(capsule->conl_cost_fun+stage-1, n_update, idx, p);
+            capsule->conl_cost_fun_jac_hess[stage-1].set_param_sparse(capsule->conl_cost_fun_jac_hess+stage-1, n_update, idx, p);
+        {%- elif cost.cost_type == "EXTERNAL" %}
+            capsule->ext_cost_fun[stage-1].set_param_sparse(capsule->ext_cost_fun+stage-1, n_update, idx, p);
+            capsule->ext_cost_fun_jac[stage-1].set_param_sparse(capsule->ext_cost_fun_jac+stage-1, n_update, idx, p);
+            capsule->ext_cost_fun_jac_hess[stage-1].set_param_sparse(capsule->ext_cost_fun_jac_hess+stage-1, n_update, idx, p);
+        {%- endif %}
+        }
+    }
+
+    else // stage == N
+    {
+        // terminal shooting node has no dynamics
+        // cost
+    {%- if cost.cost_type_e == "NONLINEAR_LS" %}
+        capsule->cost_y_e_fun.set_param_sparse(&capsule->cost_y_e_fun, n_update, idx, p);
+        capsule->cost_y_e_fun_jac_ut_xt.set_param_sparse(&capsule->cost_y_e_fun_jac_ut_xt, n_update, idx, p);
+        capsule->cost_y_e_hess.set_param_sparse(&capsule->cost_y_e_hess, n_update, idx, p);
+    {%- elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %}
+        capsule->conl_cost_e_fun.set_param_sparse(&capsule->conl_cost_e_fun, n_update, idx, p);
+        capsule->conl_cost_e_fun_jac_hess.set_param_sparse(&capsule->conl_cost_e_fun_jac_hess, n_update, idx, p);
+    {%- elif cost.cost_type_e == "EXTERNAL" %}
+        capsule->ext_cost_e_fun.set_param_sparse(&capsule->ext_cost_e_fun, n_update, idx, p);
+        capsule->ext_cost_e_fun_jac.set_param_sparse(&capsule->ext_cost_e_fun_jac, n_update, idx, p);
+        capsule->ext_cost_e_fun_jac_hess.set_param_sparse(&capsule->ext_cost_e_fun_jac_hess, n_update, idx, p);
+    {% endif %}
+        // constraints
+    {% if constraints.constr_type_e == "BGP" %}
+        capsule->phi_e_constraint.set_param_sparse(&capsule->phi_e_constraint, n_update, idx, p);
+    {% elif constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
+        capsule->nl_constr_h_e_fun_jac.set_param_sparse(&capsule->nl_constr_h_e_fun_jac, n_update, idx, p);
+        capsule->nl_constr_h_e_fun.set_param_sparse(&capsule->nl_constr_h_e_fun, n_update, idx, p);
+    {%- if solver_options.hessian_approx == "EXACT" %}
+        capsule->nl_constr_h_e_fun_jac_hess.set_param_sparse(&capsule->nl_constr_h_e_fun_jac_hess, n_update, idx, p);
+    {%- endif %}
+    {% endif %}
+    }
+{% endif %}{# if dims.np #}
+
+    return solver_status;
+}
 
 int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule* capsule)
 {
-    // solve NLP 
+    // solve NLP
     int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out);
 
     return solver_status;
@@ -2265,6 +2474,9 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule)
 {
     // before destroying, keep some info
     const int N = capsule->nlp_solver_plan->N;
+    {%- if custom_update_filename != "" %}
+    custom_update_terminate_function(capsule);
+    {%- endif %}
     // free memory
     ocp_nlp_solver_opts_destroy(capsule->nlp_opts);
     ocp_nlp_in_destroy(capsule->nlp_in);
@@ -2280,11 +2492,11 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule)
 {%- if solver_options.integrator_type == "IRK" %}
     for (int i = 0; i < N; i++)
     {
-        external_function_param_casadi_free(&capsule->impl_dae_fun[i]);
-        external_function_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_z[i]);
-        external_function_param_casadi_free(&capsule->impl_dae_jac_x_xdot_u_z[i]);
+        external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_fun[i]);
+        external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_fun_jac_x_xdot_z[i]);
+        external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_jac_x_xdot_u_z[i]);
     {%- if solver_options.hessian_approx == "EXACT" %}
-        external_function_param_casadi_free(&capsule->impl_dae_hess[i]);
+        external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_hess[i]);
     {%- endif %}
     }
     free(capsule->impl_dae_fun);
@@ -2297,8 +2509,8 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule)
 {%- elif solver_options.integrator_type == "LIFTED_IRK" %}
     for (int i = 0; i < N; i++)
     {
-        external_function_param_casadi_free(&capsule->impl_dae_fun[i]);
-        external_function_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_u[i]);
+        external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_fun[i]);
+        external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_fun_jac_x_xdot_u[i]);
     }
     free(capsule->impl_dae_fun);
     free(capsule->impl_dae_fun_jac_x_xdot_u);
@@ -2354,7 +2566,7 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule)
     {%- if solver_options.hessian_approx == "EXACT" %}
     free(capsule->discr_dyn_phi_fun_jac_ut_xt_hess);
     {%- endif %}
-    
+
 {%- endif %}
 
     // cost
@@ -2362,6 +2574,9 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule)
     external_function_param_casadi_free(&capsule->cost_y_0_fun);
     external_function_param_casadi_free(&capsule->cost_y_0_fun_jac_ut_xt);
     external_function_param_casadi_free(&capsule->cost_y_0_hess);
+{%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %}
+    external_function_param_casadi_free(&capsule->conl_cost_0_fun);
+    external_function_param_casadi_free(&capsule->conl_cost_0_fun_jac_hess);
 {%- elif cost.cost_type_0 == "EXTERNAL" %}
     external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun);
     external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun_jac);
@@ -2377,6 +2592,14 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule)
     free(capsule->cost_y_fun);
     free(capsule->cost_y_fun_jac_ut_xt);
     free(capsule->cost_y_hess);
+{%- elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %}
+    for (int i = 0; i < N - 1; i++)
+    {
+        external_function_param_casadi_free(&capsule->conl_cost_fun[i]);
+        external_function_param_casadi_free(&capsule->conl_cost_fun_jac_hess[i]);
+    }
+    free(capsule->conl_cost_fun);
+    free(capsule->conl_cost_fun_jac_hess);
 {%- elif cost.cost_type == "EXTERNAL" %}
     for (int i = 0; i < N - 1; i++)
     {
@@ -2392,6 +2615,9 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule)
     external_function_param_casadi_free(&capsule->cost_y_e_fun);
     external_function_param_casadi_free(&capsule->cost_y_e_fun_jac_ut_xt);
     external_function_param_casadi_free(&capsule->cost_y_e_hess);
+{%- elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %}
+    external_function_param_casadi_free(&capsule->conl_cost_e_fun);
+    external_function_param_casadi_free(&capsule->conl_cost_e_fun_jac_hess);
 {%- elif cost.cost_type_e == "EXTERNAL" %}
     external_function_param_{{ cost.cost_ext_fun_type_e }}_free(&capsule->ext_cost_e_fun);
     external_function_param_{{ cost.cost_ext_fun_type_e }}_free(&capsule->ext_cost_e_fun_jac);
@@ -2438,15 +2664,6 @@ 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_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)
 {
@@ -2461,8 +2678,13 @@ void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule* capsul
 
     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\talpha");
+    if (stat_n > 8)
+        printf("\t\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp");
+    printf("\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++)
@@ -2493,3 +2715,27 @@ void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule* capsul
 {%- endif %}
 }
 
+int {{ model.name }}_acados_custom_update({{ model.name }}_solver_capsule* capsule, double* data, int data_len)
+{
+{%- if custom_update_filename == "" %}
+    (void)capsule;
+    (void)data;
+    (void)data_len;
+    printf("\ndummy function that can be called in between solver calls to update parameters or numerical data efficiently in C.\n");
+    printf("nothing set yet..\n");
+    return 1;
+{% else %}
+    custom_update_function(capsule, data, data_len);
+{%- endif %}
+}
+
+
+
+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; }
diff --git a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.h b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.h
index 1c82ef3ba0..5cf38aa8c8 100644
--- a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.h
+++ b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -74,6 +71,12 @@
 extern "C" {
 #endif
 
+{%- if not solver_options.custom_update_filename %}
+    {%- set custom_update_filename = "" %}
+{% else %}
+    {%- set custom_update_filename = solver_options.custom_update_filename %}
+{%- endif %}
+
 // ** capsule for solver data **
 typedef struct {{ model.name }}_solver_capsule
 {
@@ -99,15 +102,15 @@ typedef struct {{ model.name }}_solver_capsule
     external_function_param_casadi *hess_vde_casadi;
 {%- endif %}
 {% elif solver_options.integrator_type == "IRK" %}
-    external_function_param_casadi *impl_dae_fun;
-    external_function_param_casadi *impl_dae_fun_jac_x_xdot_z;
-    external_function_param_casadi *impl_dae_jac_x_xdot_u_z;
+    external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun;
+    external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun_jac_x_xdot_z;
+    external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_jac_x_xdot_u_z;
 {% if solver_options.hessian_approx == "EXACT" %}
-    external_function_param_casadi *impl_dae_hess;
+    external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_hess;
 {%- endif %}
 {% elif solver_options.integrator_type == "LIFTED_IRK" %}
-    external_function_param_casadi *impl_dae_fun;
-    external_function_param_casadi *impl_dae_fun_jac_x_xdot_u;
+    external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun;
+    external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun_jac_x_xdot_u;
 {% elif solver_options.integrator_type == "GNSF" %}
     external_function_param_casadi *gnsf_phi_fun;
     external_function_param_casadi *gnsf_phi_fun_jac_y;
@@ -128,6 +131,9 @@ typedef struct {{ model.name }}_solver_capsule
     external_function_param_casadi *cost_y_fun;
     external_function_param_casadi *cost_y_fun_jac_ut_xt;
     external_function_param_casadi *cost_y_hess;
+{% elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %}
+    external_function_param_casadi *conl_cost_fun;
+    external_function_param_casadi *conl_cost_fun_jac_hess;
 {%- elif cost.cost_type == "EXTERNAL" %}
     external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun;
     external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun_jac;
@@ -138,6 +144,9 @@ typedef struct {{ model.name }}_solver_capsule
     external_function_param_casadi cost_y_0_fun;
     external_function_param_casadi cost_y_0_fun_jac_ut_xt;
     external_function_param_casadi cost_y_0_hess;
+{% elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %}
+    external_function_param_casadi conl_cost_0_fun;
+    external_function_param_casadi conl_cost_0_fun_jac_hess;
 {% elif cost.cost_type_0 == "EXTERNAL" %}
     external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun;
     external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun_jac;
@@ -148,6 +157,9 @@ typedef struct {{ model.name }}_solver_capsule
     external_function_param_casadi cost_y_e_fun;
     external_function_param_casadi cost_y_e_fun_jac_ut_xt;
     external_function_param_casadi cost_y_e_hess;
+{% elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %}
+    external_function_param_casadi conl_cost_e_fun;
+    external_function_param_casadi conl_cost_e_fun_jac_hess;
 {% elif cost.cost_type_e == "EXTERNAL" %}
     external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun;
     external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun_jac;
@@ -160,8 +172,10 @@ typedef struct {{ model.name }}_solver_capsule
 {% elif constraints.constr_type == "BGH" and dims.nh > 0 %}
     external_function_param_casadi *nl_constr_h_fun_jac;
     external_function_param_casadi *nl_constr_h_fun;
+{%- if solver_options.hessian_approx == "EXACT" %}
     external_function_param_casadi *nl_constr_h_fun_jac_hess;
 {%- endif %}
+{%- endif %}
 
 
 {% if constraints.constr_type_e == "BGP" %}
@@ -169,8 +183,14 @@ typedef struct {{ model.name }}_solver_capsule
 {% elif constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
     external_function_param_casadi nl_constr_h_e_fun_jac;
     external_function_param_casadi nl_constr_h_e_fun;
+{%- if solver_options.hessian_approx == "EXACT" %}
     external_function_param_casadi nl_constr_h_e_fun_jac_hess;
 {%- endif %}
+{%- endif %}
+
+{%- if custom_update_filename != "" %}
+    void * custom_update_memory;
+{%- endif %}
 
 } {{ model.name }}_solver_capsule;
 
@@ -179,7 +199,7 @@ ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_free_capsule({{ model.name }}_s
 
 ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_create({{ model.name }}_solver_capsule * capsule);
 
-ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule);
+ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule, int reset_qp_solver_mem);
 
 /**
  * Generic version of {{ model.name }}_acados_create which allows to use a different number of shooting intervals than
@@ -197,10 +217,14 @@ ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_time_steps({{ model.name
  */
 ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_capsule * capsule, int qp_solver_cond_N);
 ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * capsule, int stage, double *value, int np);
+ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_params_sparse({{ model.name }}_solver_capsule * capsule, int stage, int *idx, double *p, int n_update);
+
 ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule);
 ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule);
 ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsule);
-                     
+ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_custom_update({{ model.name }}_solver_capsule* capsule, double* data, int data_len);
+
+
 ACADOS_SYMBOL_EXPORT ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule * capsule);
 ACADOS_SYMBOL_EXPORT ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule * capsule);
 ACADOS_SYMBOL_EXPORT ocp_nlp_out *{{ model.name }}_acados_get_sens_out({{ model.name }}_solver_capsule * capsule);
diff --git a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd
index 09f8755cbe..233e3f79da 100644
--- a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd
+++ b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd
@@ -1,8 +1,5 @@
 #
-# 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
+# Copyright (c) The acados authors.
 #
 # This file is part of acados.
 #
@@ -47,11 +44,14 @@ cdef extern from "acados_solver_{{ model.name }}.h":
     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_update_params_sparse "{{ model.name }}_acados_update_params_sparse"(nlp_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
     int acados_solve "{{ model.name }}_acados_solve"(nlp_solver_capsule * capsule)
-    int acados_reset "{{ model.name }}_acados_reset"(nlp_solver_capsule * capsule)
+    int acados_reset "{{ model.name }}_acados_reset"(nlp_solver_capsule * capsule, int reset_qp_solver_mem)
     int acados_free "{{ model.name }}_acados_free"(nlp_solver_capsule * capsule)
     void acados_print_stats "{{ model.name }}_acados_print_stats"(nlp_solver_capsule * capsule)
 
+    int acados_custom_update "{{ model.name }}_acados_custom_update"(nlp_solver_capsule* capsule, double * data, int data_len)
+
     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)
diff --git a/third_party/acados/acados_template/c_templates_tera/h_e_constraint.in.h b/third_party/acados/acados_template/c_templates_tera/constraints.in.h
similarity index 54%
rename from third_party/acados/acados_template/c_templates_tera/h_e_constraint.in.h
rename to third_party/acados/acados_template/c_templates_tera/constraints.in.h
index a5dd711641..d71ce5cc22 100644
--- a/third_party/acados/acados_template/c_templates_tera/h_e_constraint.in.h
+++ b/third_party/acados/acados_template/c_templates_tera/constraints.in.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -31,14 +28,56 @@
  * POSSIBILITY OF SUCH DAMAGE.;
  */
 
-
-#ifndef {{ model.name }}_H_E_CONSTRAINT
-#define {{ model.name }}_H_E_CONSTRAINT
+#ifndef {{ model.name }}_CONSTRAINTS
+#define {{ model.name }}_CONSTRAINTS
 
 #ifdef __cplusplus
 extern "C" {
 #endif
 
+{% if dims.nphi > 0 %}
+int {{ model.name }}_phi_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_phi_constraint_work(int *, int *, int *, int *);
+const int *{{ model.name }}_phi_constraint_sparsity_in(int);
+const int *{{ model.name }}_phi_constraint_sparsity_out(int);
+int {{ model.name }}_phi_constraint_n_in(void);
+int {{ model.name }}_phi_constraint_n_out(void);
+{% endif %}
+
+{% if dims.nphi_e > 0 %}
+int {{ model.name }}_phi_e_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_phi_e_constraint_work(int *, int *, int *, int *);
+const int *{{ model.name }}_phi_e_constraint_sparsity_in(int);
+const int *{{ model.name }}_phi_e_constraint_sparsity_out(int);
+int {{ model.name }}_phi_e_constraint_n_in(void);
+int {{ model.name }}_phi_e_constraint_n_out(void);
+{% endif %}
+
+{% if dims.nh > 0 %}
+int {{ model.name }}_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *);
+const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_in(int);
+const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_out(int);
+int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_in(void);
+int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_out(void);
+
+int {{ model.name }}_constr_h_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_constr_h_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_constr_h_fun_sparsity_in(int);
+const int *{{ model.name }}_constr_h_fun_sparsity_out(int);
+int {{ model.name }}_constr_h_fun_n_in(void);
+int {{ model.name }}_constr_h_fun_n_out(void);
+
+{% if solver_options.hessian_approx == "EXACT" -%}
+int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_in(int);
+const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_out(int);
+int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_in(void);
+int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_out(void);
+{% endif %}
+{% endif %}
+
 {% if dims.nh_e > 0 %}
 int {{ model.name }}_constr_h_e_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
 int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_work(int *, int *, int *, int *);
@@ -68,4 +107,4 @@ int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_out(void);
 } /* extern "C" */
 #endif
 
-#endif  // {{ model.name }}_H_E_CONSTRAINT
+#endif  // {{ model.name }}_CONSTRAINTS
diff --git a/third_party/acados/acados_template/c_templates_tera/cost.in.h b/third_party/acados/acados_template/c_templates_tera/cost.in.h
new file mode 100644
index 0000000000..45eb09c12e
--- /dev/null
+++ b/third_party/acados/acados_template/c_templates_tera/cost.in.h
@@ -0,0 +1,238 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * 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.;
+ */
+
+
+#ifndef {{ model.name }}_COST
+#define {{ model.name }}_COST
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// Cost at initial shooting node
+{% if cost.cost_type_0 == "NONLINEAR_LS" %}
+int {{ model.name }}_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_y_0_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_y_0_fun_sparsity_in(int);
+const int *{{ model.name }}_cost_y_0_fun_sparsity_out(int);
+int {{ model.name }}_cost_y_0_fun_n_in(void);
+int {{ model.name }}_cost_y_0_fun_n_out(void);
+
+int {{ model.name }}_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_in(int);
+const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_out(int);
+int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in(void);
+int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out(void);
+
+int {{ model.name }}_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_y_0_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_y_0_hess_sparsity_in(int);
+const int *{{ model.name }}_cost_y_0_hess_sparsity_out(int);
+int {{ model.name }}_cost_y_0_hess_n_in(void);
+int {{ model.name }}_cost_y_0_hess_n_out(void);
+{% elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %}
+
+int {{ model.name }}_conl_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_conl_cost_0_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_conl_cost_0_fun_sparsity_in(int);
+const int *{{ model.name }}_conl_cost_0_fun_sparsity_out(int);
+int {{ model.name }}_conl_cost_0_fun_n_in(void);
+int {{ model.name }}_conl_cost_0_fun_n_out(void);
+
+int {{ model.name }}_conl_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_conl_cost_0_fun_jac_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_conl_cost_0_fun_jac_hess_sparsity_in(int);
+const int *{{ model.name }}_conl_cost_0_fun_jac_hess_sparsity_out(int);
+int {{ model.name }}_conl_cost_0_fun_jac_hess_n_in(void);
+int {{ model.name }}_conl_cost_0_fun_jac_hess_n_out(void);
+
+{% elif cost.cost_type_0 == "EXTERNAL" %}
+    {%- if cost.cost_ext_fun_type_0 == "casadi" %}
+int {{ model.name }}_cost_ext_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_ext_cost_0_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_in(int);
+const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_out(int);
+int {{ model.name }}_cost_ext_cost_0_fun_n_in(void);
+int {{ model.name }}_cost_ext_cost_0_fun_n_out(void);
+
+int {{ model.name }}_cost_ext_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_in(int);
+const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_out(int);
+int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in(void);
+int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out(void);
+
+int {{ model.name }}_cost_ext_cost_0_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_ext_cost_0_fun_jac_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_in(int);
+const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_out(int);
+int {{ model.name }}_cost_ext_cost_0_fun_jac_n_in(void);
+int {{ model.name }}_cost_ext_cost_0_fun_jac_n_out(void);
+    {%- else %}
+int {{ cost.cost_function_ext_cost_0 }}(void **, void **, void *);
+    {%- endif %}
+{% endif %}
+
+
+// Cost at path shooting node
+{% if cost.cost_type == "NONLINEAR_LS" %}
+int {{ model.name }}_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_y_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_y_fun_sparsity_in(int);
+const int *{{ model.name }}_cost_y_fun_sparsity_out(int);
+int {{ model.name }}_cost_y_fun_n_in(void);
+int {{ model.name }}_cost_y_fun_n_out(void);
+
+int {{ model.name }}_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_in(int);
+const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_out(int);
+int {{ model.name }}_cost_y_fun_jac_ut_xt_n_in(void);
+int {{ model.name }}_cost_y_fun_jac_ut_xt_n_out(void);
+
+int {{ model.name }}_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_y_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_y_hess_sparsity_in(int);
+const int *{{ model.name }}_cost_y_hess_sparsity_out(int);
+int {{ model.name }}_cost_y_hess_n_in(void);
+int {{ model.name }}_cost_y_hess_n_out(void);
+
+{% elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %}
+int {{ model.name }}_conl_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_conl_cost_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_conl_cost_fun_sparsity_in(int);
+const int *{{ model.name }}_conl_cost_fun_sparsity_out(int);
+int {{ model.name }}_conl_cost_fun_n_in(void);
+int {{ model.name }}_conl_cost_fun_n_out(void);
+
+int {{ model.name }}_conl_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_conl_cost_fun_jac_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_conl_cost_fun_jac_hess_sparsity_in(int);
+const int *{{ model.name }}_conl_cost_fun_jac_hess_sparsity_out(int);
+int {{ model.name }}_conl_cost_fun_jac_hess_n_in(void);
+int {{ model.name }}_conl_cost_fun_jac_hess_n_out(void);
+{% elif cost.cost_type == "EXTERNAL" %}
+    {%- if cost.cost_ext_fun_type == "casadi" %}
+int {{ model.name }}_cost_ext_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_ext_cost_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_ext_cost_fun_sparsity_in(int);
+const int *{{ model.name }}_cost_ext_cost_fun_sparsity_out(int);
+int {{ model.name }}_cost_ext_cost_fun_n_in(void);
+int {{ model.name }}_cost_ext_cost_fun_n_out(void);
+
+int {{ model.name }}_cost_ext_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_ext_cost_fun_jac_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_in(int);
+const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_out(int);
+int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_in(void);
+int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_out(void);
+
+int {{ model.name }}_cost_ext_cost_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_ext_cost_fun_jac_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_in(int);
+const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_out(int);
+int {{ model.name }}_cost_ext_cost_fun_jac_n_in(void);
+int {{ model.name }}_cost_ext_cost_fun_jac_n_out(void);
+    {%- else %}
+int {{ cost.cost_function_ext_cost }}(void **, void **, void *);
+    {%- endif %}
+{% endif %}
+
+// Cost at terminal shooting node
+{% if cost.cost_type_e == "NONLINEAR_LS" %}
+int {{ model.name }}_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_y_e_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_y_e_fun_sparsity_in(int);
+const int *{{ model.name }}_cost_y_e_fun_sparsity_out(int);
+int {{ model.name }}_cost_y_e_fun_n_in(void);
+int {{ model.name }}_cost_y_e_fun_n_out(void);
+
+int {{ model.name }}_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_in(int);
+const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_out(int);
+int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in(void);
+int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out(void);
+
+int {{ model.name }}_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_y_e_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_y_e_hess_sparsity_in(int);
+const int *{{ model.name }}_cost_y_e_hess_sparsity_out(int);
+int {{ model.name }}_cost_y_e_hess_n_in(void);
+int {{ model.name }}_cost_y_e_hess_n_out(void);
+{% elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %}
+int {{ model.name }}_conl_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_conl_cost_e_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_conl_cost_e_fun_sparsity_in(int);
+const int *{{ model.name }}_conl_cost_e_fun_sparsity_out(int);
+int {{ model.name }}_conl_cost_e_fun_n_in(void);
+int {{ model.name }}_conl_cost_e_fun_n_out(void);
+
+int {{ model.name }}_conl_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_conl_cost_e_fun_jac_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_conl_cost_e_fun_jac_hess_sparsity_in(int);
+const int *{{ model.name }}_conl_cost_e_fun_jac_hess_sparsity_out(int);
+int {{ model.name }}_conl_cost_e_fun_jac_hess_n_in(void);
+int {{ model.name }}_conl_cost_e_fun_jac_hess_n_out(void);
+{% elif cost.cost_type_e == "EXTERNAL" %}
+    {%- if cost.cost_ext_fun_type_e == "casadi" %}
+int {{ model.name }}_cost_ext_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_ext_cost_e_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_in(int);
+const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_out(int);
+int {{ model.name }}_cost_ext_cost_e_fun_n_in(void);
+int {{ model.name }}_cost_ext_cost_e_fun_n_out(void);
+
+int {{ model.name }}_cost_ext_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_in(int);
+const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_out(int);
+int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in(void);
+int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out(void);
+
+int {{ model.name }}_cost_ext_cost_e_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_ext_cost_e_fun_jac_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_in(int);
+const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_out(int);
+int {{ model.name }}_cost_ext_cost_e_fun_jac_n_in(void);
+int {{ model.name }}_cost_ext_cost_e_fun_jac_n_out(void);
+    {%- else %}
+int {{ cost.cost_function_ext_cost_e }}(void **, void **, void *);
+    {%- endif %}
+{% endif %}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // {{ model.name }}_COST
diff --git a/third_party/acados/acados_template/c_templates_tera/cost_y_0_fun.in.h b/third_party/acados/acados_template/c_templates_tera/cost_y_0_fun.in.h
deleted file mode 100644
index 347446e3f4..0000000000
--- a/third_party/acados/acados_template/c_templates_tera/cost_y_0_fun.in.h
+++ /dev/null
@@ -1,69 +0,0 @@
-/*
- * 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.;
- */
-
-
-#ifndef {{ model.name }}_Y_0_COST
-#define {{ model.name }}_Y_0_COST
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-{% if cost.cost_type_0 == "NONLINEAR_LS" %}
-int {{ model.name }}_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_cost_y_0_fun_work(int *, int *, int *, int *);
-const int *{{ model.name }}_cost_y_0_fun_sparsity_in(int);
-const int *{{ model.name }}_cost_y_0_fun_sparsity_out(int);
-int {{ model.name }}_cost_y_0_fun_n_in(void);
-int {{ model.name }}_cost_y_0_fun_n_out(void);
-
-int {{ model.name }}_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *);
-const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_in(int);
-const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_out(int);
-int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in(void);
-int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out(void);
-
-int {{ model.name }}_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_cost_y_0_hess_work(int *, int *, int *, int *);
-const int *{{ model.name }}_cost_y_0_hess_sparsity_in(int);
-const int *{{ model.name }}_cost_y_0_hess_sparsity_out(int);
-int {{ model.name }}_cost_y_0_hess_n_in(void);
-int {{ model.name }}_cost_y_0_hess_n_out(void);
-{% endif %}
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#endif  // {{ model.name }}_Y_0_COST
diff --git a/third_party/acados/acados_template/c_templates_tera/cost_y_e_fun.in.h b/third_party/acados/acados_template/c_templates_tera/cost_y_e_fun.in.h
deleted file mode 100644
index acc99009fe..0000000000
--- a/third_party/acados/acados_template/c_templates_tera/cost_y_e_fun.in.h
+++ /dev/null
@@ -1,69 +0,0 @@
-/*
- * 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.;
- */
-
-
-#ifndef {{ model.name }}_Y_E_COST
-#define {{ model.name }}_Y_E_COST
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-{% if cost.cost_type_e == "NONLINEAR_LS" %}
-int {{ model.name }}_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_cost_y_e_fun_work(int *, int *, int *, int *);
-const int *{{ model.name }}_cost_y_e_fun_sparsity_in(int);
-const int *{{ model.name }}_cost_y_e_fun_sparsity_out(int);
-int {{ model.name }}_cost_y_e_fun_n_in(void);
-int {{ model.name }}_cost_y_e_fun_n_out(void);
-
-int {{ model.name }}_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *);
-const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_in(int);
-const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_out(int);
-int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in(void);
-int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out(void);
-
-int {{ model.name }}_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_cost_y_e_hess_work(int *, int *, int *, int *);
-const int *{{ model.name }}_cost_y_e_hess_sparsity_in(int);
-const int *{{ model.name }}_cost_y_e_hess_sparsity_out(int);
-int {{ model.name }}_cost_y_e_hess_n_in(void);
-int {{ model.name }}_cost_y_e_hess_n_out(void);
-{% endif %}
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#endif  // {{ model.name }}_Y_E_COST
diff --git a/third_party/acados/acados_template/c_templates_tera/cost_y_fun.in.h b/third_party/acados/acados_template/c_templates_tera/cost_y_fun.in.h
deleted file mode 100644
index 1e03780cc1..0000000000
--- a/third_party/acados/acados_template/c_templates_tera/cost_y_fun.in.h
+++ /dev/null
@@ -1,69 +0,0 @@
-/*
- * 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.;
- */
-
-
-#ifndef {{ model.name }}_Y_COST
-#define {{ model.name }}_Y_COST
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-{% if cost.cost_type == "NONLINEAR_LS" %}
-int {{ model.name }}_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_cost_y_fun_work(int *, int *, int *, int *);
-const int *{{ model.name }}_cost_y_fun_sparsity_in(int);
-const int *{{ model.name }}_cost_y_fun_sparsity_out(int);
-int {{ model.name }}_cost_y_fun_n_in(void);
-int {{ model.name }}_cost_y_fun_n_out(void);
-
-int {{ model.name }}_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
-const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_in(int);
-const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_out(int);
-int {{ model.name }}_cost_y_fun_jac_ut_xt_n_in(void);
-int {{ model.name }}_cost_y_fun_jac_ut_xt_n_out(void);
-
-int {{ model.name }}_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_cost_y_hess_work(int *, int *, int *, int *);
-const int *{{ model.name }}_cost_y_hess_sparsity_in(int);
-const int *{{ model.name }}_cost_y_hess_sparsity_out(int);
-int {{ model.name }}_cost_y_hess_n_in(void);
-int {{ model.name }}_cost_y_hess_n_out(void);
-{% endif %}
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#endif  // {{ model.name }}_Y_COST
diff --git a/third_party/acados/acados_template/c_templates_tera/external_cost.in.h b/third_party/acados/acados_template/c_templates_tera/external_cost.in.h
deleted file mode 100644
index d200dba45e..0000000000
--- a/third_party/acados/acados_template/c_templates_tera/external_cost.in.h
+++ /dev/null
@@ -1,74 +0,0 @@
-/*
- * 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.;
- */
-
-
-#ifndef {{ model.name }}_EXT_COST
-#define {{ model.name }}_EXT_COST
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-{% if cost.cost_ext_fun_type == "casadi" %}
-{% if cost.cost_type == "EXTERNAL" %}
-int {{ model.name }}_cost_ext_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_cost_ext_cost_fun_work(int *, int *, int *, int *);
-const int *{{ model.name }}_cost_ext_cost_fun_sparsity_in(int);
-const int *{{ model.name }}_cost_ext_cost_fun_sparsity_out(int);
-int {{ model.name }}_cost_ext_cost_fun_n_in(void);
-int {{ model.name }}_cost_ext_cost_fun_n_out(void);
-
-int {{ model.name }}_cost_ext_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_cost_ext_cost_fun_jac_hess_work(int *, int *, int *, int *);
-const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_in(int);
-const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_out(int);
-int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_in(void);
-int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_out(void);
-
-int {{ model.name }}_cost_ext_cost_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_cost_ext_cost_fun_jac_work(int *, int *, int *, int *);
-const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_in(int);
-const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_out(int);
-int {{ model.name }}_cost_ext_cost_fun_jac_n_in(void);
-int {{ model.name }}_cost_ext_cost_fun_jac_n_out(void);
-{% endif %}
-
-{% else %}
-int {{ cost.cost_function_ext_cost }}(void **, void **, void *);
-{% endif %}
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#endif  // {{ model.name }}_EXT_COST
diff --git a/third_party/acados/acados_template/c_templates_tera/external_cost_0.in.h b/third_party/acados/acados_template/c_templates_tera/external_cost_0.in.h
deleted file mode 100644
index 8152447e5f..0000000000
--- a/third_party/acados/acados_template/c_templates_tera/external_cost_0.in.h
+++ /dev/null
@@ -1,75 +0,0 @@
-/*
- * 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.;
- */
-
-
-#ifndef {{ model.name }}_EXT_COST_0
-#define {{ model.name }}_EXT_COST_0
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-{% if cost.cost_ext_fun_type_0 == "casadi" %}
-
-{% if cost.cost_type_0 == "EXTERNAL" %}
-int {{ model.name }}_cost_ext_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_cost_ext_cost_0_fun_work(int *, int *, int *, int *);
-const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_in(int);
-const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_out(int);
-int {{ model.name }}_cost_ext_cost_0_fun_n_in(void);
-int {{ model.name }}_cost_ext_cost_0_fun_n_out(void);
-
-int {{ model.name }}_cost_ext_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_work(int *, int *, int *, int *);
-const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_in(int);
-const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_out(int);
-int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in(void);
-int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out(void);
-
-int {{ model.name }}_cost_ext_cost_0_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_cost_ext_cost_0_fun_jac_work(int *, int *, int *, int *);
-const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_in(int);
-const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_out(int);
-int {{ model.name }}_cost_ext_cost_0_fun_jac_n_in(void);
-int {{ model.name }}_cost_ext_cost_0_fun_jac_n_out(void);
-{% endif %}
-
-{% else %}
-int {{ cost.cost_function_ext_cost_0 }}(void **, void **, void *);
-{% endif %}
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#endif  // {{ model.name }}_EXT_COST_0
diff --git a/third_party/acados/acados_template/c_templates_tera/external_cost_e.in.h b/third_party/acados/acados_template/c_templates_tera/external_cost_e.in.h
deleted file mode 100644
index 56485db4c7..0000000000
--- a/third_party/acados/acados_template/c_templates_tera/external_cost_e.in.h
+++ /dev/null
@@ -1,74 +0,0 @@
-/*
- * 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.;
- */
-
-
-#ifndef {{ model.name }}_EXT_COST_E
-#define {{ model.name }}_EXT_COST_E
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-{% if cost.cost_ext_fun_type_e == "casadi" %}
-{% if cost.cost_type_e == "EXTERNAL" %}
-int {{ model.name }}_cost_ext_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_cost_ext_cost_e_fun_work(int *, int *, int *, int *);
-const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_in(int);
-const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_out(int);
-int {{ model.name }}_cost_ext_cost_e_fun_n_in(void);
-int {{ model.name }}_cost_ext_cost_e_fun_n_out(void);
-
-int {{ model.name }}_cost_ext_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_work(int *, int *, int *, int *);
-const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_in(int);
-const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_out(int);
-int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in(void);
-int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out(void);
-
-int {{ model.name }}_cost_ext_cost_e_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_cost_ext_cost_e_fun_jac_work(int *, int *, int *, int *);
-const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_in(int);
-const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_out(int);
-int {{ model.name }}_cost_ext_cost_e_fun_jac_n_in(void);
-int {{ model.name }}_cost_ext_cost_e_fun_jac_n_out(void);
-{% endif %}
-
-{% else %}
-int {{ cost.cost_function_ext_cost_e }}(void **, void **, void *);
-{% endif %}
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#endif  // {{ model.name }}_EXT_COST_E
diff --git a/third_party/acados/acados_template/c_templates_tera/h_constraint.in.h b/third_party/acados/acados_template/c_templates_tera/h_constraint.in.h
deleted file mode 100644
index e49176c6ef..0000000000
--- a/third_party/acados/acados_template/c_templates_tera/h_constraint.in.h
+++ /dev/null
@@ -1,70 +0,0 @@
-/*
- * 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.;
- */
-
-#ifndef {{ model.name }}_H_CONSTRAINT
-#define {{ model.name }}_H_CONSTRAINT
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-{% if dims.nh > 0 %}
-int {{ model.name }}_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *);
-const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_in(int);
-const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_out(int);
-int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_in(void);
-int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_out(void);
-
-int {{ model.name }}_constr_h_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_constr_h_fun_work(int *, int *, int *, int *);
-const int *{{ model.name }}_constr_h_fun_sparsity_in(int);
-const int *{{ model.name }}_constr_h_fun_sparsity_out(int);
-int {{ model.name }}_constr_h_fun_n_in(void);
-int {{ model.name }}_constr_h_fun_n_out(void);
-
-{% if solver_options.hessian_approx == "EXACT" -%}
-int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *);
-const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_in(int);
-const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_out(int);
-int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_in(void);
-int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_out(void);
-{% endif %}
-{% endif %}
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#endif  // {{ model.name }}_H_CONSTRAINT
diff --git a/third_party/acados/acados_template/c_templates_tera/main.in.c b/third_party/acados/acados_template/c_templates_tera/main.in.c
index 99c4f13be1..92a8b33eac 100644
--- a/third_party/acados/acados_template/c_templates_tera/main.in.c
+++ b/third_party/acados/acados_template/c_templates_tera/main.in.c
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -31,6 +28,11 @@
  * POSSIBILITY OF SUCH DAMAGE.;
  */
 
+{%- if not solver_options.custom_update_filename %}
+    {%- set custom_update_filename = "" %}
+{% else %}
+    {%- set custom_update_filename = solver_options.custom_update_filename %}
+{%- endif %}
 
 // standard
 #include <stdio.h>
@@ -42,6 +44,9 @@
 #include "acados_c/external_function_interface.h"
 #include "acados_solver_{{ model.name }}.h"
 
+// blasfeo
+#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
+
 #define NX     {{ model.name | upper }}_NX
 #define NZ     {{ model.name | upper }}_NZ
 #define NU     {{ model.name | upper }}_NU
@@ -191,6 +196,11 @@ int main()
         printf("{{ model.name }}_acados_solve() failed with status %d.\n", status);
     }
 
+
+{%- if custom_update_filename != "" %}
+    {{ model.name }}_acados_custom_update(acados_ocp_capsule, xtraj, NX*(N+1));
+{%- endif %}
+
     // get solution
     ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf);
     ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter);
diff --git a/third_party/acados/acados_template/c_templates_tera/main_sim.in.c b/third_party/acados/acados_template/c_templates_tera/main_sim.in.c
index 743e81d593..8960aa0035 100644
--- a/third_party/acados/acados_template/c_templates_tera/main_sim.in.c
+++ b/third_party/acados/acados_template/c_templates_tera/main_sim.in.c
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/acados_template/c_templates_tera/acados_mex_create.in.c b/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_create.in.c
similarity index 98%
rename from third_party/acados/acados_template/c_templates_tera/acados_mex_create.in.c
rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_create.in.c
index e67a51567a..24ae94ac2c 100644
--- a/third_party/acados/acados_template/c_templates_tera/acados_mex_create.in.c
+++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_create.in.c
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/acados_template/c_templates_tera/acados_mex_free.in.c b/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_free.in.c
similarity index 88%
rename from third_party/acados/acados_template/c_templates_tera/acados_mex_free.in.c
rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_free.in.c
index 560adb0b98..bd457969b2 100644
--- a/third_party/acados/acados_template/c_templates_tera/acados_mex_free.in.c
+++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_free.in.c
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/acados_template/c_templates_tera/acados_mex_set.in.c b/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_set.in.c
similarity index 76%
rename from third_party/acados/acados_template/c_templates_tera/acados_mex_set.in.c
rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_set.in.c
index f8e1e5e445..78a308df49 100644
--- a/third_party/acados/acados_template/c_templates_tera/acados_mex_set.in.c
+++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_set.in.c
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -59,9 +56,6 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     /* RHS */
     int min_nrhs = 6;
 
-    char *ext_fun_type = mxArrayToString( prhs[0] );
-    char *ext_fun_type_e = mxArrayToString( prhs[1] );
-
     // C ocp
     const mxArray *C_ocp = prhs[2];
     // capsule
@@ -378,45 +372,63 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     //     }
     // }
     // initializations
-    else if (!strcmp(field, "init_x"))
+    else if (!strcmp(field, "init_x") || !strcmp(field, "x"))
     {
-        if (nrhs!=min_nrhs)
-            MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field)
-
-        acados_size = (N+1) * nx;
-        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
-        for (int ii=0; ii<=N; ii++)
+        if (nrhs == min_nrhs)
+        {
+            acados_size = (N+1) * nx;
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            for (int ii=0; ii<=N; ii++)
+            {
+                ocp_nlp_out_set(config, dims, out, ii, "x", value+ii*nx);
+            }
+        }
+        else // (nrhs == min_nrhs + 1)
         {
-            ocp_nlp_out_set(config, dims, out, ii, "x", value+ii*nx);
+            acados_size = nx;
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            ocp_nlp_out_set(config, dims, out, s0, "x", value);
         }
     }
-    else if (!strcmp(field, "init_u"))
+    else if (!strcmp(field, "init_u") || !strcmp(field, "u"))
     {
-        if (nrhs!=min_nrhs)
-            MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field)
-
-        acados_size = N*nu;
-        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
-        for (int ii=0; ii<N; ii++)
+        if (nrhs==min_nrhs)
         {
-            ocp_nlp_out_set(config, dims, out, ii, "u", value+ii*nu);
+            acados_size = N*nu;
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            for (int ii=0; ii<N; ii++)
+            {
+                ocp_nlp_out_set(config, dims, out, ii, "u", value+ii*nu);
+            }
+        }
+        else // (nrhs == min_nrhs + 1)
+        {
+            acados_size = nu;
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            ocp_nlp_out_set(config, dims, out, s0, "u", value);
         }
     }
-    else if (!strcmp(field, "init_z"))
+    else if (!strcmp(field, "init_z")||!strcmp(field, "z"))
     {
         sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
         sim_solver_t type = sim_plan.sim_solver;
         if (type == IRK)
         {
             int nz = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "z");
-            if (nrhs!=min_nrhs)
-                MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field)
-
-            acados_size = N*nz;
-            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
-            for (int ii=0; ii<N; ii++)
+            if (nrhs==min_nrhs)
+            {
+                acados_size = N*nz;
+                MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+                for (int ii=0; ii<N; ii++)
+                {
+                    ocp_nlp_set(config, solver, ii, "z_guess", value+ii*nz);
+                }
+            }
+            else // (nrhs==min_nrhs+1)
             {
-                ocp_nlp_set(config, solver, ii, "z_guess", value+ii*nz);
+                acados_size = nz;
+                MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+                ocp_nlp_set(config, solver, s0, "z_guess", value);
             }
         }
         else
@@ -424,21 +436,27 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
             MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_z", "irk")
         }
     }
-    else if (!strcmp(field, "init_xdot"))
+    else if (!strcmp(field, "init_xdot")||!strcmp(field, "xdot"))
     {
         sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
         sim_solver_t type = sim_plan.sim_solver;
         if (type == IRK)
         {
             int nx = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "x");
-            if (nrhs!=min_nrhs)
-                MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field)
-
-            acados_size = N*nx;
-            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
-            for (int ii=0; ii<N; ii++)
+            if (nrhs==min_nrhs)
             {
-                ocp_nlp_set(config, solver, ii, "xdot_guess", value+ii*nx);
+                acados_size = N*nx;
+                MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+                for (int ii=0; ii<N; ii++)
+                {
+                    ocp_nlp_set(config, solver, ii, "xdot_guess", value+ii*nx);
+                }
+            }
+            else // nrhs==min_nrhs+1)
+            {
+                acados_size = nx;
+                MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+                ocp_nlp_set(config, solver, s0, "xdot_guess", value);
             }
         }
         else
@@ -446,7 +464,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
             MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_z", "irk")
         }
     }
-    else if (!strcmp(field, "init_gnsf_phi"))
+    else if (!strcmp(field, "init_gnsf_phi")||!strcmp(field, "gnsf_phi"))
     {
         sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
         sim_solver_t type = sim_plan.sim_solver;
@@ -454,14 +472,20 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
         {
             int nout = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "init_gnsf_phi");
 
-            if (nrhs!=min_nrhs)
-                MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field)
-
-            acados_size = N*nout;
-            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
-            for (int ii=0; ii<N; ii++)
+            if (nrhs==min_nrhs)
             {
-                ocp_nlp_set(config, solver, ii, "gnsf_phi_guess", value+ii*nx);
+                acados_size = N*nout;
+                MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+                for (int ii=0; ii<N; ii++)
+                {
+                    ocp_nlp_set(config, solver, ii, "gnsf_phi_guess", value+ii*nx);
+                }
+            }
+            else // (nrhs==min_nrhs+1)
+            {
+                acados_size = nout;
+                MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+                ocp_nlp_set(config, solver, s0, "gnsf_phi_guess", value);
             }
         }
         else
@@ -469,36 +493,74 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
             MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_gnsf_phi", "irk_gnsf")
         }
     }
-    else if (!strcmp(field, "init_pi"))
+    else if (!strcmp(field, "init_pi")||!strcmp(field, "pi"))
     {
-        if (nrhs!=min_nrhs)
-            MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field)
-
-        acados_size = N*nx;
-        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
-        for (int ii=0; ii<N; ii++)
+        if (nrhs==min_nrhs)
         {
-            ocp_nlp_out_set(config, dims, out, ii, "pi", value+ii*nx);
+            acados_size = N*nx;
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            for (int ii=0; ii<N; ii++)
+            {
+                ocp_nlp_out_set(config, dims, out, ii, "pi", value+ii*nx);
+            }
+        }
+        else // (nrhs==min_nrhs+1)
+        {
+            acados_size = nx;
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            ocp_nlp_out_set(config, dims, out, s0, "pi", value);
         }
     }
-    else if (!strcmp(field, "init_lam"))
+    else if (!strcmp(field, "init_lam")||!strcmp(field, "lam"))
     {
-        for (int ii=s0; ii<se; ii++)
+        if (nrhs==min_nrhs)
         {
-            int nlam = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lam");
-            MEX_DIM_CHECK_VEC(fun_name, "lam", nrow*ncol, nlam);
-
-            ocp_nlp_out_set(config, dims, out, ii, "lam", value);
+            MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field)
+        }
+        else //(nrhs==min_nrhs+1)
+        {
+            acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "lam");
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            ocp_nlp_out_set(config, dims, out, s0, "lam", value);
         }
     }
-    else if (!strcmp(field, "init_t"))
+    else if (!strcmp(field, "init_t")||!strcmp(field, "t"))
     {
-        for (int ii=s0; ii<se; ii++)
+        if (nrhs==min_nrhs)
         {
-            int nt = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "t");
-            MEX_DIM_CHECK_VEC(fun_name, "t", nrow*ncol, nt);
-
-            ocp_nlp_out_set(config, dims, out, ii, "t", value);
+            MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field)
+        }
+        else //(nrhs==min_nrhs+1)
+        {
+            acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "t");
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            ocp_nlp_out_set(config, dims, out, s0, "t", value);
+        }
+    }
+    else if (!strcmp(field, "init_sl")||!strcmp(field, "sl"))
+    {
+        if (nrhs==min_nrhs)
+        {
+            MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field)
+        }
+        else //(nrhs==min_nrhs+1)
+        {
+            acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "sl");
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            ocp_nlp_out_set(config, dims, out, s0, field, value);
+        }
+    }
+    else if (!strcmp(field, "init_su")||!strcmp(field, "su"))
+    {
+        if (nrhs==min_nrhs)
+        {
+            MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field)
+        }
+        else //(nrhs==min_nrhs+1)
+        {
+            acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "su");
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            ocp_nlp_out_set(config, dims, out, s0, field, value);
         }
     }
     else if (!strcmp(field, "p"))
@@ -558,8 +620,8 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     else
     {
         MEX_FIELD_NOT_SUPPORTED_SUGGEST(fun_name, field, "p, constr_x0,\
- constr_lbx, constr_ubx, constr_C, constr_D, constr_lg, constr_ug, constr_lh, constr_uh\
- constr_lbu, constr_ubu, cost_y_ref[_e],\
+ constr_lbx, constr_ubx, constr_C, constr_D, constr_lg, constr_ug, constr_lh, constr_uh,\
+ constr_lbu, constr_ubu, cost_y_ref[_e], sl, su, x, xdot, u, pi, lam, z, \
  cost_Vu, cost_Vx, cost_Vz, cost_W, cost_Z, cost_Zl, cost_Zu, cost_z,\
  cost_zl, cost_zu, init_x, init_u, init_z, init_xdot, init_gnsf_phi,\
  init_pi, nlp_solver_max_iter, qp_warm_start, warm_start_first_qp, print_level");
diff --git a/third_party/acados/acados_template/c_templates_tera/acados_mex_solve.in.c b/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_solve.in.c
similarity index 86%
rename from third_party/acados/acados_template/c_templates_tera/acados_mex_solve.in.c
rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_solve.in.c
index c673d4bf22..300a440cc9 100644
--- a/third_party/acados/acados_template/c_templates_tera/acados_mex_solve.in.c
+++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_solve.in.c
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c b/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_sim_solver_sfun.in.c
similarity index 95%
rename from third_party/acados/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c
rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_sim_solver_sfun.in.c
index 68a6a3f80f..bd73ff69a4 100644
--- a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c
+++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_sim_solver_sfun.in.c
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/acados_template/c_templates_tera/acados_solver_sfun.in.c b/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c
similarity index 85%
rename from third_party/acados/acados_template/c_templates_tera/acados_solver_sfun.in.c
rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c
index 96e0983de6..3dd248037a 100644
--- a/third_party/acados/acados_template/c_templates_tera/acados_solver_sfun.in.c
+++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -61,6 +58,8 @@ static void mdlInitializeSizes (SimStruct *S)
     ssSetNumContStates(S, 0);
     ssSetNumDiscStates(S, 0);
 
+    int N = {{ model.name | upper }}_N;
+
   {%- for key, val in simulink_opts.inputs -%}
     {%- if val != 0 and val != 1 -%}
       {{ throw(message = "simulink_opts.inputs must be 0 or 1, got val") }}
@@ -117,6 +116,12 @@ static void mdlInitializeSizes (SimStruct *S)
   {%- if dims.nh > 0 and simulink_opts.inputs.uh -%}  {#- uh #}
     {%- set n_inputs = n_inputs + 1 -%}
   {%- endif -%}
+  {%- if dims.nh_e > 0 and simulink_opts.inputs.lh_e -%}  {#- lh_e #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+  {%- if dims.nh_e > 0 and simulink_opts.inputs.uh_e -%}  {#- uh_e #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
 
   {%- for key, val in simulink_opts.inputs -%}
     {%- if val != 0 and val != 1 -%}
@@ -177,7 +182,7 @@ static void mdlInitializeSizes (SimStruct *S)
   {%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%}  {#- parameter_traj #}
     {%- set i_input = i_input + 1 %}
     // parameters
-    ssSetInputPortVectorDimension(S, {{ i_input }}, ({{ dims.N }}+1) * {{ dims.np }});
+    ssSetInputPortVectorDimension(S, {{ i_input }}, (N+1) * {{ dims.np }});
   {%- endif %}
 
   {%- if dims.ny > 0 and simulink_opts.inputs.y_ref_0 %}
@@ -235,23 +240,34 @@ static void mdlInitializeSizes (SimStruct *S)
   {%- if dims.ng > 0 and simulink_opts.inputs.lg -%}  {#- lg #}
     {%- set i_input = i_input + 1 %}
     // lg
-    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ng }});
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.ng }});
   {%- endif -%}
   {%- if dims.ng > 0 and simulink_opts.inputs.ug -%}  {#- ug #}
     {%- set i_input = i_input + 1 %}
     // ug
-    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ng }});
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.ng }});
   {%- endif -%}
 
   {%- if dims.nh > 0 and simulink_opts.inputs.lh -%}  {#- lh #}
     {%- set i_input = i_input + 1 %}
     // lh
-    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh }});
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nh }});
   {%- endif -%}
   {%- if dims.nh > 0 and simulink_opts.inputs.uh -%}  {#- uh #}
     {%- set i_input = i_input + 1 %}
     // uh
-    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh }});
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nh }});
+  {%- endif -%}
+
+  {%- if dims.nh_e > 0 and simulink_opts.inputs.lh_e -%}  {#- lh_e #}
+    {%- set i_input = i_input + 1 %}
+    // lh_e
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh_e }});
+  {%- endif -%}
+  {%- if dims.nh_e > 0 and simulink_opts.inputs.uh_e -%}  {#- uh_e #}
+    {%- set i_input = i_input + 1 %}
+    // uh_e
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh_e }});
   {%- endif -%}
 
   {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %}  {#- cost_W_0 #}
@@ -312,11 +328,21 @@ static void mdlInitializeSizes (SimStruct *S)
     ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 );
   {%- endif %}
 
+  {%- if simulink_opts.outputs.cost_value == 1 %}
+    {%- set i_output = i_output + 1 %}
+    ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 );
+  {%- endif %}
+
   {%- if simulink_opts.outputs.KKT_residual == 1 %}
     {%- set i_output = i_output + 1 %}
     ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 );
   {%- endif %}
 
+  {%- if simulink_opts.outputs.KKT_residuals == 1 %}
+    {%- set i_output = i_output + 1 %}
+    ssSetOutputPortVectorDimension(S, {{ i_output }}, 4 );
+  {%- endif %}
+
   {%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %}
     {%- set i_output = i_output + 1 %}
     ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nx }} ); // state at shooting node 1
@@ -404,7 +430,9 @@ static void mdlOutputs(SimStruct *S, int_T tid)
 
     InputRealPtrsType in_sign;
 
-    {%- set buffer_sizes = [dims.nbx_0, dims.np, dims.nbx, dims.nbu, dims.ng, dims.nh, dims.nx] -%}
+    int N = {{ model.name | upper }}_N;
+
+    {%- set buffer_sizes = [dims.nbx_0, dims.np, dims.nbx, dims.nbx_e, dims.nbu, dims.ng, dims.nh, dims.ng_e, dims.nh_e] -%}
 
   {%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %}  {# y_ref_0 #}
     {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_0)) %}
@@ -457,7 +485,7 @@ static void mdlOutputs(SimStruct *S, int_T tid)
     in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
 
     // update value of parameters
-    for (int ii = 0; ii <= {{ dims.N }}; ii++)
+    for (int ii = 0; ii <= N; ii++)
     {
         for (int jj = 0; jj < {{ dims.np }}; jj++)
             buffer[jj] = (double)(*in_sign[ii*{{dims.np}}+jj]);
@@ -481,7 +509,7 @@ static void mdlOutputs(SimStruct *S, int_T tid)
     {%- set i_input = i_input + 1 %}
     in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
 
-    for (int ii = 1; ii < {{ dims.N }}; ii++)
+    for (int ii = 1; ii < N; ii++)
     {
         for (int jj = 0; jj < {{ dims.ny }}; jj++)
             buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.ny }}+jj]);
@@ -497,14 +525,14 @@ static void mdlOutputs(SimStruct *S, int_T tid)
     for (int i = 0; i < {{ dims.ny_e }}; i++)
         buffer[i] = (double)(*in_sign[i]);
 
-    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "yref", (void *) buffer);
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", (void *) buffer);
   {%- endif %}
 
   {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%}  {#- lbx #}
     // lbx
     {%- set i_input = i_input + 1 %}
     in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
-    for (int ii = 1; ii < {{ dims.N }}; ii++)
+    for (int ii = 1; ii < N; ii++)
     {
         for (int jj = 0; jj < {{ dims.nbx }}; jj++)
             buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.nbx }}+jj]);
@@ -515,7 +543,7 @@ static void mdlOutputs(SimStruct *S, int_T tid)
     // ubx
     {%- set i_input = i_input + 1 %}
     in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
-    for (int ii = 1; ii < {{ dims.N }}; ii++)
+    for (int ii = 1; ii < N; ii++)
     {
         for (int jj = 0; jj < {{ dims.nbx }}; jj++)
             buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.nbx }}+jj]);
@@ -531,7 +559,7 @@ static void mdlOutputs(SimStruct *S, int_T tid)
 
     for (int i = 0; i < {{ dims.nbx_e }}; i++)
         buffer[i] = (double)(*in_sign[i]);
-    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "lbx", buffer);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", buffer);
   {%- endif %}
   {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%}  {#- ubx_e #}
     // ubx_e
@@ -540,7 +568,7 @@ static void mdlOutputs(SimStruct *S, int_T tid)
 
     for (int i = 0; i < {{ dims.nbx_e }}; i++)
         buffer[i] = (double)(*in_sign[i]);
-    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "ubx", buffer);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", buffer);
   {%- endif %}
 
 
@@ -548,7 +576,7 @@ static void mdlOutputs(SimStruct *S, int_T tid)
     // lbu
     {%- set i_input = i_input + 1 %}
     in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
-    for (int ii = 0; ii < {{ dims.N }}; ii++)
+    for (int ii = 0; ii < N; ii++)
     {
         for (int jj = 0; jj < {{ dims.nbu }}; jj++)
             buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]);
@@ -559,7 +587,7 @@ static void mdlOutputs(SimStruct *S, int_T tid)
     // ubu
     {%- set i_input = i_input + 1 %}
     in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
-    for (int ii = 0; ii < {{ dims.N }}; ii++)
+    for (int ii = 0; ii < N; ii++)
     {
         for (int jj = 0; jj < {{ dims.nbu }}; jj++)
             buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]);
@@ -572,44 +600,67 @@ static void mdlOutputs(SimStruct *S, int_T tid)
     {%- set i_input = i_input + 1 %}
     in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
 
-    for (int i = 0; i < {{ dims.ng }}; i++)
-        buffer[i] = (double)(*in_sign[i]);
-
-    for (int ii = 0; ii < {{ dims.N }}; ii++)
-        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lg", buffer);
+    for (int ii = 0; ii < N; ii++)
+    {
+        for (int jj = 0; jj < {{ dims.ng }}; jj++)
+            buffer[jj] = (double)(*in_sign[ii*{{ dims.ng }}+jj]);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lg", (void *) buffer);
+    }
   {%- endif -%}
   {%- if dims.ng > 0 and simulink_opts.inputs.ug -%}  {#- ug #}
     // ug
     {%- set i_input = i_input + 1 %}
     in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
 
-    for (int i = 0; i < {{ dims.ng }}; i++)
-        buffer[i] = (double)(*in_sign[i]);
-
-    for (int ii = 0; ii < {{ dims.N }}; ii++)
-        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ug", buffer);
+    for (int ii = 0; ii < N; ii++)
+    {
+        for (int jj = 0; jj < {{ dims.ng }}; jj++)
+            buffer[jj] = (double)(*in_sign[ii*{{ dims.ng }}+jj]);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ug", (void *) buffer);
+    }
   {%- endif -%}
+
   {%- if dims.nh > 0 and simulink_opts.inputs.lh -%}  {#- lh #}
     // lh
     {%- set i_input = i_input + 1 %}
     in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
 
-    for (int i = 0; i < {{ dims.nh }}; i++)
-        buffer[i] = (double)(*in_sign[i]);
-
-    for (int ii = 0; ii < {{ dims.N }}; ii++)
-        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", buffer);
+    for (int ii = 0; ii < N; ii++)
+    {
+        for (int jj = 0; jj < {{ dims.nh }}; jj++)
+            buffer[jj] = (double)(*in_sign[ii*{{ dims.nh }}+jj]);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", (void *) buffer);
+    }
   {%- endif -%}
   {%- if dims.nh > 0 and simulink_opts.inputs.uh -%}  {#- uh #}
     // uh
     {%- set i_input = i_input + 1 %}
     in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
 
-    for (int i = 0; i < {{ dims.nh }}; i++)
-        buffer[i] = (double)(*in_sign[i]);
+    for (int ii = 0; ii < N; ii++)
+    {
+        for (int jj = 0; jj < {{ dims.nh }}; jj++)
+            buffer[jj] = (double)(*in_sign[ii*{{ dims.nh }}+jj]);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", (void *) buffer);
+    }
+  {%- endif -%}
+
 
-    for (int ii = 0; ii < {{ dims.N }}; ii++)
-        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", buffer);
+  {%- if dims.nh_e > 0 and simulink_opts.inputs.lh_e -%}  {#- lh_e #}
+    // lh_e
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+    for (int i = 0; i < {{ dims.nh_e }}; i++)
+        buffer[i] = (double)(*in_sign[i]);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", buffer);
+  {%- endif -%}
+  {%- if dims.nh_e > 0 and simulink_opts.inputs.uh_e -%}  {#- uh_e #}
+    // uh_e
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+    for (int i = 0; i < {{ dims.nh_e }}; i++)
+        buffer[i] = (double)(*in_sign[i]);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", buffer);
   {%- endif -%}
 
   {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %}  {#- cost_W_0 #}
@@ -629,7 +680,7 @@ static void mdlOutputs(SimStruct *S, int_T tid)
     for (int i = 0; i < {{ dims.ny * dims.ny }}; i++)
         buffer[i] = (double)(*in_sign[i]);
 
-    for (int ii = 1; ii < {{ dims.N }}; ii++)
+    for (int ii = 1; ii < N; ii++)
         ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "W", buffer);
   {%- endif %}
 
@@ -640,7 +691,7 @@ static void mdlOutputs(SimStruct *S, int_T tid)
     for (int i = 0; i < {{ dims.ny_e * dims.ny_e }}; i++)
         buffer[i] = (double)(*in_sign[i]);
 
-    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "W", buffer);
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", buffer);
   {%- endif %}
 
   {%- if simulink_opts.inputs.reset_solver %}  {#- reset_solver #}
@@ -650,7 +701,7 @@ static void mdlOutputs(SimStruct *S, int_T tid)
     double reset = (double)(*in_sign[0]);
     if (reset)
     {
-        {{ model.name }}_acados_reset(capsule);
+        {{ model.name }}_acados_reset(capsule, 1);
     }
   {%- endif %}
 
@@ -670,7 +721,7 @@ static void mdlOutputs(SimStruct *S, int_T tid)
     // u_init
     {%- set i_input = i_input + 1 %}
     in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
-    for (int ii = 0; ii < {{ dims.N }}; ii++)
+    for (int ii = 0; ii < N; ii++)
     {
         for (int jj = 0; jj < {{ dims.nu }}; jj++)
             buffer[jj] = (double)(*in_sign[(ii)*{{ dims.nu }}+jj]);
@@ -686,7 +737,7 @@ static void mdlOutputs(SimStruct *S, int_T tid)
 
     /* set outputs */
     // assign pointers to output signals
-    real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
+    real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_KKT_residuals, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin, *out_cost_value;
     int tmp_int;
 
     {%- set i_output = -1 -%}{# note here i_output is 0-based #}
@@ -699,7 +750,7 @@ static void mdlOutputs(SimStruct *S, int_T tid)
   {%- if simulink_opts.outputs.utraj == 1 %}
     {%- set i_output = i_output + 1 %}
     out_utraj = ssGetOutputPortRealSignal(S, {{ i_output }});
-    for (int ii = 0; ii < {{ dims.N }}; ii++)
+    for (int ii = 0; ii < N; ii++)
         ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii,
                         "u", (void *) (out_utraj + ii * {{ dims.nu }}));
   {%- endif %}
@@ -719,12 +770,32 @@ static void mdlOutputs(SimStruct *S, int_T tid)
     *out_status = (real_t) acados_status;
   {%- endif %}
 
+  {%- if simulink_opts.outputs.cost_value == 1 %}
+    {%- set i_output = i_output + 1 %}
+    out_cost_value = ssGetOutputPortRealSignal(S, {{ i_output }});
+    ocp_nlp_eval_cost(capsule->nlp_solver, nlp_in, nlp_out);
+    ocp_nlp_get(nlp_config, capsule->nlp_solver, "cost_value", (void *) out_cost_value);
+  {%- endif %}
+
   {%- if simulink_opts.outputs.KKT_residual == 1 %}
     {%- set i_output = i_output + 1 %}
     out_KKT_res = ssGetOutputPortRealSignal(S, {{ i_output }});
     *out_KKT_res = (real_t) nlp_out->inf_norm_res;
   {%- endif %}
 
+  {%- if simulink_opts.outputs.KKT_residuals == 1 %}
+    {%- set i_output = i_output + 1 %}
+    out_KKT_residuals = ssGetOutputPortRealSignal(S, {{ i_output }});
+
+    {%- if solver_options.nlp_solver_type == "SQP_RTI" %}
+    ocp_nlp_eval_residuals(capsule->nlp_solver, nlp_in, nlp_out);
+    {%- endif %}
+    ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_stat", (void *) &out_KKT_residuals[0]);
+    ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_eq", (void *) &out_KKT_residuals[1]);
+    ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_ineq", (void *) &out_KKT_residuals[2]);
+    ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_comp", (void *) &out_KKT_residuals[3]);
+  {%- endif %}
+
   {%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %}
     {%- set i_output = i_output + 1 %}
     out_x1 = ssGetOutputPortRealSignal(S, {{ i_output }});
diff --git a/third_party/acados/acados_template/c_templates_tera/main_mex.in.c b/third_party/acados/acados_template/c_templates_tera/matlab_templates/main_mex.in.c
similarity index 95%
rename from third_party/acados/acados_template/c_templates_tera/main_mex.in.c
rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/main_mex.in.c
index 8da5db29a0..851a3cc04f 100644
--- a/third_party/acados/acados_template/c_templates_tera/main_mex.in.c
+++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/main_mex.in.c
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/acados_template/c_templates_tera/make_main_mex.in.m b/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_main_mex.in.m
similarity index 93%
rename from third_party/acados/acados_template/c_templates_tera/make_main_mex.in.m
rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/make_main_mex.in.m
index 9188686a0d..d217948456 100644
--- a/third_party/acados/acados_template/c_templates_tera/make_main_mex.in.m
+++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_main_mex.in.m
@@ -1,8 +1,5 @@
 %
-% 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
+% Copyright (c) The acados authors.
 %
 % This file is part of acados.
 %
@@ -29,6 +26,7 @@
 % 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.;
+
 %
 
 function make_main_mex_{{ model.name }}()
diff --git a/third_party/acados/acados_template/c_templates_tera/make_mex.in.m b/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_mex.in.m
similarity index 81%
rename from third_party/acados/acados_template/c_templates_tera/make_mex.in.m
rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/make_mex.in.m
index cde30f6f41..5e35827137 100644
--- a/third_party/acados/acados_template/c_templates_tera/make_mex.in.m
+++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_mex.in.m
@@ -1,8 +1,5 @@
 %
-% 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
+% Copyright (c) The acados authors.
 %
 % This file is part of acados.
 %
@@ -29,6 +26,7 @@
 % 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.;
+
 %
 
 function make_mex_{{ model.name }}()
@@ -48,6 +46,23 @@ function make_mex_{{ model.name }}()
     blasfeo_include = ['-I', fullfile(acados_folder, 'external', 'blasfeo', 'include')];
     hpipm_include = ['-I', fullfile(acados_folder, 'external', 'hpipm', 'include')];
 
+    % load linking information of compiled acados
+    link_libs_core_filename = fullfile(acados_folder, 'lib', 'link_libs.json');
+    addpath(fullfile(acados_folder, 'external', 'jsonlab'));
+    link_libs = loadjson(link_libs_core_filename);
+
+    % add necessary link instructs
+    acados_lib_extra = {};
+    lib_names = fieldnames(link_libs);
+    for idx = 1 : numel(lib_names)
+        lib_name = lib_names{idx};
+        link_arg = link_libs.(lib_name);
+        if ~isempty(link_arg)
+            acados_lib_extra = [acados_lib_extra, link_arg];
+        end
+    end
+
+
     mex_include = ['-I', fullfile(acados_folder, 'interfaces', 'acados_matlab_octave')];
 
     mex_names = { ...
@@ -94,7 +109,8 @@ function make_mex_{{ model.name }}()
         if is_octave()
     %        mkoctfile -p CFLAGS
             mex(acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,...
-            acados_lib_path, template_lib_path, mex_include, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii})
+                template_lib_path, mex_include, acados_lib_path, '-lacados', '-lhpipm', '-lblasfeo',...
+                acados_lib_extra{:}, mex_files{ii})
         else
             if ismac()
                 FLAGS = 'CFLAGS=$CFLAGS -std=c99';
@@ -102,7 +118,8 @@ function make_mex_{{ model.name }}()
                 FLAGS = 'CFLAGS=$CFLAGS -std=c99 -fopenmp';
             end
             mex(FLAGS, acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,...
-                acados_lib_path, template_lib_path, mex_include, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii})
+                template_lib_path, mex_include, acados_lib_path, '-lacados', '-lhpipm', '-lblasfeo',...
+                acados_lib_extra{:}, mex_files{ii})
         end
     end
 
diff --git a/third_party/acados/acados_template/c_templates_tera/make_sfun.in.m b/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m
similarity index 74%
rename from third_party/acados/acados_template/c_templates_tera/make_sfun.in.m
rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m
index 77603a78fa..5d74c523f8 100644
--- a/third_party/acados/acados_template/c_templates_tera/make_sfun.in.m
+++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m
@@ -1,8 +1,5 @@
 %
-% 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
+% Copyright (c) The acados authors.
 %
 % This file is part of acados.
 %
@@ -29,6 +26,7 @@
 % 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.;
+
 %
 
 SOURCES = { ...
@@ -133,6 +131,9 @@ COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_OSQP ' ];
 {%- elif solver_options.qp_solver is containing("QPDUNES") %}
 CFLAGS = [ CFLAGS, ' -DACADOS_WITH_QPDUNES ' ];
 COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_QPDUNES ' ];
+{%- elif solver_options.qp_solver is containing("DAQP") %}
+CFLAGS = [ CFLAGS, ' -DACADOS_WITH_DAQP' ];
+COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_DAQP' ];
 {%- elif solver_options.qp_solver is containing("HPMPC") %}
 CFLAGS = [ CFLAGS, ' -DACADOS_WITH_HPMPC ' ];
 COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_HPMPC ' ];
@@ -153,129 +154,176 @@ LIBS{end+1} = '{{ acados_link_libs.osqp }}';
     {% if solver_options.qp_solver is containing("QPOASES") %}
 LIBS{end+1} = '-lqpOASES_e';
     {% endif %}
+    {% if solver_options.qp_solver is containing("DAQP") %}
+LIBS{end+1} = '-ldaqp';
+    {% endif %}
 {%- endif %}
 
-mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
-    LIB_PATH, LIBS{:}, SOURCES{:}, ...
-    '-output', 'acados_solver_sfunction_{{ model.name }}' );
+
+try
+    %     mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
+    mex('-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
+            LIB_PATH, LIBS{:}, SOURCES{:}, ...
+            '-output', 'acados_solver_sfunction_{{ model.name }}' );
+catch exception
+    disp('make_sfun failed with the following exception:')
+    disp(exception);
+    disp('Try adding -v to the mex command above to get more information.')
+    keyboard
+end
 
 fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_{{ model.name }}', '.', ...
     eval('mexext')] );
 
 
-%% print note on usage of s-function
+%% print note on usage of s-function, and create I/O port names vectors
 fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
 input_note = 'Inputs are:\n';
 i_in = 1;
 
+global sfun_input_names
+sfun_input_names = {};
 
 {%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%}  {#- lbx_0 #}
 input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
                     ' size [{{ dims.nbx_0 }}]\n ');
+sfun_input_names = [sfun_input_names; 'lbx_0 [{{ dims.nbx_0 }}]'];
 i_in = i_in + 1;
 {%- endif %}
 
 {%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%}  {#- ubx_0 #}
 input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
                     ' size [{{ dims.nbx_0 }}]\n ');
+sfun_input_names = [sfun_input_names; 'ubx_0 [{{ dims.nbx_0 }}]'];
 i_in = i_in + 1;
 {%- endif %}
 
 {%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%}  {#- parameter_traj #}
-input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
+input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N,',...
                     ' size [{{ (dims.N+1)*dims.np }}]\n ');
+sfun_input_names = [sfun_input_names; 'parameter_traj [{{ (dims.N+1)*dims.np }}]'];
 i_in = i_in + 1;
 {%- endif %}
 
 {%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %}
 input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [{{ dims.ny_0 }}]\n ');
+sfun_input_names = [sfun_input_names; 'y_ref_0 [{{ dims.ny_0 }}]'];
 i_in = i_in + 1;
 {%- endif %}
 
 {%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %}
 input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
                     ' size [{{ (dims.N-1) * dims.ny }}]\n ');
+sfun_input_names = [sfun_input_names; 'y_ref [{{ (dims.N-1) * dims.ny }}]'];
 i_in = i_in + 1;
 {%- endif %}
 
 {%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %}
 input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [{{ dims.ny_e }}]\n ');
+sfun_input_names = [sfun_input_names; 'y_ref_e [{{ dims.ny_e }}]'];
 i_in = i_in + 1;
 {%- endif %}
 
 {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%}  {#- lbx #}
 input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [{{ (dims.N-1) * dims.nbx }}]\n ');
+sfun_input_names = [sfun_input_names; 'lbx [{{ (dims.N-1) * dims.nbx }}]'];
 i_in = i_in + 1;
 {%- endif %}
 {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%}  {#- ubx #}
 input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [{{ (dims.N-1) * dims.nbx }}]\n ');
+sfun_input_names = [sfun_input_names; 'ubx [{{ (dims.N-1) * dims.nbx }}]'];
 i_in = i_in + 1;
 {%- endif %}
 
 
 {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%}  {#- lbx_e #}
 input_note = strcat(input_note, num2str(i_in), ') lbx_e (lbx at shooting node N), size [{{ dims.nbx_e }}]\n ');
+sfun_input_names = [sfun_input_names; 'lbx_e [{{ dims.nbx_e }}]'];
 i_in = i_in + 1;
 {%- endif %}
 {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%}  {#- ubx_e #}
 input_note = strcat(input_note, num2str(i_in), ') ubx_e (ubx at shooting node N), size [{{ dims.nbx_e }}]\n ');
+sfun_input_names = [sfun_input_names; 'ubx_e [{{ dims.nbx_e }}]'];
 i_in = i_in + 1;
 {%- endif %}
 
 {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%}  {#- lbu #}
 input_note = strcat(input_note, num2str(i_in), ') lbu for shooting nodes 0 to N-1, size [{{ dims.N*dims.nbu }}]\n ');
+sfun_input_names = [sfun_input_names; 'lbu [{{ dims.N*dims.nbu }}]'];
 i_in = i_in + 1;
 {%- endif -%}
 {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%}  {#- ubu #}
 input_note = strcat(input_note, num2str(i_in), ') ubu for shooting nodes 0 to N-1, size [{{ dims.N*dims.nbu }}]\n ');
+sfun_input_names = [sfun_input_names; 'ubu [{{ dims.N*dims.nbu }}]'];
 i_in = i_in + 1;
 {%- endif -%}
 
 {%- if dims.ng > 0 and simulink_opts.inputs.lg -%}  {#- lg #}
-input_note = strcat(input_note, num2str(i_in), ') lg, size [{{ dims.ng }}]\n ');
+input_note = strcat(input_note, num2str(i_in), ') lg for shooting nodes 0 to N-1, size [{{ dims.N*dims.ng }}]\n ');
+sfun_input_names = [sfun_input_names; 'lg [{{ dims.N*dims.ng }}]'];
 i_in = i_in + 1;
 {%- endif %}
 {%- if dims.ng > 0 and simulink_opts.inputs.ug -%}  {#- ug #}
-input_note = strcat(input_note, num2str(i_in), ') ug, size [{{ dims.ng }}]\n ');
+input_note = strcat(input_note, num2str(i_in), ') ug for shooting nodes 0 to N-1, size [{{ dims.N*dims.ng }}]\n ');
+sfun_input_names = [sfun_input_names; 'ug [{{ dims.N*dims.ng }}]'];
 i_in = i_in + 1;
 {%- endif %}
 
 {%- if dims.nh > 0 and simulink_opts.inputs.lh -%}  {#- lh #}
-input_note = strcat(input_note, num2str(i_in), ') lh, size [{{ dims.nh }}]\n ');
+input_note = strcat(input_note, num2str(i_in), ') lh for shooting nodes 0 to N-1, size [{{ dims.N*dims.nh }}]\n ');
+sfun_input_names = [sfun_input_names; 'lh [{{ dims.N*dims.nh }}]'];
 i_in = i_in + 1;
 {%- endif %}
 {%- if dims.nh > 0 and simulink_opts.inputs.uh -%}  {#- uh #}
-input_note = strcat(input_note, num2str(i_in), ') uh, size [{{ dims.nh }}]\n ');
+input_note = strcat(input_note, num2str(i_in), ') uh for shooting nodes 0 to N-1, size [{{ dims.N*dims.nh }}]\n ');
+sfun_input_names = [sfun_input_names; 'uh [{{ dims.N*dims.nh }}]'];
+i_in = i_in + 1;
+{%- endif %}
+
+{%- if dims.nh_e > 0 and simulink_opts.inputs.lh_e -%}  {#- lh_e #}
+input_note = strcat(input_note, num2str(i_in), ') lh_e, size [{{ dims.nh_e }}]\n ');
+sfun_input_names = [sfun_input_names; 'lh_e [{{ dims.nh_e }}]'];
+i_in = i_in + 1;
+{%- endif %}
+{%- if dims.nh_e > 0 and simulink_opts.inputs.uh_e -%}  {#- uh_e #}
+input_note = strcat(input_note, num2str(i_in), ') uh_e, size [{{ dims.nh_e }}]\n ');
+sfun_input_names = [sfun_input_names; 'uh_e [{{ dims.nh_e }}]'];
 i_in = i_in + 1;
 {%- endif %}
 
 {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %}  {#- cost_W_0 #}
 input_note = strcat(input_note, num2str(i_in), ') cost_W_0 in column-major format, size [{{ dims.ny_0 * dims.ny_0 }}]\n ');
+sfun_input_names = [sfun_input_names; 'cost_W_0 [{{ dims.ny_0 * dims.ny_0 }}]'];
 i_in = i_in + 1;
 {%- endif %}
 
 {%- if dims.ny > 0 and simulink_opts.inputs.cost_W %}  {#- cost_W #}
 input_note = strcat(input_note, num2str(i_in), ') cost_W in column-major format, that is set for all intermediate shooting nodes: 1 to N-1, size [{{ dims.ny * dims.ny }}]\n ');
+sfun_input_names = [sfun_input_names; 'cost_W [{{ dims.ny * dims.ny }}]'];
 i_in = i_in + 1;
 {%- endif %}
 
 {%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %}  {#- cost_W_e #}
 input_note = strcat(input_note, num2str(i_in), ') cost_W_e in column-major format, size [{{ dims.ny_e * dims.ny_e }}]\n ');
+sfun_input_names = [sfun_input_names; 'cost_W_e [{{ dims.ny_e * dims.ny_e }}]'];
 i_in = i_in + 1;
 {%- endif %}
 
 {%- if simulink_opts.inputs.reset_solver %}  {#- reset_solver #}
 input_note = strcat(input_note, num2str(i_in), ') reset_solver determines if iterate is set to all zeros before other initializations (x_init, u_init) are set and before solver is called, size [1]\n ');
+sfun_input_names = [sfun_input_names; 'reset_solver [1]'];
 i_in = i_in + 1;
 {%- endif %}
 
 {%- if simulink_opts.inputs.x_init %}  {#- x_init #}
 input_note = strcat(input_note, num2str(i_in), ') initialization of x for all shooting nodes, size [{{ dims.nx * (dims.N+1) }}]\n ');
+sfun_input_names = [sfun_input_names; 'x_init [{{ dims.nx * (dims.N+1) }}]'];
 i_in = i_in + 1;
 {%- endif %}
 
 {%- if simulink_opts.inputs.u_init %}  {#- u_init #}
 input_note = strcat(input_note, num2str(i_in), ') initialization of u for shooting nodes 0 to N-1, size [{{ dims.nu * (dims.N) }}]\n ');
+sfun_input_names = [sfun_input_names; 'u_init [{{ dims.nu * (dims.N) }}]'];
 i_in = i_in + 1;
 {%- endif %}
 
@@ -286,59 +334,99 @@ disp(' ')
 output_note = 'Outputs are:\n';
 i_out = 0;
 
+global sfun_output_names
+sfun_output_names = {};
+
 {%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %}
 i_out = i_out + 1;
 output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [{{ dims.nu }}]\n ');
+sfun_output_names = [sfun_output_names; 'u0 [{{ dims.nu }}]'];
 {%- endif %}
 
 {%- if simulink_opts.outputs.utraj == 1 %}
 i_out = i_out + 1;
 output_note = strcat(output_note, num2str(i_out), ') utraj, control input concatenated for nodes 0 to N-1, size [{{ dims.nu * dims.N }}]\n ');
+sfun_output_names = [sfun_output_names; 'utraj [{{ dims.nu * dims.N }}]'];
 {%- endif %}
 
 {%- if simulink_opts.outputs.xtraj == 1 %}
 i_out = i_out + 1;
 output_note = strcat(output_note, num2str(i_out), ') xtraj, state concatenated for nodes 0 to N, size [{{ dims.nx * (dims.N + 1) }}]\n ');
+sfun_output_names = [sfun_output_names; 'xtraj [{{ dims.nx * (dims.N + 1) }}]'];
 {%- endif %}
 
 {%- if simulink_opts.outputs.solver_status == 1 %}
 i_out = i_out + 1;
 output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
+sfun_output_names = [sfun_output_names; 'solver_status'];
 {%- endif %}
 
+{%- if simulink_opts.outputs.cost_value == 1 %}
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') cost function value\n ');
+sfun_output_names = [sfun_output_names; 'cost_value'];
+{%- endif %}
+
+
 {%- if simulink_opts.outputs.KKT_residual == 1 %}
 i_out = i_out + 1;
 output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
+sfun_output_names = [sfun_output_names; 'KKT_residual'];
+{%- endif %}
+
+{%- if simulink_opts.outputs.KKT_residuals == 1 %}
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') KKT residuals, size [4] (stat, eq, ineq, comp)\n ');
+sfun_output_names = [sfun_output_names; 'KKT_residuals [4]'];
 {%- endif %}
 
 {%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %}
 i_out = i_out + 1;
 output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
+sfun_output_names = [sfun_output_names; 'x1'];
 {%- endif %}
 
 {%- if simulink_opts.outputs.CPU_time == 1 %}
 i_out = i_out + 1;
 output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
+sfun_output_names = [sfun_output_names; 'CPU_time'];
 {%- endif %}
 
 {%- if simulink_opts.outputs.CPU_time_sim == 1 %}
 i_out = i_out + 1;
 output_note = strcat(output_note, num2str(i_out), ') CPU time integrator\n ');
+sfun_output_names = [sfun_output_names; 'CPU_time_sim'];
 {%- endif %}
 
 {%- if simulink_opts.outputs.CPU_time_qp == 1 %}
 i_out = i_out + 1;
 output_note = strcat(output_note, num2str(i_out), ') CPU time QP solution\n ');
+sfun_output_names = [sfun_output_names; 'CPU_time_qp'];
 {%- endif %}
 
 {%- if simulink_opts.outputs.CPU_time_lin == 1 %}
 i_out = i_out + 1;
 output_note = strcat(output_note, num2str(i_out), ') CPU time linearization (including integrator)\n ');
+sfun_output_names = [sfun_output_names; 'CPU_time_lin'];
 {%- endif %}
 
 {%- if simulink_opts.outputs.sqp_iter == 1 %}
 i_out = i_out + 1;
 output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
+sfun_output_names = [sfun_output_names; 'sqp_iter'];
 {%- endif %}
 
 fprintf(output_note)
+
+% The mask drawing command is:
+% ---
+% global sfun_input_names sfun_output_names
+% for i = 1:length(sfun_input_names)
+% 	port_label('input', i, sfun_input_names{i})
+% end
+% for i = 1:length(sfun_output_names)
+% 	port_label('output', i, sfun_output_names{i})
+% end
+% ---
+% It can be used by copying it in sfunction/Mask/Edit mask/Icon drawing commands
+%   (you can access it wirth ctrl+M on the s-function)
\ No newline at end of file
diff --git a/third_party/acados/acados_template/c_templates_tera/make_sfun_sim.in.m b/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun_sim.in.m
similarity index 70%
rename from third_party/acados/acados_template/c_templates_tera/make_sfun_sim.in.m
rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun_sim.in.m
index a0c503e41a..e4c32a8c19 100644
--- a/third_party/acados/acados_template/c_templates_tera/make_sfun_sim.in.m
+++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun_sim.in.m
@@ -1,8 +1,5 @@
 %
-% 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
+% Copyright (c) The acados authors.
 %
 % This file is part of acados.
 %
@@ -29,17 +26,19 @@
 % 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.;
+
 %
 
 SOURCES = [ 'acados_sim_solver_sfunction_{{ model.name }}.c ', ...
             'acados_sim_solver_{{ model.name }}.c ', ...
             {%- if  solver_options.integrator_type == 'ERK' %}
-            '{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c ', ...
+            '{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c ',...
             '{{ model.name }}_model/{{ model.name }}_expl_vde_forw.c ',...
+            '{{ model.name }}_model/{{ model.name }}_expl_vde_adj.c ',...
                 {%- if solver_options.hessian_approx == 'EXACT' %}
             '{{ model.name }}_model/{{ model.name }}_expl_ode_hess.c ',...
                 {%- endif %}
-        {%- elif solver_options.integrator_type == "IRK" %}
+            {%- elif solver_options.integrator_type == "IRK" %}
             '{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c ', ...
             '{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c ', ...
             '{{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c ', ...
@@ -47,15 +46,15 @@ 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 '
+                {%- 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 '
+            '{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c ',...
             {%- endif %}
           ];
 
@@ -71,25 +70,42 @@ LIB_PATH = '{{ acados_lib_path }}';
 
 LIBS = '-lacados -lblasfeo -lhpipm';
 
-eval( [ 'mex -v -output  acados_sim_solver_sfunction_{{ model.name }} ', ...
-    CFLAGS, INCS, ' ', SOURCES, ' -L', LIB_PATH, ' ', LIBS ]);
+try
+    % eval( [ 'mex -v -output  acados_sim_solver_sfunction_{{ model.name }} ', ...
+    eval( [ 'mex -output  acados_sim_solver_sfunction_{{ model.name }} ', ...
+        CFLAGS, INCS, ' ', SOURCES, ' -L', LIB_PATH, ' ', LIBS ]);
+
+catch exception
+    disp('make_sfun failed with the following exception:')
+    disp(exception);
+    disp('Try adding -v to the mex command above to get more information.')
+    keyboard
+end
+
 
 fprintf( [ '\n\nSuccessfully created sfunction:\nacados_sim_solver_sfunction_{{ model.name }}', '.', ...
     eval('mexext')] );
 
 
+global sfun_sim_input_names
+sfun_sim_input_names = {};
+
 %% print note on usage of s-function
 fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
 input_note = 'Inputs are:\n1) x0, initial state, size [{{ dims.nx }}]\n ';
 i_in = 2;
+sfun_sim_input_names = [sfun_sim_input_names; 'x0 [{{ dims.nx }}]'];
+
 {%- if dims.nu > 0 %}
 input_note = strcat(input_note, num2str(i_in), ') u, size [{{ dims.nu }}]\n ');
 i_in = i_in + 1;
+sfun_sim_input_names = [sfun_sim_input_names; 'u [{{ dims.nu }}]'];
 {%- endif %}
 
 {%- if dims.np > 0 %}
 input_note = strcat(input_note, num2str(i_in), ') parameters, size [{{ dims.np }}]\n ');
 i_in = i_in + 1;
+sfun_sim_input_names = [sfun_sim_input_names; 'p [{{ dims.np }}]'];
 {%- endif %}
 
 
@@ -97,7 +113,25 @@ fprintf(input_note)
 
 disp(' ')
 
+global sfun_sim_output_names
+sfun_sim_output_names = {};
+
 output_note = strcat('Outputs are:\n', ...
                 '1) x1 - simulated state, size [{{ dims.nx }}]\n');
+sfun_sim_output_names = [sfun_sim_output_names; 'x1 [{{ dims.nx }}]'];
 
 fprintf(output_note)
+
+
+% The mask drawing command is:
+% ---
+% global sfun_sim_input_names sfun_sim_output_names
+% for i = 1:length(sfun_sim_input_names)
+% 	port_label('input', i, sfun_sim_input_names{i})
+% end
+% for i = 1:length(sfun_sim_output_names)
+% 	port_label('output', i, sfun_sim_output_names{i})
+% end
+% ---
+% It can be used by copying it in sfunction/Mask/Edit mask/Icon drawing commands
+%   (you can access it wirth ctrl+M on the s-function)
\ No newline at end of file
diff --git a/third_party/acados/acados_template/c_templates_tera/mex_solver.in.m b/third_party/acados/acados_template/c_templates_tera/matlab_templates/mex_solver.in.m
similarity index 61%
rename from third_party/acados/acados_template/c_templates_tera/mex_solver.in.m
rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/mex_solver.in.m
index 278e0a605c..3743212830 100644
--- a/third_party/acados/acados_template/c_templates_tera/mex_solver.in.m
+++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/mex_solver.in.m
@@ -1,8 +1,5 @@
 %
-% 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
+% Copyright (c) The acados authors.
 %
 % This file is part of acados.
 %
@@ -29,6 +26,7 @@
 % 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.;
+
 %
 
 classdef {{ model.name }}_mex_solver < handle
@@ -38,6 +36,9 @@ classdef {{ model.name }}_mex_solver < handle
         C_ocp_ext_fun
         cost_ext_fun_type
         cost_ext_fun_type_e
+        N
+        name
+        code_gen_dir
     end % properties
 
 
@@ -52,13 +53,21 @@ classdef {{ model.name }}_mex_solver < handle
             addpath('.')
             obj.cost_ext_fun_type = '{{ cost.cost_ext_fun_type }}';
             obj.cost_ext_fun_type_e = '{{ cost.cost_ext_fun_type_e }}';
+            obj.N = {{ dims.N }};
+            obj.name = '{{ model.name }}';
+            obj.code_gen_dir = pwd();
         end
 
         % destructor
         function delete(obj)
+            disp("delete template...");
+            return_dir = pwd();
+            cd(obj.code_gen_dir);
             if ~isempty(obj.C_ocp)
                 acados_mex_free_{{ model.name }}(obj.C_ocp);
             end
+            cd(return_dir);
+            disp("done.");
         end
 
         % solve
@@ -112,6 +121,101 @@ classdef {{ model.name }}_mex_solver < handle
         end
 
 
+        function [] = store_iterate(varargin)
+            %%%  Stores the current iterate of the ocp solver in a json file.
+            %%% param1: filename: if not set, use model_name + timestamp + '.json'
+            %%% param2: overwrite: if false and filename exists add timestamp to filename
+
+            obj = varargin{1};
+            filename = '';
+            overwrite = false;
+
+            if nargin>=2
+                filename = varargin{2};
+                if ~isa(filename, 'char')
+                    error('filename must be a char vector, use '' ''');
+                end
+            end
+
+            if nargin==3
+                overwrite = varargin{3};
+            end
+
+            if nargin > 3
+                disp('acados_ocp.get: wrong number of input arguments (1 or 2 allowed)');
+            end
+
+            if strcmp(filename,'')
+                filename = [obj.name '_iterate.json'];
+            end
+            if ~overwrite
+                % append timestamp
+                if exist(filename, 'file')
+                    filename = filename(1:end-5);
+                    filename = [filename '_' datestr(now,'yyyy-mm-dd-HH:MM:SS') '.json'];
+                end
+            end
+            filename = fullfile(pwd, filename);
+
+            % get iterate:
+            solution = struct();
+            for i=0:obj.N
+                solution.(['x_' num2str(i)]) = obj.get('x', i);
+                solution.(['lam_' num2str(i)]) = obj.get('lam', i);
+                solution.(['t_' num2str(i)]) = obj.get('t', i);
+                solution.(['sl_' num2str(i)]) = obj.get('sl', i);
+                solution.(['su_' num2str(i)]) = obj.get('su', i);
+            end
+            for i=0:obj.N-1
+                solution.(['z_' num2str(i)]) = obj.get('z', i);
+                solution.(['u_' num2str(i)]) = obj.get('u', i);
+                solution.(['pi_' num2str(i)]) = obj.get('pi', i);
+            end
+
+            acados_folder = getenv('ACADOS_INSTALL_DIR');
+            addpath(fullfile(acados_folder, 'external', 'jsonlab'));
+            savejson('', solution, filename);
+
+            json_string = savejson('', solution, 'ForceRootName', 0);
+
+            fid = fopen(filename, 'w');
+            if fid == -1, error('store_iterate: Cannot create JSON file'); end
+            fwrite(fid, json_string, 'char');
+            fclose(fid);
+
+            disp(['stored current iterate in ' filename]);
+        end
+
+
+        function [] = load_iterate(obj, filename)
+            %%%  Loads the iterate stored in json file with filename into the ocp solver.
+            acados_folder = getenv('ACADOS_INSTALL_DIR');
+            addpath(fullfile(acados_folder, 'external', 'jsonlab'));
+            filename = fullfile(pwd, filename);
+
+            if ~exist(filename, 'file')
+                error(['load_iterate: failed, file does not exist: ' filename])
+            end
+
+            solution = loadjson(filename);
+            keys = fieldnames(solution);
+
+            for k = 1:numel(keys)
+                key = keys{k};
+                key_parts = strsplit(key, '_');
+                field = key_parts{1};
+                stage = key_parts{2};
+
+                val = solution.(key);
+
+                % check if array is empty (can happen for z)
+                if numel(val) > 0
+                    obj.set(field, val, str2num(stage))
+                end
+            end
+        end
+
+
         % print
         function print(varargin)
             if nargin < 2
diff --git a/third_party/acados/acados_template/c_templates_tera/model.in.h b/third_party/acados/acados_template/c_templates_tera/model.in.h
index 918e2bc29e..e5059df9ff 100644
--- a/third_party/acados/acados_template/c_templates_tera/model.in.h
+++ b/third_party/acados/acados_template/c_templates_tera/model.in.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -47,7 +44,8 @@ extern "C" {
 {%- endif %}
 
 {% if solver_options.integrator_type == "IRK" or solver_options.integrator_type == "LIFTED_IRK" %}
-// implicit ODE
+  {% if model.dyn_ext_fun_type == "casadi" %}
+// implicit ODE: function
 int {{ model.name }}_impl_dae_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
 int {{ model.name }}_impl_dae_fun_work(int *, int *, int *, int *);
 const int *{{ model.name }}_impl_dae_fun_sparsity_in(int);
@@ -55,7 +53,7 @@ const int *{{ model.name }}_impl_dae_fun_sparsity_out(int);
 int {{ model.name }}_impl_dae_fun_n_in(void);
 int {{ model.name }}_impl_dae_fun_n_out(void);
 
-// implicit ODE
+// implicit ODE: function + jacobians
 int {{ model.name }}_impl_dae_fun_jac_x_xdot_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
 int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_work(int *, int *, int *, int *);
 const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_in(int);
@@ -63,7 +61,7 @@ const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out(int);
 int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in(void);
 int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out(void);
 
-// implicit ODE
+// implicit ODE: jacobians only
 int {{ model.name }}_impl_dae_jac_x_xdot_u_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
 int {{ model.name }}_impl_dae_jac_x_xdot_u_z_work(int *, int *, int *, int *);
 const int *{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in(int);
@@ -79,14 +77,23 @@ const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_out(int);
 int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_in(void);
 int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_out(void);
 
-{%- if hessian_approx == "EXACT" %}
+	{%- if hessian_approx == "EXACT" %}
+// implicit ODE - hessian
 int {{ model.name }}_impl_dae_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
 int {{ model.name }}_impl_dae_hess_work(int *, int *, int *, int *);
 const int *{{ model.name }}_impl_dae_hess_sparsity_in(int);
 const int *{{ model.name }}_impl_dae_hess_sparsity_out(int);
 int {{ model.name }}_impl_dae_hess_n_in(void);
 int {{ model.name }}_impl_dae_hess_n_out(void);
-{%- endif %}
+	{% endif %}
+  {% else %}{# ext_fun_type #}
+    {%- if hessian_approx == "EXACT" %}
+int {{ model.dyn_impl_dae_hess }}(void **, void **, void *);
+    {% endif %}
+int {{ model.dyn_impl_dae_fun_jac }}(void **, void **, void *);
+int {{ model.dyn_impl_dae_jac }}(void **, void **, void *);
+int {{ model.dyn_impl_dae_fun }}(void **, void **, void *);
+  {% endif %}{# ext_fun_type #}
 
 {% elif solver_options.integrator_type == "GNSF" %}
 /* GNSF Functions */
diff --git a/third_party/acados/acados_template/c_templates_tera/phi_e_constraint.in.h b/third_party/acados/acados_template/c_templates_tera/phi_e_constraint.in.h
deleted file mode 100644
index dc8e293ad7..0000000000
--- a/third_party/acados/acados_template/c_templates_tera/phi_e_constraint.in.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#ifndef {{ model.name }}_PHI_E_CONSTRAINT
-#define {{ model.name }}_PHI_E_CONSTRAINT
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-{% if dims.nphi_e > 0 %}
-int {{ model.name }}_phi_e_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_phi_e_constraint_work(int *, int *, int *, int *);
-const int *{{ model.name }}_phi_e_constraint_sparsity_in(int);
-const int *{{ model.name }}_phi_e_constraint_sparsity_out(int);
-int {{ model.name }}_phi_e_constraint_n_in(void);
-int {{ model.name }}_phi_e_constraint_n_out(void);
-{% endif %}
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#endif  // {{ model.name }}_PHI_E_CONSTRAINT
diff --git a/third_party/acados/acados_template/casadi_function_generation.py b/third_party/acados/acados_template/casadi_function_generation.py
new file mode 100644
index 0000000000..6373a2809d
--- /dev/null
+++ b/third_party/acados/acados_template/casadi_function_generation.py
@@ -0,0 +1,708 @@
+#
+# Copyright (c) The acados authors.
+#
+# 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.;
+#
+
+import os
+import casadi as ca
+from .utils import is_empty, casadi_length
+
+
+def get_casadi_symbol(x):
+    if isinstance(x, ca.MX):
+        return ca.MX.sym
+    elif isinstance(x, ca.SX):
+        return ca.SX.sym
+    else:
+        raise TypeError("Expected casadi SX or MX.")
+
+################
+# Dynamics
+################
+
+
+def generate_c_code_discrete_dynamics( model, opts ):
+
+    casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
+
+    # load model
+    x = model.x
+    u = model.u
+    p = model.p
+    phi = model.disc_dyn_expr
+    model_name = model.name
+    nx = casadi_length(x)
+
+    symbol = get_casadi_symbol(x)
+    # assume nx1 = nx !!!
+    lam = symbol('lam', nx, 1)
+
+    # generate jacobians
+    ux = ca.vertcat(u,x)
+    jac_ux = ca.jacobian(phi, ux)
+    # generate adjoint
+    adj_ux = ca.jtimes(phi, ux, lam, True)
+    # generate hessian
+    hess_ux = ca.jacobian(adj_ux, ux)
+
+    # change directory
+    cwd = os.getcwd()
+    model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model'))
+    if not os.path.exists(model_dir):
+        os.makedirs(model_dir)
+    os.chdir(model_dir)
+
+    # set up & generate ca.Functions
+    fun_name = model_name + '_dyn_disc_phi_fun'
+    phi_fun = ca.Function(fun_name, [x, u, p], [phi])
+    phi_fun.generate(fun_name, casadi_codegen_opts)
+
+    fun_name = model_name + '_dyn_disc_phi_fun_jac'
+    phi_fun_jac_ut_xt = ca.Function(fun_name, [x, u, p], [phi, jac_ux.T])
+    phi_fun_jac_ut_xt.generate(fun_name, casadi_codegen_opts)
+
+    fun_name = model_name + '_dyn_disc_phi_fun_jac_hess'
+    phi_fun_jac_ut_xt_hess = ca.Function(fun_name, [x, u, lam, p], [phi, jac_ux.T, hess_ux])
+    phi_fun_jac_ut_xt_hess.generate(fun_name, casadi_codegen_opts)
+
+    os.chdir(cwd)
+    return
+
+
+
+def generate_c_code_explicit_ode( model, opts ):
+
+    casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
+
+    generate_hess = opts["generate_hess"]
+
+    # load model
+    x = model.x
+    u = model.u
+    p = model.p
+    f_expl = model.f_expl_expr
+    model_name = model.name
+
+    ## get model dimensions
+    nx = x.size()[0]
+    nu = u.size()[0]
+
+    symbol = get_casadi_symbol(x)
+
+    ## set up functions to be exported
+    Sx = symbol('Sx', nx, nx)
+    Sp = symbol('Sp', nx, nu)
+    lambdaX = symbol('lambdaX', nx, 1)
+
+    fun_name = model_name + '_expl_ode_fun'
+
+    ## Set up functions
+    expl_ode_fun = ca.Function(fun_name, [x, u, p], [f_expl])
+
+    vdeX = ca.jtimes(f_expl,x,Sx)
+    vdeP = ca.jacobian(f_expl,u) + ca.jtimes(f_expl,x,Sp)
+
+    fun_name = model_name + '_expl_vde_forw'
+
+    expl_vde_forw = ca.Function(fun_name, [x, Sx, Sp, u, p], [f_expl, vdeX, vdeP])
+
+    adj = ca.jtimes(f_expl, ca.vertcat(x, u), lambdaX, True)
+
+    fun_name = model_name + '_expl_vde_adj'
+    expl_vde_adj = ca.Function(fun_name, [x, lambdaX, u, p], [adj])
+
+    if generate_hess:
+        S_forw = ca.vertcat(ca.horzcat(Sx, Sp), ca.horzcat(ca.DM.zeros(nu,nx), ca.DM.eye(nu)))
+        hess = ca.mtimes(ca.transpose(S_forw),ca.jtimes(adj, ca.vertcat(x,u), S_forw))
+        hess2 = []
+        for j in range(nx+nu):
+            for i in range(j,nx+nu):
+                hess2 = ca.vertcat(hess2, hess[i,j])
+
+        fun_name = model_name + '_expl_ode_hess'
+        expl_ode_hess = ca.Function(fun_name, [x, Sx, Sp, lambdaX, u, p], [adj, hess2])
+
+    # change directory
+    cwd = os.getcwd()
+    model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model'))
+    if not os.path.exists(model_dir):
+        os.makedirs(model_dir)
+    os.chdir(model_dir)
+
+    # generate C code
+    fun_name = model_name + '_expl_ode_fun'
+    expl_ode_fun.generate(fun_name, casadi_codegen_opts)
+
+    fun_name = model_name + '_expl_vde_forw'
+    expl_vde_forw.generate(fun_name, casadi_codegen_opts)
+
+    fun_name = model_name + '_expl_vde_adj'
+    expl_vde_adj.generate(fun_name, casadi_codegen_opts)
+
+    if generate_hess:
+        fun_name = model_name + '_expl_ode_hess'
+        expl_ode_hess.generate(fun_name, casadi_codegen_opts)
+    os.chdir(cwd)
+
+    return
+
+
+def generate_c_code_implicit_ode( model, opts ):
+
+    casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
+
+    # load model
+    x = model.x
+    xdot = model.xdot
+    u = model.u
+    z = model.z
+    p = model.p
+    f_impl = model.f_impl_expr
+    model_name = model.name
+
+    # get model dimensions
+    nx = casadi_length(x)
+    nz = casadi_length(z)
+
+    # generate jacobians
+    jac_x       = ca.jacobian(f_impl, x)
+    jac_xdot    = ca.jacobian(f_impl, xdot)
+    jac_u       = ca.jacobian(f_impl, u)
+    jac_z       = ca.jacobian(f_impl, z)
+
+    # Set up functions
+    p = model.p
+    fun_name = model_name + '_impl_dae_fun'
+    impl_dae_fun = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl])
+
+    fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z'
+    impl_dae_fun_jac_x_xdot_z = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z])
+
+    fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z'
+    impl_dae_fun_jac_x_xdot_u_z = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u, jac_z])
+
+    fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u'
+    impl_dae_fun_jac_x_xdot_u = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u])
+
+    fun_name = model_name + '_impl_dae_jac_x_xdot_u_z'
+    impl_dae_jac_x_xdot_u_z = ca.Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z])
+
+    if opts["generate_hess"]:
+        x_xdot_z_u = ca.vertcat(x, xdot, z, u)
+        symbol = get_casadi_symbol(x)
+        multiplier = symbol('multiplier', nx + nz)
+        ADJ = ca.jtimes(f_impl, x_xdot_z_u, multiplier, True)
+        HESS = ca.jacobian(ADJ, x_xdot_z_u)
+        fun_name = model_name + '_impl_dae_hess'
+        impl_dae_hess = ca.Function(fun_name, [x, xdot, u, z, multiplier, p], [HESS])
+
+    # change directory
+    cwd = os.getcwd()
+    model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model'))
+    if not os.path.exists(model_dir):
+        os.makedirs(model_dir)
+    os.chdir(model_dir)
+
+    # generate C code
+    fun_name = model_name + '_impl_dae_fun'
+    impl_dae_fun.generate(fun_name, casadi_codegen_opts)
+
+    fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z'
+    impl_dae_fun_jac_x_xdot_z.generate(fun_name, casadi_codegen_opts)
+
+    fun_name = model_name + '_impl_dae_jac_x_xdot_u_z'
+    impl_dae_jac_x_xdot_u_z.generate(fun_name, casadi_codegen_opts)
+
+    fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z'
+    impl_dae_fun_jac_x_xdot_u_z.generate(fun_name, casadi_codegen_opts)
+
+    fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u'
+    impl_dae_fun_jac_x_xdot_u.generate(fun_name, casadi_codegen_opts)
+
+    if opts["generate_hess"]:
+        fun_name = model_name + '_impl_dae_hess'
+        impl_dae_hess.generate(fun_name, casadi_codegen_opts)
+
+    os.chdir(cwd)
+    return
+
+
+def generate_c_code_gnsf( model, opts ):
+
+    casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
+
+    model_name = model.name
+
+    # set up directory
+    cwd = os.getcwd()
+    model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model'))
+    if not os.path.exists(model_dir):
+        os.makedirs(model_dir)
+    os.chdir(model_dir)
+
+    # obtain gnsf dimensions
+    get_matrices_fun = model.get_matrices_fun
+    phi_fun = model.phi_fun
+
+    size_gnsf_A = get_matrices_fun.size_out(0)
+    gnsf_nx1 = size_gnsf_A[1]
+    gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
+    gnsf_nuhat = max(phi_fun.size_in(1))
+    gnsf_ny = max(phi_fun.size_in(0))
+    gnsf_nout = max(phi_fun.size_out(0))
+
+    # set up expressions
+    # if the model uses ca.MX because of cost/constraints
+    # the DAE can be exported as ca.SX -> detect GNSF in Matlab
+    # -> evaluated ca.SX GNSF functions with ca.MX.
+    u = model.u
+    symbol = get_casadi_symbol(u)
+
+    y = symbol("y", gnsf_ny, 1)
+    uhat = symbol("uhat", gnsf_nuhat, 1)
+    p = model.p
+    x1 = symbol("gnsf_x1", gnsf_nx1, 1)
+    x1dot = symbol("gnsf_x1dot", gnsf_nx1, 1)
+    z1 = symbol("gnsf_z1", gnsf_nz1, 1)
+    dummy = symbol("gnsf_dummy", 1, 1)
+    empty_var = symbol("gnsf_empty_var", 0, 0)
+
+    ## generate C code
+    fun_name = model_name + '_gnsf_phi_fun'
+    phi_fun_ = ca.Function(fun_name, [y, uhat, p], [phi_fun(y, uhat, p)])
+    phi_fun_.generate(fun_name, casadi_codegen_opts)
+
+    fun_name = model_name + '_gnsf_phi_fun_jac_y'
+    phi_fun_jac_y = model.phi_fun_jac_y
+    phi_fun_jac_y_ = ca.Function(fun_name, [y, uhat, p], phi_fun_jac_y(y, uhat, p))
+    phi_fun_jac_y_.generate(fun_name, casadi_codegen_opts)
+
+    fun_name = model_name + '_gnsf_phi_jac_y_uhat'
+    phi_jac_y_uhat = model.phi_jac_y_uhat
+    phi_jac_y_uhat_ = ca.Function(fun_name, [y, uhat, p], phi_jac_y_uhat(y, uhat, p))
+    phi_jac_y_uhat_.generate(fun_name, casadi_codegen_opts)
+
+    fun_name = model_name + '_gnsf_f_lo_fun_jac_x1k1uz'
+    f_lo_fun_jac_x1k1uz = model.f_lo_fun_jac_x1k1uz
+    f_lo_fun_jac_x1k1uz_eval = f_lo_fun_jac_x1k1uz(x1, x1dot, z1, u, p)
+
+    # avoid codegeneration issue
+    if not isinstance(f_lo_fun_jac_x1k1uz_eval, tuple) and is_empty(f_lo_fun_jac_x1k1uz_eval):
+        f_lo_fun_jac_x1k1uz_eval = [empty_var]
+
+    f_lo_fun_jac_x1k1uz_ = ca.Function(fun_name, [x1, x1dot, z1, u, p],
+                 f_lo_fun_jac_x1k1uz_eval)
+    f_lo_fun_jac_x1k1uz_.generate(fun_name, casadi_codegen_opts)
+
+    fun_name = model_name + '_gnsf_get_matrices_fun'
+    get_matrices_fun_ = ca.Function(fun_name, [dummy], get_matrices_fun(1))
+    get_matrices_fun_.generate(fun_name, casadi_codegen_opts)
+
+    # remove fields for json dump
+    del model.phi_fun
+    del model.phi_fun_jac_y
+    del model.phi_jac_y_uhat
+    del model.f_lo_fun_jac_x1k1uz
+    del model.get_matrices_fun
+
+    os.chdir(cwd)
+
+    return
+
+
+################
+# Cost
+################
+
+def generate_c_code_external_cost(model, stage_type, opts):
+
+    casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
+
+    x = model.x
+    p = model.p
+    u = model.u
+    z = model.z
+    symbol = get_casadi_symbol(x)
+
+    if stage_type == 'terminal':
+        suffix_name = "_cost_ext_cost_e_fun"
+        suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess"
+        suffix_name_jac = "_cost_ext_cost_e_fun_jac"
+        ext_cost = model.cost_expr_ext_cost_e
+        custom_hess = model.cost_expr_ext_cost_custom_hess_e
+        # Last stage cannot depend on u and z
+        u = symbol("u", 0, 0)
+        z = symbol("z", 0, 0)
+
+    elif stage_type == 'path':
+        suffix_name = "_cost_ext_cost_fun"
+        suffix_name_hess = "_cost_ext_cost_fun_jac_hess"
+        suffix_name_jac = "_cost_ext_cost_fun_jac"
+        ext_cost = model.cost_expr_ext_cost
+        custom_hess = model.cost_expr_ext_cost_custom_hess
+
+    elif stage_type == 'initial':
+        suffix_name = "_cost_ext_cost_0_fun"
+        suffix_name_hess = "_cost_ext_cost_0_fun_jac_hess"
+        suffix_name_jac = "_cost_ext_cost_0_fun_jac"
+        ext_cost = model.cost_expr_ext_cost_0
+        custom_hess = model.cost_expr_ext_cost_custom_hess_0
+
+    nunx = x.shape[0] + u.shape[0]
+
+    # set up functions to be exported
+    fun_name = model.name + suffix_name
+    fun_name_hess = model.name + suffix_name_hess
+    fun_name_jac = model.name + suffix_name_jac
+
+    # generate expression for full gradient and Hessian
+    hess_uxz, grad_uxz = ca.hessian(ext_cost, ca.vertcat(u, x, z))
+
+    hess_ux = hess_uxz[:nunx, :nunx]
+    hess_z = hess_uxz[nunx:, nunx:]
+    hess_z_ux = hess_uxz[nunx:, :nunx]
+
+    if custom_hess is not None:
+        hess_ux = custom_hess
+
+    ext_cost_fun = ca.Function(fun_name, [x, u, z, p], [ext_cost])
+
+    ext_cost_fun_jac_hess = ca.Function(
+        fun_name_hess, [x, u, z, p], [ext_cost, grad_uxz, hess_ux, hess_z, hess_z_ux]
+    )
+    ext_cost_fun_jac = ca.Function(
+        fun_name_jac, [x, u, z, p], [ext_cost, grad_uxz]
+    )
+
+    # change directory
+    cwd = os.getcwd()
+    cost_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_cost'))
+    if not os.path.exists(cost_dir):
+        os.makedirs(cost_dir)
+    os.chdir(cost_dir)
+
+    ext_cost_fun.generate(fun_name, casadi_codegen_opts)
+    ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_codegen_opts)
+    ext_cost_fun_jac.generate(fun_name_jac, casadi_codegen_opts)
+
+    os.chdir(cwd)
+    return
+
+
+def generate_c_code_nls_cost( model, cost_name, stage_type, opts ):
+
+    casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
+
+    x = model.x
+    z = model.z
+    p = model.p
+    u = model.u
+
+    symbol = get_casadi_symbol(x)
+
+    if stage_type == 'terminal':
+        middle_name = '_cost_y_e'
+        u = symbol('u', 0, 0)
+        y_expr = model.cost_y_expr_e
+
+    elif stage_type == 'initial':
+        middle_name = '_cost_y_0'
+        y_expr = model.cost_y_expr_0
+
+    elif stage_type == 'path':
+        middle_name = '_cost_y'
+        y_expr = model.cost_y_expr
+
+    # change directory
+    cwd = os.getcwd()
+    cost_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_cost'))
+    if not os.path.exists(cost_dir):
+        os.makedirs(cost_dir)
+    os.chdir(cost_dir)
+
+    # set up expressions
+    cost_jac_expr = ca.transpose(ca.jacobian(y_expr, ca.vertcat(u, x)))
+    dy_dz = ca.jacobian(y_expr, z)
+    ny = casadi_length(y_expr)
+
+    y = symbol('y', ny, 1)
+
+    y_adj = ca.jtimes(y_expr, ca.vertcat(u, x), y, True)
+    y_hess = ca.jacobian(y_adj, ca.vertcat(u, x))
+
+    ## generate C code
+    suffix_name = '_fun'
+    fun_name = cost_name + middle_name + suffix_name
+    y_fun = ca.Function( fun_name, [x, u, z, p], [ y_expr ])
+    y_fun.generate( fun_name, casadi_codegen_opts )
+
+    suffix_name = '_fun_jac_ut_xt'
+    fun_name = cost_name + middle_name + suffix_name
+    y_fun_jac_ut_xt = ca.Function(fun_name, [x, u, z, p], [ y_expr, cost_jac_expr, dy_dz ])
+    y_fun_jac_ut_xt.generate( fun_name, casadi_codegen_opts )
+
+    suffix_name = '_hess'
+    fun_name = cost_name + middle_name + suffix_name
+    y_hess = ca.Function(fun_name, [x, u, z, y, p], [ y_hess ])
+    y_hess.generate( fun_name, casadi_codegen_opts )
+
+    os.chdir(cwd)
+
+    return
+
+
+
+def generate_c_code_conl_cost(model, cost_name, stage_type, opts):
+
+    casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
+
+    x = model.x
+    z = model.z
+    p = model.p
+
+    symbol = get_casadi_symbol(x)
+
+    if stage_type == 'terminal':
+        u = symbol('u', 0, 0)
+
+        yref = model.cost_r_in_psi_expr_e
+        inner_expr = model.cost_y_expr_e - yref
+        outer_expr = model.cost_psi_expr_e
+        res_expr = model.cost_r_in_psi_expr_e
+
+        suffix_name_fun = '_conl_cost_e_fun'
+        suffix_name_fun_jac_hess = '_conl_cost_e_fun_jac_hess'
+
+        custom_hess = model.cost_conl_custom_outer_hess_e
+
+    elif stage_type == 'initial':
+        u = model.u
+
+        yref = model.cost_r_in_psi_expr_0
+        inner_expr = model.cost_y_expr_0 - yref
+        outer_expr = model.cost_psi_expr_0
+        res_expr = model.cost_r_in_psi_expr_0
+
+        suffix_name_fun = '_conl_cost_0_fun'
+        suffix_name_fun_jac_hess = '_conl_cost_0_fun_jac_hess'
+
+        custom_hess = model.cost_conl_custom_outer_hess_0
+
+    elif stage_type == 'path':
+        u = model.u
+
+        yref = model.cost_r_in_psi_expr
+        inner_expr = model.cost_y_expr - yref
+        outer_expr = model.cost_psi_expr
+        res_expr = model.cost_r_in_psi_expr
+
+        suffix_name_fun = '_conl_cost_fun'
+        suffix_name_fun_jac_hess = '_conl_cost_fun_jac_hess'
+
+        custom_hess = model.cost_conl_custom_outer_hess
+
+    # set up function names
+    fun_name_cost_fun = model.name + suffix_name_fun
+    fun_name_cost_fun_jac_hess = model.name + suffix_name_fun_jac_hess
+
+    # set up functions to be exported
+    outer_loss_fun = ca.Function('psi', [res_expr, p], [outer_expr])
+    cost_expr = outer_loss_fun(inner_expr, p)
+
+    outer_loss_grad_fun = ca.Function('outer_loss_grad', [res_expr, p], [ca.jacobian(outer_expr, res_expr).T])
+
+    if custom_hess is None:
+        outer_hess_fun = ca.Function('inner_hess', [res_expr, p], [ca.hessian(outer_loss_fun(res_expr, p), res_expr)[0]])
+    else:
+        outer_hess_fun = ca.Function('inner_hess', [res_expr, p], [custom_hess])
+
+    Jt_ux_expr = ca.jacobian(inner_expr, ca.vertcat(u, x)).T
+    Jt_z_expr = ca.jacobian(inner_expr, z).T
+
+    cost_fun = ca.Function(
+        fun_name_cost_fun,
+        [x, u, z, yref, p],
+        [cost_expr])
+
+    cost_fun_jac_hess = ca.Function(
+        fun_name_cost_fun_jac_hess,
+        [x, u, z, yref, p],
+        [cost_expr, outer_loss_grad_fun(inner_expr, p), Jt_ux_expr, Jt_z_expr, outer_hess_fun(inner_expr, p)]
+    )
+    # change directory
+    cwd = os.getcwd()
+    cost_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_cost'))
+    if not os.path.exists(cost_dir):
+        os.makedirs(cost_dir)
+    os.chdir(cost_dir)
+
+    # generate C code
+    cost_fun.generate(fun_name_cost_fun, casadi_codegen_opts)
+    cost_fun_jac_hess.generate(fun_name_cost_fun_jac_hess, casadi_codegen_opts)
+
+    os.chdir(cwd)
+
+    return
+
+
+################
+# Constraints
+################
+def generate_c_code_constraint( model, con_name, is_terminal, opts ):
+
+    casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
+
+    # load constraint variables and expression
+    x = model.x
+    p = model.p
+
+    symbol = get_casadi_symbol(x)
+
+    if is_terminal:
+        con_h_expr = model.con_h_expr_e
+        con_phi_expr = model.con_phi_expr_e
+        # create dummy u, z
+        u = symbol('u', 0, 0)
+        z = symbol('z', 0, 0)
+    else:
+        con_h_expr = model.con_h_expr
+        con_phi_expr = model.con_phi_expr
+        u = model.u
+        z = model.z
+
+    if (not is_empty(con_h_expr)) and (not is_empty(con_phi_expr)):
+        raise Exception("acados: you can either have constraint_h, or constraint_phi, not both.")
+
+    if (is_empty(con_h_expr) and is_empty(con_phi_expr)):
+        # both empty -> nothing to generate
+        return
+
+    if is_empty(con_h_expr):
+        constr_type = 'BGP'
+    else:
+        constr_type = 'BGH'
+
+    if is_empty(p):
+        p = symbol('p', 0, 0)
+
+    if is_empty(z):
+        z = symbol('z', 0, 0)
+
+    if not (is_empty(con_h_expr)) and opts['generate_hess']:
+        # multipliers for hessian
+        nh = casadi_length(con_h_expr)
+        lam_h = symbol('lam_h', nh, 1)
+
+    # set up & change directory
+    cwd = os.getcwd()
+    constraints_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_constraints'))
+    if not os.path.exists(constraints_dir):
+        os.makedirs(constraints_dir)
+    os.chdir(constraints_dir)
+
+    # export casadi functions
+    if constr_type == 'BGH':
+        if is_terminal:
+            fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt'
+        else:
+            fun_name = con_name + '_constr_h_fun_jac_uxt_zt'
+
+        jac_ux_t = ca.transpose(ca.jacobian(con_h_expr, ca.vertcat(u,x)))
+        jac_z_t = ca.jacobian(con_h_expr, z)
+        constraint_fun_jac_tran = ca.Function(fun_name, [x, u, z, p], \
+                [con_h_expr, jac_ux_t, jac_z_t])
+
+        constraint_fun_jac_tran.generate(fun_name, casadi_codegen_opts)
+        if opts['generate_hess']:
+
+            if is_terminal:
+                fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt_hess'
+            else:
+                fun_name = con_name + '_constr_h_fun_jac_uxt_zt_hess'
+
+            # adjoint
+            adj_ux = ca.jtimes(con_h_expr, ca.vertcat(u, x), lam_h, True)
+            # hessian
+            hess_ux = ca.jacobian(adj_ux, ca.vertcat(u, x))
+
+            adj_z = ca.jtimes(con_h_expr, z, lam_h, True)
+            hess_z = ca.jacobian(adj_z, z)
+
+            # set up functions
+            constraint_fun_jac_tran_hess = \
+                ca.Function(fun_name, [x, u, lam_h, z, p], \
+                    [con_h_expr, jac_ux_t, hess_ux, jac_z_t, hess_z])
+
+            # generate C code
+            constraint_fun_jac_tran_hess.generate(fun_name, casadi_codegen_opts)
+
+        if is_terminal:
+            fun_name = con_name + '_constr_h_e_fun'
+        else:
+            fun_name = con_name + '_constr_h_fun'
+        h_fun = ca.Function(fun_name, [x, u, z, p], [con_h_expr])
+        h_fun.generate(fun_name, casadi_codegen_opts)
+
+    else: # BGP constraint
+        if is_terminal:
+            fun_name = con_name + '_phi_e_constraint'
+            r = model.con_r_in_phi_e
+            con_r_expr = model.con_r_expr_e
+        else:
+            fun_name = con_name + '_phi_constraint'
+            r = model.con_r_in_phi
+            con_r_expr = model.con_r_expr
+
+        nphi = casadi_length(con_phi_expr)
+        con_phi_expr_x_u_z = ca.substitute(con_phi_expr, r, con_r_expr)
+        phi_jac_u = ca.jacobian(con_phi_expr_x_u_z, u)
+        phi_jac_x = ca.jacobian(con_phi_expr_x_u_z, x)
+        phi_jac_z = ca.jacobian(con_phi_expr_x_u_z, z)
+
+        hess = ca.hessian(con_phi_expr[0], r)[0]
+        for i in range(1, nphi):
+            hess = ca.vertcat(hess, ca.hessian(con_phi_expr[i], r)[0])
+
+        r_jac_u = ca.jacobian(con_r_expr, u)
+        r_jac_x = ca.jacobian(con_r_expr, x)
+
+        constraint_phi = \
+            ca.Function(fun_name, [x, u, z, p], \
+                [con_phi_expr_x_u_z, \
+                ca.vertcat(ca.transpose(phi_jac_u), ca.transpose(phi_jac_x)), \
+                ca.transpose(phi_jac_z), \
+                hess,
+                ca.vertcat(ca.transpose(r_jac_u), ca.transpose(r_jac_x))])
+
+        constraint_phi.generate(fun_name, casadi_codegen_opts)
+
+    # change directory back
+    os.chdir(cwd)
+
+    return
+
diff --git a/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c b/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c
new file mode 100644
index 0000000000..b39ff2e23b
--- /dev/null
+++ b/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c
@@ -0,0 +1,819 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * 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.;
+ */
+
+// This is a template based custom_update function
+#include <stdlib.h>
+#include <stdio.h>
+#include <assert.h>
+#include <math.h>
+
+#include "custom_update_function.h"
+#include "acados_solver_{{ model.name }}.h"
+#include "acados_c/ocp_nlp_interface.h"
+#include "acados/utils/mem.h"
+
+#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
+#include "blasfeo/include/blasfeo_d_blasfeo_api.h"
+
+
+typedef struct custom_memory
+{
+    // covariance matrics
+    struct blasfeo_dmat *uncertainty_matrix_buffer;      // shape = (N+1, nx, nx)
+    // covariance matrix of the additive disturbance
+    struct blasfeo_dmat W_mat;                           // shape = (nw, nw)
+    struct blasfeo_dmat unc_jac_G_mat;                   // shape = (nx, nw)
+    struct blasfeo_dmat temp_GW_mat;                     // shape = (nx, nw)
+    struct blasfeo_dmat GWG_mat;                         // shape = (nx, nx)
+    // sensitivity matrices
+    struct blasfeo_dmat A_mat;                           // shape = (nx, nx)
+    struct blasfeo_dmat B_mat;                           // shape = (nx, nu)
+    // matrix in linear constraints
+    struct blasfeo_dmat Cg_mat;                          // shape = (ng, nx)
+    struct blasfeo_dmat Dg_mat;                          // shape = (ng, nu)
+    struct blasfeo_dmat Cg_e_mat;                        // shape = (ng_e, nx)
+    struct blasfeo_dmat dummy_Dgh_e_mat;                 // shape = (ngh_e_max, nu)
+    // matrix in nonlinear constraints
+    struct blasfeo_dmat Ch_mat;                          // shape = (nh, nx)
+    struct blasfeo_dmat Dh_mat;                          // shape = (nh, nu)
+    struct blasfeo_dmat Ch_e_mat;                        // shape = (nh_e, nx)
+    // feedback gain matrix
+    struct blasfeo_dmat K_mat;                           // shape = (nu, nx)
+    // AK = A - B@K
+    struct blasfeo_dmat AK_mat;                          // shape = (nx, nx)
+    // A@P_k
+    struct blasfeo_dmat temp_AP_mat;                     // shape = (nx, nx)
+    // K@P_k, K@P_k@K^T
+    struct blasfeo_dmat temp_KP_mat;                     // shape = (nu, nx)
+    struct blasfeo_dmat temp_KPK_mat;                    // shape = (nu, nu)
+    // C + D @ K, (C + D @ K) @ P_k
+    struct blasfeo_dmat temp_CaDK_mat;                   // shape = (ngh_me_max, nx)
+    struct blasfeo_dmat temp_CaDKmP_mat;                 // shape = (ngh_me_max, nx)
+    struct blasfeo_dmat temp_beta_mat;                   // shape = (ngh_me_max, ngh_me_max)
+
+    double *d_A_mat;                                     // shape = (nx, nx)
+    double *d_B_mat;                                     // shape = (nx, nu)
+    double *d_Cg_mat;                                    // shape = (ng, nx)
+    double *d_Dg_mat;                                    // shape = (ng, nu)
+    double *d_Cg_e_mat;                                  // shape = (ng_e, nx)
+    double *d_Cgh_mat;                                   // shape = (ng+nh, nx)
+    double *d_Dgh_mat;                                   // shape = (ng+nh, nu)
+    double *d_Cgh_e_mat;                                 // shape = (ng_e+nh_e, nx)
+    double *d_state_vec;
+    // upper and lower bounds on state variables
+    double *d_lbx;                                       // shape = (nbx,)
+    double *d_ubx;                                       // shape = (nbx,)
+    double *d_lbx_e;                                     // shape = (nbx_e,)
+    double *d_ubx_e;                                     // shape = (nbx_e,)
+    // tightened upper and lower bounds on state variables
+    double *d_lbx_tightened;                             // shape = (nbx,)
+    double *d_ubx_tightened;                             // shape = (nbx,)
+    double *d_lbx_e_tightened;                           // shape = (nbx_e,)
+    double *d_ubx_e_tightened;                           // shape = (nbx_e,)
+    // upper and lower bounds on control inputs
+    double *d_lbu;                                       // shape = (nbu,)
+    double *d_ubu;                                       // shape = (nbu,)
+    // tightened upper and lower bounds on control inputs
+    double *d_lbu_tightened;                             // shape = (nbu,)
+    double *d_ubu_tightened;                             // shape = (nbu,)
+    // upper and lower bounds on polytopic constraints
+    double *d_lg;                                        // shape = (ng,)
+    double *d_ug;                                        // shape = (ng,)
+    double *d_lg_e;                                      // shape = (ng_e,)
+    double *d_ug_e;                                      // shape = (ng_e,)
+    // tightened lower bounds on polytopic constraints
+    double *d_lg_tightened;                              // shape = (ng,)
+    double *d_ug_tightened;                              // shape = (ng,)
+    double *d_lg_e_tightened;                            // shape = (ng_e,)
+    double *d_ug_e_tightened;                            // shape = (ng_e,)
+    // upper and lower bounds on nonlinear constraints
+    double *d_lh;                                        // shape = (nh,)
+    double *d_uh;                                        // shape = (nh,)
+    double *d_lh_e;                                      // shape = (nh_e,)
+    double *d_uh_e;                                      // shape = (nh_e,)
+    // tightened upper and lower bounds on nonlinear constraints
+    double *d_lh_tightened;                              // shape = (nh,)
+    double *d_uh_tightened;                              // shape = (nh,)
+    double *d_lh_e_tightened;                            // shape = (nh_e,)
+    double *d_uh_e_tightened;                            // shape = (nh_e,)
+
+    int *idxbx;                                          // shape = (nbx,)
+    int *idxbu;                                          // shape = (nbu,)
+    int *idxbx_e;                                        // shape = (nbx_e,)
+
+    void *raw_memory; // Pointer to allocated memory, to be used for freeing
+} custom_memory;
+
+static int int_max(int num1, int num2)
+{
+    return (num1 > num2 ) ? num1 : num2;
+}
+
+
+static int custom_memory_calculate_size(ocp_nlp_config *nlp_config, ocp_nlp_dims *nlp_dims)
+{
+    int N = nlp_dims->N;
+    int nx = {{ dims.nx }};
+    int nu = {{ dims.nu }};
+    int nw = {{ zoro_description.nw }};
+
+    int ng = {{ dims.ng }};
+    int nh = {{ dims.nh }};
+    int nbx = {{ dims.nbx }};
+    int nbu = {{ dims.nbu }};
+
+    int ng_e = {{ dims.ng_e }};
+    int nh_e = {{ dims.nh_e }};
+    int ngh_e_max = int_max(ng_e, nh_e);
+    int ngh_me_max = int_max(ngh_e_max, int_max(ng, nh));
+    int nbx_e = {{ dims.nbx_e }};
+
+    assert({{zoro_description.nlbx_t}} <= nbx);
+    assert({{zoro_description.nubx_t}} <= nbx);
+    assert({{zoro_description.nlbu_t}} <= nbu);
+    assert({{zoro_description.nubu_t}} <= nbu);
+    assert({{zoro_description.nlg_t}} <= ng);
+    assert({{zoro_description.nug_t}} <= ng);
+    assert({{zoro_description.nlh_t}} <= nh);
+    assert({{zoro_description.nuh_t}} <= nh);
+    assert({{zoro_description.nlbx_e_t}} <= nbx_e);
+    assert({{zoro_description.nubx_e_t}} <= nbx_e);
+    assert({{zoro_description.nlg_e_t}} <= ng_e);
+    assert({{zoro_description.nug_e_t}} <= ng_e);
+    assert({{zoro_description.nlh_e_t}} <= nh_e);
+    assert({{zoro_description.nuh_e_t}} <= nh_e);
+
+    acados_size_t size = sizeof(custom_memory);
+    size += nbx * sizeof(int);
+    /* blasfeo structs */
+    size += (N + 1) * sizeof(struct blasfeo_dmat);
+    /* blasfeo mem: mat */
+    size += (N + 1) * blasfeo_memsize_dmat(nx, nx); // uncertainty_matrix_buffer
+    size += blasfeo_memsize_dmat(nw, nw);           // W_mat
+    size += 2 * blasfeo_memsize_dmat(nx, nw);       // unc_jac_G_mat, temp_GW_mat
+    size += 4 * blasfeo_memsize_dmat(nx, nx);       // GWG_mat, A_mat, AK_mat, temp_AP_mat
+    size += blasfeo_memsize_dmat(nx, nu);           // B_mat
+    size += 2 * blasfeo_memsize_dmat(nu, nx);       // K_mat, temp_KP_mat
+    size += blasfeo_memsize_dmat(nu, nu);           // temp_KPK_mat
+    size += blasfeo_memsize_dmat(ng, nx);           // Cg_mat
+    size += blasfeo_memsize_dmat(ng, nu);           // Dg_mat
+    size += blasfeo_memsize_dmat(ng_e, nx);         // Cg_e_mat
+    size += blasfeo_memsize_dmat(ngh_e_max, nu);    // dummy_Dgh_e_mat
+    size += blasfeo_memsize_dmat(nh, nx);           // Ch_mat
+    size += blasfeo_memsize_dmat(nh, nu);           // Dh_mat
+    size += blasfeo_memsize_dmat(nh_e, nx);         // Ch_e_mat
+    size += 2 * blasfeo_memsize_dmat(ngh_me_max, nx);           // temp_CaDK_mat, temp_CaDKmP_mat
+    size += blasfeo_memsize_dmat(ngh_me_max, ngh_me_max);       // temp_beta_mat
+    /* blasfeo mem: vec */
+    /* Arrays */
+    size += nx*nx *sizeof(double);                  // d_A_mat
+    size += nx*nu *sizeof(double);                  // d_B_mat
+    size += (ng + ng_e) * nx * sizeof(double);      // d_Cg_mat, d_Cg_e_mat
+    size += (ng) * nu * sizeof(double);             // d_Dg_mat
+    size += (nh + nh_e + ng + ng_e) * nx * sizeof(double);      // d_Cgh_mat, d_Cgh_e_mat
+    size += (nh + ng) * nu * sizeof(double);        // d_Dgh_mat
+    // d_state_vec
+    size += nx *sizeof(double);
+    // constraints and tightened constraints
+    size += 4 * (nbx + nbu + ng + nh)*sizeof(double);
+    size += 4 * (nbx_e + ng_e + nh_e)*sizeof(double);
+    size += (nbx + nbu + nbx_e)*sizeof(int);        // idxbx, idxbu, idxbx_e
+
+    size += 1 * 8; // initial alignment
+    make_int_multiple_of(64, &size);
+    size += 1 * 64;
+
+    return size;
+}
+
+
+static custom_memory *custom_memory_assign(ocp_nlp_config *nlp_config, ocp_nlp_dims *nlp_dims, void *raw_memory)
+{
+    int N = nlp_dims->N;
+    int nx = {{ dims.nx }};
+    int nu = {{ dims.nu }};
+    int nw = {{ zoro_description.nw }};
+
+    int ng = {{ dims.ng }};
+    int nh = {{ dims.nh }};
+    int nbx = {{ dims.nbx }};
+    int nbu = {{ dims.nbu }};
+
+    int ng_e = {{ dims.ng_e }};
+    int nh_e = {{ dims.nh_e }};
+    int ngh_e_max = int_max(ng_e, nh_e);
+    int ngh_me_max = int_max(ngh_e_max, int_max(ng, nh));
+    int nbx_e = {{ dims.nbx_e }};
+
+    char *c_ptr = (char *) raw_memory;
+    custom_memory *mem = (custom_memory *) c_ptr;
+    c_ptr += sizeof(custom_memory);
+
+    align_char_to(8, &c_ptr);
+    assign_and_advance_blasfeo_dmat_structs(N+1, &mem->uncertainty_matrix_buffer, &c_ptr);
+
+    align_char_to(64, &c_ptr);
+
+    for (int ii = 0; ii <= N; ii++)
+    {
+        assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->uncertainty_matrix_buffer[ii], &c_ptr);
+    }
+    // Disturbance Dynamics
+    assign_and_advance_blasfeo_dmat_mem(nw, nw, &mem->W_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nx, nw, &mem->unc_jac_G_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nx, nw, &mem->temp_GW_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->GWG_mat, &c_ptr);
+    // System Dynamics
+    assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->A_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nx, nu, &mem->B_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(ng, nx, &mem->Cg_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(ng, nu, &mem->Dg_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(ng_e, nx, &mem->Cg_e_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(ngh_e_max, nu, &mem->dummy_Dgh_e_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nh, nx, &mem->Ch_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nh, nu, &mem->Dh_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nh_e, nx, &mem->Ch_e_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nu, nx, &mem->K_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->AK_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->temp_AP_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nu, nx, &mem->temp_KP_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nu, nu, &mem->temp_KPK_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(ngh_me_max, nx, &mem->temp_CaDK_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(ngh_me_max, nx, &mem->temp_CaDKmP_mat, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(ngh_me_max, ngh_me_max, &mem->temp_beta_mat, &c_ptr);
+
+    assign_and_advance_double(nx*nx, &mem->d_A_mat, &c_ptr);
+    assign_and_advance_double(nx*nu, &mem->d_B_mat, &c_ptr);
+    assign_and_advance_double(ng*nx, &mem->d_Cg_mat, &c_ptr);
+    assign_and_advance_double(ng*nu, &mem->d_Dg_mat, &c_ptr);
+    assign_and_advance_double(ng_e*nx, &mem->d_Cg_e_mat, &c_ptr);
+    assign_and_advance_double((ng + nh)*nx, &mem->d_Cgh_mat, &c_ptr);
+    assign_and_advance_double((ng + nh)*nu, &mem->d_Dgh_mat, &c_ptr);
+    assign_and_advance_double((ng_e + nh_e)*nx, &mem->d_Cgh_e_mat, &c_ptr);
+    assign_and_advance_double(nx, &mem->d_state_vec, &c_ptr);
+    assign_and_advance_double(nbx, &mem->d_lbx, &c_ptr);
+    assign_and_advance_double(nbx, &mem->d_ubx, &c_ptr);
+    assign_and_advance_double(nbx_e, &mem->d_lbx_e, &c_ptr);
+    assign_and_advance_double(nbx_e, &mem->d_ubx_e, &c_ptr);
+    assign_and_advance_double(nbx, &mem->d_lbx_tightened, &c_ptr);
+    assign_and_advance_double(nbx, &mem->d_ubx_tightened, &c_ptr);
+    assign_and_advance_double(nbx_e, &mem->d_lbx_e_tightened, &c_ptr);
+    assign_and_advance_double(nbx_e, &mem->d_ubx_e_tightened, &c_ptr);
+    assign_and_advance_double(nbu, &mem->d_lbu, &c_ptr);
+    assign_and_advance_double(nbu, &mem->d_ubu, &c_ptr);
+    assign_and_advance_double(nbu, &mem->d_lbu_tightened, &c_ptr);
+    assign_and_advance_double(nbu, &mem->d_ubu_tightened, &c_ptr);
+    assign_and_advance_double(ng, &mem->d_lg, &c_ptr);
+    assign_and_advance_double(ng, &mem->d_ug, &c_ptr);
+    assign_and_advance_double(ng_e, &mem->d_lg_e, &c_ptr);
+    assign_and_advance_double(ng_e, &mem->d_ug_e, &c_ptr);
+    assign_and_advance_double(ng, &mem->d_lg_tightened, &c_ptr);
+    assign_and_advance_double(ng, &mem->d_ug_tightened, &c_ptr);
+    assign_and_advance_double(ng_e, &mem->d_lg_e_tightened, &c_ptr);
+    assign_and_advance_double(ng_e, &mem->d_ug_e_tightened, &c_ptr);
+    assign_and_advance_double(nh, &mem->d_lh, &c_ptr);
+    assign_and_advance_double(nh, &mem->d_uh, &c_ptr);
+    assign_and_advance_double(nh_e, &mem->d_lh_e, &c_ptr);
+    assign_and_advance_double(nh_e, &mem->d_uh_e, &c_ptr);
+    assign_and_advance_double(nh, &mem->d_lh_tightened, &c_ptr);
+    assign_and_advance_double(nh, &mem->d_uh_tightened, &c_ptr);
+    assign_and_advance_double(nh_e, &mem->d_lh_e_tightened, &c_ptr);
+    assign_and_advance_double(nh_e, &mem->d_uh_e_tightened, &c_ptr);
+
+    assign_and_advance_int(nbx, &mem->idxbx, &c_ptr);
+    assign_and_advance_int(nbu, &mem->idxbu, &c_ptr);
+    assign_and_advance_int(nbx_e, &mem->idxbx_e, &c_ptr);
+
+    assert((char *) raw_memory + custom_memory_calculate_size(nlp_config, nlp_dims) >= c_ptr);
+    mem->raw_memory = raw_memory;
+
+    return mem;
+}
+
+
+
+static void *custom_memory_create({{ model.name }}_solver_capsule* capsule)
+{
+    printf("\nin custom_memory_create_function\n");
+
+    ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule);
+    ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(capsule);
+    acados_size_t bytes = custom_memory_calculate_size(nlp_config, nlp_dims);
+
+    void *ptr = acados_calloc(1, bytes);
+
+    custom_memory *custom_mem = custom_memory_assign(nlp_config, nlp_dims, ptr);
+    custom_mem->raw_memory = ptr;
+
+    return custom_mem;
+}
+
+
+static void custom_val_init_function(ocp_nlp_dims *nlp_dims, ocp_nlp_in *nlp_in, ocp_nlp_solver *nlp_solver, custom_memory *custom_mem)
+{
+    int N = nlp_dims->N;
+    int nx = {{ dims.nx }};
+    int nu = {{ dims.nu }};
+    int nw = {{ zoro_description.nw }};
+
+    int ng = {{ dims.ng }};
+    int nh = {{ dims.nh }};
+    int nbx = {{ dims.nbx }};
+    int nbu = {{ dims.nbu }};
+
+    int ng_e = {{ dims.ng_e }};
+    int nh_e = {{ dims.nh_e }};
+    int ngh_e_max = int_max(ng_e, nh_e);
+    int nbx_e = {{ dims.nbx_e }};
+
+    /* Get the state constraint bounds */
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "idxbx", custom_mem->idxbx);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "idxbx", custom_mem->idxbx_e);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbx", custom_mem->d_lbx);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubx", custom_mem->d_ubx);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lbx", custom_mem->d_lbx_e);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ubx", custom_mem->d_ubx_e);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "idxbu", custom_mem->idxbu);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbu", custom_mem->d_lbu);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubu", custom_mem->d_ubu);
+    // Get the Jacobians and the bounds of the linear constraints
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lg", custom_mem->d_lg);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ug", custom_mem->d_ug);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lg", custom_mem->d_lg_e);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ug", custom_mem->d_ug_e);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "C", custom_mem->d_Cg_mat);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "D", custom_mem->d_Dg_mat);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "C", custom_mem->d_Cg_e_mat);
+    blasfeo_pack_dmat(ng, nx, custom_mem->d_Cg_mat, ng, &custom_mem->Cg_mat, 0, 0);
+    blasfeo_pack_dmat(ng, nu, custom_mem->d_Dg_mat, ng, &custom_mem->Dg_mat, 0, 0);
+    blasfeo_pack_dmat(ng_e, nx, custom_mem->d_Cg_e_mat, ng_e, &custom_mem->Cg_e_mat, 0, 0);
+    blasfeo_dgese(ngh_e_max, nu, 0., &custom_mem->dummy_Dgh_e_mat, 0, 0); //fill with zeros
+    // NOTE: fixed lower and upper bounds of nonlinear constraints
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lh", custom_mem->d_lh);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "uh", custom_mem->d_uh);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lh", custom_mem->d_lh_e);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "uh", custom_mem->d_uh_e);
+
+    /* Initilize tightened constraints*/
+    // NOTE: tightened constraints are only initialized once
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbx", custom_mem->d_lbx_tightened);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubx", custom_mem->d_ubx_tightened);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lbx", custom_mem->d_lbx_e_tightened);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ubx", custom_mem->d_ubx_e_tightened);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbu", custom_mem->d_lbu_tightened);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubu", custom_mem->d_ubu_tightened);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lg", custom_mem->d_lg_tightened);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ug", custom_mem->d_ug_tightened);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lg", custom_mem->d_lg_e_tightened);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ug", custom_mem->d_ug_e_tightened);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lh", custom_mem->d_lh_tightened);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "uh", custom_mem->d_uh_tightened);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lh", custom_mem->d_lh_e_tightened);
+    ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "uh", custom_mem->d_uh_e_tightened);
+
+    /* Initialize the W matrix */
+    // blasfeo_dgese(nw, nw, 0., &custom_mem->W_mat, 0, 0);
+{%- for ir in range(end=zoro_description.nw) %}
+    {%- for ic in range(end=zoro_description.nw) %}
+    blasfeo_dgein1({{zoro_description.W_mat[ir][ic]}}, &custom_mem->W_mat, {{ir}}, {{ic}});
+    {%- endfor %}
+{%- endfor %}
+
+{%- for ir in range(end=dims.nx) %}
+    {%- for ic in range(end=zoro_description.nw) %}
+    blasfeo_dgein1({{zoro_description.unc_jac_G_mat[ir][ic]}}, &custom_mem->unc_jac_G_mat, {{ir}}, {{ic}});
+    {%- endfor %}
+{%- endfor %}
+
+    // NOTE: if G is changing this is not in init!
+    // temp_GW_mat = unc_jac_G_mat * W_mat
+    blasfeo_dgemm_nn(nx, nw, nw, 1.0, &custom_mem->unc_jac_G_mat, 0, 0,
+                        &custom_mem->W_mat, 0, 0, 0.0,
+                        &custom_mem->temp_GW_mat, 0, 0, &custom_mem->temp_GW_mat, 0, 0);
+    // GWG_mat = temp_GW_mat * unc_jac_G_mat^T
+    blasfeo_dgemm_nt(nx, nx, nw, 1.0, &custom_mem->temp_GW_mat, 0, 0,
+                        &custom_mem->unc_jac_G_mat, 0, 0, 0.0,
+                        &custom_mem->GWG_mat, 0, 0, &custom_mem->GWG_mat, 0, 0);
+
+    /* Initialize the uncertainty_matrix_buffer[0] */
+{%- for ir in range(end=dims.nx) %}
+    {%- for ic in range(end=dims.nx) %}
+    blasfeo_dgein1({{zoro_description.P0_mat[ir][ic]}}, &custom_mem->uncertainty_matrix_buffer[0], {{ir}}, {{ic}});
+    {%- endfor %}
+{%- endfor %}
+
+    /* Initialize the feedback gain matrix */
+{%- for ir in range(end=dims.nu) %}
+    {%- for ic in range(end=dims.nx) %}
+    blasfeo_dgein1({{zoro_description.fdbk_K_mat[ir][ic]}}, &custom_mem->K_mat, {{ir}}, {{ic}});
+    {%- endfor %}
+{%- endfor %}
+}
+
+
+int custom_update_init_function({{ model.name }}_solver_capsule* capsule)
+{
+    capsule->custom_update_memory = custom_memory_create(capsule);
+    ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(capsule);
+
+    ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule);
+    ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(capsule);
+    custom_val_init_function(nlp_dims, nlp_in, nlp_solver, capsule->custom_update_memory);
+    return 1;
+}
+
+static void compute_gh_beta(struct blasfeo_dmat* K_mat, struct blasfeo_dmat* C_mat,
+                         struct blasfeo_dmat* D_mat, struct blasfeo_dmat* CaDK_mat,
+                         struct blasfeo_dmat* CaDKmP_mat, struct blasfeo_dmat* beta_mat,
+                         struct blasfeo_dmat* P_mat,
+                         int n_cstr, int nx, int nu)
+{
+    // (C+DK)@P@(C^T+K^TD^T)
+    // CaDK_mat = C_mat + D_mat @ K_mat
+    blasfeo_dgemm_nn(n_cstr, nx, nu, 1.0, D_mat, 0, 0,
+                        K_mat, 0, 0, 1.0,
+                        C_mat, 0, 0, CaDK_mat, 0, 0);
+    // CaDKmP_mat = CaDK_mat @ P_mat
+    blasfeo_dgemm_nn(n_cstr, nx, nx, 1.0, CaDK_mat, 0, 0,
+                        P_mat, 0, 0, 0.0,
+                        CaDKmP_mat, 0, 0, CaDKmP_mat, 0, 0);
+    // beta_mat = CaDKmP_mat @ CaDK_mat^T
+    blasfeo_dgemm_nt(n_cstr, n_cstr, nx, 1.0, CaDKmP_mat, 0, 0,
+                        CaDK_mat, 0, 0, 0.0,
+                        beta_mat, 0, 0, beta_mat, 0, 0);
+}
+
+static void compute_KPK(struct blasfeo_dmat* K_mat, struct blasfeo_dmat* temp_KP_mat,
+                        struct blasfeo_dmat* temp_KPK_mat, struct blasfeo_dmat* P_mat,
+                        int nx, int nu)
+{
+    // K @ P_k @ K^T
+    // temp_KP_mat = K_mat @ P_mat
+    blasfeo_dgemm_nn(nu, nx, nx, 1.0, K_mat, 0, 0,
+                     P_mat, 0, 0, 0.0,
+                     temp_KP_mat, 0, 0, temp_KP_mat, 0, 0);
+    // temp_KPK_mat = temp_KP_mat @ K_mat^T
+    blasfeo_dgemm_nt(nu, nu, nx, 1.0, temp_KP_mat, 0, 0,
+                     K_mat, 0, 0, 0.0,
+                     temp_KPK_mat, 0, 0, temp_KPK_mat, 0, 0);
+}
+
+static void compute_next_P_matrix(struct blasfeo_dmat* P_mat, struct blasfeo_dmat* P_next_mat,
+                                  struct blasfeo_dmat* A_mat, struct blasfeo_dmat* B_mat,
+                                  struct blasfeo_dmat* K_mat, struct blasfeo_dmat* W_mat,
+                                  struct blasfeo_dmat* AK_mat, struct blasfeo_dmat* temp_AP_mat, int nx, int nu)
+{
+    // AK_mat = -B@K + A
+    blasfeo_dgemm_nn(nx, nx, nu, -1.0, B_mat, 0, 0, K_mat, 0, 0,
+                        1.0, A_mat, 0, 0, AK_mat, 0, 0);
+    // temp_AP_mat = AK_mat @ P_k
+    blasfeo_dgemm_nn(nx, nx, nx, 1.0, AK_mat, 0, 0,
+                        P_mat, 0, 0, 0.0,
+                        temp_AP_mat, 0, 0, temp_AP_mat, 0, 0);
+    // P_{k+1} = temp_AP_mat @ AK_mat^T + GWG_mat
+    blasfeo_dgemm_nt(nx, nx, nx, 1.0, temp_AP_mat, 0, 0,
+                        AK_mat, 0, 0, 1.0,
+                        W_mat, 0, 0, P_next_mat, 0, 0);
+}
+
+static void reset_P0_matrix(ocp_nlp_dims *nlp_dims, struct blasfeo_dmat* P_mat, double* data)
+{
+    int nx = nlp_dims->nx[0];
+    blasfeo_pack_dmat(nx, nx, data, nx, P_mat, 0, 0);
+}
+
+static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out, custom_memory *custom_mem)
+{
+    ocp_nlp_config *nlp_config = solver->config;
+    ocp_nlp_dims *nlp_dims = solver->dims;
+
+    int N = nlp_dims->N;
+    int nx = nlp_dims->nx[0];
+    int nu = nlp_dims->nu[0];
+    int nx_sqr = nx*nx;
+    int nbx = {{ dims.nbx }};
+    int nbu = {{ dims.nbu }};
+    int ng = {{ dims.ng }};
+    int nh = {{ dims.nh }};
+    int ng_e = {{ dims.ng_e }};
+    int nh_e = {{ dims.nh_e }};
+    int nbx_e = {{ dims.nbx_e }};
+    double backoff_scaling_gamma = {{ zoro_description.backoff_scaling_gamma }};
+
+    // First Stage
+    // NOTE: lbx_0 and ubx_0 should not be tightened.
+    // NOTE: lg_0 and ug_0 are not tightened.
+    // NOTE: lh_0 and uh_0 are not tightened.
+{%- if zoro_description.nlbu_t + zoro_description.nubu_t > 0 %}
+    compute_KPK(&custom_mem->K_mat, &custom_mem->temp_KP_mat,
+                &custom_mem->temp_KPK_mat, &(custom_mem->uncertainty_matrix_buffer[0]), nx, nu);
+
+{%- if zoro_description.nlbu_t > 0 %}
+    // backoff lbu
+    {%- for it in zoro_description.idx_lbu_t %}
+    custom_mem->d_lbu_tightened[{{it}}]
+        = custom_mem->d_lbu[{{it}}]
+            + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat,
+                custom_mem->idxbu[{{it}}],custom_mem->idxbu[{{it}}]));
+    {%- endfor %}
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbu", custom_mem->d_lbu_tightened);
+{%- endif %}
+{%- if zoro_description.nubu_t > 0 %}
+    // backoff ubu
+    {%- for it in zoro_description.idx_ubu_t %}
+    custom_mem->d_ubu_tightened[{{it}}]
+        = custom_mem->d_ubu[{{it}}]
+            - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat,
+                custom_mem->idxbu[{{it}}],custom_mem->idxbu[{{it}}]));
+    {%- endfor %}
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubu", custom_mem->d_ubu_tightened);
+{%- endif %}
+{%- endif %}
+    // Middle Stages
+    // constraint tightening: for next stage based on dynamics of ii stage
+    // P[ii+1] = (A-B@K) @ P[ii] @ (A-B@K).T + G@W@G.T
+    for (int ii = 0; ii < N-1; ii++)
+    {
+        // get and pack: A, B
+        ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, ii, "A", custom_mem->d_A_mat);
+        blasfeo_pack_dmat(nx, nx, custom_mem->d_A_mat, nx, &custom_mem->A_mat, 0, 0);
+        ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, ii, "B", custom_mem->d_B_mat);
+        blasfeo_pack_dmat(nx, nu, custom_mem->d_B_mat, nx, &custom_mem->B_mat, 0, 0);
+
+        compute_next_P_matrix(&(custom_mem->uncertainty_matrix_buffer[ii]),
+                              &(custom_mem->uncertainty_matrix_buffer[ii+1]),
+                              &custom_mem->A_mat, &custom_mem->B_mat,
+                              &custom_mem->K_mat, &custom_mem->GWG_mat,
+                              &custom_mem->AK_mat, &custom_mem->temp_AP_mat, nx, nu);
+
+        // state constraints
+{%- if zoro_description.nlbx_t + zoro_description.nubx_t> 0 %}
+    {%- if zoro_description.nlbx_t > 0 %}
+        // lbx
+        {%- for it in zoro_description.idx_lbx_t %}
+        custom_mem->d_lbx_tightened[{{it}}]
+            = custom_mem->d_lbx[{{it}}]
+                + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[ii+1],
+                    custom_mem->idxbx[{{it}}],custom_mem->idxbx[{{it}}]));
+        {%- endfor %}
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lbx", custom_mem->d_lbx_tightened);
+    {%- endif %}
+    {% if zoro_description.nubx_t > 0 %}
+        // ubx
+        {%- for it in zoro_description.idx_ubx_t %}
+        custom_mem->d_ubx_tightened[{{it}}] = custom_mem->d_ubx[{{it}}]
+                - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[ii+1],
+                    custom_mem->idxbx[{{it}}],custom_mem->idxbx[{{it}}]));
+        {%- endfor %}
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ubx", custom_mem->d_ubx_tightened);
+    {%- endif %}
+{%- endif %}
+
+{%- if zoro_description.nlbu_t + zoro_description.nubu_t > 0 %}
+        // input constraints
+        compute_KPK(&custom_mem->K_mat, &custom_mem->temp_KP_mat,
+            &custom_mem->temp_KPK_mat, &(custom_mem->uncertainty_matrix_buffer[ii+1]), nx, nu);
+
+    {%- if zoro_description.nlbu_t > 0 %}
+        {%- for it in zoro_description.idx_lbu_t %}
+        custom_mem->d_lbu_tightened[{{it}}] = custom_mem->d_lbu[{{it}}]
+                + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat,
+                    custom_mem->idxbu[{{it}}], custom_mem->idxbu[{{it}}]));
+        {%- endfor %}
+
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lbu", custom_mem->d_lbu_tightened);
+    {%- endif %}
+    {%- if zoro_description.nubu_t > 0 %}
+        {%- for it in zoro_description.idx_ubu_t %}
+        custom_mem->d_ubu_tightened[{{it}}] = custom_mem->d_ubu[{{it}}]
+                - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat,
+                    custom_mem->idxbu[{{it}}], custom_mem->idxbu[{{it}}]));
+        {%- endfor %}
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ubu", custom_mem->d_ubu_tightened);
+    {%- endif %}
+{%- endif %}
+
+{%- if zoro_description.nlg_t + zoro_description.nug_t > 0 %}
+        // Linear constraints: g
+        compute_gh_beta(&custom_mem->K_mat, &custom_mem->Cg_mat,
+                     &custom_mem->Dg_mat, &custom_mem->temp_CaDK_mat,
+                     &custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat,
+                     &custom_mem->uncertainty_matrix_buffer[ii+1], ng, nx, nu);
+
+    {%- if zoro_description.nlg_t > 0 %}
+        {%- for it in zoro_description.idx_lg_t %}
+        custom_mem->d_lg_tightened[{{it}}]
+            = custom_mem->d_lg[{{it}}]
+                + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
+        {%- endfor %}
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lg", custom_mem->d_lg_tightened);
+    {%- endif %}
+    {%- if zoro_description.nug_t > 0 %}
+        {%- for it in zoro_description.idx_ug_t %}
+        custom_mem->d_ug_tightened[{{it}}]
+            = custom_mem->d_ug[{{it}}]
+                - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
+        {%- endfor %}
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ug", custom_mem->d_ug_tightened);
+    {%- endif %}
+{%- endif %}
+
+
+{%- if zoro_description.nlh_t + zoro_description.nuh_t > 0 %}
+        // nonlinear constraints: h
+        // Get C_{k+1} and D_{k+1}
+        ocp_nlp_get_at_stage(solver->config, nlp_dims, solver, ii+1, "C", custom_mem->d_Cgh_mat);
+        ocp_nlp_get_at_stage(solver->config, nlp_dims, solver, ii+1, "D", custom_mem->d_Dgh_mat);
+        // NOTE: the d_Cgh_mat is column-major, the first ng rows are the Jacobians of the linear constraints
+        blasfeo_pack_dmat(nh, nx, custom_mem->d_Cgh_mat+ng, ng+nh, &custom_mem->Ch_mat, 0, 0);
+        blasfeo_pack_dmat(nh, nu, custom_mem->d_Dgh_mat+ng, ng+nh, &custom_mem->Dh_mat, 0, 0);
+
+        compute_gh_beta(&custom_mem->K_mat, &custom_mem->Ch_mat,
+                     &custom_mem->Dh_mat, &custom_mem->temp_CaDK_mat,
+                     &custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat,
+                     &custom_mem->uncertainty_matrix_buffer[ii+1], nh, nx, nu);
+
+    {%- if zoro_description.nlh_t > 0 %}
+        {%- for it in zoro_description.idx_lh_t %}
+        custom_mem->d_lh_tightened[{{it}}]
+            = custom_mem->d_lh[{{it}}]
+                + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
+        {%- endfor %}
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lh", custom_mem->d_lh_tightened);
+    {%- endif %}
+    {%- if zoro_description.nuh_t > 0 %}
+        {%- for it in zoro_description.idx_uh_t %}
+        custom_mem->d_uh_tightened[{{it}}] = custom_mem->d_uh[{{it}}]
+                        - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
+        {%- endfor %}
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "uh", custom_mem->d_uh_tightened);
+    {%- endif %}
+{%- endif %}
+    }
+
+    // Last stage
+    // get and pack: A, B
+    ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, N-1, "A", custom_mem->d_A_mat);
+    blasfeo_pack_dmat(nx, nx, custom_mem->d_A_mat, nx, &custom_mem->A_mat, 0, 0);
+    ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, N-1, "B", custom_mem->d_B_mat);
+    blasfeo_pack_dmat(nx, nu, custom_mem->d_B_mat, nx, &custom_mem->B_mat, 0, 0);
+    // AK_mat = -B*K + A
+    compute_next_P_matrix(&(custom_mem->uncertainty_matrix_buffer[N-1]),
+                        &(custom_mem->uncertainty_matrix_buffer[N]),
+                        &custom_mem->A_mat, &custom_mem->B_mat,
+                        &custom_mem->K_mat, &custom_mem->GWG_mat,
+                        &custom_mem->AK_mat, &custom_mem->temp_AP_mat, nx, nu);
+
+    // state constraints nlbx_e_t
+{%- if zoro_description.nlbx_e_t + zoro_description.nubx_e_t> 0 %}
+{%- if zoro_description.nlbx_e_t > 0 %}
+    // lbx_e
+    {%- for it in zoro_description.idx_lbx_e_t %}
+    custom_mem->d_lbx_e_tightened[{{it}}]
+        = custom_mem->d_lbx_e[{{it}}]
+            + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[N],
+                custom_mem->idxbx_e[{{it}}],custom_mem->idxbx_e[{{it}}]));
+    {%- endfor %}
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", custom_mem->d_lbx_e_tightened);
+{%- endif %}
+{% if zoro_description.nubx_e_t > 0 %}
+    // ubx_e
+    {%- for it in zoro_description.idx_ubx_e_t %}
+    custom_mem->d_ubx_e_tightened[{{it}}] = custom_mem->d_ubx_e[{{it}}]
+            - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[N],
+                custom_mem->idxbx_e[{{it}}],custom_mem->idxbx_e[{{it}}]));
+    {%- endfor %}
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", custom_mem->d_ubx_e_tightened);
+{%- endif %}
+{%- endif %}
+
+{%- if zoro_description.nlg_e_t + zoro_description.nug_e_t > 0 %}
+    // Linear constraints: g
+    compute_gh_beta(&custom_mem->K_mat, &custom_mem->Cg_mat,
+                    &custom_mem->dummy_Dgh_e_mat, &custom_mem->temp_CaDK_mat,
+                    &custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat,
+                    &custom_mem->uncertainty_matrix_buffer[N], ng, nx, nu);
+
+{%- if zoro_description.nlg_e_t > 0 %}
+    {%- for it in zoro_description.idx_lg_e_t %}
+    custom_mem->d_lg_e_tightened[{{it}}]
+        = custom_mem->d_lg_e[{{it}}]
+            + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
+    {%- endfor %}
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lg", custom_mem->d_lg_e_tightened);
+{%- endif %}
+{%- if zoro_description.nug_e_t > 0 %}
+    {%- for it in zoro_description.idx_ug_e_t %}
+    custom_mem->d_ug_e_tightened[{{it}}]
+        = custom_mem->d_ug_e[{{it}}]
+            - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
+    {%- endfor %}
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ug", custom_mem->d_ug_e_tightened);
+{%- endif %}
+{%- endif %}
+
+
+{%- if zoro_description.nlh_e_t + zoro_description.nuh_e_t > 0 %}
+    // nonlinear constraints: h
+    // Get C_{k+1} and D_{k+1}
+    ocp_nlp_get_at_stage(solver->config, nlp_dims, solver, N, "C", custom_mem->d_Cgh_mat);
+    // NOTE: the d_Cgh_mat is column-major, the first ng rows are the Jacobians of the linear constraints
+    blasfeo_pack_dmat(nh, nx, custom_mem->d_Cgh_mat+ng, ng+nh, &custom_mem->Ch_mat, 0, 0);
+
+    compute_gh_beta(&custom_mem->K_mat, &custom_mem->Ch_mat,
+                    &custom_mem->dummy_Dgh_e_mat, &custom_mem->temp_CaDK_mat,
+                    &custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat,
+                    &custom_mem->uncertainty_matrix_buffer[N], nh, nx, nu);
+
+    {%- if zoro_description.nlh_e_t > 0 %}
+        {%- for it in zoro_description.idx_lh_e_t %}
+        custom_mem->d_lh_e_tightened[{{it}}]
+            = custom_mem->d_lh_e[{{it}}]
+                + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
+        {%- endfor %}
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", custom_mem->d_lh_e_tightened);
+    {%- endif %}
+    {%- if zoro_description.nuh_e_t > 0 %}
+        {%- for it in zoro_description.idx_uh_e_t %}
+        custom_mem->d_uh_e_tightened[{{it}}] = custom_mem->d_uh_e[{{it}}]
+                        - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
+        {%- endfor %}
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", custom_mem->d_uh_e_tightened);
+    {%- endif %}
+{%- endif %}
+
+}
+
+
+int custom_update_function({{ model.name }}_solver_capsule* capsule, double* data, int data_len)
+{
+    custom_memory *custom_mem = (custom_memory *) capsule->custom_update_memory;
+    ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(capsule);
+    ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule);
+    ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(capsule);
+    ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(capsule);
+    ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(capsule);
+    void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(capsule);
+
+    if (data_len > 0)
+    {
+        reset_P0_matrix(nlp_dims, &custom_mem->uncertainty_matrix_buffer[0], data);
+    }
+    uncertainty_propagate_and_update(nlp_solver, nlp_in, nlp_out, custom_mem);
+
+    return 1;
+}
+
+
+int custom_update_terminate_function({{ model.name }}_solver_capsule* capsule)
+{
+    custom_memory *mem = capsule->custom_update_memory;
+
+    free(mem->raw_memory);
+    return 1;
+}
+
+// useful prints for debugging
+
+/*
+printf("A_mat:\n");
+blasfeo_print_exp_dmat(nx, nx, &custom_mem->A_mat, 0, 0);
+printf("B_mat:\n");
+blasfeo_print_exp_dmat(nx, nu, &custom_mem->B_mat, 0, 0);
+printf("K_mat:\n");
+blasfeo_print_exp_dmat(nu, nx, &custom_mem->K_mat, 0, 0);
+printf("AK_mat:\n");
+blasfeo_print_exp_dmat(nx, nx, &custom_mem->AK_mat, 0, 0);
+printf("temp_AP_mat:\n");
+blasfeo_print_exp_dmat(nx, nx, &custom_mem->temp_AP_mat, 0, 0);
+printf("W_mat:\n");
+blasfeo_print_exp_dmat(nx, nx, &custom_mem->W_mat, 0, 0);
+printf("P_k+1:\n");
+blasfeo_print_exp_dmat(nx, nx, &(custom_mem->uncertainty_matrix_buffer[ii+1]), 0, 0);*/
\ No newline at end of file
diff --git a/third_party/acados/acados_template/c_templates_tera/phi_constraint.in.h b/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.h
similarity index 59%
rename from third_party/acados/acados_template/c_templates_tera/phi_constraint.in.h
rename to third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.h
index 283ed7f889..9611ea210c 100644
--- a/third_party/acados/acados_template/c_templates_tera/phi_constraint.in.h
+++ b/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -31,25 +28,17 @@
  * POSSIBILITY OF SUCH DAMAGE.;
  */
 
-#ifndef {{ model.name }}_PHI_CONSTRAINT
-#define {{ model.name }}_PHI_CONSTRAINT
+#include "acados_solver_{{ model.name }}.h"
 
-#ifdef __cplusplus
-extern "C" {
-#endif
+// Called at the end of solver creation.
+// This is allowed to allocate memory and store the pointer to it into capsule->custom_update_memory.
+int custom_update_init_function({{ model.name }}_solver_capsule* capsule);
 
-{% if dims.nphi > 0 %}
-// implicit ODE
-int {{ model.name }}_phi_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int {{ model.name }}_phi_constraint_work(int *, int *, int *, int *);
-const int *{{ model.name }}_phi_constraint_sparsity_in(int);
-const int *{{ model.name }}_phi_constraint_sparsity_out(int);
-int {{ model.name }}_phi_constraint_n_in(void);
-int {{ model.name }}_phi_constraint_n_out(void);
-{% endif %}
 
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
+// Custom update function that can be called between solver calls
+int custom_update_function({{ model.name }}_solver_capsule* capsule, double* data, int data_len);
 
-#endif  // {{ model.name }}_PHI_CONSTRAINT
+
+// Called just before destroying the solver.
+// Responsible to free allocated memory, stored at capsule->custom_update_memory.
+int custom_update_terminate_function({{ model.name }}_solver_capsule* capsule);
diff --git a/third_party/acados/acados_template/generate_c_code_constraint.py b/third_party/acados/acados_template/generate_c_code_constraint.py
deleted file mode 100644
index c79ddc129a..0000000000
--- a/third_party/acados/acados_template/generate_c_code_constraint.py
+++ /dev/null
@@ -1,180 +0,0 @@
-#
-# 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.;
-#
-
-import os
-from casadi import *
-from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_length, casadi_version_warning
-
-def generate_c_code_constraint( model, con_name, is_terminal, opts ):
-
-    casadi_version = CasadiMeta.version()
-    casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
-
-    if casadi_version not in (ALLOWED_CASADI_VERSIONS):
-        casadi_version_warning(casadi_version)
-
-    # load constraint variables and expression
-    x = model.x
-    p = model.p
-
-    if isinstance(x, casadi.MX):
-        symbol = MX.sym
-    else:
-        symbol = SX.sym
-
-    if is_terminal:
-        con_h_expr = model.con_h_expr_e
-        con_phi_expr = model.con_phi_expr_e
-        # create dummy u, z
-        u = symbol('u', 0, 0)
-        z = symbol('z', 0, 0)
-    else:
-        con_h_expr = model.con_h_expr
-        con_phi_expr = model.con_phi_expr
-        u = model.u
-        z = model.z
-
-    if (not is_empty(con_h_expr)) and (not is_empty(con_phi_expr)):
-        raise Exception("acados: you can either have constraint_h, or constraint_phi, not both.")
-
-    if not (is_empty(con_h_expr) and is_empty(con_phi_expr)):
-        if is_empty(con_h_expr):
-            constr_type = 'BGP'
-        else:
-            constr_type = 'BGH'
-
-        if is_empty(p):
-            p = symbol('p', 0, 0)
-
-        if is_empty(z):
-            z = symbol('z', 0, 0)
-
-        if not (is_empty(con_h_expr)) and opts['generate_hess']:
-            # multipliers for hessian
-            nh = casadi_length(con_h_expr)
-            lam_h = symbol('lam_h', nh, 1)
-
-        # set up & change directory
-        code_export_dir = opts["code_export_directory"]
-        if not os.path.exists(code_export_dir):
-            os.makedirs(code_export_dir)
-
-        cwd = os.getcwd()
-        os.chdir(code_export_dir)
-        gen_dir = con_name + '_constraints'
-        if not os.path.exists(gen_dir):
-            os.mkdir(gen_dir)
-        gen_dir_location = os.path.join('.', gen_dir)
-        os.chdir(gen_dir_location)
-
-        # export casadi functions
-        if constr_type == 'BGH':
-            if is_terminal:
-                fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt'
-            else:
-                fun_name = con_name + '_constr_h_fun_jac_uxt_zt'
-
-            jac_ux_t = transpose(jacobian(con_h_expr, vertcat(u,x)))
-            jac_z_t = jacobian(con_h_expr, z)
-            constraint_fun_jac_tran = Function(fun_name, [x, u, z, p], \
-                    [con_h_expr, jac_ux_t, jac_z_t])
-
-            constraint_fun_jac_tran.generate(fun_name, casadi_opts)
-            if opts['generate_hess']:
-
-                if is_terminal:
-                    fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt_hess'
-                else:
-                    fun_name = con_name + '_constr_h_fun_jac_uxt_zt_hess'
-
-                # adjoint
-                adj_ux = jtimes(con_h_expr, vertcat(u, x), lam_h, True)
-                # hessian
-                hess_ux = jacobian(adj_ux, vertcat(u, x))
-
-                adj_z = jtimes(con_h_expr, z, lam_h, True)
-                hess_z = jacobian(adj_z, z)
-
-                # set up functions
-                constraint_fun_jac_tran_hess = \
-                    Function(fun_name, [x, u, lam_h, z, p], \
-                      [con_h_expr, jac_ux_t, hess_ux, jac_z_t, hess_z])
-
-                # generate C code
-                constraint_fun_jac_tran_hess.generate(fun_name, casadi_opts)
-
-            if is_terminal:
-                fun_name = con_name + '_constr_h_e_fun'
-            else:
-                fun_name = con_name + '_constr_h_fun'
-            h_fun = Function(fun_name, [x, u, z, p], [con_h_expr])
-            h_fun.generate(fun_name, casadi_opts)
-
-        else: # BGP constraint
-            if is_terminal:
-                fun_name = con_name + '_phi_e_constraint'
-                r = model.con_r_in_phi_e
-                con_r_expr = model.con_r_expr_e
-            else:
-                fun_name = con_name + '_phi_constraint'
-                r = model.con_r_in_phi
-                con_r_expr = model.con_r_expr
-
-            nphi = casadi_length(con_phi_expr)
-            con_phi_expr_x_u_z = substitute(con_phi_expr, r, con_r_expr)
-            phi_jac_u = jacobian(con_phi_expr_x_u_z, u)
-            phi_jac_x = jacobian(con_phi_expr_x_u_z, x)
-            phi_jac_z = jacobian(con_phi_expr_x_u_z, z)
-
-            hess = hessian(con_phi_expr[0], r)[0]
-            for i in range(1, nphi):
-                hess = vertcat(hess, hessian(con_phi_expr[i], r)[0])
-
-            r_jac_u = jacobian(con_r_expr, u)
-            r_jac_x = jacobian(con_r_expr, x)
-
-            constraint_phi = \
-                Function(fun_name, [x, u, z, p], \
-                    [con_phi_expr_x_u_z, \
-                    vertcat(transpose(phi_jac_u), \
-                    transpose(phi_jac_x)), \
-                    transpose(phi_jac_z), \
-                    hess, vertcat(transpose(r_jac_u), \
-                    transpose(r_jac_x))])
-
-            constraint_phi.generate(fun_name, casadi_opts)
-
-        # change directory back
-        os.chdir(cwd)
-
-    return
diff --git a/third_party/acados/acados_template/generate_c_code_discrete_dynamics.py b/third_party/acados/acados_template/generate_c_code_discrete_dynamics.py
deleted file mode 100644
index c6a245ff81..0000000000
--- a/third_party/acados/acados_template/generate_c_code_discrete_dynamics.py
+++ /dev/null
@@ -1,98 +0,0 @@
-#
-# 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.
-#
-
-import os
-import casadi as ca
-from .utils import ALLOWED_CASADI_VERSIONS, casadi_length, casadi_version_warning
-
-def generate_c_code_discrete_dynamics( model, opts ):
-
-    casadi_version = ca.CasadiMeta.version()
-    casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
-
-    if casadi_version not in (ALLOWED_CASADI_VERSIONS):
-        casadi_version_warning(casadi_version)
-    
-    # load model
-    x = model.x
-    u = model.u
-    p = model.p
-    phi = model.disc_dyn_expr
-    model_name = model.name
-    nx = casadi_length(x)
-
-    if isinstance(phi, ca.MX):
-        symbol = ca.MX.sym
-    elif isinstance(phi, ca.SX):
-        symbol = ca.SX.sym
-    else:
-        Exception("generate_c_code_disc_dyn: disc_dyn_expr must be a CasADi expression, you have type: {}".format(type(phi)))
-
-    # assume nx1 = nx !!!
-    lam = symbol('lam', nx, 1)
-
-    # generate jacobians
-    ux = ca.vertcat(u,x)
-    jac_ux = ca.jacobian(phi, ux)
-    # generate adjoint
-    adj_ux = ca.jtimes(phi, ux, lam, True)
-    # generate hessian
-    hess_ux = ca.jacobian(adj_ux, ux)
-    
-    ## change directory
-    code_export_dir = opts["code_export_directory"]
-    if not os.path.exists(code_export_dir):
-        os.makedirs(code_export_dir)
-
-    cwd = os.getcwd()
-    os.chdir(code_export_dir)
-    model_dir = model_name + '_model'
-    if not os.path.exists(model_dir):
-        os.mkdir(model_dir)
-    model_dir_location = os.path.join('.', model_dir)
-    os.chdir(model_dir_location)
-
-    # set up & generate Functions
-    fun_name = model_name + '_dyn_disc_phi_fun'
-    phi_fun = ca.Function(fun_name, [x, u, p], [phi])
-    phi_fun.generate(fun_name, casadi_opts)
-
-    fun_name = model_name + '_dyn_disc_phi_fun_jac'
-    phi_fun_jac_ut_xt = ca.Function(fun_name, [x, u, p], [phi, jac_ux.T])
-    phi_fun_jac_ut_xt.generate(fun_name, casadi_opts)
-
-    fun_name = model_name + '_dyn_disc_phi_fun_jac_hess'
-    phi_fun_jac_ut_xt_hess = ca.Function(fun_name, [x, u, lam, p], [phi, jac_ux.T, hess_ux])
-    phi_fun_jac_ut_xt_hess.generate(fun_name, casadi_opts)
-    
-    os.chdir(cwd)
diff --git a/third_party/acados/acados_template/generate_c_code_explicit_ode.py b/third_party/acados/acados_template/generate_c_code_explicit_ode.py
deleted file mode 100644
index 76e6400535..0000000000
--- a/third_party/acados/acados_template/generate_c_code_explicit_ode.py
+++ /dev/null
@@ -1,124 +0,0 @@
-#
-# 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.;
-#
-
-import os
-from casadi import *
-from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_version_warning
-
-def generate_c_code_explicit_ode( model, opts ):
-
-    casadi_version = CasadiMeta.version()
-    casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
-    if casadi_version not in (ALLOWED_CASADI_VERSIONS):
-        casadi_version_warning(casadi_version)
-
-
-    generate_hess = opts["generate_hess"]
-    code_export_dir = opts["code_export_directory"]
-
-    # load model
-    x = model.x
-    u = model.u
-    p = model.p
-    f_expl = model.f_expl_expr
-    model_name = model.name
-
-    ## get model dimensions
-    nx = x.size()[0]
-    nu = u.size()[0]
-
-    if isinstance(f_expl, casadi.MX):
-        symbol = MX.sym
-    elif isinstance(f_expl, casadi.SX):
-        symbol = SX.sym
-    else:
-        raise Exception("Invalid type for f_expl! Possible types are 'SX' and 'MX'. Exiting.")
-    ## set up functions to be exported
-    Sx = symbol('Sx', nx, nx)
-    Sp = symbol('Sp', nx, nu)
-    lambdaX = symbol('lambdaX', nx, 1)
-
-    fun_name = model_name + '_expl_ode_fun'
-
-    ## Set up functions
-    expl_ode_fun = Function(fun_name, [x, u, p], [f_expl])
-
-    vdeX = jtimes(f_expl,x,Sx)
-    vdeP = jacobian(f_expl,u) + jtimes(f_expl,x,Sp)
-
-    fun_name = model_name + '_expl_vde_forw'
-
-    expl_vde_forw = Function(fun_name, [x, Sx, Sp, u, p], [f_expl, vdeX, vdeP])
-
-    adj = jtimes(f_expl, vertcat(x, u), lambdaX, True)
-
-    fun_name = model_name + '_expl_vde_adj'
-    expl_vde_adj = Function(fun_name, [x, lambdaX, u, p], [adj])
-
-    if generate_hess:
-        S_forw = vertcat(horzcat(Sx, Sp), horzcat(DM.zeros(nu,nx), DM.eye(nu)))
-        hess = mtimes(transpose(S_forw),jtimes(adj, vertcat(x,u), S_forw))
-        hess2 = []
-        for j in range(nx+nu):
-            for i in range(j,nx+nu):
-                hess2 = vertcat(hess2, hess[i,j])
-
-        fun_name = model_name + '_expl_ode_hess'
-        expl_ode_hess = Function(fun_name, [x, Sx, Sp, lambdaX, u, p], [adj, hess2])
-
-    ## generate C code
-    if not os.path.exists(code_export_dir):
-        os.makedirs(code_export_dir)
-
-    cwd = os.getcwd()
-    os.chdir(code_export_dir)
-    model_dir = model_name + '_model'
-    if not os.path.exists(model_dir):
-        os.mkdir(model_dir)
-    model_dir_location = os.path.join('.', model_dir)
-    os.chdir(model_dir_location)
-    fun_name = model_name + '_expl_ode_fun'
-    expl_ode_fun.generate(fun_name, casadi_opts)
-
-    fun_name = model_name + '_expl_vde_forw'
-    expl_vde_forw.generate(fun_name, casadi_opts)
-
-    fun_name = model_name + '_expl_vde_adj'
-    expl_vde_adj.generate(fun_name, casadi_opts)
-
-    if generate_hess:
-        fun_name = model_name + '_expl_ode_hess'
-        expl_ode_hess.generate(fun_name, casadi_opts)
-    os.chdir(cwd)
-
-    return
diff --git a/third_party/acados/acados_template/generate_c_code_external_cost.py b/third_party/acados/acados_template/generate_c_code_external_cost.py
deleted file mode 100644
index 8396522619..0000000000
--- a/third_party/acados/acados_template/generate_c_code_external_cost.py
+++ /dev/null
@@ -1,116 +0,0 @@
-#
-# 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.;
-#
-
-import os
-from casadi import SX, MX, Function, transpose, vertcat, horzcat, hessian, CasadiMeta
-from .utils import ALLOWED_CASADI_VERSIONS, casadi_version_warning
-
-
-def generate_c_code_external_cost(model, stage_type, opts):
-
-    casadi_version = CasadiMeta.version()
-    casadi_opts = dict(mex=False, casadi_int="int", casadi_real="double")
-
-    if casadi_version not in (ALLOWED_CASADI_VERSIONS):
-        casadi_version_warning(casadi_version)
-
-    x = model.x
-    p = model.p
-
-    if isinstance(x, MX):
-        symbol = MX.sym
-    else:
-        symbol = SX.sym
-
-    if stage_type == 'terminal':
-        suffix_name = "_cost_ext_cost_e_fun"
-        suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess"
-        suffix_name_jac = "_cost_ext_cost_e_fun_jac"
-        u = symbol("u", 0, 0)
-        ext_cost = model.cost_expr_ext_cost_e
-        custom_hess = model.cost_expr_ext_cost_custom_hess_e
-
-    elif stage_type == 'path':
-        suffix_name = "_cost_ext_cost_fun"
-        suffix_name_hess = "_cost_ext_cost_fun_jac_hess"
-        suffix_name_jac = "_cost_ext_cost_fun_jac"
-        u = model.u
-        ext_cost = model.cost_expr_ext_cost
-        custom_hess = model.cost_expr_ext_cost_custom_hess
-
-    elif stage_type == 'initial':
-        suffix_name = "_cost_ext_cost_0_fun"
-        suffix_name_hess = "_cost_ext_cost_0_fun_jac_hess"
-        suffix_name_jac = "_cost_ext_cost_0_fun_jac"
-        u = model.u
-        ext_cost = model.cost_expr_ext_cost_0
-        custom_hess = model.cost_expr_ext_cost_custom_hess_0
-
-    # set up functions to be exported
-    fun_name = model.name + suffix_name
-    fun_name_hess = model.name + suffix_name_hess
-    fun_name_jac = model.name + suffix_name_jac
-
-    # generate expression for full gradient and Hessian
-    full_hess, grad = hessian(ext_cost, vertcat(u, x))
-
-    if custom_hess is not None:
-        full_hess = custom_hess
-
-    ext_cost_fun = Function(fun_name, [x, u, p], [ext_cost])
-    ext_cost_fun_jac_hess = Function(
-        fun_name_hess, [x, u, p], [ext_cost, grad, full_hess]
-    )
-    ext_cost_fun_jac = Function(
-        fun_name_jac, [x, u, p], [ext_cost, grad]
-    )
-
-    # generate C code
-    code_export_dir = opts["code_export_directory"]
-    if not os.path.exists(code_export_dir):
-        os.makedirs(code_export_dir)
-
-    cwd = os.getcwd()
-    os.chdir(code_export_dir)
-    gen_dir = model.name + '_cost'
-    if not os.path.exists(gen_dir):
-        os.mkdir(gen_dir)
-    gen_dir_location = "./" + gen_dir
-    os.chdir(gen_dir_location)
-
-    ext_cost_fun.generate(fun_name, casadi_opts)
-    ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_opts)
-    ext_cost_fun_jac.generate(fun_name_jac, casadi_opts)
-
-    os.chdir(cwd)
-    return
diff --git a/third_party/acados/acados_template/generate_c_code_gnsf.py b/third_party/acados/acados_template/generate_c_code_gnsf.py
deleted file mode 100644
index 97acb8e330..0000000000
--- a/third_party/acados/acados_template/generate_c_code_gnsf.py
+++ /dev/null
@@ -1,131 +0,0 @@
-#
-# 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.;
-#
-
-import os
-from casadi import *
-from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_version_warning
-
-def generate_c_code_gnsf( model, opts ):
-
-    casadi_version = CasadiMeta.version()
-    casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
-    if casadi_version not in (ALLOWED_CASADI_VERSIONS):
-        casadi_version_warning(casadi_version)
-
-    model_name = model.name
-    code_export_dir = opts["code_export_directory"]
-
-    # set up directory
-    if not os.path.exists(code_export_dir):
-        os.makedirs(code_export_dir)
-
-    cwd = os.getcwd()
-    os.chdir(code_export_dir)
-    model_dir = model_name + '_model'
-    if not os.path.exists(model_dir):
-        os.mkdir(model_dir)
-    model_dir_location = os.path.join('.', model_dir)
-    os.chdir(model_dir_location)
-
-    # obtain gnsf dimensions
-    get_matrices_fun = model.get_matrices_fun
-    phi_fun = model.phi_fun
-
-    size_gnsf_A = get_matrices_fun.size_out(0)
-    gnsf_nx1 = size_gnsf_A[1]
-    gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
-    gnsf_nuhat = max(phi_fun.size_in(1))
-    gnsf_ny = max(phi_fun.size_in(0))
-    gnsf_nout = max(phi_fun.size_out(0))
-
-    # set up expressions
-    # if the model uses MX because of cost/constraints
-    # the DAE can be exported as SX -> detect GNSF in Matlab
-    # -> evaluated SX GNSF functions with MX.
-    u = model.u
-
-    if isinstance(u, casadi.MX):
-        symbol = MX.sym
-    else:
-        symbol = SX.sym
-
-    y = symbol("y", gnsf_ny, 1)
-    uhat = symbol("uhat", gnsf_nuhat, 1)
-    p = model.p
-    x1 = symbol("gnsf_x1", gnsf_nx1, 1)
-    x1dot = symbol("gnsf_x1dot", gnsf_nx1, 1)
-    z1 = symbol("gnsf_z1", gnsf_nz1, 1)
-    dummy = symbol("gnsf_dummy", 1, 1)
-    empty_var = symbol("gnsf_empty_var", 0, 0)
-
-    ## generate C code
-    fun_name = model_name + '_gnsf_phi_fun'
-    phi_fun_ = Function(fun_name, [y, uhat, p], [phi_fun(y, uhat, p)])
-    phi_fun_.generate(fun_name, casadi_opts)
-
-    fun_name = model_name + '_gnsf_phi_fun_jac_y'
-    phi_fun_jac_y = model.phi_fun_jac_y
-    phi_fun_jac_y_ = Function(fun_name, [y, uhat, p], phi_fun_jac_y(y, uhat, p))
-    phi_fun_jac_y_.generate(fun_name, casadi_opts)
-
-    fun_name = model_name + '_gnsf_phi_jac_y_uhat'
-    phi_jac_y_uhat = model.phi_jac_y_uhat
-    phi_jac_y_uhat_ = Function(fun_name, [y, uhat, p], phi_jac_y_uhat(y, uhat, p))
-    phi_jac_y_uhat_.generate(fun_name, casadi_opts)
-
-    fun_name = model_name + '_gnsf_f_lo_fun_jac_x1k1uz'
-    f_lo_fun_jac_x1k1uz = model.f_lo_fun_jac_x1k1uz
-    f_lo_fun_jac_x1k1uz_eval = f_lo_fun_jac_x1k1uz(x1, x1dot, z1, u, p)
-
-    # avoid codegeneration issue
-    if not isinstance(f_lo_fun_jac_x1k1uz_eval, tuple) and is_empty(f_lo_fun_jac_x1k1uz_eval):
-        f_lo_fun_jac_x1k1uz_eval = [empty_var]
-
-    f_lo_fun_jac_x1k1uz_ = Function(fun_name, [x1, x1dot, z1, u, p],
-                 f_lo_fun_jac_x1k1uz_eval)
-    f_lo_fun_jac_x1k1uz_.generate(fun_name, casadi_opts)
-
-    fun_name = model_name + '_gnsf_get_matrices_fun'
-    get_matrices_fun_ = Function(fun_name, [dummy], get_matrices_fun(1))
-    get_matrices_fun_.generate(fun_name, casadi_opts)
-
-    # remove fields for json dump
-    del model.phi_fun
-    del model.phi_fun_jac_y
-    del model.phi_jac_y_uhat
-    del model.f_lo_fun_jac_x1k1uz
-    del model.get_matrices_fun
-
-    os.chdir(cwd)
-
-    return
diff --git a/third_party/acados/acados_template/generate_c_code_implicit_ode.py b/third_party/acados/acados_template/generate_c_code_implicit_ode.py
deleted file mode 100644
index f2d50b43ab..0000000000
--- a/third_party/acados/acados_template/generate_c_code_implicit_ode.py
+++ /dev/null
@@ -1,139 +0,0 @@
-#
-# 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.;
-#
-
-import os
-from casadi import *
-from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_length, casadi_version_warning
-
-def generate_c_code_implicit_ode( model, opts ):
-
-    casadi_version = CasadiMeta.version()
-    casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
-    if casadi_version not in (ALLOWED_CASADI_VERSIONS):
-        casadi_version_warning(casadi_version)
-
-    generate_hess = opts["generate_hess"]
-    code_export_dir = opts["code_export_directory"]
-
-    ## load model
-    x = model.x
-    xdot = model.xdot
-    u = model.u
-    z = model.z
-    p = model.p
-    f_impl = model.f_impl_expr
-    model_name = model.name
-
-    ## get model dimensions
-    nx = casadi_length(x)
-    nu = casadi_length(u)
-    nz = casadi_length(z)
-
-    ## generate jacobians
-    jac_x       = jacobian(f_impl, x)
-    jac_xdot    = jacobian(f_impl, xdot)
-    jac_u       = jacobian(f_impl, u)
-    jac_z       = jacobian(f_impl, z)
-
-    ## generate hessian
-    x_xdot_z_u = vertcat(x, xdot, z, u)
-
-    if isinstance(x, casadi.MX):
-        symbol = MX.sym
-    else:
-        symbol = SX.sym
-
-    multiplier  = symbol('multiplier', nx + nz)
-
-    ADJ = jtimes(f_impl, x_xdot_z_u, multiplier, True)
-    HESS = jacobian(ADJ, x_xdot_z_u)
-
-    ## Set up functions
-    p = model.p
-    fun_name = model_name + '_impl_dae_fun'
-    impl_dae_fun = Function(fun_name, [x, xdot, u, z, p], [f_impl])
-
-    fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z'
-    impl_dae_fun_jac_x_xdot_z = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z])
-
-    # fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z'
-    # impl_dae_fun_jac_x_xdot = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z])
-
-    # fun_name = model_name + '_impl_dae_jac_x_xdot_u'
-    # impl_dae_jac_x_xdot_u = Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z])
-
-    fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z'
-    impl_dae_fun_jac_x_xdot_u_z = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u, jac_z])
-
-    fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u'
-    impl_dae_fun_jac_x_xdot_u = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u])
-
-    fun_name = model_name + '_impl_dae_jac_x_xdot_u_z'
-    impl_dae_jac_x_xdot_u_z = Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z])
-
-
-    fun_name = model_name + '_impl_dae_hess'
-    impl_dae_hess = Function(fun_name, [x, xdot, u, z, multiplier, p], [HESS])
-
-    # generate C code
-    if not os.path.exists(code_export_dir):
-        os.makedirs(code_export_dir)
-
-    cwd = os.getcwd()
-    os.chdir(code_export_dir)
-    model_dir = model_name + '_model'
-    if not os.path.exists(model_dir):
-        os.mkdir(model_dir)
-    model_dir_location = os.path.join('.', model_dir)
-    os.chdir(model_dir_location)
-
-    fun_name = model_name + '_impl_dae_fun'
-    impl_dae_fun.generate(fun_name, casadi_opts)
-
-    fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z'
-    impl_dae_fun_jac_x_xdot_z.generate(fun_name, casadi_opts)
-
-    fun_name = model_name + '_impl_dae_jac_x_xdot_u_z'
-    impl_dae_jac_x_xdot_u_z.generate(fun_name, casadi_opts)
-
-    fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z'
-    impl_dae_fun_jac_x_xdot_u_z.generate(fun_name, casadi_opts)
-
-    fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u'
-    impl_dae_fun_jac_x_xdot_u.generate(fun_name, casadi_opts)
-
-    if generate_hess:
-        fun_name = model_name + '_impl_dae_hess'
-        impl_dae_hess.generate(fun_name, casadi_opts)
-
-    os.chdir(cwd)
diff --git a/third_party/acados/acados_template/generate_c_code_nls_cost.py b/third_party/acados/acados_template/generate_c_code_nls_cost.py
deleted file mode 100644
index ffcd78ca7b..0000000000
--- a/third_party/acados/acados_template/generate_c_code_nls_cost.py
+++ /dev/null
@@ -1,113 +0,0 @@
-#
-# 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.;
-#
-
-import os
-from casadi import *
-from .utils import ALLOWED_CASADI_VERSIONS, casadi_length, casadi_version_warning
-
-def generate_c_code_nls_cost( model, cost_name, stage_type, opts ):
-
-    casadi_version = CasadiMeta.version()
-    casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
-
-    if casadi_version not in (ALLOWED_CASADI_VERSIONS):
-        casadi_version_warning(casadi_version)
-
-    x = model.x
-    p = model.p
-
-    if isinstance(x, casadi.MX):
-        symbol = MX.sym
-    else:
-        symbol = SX.sym
-
-    if stage_type == 'terminal':
-        middle_name = '_cost_y_e'
-        u = symbol('u', 0, 0)
-        cost_expr = model.cost_y_expr_e
-
-    elif stage_type == 'initial':
-        middle_name = '_cost_y_0'
-        u = model.u
-        cost_expr = model.cost_y_expr_0
-
-    elif stage_type == 'path':
-        middle_name = '_cost_y'
-        u = model.u
-        cost_expr = model.cost_y_expr
-
-    # set up directory
-    code_export_dir = opts["code_export_directory"]
-    if not os.path.exists(code_export_dir):
-        os.makedirs(code_export_dir)
-
-    cwd = os.getcwd()
-    os.chdir(code_export_dir)
-    gen_dir = cost_name + '_cost'
-    if not os.path.exists(gen_dir):
-        os.mkdir(gen_dir)
-    gen_dir_location = os.path.join('.', gen_dir)
-    os.chdir(gen_dir_location)
-
-    # set up expressions
-    cost_jac_expr = transpose(jacobian(cost_expr, vertcat(u, x)))
-
-    ny = casadi_length(cost_expr)
-
-    y = symbol('y', ny, 1)
-
-    y_adj = jtimes(cost_expr, vertcat(u, x), y, True)
-    y_hess = jacobian(y_adj, vertcat(u, x))
-
-    ## generate C code
-    suffix_name = '_fun'
-    fun_name = cost_name + middle_name + suffix_name
-    y_fun = Function( fun_name, [x, u, p], \
-            [ cost_expr ])
-    y_fun.generate( fun_name, casadi_opts )
-
-    suffix_name = '_fun_jac_ut_xt'
-    fun_name = cost_name + middle_name + suffix_name
-    y_fun_jac_ut_xt = Function(fun_name, [x, u, p], \
-            [ cost_expr, cost_jac_expr ])
-    y_fun_jac_ut_xt.generate( fun_name, casadi_opts )
-
-    suffix_name = '_hess'
-    fun_name = cost_name + middle_name + suffix_name
-    y_hess = Function(fun_name, [x, u, y, p], [ y_hess ])
-    y_hess.generate( fun_name, casadi_opts )
-
-    os.chdir(cwd)
-
-    return
-
diff --git a/third_party/acados/acados_template/gnsf/__init__.py b/third_party/acados/acados_template/gnsf/__init__.py
new file mode 100644
index 0000000000..8b13789179
--- /dev/null
+++ b/third_party/acados/acados_template/gnsf/__init__.py
@@ -0,0 +1 @@
+
diff --git a/third_party/acados/acados_template/gnsf/check_reformulation.py b/third_party/acados/acados_template/gnsf/check_reformulation.py
new file mode 100644
index 0000000000..2bdfbbc336
--- /dev/null
+++ b/third_party/acados/acados_template/gnsf/check_reformulation.py
@@ -0,0 +1,216 @@
+#
+# Copyright (c) The acados authors.
+#
+# 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.;
+#
+
+from acados_template.utils import casadi_length
+from casadi import *
+import numpy as np
+
+
+def check_reformulation(model, gnsf, print_info):
+
+    ## Description:
+    # this function takes the implicit ODE/ index-1 DAE and a gnsf structure
+    # to evaluate both models at num_eval random points x0, x0dot, z0, u0
+    # if for all points the relative error is <= TOL, the function will return::
+    # 1, otherwise it will give an error.
+
+    TOL = 1e-14
+    num_eval = 10
+
+    # get dimensions
+    nx = gnsf["nx"]
+    nu = gnsf["nu"]
+    nz = gnsf["nz"]
+    nx1 = gnsf["nx1"]
+    nx2 = gnsf["nx2"]
+    nz1 = gnsf["nz1"]
+    nz2 = gnsf["nz2"]
+    n_out = gnsf["n_out"]
+
+    # get model matrices
+    A = gnsf["A"]
+    B = gnsf["B"]
+    C = gnsf["C"]
+    E = gnsf["E"]
+    c = gnsf["c"]
+
+    L_x = gnsf["L_x"]
+    L_xdot = gnsf["L_xdot"]
+    L_z = gnsf["L_z"]
+    L_u = gnsf["L_u"]
+
+    A_LO = gnsf["A_LO"]
+    E_LO = gnsf["E_LO"]
+    B_LO = gnsf["B_LO"]
+    c_LO = gnsf["c_LO"]
+
+    I_x1 = range(nx1)
+    I_x2 = range(nx1, nx)
+
+    I_z1 = range(nz1)
+    I_z2 = range(nz1, nz)
+
+    idx_perm_f = gnsf["idx_perm_f"]
+
+    # get casadi variables
+    x = gnsf["x"]
+    xdot = gnsf["xdot"]
+    z = gnsf["z"]
+    u = gnsf["u"]
+    y = gnsf["y"]
+    uhat = gnsf["uhat"]
+    p = gnsf["p"]
+
+    # create functions
+    impl_dae_fun = Function("impl_dae_fun", [x, xdot, u, z, p], [model.f_impl_expr])
+    phi_fun = Function("phi_fun", [y, uhat, p], [gnsf["phi_expr"]])
+    f_lo_fun = Function(
+        "f_lo_fun", [x[range(nx1)], xdot[range(nx1)], z, u, p], [gnsf["f_lo_expr"]]
+    )
+
+    # print(gnsf)
+    # print(gnsf["n_out"])
+
+    for i_check in range(num_eval):
+        # generate random values
+        x0 = np.random.rand(nx, 1)
+        x0dot = np.random.rand(nx, 1)
+        z0 = np.random.rand(nz, 1)
+        u0 = np.random.rand(nu, 1)
+
+        if gnsf["ny"] > 0:
+            y0 = L_x @ x0[I_x1] + L_xdot @ x0dot[I_x1] + L_z @ z0[I_z1]
+        else:
+            y0 = []
+        if gnsf["nuhat"] > 0:
+            uhat0 = L_u @ u0
+        else:
+            uhat0 = []
+
+        # eval functions
+        p0 = np.random.rand(gnsf["np"], 1)
+        f_impl_val = impl_dae_fun(x0, x0dot, u0, z0, p0).full()
+        phi_val = phi_fun(y0, uhat0, p0)
+        f_lo_val = f_lo_fun(x0[I_x1], x0dot[I_x1], z0[I_z1], u0, p0)
+
+        f_impl_val = f_impl_val[idx_perm_f]
+        # eval gnsf
+        if n_out > 0:
+            C_phi = C @ phi_val
+        else:
+            C_phi = np.zeros((nx1 + nz1, 1))
+        try:
+            gnsf_val1 = (
+                A @ x0[I_x1] + B @ u0 + C_phi + c - E @ vertcat(x0dot[I_x1], z0[I_z1])
+            )
+            # gnsf_1 = (A @ x[I_x1] + B @ u + C_phi + c - E @ vertcat(xdot[I_x1], z[I_z1]))
+        except:
+            import pdb
+
+            pdb.set_trace()
+
+        if nx2 > 0:  # eval LOS:
+            gnsf_val2 = (
+                A_LO @ x0[I_x2]
+                + B_LO @ u0
+                + c_LO
+                + f_lo_val
+                - E_LO @ vertcat(x0dot[I_x2], z0[I_z2])
+            )
+            gnsf_val = vertcat(gnsf_val1, gnsf_val2).full()
+        else:
+            gnsf_val = gnsf_val1.full()
+        # compute error and check
+        rel_error = np.linalg.norm(f_impl_val - gnsf_val) / np.linalg.norm(f_impl_val)
+
+        if rel_error > TOL:
+            print("transcription failed rel_error > TOL")
+            print("you are in debug mode now: import pdb; pdb.set_trace()")
+            abs_error = gnsf_val - f_impl_val
+            # T = table(f_impl_val, gnsf_val, abs_error)
+            # print(T)
+            print("abs_error:", abs_error)
+            #         error('transcription failed rel_error > TOL')
+            #         check = 0
+            import pdb
+
+            pdb.set_trace()
+    if print_info:
+        print(" ")
+        print("model reformulation checked: relative error <= TOL = ", str(TOL))
+        print(" ")
+        check = 1
+    ## helpful for debugging:
+    # # use in calling function and compare
+    # # compare f_impl(i) with gnsf_val1(i)
+    #
+
+    #     nx  = gnsf['nx']
+    #     nu  = gnsf['nu']
+    #     nz  = gnsf['nz']
+    #     nx1 = gnsf['nx1']
+    #     nx2 = gnsf['nx2']
+    #
+    #         A  = gnsf['A']
+    #     B  = gnsf['B']
+    #     C  = gnsf['C']
+    #     E  = gnsf['E']
+    #     c  = gnsf['c']
+    #
+    #     L_x    = gnsf['L_x']
+    #     L_z    = gnsf['L_z']
+    #     L_xdot = gnsf['L_xdot']
+    #     L_u    = gnsf['L_u']
+    #
+    #     A_LO = gnsf['A_LO']
+    #
+    #     x0 = rand(nx, 1)
+    #     x0dot = rand(nx, 1)
+    #     z0 = rand(nz, 1)
+    #     u0 = rand(nu, 1)
+    #     I_x1 = range(nx1)
+    #     I_x2 = nx1+range(nx)
+    #
+    #     y0 = L_x @ x0[I_x1] + L_xdot @ x0dot[I_x1] + L_z @ z0
+    #     uhat0 = L_u @ u0
+    #
+    #     gnsf_val1 = (A @ x[I_x1] + B @ u + #         C @ phi_current + c) - E @ [xdot[I_x1] z]
+    #     gnsf_val1 = gnsf_val1.simplify()
+    #
+    # #     gnsf_val2 = A_LO @ x[I_x2] + gnsf['f_lo_fun'](x[I_x1], xdot[I_x1], z, u) - xdot[I_x2]
+    #     gnsf_val2 =  A_LO @ x[I_x2] + gnsf['f_lo_fun'](x[I_x1], xdot[I_x1], z, u) - xdot[I_x2]
+    #
+    #
+    #     gnsf_val = [gnsf_val1 gnsf_val2]
+    #     gnsf_val = gnsf_val.simplify()
+    #     dyn_expr_f = dyn_expr_f.simplify()
+    # import pdb; pdb.set_trace()
+
+    return check
diff --git a/third_party/acados/acados_template/gnsf/detect_affine_terms_reduce_nonlinearity.py b/third_party/acados/acados_template/gnsf/detect_affine_terms_reduce_nonlinearity.py
new file mode 100644
index 0000000000..ebf1f373a4
--- /dev/null
+++ b/third_party/acados/acados_template/gnsf/detect_affine_terms_reduce_nonlinearity.py
@@ -0,0 +1,278 @@
+#
+# Copyright (c) The acados authors.
+#
+# 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.;
+#
+
+from casadi import *
+from .check_reformulation import check_reformulation
+from .determine_input_nonlinearity_function import determine_input_nonlinearity_function
+from ..utils import casadi_length, print_casadi_expression
+
+
+def detect_affine_terms_reduce_nonlinearity(gnsf, acados_ocp, print_info):
+
+    ## Description
+    # this function takes a gnsf structure with trivial model matrices (A, B,
+    # E, c are zeros, and C is eye).
+    # It detects all affine linear terms and sets up an equivalent model in the
+    # GNSF structure, where all affine linear terms are modeled through the
+    # matrices A, B, E, c and the linear output system (LOS) is empty.
+    # NOTE: model is just taken as an argument to check equivalence of the
+    # models within the function.
+
+    model = acados_ocp.model
+    if print_info:
+        print(" ")
+        print("====================================================================")
+        print(" ")
+        print("============  Detect affine-linear dependencies   ==================")
+        print(" ")
+        print("====================================================================")
+        print(" ")
+    # symbolics
+    x = gnsf["x"]
+    xdot = gnsf["xdot"]
+    u = gnsf["u"]
+    z = gnsf["z"]
+
+    # dimensions
+    nx = gnsf["nx"]
+    nu = gnsf["nu"]
+    nz = gnsf["nz"]
+
+    ny_old = gnsf["ny"]
+    nuhat_old = gnsf["nuhat"]
+
+    ## Represent all affine dependencies through the model matrices A, B, E, c
+    ## determine A
+    n_nodes_current = n_nodes(gnsf["phi_expr"])
+
+    for ii in range(casadi_length(gnsf["phi_expr"])):
+        fii = gnsf["phi_expr"][ii]
+        for ix in range(nx):
+            var = x[ix]
+            varname = var.name
+            # symbolic jacobian of fii w.r.t. xi
+            jac_fii_xi = jacobian(fii, var)
+            if jac_fii_xi.is_constant():
+                # jacobian value
+                jac_fii_xi_fun = Function("jac_fii_xi_fun", [x[1]], [jac_fii_xi])
+                # x[1] as input just to have a scalar input and call the function as follows:
+                gnsf["A"][ii, ix] = jac_fii_xi_fun(0).full()
+            else:
+                gnsf["A"][ii, ix] = 0
+                if print_info:
+                    print(
+                        "phi(",
+                        str(ii),
+                        ") is nonlinear in x(",
+                        str(ix),
+                        ") = ",
+                        varname,
+                    )
+                    print(fii)
+                    print("-----------------------------------------------------")
+    f_next = gnsf["phi_expr"] - gnsf["A"] @ x
+    f_next = simplify(f_next)
+    n_nodes_next = n_nodes(f_next)
+
+    if print_info:
+        print("\n")
+        print(f"determined matrix A:")
+        print(gnsf["A"])
+        print(f"reduced nonlinearity from  {n_nodes_current} to {n_nodes_next} nodes")
+    # assert(n_nodes_current >= n_nodes_next,'n_nodes_current >= n_nodes_next FAILED')
+    gnsf["phi_expr"] = f_next
+
+    check_reformulation(model, gnsf, print_info)
+
+    ## determine B
+    n_nodes_current = n_nodes(gnsf["phi_expr"])
+
+    for ii in range(casadi_length(gnsf["phi_expr"])):
+        fii = gnsf["phi_expr"][ii]
+        for iu in range(nu):
+            var = u[iu]
+            varname = var.name
+            # symbolic jacobian of fii w.r.t. ui
+            jac_fii_ui = jacobian(fii, var)
+            if jac_fii_ui.is_constant():  # i.e. hessian is structural zero:
+                # jacobian value
+                jac_fii_ui_fun = Function("jac_fii_ui_fun", [x[1]], [jac_fii_ui])
+                gnsf["B"][ii, iu] = jac_fii_ui_fun(0).full()
+            else:
+                gnsf["B"][ii, iu] = 0
+                if print_info:
+                    print(f"phi({ii}) is nonlinear in u(", str(iu), ") = ", varname)
+                    print(fii)
+                    print("-----------------------------------------------------")
+    f_next = gnsf["phi_expr"] - gnsf["B"] @ u
+    f_next = simplify(f_next)
+    n_nodes_next = n_nodes(f_next)
+
+    if print_info:
+        print("\n")
+        print(f"determined matrix B:")
+        print(gnsf["B"])
+        print(f"reduced nonlinearity from  {n_nodes_current} to {n_nodes_next} nodes")
+
+    gnsf["phi_expr"] = f_next
+
+    check_reformulation(model, gnsf, print_info)
+
+    ## determine E
+    n_nodes_current = n_nodes(gnsf["phi_expr"])
+    k = vertcat(xdot, z)
+
+    for ii in range(casadi_length(gnsf["phi_expr"])):
+        fii = gnsf["phi_expr"][ii]
+        for ik in range(casadi_length(k)):
+            # symbolic jacobian of fii w.r.t. ui
+            var = k[ik]
+            varname = var.name
+            jac_fii_ki = jacobian(fii, var)
+            if jac_fii_ki.is_constant():
+                # jacobian value
+                jac_fii_ki_fun = Function("jac_fii_ki_fun", [x[1]], [jac_fii_ki])
+                gnsf["E"][ii, ik] = -jac_fii_ki_fun(0).full()
+            else:
+                gnsf["E"][ii, ik] = 0
+                if print_info:
+                    print(f"phi( {ii}) is nonlinear in xdot_z({ik}) = ", varname)
+                    print(fii)
+                    print("-----------------------------------------------------")
+    f_next = gnsf["phi_expr"] + gnsf["E"] @ k
+    f_next = simplify(f_next)
+    n_nodes_next = n_nodes(f_next)
+
+    if print_info:
+        print("\n")
+        print(f"determined matrix E:")
+        print(gnsf["E"])
+        print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes")
+
+    gnsf["phi_expr"] = f_next
+    check_reformulation(model, gnsf, print_info)
+
+    ## determine constant term c
+
+    n_nodes_current = n_nodes(gnsf["phi_expr"])
+    for ii in range(casadi_length(gnsf["phi_expr"])):
+        fii = gnsf["phi_expr"][ii]
+        if fii.is_constant():
+            # function value goes into c
+            fii_fun = Function("fii_fun", [x[1]], [fii])
+            gnsf["c"][ii] = fii_fun(0).full()
+        else:
+            gnsf["c"][ii] = 0
+            if print_info:
+                print(f"phi(", str(ii), ") is NOT constant")
+                print(fii)
+                print("-----------------------------------------------------")
+    gnsf["phi_expr"] = gnsf["phi_expr"] - gnsf["c"]
+    gnsf["phi_expr"] = simplify(gnsf["phi_expr"])
+    n_nodes_next = n_nodes(gnsf["phi_expr"])
+
+    if print_info:
+        print("\n")
+        print(f"determined vector c:")
+        print(gnsf["c"])
+        print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes")
+
+    check_reformulation(model, gnsf, print_info)
+
+    ## determine nonlinearity & corresponding matrix C
+    ## Reduce dimension of phi
+    n_nodes_current = n_nodes(gnsf["phi_expr"])
+    ind_non_zero = []
+    for ii in range(casadi_length(gnsf["phi_expr"])):
+        fii = gnsf["phi_expr"][ii]
+        fii = simplify(fii)
+        if not fii.is_zero():
+            ind_non_zero = list(set.union(set(ind_non_zero), set([ii])))
+    gnsf["phi_expr"] = gnsf["phi_expr"][ind_non_zero]
+
+    # C
+    gnsf["C"] = np.zeros((nx + nz, len(ind_non_zero)))
+    for ii in range(len(ind_non_zero)):
+        gnsf["C"][ind_non_zero[ii], ii] = 1
+    gnsf = determine_input_nonlinearity_function(gnsf)
+    n_nodes_next = n_nodes(gnsf["phi_expr"])
+
+    if print_info:
+        print(" ")
+        print("determined matrix C:")
+        print(gnsf["C"])
+        print(
+            "---------------------------------------------------------------------------------"
+        )
+        print(
+            "------------- Success: Affine linear terms detected -----------------------------"
+        )
+        print(
+            "---------------------------------------------------------------------------------"
+        )
+        print(
+            f'reduced nonlinearity dimension n_out from  {nx+nz}  to  {gnsf["n_out"]}'
+        )
+        print(f"reduced nonlinearity from  {n_nodes_current} to {n_nodes_next} nodes")
+        print(" ")
+        print("phi now reads as:")
+        print_casadi_expression(gnsf["phi_expr"])
+
+    ## determine input of nonlinearity function
+    check_reformulation(model, gnsf, print_info)
+
+    gnsf["ny"] = casadi_length(gnsf["y"])
+    gnsf["nuhat"] = casadi_length(gnsf["uhat"])
+
+    if print_info:
+        print(
+            "-----------------------------------------------------------------------------------"
+        )
+        print(" ")
+        print(
+            f"reduced input ny    of phi from  ",
+            str(ny_old),
+            "   to  ",
+            str(gnsf["ny"]),
+        )
+        print(
+            f"reduced input nuhat of phi from  ",
+            str(nuhat_old),
+            "   to  ",
+            str(gnsf["nuhat"]),
+        )
+        print(
+            "-----------------------------------------------------------------------------------"
+        )
+
+    # if print_info:
+    #     print(f"gnsf: {gnsf}")
+
+    return gnsf
diff --git a/third_party/acados/acados_template/gnsf/detect_gnsf_structure.py b/third_party/acados/acados_template/gnsf/detect_gnsf_structure.py
new file mode 100644
index 0000000000..24ffe643b8
--- /dev/null
+++ b/third_party/acados/acados_template/gnsf/detect_gnsf_structure.py
@@ -0,0 +1,240 @@
+#
+# Copyright (c) The acados authors.
+#
+# 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.;
+#
+#   Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com
+
+from casadi import Function, jacobian, SX, vertcat, horzcat
+
+from .determine_trivial_gnsf_transcription import determine_trivial_gnsf_transcription
+from .detect_affine_terms_reduce_nonlinearity import (
+    detect_affine_terms_reduce_nonlinearity,
+)
+from .reformulate_with_LOS import reformulate_with_LOS
+from .reformulate_with_invertible_E_mat import reformulate_with_invertible_E_mat
+from .structure_detection_print_summary import structure_detection_print_summary
+from .check_reformulation import check_reformulation
+
+
+def detect_gnsf_structure(acados_ocp, transcribe_opts=None):
+
+    ## Description
+    # This function takes a CasADi implicit ODE or index-1 DAE model "model"
+    # consisting of a CasADi expression f_impl in the symbolic CasADi
+    # variables x, xdot, u, z, (and possibly parameters p), which are also part
+    # of the model, as well as a model name.
+    # It will create a struct "gnsf" containing all information needed to use
+    # it with the gnsf integrator in acados.
+    # Additionally it will create the struct "reordered_model" which contains
+    # the permuted state vector and permuted f_impl, in which additionally some
+    # functions, which were made part of the linear output system of the gnsf,
+    # have changed signs.
+
+    # Options: transcribe_opts is a Matlab struct consisting of booleans:
+    #   print_info: if extensive information on how the model is processed
+    #       is printed to the console.
+    #   generate_gnsf_model: if the neccessary C functions to simulate the gnsf
+    #       model with the acados implementation of the GNSF exploiting
+    #       integrator should be generated.
+    #   generate_gnsf_model: if the neccessary C functions to simulate the
+    #       reordered model with the acados implementation of the IRK
+    #       integrator should be generated.
+    #   check_E_invertibility: if the transcription method should check if the
+    #       assumption that the main blocks of the matrix gnsf.E are invertible
+    #       holds. If not, the method will try to reformulate the gnsf model
+    #       with a different model, such that the assumption holds.
+
+    # acados_root_dir = getenv('ACADOS_INSTALL_DIR')
+
+    ## load transcribe_opts
+    if transcribe_opts is None:
+        print("WARNING: GNSF structure detection called without transcribe_opts")
+        print(" using default settings")
+        print("")
+        transcribe_opts = dict()
+
+    if "print_info" in transcribe_opts:
+        print_info = transcribe_opts["print_info"]
+    else:
+        print_info = 1
+        print("print_info option was not set - default is true")
+
+    if "detect_LOS" in transcribe_opts:
+        detect_LOS = transcribe_opts["detect_LOS"]
+    else:
+        detect_LOS = 1
+        if print_info:
+            print("detect_LOS option was not set - default is true")
+
+    if "check_E_invertibility" in transcribe_opts:
+        check_E_invertibility = transcribe_opts["check_E_invertibility"]
+    else:
+        check_E_invertibility = 1
+        if print_info:
+            print("check_E_invertibility option was not set - default is true")
+
+    ## Reformulate implicit index-1 DAE into GNSF form
+    # (Generalized nonlinear static feedback)
+    gnsf = determine_trivial_gnsf_transcription(acados_ocp, print_info)
+    gnsf = detect_affine_terms_reduce_nonlinearity(gnsf, acados_ocp, print_info)
+
+    if detect_LOS:
+        gnsf = reformulate_with_LOS(acados_ocp, gnsf, print_info)
+
+    if check_E_invertibility:
+        gnsf = reformulate_with_invertible_E_mat(gnsf, acados_ocp, print_info)
+
+    # detect purely linear model
+    if gnsf["nx1"] == 0 and gnsf["nz1"] == 0 and gnsf["nontrivial_f_LO"] == 0:
+        gnsf["purely_linear"] = 1
+    else:
+        gnsf["purely_linear"] = 0
+
+    structure_detection_print_summary(gnsf, acados_ocp)
+    check_reformulation(acados_ocp.model, gnsf, print_info)
+
+    ## copy relevant fields from gnsf to model
+    acados_ocp.model.get_matrices_fun = Function()
+    dummy = acados_ocp.model.x[0]
+    model_name = acados_ocp.model.name
+
+    get_matrices_fun = Function(
+        f"{model_name}_gnsf_get_matrices_fun",
+        [dummy],
+        [
+            gnsf["A"],
+            gnsf["B"],
+            gnsf["C"],
+            gnsf["E"],
+            gnsf["L_x"],
+            gnsf["L_xdot"],
+            gnsf["L_z"],
+            gnsf["L_u"],
+            gnsf["A_LO"],
+            gnsf["c"],
+            gnsf["E_LO"],
+            gnsf["B_LO"],
+            gnsf["nontrivial_f_LO"],
+            gnsf["purely_linear"],
+            gnsf["ipiv_x"] + 1,
+            gnsf["ipiv_z"] + 1,
+            gnsf["c_LO"],
+        ],
+    )
+
+    phi = gnsf["phi_expr"]
+    y = gnsf["y"]
+    uhat = gnsf["uhat"]
+    p = gnsf["p"]
+
+    jac_phi_y = jacobian(phi, y)
+    jac_phi_uhat = jacobian(phi, uhat)
+
+    phi_fun = Function(f"{model_name}_gnsf_phi_fun", [y, uhat, p], [phi])
+    acados_ocp.model.phi_fun = phi_fun
+    acados_ocp.model.phi_fun_jac_y = Function(
+        f"{model_name}_gnsf_phi_fun_jac_y", [y, uhat, p], [phi, jac_phi_y]
+    )
+    acados_ocp.model.phi_jac_y_uhat = Function(
+        f"{model_name}_gnsf_phi_jac_y_uhat", [y, uhat, p], [jac_phi_y, jac_phi_uhat]
+    )
+
+    x1 = acados_ocp.model.x[gnsf["idx_perm_x"][: gnsf["nx1"]]]
+    x1dot = acados_ocp.model.xdot[gnsf["idx_perm_x"][: gnsf["nx1"]]]
+    if gnsf["nz1"] > 0:
+        z1 = acados_ocp.model.z[gnsf["idx_perm_z"][: gnsf["nz1"]]]
+    else:
+        z1 = SX.sym("z1", 0, 0)
+    f_lo = gnsf["f_lo_expr"]
+    u = acados_ocp.model.u
+    acados_ocp.model.f_lo_fun_jac_x1k1uz = Function(
+        f"{model_name}_gnsf_f_lo_fun_jac_x1k1uz",
+        [x1, x1dot, z1, u, p],
+        [
+            f_lo,
+            horzcat(
+                jacobian(f_lo, x1),
+                jacobian(f_lo, x1dot),
+                jacobian(f_lo, u),
+                jacobian(f_lo, z1),
+            ),
+        ],
+    )
+
+    acados_ocp.model.get_matrices_fun = get_matrices_fun
+
+    size_gnsf_A = gnsf["A"].shape
+    acados_ocp.dims.gnsf_nx1 = size_gnsf_A[1]
+    acados_ocp.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
+    acados_ocp.dims.gnsf_nuhat = max(phi_fun.size_in(1))
+    acados_ocp.dims.gnsf_ny = max(phi_fun.size_in(0))
+    acados_ocp.dims.gnsf_nout = max(phi_fun.size_out(0))
+
+    # # dim
+    # model['dim_gnsf_nx1'] = gnsf['nx1']
+    # model['dim_gnsf_nx2'] = gnsf['nx2']
+    # model['dim_gnsf_nz1'] = gnsf['nz1']
+    # model['dim_gnsf_nz2'] = gnsf['nz2']
+    # model['dim_gnsf_nuhat'] = gnsf['nuhat']
+    # model['dim_gnsf_ny'] = gnsf['ny']
+    # model['dim_gnsf_nout'] = gnsf['n_out']
+
+    # # sym
+    # model['sym_gnsf_y'] = gnsf['y']
+    # model['sym_gnsf_uhat'] = gnsf['uhat']
+
+    # # data
+    # model['dyn_gnsf_A'] = gnsf['A']
+    # model['dyn_gnsf_A_LO'] = gnsf['A_LO']
+    # model['dyn_gnsf_B'] = gnsf['B']
+    # model['dyn_gnsf_B_LO'] = gnsf['B_LO']
+    # model['dyn_gnsf_E'] = gnsf['E']
+    # model['dyn_gnsf_E_LO'] = gnsf['E_LO']
+    # model['dyn_gnsf_C'] = gnsf['C']
+    # model['dyn_gnsf_c'] = gnsf['c']
+    # model['dyn_gnsf_c_LO'] = gnsf['c_LO']
+    # model['dyn_gnsf_L_x'] = gnsf['L_x']
+    # model['dyn_gnsf_L_xdot'] = gnsf['L_xdot']
+    # model['dyn_gnsf_L_z'] = gnsf['L_z']
+    # model['dyn_gnsf_L_u'] = gnsf['L_u']
+    # model['dyn_gnsf_idx_perm_x'] = gnsf['idx_perm_x']
+    # model['dyn_gnsf_ipiv_x'] = gnsf['ipiv_x']
+    # model['dyn_gnsf_idx_perm_z'] = gnsf['idx_perm_z']
+    # model['dyn_gnsf_ipiv_z'] = gnsf['ipiv_z']
+    # model['dyn_gnsf_idx_perm_f'] = gnsf['idx_perm_f']
+    # model['dyn_gnsf_ipiv_f'] = gnsf['ipiv_f']
+
+    # # flags
+    # model['dyn_gnsf_nontrivial_f_LO'] = gnsf['nontrivial_f_LO']
+    # model['dyn_gnsf_purely_linear'] = gnsf['purely_linear']
+
+    # # casadi expr
+    # model['dyn_gnsf_expr_phi'] = gnsf['phi_expr']
+    # model['dyn_gnsf_expr_f_lo'] = gnsf['f_lo_expr']
+
+    return acados_ocp
diff --git a/third_party/acados/acados_template/gnsf/determine_input_nonlinearity_function.py b/third_party/acados/acados_template/gnsf/determine_input_nonlinearity_function.py
new file mode 100644
index 0000000000..94aa001c79
--- /dev/null
+++ b/third_party/acados/acados_template/gnsf/determine_input_nonlinearity_function.py
@@ -0,0 +1,110 @@
+#
+# Copyright (c) The acados authors.
+#
+# 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.;
+#
+#   Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com
+
+from casadi import *
+from ..utils import casadi_length, is_empty
+
+
+def determine_input_nonlinearity_function(gnsf):
+
+    ## Description
+    # this function takes a structure gnsf and updates the matrices L_x,
+    # L_xdot, L_z, L_u and CasADi vectors y, uhat of this structure as follows:
+
+    # given a CasADi expression phi_expr, which may depend on the variables
+    # (x1, x1dot, z, u), this function determines a vector y (uhat) consisting
+    # of all components of (x1, x1dot, z) (respectively u) that enter phi_expr.
+    # Additionally matrices L_x, L_xdot, L_z, L_u are determined such that
+    #           y    = L_x * x + L_xdot * xdot + L_z * z
+    #           uhat = L_u * u
+    # Furthermore the dimensions ny, nuhat, n_out are updated
+
+    ## y
+    y = SX.sym('y', 0, 0)
+    # components of x1
+    for ii in range(gnsf["nx1"]):
+        if which_depends(gnsf["phi_expr"], gnsf["x"][ii])[0]:
+            y = vertcat(y, gnsf["x"][ii])
+        # else:
+        # x[ii] is not part of y
+    # components of x1dot
+    for ii in range(gnsf["nx1"]):
+        if which_depends(gnsf["phi_expr"], gnsf["xdot"][ii])[0]:
+            print(gnsf["phi_expr"], "depends on", gnsf["xdot"][ii])
+            y = vertcat(y, gnsf["xdot"][ii])
+        # else:
+        # xdot[ii] is not part of y
+    # components of z
+    for ii in range(gnsf["nz1"]):
+        if which_depends(gnsf["phi_expr"], gnsf["z"][ii])[0]:
+            y = vertcat(y, gnsf["z"][ii])
+        # else:
+        # z[ii] is not part of y
+    ## uhat
+    uhat = SX.sym('uhat', 0, 0)
+    # components of u
+    for ii in range(gnsf["nu"]):
+        if which_depends(gnsf["phi_expr"], gnsf["u"][ii])[0]:
+            uhat = vertcat(uhat, gnsf["u"][ii])
+        # else:
+        # u[ii] is not part of uhat
+    ## generate gnsf['phi_expr_fun']
+    # linear input matrices
+    if is_empty(y):
+        gnsf["L_x"] = []
+        gnsf["L_xdot"] = []
+        gnsf["L_u"] = []
+        gnsf["L_z"] = []
+    else:
+        dummy = SX.sym("dummy_input", 0)
+        L_x_fun = Function(
+            "L_x_fun", [dummy], [jacobian(y, gnsf["x"][range(gnsf["nx1"])])]
+        )
+        L_xdot_fun = Function(
+            "L_xdot_fun", [dummy], [jacobian(y, gnsf["xdot"][range(gnsf["nx1"])])]
+        )
+        L_z_fun = Function(
+            "L_z_fun", [dummy], [jacobian(y, gnsf["z"][range(gnsf["nz1"])])]
+        )
+        L_u_fun = Function("L_u_fun", [dummy], [jacobian(uhat, gnsf["u"])])
+
+        gnsf["L_x"] = L_x_fun(0).full()
+        gnsf["L_xdot"] = L_xdot_fun(0).full()
+        gnsf["L_u"] = L_u_fun(0).full()
+        gnsf["L_z"] = L_z_fun(0).full()
+    gnsf["y"] = y
+    gnsf["uhat"] = uhat
+
+    gnsf["ny"] = casadi_length(y)
+    gnsf["nuhat"] = casadi_length(uhat)
+    gnsf["n_out"] = casadi_length(gnsf["phi_expr"])
+
+    return gnsf
diff --git a/third_party/acados/acados_template/gnsf/determine_trivial_gnsf_transcription.py b/third_party/acados/acados_template/gnsf/determine_trivial_gnsf_transcription.py
new file mode 100644
index 0000000000..23c2440537
--- /dev/null
+++ b/third_party/acados/acados_template/gnsf/determine_trivial_gnsf_transcription.py
@@ -0,0 +1,155 @@
+#
+# Copyright (c) The acados authors.
+#
+# 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.;
+#
+
+from casadi import *
+import numpy as np
+from ..utils import casadi_length, idx_perm_to_ipiv
+from .determine_input_nonlinearity_function import determine_input_nonlinearity_function
+from .check_reformulation import check_reformulation
+
+
+def determine_trivial_gnsf_transcription(acados_ocp, print_info):
+    ## Description
+    # this function takes a model of an implicit ODE/ index-1 DAE and sets up
+    # an equivalent model in the GNSF structure, with empty linear output
+    # system and trivial model matrices, i.e. A, B, E, c are zeros, and C is
+    # eye. - no structure is exploited
+
+    model = acados_ocp.model
+    # initial print
+    print("*****************************************************************")
+    print(" ")
+    print(f"******      Restructuring {model.name} model    ***********")
+    print(" ")
+    print("*****************************************************************")
+
+    # load model
+    f_impl_expr = model.f_impl_expr
+
+    model_name_prefix = model.name
+
+    # x
+    x = model.x
+    nx = acados_ocp.dims.nx
+    # check type
+    if isinstance(x[0], SX):
+        isSX = True
+    else:
+        print("GNSF detection only works for SX CasADi type!!!")
+        import pdb
+
+        pdb.set_trace()
+    # xdot
+    xdot = model.xdot
+    # u
+    nu = acados_ocp.dims.nu
+    if nu == 0:
+        u = SX.sym("u", 0, 0)
+    else:
+        u = model.u
+
+    nz = acados_ocp.dims.nz
+    if nz == 0:
+        z = SX.sym("z", 0, 0)
+    else:
+        z = model.z
+
+    p = model.p
+    nparam = acados_ocp.dims.np
+
+    # avoid SX of size 0x1
+    if casadi_length(u) == 0:
+        u = SX.sym("u", 0, 0)
+        nu = 0
+    ## initialize gnsf struct
+    # dimensions
+    gnsf = {"nx": nx, "nu": nu, "nz": nz, "np": nparam}
+    gnsf["nx1"] = nx
+    gnsf["nx2"] = 0
+    gnsf["nz1"] = nz
+    gnsf["nz2"] = 0
+    gnsf["nuhat"] = nu
+    gnsf["ny"] = 2 * nx + nz
+
+    gnsf["phi_expr"] = f_impl_expr
+    gnsf["A"] = np.zeros((nx + nz, nx))
+    gnsf["B"] = np.zeros((nx + nz, nu))
+    gnsf["E"] = np.zeros((nx + nz, nx + nz))
+    gnsf["c"] = np.zeros((nx + nz, 1))
+    gnsf["C"] = np.eye(nx + nz)
+    gnsf["name"] = model_name_prefix
+
+    gnsf["x"] = x
+    gnsf["xdot"] = xdot
+    gnsf["z"] = z
+    gnsf["u"] = u
+    gnsf["p"] = p
+
+    gnsf = determine_input_nonlinearity_function(gnsf)
+
+    gnsf["A_LO"] = []
+    gnsf["E_LO"] = []
+    gnsf["B_LO"] = []
+    gnsf["c_LO"] = []
+    gnsf["f_lo_expr"] = []
+
+    # permutation
+    gnsf["idx_perm_x"] = range(nx)  # matlab-style)
+    gnsf["ipiv_x"] = idx_perm_to_ipiv(gnsf["idx_perm_x"])  # blasfeo-style
+    gnsf["idx_perm_z"] = range(nz)
+    gnsf["ipiv_z"] = idx_perm_to_ipiv(gnsf["idx_perm_z"])
+    gnsf["idx_perm_f"] = range((nx + nz))
+    gnsf["ipiv_f"] = idx_perm_to_ipiv(gnsf["idx_perm_f"])
+
+    gnsf["nontrivial_f_LO"] = 0
+
+    check_reformulation(model, gnsf, print_info)
+    if print_info:
+        print(f"Success: Set up equivalent GNSF model with trivial matrices")
+        print(" ")
+    if print_info:
+        print(
+            "-----------------------------------------------------------------------------------"
+        )
+        print(" ")
+        print(
+            "reduced input ny    of phi from  ",
+            str(2 * nx + nz),
+            "   to  ",
+            str(gnsf["ny"]),
+        )
+        print(
+            "reduced input nuhat of phi from  ", str(nu), "   to  ", str(gnsf["nuhat"])
+        )
+        print(" ")
+        print(
+            "-----------------------------------------------------------------------------------"
+        )
+    return gnsf
diff --git a/third_party/acados/acados_template/gnsf/matlab to python.md b/third_party/acados/acados_template/gnsf/matlab to python.md
new file mode 100644
index 0000000000..53a0ed971e
--- /dev/null
+++ b/third_party/acados/acados_template/gnsf/matlab to python.md	
@@ -0,0 +1,43 @@
+# matlab to python
+
+% -> #
+
+; -> 
+
+from casadi import *
+->
+from casadi import *
+
+
+print\('(.*)'\)
+print('$1')
+
+print\(\['(.*)'\]\)
+print(f'$1')
+
+keyboard
+import pdb; pdb.set_trace()
+
+
+range((([^))]*))
+range($1)
+
+\s*end
+->
+nothing
+
+
+if (.*)
+if $1:
+
+else
+else:
+
+num2str
+str
+
+for ([a-z_]*) =
+for $1 in
+
+length\(
+len(
\ No newline at end of file
diff --git a/third_party/acados/acados_template/gnsf/reformulate_with_LOS.py b/third_party/acados/acados_template/gnsf/reformulate_with_LOS.py
new file mode 100644
index 0000000000..297a56556c
--- /dev/null
+++ b/third_party/acados/acados_template/gnsf/reformulate_with_LOS.py
@@ -0,0 +1,394 @@
+#
+# Copyright (c) The acados authors.
+#
+# 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.;
+#
+#   Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com
+
+from .determine_input_nonlinearity_function import determine_input_nonlinearity_function
+from .check_reformulation import check_reformulation
+from casadi import *
+from ..utils import casadi_length, idx_perm_to_ipiv, is_empty
+
+
+def reformulate_with_LOS(acados_ocp, gnsf, print_info):
+
+    ## Description:
+    # This function takes an intitial transcription of the implicit ODE model
+    # "model" into "gnsf" and reformulates "gnsf" with a linear output system
+    # (LOS), containing as many states of the model as possible.
+    # Therefore it might be that the state vector and the implicit function
+    # vector have to be reordered. This reordered model is part of the output,
+    # namely reordered_model.
+
+    ## import CasADi and load models
+    model = acados_ocp.model
+
+    # symbolics
+    x = gnsf["x"]
+    xdot = gnsf["xdot"]
+    u = gnsf["u"]
+    z = gnsf["z"]
+
+    # dimensions
+    nx = gnsf["nx"]
+    nz = gnsf["nz"]
+
+    # get model matrices
+    A = gnsf["A"]
+    B = gnsf["B"]
+    C = gnsf["C"]
+    E = gnsf["E"]
+    c = gnsf["c"]
+
+    A_LO = gnsf["A_LO"]
+
+    y = gnsf["y"]
+
+    phi_old = gnsf["phi_expr"]
+
+    if print_info:
+        print(" ")
+        print("=================================================================")
+        print(" ")
+        print("================    Detect Linear Output System   ===============")
+        print(" ")
+        print("=================================================================")
+        print(" ")
+    ## build initial I_x1 and I_x2_candidates
+    # I_xrange( all components of x for which either xii or xdot_ii enters y):
+    # I_LOS_candidates: the remaining components
+
+    I_nsf_components = set()
+    I_LOS_candidates = set()
+
+    if gnsf["ny"] > 0:
+        for ii in range(nx):
+            if which_depends(y, x[ii])[0] or which_depends(y, xdot[ii])[0]:
+                # i.e. xii or xiidot are part of y, and enter phi_expr
+                if print_info:
+                    print(f"x_{ii} is part of x1")
+                I_nsf_components = set.union(I_nsf_components, set([ii]))
+            else:
+                # i.e. neither xii nor xiidot are part of y, i.e. enter phi_expr
+                I_LOS_candidates = set.union(I_LOS_candidates, set([ii]))
+                if print_info:
+                    print(" ")
+        for ii in range(nz):
+            if which_depends(y, z[ii])[0]:
+                # i.e. xii or xiidot are part of y, and enter phi_expr
+                if print_info:
+                    print(f"z_{ii} is part of x1")
+                I_nsf_components = set.union(I_nsf_components, set([ii + nx]))
+            else:
+                # i.e. neither xii nor xiidot are part of y, i.e. enter phi_expr
+                I_LOS_candidates = set.union(I_LOS_candidates, set([ii + nx]))
+    else:
+        I_LOS_candidates = set(range((nx + nz)))
+    if print_info:
+        print(" ")
+        print(f"I_LOS_candidates {I_LOS_candidates}")
+    new_nsf_components = I_nsf_components
+    I_nsf_eq = set([])
+    unsorted_dyn = set(range(nx + nz))
+    xdot_z = vertcat(xdot, z)
+
+    ## determine components of Linear Output System
+    # determine maximal index set I_x2
+    # such that the components x(I_x2) can be written as a LOS
+    Eq_map = []
+    while True:
+        ## find equations corresponding to new_nsf_components
+        for ii in new_nsf_components:
+            current_var = xdot_z[ii]
+            var_name = current_var.name
+
+            # print( unsorted_dyn)
+            # print("np.nonzero(E[:,ii])[0]",np.nonzero(E[:,ii])[0])
+            I_eq = set.intersection(set(np.nonzero(E[:, ii])[0]), unsorted_dyn)
+            if len(I_eq) == 1:
+                i_eq = I_eq.pop()
+                if print_info:
+                    print(f"component {i_eq} is associated with state {ii}")
+            elif len(I_eq) > 1:  # x_ii_dot occurs in more than 1 eq linearly
+                # find the equation with least linear dependencies on
+                # I_LOS_cancidates
+                number_of_eq = 0
+                candidate_dependencies = np.zeros(len(I_eq), 1)
+                I_x2_candidates = set.intersection(I_LOS_candidates, set(range(nx)))
+                for eq in I_eq:
+                    depending_candidates = set.union(
+                        np.nonzero(E[eq, I_LOS_candidates])[0],
+                        np.nonzero(A[eq, I_x2_candidates])[0],
+                    )
+                    candidate_dependencies[number_of_eq] = +len(depending_candidates)
+                    number_of_eq += 1
+                    number_of_eq = np.argmin(candidate_dependencies)
+                i_eq = I_eq[number_of_eq]
+            else:  ## x_ii_dot does not occur linearly in any of the unsorted dynamics
+                for j in unsorted_dyn:
+                    phi_eq_j = gnsf["phi_expr"][np.nonzero(C[j, :])[0]]
+                    if which_depends(phi_eq_j, xdot_z(ii))[0]:
+                        I_eq = set.union(I_eq, j)
+                if is_empty(I_eq):
+                    I_eq = unsorted_dyn
+                # find the equation with least linear dependencies on I_LOS_cancidates
+                number_of_eq = 0
+                candidate_dependencies = np.zeros(len(I_eq), 1)
+                I_x2_candidates = set.intersection(I_LOS_candidates, set(range(nx)))
+                for eq in I_eq:
+                    depending_candidates = set.union(
+                        np.nonzero(E[eq, I_LOS_candidates])[0],
+                        np.nonzero(A[eq, I_x2_candidates])[0],
+                    )
+                    candidate_dependencies[number_of_eq] = +len(depending_candidates)
+                    number_of_eq += 1
+                    number_of_eq = np.argmin(candidate_dependencies)
+                i_eq = I_eq[number_of_eq]
+                ## add 1 * [xdot,z](ii) to both sides of i_eq
+                if print_info:
+                    print(
+                        "adding 1 * ",
+                        var_name,
+                        " to both sides of equation ",
+                        i_eq,
+                        ".",
+                    )
+                gnsf["E"][i_eq, ii] = 1
+                i_phi = np.nonzero(gnsf["C"][i_eq, :])
+                if is_empty(i_phi):
+                    i_phi = len(gnsf["phi_expr"]) + 1
+                    gnsf["C"][i_eq, i_phi] = 1  # add column to C with 1 entry
+                    gnsf["phi_expr"] = vertcat(gnsf["phi_expr"], 0)
+                    gnsf["phi_expr"][i_phi] = (
+                        gnsf["phi_expr"](i_phi)
+                        + gnsf["E"][i_eq, ii] / gnsf["C"][i_eq, i_phi] * xdot_z[ii]
+                    )
+                if print_info:
+                    print(
+                        "detected equation ",
+                        i_eq,
+                        " to correspond to variable ",
+                        var_name,
+                    )
+            I_nsf_eq = set.union(I_nsf_eq, {i_eq})
+            # remove i_eq from unsorted_dyn
+            unsorted_dyn.remove(i_eq)
+            Eq_map.append([ii, i_eq])
+
+        ## add components to I_x1
+        for eq in I_nsf_eq:
+            I_linear_dependence = set.union(
+                set(np.nonzero(A[eq, :])[0]), set(np.nonzero(E[eq, :])[0])
+            )
+            I_nsf_components = set.union(I_linear_dependence, I_nsf_components)
+            # I_nsf_components = I_nsf_components[:]
+
+        new_nsf_components = set.intersection(I_LOS_candidates, I_nsf_components)
+        if is_empty(new_nsf_components):
+            if print_info:
+                print("new_nsf_components is empty")
+            break
+        # remove new_nsf_components from candidates
+        I_LOS_candidates = set.difference(I_LOS_candidates, new_nsf_components)
+    if not is_empty(Eq_map):
+        # [~, new_eq_order] = sort(Eq_map(1,:))
+        # I_nsf_eq = Eq_map(2, new_eq_order )
+        for count, m in enumerate(Eq_map):
+            m.append(count)
+        sorted(Eq_map, key=lambda x: x[1])
+        new_eq_order = [m[2] for m in Eq_map]
+        Eq_map = [Eq_map[i] for i in new_eq_order]
+        I_nsf_eq = [m[1] for m in Eq_map]
+
+    else:
+        I_nsf_eq = []
+
+    I_LOS_components = I_LOS_candidates
+    I_LOS_eq = sorted(set.difference(set(range(nx + nz)), I_nsf_eq))
+    I_nsf_eq = sorted(I_nsf_eq)
+
+    I_x1 = set.intersection(I_nsf_components, set(range(nx)))
+    I_z1 = set.intersection(I_nsf_components, set(range(nx, nx + nz)))
+    I_z1 = set([i - nx for i in I_z1])
+
+    I_x2 = set.intersection(I_LOS_components, set(range(nx)))
+    I_z2 = set.intersection(I_LOS_components, set(range(nx, nx + nz)))
+    I_z2 = set([i - nx for i in I_z2])
+
+    if print_info:
+        print(f"I_x1 {I_x1}, I_x2 {I_x2}")
+
+    ## permute x, xdot
+    if is_empty(I_x1):
+        x1 = []
+        x1dot = []
+    else:
+        x1 = x[list(I_x1)]
+        x1dot = xdot[list(I_x1)]
+    if is_empty(I_x2):
+        x2 = []
+        x2dot = []
+    else:
+        x2 = x[list(I_x2)]
+        x2dot = xdot[list(I_x2)]
+    if is_empty(I_z1):
+        z1 = []
+    else:
+        z1 = z(I_z1)
+    if is_empty(I_z2):
+        z2 = []
+    else:
+        z2 = z[list(I_z2)]
+
+    I_x1 = sorted(I_x1)
+    I_x2 = sorted(I_x2)
+    I_z1 = sorted(I_z1)
+    I_z2 = sorted(I_z2)
+    gnsf["xdot"] = vertcat(x1dot, x2dot)
+    gnsf["x"] = vertcat(x1, x2)
+    gnsf["z"] = vertcat(z1, z2)
+    gnsf["nx1"] = len(I_x1)
+    gnsf["nx2"] = len(I_x2)
+    gnsf["nz1"] = len(I_z1)
+    gnsf["nz2"] = len(I_z2)
+
+    # store permutations
+    gnsf["idx_perm_x"] = I_x1 + I_x2
+    gnsf["ipiv_x"] = idx_perm_to_ipiv(gnsf["idx_perm_x"])
+    gnsf["idx_perm_z"] = I_z1 + I_z2
+    gnsf["ipiv_z"] = idx_perm_to_ipiv(gnsf["idx_perm_z"])
+    gnsf["idx_perm_f"] = I_nsf_eq + I_LOS_eq
+    gnsf["ipiv_f"] = idx_perm_to_ipiv(gnsf["idx_perm_f"])
+
+    f_LO = SX.sym("f_LO", 0, 0)
+
+    ## rewrite I_LOS_eq as LOS
+    if gnsf["n_out"] == 0:
+        C_phi = np.zeros(gnsf["nx"] + gnsf["nz"], 1)
+    else:
+        C_phi = C @ phi_old
+    if gnsf["nx1"] == 0:
+        Ax1 = np.zeros(gnsf["nx"] + gnsf["nz"], 1)
+    else:
+        Ax1 = A[:, sorted(I_x1)] @ x1
+    if gnsf["nx1"] + gnsf["nz1"] == 0:
+        lhs_nsf = np.zeros(gnsf["nx"] + gnsf["nz"], 1)
+    else:
+        lhs_nsf = E[:, sorted(I_nsf_components)] @ vertcat(x1, z1)
+    n_LO = len(I_LOS_eq)
+    B_LO = np.zeros((n_LO, gnsf["nu"]))
+    A_LO = np.zeros((gnsf["nx2"] + gnsf["nz2"], gnsf["nx2"]))
+    E_LO = np.zeros((n_LO, n_LO))
+    c_LO = np.zeros((n_LO, 1))
+
+    I_LOS_eq = list(I_LOS_eq)
+    for eq in I_LOS_eq:
+        i_LO = I_LOS_eq.index(eq)
+        f_LO = vertcat(f_LO, Ax1[eq] + C_phi[eq] - lhs_nsf[eq])
+        print(f"eq {eq} I_LOS_components {I_LOS_components}, i_LO {i_LO}, f_LO {f_LO}")
+        E_LO[i_LO, :] = E[eq, sorted(I_LOS_components)]
+        A_LO[i_LO, :] = A[eq, I_x2]
+        c_LO[i_LO, :] = c[eq]
+        B_LO[i_LO, :] = B[eq, :]
+    if casadi_length(f_LO) == 0:
+        f_LO = SX.zeros((gnsf["nx2"] + gnsf["nz2"], 1))
+    f_LO = simplify(f_LO)
+    gnsf["A_LO"] = A_LO
+    gnsf["E_LO"] = E_LO
+    gnsf["B_LO"] = B_LO
+    gnsf["c_LO"] = c_LO
+    gnsf["f_lo_expr"] = f_LO
+
+    ## remove I_LOS_eq from NSF type system
+    gnsf["A"] = gnsf["A"][np.ix_(sorted(I_nsf_eq), sorted(I_x1))]
+    gnsf["B"] = gnsf["B"][sorted(I_nsf_eq), :]
+    gnsf["C"] = gnsf["C"][sorted(I_nsf_eq), :]
+    gnsf["E"] = gnsf["E"][np.ix_(sorted(I_nsf_eq), sorted(I_nsf_components))]
+    gnsf["c"] = gnsf["c"][sorted(I_nsf_eq), :]
+
+    ## reduce phi, C
+    I_nonzero = []
+    for ii in range(gnsf["C"].shape[1]):  # n_colums of C:
+        print(f"ii {ii}")
+        if not all(gnsf["C"][:, ii] == 0):  # if column ~= 0
+            I_nonzero.append(ii)
+    gnsf["C"] = gnsf["C"][:, I_nonzero]
+    gnsf["phi_expr"] = gnsf["phi_expr"][I_nonzero]
+
+    gnsf = determine_input_nonlinearity_function(gnsf)
+
+    check_reformulation(model, gnsf, print_info)
+
+    gnsf["nontrivial_f_LO"] = 0
+    if not is_empty(gnsf["f_lo_expr"]):
+        for ii in range(casadi_length(gnsf["f_lo_expr"])):
+            fii = gnsf["f_lo_expr"][ii]
+            if not fii.is_zero():
+                gnsf["nontrivial_f_LO"] = 1
+            if not gnsf["nontrivial_f_LO"] and print_info:
+                print("f_LO is fully trivial (== 0)")
+    check_reformulation(model, gnsf, print_info)
+
+    if print_info:
+        print("")
+        print(
+            "---------------------------------------------------------------------------------"
+        )
+        print(
+            "------------- Success: Linear Output System (LOS) detected ----------------------"
+        )
+        print(
+            "---------------------------------------------------------------------------------"
+        )
+        print("")
+        print(
+            "==>>  moved  ",
+            gnsf["nx2"],
+            "differential states and ",
+            gnsf["nz2"],
+            " algebraic variables to the Linear Output System",
+        )
+        print(
+            "==>>  recuced output dimension of phi from  ",
+            casadi_length(phi_old),
+            " to ",
+            casadi_length(gnsf["phi_expr"]),
+        )
+        print(" ")
+        print("Matrices defining the LOS read as")
+        print(" ")
+        print("E_LO =")
+        print(gnsf["E_LO"])
+        print("A_LO =")
+        print(gnsf["A_LO"])
+        print("B_LO =")
+        print(gnsf["B_LO"])
+        print("c_LO =")
+        print(gnsf["c_LO"])
+
+    return gnsf
diff --git a/third_party/acados/acados_template/gnsf/reformulate_with_invertible_E_mat.py b/third_party/acados/acados_template/gnsf/reformulate_with_invertible_E_mat.py
new file mode 100644
index 0000000000..21ab8ebfd5
--- /dev/null
+++ b/third_party/acados/acados_template/gnsf/reformulate_with_invertible_E_mat.py
@@ -0,0 +1,167 @@
+#
+# Copyright (c) The acados authors.
+#
+# 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.;
+#
+#   Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com
+
+from casadi import *
+from .determine_input_nonlinearity_function import determine_input_nonlinearity_function
+from .check_reformulation import check_reformulation
+
+
+def reformulate_with_invertible_E_mat(gnsf, model, print_info):
+    ## Description
+    # this function checks that the necessary condition to apply the gnsf
+    # structure exploiting integrator to a model, namely that the matrices E11,
+    # E22 are invertible holds.
+    # if this is not the case, it will make these matrices invertible and add:
+    # corresponding terms, to the term C * phi, such that the obtained model is
+    # still equivalent
+
+    # check invertibility of E11, E22 and reformulate if needed:
+    ind_11 = range(gnsf["nx1"])
+    ind_22 = range(gnsf["nx1"], gnsf["nx1"] + gnsf["nz1"])
+
+    if print_info:
+        print(" ")
+        print("----------------------------------------------------")
+        print("checking rank of E11 and E22")
+        print("----------------------------------------------------")
+    ## check if E11, E22 are invertible:
+    z_check = False
+    if gnsf["nz1"] > 0:
+        z_check = (
+            np.linalg.matrix_rank(gnsf["E"][np.ix_(ind_22, ind_22)]) != gnsf["nz1"]
+        )
+
+    if (
+        np.linalg.matrix_rank(gnsf["E"][np.ix_(ind_11, ind_11)]) != gnsf["nx1"]
+        or z_check
+    ):
+        # print warning (always)
+        print(f"the rank of E11 or E22 is not full after the reformulation")
+        print("")
+        print(
+            f"the script will try to reformulate the model with an invertible matrix instead"
+        )
+        print(
+            f"NOTE: this feature is based on a heuristic, it should be used with care!!!"
+        )
+
+        ## load models
+        xdot = gnsf["xdot"]
+        z = gnsf["z"]
+
+        # # GNSF
+        # get dimensions
+        nx1 = gnsf["nx1"]
+        x1dot = xdot[range(nx1)]
+
+        k = vertcat(x1dot, z)
+        for i in [1, 2]:
+            if i == 1:
+                ind = range(gnsf["nx1"])
+            else:
+                ind = range(gnsf["nx1"], gnsf["nx1"] + gnsf["nz1"])
+            mat = gnsf["E"][np.ix_(ind, ind)]
+            import pdb
+
+            pdb.set_trace()
+            while np.linalg.matrix_rank(mat) < len(ind):
+                # import pdb; pdb.set_trace()
+                if print_info:
+                    print(" ")
+                    print(f"the rank of E", str(i), str(i), " is not full")
+                    print(
+                        f"the algorithm will try to reformulate the model with an invertible matrix instead"
+                    )
+                    print(
+                        f"NOTE: this feature is not super stable and might need more testing!!!!!!"
+                    )
+                for sub_max in ind:
+                    sub_ind = range(min(ind), sub_max)
+                    # regard the submatrix mat(sub_ind, sub_ind)
+                    sub_mat = gnsf["E"][sub_ind, sub_ind]
+                    if np.linalg.matrix_rank(sub_mat) < len(sub_ind):
+                        # reformulate the model by adding a 1 to last diagonal
+                        # element and changing rhs respectively.
+                        gnsf["E"][sub_max, sub_max] = gnsf["E"][sub_max, sub_max] + 1
+                        # this means adding the term 1 * k(sub_max) to the sub_max
+                        # row of the l.h.s
+                        if len(np.nonzero(gnsf["C"][sub_max, :])[0]) == 0:
+                            # if isempty(find(gnsf['C'](sub_max,:), 1)):
+                            # add new nonlinearity entry
+                            gnsf["C"][sub_max, gnsf["n_out"] + 1] = 1
+                            gnsf["phi_expr"] = vertcat(gnsf["phi_expr"], k[sub_max])
+                        else:
+                            ind_f = np.nonzero(gnsf["C"][sub_max, :])[0]
+                            if len(ind_f) != 1:
+                                raise Exception("C is assumed to be a selection matrix")
+                            else:
+                                ind_f = ind_f[0]
+                            # add term to corresponding nonlinearity entry
+                            # note: herbey we assume that C is a selection matrix,
+                            # i.e. gnsf['phi_expr'](ind_f) is only entering one equation
+
+                            gnsf["phi_expr"][ind_f] = (
+                                gnsf["phi_expr"][ind_f]
+                                + k[sub_max] / gnsf["C"][sub_max, ind_f]
+                            )
+                            gnsf = determine_input_nonlinearity_function(gnsf)
+                            check_reformulation(model, gnsf, print_info)
+        print("successfully reformulated the model with invertible matrices E11, E22")
+    else:
+        if print_info:
+            print(" ")
+            print(
+                "the rank of both E11 and E22 is naturally full after the reformulation "
+            )
+            print("==>  model reformulation finished")
+            print(" ")
+    if (gnsf['nx2'] > 0 or gnsf['nz2'] > 0) and det(gnsf["E_LO"]) == 0:
+        print(
+            "_______________________________________________________________________________________________________"
+        )
+        print(" ")
+        print("TAKE CARE ")
+        print("E_LO matrix is NOT regular after automatic transcription!")
+        print("->> this means the model CANNOT be used with the gnsf integrator")
+        print(
+            "->> it probably means that one entry (of xdot or z) that was moved to the linear output type system"
+        )
+        print("    does not appear in the model at all (zero column in E_LO)")
+        print(" OR: the columns of E_LO are linearly dependent ")
+        print(" ")
+        print(
+            " SOLUTIONs: a) go through your model & check equations the method wanted to move to LOS"
+        )
+        print("            b) deactivate the detect_LOS option")
+        print(
+            "_______________________________________________________________________________________________________"
+        )
+    return gnsf
diff --git a/third_party/acados/acados_template/gnsf/structure_detection_print_summary.py b/third_party/acados/acados_template/gnsf/structure_detection_print_summary.py
new file mode 100644
index 0000000000..db2d18758e
--- /dev/null
+++ b/third_party/acados/acados_template/gnsf/structure_detection_print_summary.py
@@ -0,0 +1,174 @@
+#
+# Copyright (c) The acados authors.
+#
+# 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.;
+#
+#   Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com
+
+from casadi import n_nodes
+import numpy as np
+
+
+def structure_detection_print_summary(gnsf, acados_ocp):
+
+    ## Description
+    # this function prints the most important info after determining a GNSF
+    # reformulation of the implicit model "initial_model" into "gnsf", which is
+    # equivalent to the "reordered_model".
+    model = acados_ocp.model
+    # # GNSF
+    # get dimensions
+    nx = gnsf["nx"]
+    nu = gnsf["nu"]
+    nz = gnsf["nz"]
+
+    nx1 = gnsf["nx1"]
+    nx2 = gnsf["nx2"]
+
+    nz1 = gnsf["nz1"]
+    nz2 = gnsf["nz2"]
+
+    # np = gnsf['np']
+    n_out = gnsf["n_out"]
+    ny = gnsf["ny"]
+    nuhat = gnsf["nuhat"]
+
+    #
+    f_impl_expr = model.f_impl_expr
+    n_nodes_initial = n_nodes(model.f_impl_expr)
+    # x_old = model.x
+    # f_impl_old = model.f_impl_expr
+
+    x = gnsf["x"]
+    z = gnsf["z"]
+
+    phi_current = gnsf["phi_expr"]
+
+    ## PRINT SUMMARY -- STRUCHTRE DETECTION
+    print(" ")
+    print(
+        "*********************************************************************************************"
+    )
+    print(" ")
+    print(
+        "******************        SUCCESS: GNSF STRUCTURE DETECTION COMPLETE !!!      ***************"
+    )
+    print(" ")
+    print(
+        "*********************************************************************************************"
+    )
+    print(" ")
+    print(
+        f"========================= STRUCTURE DETECTION SUMMARY ===================================="
+    )
+    print(" ")
+    print("-------- Nonlinear Static Feedback type system --------")
+    print(" ")
+    print(" successfully transcribed dynamic system model into GNSF structure ")
+    print(" ")
+    print(
+        "reduced dimension of nonlinearity phi from        ",
+        str(nx + nz),
+        " to ",
+        str(gnsf["n_out"]),
+    )
+    print(" ")
+    print(
+        "reduced input dimension of nonlinearity phi from  ",
+        2 * nx + nz + nu,
+        " to ",
+        gnsf["ny"] + gnsf["nuhat"],
+    )
+    print(" ")
+    print(f"reduced number of nodes in CasADi expression of nonlinearity phi from  {n_nodes_initial}  to  {n_nodes(phi_current)}\n")
+    print("----------- Linear Output System (LOS) ---------------")
+    if nx2 + nz2 > 0:
+        print(" ")
+        print(f"introduced Linear Output System of size           ", str(nx2 + nz2))
+        print(" ")
+        if nx2 > 0:
+            print("consisting of the states:")
+            print(" ")
+            print(x[range(nx1, nx)])
+            print(" ")
+        if nz2 > 0:
+            print("and algebraic variables:")
+            print(" ")
+            print(z[range(nz1, nz)])
+            print(" ")
+        if gnsf["purely_linear"] == 1:
+            print(" ")
+            print("Model is fully linear!")
+            print(" ")
+    if not all(gnsf["idx_perm_x"] == np.array(range(nx))):
+        print(" ")
+        print(
+            "--------------------------------------------------------------------------------------------------"
+        )
+        print(
+            "NOTE: permuted differential state vector x, such that x_gnsf = x(idx_perm_x) with idx_perm_x ="
+        )
+        print(" ")
+        print(gnsf["idx_perm_x"])
+    if nz != 0 and not all(gnsf["idx_perm_z"] == np.array(range(nz))):
+        print(" ")
+        print(
+            "--------------------------------------------------------------------------------------------------"
+        )
+        print(
+            "NOTE: permuted algebraic state vector z, such that z_gnsf = z(idx_perm_z) with idx_perm_z ="
+        )
+        print(" ")
+        print(gnsf["idx_perm_z"])
+    if not all(gnsf["idx_perm_f"] == np.array(range(nx + nz))):
+        print(" ")
+        print(
+            "--------------------------------------------------------------------------------------------------"
+        )
+        print(
+            "NOTE: permuted rhs expression vector f, such that f_gnsf = f(idx_perm_f) with idx_perm_f ="
+        )
+        print(" ")
+        print(gnsf["idx_perm_f"])
+    ## print GNSF dimensions
+    print(
+        "--------------------------------------------------------------------------------------------------------"
+    )
+    print(" ")
+    print("The dimensions of the GNSF reformulated model read as:")
+    print(" ")
+    # T_dim = table(nx, nu, nz, np, nx1, nz1, n_out, ny, nuhat)
+    # print( T_dim )
+    print(f"nx    ", {nx})
+    print(f"nu    ", {nu})
+    print(f"nz    ", {nz})
+    # print(f"np    ", {np})
+    print(f"nx1   ", {nx1})
+    print(f"nz1   ", {nz1})
+    print(f"n_out ", {n_out})
+    print(f"ny    ", {ny})
+    print(f"nuhat ", {nuhat})
diff --git a/third_party/acados/acados_template/simulink_default_opts.json b/third_party/acados/acados_template/simulink_default_opts.json
index 70c939cb88..5d178fef85 100644
--- a/third_party/acados/acados_template/simulink_default_opts.json
+++ b/third_party/acados/acados_template/simulink_default_opts.json
@@ -4,7 +4,9 @@
         "utraj": 0,
         "xtraj": 0,
         "solver_status": 1,
+        "cost_value": 0,
         "KKT_residual": 1,
+        "KKT_residuals": 0,
         "x1": 1,
         "CPU_time": 1,
         "CPU_time_sim": 0,
@@ -29,6 +31,8 @@
         "ug": 1,
         "lh": 1,
         "uh": 1,
+        "lh_e": 1,
+        "uh_e": 1,
         "cost_W_0": 0,
         "cost_W": 0,
         "cost_W_e": 0,
diff --git a/third_party/acados/acados_template/utils.py b/third_party/acados/acados_template/utils.py
index 8f44f61e7e..d6f6c02f84 100644
--- a/third_party/acados/acados_template/utils.py
+++ b/third_party/acados/acados_template/utils.py
@@ -1,9 +1,6 @@
 # -*- coding: future_fstrings -*-
 #
-# 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
+# Copyright (c) The acados authors.
 #
 # This file is part of acados.
 #
@@ -38,10 +35,17 @@ import shutil
 import numpy as np
 from casadi import SX, MX, DM, Function, CasadiMeta
 
-ALLOWED_CASADI_VERSIONS = ('3.5.5', '3.5.4', '3.5.3', '3.5.2', '3.5.1', '3.4.5', '3.4.0')
+ALLOWED_CASADI_VERSIONS = ('3.5.6', '3.5.5', '3.5.4', '3.5.3', '3.5.2', '3.5.1', '3.4.5', '3.4.0')
 
 TERA_VERSION = "0.0.34"
 
+PLATFORM2TERA = {
+    "linux": "linux",
+    "darwin": "osx",
+    "win32": "windows"
+}
+
+
 def get_acados_path():
     ACADOS_PATH = os.environ.get('ACADOS_SOURCE_DIR')
     if not ACADOS_PATH:
@@ -72,20 +76,17 @@ def get_tera_exec_path():
     return TERA_PATH
 
 
-platform2tera = {
-    "linux": "linux",
-    "darwin": "osx",
-    "win32": "windows"
-}
-
-
-def casadi_version_warning(casadi_version):
-    msg =  'Warning: Please note that the following versions of CasADi  are '
-    msg += 'officially supported: {}.\n '.format(" or ".join(ALLOWED_CASADI_VERSIONS))
-    msg += 'If there is an incompatibility with the CasADi generated code, '
-    msg += 'please consider changing your CasADi version.\n'
-    msg += 'Version {} currently in use.'.format(casadi_version)
-    print(msg)
+def check_casadi_version():
+    casadi_version = CasadiMeta.version()
+    if casadi_version in ALLOWED_CASADI_VERSIONS:
+        return
+    else:
+        msg =  'Warning: Please note that the following versions of CasADi  are '
+        msg += 'officially supported: {}.\n '.format(" or ".join(ALLOWED_CASADI_VERSIONS))
+        msg += 'If there is an incompatibility with the CasADi generated code, '
+        msg += 'please consider changing your CasADi version.\n'
+        msg += 'Version {} currently in use.'.format(casadi_version)
+        print(msg)
 
 
 def is_column(x):
@@ -118,11 +119,16 @@ def is_empty(x):
             return True
         else:
             return False
-    elif x == None or x == []:
+    elif x == None:
         return True
+    elif isinstance(x, (set, list)):
+        if len(x)==0:
+            return True
+        else:
+            return False
     else:
         raise Exception("is_empty expects one of the following types: casadi.MX, casadi.SX, "
-                        + "None, numpy array empty list. Got: " + str(type(x)))
+                        + "None, numpy array empty list, set. Got: " + str(type(x)))
 
 
 def casadi_length(x):
@@ -155,6 +161,14 @@ def make_model_consistent(model):
 
     return model
 
+def get_lib_ext():
+    lib_ext = '.so'
+    if sys.platform == 'darwin':
+        lib_ext = '.dylib'
+    elif os.name == 'nt':
+        lib_ext = ''
+
+    return lib_ext
 
 def get_tera():
     tera_path = get_tera_exec_path()
@@ -165,7 +179,7 @@ def get_tera():
 
     repo_url = "https://github.com/acados/tera_renderer/releases"
     url = "{}/download/v{}/t_renderer-v{}-{}".format(
-        repo_url, TERA_VERSION, TERA_VERSION, platform2tera[sys.platform])
+        repo_url, TERA_VERSION, TERA_VERSION, PLATFORM2TERA[sys.platform])
 
     manual_install = 'For manual installation follow these instructions:\n'
     manual_install += '1 Download binaries from {}\n'.format(url)
@@ -200,17 +214,18 @@ def get_tera():
     sys.exit(1)
 
 
-def render_template(in_file, out_file, template_dir, json_path):
+def render_template(in_file, out_file, output_dir, json_path, template_glob=None):
+
+    acados_path = os.path.dirname(os.path.abspath(__file__))
+    if template_glob is None:
+        template_glob = os.path.join(acados_path, 'c_templates_tera', '**', '*')
     cwd = os.getcwd()
-    if not os.path.exists(template_dir):
-        os.mkdir(template_dir)
-    os.chdir(template_dir)
 
-    tera_path = get_tera()
+    if not os.path.exists(output_dir):
+        os.makedirs(output_dir)
+    os.chdir(output_dir)
 
-    # setting up loader and environment
-    acados_path = os.path.dirname(os.path.abspath(__file__))
-    template_glob = os.path.join(acados_path, 'c_templates_tera', '*')
+    tera_path = get_tera()
 
     # call tera as system cmd
     os_cmd = f"{tera_path} '{template_glob}' '{in_file}' '{json_path}' '{out_file}'"
@@ -220,21 +235,24 @@ def render_template(in_file, out_file, template_dir, json_path):
 
     status = os.system(os_cmd)
     if (status != 0):
-        raise Exception(f'Rendering of {in_file} failed!\n\nAttempted to execute OS command:\n{os_cmd}\n\nExiting.\n')
+        raise Exception(f'Rendering of {in_file} failed!\n\nAttempted to execute OS command:\n{os_cmd}\n\n')
 
     os.chdir(cwd)
 
 
 ## Conversion functions
-def np_array_to_list(np_array):
-    if isinstance(np_array, (np.ndarray)):
-        return np_array.tolist()
-    elif isinstance(np_array, (SX)):
-        return DM(np_array).full()
-    elif isinstance(np_array, (DM)):
-        return np_array.full()
+def make_object_json_dumpable(input):
+    if isinstance(input, (np.ndarray)):
+        return input.tolist()
+    elif isinstance(input, (SX)):
+        return input.serialize()
+    elif isinstance(input, (MX)):
+        # NOTE: MX expressions can not be serialized, only Functions.
+        return input.__str__()
+    elif isinstance(input, (DM)):
+        return input.full()
     else:
-        raise(Exception(f"Cannot convert to list type {type(np_array)}"))
+        raise TypeError(f"Cannot make input of type {type(input)} dumpable.")
 
 
 def format_class_dict(d):
@@ -251,7 +269,7 @@ def format_class_dict(d):
     return out
 
 
-def get_ocp_nlp_layout():
+def get_ocp_nlp_layout() -> dict:
     python_interface_path = get_python_interface_path()
     abs_path = os.path.join(python_interface_path, 'acados_layout.json')
     with open(abs_path, 'r') as f:
@@ -259,62 +277,12 @@ def get_ocp_nlp_layout():
     return ocp_nlp_layout
 
 
-def ocp_check_against_layout(ocp_nlp, ocp_dims):
-    """
-    Check dimensions against layout
-    Parameters
-    ---------
-    ocp_nlp : dict
-        dictionary loaded from JSON to be post-processed.
-
-    ocp_dims : instance of AcadosOcpDims
-    """
-
-    ocp_nlp_layout = get_ocp_nlp_layout()
-
-    ocp_check_against_layout_recursion(ocp_nlp, ocp_dims, ocp_nlp_layout)
-    return
-
-
-def ocp_check_against_layout_recursion(ocp_nlp, ocp_dims, layout):
-
-    for key, item in ocp_nlp.items():
-
-        try:
-            layout_of_key = layout[key]
-        except KeyError:
-            raise Exception("ocp_check_against_layout_recursion: field" \
-                            " '{0}' is not in layout but in OCP description.".format(key))
-
-        if isinstance(item, dict):
-            ocp_check_against_layout_recursion(item, ocp_dims, layout_of_key)
-
-        if 'ndarray' in layout_of_key:
-            if isinstance(item, int) or isinstance(item, float):
-                item = np.array([item])
-        if isinstance(item, (list, np.ndarray)) and (layout_of_key[0] != 'str'):
-            dim_layout = []
-            dim_names = layout_of_key[1]
-
-            for dim_name in dim_names:
-                dim_layout.append(ocp_dims[dim_name])
-
-            dims = tuple(dim_layout)
-
-            item = np.array(item)
-            item_dims = item.shape
-            if len(item_dims) != len(dims):
-                raise Exception('Mismatching dimensions for field {0}. ' \
-                    'Expected {1} dimensional array, got {2} dimensional array.' \
-                        .format(key, len(dims), len(item_dims)))
-
-            if np.prod(item_dims) != 0 or np.prod(dims) != 0:
-                if dims != item_dims:
-                    raise Exception('acados -- mismatching dimensions for field {0}. ' \
-                        'Provided data has dimensions {1}, ' \
-                        'while associated dimensions {2} are {3}' \
-                            .format(key, item_dims, dim_names, dims))
-    return
+def get_default_simulink_opts() -> dict:
+    python_interface_path = get_python_interface_path()
+    abs_path = os.path.join(python_interface_path, 'simulink_default_opts.json')
+    with open(abs_path, 'r') as f:
+        simulink_opts = json.load(f)
+    return simulink_opts
 
 
 def J_to_idx(J):
@@ -324,9 +292,9 @@ def J_to_idx(J):
         this_idx = np.nonzero(J[i,:])[0]
         if len(this_idx) != 1:
             raise Exception('Invalid J matrix structure detected, ' \
-                'must contain one nonzero element per row. Exiting.')
+                'must contain one nonzero element per row.')
         if this_idx.size > 0 and J[i,this_idx[0]] != 1:
-            raise Exception('J matrices can only contain 1s. Exiting.')
+            raise Exception('J matrices can only contain 1s.')
         idx[i] = this_idx[0]
     return idx
 
@@ -342,7 +310,7 @@ def J_to_idx_slack(J):
             idx[i_idx] = i
             i_idx = i_idx + 1
         elif len(this_idx) > 1:
-            raise Exception('J_to_idx_slack: Invalid J matrix. Exiting. ' \
+            raise Exception('J_to_idx_slack: Invalid J matrix. ' \
                 'Found more than one nonzero in row ' + str(i))
         if this_idx.size > 0 and J[i,this_idx[0]] != 1:
             raise Exception('J_to_idx_slack: J matrices can only contain 1s, ' \
@@ -376,13 +344,13 @@ def acados_dae_model_json_dump(model):
     # dump
     json_file = model_name + '_acados_dae.json'
     with open(json_file, 'w') as f:
-        json.dump(dae_dict, f, default=np_array_to_list, indent=4, sort_keys=True)
+        json.dump(dae_dict, f, default=make_object_json_dumpable, indent=4, sort_keys=True)
     print("dumped ", model_name, " dae to file:", json_file, "\n")
 
 
-def set_up_imported_gnsf_model(acados_formulation):
+def set_up_imported_gnsf_model(acados_ocp):
 
-    gnsf = acados_formulation.gnsf_model
+    gnsf = acados_ocp.gnsf_model
 
     # check CasADi version
     # dump_casadi_version = gnsf['casadi_version']
@@ -402,39 +370,66 @@ def set_up_imported_gnsf_model(acados_formulation):
 
     # obtain gnsf dimensions
     size_gnsf_A = get_matrices_fun.size_out(0)
-    acados_formulation.dims.gnsf_nx1 = size_gnsf_A[1]
-    acados_formulation.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
-    acados_formulation.dims.gnsf_nuhat = max(phi_fun.size_in(1))
-    acados_formulation.dims.gnsf_ny = max(phi_fun.size_in(0))
-    acados_formulation.dims.gnsf_nout = max(phi_fun.size_out(0))
+    acados_ocp.dims.gnsf_nx1 = size_gnsf_A[1]
+    acados_ocp.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
+    acados_ocp.dims.gnsf_nuhat = max(phi_fun.size_in(1))
+    acados_ocp.dims.gnsf_ny = max(phi_fun.size_in(0))
+    acados_ocp.dims.gnsf_nout = max(phi_fun.size_out(0))
 
     # save gnsf functions in model
-    acados_formulation.model.phi_fun = phi_fun
-    acados_formulation.model.phi_fun_jac_y = phi_fun_jac_y
-    acados_formulation.model.phi_jac_y_uhat = phi_jac_y_uhat
-    acados_formulation.model.get_matrices_fun = get_matrices_fun
+    acados_ocp.model.phi_fun = phi_fun
+    acados_ocp.model.phi_fun_jac_y = phi_fun_jac_y
+    acados_ocp.model.phi_jac_y_uhat = phi_jac_y_uhat
+    acados_ocp.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])
+    acados_ocp.model.gnsf['nontrivial_f_LO'] = int(get_matrices_out[12])
+    acados_ocp.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
+        acados_ocp.model.f_lo_fun_jac_x1k1uz = f_lo_fun_jac_x1k1uz
     else:
-        dummy_var_x1 = SX.sym('dummy_var_x1', acados_formulation.dims.gnsf_nx1)
-        dummy_var_x1dot = SX.sym('dummy_var_x1dot', acados_formulation.dims.gnsf_nx1)
-        dummy_var_z1 = SX.sym('dummy_var_z1', acados_formulation.dims.gnsf_nz1)
-        dummy_var_u = SX.sym('dummy_var_z1', acados_formulation.dims.nu)
-        dummy_var_p = SX.sym('dummy_var_z1', acados_formulation.dims.np)
+        dummy_var_x1 = SX.sym('dummy_var_x1', acados_ocp.dims.gnsf_nx1)
+        dummy_var_x1dot = SX.sym('dummy_var_x1dot', acados_ocp.dims.gnsf_nx1)
+        dummy_var_z1 = SX.sym('dummy_var_z1', acados_ocp.dims.gnsf_nz1)
+        dummy_var_u = SX.sym('dummy_var_z1', acados_ocp.dims.nu)
+        dummy_var_p = SX.sym('dummy_var_z1', acados_ocp.dims.np)
         empty_var = SX.sym('empty_var', 0, 0)
 
         empty_fun = Function('empty_fun', \
             [dummy_var_x1, dummy_var_x1dot, dummy_var_z1, dummy_var_u, dummy_var_p],
                 [empty_var])
-        acados_formulation.model.f_lo_fun_jac_x1k1uz = empty_fun
+        acados_ocp.model.f_lo_fun_jac_x1k1uz = empty_fun
+
+    del acados_ocp.gnsf_model
+
+
+def idx_perm_to_ipiv(idx_perm):
+    n = len(idx_perm)
+    vec = list(range(n))
+    ipiv = np.zeros(n)
+
+    print(n, idx_perm)
+    # import pdb; pdb.set_trace()
+    for ii in range(n):
+        idx0 = idx_perm[ii]
+        for jj in range(ii,n):
+            if vec[jj]==idx0:
+                idx1 = jj
+                break
+        tmp = vec[ii]
+        vec[ii] = vec[idx1]
+        vec[idx1] = tmp
+        ipiv[ii] = idx1
+
+    ipiv = ipiv-1 # C 0-based indexing
+    return ipiv
+
 
-    del acados_formulation.gnsf_model
+def print_casadi_expression(f):
+    for ii in range(casadi_length(f)):
+        print(f[ii,:])
diff --git a/third_party/acados/acados_template/zoro_description.py b/third_party/acados/acados_template/zoro_description.py
new file mode 100644
index 0000000000..4d795c1502
--- /dev/null
+++ b/third_party/acados/acados_template/zoro_description.py
@@ -0,0 +1,78 @@
+# Copyright (c) The acados authors.
+#
+# 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.;
+
+from dataclasses import dataclass, field
+import numpy as np
+
+
+@dataclass
+class ZoroDescription:
+    """
+    Zero-Order Robust Optimization scheme.
+
+    For advanced users.
+    """
+    backoff_scaling_gamma: float = 1.0
+    fdbk_K_mat: np.ndarray = None
+    unc_jac_G_mat: np.ndarray = None    # default: an identity matrix
+    P0_mat: np.ndarray = None
+    W_mat: np.ndarray = None
+    idx_lbx_t: list = field(default_factory=list)
+    idx_ubx_t: list = field(default_factory=list)
+    idx_lbx_e_t: list = field(default_factory=list)
+    idx_ubx_e_t: list = field(default_factory=list)
+    idx_lbu_t: list = field(default_factory=list)
+    idx_ubu_t: list = field(default_factory=list)
+    idx_lg_t: list = field(default_factory=list)
+    idx_ug_t: list = field(default_factory=list)
+    idx_lg_e_t: list = field(default_factory=list)
+    idx_ug_e_t: list = field(default_factory=list)
+    idx_lh_t: list = field(default_factory=list)
+    idx_uh_t: list = field(default_factory=list)
+    idx_lh_e_t: list = field(default_factory=list)
+    idx_uh_e_t: list = field(default_factory=list)
+
+def process_zoro_description(zoro_description: ZoroDescription):
+    zoro_description.nw, _ = zoro_description.W_mat.shape
+    if zoro_description.unc_jac_G_mat is None:
+        zoro_description.unc_jac_G_mat = np.eye(zoro_description.nw)
+    zoro_description.nlbx_t = len(zoro_description.idx_lbx_t)
+    zoro_description.nubx_t = len(zoro_description.idx_ubx_t)
+    zoro_description.nlbx_e_t = len(zoro_description.idx_lbx_e_t)
+    zoro_description.nubx_e_t = len(zoro_description.idx_ubx_e_t)
+    zoro_description.nlbu_t = len(zoro_description.idx_lbu_t)
+    zoro_description.nubu_t = len(zoro_description.idx_ubu_t)
+    zoro_description.nlg_t = len(zoro_description.idx_lg_t)
+    zoro_description.nug_t = len(zoro_description.idx_ug_t)
+    zoro_description.nlg_e_t = len(zoro_description.idx_lg_e_t)
+    zoro_description.nug_e_t = len(zoro_description.idx_ug_e_t)
+    zoro_description.nlh_t = len(zoro_description.idx_lh_t)
+    zoro_description.nuh_t = len(zoro_description.idx_uh_t)
+    zoro_description.nlh_e_t = len(zoro_description.idx_lh_e_t)
+    zoro_description.nuh_e_t = len(zoro_description.idx_uh_e_t)
+    return zoro_description.__dict__
diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_common.h b/third_party/acados/include/acados/dense_qp/dense_qp_common.h
index f3809c4294..2a9a974f99 100644
--- a/third_party/acados/include/acados/dense_qp/dense_qp_common.h
+++ b/third_party/acados/include/acados/dense_qp/dense_qp_common.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_daqp.h b/third_party/acados/include/acados/dense_qp/dense_qp_daqp.h
new file mode 100644
index 0000000000..b262089b4f
--- /dev/null
+++ b/third_party/acados/include/acados/dense_qp/dense_qp_daqp.h
@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * 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.;
+ */
+
+
+#ifndef ACADOS_DENSE_QP_DENSE_QP_DAQP_H_
+#define ACADOS_DENSE_QP_DENSE_QP_DAQP_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// blasfeo
+#include "blasfeo/include/blasfeo_common.h"
+
+// daqp
+#include "daqp/include/types.h"
+
+// acados
+#include "acados/dense_qp/dense_qp_common.h"
+#include "acados/utils/types.h"
+
+
+typedef struct dense_qp_daqp_opts_
+{
+    DAQPSettings* daqp_opts;
+    int warm_start;
+} dense_qp_daqp_opts;
+
+
+typedef struct dense_qp_daqp_memory_
+{
+    double* lb_tmp;
+    double* ub_tmp;
+    int* idxb;
+    int* idxv_to_idxb;
+    int* idxs;
+    int* idxdaqp_to_idxs;
+
+    double* Zl;
+    double* Zu;
+    double* zl;
+    double* zu;
+    double* d_ls;
+    double* d_us;
+
+    double time_qp_solver_call;
+    int iter;
+    DAQPWorkspace * daqp_work;
+
+} dense_qp_daqp_memory;
+
+// opts
+acados_size_t dense_qp_daqp_opts_calculate_size(void *config, dense_qp_dims *dims);
+//
+void *dense_qp_daqp_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory);
+//
+void dense_qp_daqp_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_);
+//
+void dense_qp_daqp_opts_update(void *config, dense_qp_dims *dims, void *opts_);
+//
+// memory 
+acados_size_t dense_qp_daqp_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
+//
+void *dense_qp_daqp_workspace_assign(void *config, dense_qp_dims *dims, void *raw_memory);
+//
+acados_size_t dense_qp_daqp_memory_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
+//
+void *dense_qp_daqp_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory);
+//
+// functions
+int dense_qp_daqp(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_);
+//
+void dense_qp_daqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void dense_qp_daqp_memory_reset(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void dense_qp_daqp_config_initialize_default(void *config_);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_DENSE_QP_DENSE_QP_DAQP_H_
diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h b/third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h
index 20eedc26a2..136279d666 100644
--- a/third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h
+++ b/third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h b/third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h
index 646f11f06f..d051cb15f7 100644
--- a/third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h
+++ b/third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_qore.h b/third_party/acados/include/acados/dense_qp/dense_qp_qore.h
index 52606fac5d..392e472918 100644
--- a/third_party/acados/include/acados/dense_qp/dense_qp_qore.h
+++ b/third_party/acados/include/acados/dense_qp/dense_qp_qore.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h b/third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h
index 9f0bb1af68..0e13d3ef64 100644
--- a/third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h
+++ b/third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
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 60d835fc58..ba97db9768 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
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -419,6 +416,9 @@ void ocp_nlp_embed_initial_value(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp
 void ocp_nlp_update_variables_sqp(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, double alpha);
 //
+int ocp_nlp_precompute_common(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);
+//
 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,
             int check_early_termination);
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h
index 7f7a30faf3..9dbf16f6dc 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -82,9 +79,6 @@ acados_size_t ocp_nlp_constraints_bgh_dims_calculate_size(void *config);
 //
 void *ocp_nlp_constraints_bgh_dims_assign(void *config, void *raw_memory);
 //
-void ocp_nlp_constraints_bgh_dims_initialize(void *config, void *dims, int nx, int nu, int nz, int nbx,
-                                         int nbu, int ng, int nh, int dummy0, int ns);
-//
 void ocp_nlp_constraints_bgh_dims_get(void *config_, void *dims_, const char *field, int* value);
 //
 void ocp_nlp_constraints_bgh_dims_set(void *config_, void *dims_,
@@ -116,6 +110,9 @@ void *ocp_nlp_constraints_bgh_model_assign(void *config, void *dims, void *raw_m
 int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_,
                          void *model_, const char *field, void *value);
 
+//
+void ocp_nlp_constraints_bgh_model_get(void *config_, void *dims_,
+                         void *model_, const char *field, void *value);
 
 
 /************************************************
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h
index beeec78411..eb05edf7a6 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -63,7 +60,7 @@ typedef struct
     int nbu;
     int nbx;
     int ng;   // number of general linear constraints
-    int nphi; // dimension of convex outer part 
+    int nphi; // dimension of convex outer part
     int ns;   // nsbu + nsbx + nsg + nsphi
     int nsbu; // number of softened input bounds
     int nsbx; // number of softened state bounds
@@ -81,9 +78,6 @@ acados_size_t ocp_nlp_constraints_bgp_dims_calculate_size(void *config);
 //
 void *ocp_nlp_constraints_bgp_dims_assign(void *config, void *raw_memory);
 //
-void ocp_nlp_constraints_bgp_dims_initialize(void *config, void *dims, int nx, int nu, int nz, 
-		int nbx, int nbu, int ng, int nphi, int nq, int ns);
-//
 void ocp_nlp_constraints_bgp_dims_get(void *config_, void *dims_, const char *field, int* value);
 
 
@@ -109,6 +103,9 @@ void *ocp_nlp_constraints_bgp_assign(void *config, void *dims, void *raw_memory)
 //
 int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_,
                          void *model_, const char *field, void *value);
+//
+void ocp_nlp_constraints_bgp_model_get(void *config_, void *dims_,
+                         void *model_, const char *field, void *value);
 
 /* options */
 
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h
index 7cadecab46..bb73c468de 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -60,11 +57,10 @@ typedef struct
 {
     acados_size_t (*dims_calculate_size)(void *config);
     void *(*dims_assign)(void *config, void *raw_memory);
-    void (*dims_initialize)(void *config, void *dims, int nx, int nu, int nz, int nbx, int nbu, int ng,
-                            int nh, int nq, int ns);
     acados_size_t (*model_calculate_size)(void *config, void *dims);
     void *(*model_assign)(void *config, void *dims, void *raw_memory);
     int (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value);
+    void (*model_get)(void *config_, void *dims_, void *model_, const char *field, void *value);
     acados_size_t (*opts_calculate_size)(void *config, void *dims);
     void *(*opts_assign)(void *config, void *dims, void *raw_memory);
     void (*opts_initialize_default)(void *config, void *dims, void *opts);
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h
index c9fbbfb404..eb40564036 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -33,8 +30,8 @@
 
 
 ///
-/// \defgroup ocp_nlp_cost ocp_nlp_cost 
-/// 
+/// \defgroup ocp_nlp_cost ocp_nlp_cost
+///
 
 /// \addtogroup ocp_nlp_cost ocp_nlp_cost
 /// @{
@@ -62,7 +59,6 @@ typedef struct
 {
     acados_size_t (*dims_calculate_size)(void *config);
     void *(*dims_assign)(void *config, void *raw_memory);
-    void (*dims_initialize)(void *config, void *dims, int nx, int nu, int ny, int ns, int nz);
     void (*dims_set)(void *config_, void *dims_, const char *field, int *value);
     void (*dims_get)(void *config_, void *dims_, const char *field, int *value);
     acados_size_t (*model_calculate_size)(void *config, void *dims);
@@ -91,6 +87,8 @@ typedef struct
     // computes the cost function value (intended for globalization)
     void (*compute_fun)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
     void (*config_initialize_default)(void *config);
+    void (*precompute)(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
+
 } ocp_nlp_cost_config;
 
 //
@@ -105,5 +103,5 @@ ocp_nlp_cost_config *ocp_nlp_cost_config_assign(void *raw_memory);
 #endif
 
 #endif  // ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_
-/// @} 
-/// @} 
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_conl.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_conl.h
new file mode 100644
index 0000000000..2eb3f5d127
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_conl.h
@@ -0,0 +1,207 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * 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.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_cost ocp_nlp_cost
+/// @{
+/// \addtogroup ocp_nlp_cost_conl ocp_nlp_cost_conl
+/// \brief This module implements convex-over-nonlinear costs of the form
+/// \f$\min_{x,u,z} \psi(y(x,u,z,p) - y_{\text{ref}}, p)\f$,
+
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_
+#define ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// blasfeo
+#include "blasfeo/include/blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+typedef struct
+{
+    int nx;  // number of states
+    int nz;  // number of algebraic variables
+    int nu;  // number of inputs
+    int ny;  // number of outputs
+    int ns;  // number of slacks
+} ocp_nlp_cost_conl_dims;
+
+//
+acados_size_t ocp_nlp_cost_conl_dims_calculate_size(void *config);
+//
+void *ocp_nlp_cost_conl_dims_assign(void *config, void *raw_memory);
+//
+void ocp_nlp_cost_conl_dims_initialize(void *config, void *dims, int nx, int nu, int ny, int ns, int nz);
+//
+void ocp_nlp_cost_conl_dims_set(void *config_, void *dims_, const char *field, int* value);
+//
+void ocp_nlp_cost_conl_dims_get(void *config_, void *dims_, const char *field, int* value);
+
+
+
+/************************************************
+ * model
+ ************************************************/
+
+typedef struct
+{
+    // slack penalty has the form z^T * s + .5 * s^T * Z * s
+    external_function_generic *conl_cost_fun;
+    external_function_generic *conl_cost_fun_jac_hess;
+    struct blasfeo_dvec y_ref;
+    struct blasfeo_dvec Z;              // diagonal Hessian of slacks as vector
+    struct blasfeo_dvec z;              // gradient of slacks as vector
+    double scaling;
+} ocp_nlp_cost_conl_model;
+
+//
+acados_size_t ocp_nlp_cost_conl_model_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_cost_conl_model_assign(void *config, void *dims, void *raw_memory);
+//
+int ocp_nlp_cost_conl_model_set(void *config_, void *dims_, void *model_, const char *field, void *value_);
+
+
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    bool gauss_newton_hess;  // dummy options, we always use a gauss-newton hessian
+} ocp_nlp_cost_conl_opts;
+
+//
+acados_size_t ocp_nlp_cost_conl_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_cost_conl_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_cost_conl_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_cost_conl_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_cost_conl_opts_set(void *config, void *opts, const char *field, void *value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+typedef struct
+{
+    struct blasfeo_dvec grad;    // gradient of cost function
+    struct blasfeo_dvec *ux;     // pointer to ux in nlp_out
+    struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out
+    struct blasfeo_dmat *RSQrq;  // pointer to RSQrq in qp_in
+    struct blasfeo_dvec *Z;      // pointer to Z in qp_in
+    struct blasfeo_dvec *z_alg;         ///< pointer to z in sim_out
+    struct blasfeo_dmat *dzdux_tran;    ///< pointer to sensitivity of a wrt ux in sim_out
+    double fun;                         ///< value of the cost function
+} ocp_nlp_cost_conl_memory;
+
+//
+acados_size_t ocp_nlp_cost_conl_memory_calculate_size(void *config, void *dims, void *opts);
+//
+void *ocp_nlp_cost_conl_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
+//
+double *ocp_nlp_cost_conl_memory_get_fun_ptr(void *memory_);
+//
+struct blasfeo_dvec *ocp_nlp_cost_conl_memory_get_grad_ptr(void *memory_);
+//
+void ocp_nlp_cost_conl_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
+//
+void ocp_nlp_cost_conl_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
+//
+void ocp_nlp_cost_conl_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
+//
+void ocp_nlp_cost_conl_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
+//
+void ocp_nlp_cost_conl_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
+//
+void ocp_nlp_cost_conl_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dmat W;             // hessian of outer loss function
+    struct blasfeo_dmat W_chol;        // cholesky factor of hessian of outer loss function
+    struct blasfeo_dmat Jt_ux;         // jacobian of inner residual function
+    struct blasfeo_dmat Jt_ux_tilde;   // jacobian of inner residual function plus gradient contribution of algebraic variables
+    struct blasfeo_dmat Jt_z;          // jacobian of inner residual function wrt algebraic variables
+    struct blasfeo_dmat tmp_nv_ny;
+    struct blasfeo_dvec tmp_ny;
+    struct blasfeo_dvec tmp_2ns;
+} ocp_nlp_cost_conl_workspace;
+
+//
+acados_size_t ocp_nlp_cost_conl_workspace_calculate_size(void *config, void *dims, void *opts);
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_cost_conl_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_conl_config_initialize_default(void *config);
+//
+void ocp_nlp_cost_conl_initialize(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+//
+void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_conl_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_
+/// @}
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h
index f2196dbee7..78958270de 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -54,6 +51,7 @@ extern "C" {
 typedef struct
 {
     int nx;  // number of states
+    int nz;  // number of algebraic variables
     int nu;  // number of inputs
     int ns;  // number of slacks
 } ocp_nlp_cost_external_dims;
@@ -63,9 +61,6 @@ acados_size_t ocp_nlp_cost_external_dims_calculate_size(void *config);
 //
 void *ocp_nlp_cost_external_dims_assign(void *config, void *raw_memory);
 //
-void ocp_nlp_cost_external_dims_initialize(void *config, void *dims, int nx,
-                                           int nu, int ny, int ns, int nz);
-//
 void ocp_nlp_cost_external_dims_set(void *config_, void *dims_, const char *field, int* value);
 //
 void ocp_nlp_cost_external_dims_get(void *config_, void *dims_, const char *field, int* value);
@@ -122,7 +117,7 @@ typedef struct
 {
     struct blasfeo_dvec grad;    // gradient of cost function
     struct blasfeo_dvec *ux;     // pointer to ux in nlp_out
-    struct blasfeo_dvec *tmp_ux;     // pointer to tmp_ux in nlp_out
+    struct blasfeo_dvec *tmp_ux; // pointer to tmp_ux in nlp_out
     struct blasfeo_dmat *RSQrq;  // pointer to RSQrq in qp_in
     struct blasfeo_dvec *Z;      // pointer to Z in qp_in
     struct blasfeo_dvec *z_alg;         ///< pointer to z in sim_out
@@ -157,7 +152,10 @@ void ocp_nlp_cost_external_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_
 
 typedef struct
 {
-    struct blasfeo_dmat tmp_nv_nv;
+    struct blasfeo_dmat tmp_nunx_nunx;
+    struct blasfeo_dmat tmp_nz_nz;
+    struct blasfeo_dmat tmp_nz_nunx;
+    struct blasfeo_dvec tmp_nunxnz;
     struct blasfeo_dvec tmp_2ns;  // temporary vector of dimension 2*ns
 } ocp_nlp_cost_external_workspace;
 
@@ -168,6 +166,8 @@ acados_size_t ocp_nlp_cost_external_workspace_calculate_size(void *config, void
  * functions
  ************************************************/
 
+//
+void ocp_nlp_cost_external_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
 //
 void ocp_nlp_cost_external_config_initialize_default(void *config);
 //
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h
index 3cf759504a..801e9a5b87 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -82,30 +79,14 @@ typedef struct
 acados_size_t ocp_nlp_cost_ls_dims_calculate_size(void *config);
 
 
-///  Assign memory pointed to by raw_memory to ocp_nlp-cost_ls dims struct 
+///  Assign memory pointed to by raw_memory to ocp_nlp-cost_ls dims struct
 ///
-///  \param[in] config structure containing configuration of ocp_nlp_cost 
+///  \param[in] config structure containing configuration of ocp_nlp_cost
 ///  module
-///  \param[in] raw_memory pointer to memory location  
+///  \param[in] raw_memory pointer to memory location
 ///  \param[out] []
 ///  \return dims
 void *ocp_nlp_cost_ls_dims_assign(void *config, void *raw_memory);
-
-
-///  Initialize the dimensions struct of the 
-///  ocp_nlp-cost_ls component    
-///
-///  \param[in] config structure containing configuration ocp_nlp-cost_ls component 
-///  \param[in] nx number of states
-///  \param[in] nu number of inputs
-///  \param[in] ny number of residuals
-///  \param[in] ns number of slacks
-///  \param[in] nz number of algebraic variables
-///  \param[out] dims
-///  \return size
-void ocp_nlp_cost_ls_dims_initialize(void *config, void *dims, int nx,
-        int nu, int ny, int ns, int nz);
-
 //
 void ocp_nlp_cost_ls_dims_set(void *config_, void *dims_, const char *field, int* value);
 //
@@ -117,7 +98,7 @@ void ocp_nlp_cost_ls_dims_get(void *config_, void *dims_, const char *field, int
 ////////////////////////////////////////////////////////////////////////////////
 
 
-/// structure containing the data describing the linear least-square cost 
+/// structure containing the data describing the linear least-square cost
 typedef struct
 {
     // slack penalty has the form z^T * s + .5 * s^T * Z * s
@@ -128,6 +109,8 @@ typedef struct
     struct blasfeo_dvec Z;              ///< diagonal Hessian of slacks as vector (lower and upper)
     struct blasfeo_dvec z;              ///< gradient of slacks as vector (lower and upper)
     double scaling;
+    int W_changed;                      ///< flag indicating whether W has changed and needs to be refactorized
+    int Cyt_or_scaling_changed;         ///< flag indicating whether Cyt or scaling has changed and Hessian needs to be recomputed
 } ocp_nlp_cost_ls_model;
 
 //
@@ -170,7 +153,7 @@ void ocp_nlp_cost_ls_opts_set(void *config, void *opts, const char *field, void
 
 
 
-/// structure containing the memory associated with cost_ls component 
+/// structure containing the memory associated with cost_ls component
 /// of the ocp_nlp module
 typedef struct
 {
@@ -237,7 +220,8 @@ acados_size_t ocp_nlp_cost_ls_workspace_calculate_size(void *config, void *dims,
 ////////////////////////////////////////////////////////////////////////////////
 
 
-
+// computations that are done once when solver is created
+void ocp_nlp_cost_ls_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
 //
 void ocp_nlp_cost_ls_config_initialize_default(void *config);
 //
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h
index aafb6b3540..5ec68cf580 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -38,8 +35,7 @@
 /// @{
 /// \addtogroup ocp_nlp_cost_nls ocp_nlp_cost_nls
 /// \brief This module implements nonlinear-least squares costs of the form
-/// \f$\min_{x,u} \| r(x,u) - y_{\text{ref}} \|_W^2\f$.
-/// @{
+/// \f$\min_{x,u,z} \| y(x,u,z,p) - y_{\text{ref}} \|_W^2\f$,
 
 #ifndef ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_
 #define ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_
@@ -65,6 +61,7 @@ extern "C" {
 typedef struct
 {
     int nx;  // number of states
+    int nz;  // number of algebraic variables
     int nu;  // number of inputs
     int ny;  // number of outputs
     int ns;  // number of slacks
@@ -75,8 +72,6 @@ acados_size_t ocp_nlp_cost_nls_dims_calculate_size(void *config);
 //
 void *ocp_nlp_cost_nls_dims_assign(void *config, void *raw_memory);
 //
-void ocp_nlp_cost_nls_dims_initialize(void *config, void *dims, int nx, int nu, int ny, int ns, int nz);
-//
 void ocp_nlp_cost_nls_dims_set(void *config_, void *dims_, const char *field, int* value);
 //
 void ocp_nlp_cost_nls_dims_get(void *config_, void *dims_, const char *field, int* value);
@@ -99,6 +94,7 @@ typedef struct
     struct blasfeo_dvec Z;              // diagonal Hessian of slacks as vector
     struct blasfeo_dvec z;              // gradient of slacks as vector
     double scaling;
+    int W_changed;                      ///< flag indicating whether W has changed and needs to be refactorized
 } ocp_nlp_cost_nls_model;
 
 //
@@ -144,11 +140,11 @@ typedef struct
     struct blasfeo_dvec grad;    // gradient of cost function
     struct blasfeo_dvec *ux;     // pointer to ux in nlp_out
     struct blasfeo_dvec *tmp_ux;     // pointer to ux in tmp_nlp_out
-    struct blasfeo_dvec *z_alg;         ///< pointer to z in sim_out
-    struct blasfeo_dmat *dzdux_tran;    ///< pointer to sensitivity of a wrt ux in sim_out
     struct blasfeo_dmat *RSQrq;  // pointer to RSQrq in qp_in
     struct blasfeo_dvec *Z;      // pointer to Z in qp_in
-	double fun;                         ///< value of the cost function
+    struct blasfeo_dvec *z_alg;         ///< pointer to z in sim_out
+    struct blasfeo_dmat *dzdux_tran;    ///< pointer to sensitivity of a wrt ux in sim_out
+    double fun;                         ///< value of the cost function
 } ocp_nlp_cost_nls_memory;
 
 //
@@ -180,8 +176,11 @@ typedef struct
 {
     struct blasfeo_dmat tmp_nv_ny;
     struct blasfeo_dmat tmp_nv_nv;
+    struct blasfeo_dmat Vz;
+    struct blasfeo_dmat Cyt_tilde;
     struct blasfeo_dvec tmp_ny;
-    struct blasfeo_dvec tmp_2ns;     // temporary vector of dimension ny
+    struct blasfeo_dvec tmp_2ns;
+    struct blasfeo_dvec tmp_nz;
 } ocp_nlp_cost_nls_workspace;
 
 //
@@ -191,6 +190,8 @@ acados_size_t ocp_nlp_cost_nls_workspace_calculate_size(void *config, void *dims
  * functions
  ************************************************/
 
+//
+void ocp_nlp_cost_nls_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
 //
 void ocp_nlp_cost_nls_config_initialize_default(void *config);
 //
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 f2128a8574..43fe71b12f 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
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -68,7 +65,6 @@ typedef struct
     /* dims */
     acados_size_t (*dims_calculate_size)(void *config);
     void *(*dims_assign)(void *config, void *raw_memory);
-    void (*dims_initialize)(void *config, void *dims, int nx, int nu, int nx1, int nu1, int nz);
     void (*dims_set)(void *config_, void *dims_, const char *field, int *value);
     void (*dims_get)(void *config_, void *dims_, const char *field, int* value);
     /* model */
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h
index 59a2df4f47..3afdc9f4ed 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -75,10 +72,6 @@ typedef struct
 acados_size_t ocp_nlp_dynamics_cont_dims_calculate_size(void *config);
 //
 void *ocp_nlp_dynamics_cont_dims_assign(void *config, void *raw_memory);
-//
-void ocp_nlp_dynamics_cont_dims_initialize(void *config, void *dims, int nx, int nu, int nx1,
-                                           int nu1, int nz);
-
 //
 void ocp_nlp_dynamics_cont_dims_set(void *config_, void *dims_, const char *field, int* value);
 
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h
index 8b2a6177bf..6ea26a7010 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -68,10 +65,6 @@ typedef struct
 acados_size_t ocp_nlp_dynamics_disc_dims_calculate_size(void *config);
 //
 void *ocp_nlp_dynamics_disc_dims_assign(void *config, void *raw_memory);
-//
-void ocp_nlp_dynamics_disc_dims_initialize(void *config, void *dims, int nx, int nu, int nx1,
-                                           int nu1, int nz);
-
 //
 void ocp_nlp_dynamics_disc_dims_set(void *config_, void *dims_, const char *dim, int* value);
 
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h
index cd26788a56..9388f3fd24 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h
index df31361680..cb523525e1 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h
index f6bd7dcafd..84a023cb69 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
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 c085e00d5d..b571f3bac1 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
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h
index 104c297207..682ea206dc 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h
index e0b854bc1a..7e12952c15 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
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 74cfb042e3..fdb96417f9 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
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
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 af22c06a17..364d0f4717 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
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -145,12 +142,6 @@ acados_size_t ocp_nlp_sqp_rti_workspace_calculate_size(void *config_, void *dims
 /************************************************
  * functions
  ************************************************/
-
-void ocp_nlp_sqp_rti_preparation_step(void *config_, void *dims_,
-    void *nlp_in_, void *nlp_out_, void *opts, void *mem_, void *work_);
-//
-void ocp_nlp_sqp_rti_feedback_step(void *config_, void *dims_,
-    void *nlp_in_, void *nlp_out_, void *opts_, void *mem_, void *work_);
 //
 int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
     void *opts_, void *mem_, void *work_);
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_common.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_common.h
index caf6caf2f0..d1a45635e4 100644
--- a/third_party/acados/include/acados/ocp_qp/ocp_qp_common.h
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_common.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h
index 50b80850c6..f65f602c15 100644
--- a/third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h
index 14ac97bbf4..d23e658b48 100644
--- a/third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h
index 91690e458d..261606b842 100644
--- a/third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h
index de6ce501b1..8db53a279d 100644
--- a/third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h
index 1a3b7b2fa2..a535503f21 100644
--- a/third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h
index 4ae391a9ff..51df1b1cd6 100644
--- a/third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h
index b95a11114e..844f6048fe 100644
--- a/third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h
index ad4d094516..3b875caeb5 100644
--- a/third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h
index d4b30e007b..a78bc65bb9 100644
--- a/third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -84,6 +81,7 @@ typedef struct
     acados_size_t (*dims_calculate_size)(void *config, int N);
     ocp_qp_xcond_solver_dims *(*dims_assign)(void *config, int N, void *raw_memory);
     void (*dims_set)(void *config_, ocp_qp_xcond_solver_dims *dims, int stage, const char *field, int* value);
+    void (*dims_get)(void *config_, ocp_qp_xcond_solver_dims *dims, int stage, const char *field, int* value);
     acados_size_t (*opts_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims);
     void *(*opts_assign)(void *config, ocp_qp_xcond_solver_dims *dims, void *raw_memory);
     void (*opts_initialize_default)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts);
diff --git a/third_party/acados/include/acados/sim/sim_collocation_utils.h b/third_party/acados/include/acados/sim/sim_collocation_utils.h
index 40a0b1c0cc..045d165cbc 100644
--- a/third_party/acados/include/acados/sim/sim_collocation_utils.h
+++ b/third_party/acados/include/acados/sim/sim_collocation_utils.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/sim/sim_common.h b/third_party/acados/include/acados/sim/sim_common.h
index 1838d76f81..c4bbd6ed2b 100644
--- a/third_party/acados/include/acados/sim/sim_common.h
+++ b/third_party/acados/include/acados/sim/sim_common.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -149,6 +146,8 @@ typedef struct
     bool jac_reuse;
     // Newton_scheme *scheme;
 
+    double newton_tol; // optinally used in implicit integrators
+
     // workspace
     void *work;
 
diff --git a/third_party/acados/include/acados/sim/sim_erk_integrator.h b/third_party/acados/include/acados/sim/sim_erk_integrator.h
index 24a00c7077..fd46cb4d99 100644
--- a/third_party/acados/include/acados/sim/sim_erk_integrator.h
+++ b/third_party/acados/include/acados/sim/sim_erk_integrator.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/sim/sim_gnsf.h b/third_party/acados/include/acados/sim/sim_gnsf.h
index 5524b384e0..404532a732 100644
--- a/third_party/acados/include/acados/sim/sim_gnsf.h
+++ b/third_party/acados/include/acados/sim/sim_gnsf.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -46,12 +43,12 @@ extern "C" {
 #include "acados/sim/sim_common.h"
 
 #include "blasfeo/include/blasfeo_common.h"
-#include "blasfeo/include/blasfeo_d_aux.h"
-#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
-#include "blasfeo/include/blasfeo_d_blas.h"
-#include "blasfeo/include/blasfeo_d_kernel.h"
-#include "blasfeo/include/blasfeo_i_aux_ext_dep.h"
-#include "blasfeo/include/blasfeo_target.h"
+// #include "blasfeo/include/blasfeo_d_aux.h"
+// #include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
+// #include "blasfeo/include/blasfeo_d_blas.h"
+// #include "blasfeo/include/blasfeo_d_kernel.h"
+// #include "blasfeo/include/blasfeo_i_aux_ext_dep.h"
+// #include "blasfeo/include/blasfeo_target.h"
 
 /* 
 GNSF - Generalized Nonlinear Static Feedback Model
diff --git a/third_party/acados/include/acados/sim/sim_irk_integrator.h b/third_party/acados/include/acados/sim/sim_irk_integrator.h
index 6851bacb3a..5090aa0bb5 100644
--- a/third_party/acados/include/acados/sim/sim_irk_integrator.h
+++ b/third_party/acados/include/acados/sim/sim_irk_integrator.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h b/third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h
index 9ec2d97bed..e60bb80ebf 100644
--- a/third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h
+++ b/third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/utils/external_function_generic.h b/third_party/acados/include/acados/utils/external_function_generic.h
index 021363f26e..1e68dc155d 100644
--- a/third_party/acados/include/acados/utils/external_function_generic.h
+++ b/third_party/acados/include/acados/utils/external_function_generic.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -137,8 +134,8 @@ typedef struct
     int (*casadi_work)(int *, int *, int *, int *);
     const int *(*casadi_sparsity_in)(int);
     const int *(*casadi_sparsity_out)(int);
-    int (*casadi_n_in)();
-    int (*casadi_n_out)();
+    int (*casadi_n_in)(void);
+    int (*casadi_n_out)(void);
     double **args;
     double **res;
     double *w;
@@ -195,8 +192,8 @@ typedef struct
     int (*casadi_work)(int *, int *, int *, int *);
     const int *(*casadi_sparsity_in)(int);
     const int *(*casadi_sparsity_out)(int);
-    int (*casadi_n_in)();
-    int (*casadi_n_out)();
+    int (*casadi_n_in)(void);
+    int (*casadi_n_out)(void);
     double **args;
     double **res;
     double *w;
diff --git a/third_party/acados/include/acados/utils/math.h b/third_party/acados/include/acados/utils/math.h
index fe1da875f6..7156a82084 100644
--- a/third_party/acados/include/acados/utils/math.h
+++ b/third_party/acados/include/acados/utils/math.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -42,6 +39,7 @@ extern "C" {
 
 #if defined(__MABX2__)
 double fmax(double a, double b);
+int isnan(double x);
 #endif
 
 #define MIN(a,b) (((a)<(b))?(a):(b))
diff --git a/third_party/acados/include/acados/utils/mem.h b/third_party/acados/include/acados/utils/mem.h
index 7b9efc5ed8..681a371e36 100644
--- a/third_party/acados/include/acados/utils/mem.h
+++ b/third_party/acados/include/acados/utils/mem.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/utils/print.h b/third_party/acados/include/acados/utils/print.h
index f8568afb26..824d3cee22 100644
--- a/third_party/acados/include/acados/utils/print.h
+++ b/third_party/acados/include/acados/utils/print.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/utils/strsep.h b/third_party/acados/include/acados/utils/strsep.h
index 02f1835593..62bdfb4891 100644
--- a/third_party/acados/include/acados/utils/strsep.h
+++ b/third_party/acados/include/acados/utils/strsep.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/utils/timing.h b/third_party/acados/include/acados/utils/timing.h
index fe561d3891..b0955932da 100644
--- a/third_party/acados/include/acados/utils/timing.h
+++ b/third_party/acados/include/acados/utils/timing.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados/utils/types.h b/third_party/acados/include/acados/utils/types.h
index d3da0a86b7..a27ef9e552 100644
--- a/third_party/acados/include/acados/utils/types.h
+++ b/third_party/acados/include/acados/utils/types.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -78,7 +75,7 @@ typedef int (*casadi_function_t)(const double** arg, double** res, int* iw, doub
 enum return_values
 {
     ACADOS_SUCCESS,
-    ACADOS_FAILURE,
+    ACADOS_NAN_DETECTED,
     ACADOS_MAXITER,
     ACADOS_MINSTEP,
     ACADOS_QP_FAILURE,
diff --git a/third_party/acados/include/acados_c/condensing_interface.h b/third_party/acados/include/acados_c/condensing_interface.h
index 51fe827127..b4302078d6 100644
--- a/third_party/acados/include/acados_c/condensing_interface.h
+++ b/third_party/acados/include/acados_c/condensing_interface.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/acados_c/dense_qp_interface.h b/third_party/acados/include/acados_c/dense_qp_interface.h
index 4ecc77381d..b3af4bf682 100644
--- a/third_party/acados/include/acados_c/dense_qp_interface.h
+++ b/third_party/acados/include/acados_c/dense_qp_interface.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -41,7 +38,7 @@ extern "C" {
 
 #include "acados/dense_qp/dense_qp_common.h"
 
-typedef enum { DENSE_QP_HPIPM, DENSE_QP_QORE, DENSE_QP_QPOASES, DENSE_QP_OOQP } dense_qp_solver_t;
+typedef enum { DENSE_QP_HPIPM, DENSE_QP_QORE, DENSE_QP_QPOASES, DENSE_QP_OOQP, DENSE_QP_DAQP } dense_qp_solver_t;
 
 typedef struct
 {
diff --git a/third_party/acados/include/acados_c/external_function_interface.h b/third_party/acados/include/acados_c/external_function_interface.h
index 6838975071..d4f52db850 100644
--- a/third_party/acados/include/acados_c/external_function_interface.h
+++ b/third_party/acados/include/acados_c/external_function_interface.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
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 50bc0cf1af..dd3e596f8b 100644
--- a/third_party/acados/include/acados_c/ocp_nlp_interface.h
+++ b/third_party/acados/include/acados_c/ocp_nlp_interface.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -66,6 +63,7 @@ typedef enum
 {
     LINEAR_LS,
     NONLINEAR_LS,
+    CONVEX_OVER_NONLINEAR,
     EXTERNAL,
     INVALID_COST,
 } ocp_nlp_cost_t;
@@ -246,6 +244,10 @@ ACADOS_SYMBOL_EXPORT int ocp_nlp_cost_model_set(ocp_nlp_config *config, ocp_nlp_
 ACADOS_SYMBOL_EXPORT int ocp_nlp_constraints_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims,
         ocp_nlp_in *in, int stage, const char *field, void *value);
 
+///
+ACADOS_SYMBOL_EXPORT void ocp_nlp_constraints_model_get(ocp_nlp_config *config, ocp_nlp_dims *dims,
+        ocp_nlp_in *in, int stage, const char *field, void *value);
+
 /* out */
 
 /// Constructs an output struct for the non-linear program.
@@ -297,7 +299,7 @@ ACADOS_SYMBOL_EXPORT void ocp_nlp_constraint_dims_get_from_attr(ocp_nlp_config *
 ACADOS_SYMBOL_EXPORT void ocp_nlp_cost_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
         int stage, const char *field, int *dims_out);
 
-void ocp_nlp_dynamics_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+ACADOS_SYMBOL_EXPORT void ocp_nlp_qp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
         int stage, const char *field, int *dims_out);
 
 /* opts */
@@ -386,8 +388,6 @@ ACADOS_SYMBOL_EXPORT int ocp_nlp_precompute(ocp_nlp_solver *solver, ocp_nlp_in *
 /// \param nlp_out The output struct.
 ACADOS_SYMBOL_EXPORT void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out);
 
-//
-void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out);
 
 /// Computes the residuals.
 ///
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 2582f142da..3dc3f1a532 100644
--- a/third_party/acados/include/acados_c/ocp_qp_interface.h
+++ b/third_party/acados/include/acados_c/ocp_qp_interface.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
@@ -87,6 +84,11 @@ typedef enum {
 #else
     FULL_CONDENSING_QPOASES_NOT_AVAILABLE,
 #endif
+#ifdef ACADOS_WITH_DAQP
+    FULL_CONDENSING_DAQP,
+#else
+    FULL_CONDENSING_DAQP_NOT_AVAILABLE,
+#endif
 #ifdef ACADOS_WITH_QORE
     FULL_CONDENSING_QORE,
 #else
diff --git a/third_party/acados/include/acados_c/sim_interface.h b/third_party/acados/include/acados_c/sim_interface.h
index 5dce6f153a..09a05d6995 100644
--- a/third_party/acados/include/acados_c/sim_interface.h
+++ b/third_party/acados/include/acados_c/sim_interface.h
@@ -1,8 +1,5 @@
 /*
- * 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
+ * Copyright (c) The acados authors.
  *
  * This file is part of acados.
  *
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_blas_api.h b/third_party/acados/include/blasfeo/include/blasfeo_d_blas_api.h
index ff9e9d4c16..2eab1e4094 100644
--- a/third_party/acados/include/blasfeo/include/blasfeo_d_blas_api.h
+++ b/third_party/acados/include/blasfeo/include/blasfeo_d_blas_api.h
@@ -50,18 +50,20 @@
 #define BLASFEO_CBLAS_ENUM
 #ifdef FORTRAN_BLAS_API
 #ifndef CBLAS_H
-enum CBLAS_ORDER {CblasRowMajor=101, CblasColMajor=102};
+enum CBLAS_LAYOUT {CblasRowMajor=101, CblasColMajor=102};
 enum CBLAS_TRANSPOSE {CblasNoTrans=111, CblasTrans=112, CblasConjTrans=113};
 enum CBLAS_UPLO {CblasUpper=121, CblasLower=122};
 enum CBLAS_DIAG {CblasNonUnit=131, CblasUnit=132};
 enum CBLAS_SIDE {CblasLeft=141, CblasRight=142};
+#define CBLAS_ORDER CBLAS_LAYOUT /* this for backward compatibility with CBLAS_ORDER */
 #endif // CBLAS_H
 #else // FORTRAN_BLAS_API
-enum BLASFEO_CBLAS_ORDER {BlasfeoCblasRowMajor=101, BlasfeoCblasColMajor=102};
+enum BLASFEO_CBLAS_LAYOUT {BlasfeoCblasRowMajor=101, BlasfeoCblasColMajor=102};
 enum BLASFEO_CBLAS_TRANSPOSE {BlasfeoCblasNoTrans=111, BlasfeoCblasTrans=112, BlasfeoCblasConjTrans=113};
 enum BLASFEO_CBLAS_UPLO {BlasfeoCblasUpper=121, BlasfeoCblasLower=122};
 enum BLASFEO_CBLAS_DIAG {BlasfeoCblasNonUnit=131, BlasfeoCblasUnit=132};
 enum BLASFEO_CBLAS_SIDE {BlasfeoCblasLeft=141, BlasfeoCblasRight=142};
+#define BLASFEO_CBLAS_ORDER BLASFEO_CBLAS_LAYOUT /* this for backward compatibility with BLASFEO_CBLAS_ORDER */
 #endif // FORTRAN_BLAS_API
 #endif // BLASFEO_CBLAS_ENUM
 #endif // CBLAS_API
@@ -151,15 +153,19 @@ void cblas_dswap(const int N, double *X, const int incX, double *Y, const int in
 //
 void cblas_dcopy(const int N, const double *X, const int incX, double *Y, const int incY);
 
+// CBLAS 2
+//
+void cblas_dgemv(const enum CBLAS_LAYOUT layout, const enum CBLAS_TRANSPOSE TransA, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *X, const int incX, const double beta, double *Y, const int incY);
+
 // CBLAS 3
 //
-void cblas_dgemm(const enum CBLAS_ORDER Order, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *B, const int ldb, const double beta, double *C, const int ldc);
+void cblas_dgemm(const enum CBLAS_LAYOUT layout, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *B, const int ldb, const double beta, double *C, const int ldc);
 //
-void cblas_dsyrk(const enum CBLAS_ORDER Order, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE Trans, const int N, const int K, const double alpha, const double *A, const int lda, const double beta, double *C, const int ldc);
+void cblas_dsyrk(const enum CBLAS_LAYOUT layout, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE Trans, const int N, const int K, const double alpha, const double *A, const int lda, const double beta, double *C, const int ldc);
 //
-void cblas_dtrmm(const enum CBLAS_ORDER Order, const enum CBLAS_SIDE Side, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb);
+void cblas_dtrmm(const enum CBLAS_LAYOUT layout, const enum CBLAS_SIDE Side, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb);
 //
-void cblas_dtrsm(const enum CBLAS_ORDER Order, const enum CBLAS_SIDE Side, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb);
+void cblas_dtrsm(const enum CBLAS_LAYOUT layout, const enum CBLAS_SIDE Side, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb);
 
 
 
@@ -240,15 +246,19 @@ void blasfeo_cblas_dswap(const int N, double *X, const int incX, double *Y, cons
 //
 void blasfeo_cblas_dcopy(const int N, const double *X, const int incX, double *Y, const int incY);
 
+// CBLAS 2
+//
+void blasfeo_cblas_dgemv(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const int M, const int N, const double alpha, const double *A, const int lda, const double *X, const int incX, const double beta, double *Y, const int incY);
+
 // CBLAS 3
 //
-void blasfeo_cblas_dgemm(const enum BLASFEO_CBLAS_ORDER Order, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *B, const int ldb, const double beta, double *C, const int ldc);
+void blasfeo_cblas_dgemm(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *B, const int ldb, const double beta, double *C, const int ldc);
 //
-void blasfeo_cblas_dsyrk(const enum BLASFEO_CBLAS_ORDER Order, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE Trans, const int N, const int K, const double alpha, const double *A, const int lda, const double beta, double *C, const int ldc);
+void blasfeo_cblas_dsyrk(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE Trans, const int N, const int K, const double alpha, const double *A, const int lda, const double beta, double *C, const int ldc);
 //
-void blasfeo_cblas_dtrmm(const enum BLASFEO_CBLAS_ORDER Order, const enum BLASFEO_CBLAS_SIDE Side, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb);
+void blasfeo_cblas_dtrmm(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_SIDE Side, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb);
 //
-void blasfeo_cblas_dtrsm(const enum BLASFEO_CBLAS_ORDER Order, const enum BLASFEO_CBLAS_SIDE Side, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb);
+void blasfeo_cblas_dtrsm(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_SIDE Side, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb);
 
 
 
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_target.h b/third_party/acados/include/blasfeo/include/blasfeo_target.h
index 4ff4f307b3..51f617a649 100644
--- a/third_party/acados/include/blasfeo/include/blasfeo_target.h
+++ b/third_party/acados/include/blasfeo/include/blasfeo_target.h
@@ -1,13 +1,13 @@
-#ifndef TARGET_ARMV8A_ARM_CORTEX_A57
-#define TARGET_ARMV8A_ARM_CORTEX_A57
+#ifndef TARGET_X64_INTEL_HASWELL
+#define TARGET_X64_INTEL_HASWELL
 #endif
 
 #ifndef TARGET_NEED_FEATURE_AVX2
-/* #undef TARGET_NEED_FEATURE_AVX2 */
+#define TARGET_NEED_FEATURE_AVX2 1
 #endif
 
 #ifndef TARGET_NEED_FEATURE_FMA
-/* #undef TARGET_NEED_FEATURE_FMA */
+#define TARGET_NEED_FEATURE_FMA 1
 #endif
 
 #ifndef TARGET_NEED_FEATURE_SSE3
@@ -27,11 +27,11 @@
 #endif
 
 #ifndef TARGET_NEED_FEATURE_VFPv4
-#define TARGET_NEED_FEATURE_VFPv4 1
+/* #undef TARGET_NEED_FEATURE_VFPv4 */
 #endif
 
 #ifndef TARGET_NEED_FEATURE_NEONv2
-#define TARGET_NEED_FEATURE_NEONv2 1
+/* #undef TARGET_NEED_FEATURE_NEONv2 */
 #endif
 
 #ifndef LA_HIGH_PERFORMANCE
diff --git a/third_party/acados/include/qpOASES_e/Constants.h b/third_party/acados/include/qpOASES_e/Constants.h
index 0e3dcd19f4..13c777d75b 100644
--- a/third_party/acados/include/qpOASES_e/Constants.h
+++ b/third_party/acados/include/qpOASES_e/Constants.h
@@ -1,134 +1,134 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *	See the GNU Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES_e/Constants.h
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.1embedded
- *	\date 2007-2015
- *
- *	Definition of all global constants.
- */
-
-
-#ifndef QPOASES_CONSTANTS_H
-#define QPOASES_CONSTANTS_H
-
-
-#include <qpOASES_e/Types.h>
-
-#ifdef __CODE_GENERATION__
-
-  #define CONVERTTOSTRINGAUX(x) #x
-  #define CONVERTTOSTRING(x) CONVERTTOSTRINGAUX(x)
-
-  #ifndef QPOASES_CUSTOM_INTERFACE
-  #include "acado_qpoases3_interface.h"
-  #else
-  #include CONVERTTOSTRING(QPOASES_CUSTOM_INTERFACE)
-  #endif
-
-#endif
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-#ifndef __EXTERNAL_DIMENSIONS__
-
-  /*#define QPOASES_NVMAX  50
-  #define QPOASES_NCMAX  100*/
-  #define QPOASES_NVMAX  287
-  #define QPOASES_NCMAX  709
-
-#endif /* __EXTERNAL_DIMENSIONS__ */
-
-
-/** Maximum number of variables within a QP formulation.
- *	Note: this value has to be positive! */
-#define NVMAX  QPOASES_NVMAX
-
-/** Maximum number of constraints within a QP formulation.
- *	Note: this value has to be positive! */
-#define NCMAX  QPOASES_NCMAX
-
-#if ( QPOASES_NVMAX > QPOASES_NCMAX )
-#define NVCMAX QPOASES_NVMAX
-#else
-#define NVCMAX QPOASES_NCMAX
-#endif
-
-#if ( QPOASES_NVMAX > QPOASES_NCMAX )
-#define NVCMIN QPOASES_NCMAX
-#else
-#define NVCMIN QPOASES_NVMAX
-#endif
-
-
-/** Maximum number of QPs in a sequence solved by means of the OQP interface.
- *	Note: this value has to be positive! */
-#define NQPMAX 1000
-
-
-/** Numerical value of machine precision (min eps, s.t. 1+eps > 1).
- *	Note: this value has to be positive! */
-#ifndef __CODE_GENERATION__
-
-  #ifdef __USE_SINGLE_PRECISION__
-  static const real_t QPOASES_EPS = 1.193e-07;
-  #else
-  static const real_t QPOASES_EPS = 2.221e-16;
-  #endif /* __USE_SINGLE_PRECISION__ */
-
-#endif /* __CODE_GENERATION__ */
-
-
-/** Numerical value of zero (for situations in which it would be
- *	unreasonable to compare with 0.0).
- *  Note: this value has to be positive! */
-static const real_t QPOASES_ZERO = 1.0e-25;
-
-/** Numerical value of infinity (e.g. for non-existing bounds).
- *	Note: this value has to be positive! */
-static const real_t QPOASES_INFTY = 1.0e20;
-
-/** Tolerance to used for isEqual, isZero etc.
- *	Note: this value has to be positive! */
-static const real_t QPOASES_TOL = 1.0e-25;
-
-
-/** Maximum number of characters within a string.
- *	Note: this value should be at least 41! */
-#define QPOASES_MAX_STRING_LENGTH 160
-
-
-END_NAMESPACE_QPOASES
-
-
-#endif	/* QPOASES_CONSTANTS_H */
-
-
-/*
- *	end of file
- */
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/Constants.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ *
+ *	Definition of all global constants.
+ */
+
+
+#ifndef QPOASES_CONSTANTS_H
+#define QPOASES_CONSTANTS_H
+
+
+#include <qpOASES_e/Types.h>
+
+#ifdef __CODE_GENERATION__
+
+  #define CONVERTTOSTRINGAUX(x) #x
+  #define CONVERTTOSTRING(x) CONVERTTOSTRINGAUX(x)
+
+  #ifndef QPOASES_CUSTOM_INTERFACE
+  #include "acado_qpoases3_interface.h"
+  #else
+  #include CONVERTTOSTRING(QPOASES_CUSTOM_INTERFACE)
+  #endif
+
+#endif
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+#ifndef __EXTERNAL_DIMENSIONS__
+
+  /*#define QPOASES_NVMAX  50
+  #define QPOASES_NCMAX  100*/
+  #define QPOASES_NVMAX  287
+  #define QPOASES_NCMAX  709
+
+#endif /* __EXTERNAL_DIMENSIONS__ */
+
+
+/** Maximum number of variables within a QP formulation.
+ *	Note: this value has to be positive! */
+#define NVMAX  QPOASES_NVMAX
+
+/** Maximum number of constraints within a QP formulation.
+ *	Note: this value has to be positive! */
+#define NCMAX  QPOASES_NCMAX
+
+#if ( QPOASES_NVMAX > QPOASES_NCMAX )
+#define NVCMAX QPOASES_NVMAX
+#else
+#define NVCMAX QPOASES_NCMAX
+#endif
+
+#if ( QPOASES_NVMAX > QPOASES_NCMAX )
+#define NVCMIN QPOASES_NCMAX
+#else
+#define NVCMIN QPOASES_NVMAX
+#endif
+
+
+/** Maximum number of QPs in a sequence solved by means of the OQP interface.
+ *	Note: this value has to be positive! */
+#define NQPMAX 1000
+
+
+/** Numerical value of machine precision (min eps, s.t. 1+eps > 1).
+ *	Note: this value has to be positive! */
+#ifndef __CODE_GENERATION__
+
+  #ifdef __USE_SINGLE_PRECISION__
+  static const real_t QPOASES_EPS = 1.193e-07;
+  #else
+  static const real_t QPOASES_EPS = 2.221e-16;
+  #endif /* __USE_SINGLE_PRECISION__ */
+
+#endif /* __CODE_GENERATION__ */
+
+
+/** Numerical value of zero (for situations in which it would be
+ *	unreasonable to compare with 0.0).
+ *  Note: this value has to be positive! */
+static const real_t QPOASES_ZERO = 1.0e-25;
+
+/** Numerical value of infinity (e.g. for non-existing bounds).
+ *	Note: this value has to be positive! */
+static const real_t QPOASES_INFTY = 1.0e20;
+
+/** Tolerance to used for isEqual, isZero etc.
+ *	Note: this value has to be positive! */
+static const real_t QPOASES_TOL = 1.0e-25;
+
+
+/** Maximum number of characters within a string.
+ *	Note: this value should be at least 41! */
+#define QPOASES_MAX_STRING_LENGTH 160
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_CONSTANTS_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/include/qpOASES_e/ConstraintProduct.h b/third_party/acados/include/qpOASES_e/ConstraintProduct.h
index ddfcfbe5dc..eb5400c6cb 100644
--- a/third_party/acados/include/qpOASES_e/ConstraintProduct.h
+++ b/third_party/acados/include/qpOASES_e/ConstraintProduct.h
@@ -1,62 +1,62 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *	See the GNU Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES_e/ConstraintProduct.h
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches (thanks to D. Kwame Minde Kufoalor)
- *	\version 3.1embedded
- *	\date 2009-2015
- *
- *	Declaration of the ConstraintProduct interface which allows to specify a
- *	user-defined function for evaluating the constraint product at the 
- *	current iterate to speed-up QP solution in case of a specially structured
- *	constraint matrix.
- */
-
-
-
-#ifndef QPOASES_CONSTRAINT_PRODUCT_H
-#define QPOASES_CONSTRAINT_PRODUCT_H
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/** 
- *	\brief Interface for specifying user-defined evaluations of constraint products.
- *
- *	An interface which allows to specify a user-defined function for evaluating the 
- *	constraint product at the current iterate to speed-up QP solution in case 
- *	of a specially structured constraint matrix.
- *
- *	\author Hans Joachim Ferreau (thanks to Kwame Minde Kufoalor)
- *	\version 3.1embedded
- *	\date 2009-2015
- */
-typedef int(*ConstraintProduct)( int, const real_t* const, real_t* const );
-
-
-END_NAMESPACE_QPOASES
-
-#endif	/* QPOASES_CONSTRAINT_PRODUCT_H */
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/ConstraintProduct.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches (thanks to D. Kwame Minde Kufoalor)
+ *	\version 3.1embedded
+ *	\date 2009-2015
+ *
+ *	Declaration of the ConstraintProduct interface which allows to specify a
+ *	user-defined function for evaluating the constraint product at the 
+ *	current iterate to speed-up QP solution in case of a specially structured
+ *	constraint matrix.
+ */
+
+
+
+#ifndef QPOASES_CONSTRAINT_PRODUCT_H
+#define QPOASES_CONSTRAINT_PRODUCT_H
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** 
+ *	\brief Interface for specifying user-defined evaluations of constraint products.
+ *
+ *	An interface which allows to specify a user-defined function for evaluating the 
+ *	constraint product at the current iterate to speed-up QP solution in case 
+ *	of a specially structured constraint matrix.
+ *
+ *	\author Hans Joachim Ferreau (thanks to Kwame Minde Kufoalor)
+ *	\version 3.1embedded
+ *	\date 2009-2015
+ */
+typedef int(*ConstraintProduct)( int, const real_t* const, real_t* const );
+
+
+END_NAMESPACE_QPOASES
+
+#endif	/* QPOASES_CONSTRAINT_PRODUCT_H */
diff --git a/third_party/acados/include/qpOASES_e/Indexlist.h b/third_party/acados/include/qpOASES_e/Indexlist.h
index c3026a7ffc..02d259d63d 100644
--- a/third_party/acados/include/qpOASES_e/Indexlist.h
+++ b/third_party/acados/include/qpOASES_e/Indexlist.h
@@ -1,221 +1,221 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *	See the GNU Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES_e/Indexlist.h
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.1embedded
- *	\date 2007-2015
- *
- *	Declaration of the Indexlist class designed to manage index lists of
- *	constraints and bounds within a SubjectTo object.
- */
-
-
-#ifndef QPOASES_INDEXLIST_H
-#define QPOASES_INDEXLIST_H
-
-
-#include <qpOASES_e/Utils.h>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/**
- *	\brief Stores and manages index lists.
- *
- *	This class manages index lists of active/inactive bounds/constraints.
- *
- *	\author Hans Joachim Ferreau
- *	\version 3.1embedded
- *	\date 2007-2015
- */
-typedef struct
-{
-	int *number;		/**< Array to store numbers of constraints or bounds. */
-	int *iSort;			/**< Index list to sort vector \a number */
-
-	int	length;			/**< Length of index list. */
-	int	first;			/**< Physical index of first element. */
-	int	last;			/**< Physical index of last element. */
-	int	lastusedindex;	/**< Physical index of last entry in index list. */
-	int	physicallength;	/**< Physical length of index list. */
-} Indexlist;
-
-int Indexlist_calculateMemorySize( int n);
-
-char *Indexlist_assignMemory(int n, Indexlist **mem, void *raw_memory);
-
-Indexlist *Indexlist_createMemory( int n );
-
-/** Constructor which takes the desired physical length of the index list. */
-void IndexlistCON(	Indexlist* _THIS,
-					int n	/**< Physical length of index list. */
-					);
-
-/** Copies all members from given rhs object.
- *  \return SUCCESSFUL_RETURN */
-void IndexlistCPY(	Indexlist* FROM,
-					Indexlist* TO
-					);
-
-/** Initialises index list of desired physical length.
- *	\return SUCCESSFUL_RETURN \n
- 			RET_INVALID_ARGUMENTS */
-returnValue Indexlist_init(	Indexlist* _THIS,
-							int n		/**< Physical length of index list. */
-							);
-
-/** Creates an array of all numbers within the index set in correct order.
- *	\return SUCCESSFUL_RETURN \n
- 			RET_INDEXLIST_CORRUPTED */
-returnValue Indexlist_getNumberArray(	Indexlist* _THIS,
-										int** const numberarray	/**< Output: Array of numbers (NULL on error). */
-										);
-
-/** Creates an array of all numbers within the index set in correct order.
- *	\return SUCCESSFUL_RETURN \n
-			RET_INDEXLIST_CORRUPTED */
-returnValue	Indexlist_getISortArray(	Indexlist* _THIS,
-										int** const iSortArray	/**< Output: iSort Array. */
-										);
-
-
-/** Determines the index within the index list at which a given number is stored.
- *	\return >= 0: Index of given number. \n
- 			-1: Number not found. */
-int Indexlist_getIndex(	Indexlist* _THIS,
-						int givennumber	/**< Number whose index shall be determined. */
-						);
-
-/** Returns the number stored at a given physical index.
- *	\return >= 0: Number stored at given physical index. \n
- 			-RET_INDEXLIST_OUTOFBOUNDS */
-static inline int Indexlist_getNumber(	Indexlist* _THIS,
-										int physicalindex	/**< Physical index of the number to be returned. */
-										);
-
-
-/** Returns the current length of the index list.
- *	\return Current length of the index list. */
-static inline int Indexlist_getLength(	Indexlist* _THIS
-										);
-
-/** Returns last number within the index list.
- *	\return Last number within the index list. */
-static inline int Indexlist_getLastNumber(	Indexlist* _THIS
-											);
-
-
-/** Adds number to index list.
- *	\return SUCCESSFUL_RETURN \n
- 			RET_INDEXLIST_MUST_BE_REORDERD \n
- 			RET_INDEXLIST_EXCEEDS_MAX_LENGTH */
-returnValue Indexlist_addNumber(	Indexlist* _THIS,
-									int addnumber			/**< Number to be added. */
-									);
-
-/** Removes number from index list.
- *	\return SUCCESSFUL_RETURN */
-returnValue Indexlist_removeNumber(	Indexlist* _THIS,
-									int removenumber	/**< Number to be removed. */
-									);
-
-/** Swaps two numbers within index list.
- *	\return SUCCESSFUL_RETURN */
-returnValue Indexlist_swapNumbers(	Indexlist* _THIS,
-									int number1,		/**< First number for swapping. */
-									int number2			/**< Second number for swapping. */
-									);
-
-/** Determines if a given number is contained in the index set.
- *	\return BT_TRUE iff number is contain in the index set */
-static inline BooleanType Indexlist_isMember(	Indexlist* _THIS,
-												int _number		/**< Number to be tested for membership. */
-												);
-
-
-/** Find first index j between -1 and length in sorted list of indices
- *  iSort such that numbers[iSort[j]] <= i < numbers[iSort[j+1]]. Uses
- *  bisection.
- *  \return j. */
-int Indexlist_findInsert(	Indexlist* _THIS,
-							int i
-							);
-
-
-
-/*
- *	g e t N u m b e r
- */
-static inline int Indexlist_getNumber( Indexlist* _THIS, int physicalindex )
-{
-	/* consistency check */
-	if ( ( physicalindex < 0 ) || ( physicalindex > _THIS->length ) )
-		return -RET_INDEXLIST_OUTOFBOUNDS;
-
-	return _THIS->number[physicalindex];
-}
-
-
-/*
- *	g e t L e n g t h
- */
-static inline int Indexlist_getLength( Indexlist* _THIS )
-{
-	return _THIS->length;
-}
-
-
-/*
- *	g e t L a s t N u m b e r
- */
-static inline int Indexlist_getLastNumber( Indexlist* _THIS )
-{
-	return _THIS->number[_THIS->length-1];
-}
-
-
-/*
- *	g e t L a s t N u m b e r
- */
-static inline BooleanType Indexlist_isMember( Indexlist* _THIS, int _number )
-{
-	if ( Indexlist_getIndex( _THIS,_number ) >= 0 )
-		return BT_TRUE;
-	else
-		return BT_FALSE;
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-#endif	/* QPOASES_INDEXLIST_H */
-
-
-/*
- *	end of file
- */
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/Indexlist.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ *
+ *	Declaration of the Indexlist class designed to manage index lists of
+ *	constraints and bounds within a SubjectTo object.
+ */
+
+
+#ifndef QPOASES_INDEXLIST_H
+#define QPOASES_INDEXLIST_H
+
+
+#include <qpOASES_e/Utils.h>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/**
+ *	\brief Stores and manages index lists.
+ *
+ *	This class manages index lists of active/inactive bounds/constraints.
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ */
+typedef struct
+{
+	int *number;		/**< Array to store numbers of constraints or bounds. */
+	int *iSort;			/**< Index list to sort vector \a number */
+
+	int	length;			/**< Length of index list. */
+	int	first;			/**< Physical index of first element. */
+	int	last;			/**< Physical index of last element. */
+	int	lastusedindex;	/**< Physical index of last entry in index list. */
+	int	physicallength;	/**< Physical length of index list. */
+} Indexlist;
+
+int Indexlist_calculateMemorySize( int n);
+
+char *Indexlist_assignMemory(int n, Indexlist **mem, void *raw_memory);
+
+Indexlist *Indexlist_createMemory( int n );
+
+/** Constructor which takes the desired physical length of the index list. */
+void IndexlistCON(	Indexlist* _THIS,
+					int n	/**< Physical length of index list. */
+					);
+
+/** Copies all members from given rhs object.
+ *  \return SUCCESSFUL_RETURN */
+void IndexlistCPY(	Indexlist* FROM,
+					Indexlist* TO
+					);
+
+/** Initialises index list of desired physical length.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INVALID_ARGUMENTS */
+returnValue Indexlist_init(	Indexlist* _THIS,
+							int n		/**< Physical length of index list. */
+							);
+
+/** Creates an array of all numbers within the index set in correct order.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INDEXLIST_CORRUPTED */
+returnValue Indexlist_getNumberArray(	Indexlist* _THIS,
+										int** const numberarray	/**< Output: Array of numbers (NULL on error). */
+										);
+
+/** Creates an array of all numbers within the index set in correct order.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INDEXLIST_CORRUPTED */
+returnValue	Indexlist_getISortArray(	Indexlist* _THIS,
+										int** const iSortArray	/**< Output: iSort Array. */
+										);
+
+
+/** Determines the index within the index list at which a given number is stored.
+ *	\return >= 0: Index of given number. \n
+ 			-1: Number not found. */
+int Indexlist_getIndex(	Indexlist* _THIS,
+						int givennumber	/**< Number whose index shall be determined. */
+						);
+
+/** Returns the number stored at a given physical index.
+ *	\return >= 0: Number stored at given physical index. \n
+ 			-RET_INDEXLIST_OUTOFBOUNDS */
+static inline int Indexlist_getNumber(	Indexlist* _THIS,
+										int physicalindex	/**< Physical index of the number to be returned. */
+										);
+
+
+/** Returns the current length of the index list.
+ *	\return Current length of the index list. */
+static inline int Indexlist_getLength(	Indexlist* _THIS
+										);
+
+/** Returns last number within the index list.
+ *	\return Last number within the index list. */
+static inline int Indexlist_getLastNumber(	Indexlist* _THIS
+											);
+
+
+/** Adds number to index list.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INDEXLIST_MUST_BE_REORDERD \n
+ 			RET_INDEXLIST_EXCEEDS_MAX_LENGTH */
+returnValue Indexlist_addNumber(	Indexlist* _THIS,
+									int addnumber			/**< Number to be added. */
+									);
+
+/** Removes number from index list.
+ *	\return SUCCESSFUL_RETURN */
+returnValue Indexlist_removeNumber(	Indexlist* _THIS,
+									int removenumber	/**< Number to be removed. */
+									);
+
+/** Swaps two numbers within index list.
+ *	\return SUCCESSFUL_RETURN */
+returnValue Indexlist_swapNumbers(	Indexlist* _THIS,
+									int number1,		/**< First number for swapping. */
+									int number2			/**< Second number for swapping. */
+									);
+
+/** Determines if a given number is contained in the index set.
+ *	\return BT_TRUE iff number is contain in the index set */
+static inline BooleanType Indexlist_isMember(	Indexlist* _THIS,
+												int _number		/**< Number to be tested for membership. */
+												);
+
+
+/** Find first index j between -1 and length in sorted list of indices
+ *  iSort such that numbers[iSort[j]] <= i < numbers[iSort[j+1]]. Uses
+ *  bisection.
+ *  \return j. */
+int Indexlist_findInsert(	Indexlist* _THIS,
+							int i
+							);
+
+
+
+/*
+ *	g e t N u m b e r
+ */
+static inline int Indexlist_getNumber( Indexlist* _THIS, int physicalindex )
+{
+	/* consistency check */
+	if ( ( physicalindex < 0 ) || ( physicalindex > _THIS->length ) )
+		return -RET_INDEXLIST_OUTOFBOUNDS;
+
+	return _THIS->number[physicalindex];
+}
+
+
+/*
+ *	g e t L e n g t h
+ */
+static inline int Indexlist_getLength( Indexlist* _THIS )
+{
+	return _THIS->length;
+}
+
+
+/*
+ *	g e t L a s t N u m b e r
+ */
+static inline int Indexlist_getLastNumber( Indexlist* _THIS )
+{
+	return _THIS->number[_THIS->length-1];
+}
+
+
+/*
+ *	g e t L a s t N u m b e r
+ */
+static inline BooleanType Indexlist_isMember( Indexlist* _THIS, int _number )
+{
+	if ( Indexlist_getIndex( _THIS,_number ) >= 0 )
+		return BT_TRUE;
+	else
+		return BT_FALSE;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_INDEXLIST_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/include/qpOASES_e/Matrices.h b/third_party/acados/include/qpOASES_e/Matrices.h
index 1d6da8c3c1..e2a46b3a9d 100644
--- a/third_party/acados/include/qpOASES_e/Matrices.h
+++ b/third_party/acados/include/qpOASES_e/Matrices.h
@@ -1,287 +1,287 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *	See the GNU Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES_e/Matrices.h
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.1embedded
- *	\date 2009-2015
- *
- *  Various matrix classes: Abstract base matrix class, dense and sparse matrices,
- *  including symmetry exploiting specializations.
- */
-
-
-
-#ifndef QPOASES_MATRICES_H
-#define QPOASES_MATRICES_H
-
-#ifdef __USE_SINGLE_PRECISION__
-
-	// single precision
-	#define GEMM sgemm_
-	#define GEMV sgemv_
-//	#define SYR ssyr_
-//	#define SYR2 ssyr2_
-	#define POTRF spotrf_
-
-#else
-
-	// double precision
-	#define GEMM dgemm_
-	#define GEMV dgemv_
-//	#define SYR  dsyr_
-//	#define SYR2 dsyr2_
-	#define POTRF dpotrf_
-
-#endif /* __USE_SINGLE_PRECISION__ */
-
-
-#ifdef EXTERNAL_BLAS
-	// double precision
-	void dgemm_(char *ta, char *tb, int *m, int *n, int *k, double *alpha, double *A, int *lda, double *B, int ldb, double *beta, double *C, int *ldc);
-	void dgemv_(char *ta, int *m, int *n, double *alpha, double *A, int *lda, double *x, int *incx, double *beta, double *y, int *incy);
-	void dpotrf_(char *uplo, int *m, double *A, int *lda, int *info);
-	// single precision
-	void sgemm_(char *ta, char *tb, int *m, int *n, int *k, float *alpha, float *A, int *lda, float *B, int ldb, float *beta, float *C, int *ldc);
-	void sgemv_(char *ta, int *m, int *n, float *alpha, float *A, int *lda, float *x, int *incx, float *beta, float *y, int *incy);
-	void spotrf_(char *uplo, int *m, float *A, int *lda, int *info);
-#else
-	/** Performs one of the matrix-matrix operation in double precision. */
-	void dgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*,
-			const double*, const double*, const unsigned long*, const double*, const unsigned long*,
-			const double*, double*, const unsigned long* );
-	/** Performs one of the matrix-matrix operation in single precision. */
-	void sgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*,
-			const float*, const float*, const unsigned long*, const float*, const unsigned long*,
-			const float*, float*, const unsigned long* );
-
-	/** Calculates the Cholesky factorization of a real symmetric positive definite matrix in double precision. */
-	void dpotrf_ ( const char *, const unsigned long *, double *, const unsigned long *, long * );
-	/** Calculates the Cholesky factorization of a real symmetric positive definite matrix in single precision. */
-	void spotrf_ ( const char *, const unsigned long *, float *, const unsigned long *, long * );
-
-#endif
-
-	/** Performs a symmetric rank 1 operation in double precision. */
-//	void dsyr_ ( const char *, const unsigned long *, const double *, const double *,
-//				 const unsigned long *, double *, const unsigned long *);
-	/** Performs a symmetric rank 1 operation in single precision. */
-//	void ssyr_ ( const char *, const unsigned long *, const float *, const float *,
-//				 const unsigned long *, float *, const unsigned long *);
-
-	/** Performs a symmetric rank 2 operation in double precision. */
-//	void dsyr2_ ( const char *, const unsigned long *, const double *, const double *,
-//				  const unsigned long *, const double *, const unsigned long *, double *, const unsigned long *);
-	/** Performs a symmetric rank 2 operation in single precision. */
-//	void ssyr2_ ( const char *, const unsigned long *, const float *, const float *,
-//				  const unsigned long *, const float *, const unsigned long *, float *, const unsigned long *);
-
-
-#include <qpOASES_e/Indexlist.h>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/**
- *	\brief Interfaces matrix-vector operations tailored to general dense matrices.
- *
- *	Dense matrix class (row major format).
- *
- *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
- *	\version 3.1embedded
- *	\date 2011-2015
- */
-typedef struct
-{
-	real_t *val;				/**< Vector of entries. */
-	int nRows;					/**< Number of rows. */
-	int nCols;					/**< Number of columns. */
-	int leaDim;					/**< Leading dimension. */
-} DenseMatrix;
-
-int DenseMatrix_calculateMemorySize( int m, int n );
-
-char *DenseMatrix_assignMemory( int m, int n, DenseMatrix **mem, void *raw_memory );
-
-DenseMatrix *DenseMatrix_createMemory( int m, int n );
-
-/** Constructor from vector of values.
- *  Caution: Data pointer must be valid throughout lifetime
- */
-void DenseMatrixCON(	DenseMatrix* _THIS,
-						int m,			/**< Number of rows. */
-						int n,			/**< Number of columns. */
-						int lD,			/**< Leading dimension. */
-						real_t *v		/**< Values. */
-						);
-
-void DenseMatrixCPY(	DenseMatrix* FROM,
-						DenseMatrix* TO
-						);
-
-
-/** Frees all internal memory. */
-void DenseMatrix_free( DenseMatrix* _THIS );
-
-/** Constructor from vector of values.
- *  Caution: Data pointer must be valid throughout lifetime
- */
-returnValue DenseMatrix_init(	DenseMatrix* _THIS,
-								int m,			/**< Number of rows. */
-								int n,			/**< Number of columns. */
-								int lD,			/**< Leading dimension. */
-								real_t *v		/**< Values. */
-								);
-
-
-/** Returns i-th diagonal entry.
- *	\return i-th diagonal entry */
-real_t DenseMatrix_diag(	DenseMatrix* _THIS,
-							int i			/**< Index. */
-							);
-
-/** Checks whether matrix is square and diagonal.
- *	\return BT_TRUE  iff matrix is square and diagonal; \n
- *	        BT_FALSE otherwise. */
-BooleanType DenseMatrix_isDiag( DenseMatrix* _THIS );
-
-/** Get the N-norm of the matrix
- *  \return N-norm of the matrix
- */
-real_t DenseMatrix_getNorm( DenseMatrix* _THIS,
-							int type			/**< Norm type, 1: one-norm, 2: Euclidean norm. */
-							);
-
-/** Get the N-norm of a row
- *  \return N-norm of row \a rNum
- */
-real_t DenseMatrix_getRowNorm(	DenseMatrix* _THIS,
-								int rNum,			/**< Row number. */
-								int type			/**< Norm type, 1: one-norm, 2: Euclidean norm. */
-								);
-
-/** Retrieve indexed entries of matrix row multiplied by alpha.
- *  \return SUCCESSFUL_RETURN */
-returnValue DenseMatrix_getRow(	DenseMatrix* _THIS,
-								int rNum,						/**< Row number. */
-								const Indexlist* const icols,	/**< Index list specifying columns. */
-								real_t alpha,					/**< Scalar factor. */
-								real_t *row						/**< Output row vector. */
-								);
-
-/** Retrieve indexed entries of matrix column multiplied by alpha.
- *  \return SUCCESSFUL_RETURN */
- returnValue DenseMatrix_getCol(	DenseMatrix* _THIS,
-									int cNum,						/**< Column number. */
-									const Indexlist* const irows,	/**< Index list specifying rows. */
-									real_t alpha,					/**< Scalar factor. */
-									real_t *col						/**< Output column vector. */
-									);
-
-/** Evaluate Y=alpha*A*X + beta*Y.
- *  \return SUCCESSFUL_RETURN. */
-returnValue DenseMatrix_times(	DenseMatrix* _THIS,
-								int xN,					/**< Number of vectors to multiply. */
-								real_t alpha,			/**< Scalar factor for matrix vector product. */
-								const real_t *x,		/**< Input vector to be multiplied. */
-								int xLD,				/**< Leading dimension of input x. */
-								real_t beta,			/**< Scalar factor for y. */
-								real_t *y,				/**< Output vector of results. */
-								int yLD					/**< Leading dimension of output y. */
-								);
-
-/** Evaluate Y=alpha*A'*X + beta*Y.
- *  \return SUCCESSFUL_RETURN. */
-returnValue DenseMatrix_transTimes(	DenseMatrix* _THIS,
-									int xN,				/**< Number of vectors to multiply. */
-									real_t alpha,		/**< Scalar factor for matrix vector product. */
-									const real_t *x,	/**< Input vector to be multiplied. */
-									int xLD,			/**< Leading dimension of input x. */
-									real_t beta,		/**< Scalar factor for y. */
-									real_t *y,			/**< Output vector of results. */
-									int yLD				/**< Leading dimension of output y. */
-									);
-
-/** Evaluate matrix vector product with submatrix given by Indexlist.
- *	\return SUCCESSFUL_RETURN */
- returnValue DenseMatrix_subTimes(	DenseMatrix* _THIS,
-									const Indexlist* const irows,	/**< Index list specifying rows. */
-									const Indexlist* const icols,	/**< Index list specifying columns. */
-									int xN,							/**< Number of vectors to multiply. */
-									real_t alpha,					/**< Scalar factor for matrix vector product. */
-									const real_t *x,				/**< Input vector to be multiplied. */
-									int xLD,						/**< Leading dimension of input x. */
-									real_t beta,					/**< Scalar factor for y. */
-									real_t *y,						/**< Output vector of results. */
-									int yLD,						/**< Leading dimension of output y. */
-									BooleanType yCompr 				/**< Compressed storage for y. */
-									);
-
-/** Evaluate matrix transpose vector product.
- *	\return SUCCESSFUL_RETURN */
-returnValue DenseMatrix_subTransTimes(	DenseMatrix* _THIS,
-										const Indexlist* const irows,	/**< Index list specifying rows. */
-										const Indexlist* const icols,	/**< Index list specifying columns. */
-										int xN,							/**< Number of vectors to multiply. */
-										real_t alpha,					/**< Scalar factor for matrix vector product. */
-										const real_t *x,				/**< Input vector to be multiplied. */
-										int xLD,						/**< Leading dimension of input x. */
-										real_t beta,					/**< Scalar factor for y. */
-										real_t *y,						/**< Output vector of results. */
-										int yLD							/**< Leading dimension of output y. */
-										);
-
-/** Adds given offset to diagonal of matrix.
- *	\return SUCCESSFUL_RETURN \n
- 			RET_NO_DIAGONAL_AVAILABLE */
-returnValue DenseMatrix_addToDiag(	DenseMatrix* _THIS,
-									real_t alpha		/**< Diagonal offset. */
-									);
-
-/** Prints matrix to screen.
- *	\return SUCCESSFUL_RETURN */
-returnValue DenseMatrix_print(	DenseMatrix* _THIS
-								);
-
-static inline real_t* DenseMatrix_getVal( DenseMatrix* _THIS ) { return _THIS->val; }
-
-/** Compute bilinear form y = x'*H*x using submatrix given by index list.
- *	\return SUCCESSFUL_RETURN */
-returnValue DenseMatrix_bilinear(	DenseMatrix* _THIS,
-									const Indexlist* const icols,	/**< Index list specifying columns of x. */
-									int xN,							/**< Number of vectors to multiply. */
-									const real_t *x,				/**< Input vector to be multiplied (uncompressed). */
-									int xLD,						/**< Leading dimension of input x. */
-									real_t *y,						/**< Output vector of results (compressed). */
-									int yLD							/**< Leading dimension of output y. */
-									);
-
-
-
-END_NAMESPACE_QPOASES
-
-
-#endif	/* QPOASES_MATRICES_H */
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/Matrices.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2009-2015
+ *
+ *  Various matrix classes: Abstract base matrix class, dense and sparse matrices,
+ *  including symmetry exploiting specializations.
+ */
+
+
+
+#ifndef QPOASES_MATRICES_H
+#define QPOASES_MATRICES_H
+
+#ifdef __USE_SINGLE_PRECISION__
+
+	// single precision
+	#define GEMM sgemm_
+	#define GEMV sgemv_
+//	#define SYR ssyr_
+//	#define SYR2 ssyr2_
+	#define POTRF spotrf_
+
+#else
+
+	// double precision
+	#define GEMM dgemm_
+	#define GEMV dgemv_
+//	#define SYR  dsyr_
+//	#define SYR2 dsyr2_
+	#define POTRF dpotrf_
+
+#endif /* __USE_SINGLE_PRECISION__ */
+
+
+#ifdef EXTERNAL_BLAS
+	// double precision
+	void dgemm_(char *ta, char *tb, int *m, int *n, int *k, double *alpha, double *A, int *lda, double *B, int ldb, double *beta, double *C, int *ldc);
+	void dgemv_(char *ta, int *m, int *n, double *alpha, double *A, int *lda, double *x, int *incx, double *beta, double *y, int *incy);
+	void dpotrf_(char *uplo, int *m, double *A, int *lda, int *info);
+	// single precision
+	void sgemm_(char *ta, char *tb, int *m, int *n, int *k, float *alpha, float *A, int *lda, float *B, int ldb, float *beta, float *C, int *ldc);
+	void sgemv_(char *ta, int *m, int *n, float *alpha, float *A, int *lda, float *x, int *incx, float *beta, float *y, int *incy);
+	void spotrf_(char *uplo, int *m, float *A, int *lda, int *info);
+#else
+	/** Performs one of the matrix-matrix operation in double precision. */
+	void dgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*,
+			const double*, const double*, const unsigned long*, const double*, const unsigned long*,
+			const double*, double*, const unsigned long* );
+	/** Performs one of the matrix-matrix operation in single precision. */
+	void sgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*,
+			const float*, const float*, const unsigned long*, const float*, const unsigned long*,
+			const float*, float*, const unsigned long* );
+
+	/** Calculates the Cholesky factorization of a real symmetric positive definite matrix in double precision. */
+	void dpotrf_ ( const char *, const unsigned long *, double *, const unsigned long *, long * );
+	/** Calculates the Cholesky factorization of a real symmetric positive definite matrix in single precision. */
+	void spotrf_ ( const char *, const unsigned long *, float *, const unsigned long *, long * );
+
+#endif
+
+	/** Performs a symmetric rank 1 operation in double precision. */
+//	void dsyr_ ( const char *, const unsigned long *, const double *, const double *,
+//				 const unsigned long *, double *, const unsigned long *);
+	/** Performs a symmetric rank 1 operation in single precision. */
+//	void ssyr_ ( const char *, const unsigned long *, const float *, const float *,
+//				 const unsigned long *, float *, const unsigned long *);
+
+	/** Performs a symmetric rank 2 operation in double precision. */
+//	void dsyr2_ ( const char *, const unsigned long *, const double *, const double *,
+//				  const unsigned long *, const double *, const unsigned long *, double *, const unsigned long *);
+	/** Performs a symmetric rank 2 operation in single precision. */
+//	void ssyr2_ ( const char *, const unsigned long *, const float *, const float *,
+//				  const unsigned long *, const float *, const unsigned long *, float *, const unsigned long *);
+
+
+#include <qpOASES_e/Indexlist.h>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/**
+ *	\brief Interfaces matrix-vector operations tailored to general dense matrices.
+ *
+ *	Dense matrix class (row major format).
+ *
+ *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
+ *	\version 3.1embedded
+ *	\date 2011-2015
+ */
+typedef struct
+{
+	real_t *val;				/**< Vector of entries. */
+	int nRows;					/**< Number of rows. */
+	int nCols;					/**< Number of columns. */
+	int leaDim;					/**< Leading dimension. */
+} DenseMatrix;
+
+int DenseMatrix_calculateMemorySize( int m, int n );
+
+char *DenseMatrix_assignMemory( int m, int n, DenseMatrix **mem, void *raw_memory );
+
+DenseMatrix *DenseMatrix_createMemory( int m, int n );
+
+/** Constructor from vector of values.
+ *  Caution: Data pointer must be valid throughout lifetime
+ */
+void DenseMatrixCON(	DenseMatrix* _THIS,
+						int m,			/**< Number of rows. */
+						int n,			/**< Number of columns. */
+						int lD,			/**< Leading dimension. */
+						real_t *v		/**< Values. */
+						);
+
+void DenseMatrixCPY(	DenseMatrix* FROM,
+						DenseMatrix* TO
+						);
+
+
+/** Frees all internal memory. */
+void DenseMatrix_free( DenseMatrix* _THIS );
+
+/** Constructor from vector of values.
+ *  Caution: Data pointer must be valid throughout lifetime
+ */
+returnValue DenseMatrix_init(	DenseMatrix* _THIS,
+								int m,			/**< Number of rows. */
+								int n,			/**< Number of columns. */
+								int lD,			/**< Leading dimension. */
+								real_t *v		/**< Values. */
+								);
+
+
+/** Returns i-th diagonal entry.
+ *	\return i-th diagonal entry */
+real_t DenseMatrix_diag(	DenseMatrix* _THIS,
+							int i			/**< Index. */
+							);
+
+/** Checks whether matrix is square and diagonal.
+ *	\return BT_TRUE  iff matrix is square and diagonal; \n
+ *	        BT_FALSE otherwise. */
+BooleanType DenseMatrix_isDiag( DenseMatrix* _THIS );
+
+/** Get the N-norm of the matrix
+ *  \return N-norm of the matrix
+ */
+real_t DenseMatrix_getNorm( DenseMatrix* _THIS,
+							int type			/**< Norm type, 1: one-norm, 2: Euclidean norm. */
+							);
+
+/** Get the N-norm of a row
+ *  \return N-norm of row \a rNum
+ */
+real_t DenseMatrix_getRowNorm(	DenseMatrix* _THIS,
+								int rNum,			/**< Row number. */
+								int type			/**< Norm type, 1: one-norm, 2: Euclidean norm. */
+								);
+
+/** Retrieve indexed entries of matrix row multiplied by alpha.
+ *  \return SUCCESSFUL_RETURN */
+returnValue DenseMatrix_getRow(	DenseMatrix* _THIS,
+								int rNum,						/**< Row number. */
+								const Indexlist* const icols,	/**< Index list specifying columns. */
+								real_t alpha,					/**< Scalar factor. */
+								real_t *row						/**< Output row vector. */
+								);
+
+/** Retrieve indexed entries of matrix column multiplied by alpha.
+ *  \return SUCCESSFUL_RETURN */
+ returnValue DenseMatrix_getCol(	DenseMatrix* _THIS,
+									int cNum,						/**< Column number. */
+									const Indexlist* const irows,	/**< Index list specifying rows. */
+									real_t alpha,					/**< Scalar factor. */
+									real_t *col						/**< Output column vector. */
+									);
+
+/** Evaluate Y=alpha*A*X + beta*Y.
+ *  \return SUCCESSFUL_RETURN. */
+returnValue DenseMatrix_times(	DenseMatrix* _THIS,
+								int xN,					/**< Number of vectors to multiply. */
+								real_t alpha,			/**< Scalar factor for matrix vector product. */
+								const real_t *x,		/**< Input vector to be multiplied. */
+								int xLD,				/**< Leading dimension of input x. */
+								real_t beta,			/**< Scalar factor for y. */
+								real_t *y,				/**< Output vector of results. */
+								int yLD					/**< Leading dimension of output y. */
+								);
+
+/** Evaluate Y=alpha*A'*X + beta*Y.
+ *  \return SUCCESSFUL_RETURN. */
+returnValue DenseMatrix_transTimes(	DenseMatrix* _THIS,
+									int xN,				/**< Number of vectors to multiply. */
+									real_t alpha,		/**< Scalar factor for matrix vector product. */
+									const real_t *x,	/**< Input vector to be multiplied. */
+									int xLD,			/**< Leading dimension of input x. */
+									real_t beta,		/**< Scalar factor for y. */
+									real_t *y,			/**< Output vector of results. */
+									int yLD				/**< Leading dimension of output y. */
+									);
+
+/** Evaluate matrix vector product with submatrix given by Indexlist.
+ *	\return SUCCESSFUL_RETURN */
+ returnValue DenseMatrix_subTimes(	DenseMatrix* _THIS,
+									const Indexlist* const irows,	/**< Index list specifying rows. */
+									const Indexlist* const icols,	/**< Index list specifying columns. */
+									int xN,							/**< Number of vectors to multiply. */
+									real_t alpha,					/**< Scalar factor for matrix vector product. */
+									const real_t *x,				/**< Input vector to be multiplied. */
+									int xLD,						/**< Leading dimension of input x. */
+									real_t beta,					/**< Scalar factor for y. */
+									real_t *y,						/**< Output vector of results. */
+									int yLD,						/**< Leading dimension of output y. */
+									BooleanType yCompr 				/**< Compressed storage for y. */
+									);
+
+/** Evaluate matrix transpose vector product.
+ *	\return SUCCESSFUL_RETURN */
+returnValue DenseMatrix_subTransTimes(	DenseMatrix* _THIS,
+										const Indexlist* const irows,	/**< Index list specifying rows. */
+										const Indexlist* const icols,	/**< Index list specifying columns. */
+										int xN,							/**< Number of vectors to multiply. */
+										real_t alpha,					/**< Scalar factor for matrix vector product. */
+										const real_t *x,				/**< Input vector to be multiplied. */
+										int xLD,						/**< Leading dimension of input x. */
+										real_t beta,					/**< Scalar factor for y. */
+										real_t *y,						/**< Output vector of results. */
+										int yLD							/**< Leading dimension of output y. */
+										);
+
+/** Adds given offset to diagonal of matrix.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_NO_DIAGONAL_AVAILABLE */
+returnValue DenseMatrix_addToDiag(	DenseMatrix* _THIS,
+									real_t alpha		/**< Diagonal offset. */
+									);
+
+/** Prints matrix to screen.
+ *	\return SUCCESSFUL_RETURN */
+returnValue DenseMatrix_print(	DenseMatrix* _THIS
+								);
+
+static inline real_t* DenseMatrix_getVal( DenseMatrix* _THIS ) { return _THIS->val; }
+
+/** Compute bilinear form y = x'*H*x using submatrix given by index list.
+ *	\return SUCCESSFUL_RETURN */
+returnValue DenseMatrix_bilinear(	DenseMatrix* _THIS,
+									const Indexlist* const icols,	/**< Index list specifying columns of x. */
+									int xN,							/**< Number of vectors to multiply. */
+									const real_t *x,				/**< Input vector to be multiplied (uncompressed). */
+									int xLD,						/**< Leading dimension of input x. */
+									real_t *y,						/**< Output vector of results (compressed). */
+									int yLD							/**< Leading dimension of output y. */
+									);
+
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_MATRICES_H */
diff --git a/third_party/acados/include/qpOASES_e/Options.h b/third_party/acados/include/qpOASES_e/Options.h
index b471ee0668..ca8086d2cc 100644
--- a/third_party/acados/include/qpOASES_e/Options.h
+++ b/third_party/acados/include/qpOASES_e/Options.h
@@ -1,153 +1,153 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *	See the GNU Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES_e/Options.h
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.1embedded
- *	\date 2007-2015
- *
- *	Declaration of the Options class designed to manage user-specified
- *	options for solving a QProblem.
- */
-
-
-#ifndef QPOASES_OPTIONS_H
-#define QPOASES_OPTIONS_H
-
-
-#include <qpOASES_e/Utils.h>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/**
- *	\brief Manages all user-specified options for solving QPs.
- *
- *	This class manages all user-specified options used for solving
- *	quadratic programs.
- *
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.1embedded
- *	\date 2007-2015
- */
-typedef struct
-{
-	PrintLevel printLevel;					/**< Print level. */
-
-	BooleanType enableRamping;				/**< Specifies whether ramping shall be enabled or not. */
-	BooleanType enableFarBounds;			/**< Specifies whether far bounds shall be used or not. */
-	BooleanType enableFlippingBounds;		/**< Specifies whether flipping bounds shall be used or not. */
-	BooleanType enableRegularisation;		/**< Specifies whether Hessian matrix shall be regularised in case semi-definiteness is detected. */
-	BooleanType enableFullLITests;			/**< Specifies whether condition-hardened LI test shall be used or not. */
-	BooleanType enableNZCTests;				/**< Specifies whether nonzero curvature tests shall be used. */
-	int enableDriftCorrection;				/**< Specifies the frequency of drift corrections (0 = off). */
-	int enableCholeskyRefactorisation;		/**< Specifies the frequency of full refactorisation of proj. Hessian (otherwise updates). */
-	BooleanType enableEqualities;			/**< Specifies whether equalities shall be always treated as active constraints. */
-
-	real_t terminationTolerance;			/**< Termination tolerance. */
-	real_t boundTolerance;					/**< Lower/upper (constraints') bound tolerance (an inequality constraint whose lower and upper bounds differ by less is regarded to be an equality constraint). */
-	real_t boundRelaxation;					/**< Offset for relaxing (constraints') bounds at beginning of an initial homotopy. It is also as initial value for far bounds. */
-	real_t epsNum;							/**< Numerator tolerance for ratio tests. */
-	real_t epsDen;							/**< Denominator tolerance for ratio tests. */
-	real_t maxPrimalJump;					/**< Maximum allowed jump in primal variables in nonzero curvature tests. */
-	real_t maxDualJump;						/**< Maximum allowed jump in dual variables in linear independence tests. */
-
-	real_t initialRamping;					/**< Start value for Ramping Strategy. */
-	real_t finalRamping;					/**< Final value for Ramping Strategy. */
-	real_t initialFarBounds;				/**< Initial size of Far Bounds. */
-	real_t growFarBounds;					/**< Factor to grow Far Bounds. */
-	SubjectToStatus initialStatusBounds;	/**< Initial status of bounds at first iteration. */
-	real_t epsFlipping;						/**< Tolerance of squared Cholesky diagonal factor which triggers flipping bound. */
-	int numRegularisationSteps;				/**< Maximum number of successive regularisation steps. */
-	real_t epsRegularisation;				/**< Scaling factor of identity matrix used for Hessian regularisation. */
-	int numRefinementSteps;					/**< Maximum number of iterative refinement steps. */
-	real_t epsIterRef;						/**< Early termination tolerance for iterative refinement. */
-	real_t epsLITests;						/**< Tolerance for linear independence tests. */
-	real_t epsNZCTests;						/**< Tolerance for nonzero curvature tests. */
-
-	BooleanType enableDropInfeasibles;		/**< ... */
-	int    dropBoundPriority;				/**< ... */
-	int    dropEqConPriority;				/**< ... */
-	int    dropIneqConPriority;				/**< ... */
-} Options;
-
-
-void OptionsCON(	Options* _THIS
-					);
-
-/** Copies all members from given rhs object.
- *  \return SUCCESSFUL_RETURN */
-void OptionsCPY(	Options* FROM,
-					Options* TO
-					);
-
-
-/** Sets all options to default values.
- *	\return SUCCESSFUL_RETURN */
-returnValue Options_setToDefault(	Options* _THIS
-									);
-
-/** Sets all options to values resulting in maximum reliabilty.
- *	\return SUCCESSFUL_RETURN */
-returnValue Options_setToReliable(	Options* _THIS
-									);
-
-/** Sets all options to values resulting in minimum solution time.
- *	\return SUCCESSFUL_RETURN */
-returnValue Options_setToMPC(		Options* _THIS
-									);
-
-/** Same as setToMPC( ), for ensuring backwards compatibility.
- *	\return SUCCESSFUL_RETURN */
-returnValue Options_setToFast(		Options* _THIS
-									);
-
-
-/** Ensures that all options have consistent values by automatically
- *	adjusting inconsistent ones.
- *	Note: This routine cannot (and does not try to) ensure that values
- *	      are set to reasonable values that make the QP solution work!
- *	\return SUCCESSFUL_RETURN \n 
- *		    RET_OPTIONS_ADJUSTED */
-returnValue Options_ensureConsistency(	Options* _THIS
-										);
-
-
-/** Prints values of all options.
- *	\return SUCCESSFUL_RETURN */
-returnValue Options_print(	Options* _THIS
-							);
-
-
-END_NAMESPACE_QPOASES
-
-
-#endif	/* QPOASES_OPTIONS_H */
-
-
-/*
- *	end of file
- */
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/Options.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ *
+ *	Declaration of the Options class designed to manage user-specified
+ *	options for solving a QProblem.
+ */
+
+
+#ifndef QPOASES_OPTIONS_H
+#define QPOASES_OPTIONS_H
+
+
+#include <qpOASES_e/Utils.h>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/**
+ *	\brief Manages all user-specified options for solving QPs.
+ *
+ *	This class manages all user-specified options used for solving
+ *	quadratic programs.
+ *
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ */
+typedef struct
+{
+	PrintLevel printLevel;					/**< Print level. */
+
+	BooleanType enableRamping;				/**< Specifies whether ramping shall be enabled or not. */
+	BooleanType enableFarBounds;			/**< Specifies whether far bounds shall be used or not. */
+	BooleanType enableFlippingBounds;		/**< Specifies whether flipping bounds shall be used or not. */
+	BooleanType enableRegularisation;		/**< Specifies whether Hessian matrix shall be regularised in case semi-definiteness is detected. */
+	BooleanType enableFullLITests;			/**< Specifies whether condition-hardened LI test shall be used or not. */
+	BooleanType enableNZCTests;				/**< Specifies whether nonzero curvature tests shall be used. */
+	int enableDriftCorrection;				/**< Specifies the frequency of drift corrections (0 = off). */
+	int enableCholeskyRefactorisation;		/**< Specifies the frequency of full refactorisation of proj. Hessian (otherwise updates). */
+	BooleanType enableEqualities;			/**< Specifies whether equalities shall be always treated as active constraints. */
+
+	real_t terminationTolerance;			/**< Termination tolerance. */
+	real_t boundTolerance;					/**< Lower/upper (constraints') bound tolerance (an inequality constraint whose lower and upper bounds differ by less is regarded to be an equality constraint). */
+	real_t boundRelaxation;					/**< Offset for relaxing (constraints') bounds at beginning of an initial homotopy. It is also as initial value for far bounds. */
+	real_t epsNum;							/**< Numerator tolerance for ratio tests. */
+	real_t epsDen;							/**< Denominator tolerance for ratio tests. */
+	real_t maxPrimalJump;					/**< Maximum allowed jump in primal variables in nonzero curvature tests. */
+	real_t maxDualJump;						/**< Maximum allowed jump in dual variables in linear independence tests. */
+
+	real_t initialRamping;					/**< Start value for Ramping Strategy. */
+	real_t finalRamping;					/**< Final value for Ramping Strategy. */
+	real_t initialFarBounds;				/**< Initial size of Far Bounds. */
+	real_t growFarBounds;					/**< Factor to grow Far Bounds. */
+	SubjectToStatus initialStatusBounds;	/**< Initial status of bounds at first iteration. */
+	real_t epsFlipping;						/**< Tolerance of squared Cholesky diagonal factor which triggers flipping bound. */
+	int numRegularisationSteps;				/**< Maximum number of successive regularisation steps. */
+	real_t epsRegularisation;				/**< Scaling factor of identity matrix used for Hessian regularisation. */
+	int numRefinementSteps;					/**< Maximum number of iterative refinement steps. */
+	real_t epsIterRef;						/**< Early termination tolerance for iterative refinement. */
+	real_t epsLITests;						/**< Tolerance for linear independence tests. */
+	real_t epsNZCTests;						/**< Tolerance for nonzero curvature tests. */
+
+	BooleanType enableDropInfeasibles;		/**< ... */
+	int    dropBoundPriority;				/**< ... */
+	int    dropEqConPriority;				/**< ... */
+	int    dropIneqConPriority;				/**< ... */
+} Options;
+
+
+void OptionsCON(	Options* _THIS
+					);
+
+/** Copies all members from given rhs object.
+ *  \return SUCCESSFUL_RETURN */
+void OptionsCPY(	Options* FROM,
+					Options* TO
+					);
+
+
+/** Sets all options to default values.
+ *	\return SUCCESSFUL_RETURN */
+returnValue Options_setToDefault(	Options* _THIS
+									);
+
+/** Sets all options to values resulting in maximum reliabilty.
+ *	\return SUCCESSFUL_RETURN */
+returnValue Options_setToReliable(	Options* _THIS
+									);
+
+/** Sets all options to values resulting in minimum solution time.
+ *	\return SUCCESSFUL_RETURN */
+returnValue Options_setToMPC(		Options* _THIS
+									);
+
+/** Same as setToMPC( ), for ensuring backwards compatibility.
+ *	\return SUCCESSFUL_RETURN */
+returnValue Options_setToFast(		Options* _THIS
+									);
+
+
+/** Ensures that all options have consistent values by automatically
+ *	adjusting inconsistent ones.
+ *	Note: This routine cannot (and does not try to) ensure that values
+ *	      are set to reasonable values that make the QP solution work!
+ *	\return SUCCESSFUL_RETURN \n 
+ *		    RET_OPTIONS_ADJUSTED */
+returnValue Options_ensureConsistency(	Options* _THIS
+										);
+
+
+/** Prints values of all options.
+ *	\return SUCCESSFUL_RETURN */
+returnValue Options_print(	Options* _THIS
+							);
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_OPTIONS_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/include/qpOASES_e/QProblem.h b/third_party/acados/include/qpOASES_e/QProblem.h
index 3c61a4d596..91a4a6f396 100644
--- a/third_party/acados/include/qpOASES_e/QProblem.h
+++ b/third_party/acados/include/qpOASES_e/QProblem.h
@@ -1,2369 +1,2369 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *	See the GNU Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES_e/QProblem.h
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.1embedded
- *	\date 2007-2015
- *
- *	Declaration of the QProblem class which is able to use the newly
- *	developed online active set strategy for parametric quadratic programming.
- */
-
-
-
-#ifndef QPOASES_QPROBLEM_H
-#define QPOASES_QPROBLEM_H
-
-
-#include <qpOASES_e/Bounds.h>
-#include <qpOASES_e/Options.h>
-#include <qpOASES_e/Matrices.h>
-#include <qpOASES_e/Constraints.h>
-#include <qpOASES_e/Flipper.h>
-#include <qpOASES_e/ConstraintProduct.h>
-
-
-BEGIN_NAMESPACE_QPOASES
-
-typedef struct {
-	Bounds *auxiliaryBounds;
-	Constraints *auxiliaryConstraints;
-
-	real_t *ub_new_far;
-	real_t *lb_new_far;
-	real_t *ubA_new_far;
-	real_t *lbA_new_far;
-
-	real_t *g_new;
-	real_t *lb_new;
-	real_t *ub_new;
-	real_t *lbA_new;
-	real_t *ubA_new;
-
-	real_t *g_new2;
-	real_t *lb_new2;
-	real_t *ub_new2;
-	real_t *lbA_new2;
-	real_t *ubA_new2;
-
-	real_t *delta_xFX5;
-	real_t *delta_xFR5;
-	real_t *delta_yAC5;
-	real_t *delta_yFX5;
-
-	real_t *Hx;
-
-	real_t *_H;
-
-	real_t *g_original;
-	real_t *lb_original;
-	real_t *ub_original;
-	real_t *lbA_original;
-	real_t *ubA_original;
-
-	real_t *delta_xFR;
-	real_t *delta_xFX;
-	real_t *delta_yAC;
-	real_t *delta_yFX;
-	real_t *delta_g;
-	real_t *delta_lb;
-	real_t *delta_ub;
-	real_t *delta_lbA;
-	real_t *delta_ubA;
-
-	real_t *gMod;
-
-	real_t *aFR;
-	real_t *wZ;
-
-	real_t *delta_g2;
-	real_t *delta_xFX2;
-	real_t *delta_xFR2;
-	real_t *delta_yAC2;
-	real_t *delta_yFX2;
-	real_t *nul;
-	real_t *Arow;
-
-	real_t *xiC;
-	real_t *xiC_TMP;
-	real_t *xiB;
-	real_t *Arow2;
-	real_t *num;
-
-	real_t *w;
-	real_t *tmp;
-
-	real_t *delta_g3;
-	real_t *delta_xFX3;
-	real_t *delta_xFR3;
-	real_t *delta_yAC3;
-	real_t *delta_yFX3;
-	real_t *nul2;
-
-	real_t *xiC2;
-	real_t *xiC_TMP2;
-	real_t *xiB2;
-	real_t *num2;
-
-	real_t *Hz;
-	real_t *z;
-	real_t *ZHz;
-	real_t *r;
-
-	real_t *tmp2;
-	real_t *Hz2;
-	real_t *z2;
-	real_t *r2;
-	real_t *rhs;
-
-	real_t *delta_xFX4;
-	real_t *delta_xFR4;
-	real_t *delta_yAC4;
-	real_t *delta_yFX4;
-	real_t *nul3;
-	real_t *ek;
-	real_t *x_W;
-	real_t *As;
-	real_t *Ax_W;
-
-	real_t *num3;
-	real_t *den;
-	real_t *delta_Ax_l;
-	real_t *delta_Ax_u;
-	real_t *delta_Ax;
-	real_t *delta_x;
-
-	real_t *_A;
-
-	real_t *grad;
-	real_t *AX;
-} QProblem_ws;
-
-int QProblem_ws_calculateMemorySize( unsigned int nV, unsigned int nC );
-
-char *QProblem_ws_assignMemory( unsigned int nV, unsigned int nC, QProblem_ws **mem, void *raw_memory );
-
-QProblem_ws *QProblem_ws_createMemory( unsigned int nV, unsigned int nC );
-
-/**
- *	\brief Implements the online active set strategy for QPs with general constraints.
- *
- *	A class for setting up and solving quadratic programs. The main feature is
- *	the possibily to use the newly developed online active set strategy for
- * 	parametric quadratic programming.
- *
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.1embedded
- *	\date 2007-2015
- */
-typedef struct
-{
-	QProblem_ws *ws;				/**< Workspace */
-	Bounds *bounds;					/**< Data structure for problem's bounds. */
-	Constraints *constraints;		/**< Data structure for problem's constraints. */
-	Flipper *flipper;				/**< Struct for making a temporary copy of the matrix factorisations. */
-
-	DenseMatrix* H;					/**< Hessian matrix pointer. */
-	DenseMatrix* A;					/**< Constraint matrix pointer. */
-
-	Options options;				/**< Struct containing all user-defined options for solving QPs. */
-	TabularOutput tabularOutput;	/**< Struct storing information for tabular output (printLevel == PL_TABULAR). */
-
-	real_t *g;						/**< Gradient. */
-
-	real_t *lb;						/**< Lower bound vector (on variables). */
-	real_t *ub;						/**< Upper bound vector (on variables). */
-	real_t *lbA;					/**< Lower constraints' bound vector. */
-	real_t *ubA;					/**< Upper constraints' bound vector. */
-
-	real_t *R;						/**< Cholesky factor of H (i.e. H = R^T*R). */
-
-	real_t *T;						/**< Reverse triangular matrix, A = [0 T]*Q'. */
-	real_t *Q;						/**< Orthonormal quadratic matrix, A = [0 T]*Q'. */
-
-	real_t *Ax;						/**< Stores the current A*x \n
-									 *	 (for increased efficiency only). */
-	real_t *Ax_l;					/**< Stores the current distance to lower constraints' bounds A*x-lbA \n
-									 *	 (for increased efficiency only). */
-	real_t *Ax_u;					/**< Stores the current distance to lower constraints' bounds ubA-A*x \n
-									 *	 (for increased efficiency only). */
-
-	real_t *x;						/**< Primal solution vector. */
-	real_t *y;						/**< Dual solution vector. */
-
-	real_t *delta_xFR_TMP;			/**< Temporary for determineStepDirection */
-	real_t *tempA;					/**< Temporary for determineStepDirection. */
-	real_t *tempB;					/**< Temporary for determineStepDirection. */
-	real_t *ZFR_delta_xFRz;			/**< Temporary for determineStepDirection. */
-	real_t *delta_xFRy;				/**< Temporary for determineStepDirection. */
-	real_t *delta_xFRz;				/**< Temporary for determineStepDirection. */
-	real_t *delta_yAC_TMP;			/**< Temporary for determineStepDirection. */
-
-	ConstraintProduct constraintProduct;	/**< Pointer to user-defined constraint product function. */
-
-	real_t tau;						/**< Last homotopy step length. */
-	real_t regVal;					/**< Holds the offset used to regularise Hessian matrix (zero by default). */
-
-	real_t ramp0;					/**< Start value for Ramping Strategy. */
-	real_t ramp1;					/**< Final value for Ramping Strategy. */
-
-	QProblemStatus status;			/**< Current status of the solution process. */
-	HessianType hessianType;		/**< Type of Hessian matrix. */
-
-	BooleanType haveCholesky;		/**< Flag indicating whether Cholesky decomposition has already been setup. */
-	BooleanType infeasible;			/**< QP infeasible? */
-	BooleanType unbounded;			/**< QP unbounded? */
-
-	int rampOffset;					/**< Offset index for Ramping. */
-	unsigned int count;				/**< Counts the number of hotstart function calls (internal usage only!). */
-
-	int sizeT;						/**< Matrix T is stored in a (sizeT x sizeT) array. */
-} QProblem;
-
-int QProblem_calculateMemorySize( unsigned int nV, unsigned int nC );
-
-char *QProblem_assignMemory( unsigned int nV, unsigned int nC, QProblem **mem, void *raw_memory );
-
-QProblem *QProblem_createMemory( unsigned int nV, unsigned int nC );
-
-
-/** Constructor which takes the QP dimension and Hessian type
- *  information. If the Hessian is the zero (i.e. HST_ZERO) or the
- *  identity matrix (i.e. HST_IDENTITY), respectively, no memory
- *  is allocated for it and a NULL pointer can be passed for it
- *  to the init() functions. */
-void QProblemCON(	QProblem* _THIS,
-					int _nV,	  					/**< Number of variables. */
-					int _nC,		  				/**< Number of constraints. */
-					HessianType _hessianType 		/**< Type of Hessian matrix. */
-					);
-
-/** Copies all members from given rhs object.
- *  \return SUCCESSFUL_RETURN */
-void QProblemCPY(	QProblem* FROM,
-					QProblem* TO
-					);
-
-
-/** Clears all data structures of QProblem except for QP data.
- *	\return SUCCESSFUL_RETURN \n
-			RET_RESET_FAILED */
-returnValue QProblem_reset( QProblem* _THIS );
-
-
-/** Initialises a QP problem with given QP data and tries to solve it
- *	using at most nWSR iterations.
- *
- *  Note: This function internally calls solveInitialQP for initialisation!
- *
- *	\return SUCCESSFUL_RETURN \n
-			RET_INIT_FAILED \n
-			RET_INIT_FAILED_CHOLESKY \n
-			RET_INIT_FAILED_TQ \n
-			RET_INIT_FAILED_HOTSTART \n
-			RET_INIT_FAILED_INFEASIBILITY \n
-			RET_INIT_FAILED_UNBOUNDEDNESS \n
-			RET_MAX_NWSR_REACHED \n
-			RET_INVALID_ARGUMENTS */
-returnValue QProblem_initM(	QProblem* _THIS,
-							DenseMatrix *_H,			/**< Hessian matrix. */
-							const real_t* const _g, 	/**< Gradient vector. */
-							DenseMatrix *_A,  			/**< Constraint matrix. */
-							const real_t* const _lb,	/**< Lower bound vector (on variables). \n
-															 If no lower bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ub,	/**< Upper bound vector (on variables). \n
-															 If no upper bounds exist, a NULL pointer can be passed. */
-							const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
-															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ubA,	/**< Upper constraints' bound vector. \n
-															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							int* nWSR,					/**< Input: Maximum number of working set recalculations when using initial homotopy.
-															 Output: Number of performed working set recalculations. */
-							real_t* const cputime 		/**< Input: Maximum CPU time allowed for QP initialisation. \n
-															 Output: CPU time spent for QP initialisation (if pointer passed). */
-							);
-
-
-/** Initialises a QP problem with given QP data and tries to solve it
- *	using at most nWSR iterations.
- *
- *  Note: This function internally calls solveInitialQP for initialisation!
- *
- *	\return SUCCESSFUL_RETURN \n
-			RET_INIT_FAILED \n
-			RET_INIT_FAILED_CHOLESKY \n
-			RET_INIT_FAILED_TQ \n
-			RET_INIT_FAILED_HOTSTART \n
-			RET_INIT_FAILED_INFEASIBILITY \n
-			RET_INIT_FAILED_UNBOUNDEDNESS \n
-			RET_MAX_NWSR_REACHED \n
-			RET_INVALID_ARGUMENTS */
-returnValue QProblem_init(	QProblem* _THIS,
-							real_t* const _H, 			/**< Hessian matrix. \n
-															 If Hessian matrix is trivial, a NULL pointer can be passed. */
-							const real_t* const _g, 	/**< Gradient vector. */
-							real_t* const _A,  			/**< Constraint matrix. */
-							const real_t* const _lb,	/**< Lower bound vector (on variables). \n
-															 If no lower bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ub,	/**< Upper bound vector (on variables). \n
-															 If no upper bounds exist, a NULL pointer can be passed. */
-							const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
-															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ubA,	/**< Upper constraints' bound vector. \n
-															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							int* nWSR,					/**< Input: Maximum number of working set recalculations when using initial homotopy.
-															 Output: Number of performed working set recalculations. */
-							real_t* const cputime 		/**< Input: Maximum CPU time allowed for QP initialisation. \n
-															 Output: CPU time spent for QP initialisation (if pointer passed). */
-							);
-
-/** Initialises a QP problem with given QP data to be read from files and tries to solve it
- *	using at most nWSR iterations.
- *
- *  Note: This function internally calls solveInitialQP for initialisation!
- *
- *	\return SUCCESSFUL_RETURN \n
-			RET_INIT_FAILED \n
-			RET_INIT_FAILED_CHOLESKY \n
-			RET_INIT_FAILED_TQ \n
-			RET_INIT_FAILED_HOTSTART \n
-			RET_INIT_FAILED_INFEASIBILITY \n
-			RET_INIT_FAILED_UNBOUNDEDNESS \n
-			RET_MAX_NWSR_REACHED \n
-			RET_UNABLE_TO_READ_FILE */
-returnValue QProblem_initF(	QProblem* _THIS,
-							const char* const H_file,	/**< Name of file where Hessian matrix is stored. \n
-															 If Hessian matrix is trivial, a NULL pointer can be passed. */
-							const char* const g_file,	/**< Name of file where gradient vector is stored. */
-							const char* const A_file,	/**< Name of file where constraint matrix is stored. */
-							const char* const lb_file,	/**< Name of file where lower bound vector. \n
-															If no lower bounds exist, a NULL pointer can be passed. */
-							const char* const ub_file,	/**< Name of file where upper bound vector. \n
-															If no upper bounds exist, a NULL pointer can be passed. */
-							const char* const lbA_file,	/**< Name of file where lower constraints' bound vector. \n
-															If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							const char* const ubA_file,	/**< Name of file where upper constraints' bound vector. \n
-															 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-							int* nWSR,					/**< Input: Maximum number of working set recalculations when using initial homotopy.
-															 Output: Number of performed working set recalculations. */
-							real_t* const cputime	 	/**< Input: Maximum CPU time allowed for QP initialisation. \n
-															 Output: CPU time spent for QP initialisation (if pointer passed). */
-							);
-
-/** Initialises a QP problem with given QP data and tries to solve it
- *	using at most nWSR iterations. Depending on the parameter constellation it: \n
- *	1. 0,    0,    0    : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n
- *	2. xOpt, 0,    0    : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n
- *	3. 0,    yOpt, 0    : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n
- *	4. 0,    0,    gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n
- *	5. xOpt, yOpt, 0    : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n
- *	6. xOpt, 0,    gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n
- *	7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!)
- *
- *  Note: This function internally calls solveInitialQP for initialisation!
- *
- *	\return SUCCESSFUL_RETURN \n
-			RET_INIT_FAILED \n
-			RET_INIT_FAILED_CHOLESKY \n
-			RET_INIT_FAILED_TQ \n
-			RET_INIT_FAILED_HOTSTART \n
-			RET_INIT_FAILED_INFEASIBILITY \n
-			RET_INIT_FAILED_UNBOUNDEDNESS \n
-			RET_MAX_NWSR_REACHED \n
-			RET_INVALID_ARGUMENTS */
-returnValue QProblem_initMW(	QProblem* _THIS,
-								DenseMatrix *_H, 						/**< Hessian matrix. \n
-														    				 If Hessian matrix is trivial, a NULL pointer can be passed. */
-								const real_t* const _g, 				/**< Gradient vector. */
-								DenseMatrix *_A, 			 			/**< Constraint matrix. */
-								const real_t* const _lb,				/**< Lower bound vector (on variables). \n
-																			 If no lower bounds exist, a NULL pointer can be passed. */
-								const real_t* const _ub,				/**< Upper bound vector (on variables). \n
-																			 If no upper bounds exist, a NULL pointer can be passed. */
-								const real_t* const _lbA,				/**< Lower constraints' bound vector. \n
-																			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const real_t* const _ubA,				/**< Upper constraints' bound vector. \n
-																			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								int* nWSR,								/**< Input: Maximum number of working set recalculations when using initial homotopy.
-																		 *	 Output: Number of performed working set recalculations. */
-								real_t* const cputime,					/**< Input: Maximum CPU time allowed for QP initialisation. \n
-																			 Output: CPU time spent for QP initialisation. */
-								const real_t* const xOpt,				/**< Optimal primal solution vector. \n
-																			 (If a null pointer is passed, the old primal solution is kept!) */
-								const real_t* const yOpt,				/**< Optimal dual solution vector. \n
-																			 (If a null pointer is passed, the old dual solution is kept!) */
-								Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). */
-								Constraints* const guessedConstraints,	/**< Optimal working set of constraints for solution (xOpt,yOpt). */
-								const real_t* const _R					/**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix.
-																	 		 The Cholesky factor must be stored in a real_t array of size nV*nV
-																			 in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n
-																			 (If a null pointer is passed, Cholesky decomposition is computed internally!) */
-								);
-
-/** Initialises a QP problem with given QP data and tries to solve it
- *	using at most nWSR iterations. Depending on the parameter constellation it: \n
- *	1. 0,    0,    0    : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n
- *	2. xOpt, 0,    0    : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n
- *	3. 0,    yOpt, 0    : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n
- *	4. 0,    0,    gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n
- *	5. xOpt, yOpt, 0    : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n
- *	6. xOpt, 0,    gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n
- *	7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!)
- *
- *  Note: This function internally calls solveInitialQP for initialisation!
- *
- *	\return SUCCESSFUL_RETURN \n
-			RET_INIT_FAILED \n
-			RET_INIT_FAILED_CHOLESKY \n
-			RET_INIT_FAILED_TQ \n
-			RET_INIT_FAILED_HOTSTART \n
-			RET_INIT_FAILED_INFEASIBILITY \n
-			RET_INIT_FAILED_UNBOUNDEDNESS \n
-			RET_MAX_NWSR_REACHED \n
-			RET_INVALID_ARGUMENTS */
-returnValue QProblem_initW(	QProblem* _THIS,
-							real_t* const _H, 						/**< Hessian matrix. \n
-														    			 If Hessian matrix is trivial, a NULL pointer can be passed. */
-							const real_t* const _g, 				/**< Gradient vector. */
-							real_t* const _A,  						/**< Constraint matrix. */
-							const real_t* const _lb,				/**< Lower bound vector (on variables). \n
-																		 If no lower bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ub,				/**< Upper bound vector (on variables). \n
-																		 If no upper bounds exist, a NULL pointer can be passed. */
-							const real_t* const _lbA,				/**< Lower constraints' bound vector. \n
-																		 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ubA,				/**< Upper constraints' bound vector. \n
-																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							int* nWSR,								/**< Input:  Maximum number of working set recalculations when using initial homotopy.
-																	 *	 Output: Number of performed working set recalculations. */
-							real_t* const cputime,					/**< Input:  Maximum CPU time allowed for QP initialisation. \n
-																		 Output: CPU time spent for QP initialisation. */
-							const real_t* const xOpt,				/**< Optimal primal solution vector. \n
-																		 (If a null pointer is passed, the old primal solution is kept!) */
-							const real_t* const yOpt,				/**< Optimal dual solution vector. \n
-																		 (If a null pointer is passed, the old dual solution is kept!) */
-							Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). */
-							Constraints* const guessedConstraints,	/**< Optimal working set of constraints for solution (xOpt,yOpt). */
-							const real_t* const _R					/**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix.
-																	 	 The Cholesky factor must be stored in a real_t array of size nV*nV
-																		 in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n
-																		 (If a null pointer is passed, Cholesky decomposition is computed internally!) */
-							);
-
-/** Initialises a QP problem with given QP data to be ream from files and tries to solve it
- *	using at most nWSR iterations. Depending on the parameter constellation it: \n
- *	1. 0,    0,    0    : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n
- *	2. xOpt, 0,    0    : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n
- *	3. 0,    yOpt, 0    : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n
- *	4. 0,    0,    gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n
- *	5. xOpt, yOpt, 0    : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n
- *	6. xOpt, 0,    gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n
- *	7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!)
- *
- *  Note: This function internally calls solveInitialQP for initialisation!
- *
- *	\return SUCCESSFUL_RETURN \n
-			RET_INIT_FAILED \n
-			RET_INIT_FAILED_CHOLESKY \n
-			RET_INIT_FAILED_TQ \n
-			RET_INIT_FAILED_HOTSTART \n
-			RET_INIT_FAILED_INFEASIBILITY \n
-			RET_INIT_FAILED_UNBOUNDEDNESS \n
-			RET_MAX_NWSR_REACHED \n
-			RET_UNABLE_TO_READ_FILE */
-returnValue QProblem_initFW(	QProblem* _THIS,
-								const char* const H_file,				/**< Name of file where Hessian matrix is stored. \n
-														     				 If Hessian matrix is trivial, a NULL pointer can be passed. */
-								const char* const g_file,				/**< Name of file where gradient vector is stored. */
-								const char* const A_file,				/**< Name of file where constraint matrix is stored. */
-								const char* const lb_file,				/**< Name of file where lower bound vector. \n
-																			 If no lower bounds exist, a NULL pointer can be passed. */
-								const char* const ub_file,				/**< Name of file where upper bound vector. \n
-																			 If no upper bounds exist, a NULL pointer can be passed. */
-								const char* const lbA_file,				/**< Name of file where lower constraints' bound vector. \n
-																			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const char* const ubA_file,				/**< Name of file where upper constraints' bound vector. \n
-																			 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-								int* nWSR,								/**< Input:  Maximum number of working set recalculations when using initial homotopy.
-																			 Output: Number of performed working set recalculations. */
-								real_t* const cputime,					/**< Input:  Maximum CPU time allowed for QP initialisation. \n
-																			 Output: CPU time spent for QP initialisation. */
-								const real_t* const xOpt,				/**< Optimal primal solution vector. \n
-																			 (If a null pointer is passed, the old primal solution is kept!) */
-								const real_t* const yOpt,				/**< Optimal dual solution vector. \n
-																			 (If a null pointer is passed, the old dual solution is kept!) */
-								Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). */
-								Constraints* const guessedConstraints,	/**< Optimal working set of constraints for solution (xOpt,yOpt). */
-								const char* const R_file				/**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix.
-																		 	 The Cholesky factor must be stored in a real_t array of size nV*nV
-																			 in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n
-																			 (If a null pointer is passed, Cholesky decomposition is computed internally!) */
-								);
-
-/** Solves an initialised QP sequence using the online active set strategy.
- *	QP solution is started from previous solution.
- *
- *  Note: This function internally calls solveQP/solveRegularisedQP
- *        for solving an initialised QP!
- *
- *	\return SUCCESSFUL_RETURN \n
- 			RET_MAX_NWSR_REACHED \n
- 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-			RET_HOTSTART_FAILED \n
-			RET_SHIFT_DETERMINATION_FAILED \n
-			RET_STEPDIRECTION_DETERMINATION_FAILED \n
-			RET_STEPLENGTH_DETERMINATION_FAILED \n
-			RET_HOMOTOPY_STEP_FAILED \n
-			RET_HOTSTART_STOPPED_INFEASIBILITY \n
-			RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
-returnValue QProblem_hotstart(	QProblem* _THIS,
-								const real_t* const g_new,		/**< Gradient of neighbouring QP to be solved. */
-								const real_t* const lb_new,		/**< Lower bounds of neighbouring QP to be solved. \n
-											 			 			 If no lower bounds exist, a NULL pointer can be passed. */
-								const real_t* const ub_new,		/**< Upper bounds of neighbouring QP to be solved. \n
-											 			 			 If no upper bounds exist, a NULL pointer can be passed. */
-								const real_t* const lbA_new,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
-											 			 			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const real_t* const ubA_new,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n
-											 			 			 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-								int* nWSR,						/**< Input:  Maximum number of working set recalculations; \n
-													 				 Output: Number of performed working set recalculations. */
-								real_t* const cputime 			/**< Input:  Maximum CPU time allowed for QP solution. \n
-														 			 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
-								);
-
-/** Solves an initialised QP sequence using the online active set strategy,
- *	where QP data is read from files. QP solution is started from previous solution.
- *
- *  Note: This function internally calls solveQP/solveRegularisedQP
- *        for solving an initialised QP!
- *
- *	\return SUCCESSFUL_RETURN \n
- 			RET_MAX_NWSR_REACHED \n
- 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-			RET_HOTSTART_FAILED \n
-			RET_SHIFT_DETERMINATION_FAILED \n
-			RET_STEPDIRECTION_DETERMINATION_FAILED \n
-			RET_STEPLENGTH_DETERMINATION_FAILED \n
-			RET_HOMOTOPY_STEP_FAILED \n
-			RET_HOTSTART_STOPPED_INFEASIBILITY \n
-			RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
-			RET_UNABLE_TO_READ_FILE \n
-			RET_INVALID_ARGUMENTS */
-returnValue QProblem_hotstartF(	QProblem* _THIS,
-								const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
-								const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
-											 					 If no lower bounds exist, a NULL pointer can be passed. */
-								const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
-											 					 If no upper bounds exist, a NULL pointer can be passed. */
-								const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
-											 					 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
-											 					 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-								int* nWSR, 					/**< Input:  Maximum number of working set recalculations; \n
-																 Output: Number of performed working set recalculations. */
-								real_t* const cputime 		/**< Input:  Maximum CPU time allowed for QP solution. \n
-														 		 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
-								);
-
-/** Solves an initialised QP sequence using the online active set strategy.
- *	By default, QP solution is started from previous solution. If a guess
- *	for the working set is provided, an initialised homotopy is performed.
- *
- *  Note: This function internally calls solveQP/solveRegularisedQP
- *        for solving an initialised QP!
- *
- *	\return SUCCESSFUL_RETURN \n
- 			RET_MAX_NWSR_REACHED \n
- 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-			RET_HOTSTART_FAILED \n
-			RET_SHIFT_DETERMINATION_FAILED \n
-			RET_STEPDIRECTION_DETERMINATION_FAILED \n
-			RET_STEPLENGTH_DETERMINATION_FAILED \n
-			RET_HOMOTOPY_STEP_FAILED \n
-			RET_HOTSTART_STOPPED_INFEASIBILITY \n
-			RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
-			RET_SETUP_AUXILIARYQP_FAILED */
-returnValue QProblem_hotstartW(	QProblem* _THIS,
-								const real_t* const g_new,				/**< Gradient of neighbouring QP to be solved. */
-								const real_t* const lb_new,				/**< Lower bounds of neighbouring QP to be solved. \n
-											 						 		 If no lower bounds exist, a NULL pointer can be passed. */
-								const real_t* const ub_new,				/**< Upper bounds of neighbouring QP to be solved. \n
-											 						 		 If no upper bounds exist, a NULL pointer can be passed. */
-								const real_t* const lbA_new,			/**< Lower constraints' bounds of neighbouring QP to be solved. \n
-											 						 		 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const real_t* const ubA_new,			/**< Upper constraints' bounds of neighbouring QP to be solved. \n
-											 								 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-								int* nWSR,								/**< Input: Maximum number of working set recalculations; \n
-																			 Output: Number of performed working set recalculations. */
-								real_t* const cputime,					/**< Input: Maximum CPU time allowed for QP solution. \n
-																	 		 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
-								Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). \n
-																			 (If a null pointer is passed, the previous working set of bounds is kept!) */
-								Constraints* const guessedConstraints	/**< Optimal working set of constraints for solution (xOpt,yOpt). \n
-																			 (If a null pointer is passed, the previous working set of constraints is kept!) */
-								);
-
-/** Solves an initialised QP sequence using the online active set strategy,
- *  where QP data is read from files.
- *	By default, QP solution is started from previous solution. If a guess
- *	for the working set is provided, an initialised homotopy is performed.
- *
- *  Note: This function internally calls solveQP/solveRegularisedQP
- *        for solving an initialised QP!
- *
- *	\return SUCCESSFUL_RETURN \n
- 			RET_MAX_NWSR_REACHED \n
- 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-			RET_HOTSTART_FAILED \n
-			RET_SHIFT_DETERMINATION_FAILED \n
-			RET_STEPDIRECTION_DETERMINATION_FAILED \n
-			RET_STEPLENGTH_DETERMINATION_FAILED \n
-			RET_HOMOTOPY_STEP_FAILED \n
-			RET_HOTSTART_STOPPED_INFEASIBILITY \n
-			RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
-			RET_SETUP_AUXILIARYQP_FAILED \n
-			RET_UNABLE_TO_READ_FILE \n
-			RET_INVALID_ARGUMENTS */
-returnValue QProblem_hotstartFW(	QProblem* _THIS,
-									const char* const g_file, 				/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
-									const char* const lb_file, 				/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
-											 									 If no lower bounds exist, a NULL pointer can be passed. */
-									const char* const ub_file, 				/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
-											 									 If no upper bounds exist, a NULL pointer can be passed. */
-									const char* const lbA_file, 			/**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
-											 									 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-									const char* const ubA_file, 			/**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
-											 									 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-									int* nWSR,								/**< Input: Maximum number of working set recalculations; \n
-																 				 Output: Number of performed working set recalculations. */
-									real_t* const cputime,					/**< Input: Maximum CPU time allowed for QP solution. \n
-																	 			 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
-									Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). \n
-																				 (If a null pointer is passed, the previous working set of bounds is kept!) */
-									Constraints* const guessedConstraints	/**< Optimal working set of constraints for solution (xOpt,yOpt). \n
-																				 (If a null pointer is passed, the previous working set of constraints is kept!) */
-									);
-
-
-/** Solves using the current working set
- *	\return SUCCESSFUL_RETURN \n
- *			RET_STEPDIRECTION_FAILED_TQ \n
- *			RET_STEPDIRECTION_FAILED_CHOLESKY \n
- *			RET_INVALID_ARGUMENTS */
-returnValue QProblem_solveCurrentEQP (	QProblem* _THIS,
-										const int n_rhs,		/**< Number of consecutive right hand sides */
-										const real_t* g_in,		/**< Gradient of neighbouring QP to be solved. */
-										const real_t* lb_in,	/**< Lower bounds of neighbouring QP to be solved. \n
-																	 If no lower bounds exist, a NULL pointer can be passed. */
-										const real_t* ub_in,	/**< Upper bounds of neighbouring QP to be solved. \n
-																	 If no upper bounds exist, a NULL pointer can be passed. */
-										const real_t* lbA_in,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
-																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-										const real_t* ubA_in,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n */
-										real_t* x_out,			/**< Output: Primal solution */
-										real_t* y_out			/**< Output: Dual solution */
-										);
-
-
-
-/** Returns current constraints object of the QP (deep copy).
-  *	\return SUCCESSFUL_RETURN \n
-  			RET_QPOBJECT_NOT_SETUP */
-static inline returnValue QProblem_getConstraints(	QProblem* _THIS,
-													Constraints* _constraints	/** Output: Constraints object. */
-													);
-
-
-/** Returns the number of constraints.
- *	\return Number of constraints. */
-static inline int QProblem_getNC( QProblem* _THIS );
-
-/** Returns the number of (implicitly defined) equality constraints.
- *	\return Number of (implicitly defined) equality constraints. */
-static inline int QProblem_getNEC( QProblem* _THIS );
-
-/** Returns the number of active constraints.
- *	\return Number of active constraints. */
-static inline int QProblem_getNAC( QProblem* _THIS );
-
-/** Returns the number of inactive constraints.
- *	\return Number of inactive constraints. */
-static inline int QProblem_getNIAC( QProblem* _THIS );
-
-/** Returns the dimension of null space.
- *	\return Dimension of null space. */
-int QProblem_getNZ( QProblem* _THIS );
-
-
-/** Returns the dual solution vector (deep copy).
- *	\return SUCCESSFUL_RETURN \n
-			RET_QP_NOT_SOLVED */
-returnValue QProblem_getDualSolution(	QProblem* _THIS,
-										real_t* const yOpt	/**< Output: Dual solution vector (if QP has been solved). */
-										);
-
-
-/** Defines user-defined routine for calculating the constraint product A*x
- *	\return  SUCCESSFUL_RETURN \n */
-returnValue QProblem_setConstraintProduct(	QProblem* _THIS,
-											ConstraintProduct _constraintProduct
-											);
-
-
-/** Prints concise list of properties of the current QP.
- *	\return  SUCCESSFUL_RETURN \n */
-returnValue QProblem_printProperties( QProblem* _THIS );
-
-
-
-/** Writes a vector with the state of the working set
-*	\return SUCCESSFUL_RETURN */
-returnValue QProblem_getWorkingSet(	QProblem* _THIS,
-									real_t* workingSet	/** Output: array containing state of the working set. */
-									);
-
-/** Writes a vector with the state of the working set of bounds
- *	\return SUCCESSFUL_RETURN \n
- *	        RET_INVALID_ARGUMENTS */
-returnValue QProblem_getWorkingSetBounds(	QProblem* _THIS,
-											real_t* workingSetB	/** Output: array containing state of the working set of bounds. */
-											);
-
-/** Writes a vector with the state of the working set of constraints
- *	\return SUCCESSFUL_RETURN \n
- *	        RET_INVALID_ARGUMENTS */
-returnValue QProblem_getWorkingSetConstraints(	QProblem* _THIS,
-												real_t* workingSetC	/** Output: array containing state of the working set of constraints. */
-												);
-
-
-/** Returns current bounds object of the QP (deep copy).
-  *	\return SUCCESSFUL_RETURN \n
-  			RET_QPOBJECT_NOT_SETUP */
-static inline returnValue QProblem_getBounds(	QProblem* _THIS,
-												Bounds* _bounds		/** Output: Bounds object. */
-												);
-
-
-/** Returns the number of variables.
- *	\return Number of variables. */
-static inline int QProblem_getNV( QProblem* _THIS );
-
-/** Returns the number of free variables.
- *	\return Number of free variables. */
-static inline int QProblem_getNFR( QProblem* _THIS );
-
-/** Returns the number of fixed variables.
- *	\return Number of fixed variables. */
-static inline int QProblem_getNFX( QProblem* _THIS );
-
-/** Returns the number of implicitly fixed variables.
- *	\return Number of implicitly fixed variables. */
-static inline int QProblem_getNFV( QProblem* _THIS );
-
-
-/** Returns the optimal objective function value.
- *	\return finite value: Optimal objective function value (QP was solved) \n
- 			+infinity:	  QP was not yet solved */
-real_t QProblem_getObjVal( QProblem* _THIS );
-
-/** Returns the objective function value at an arbitrary point x.
- *	\return Objective function value at point x */
-real_t QProblem_getObjValX(	QProblem* _THIS,
-							const real_t* const _x	/**< Point at which the objective function shall be evaluated. */
-							);
-
-/** Returns the primal solution vector.
- *	\return SUCCESSFUL_RETURN \n
-			RET_QP_NOT_SOLVED */
-returnValue QProblem_getPrimalSolution(	QProblem* _THIS,
-										real_t* const xOpt	/**< Output: Primal solution vector (if QP has been solved). */
-										);
-
-
-/** Returns status of the solution process.
- *	\return Status of solution process. */
-static inline QProblemStatus QProblem_getStatus( QProblem* _THIS );
-
-
-/** Returns if the QProblem object is initialised.
- *	\return BT_TRUE:  QProblem initialised \n
- 			BT_FALSE: QProblem not initialised */
-static inline BooleanType QProblem_isInitialised( QProblem* _THIS );
-
-/** Returns if the QP has been solved.
- *	\return BT_TRUE:  QProblem solved \n
- 			BT_FALSE: QProblem not solved */
-static inline BooleanType QProblem_isSolved( QProblem* _THIS );
-
-/** Returns if the QP is infeasible.
- *	\return BT_TRUE:  QP infeasible \n
- 			BT_FALSE: QP feasible (or not known to be infeasible!) */
-static inline BooleanType QProblem_isInfeasible( QProblem* _THIS );
-
-/** Returns if the QP is unbounded.
- *	\return BT_TRUE:  QP unbounded \n
- 			BT_FALSE: QP unbounded (or not known to be unbounded!) */
-static inline BooleanType QProblem_isUnbounded( QProblem* _THIS );
-
-
-/** Returns Hessian type flag (type is not determined due to _THIS call!).
- *	\return Hessian type. */
-static inline HessianType QProblem_getHessianType( QProblem* _THIS );
-
-/** Changes the print level.
- *	\return SUCCESSFUL_RETURN */
-static inline returnValue QProblem_setHessianType(	QProblem* _THIS,
-													HessianType _hessianType /**< New Hessian type. */
-													);
-
-/** Returns if the QP has been internally regularised.
- *	\return BT_TRUE:  Hessian is internally regularised for QP solution \n
- 			BT_FALSE: No internal Hessian regularisation is used for QP solution */
-static inline BooleanType QProblem_usingRegularisation( QProblem* _THIS );
-
-/** Returns current options struct.
- *	\return Current options struct. */
-static inline Options QProblem_getOptions( QProblem* _THIS );
-
-/** Overrides current options with given ones.
- *	\return SUCCESSFUL_RETURN */
-static inline returnValue QProblem_setOptions(	QProblem* _THIS,
-												Options _options	/**< New options. */
-												);
-
-/** Returns the print level.
- *	\return Print level. */
-static inline PrintLevel QProblem_getPrintLevel( QProblem* _THIS );
-
-/** Changes the print level.
- *	\return SUCCESSFUL_RETURN */
-returnValue QProblem_setPrintLevel(	QProblem* _THIS,
-									PrintLevel _printlevel	/**< New print level. */
-									);
-
-
-/** Returns the current number of QP problems solved.
- *	\return Number of QP problems solved. */
-static inline unsigned int QProblem_getCount( QProblem* _THIS );
-
-/** Resets QP problem counter (to zero).
- *	\return SUCCESSFUL_RETURN. */
-static inline returnValue QProblem_resetCounter( QProblem* _THIS );
-
-
-/** Prints a list of all options and their current values.
- *	\return  SUCCESSFUL_RETURN \n */
-returnValue QProblem_printOptions( QProblem* _THIS );
-
-
-/** Solves a QProblem whose QP data is assumed to be stored in the member variables.
- *  A guess for its primal/dual optimal solution vectors and the corresponding
- *  working sets of bounds and constraints can be provided.
- *  Note: This function is internally called by all init functions!
- *	\return SUCCESSFUL_RETURN \n
-			RET_INIT_FAILED \n
-			RET_INIT_FAILED_CHOLESKY \n
-			RET_INIT_FAILED_TQ \n
-			RET_INIT_FAILED_HOTSTART \n
-			RET_INIT_FAILED_INFEASIBILITY \n
-			RET_INIT_FAILED_UNBOUNDEDNESS \n
-			RET_MAX_NWSR_REACHED */
-returnValue QProblem_solveInitialQP(	QProblem* _THIS,
-										const real_t* const xOpt,				/**< Optimal primal solution vector.*/
-										const real_t* const yOpt,				/**< Optimal dual solution vector. */
-										Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). */
-										Constraints* const guessedConstraints,	/**< Optimal working set of constraints for solution (xOpt,yOpt). */
-										const real_t* const _R,					/**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. */
-										int* nWSR, 								/**< Input:  Maximum number of working set recalculations; \n
-														 						 *	 Output: Number of performed working set recalculations. */
-										real_t* const cputime					/**< Input:  Maximum CPU time allowed for QP solution. \n
-																	 			 *	 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
-										);
-
-/** Solves QProblem using online active set strategy.
- *  Note: This function is internally called by all hotstart functions!
- *	\return SUCCESSFUL_RETURN \n
- 			RET_MAX_NWSR_REACHED \n
- 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-			RET_HOTSTART_FAILED \n
-			RET_SHIFT_DETERMINATION_FAILED \n
-			RET_STEPDIRECTION_DETERMINATION_FAILED \n
-			RET_STEPLENGTH_DETERMINATION_FAILED \n
-			RET_HOMOTOPY_STEP_FAILED \n
-			RET_HOTSTART_STOPPED_INFEASIBILITY \n
-			RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
-returnValue QProblem_solveQP(	QProblem* _THIS,
-								const real_t* const g_new,		/**< Gradient of neighbouring QP to be solved. */
-								const real_t* const lb_new,		/**< Lower bounds of neighbouring QP to be solved. \n
-											 			 			 If no lower bounds exist, a NULL pointer can be passed. */
-								const real_t* const ub_new,		/**< Upper bounds of neighbouring QP to be solved. \n
-											 			 			 If no upper bounds exist, a NULL pointer can be passed. */
-								const real_t* const lbA_new,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
-											 			 			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const real_t* const ubA_new,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n
-											 			 			 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-								int* nWSR,						/**< Input: Maximum number of working set recalculations; \n
-													 				 Output: Number of performed working set recalculations. */
-								real_t* const cputime,			/**< Input: Maximum CPU time allowed for QP solution. \n
-														 			 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
-								int  nWSRperformed,				/**< Number of working set recalculations already performed to solve
-																	 this QP within previous solveQP() calls. This number is
-																	 always zero, except for successive calls from solveRegularisedQP()
-																	 or when using the far bound strategy. */
-								BooleanType isFirstCall			/**< Indicating whether this is the first call for current QP. */
-								);
-
-
-/** Solves QProblem using online active set strategy.
- *  Note: This function is internally called by all hotstart functions!
- *	\return SUCCESSFUL_RETURN \n
- 			RET_MAX_NWSR_REACHED \n
- 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-			RET_HOTSTART_FAILED \n
-			RET_SHIFT_DETERMINATION_FAILED \n
-			RET_STEPDIRECTION_DETERMINATION_FAILED \n
-			RET_STEPLENGTH_DETERMINATION_FAILED \n
-			RET_HOMOTOPY_STEP_FAILED \n
-			RET_HOTSTART_STOPPED_INFEASIBILITY \n
-			RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
-returnValue QProblem_solveRegularisedQP(	QProblem* _THIS,
-											const real_t* const g_new,		/**< Gradient of neighbouring QP to be solved. */
-											const real_t* const lb_new,		/**< Lower bounds of neighbouring QP to be solved. \n
-													 			 				 If no lower bounds exist, a NULL pointer can be passed. */
-											const real_t* const ub_new,		/**< Upper bounds of neighbouring QP to be solved. \n
-													 			 				 If no upper bounds exist, a NULL pointer can be passed. */
-											const real_t* const lbA_new,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
-													 			 				 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-											const real_t* const ubA_new,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n
-													 			 				 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-											int* nWSR,						/**< Input: Maximum number of working set recalculations; \n
-															 					 Output: Number of performed working set recalculations. */
-											real_t* const cputime,			/**< Input: Maximum CPU time allowed for QP solution. \n
-																 				 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
-											int  nWSRperformed,				/**< Number of working set recalculations already performed to solve
-																				 this QP within previous solveRegularisedQP() calls. This number is
-																				 always zero, except for successive calls when using the far bound strategy. */
-											BooleanType isFirstCall			/**< Indicating whether this is the first call for current QP. */
-											);
-
-
-/** Determines type of existing constraints and bounds (i.e. implicitly fixed, unbounded etc.).
- *	\return SUCCESSFUL_RETURN \n
-			RET_SETUPSUBJECTTOTYPE_FAILED */
-returnValue QProblem_setupSubjectToType( QProblem* _THIS );
-
-/** Determines type of new constraints and bounds (i.e. implicitly fixed, unbounded etc.).
- *	\return SUCCESSFUL_RETURN \n
-			RET_SETUPSUBJECTTOTYPE_FAILED */
-returnValue QProblem_setupSubjectToTypeNew(	QProblem* _THIS,
-											const real_t* const lb_new,		/**< New lower bounds. */
-											const real_t* const ub_new,		/**< New upper bounds. */
-											const real_t* const lbA_new,	/**< New lower constraints' bounds. */
-											const real_t* const ubA_new		/**< New upper constraints' bounds. */
-											);
-
-/** Computes the Cholesky decomposition of the projected Hessian (i.e. R^T*R = Z^T*H*Z).
- *  Note: If Hessian turns out not to be positive definite, the Hessian type
- *		  is set to HST_SEMIDEF accordingly.
- *	\return SUCCESSFUL_RETURN \n
- *			RET_HESSIAN_NOT_SPD \n
- *			RET_INDEXLIST_CORRUPTED */
-returnValue QProblem_computeProjectedCholesky( QProblem* _THIS );
-
-/** Computes initial Cholesky decomposition of the projected Hessian making
- *  use of the function setupCholeskyDecomposition() or setupCholeskyDecompositionProjected().
- *	\return SUCCESSFUL_RETURN \n
- *			RET_HESSIAN_NOT_SPD \n
- *			RET_INDEXLIST_CORRUPTED */
-returnValue QProblem_setupInitialCholesky( QProblem* _THIS );
-
-/** Initialises TQ factorisation of A (i.e. A*Q = [0 T]) if NO constraint is active.
- *	\return SUCCESSFUL_RETURN \n
- 			RET_INDEXLIST_CORRUPTED */
-returnValue QProblem_setupTQfactorisation( QProblem* _THIS );
-
-
-/** Obtains the desired working set for the auxiliary initial QP in
- *  accordance with the user specifications
- *  (assumes that member AX has already been initialised!)
- *	\return SUCCESSFUL_RETURN \n
-			RET_OBTAINING_WORKINGSET_FAILED \n
-			RET_INVALID_ARGUMENTS */
-returnValue QProblem_obtainAuxiliaryWorkingSet(	QProblem* _THIS,
-												const real_t* const xOpt,					/**< Optimal primal solution vector.
-																							 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
-												const real_t* const yOpt,					/**< Optimal dual solution vector.
-																							 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
-												Bounds* const guessedBounds,				/**< Guessed working set of bounds for solution (xOpt,yOpt). */
-												Constraints* const guessedConstraints,		/**< Guessed working set for solution (xOpt,yOpt). */
-												Bounds* auxiliaryBounds,					/**< Input: Allocated bound object. \n
-																							 *	 Ouput: Working set of constraints for auxiliary QP. */
-												Constraints* auxiliaryConstraints			/**< Input: Allocated bound object. \n
-																							 *	 Ouput: Working set for auxiliary QP. */
-												);
-
-/** Sets up bound and constraints data structures according to auxiliaryBounds/Constraints.
- *  (If the working set shall be setup afresh, make sure that
- *  bounds and constraints data structure have been resetted
- *  and the TQ factorisation has been initialised!)
- *	\return SUCCESSFUL_RETURN \n
-			RET_SETUP_WORKINGSET_FAILED \n
-			RET_INVALID_ARGUMENTS \n
-			RET_UNKNOWN_BUG */
-returnValue QProblem_setupAuxiliaryWorkingSet(	QProblem* _THIS,
-												Bounds* const auxiliaryBounds,				/**< Working set of bounds for auxiliary QP. */
-												Constraints* const auxiliaryConstraints,	/**< Working set of constraints for auxiliary QP. */
-												BooleanType setupAfresh						/**< Flag indicating if given working set shall be
-																							 *    setup afresh or by updating the current one. */
-												);
-
-/** Sets up the optimal primal/dual solution of the auxiliary initial QP.
- *	\return SUCCESSFUL_RETURN */
-returnValue QProblem_setupAuxiliaryQPsolution(	QProblem* _THIS,
-												const real_t* const xOpt,	/**< Optimal primal solution vector.
-																		 	 *	 If a NULL pointer is passed, all entries are set to zero. */
-												const real_t* const yOpt	/**< Optimal dual solution vector.
-																			 *	 If a NULL pointer is passed, all entries are set to zero. */
-												);
-
-/** Sets up gradient of the auxiliary initial QP for given
- *  optimal primal/dual solution and given initial working set
- *  (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
- *	\return SUCCESSFUL_RETURN */
-returnValue QProblem_setupAuxiliaryQPgradient( QProblem* _THIS );
-
-/** Sets up (constraints') bounds of the auxiliary initial QP for given
- *  optimal primal/dual solution and given initial working set
- *  (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
- *	\return SUCCESSFUL_RETURN \n
-			RET_UNKNOWN_BUG */
-returnValue QProblem_setupAuxiliaryQPbounds(	QProblem* _THIS,
-												Bounds* const auxiliaryBounds,				/**< Working set of bounds for auxiliary QP. */
-												Constraints* const auxiliaryConstraints,	/**< Working set of constraints for auxiliary QP. */
-												BooleanType useRelaxation					/**< Flag indicating if inactive (constraints') bounds shall be relaxed. */
-												);
-
-
-/** Adds a constraint to active set.
- *	\return SUCCESSFUL_RETURN \n
- 			RET_ADDCONSTRAINT_FAILED \n
-			RET_ADDCONSTRAINT_FAILED_INFEASIBILITY \n
-			RET_ENSURELI_FAILED */
-returnValue QProblem_addConstraint(	QProblem* _THIS,
-									int number,					/**< Number of constraint to be added to active set. */
-									SubjectToStatus C_status,	/**< Status of new active constraint. */
-									BooleanType updateCholesky,	/**< Flag indicating if Cholesky decomposition shall be updated. */
-									BooleanType ensureLI		/**< Ensure linear independence by exchange rules by default. */
-									);
-
-/** Checks if new active constraint to be added is linearly dependent from
- *	from row of the active constraints matrix.
- *	\return	 RET_LINEARLY_DEPENDENT \n
- 			 RET_LINEARLY_INDEPENDENT \n
-			 RET_INDEXLIST_CORRUPTED */
-returnValue QProblem_addConstraint_checkLI(	QProblem* _THIS,
-											int number			/**< Number of constraint to be added to active set. */
-											);
-
-/** Ensures linear independence of constraint matrix when a new constraint is added.
- * 	To _THIS end a bound or constraint is removed simultaneously if necessary.
- *	\return	 SUCCESSFUL_RETURN \n
- 			 RET_LI_RESOLVED \n
-			 RET_ENSURELI_FAILED \n
-			 RET_ENSURELI_FAILED_TQ \n
-			 RET_ENSURELI_FAILED_NOINDEX \n
-			 RET_REMOVE_FROM_ACTIVESET */
-returnValue QProblem_addConstraint_ensureLI(	QProblem* _THIS,
-												int number,					/**< Number of constraint to be added to active set. */
-												SubjectToStatus C_status	/**< Status of new active bound. */
-												);
-
-/** Adds a bound to active set.
- *	\return SUCCESSFUL_RETURN \n
- 			RET_ADDBOUND_FAILED \n
-			RET_ADDBOUND_FAILED_INFEASIBILITY \n
-			RET_ENSURELI_FAILED */
-returnValue QProblem_addBound(	QProblem* _THIS,
-								int number,					/**< Number of bound to be added to active set. */
-								SubjectToStatus B_status,	/**< Status of new active bound. */
-								BooleanType updateCholesky,	/**< Flag indicating if Cholesky decomposition shall be updated. */
-								BooleanType ensureLI 		/**< Ensure linear independence by exchange rules by default. */
-								);
-
-/** Checks if new active bound to be added is linearly dependent from
- *	from row of the active constraints matrix.
- *	\return	 RET_LINEARLY_DEPENDENT \n
- 			 RET_LINEARLY_INDEPENDENT */
-returnValue QProblem_addBound_checkLI(	QProblem* _THIS,
-										int number			/**< Number of bound to be added to active set. */
-										);
-
-/** Ensures linear independence of constraint matrix when a new bound is added.
- *	To _THIS end a bound or constraint is removed simultaneously if necessary.
- *	\return	 SUCCESSFUL_RETURN \n
- 			 RET_LI_RESOLVED \n
-			 RET_ENSURELI_FAILED \n
-			 RET_ENSURELI_FAILED_TQ \n
-			 RET_ENSURELI_FAILED_NOINDEX \n
-			 RET_REMOVE_FROM_ACTIVESET */
-returnValue QProblem_addBound_ensureLI(	QProblem* _THIS,
-										int number,					/**< Number of bound to be added to active set. */
-										SubjectToStatus B_status	/**< Status of new active bound. */
-										);
-
-/** Removes a constraint from active set.
- *	\return SUCCESSFUL_RETURN \n
- 			RET_CONSTRAINT_NOT_ACTIVE \n
-			RET_REMOVECONSTRAINT_FAILED \n
-			RET_HESSIAN_NOT_SPD */
-returnValue QProblem_removeConstraint(	QProblem* _THIS,
-										int number,						/**< Number of constraint to be removed from active set. */
-										BooleanType updateCholesky,		/**< Flag indicating if Cholesky decomposition shall be updated. */
-										BooleanType allowFlipping,		/**< Flag indicating if flipping bounds are allowed. */
-										BooleanType ensureNZC			/**< Flag indicating if non-zero curvature is ensured by exchange rules. */
-										);
-
-/** Removes a bounds from active set.
- *	\return SUCCESSFUL_RETURN \n
- 			RET_BOUND_NOT_ACTIVE \n
-			RET_HESSIAN_NOT_SPD \n
-			RET_REMOVEBOUND_FAILED */
-returnValue QProblem_removeBound(	QProblem* _THIS,
-									int number,						/**< Number of bound to be removed from active set. */
-									BooleanType updateCholesky,		/**< Flag indicating if Cholesky decomposition shall be updated. */
-									BooleanType allowFlipping,		/**< Flag indicating if flipping bounds are allowed. */
-									BooleanType ensureNZC			/**< Flag indicating if non-zero curvature is ensured by exchange rules. */
-									);
-
-
-/** Performs robustified ratio test yield the maximum possible step length
- *  along the homotopy path.
- *	\return  SUCCESSFUL_RETURN */
-returnValue QProblem_performPlainRatioTest(	QProblem* _THIS,
-											int nIdx, 					/**< Number of ratios to be checked. */
-											const int* const idxList, 	/**< Array containing the indices of all ratios to be checked. */
-											const real_t* const num,	/**< Array containing all numerators for performing the ratio test. */
-											const real_t* const den,	/**< Array containing all denominators for performing the ratio test. */
-											real_t epsNum,				/**< Numerator tolerance. */
-											real_t epsDen,				/**< Denominator tolerance. */
-											real_t* t,					/**< Output: Maximum possible step length along the homotopy path. */
-											int* BC_idx 				/**< Output: Index of blocking constraint. */
-											);
-
-
-/** Ensure non-zero curvature by primal jump.
- *  \return SUCCESSFUL_RETURN \n
- *          RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
-returnValue QProblem_ensureNonzeroCurvature(	QProblem* _THIS,
-												BooleanType removeBoundNotConstraint,	/**< SubjectTo to be removed is a bound. */
-												int remIdx,								/**< Index of bound/constraint to be removed. */
-												BooleanType* exchangeHappened,			/**< Output: Exchange was necessary to ensure. */
-												BooleanType* addBoundNotConstraint,		/**< SubjectTo to be added is a bound. */
-												int* addIdx,							/**< Index of bound/constraint to be added. */
-												SubjectToStatus* addStatus				/**< Status of bound/constraint to be added. */
-												);
-
-
-/** Solves the system Ta = b or T^Ta = b where T is a reverse upper triangular matrix.
- *	\return SUCCESSFUL_RETURN \n
- 			RET_DIV_BY_ZERO */
-returnValue QProblem_backsolveT(	QProblem* _THIS,
-									const real_t* const b,	/**< Right hand side vector. */
-									BooleanType transposed,	/**< Indicates if the transposed system shall be solved. */
-									real_t* const a 		/**< Output: Solution vector */
-									);
-
-
-/** Determines step direction of the shift of the QP data.
- *	\return SUCCESSFUL_RETURN */
-returnValue QProblem_determineDataShift(	QProblem* _THIS,
-											const real_t* const g_new,		/**< New gradient vector. */
-											const real_t* const lbA_new,	/**< New lower constraints' bounds. */
-											const real_t* const ubA_new,	/**< New upper constraints' bounds. */
-											const real_t* const lb_new,		/**< New lower bounds. */
-											const real_t* const ub_new,		/**< New upper bounds. */
-											real_t* const delta_g,	 		/**< Output: Step direction of gradient vector. */
-											real_t* const delta_lbA,		/**< Output: Step direction of lower constraints' bounds. */
-											real_t* const delta_ubA,		/**< Output: Step direction of upper constraints' bounds. */
-											real_t* const delta_lb,	 		/**< Output: Step direction of lower bounds. */
-											real_t* const delta_ub,	 		/**< Output: Step direction of upper bounds. */
-											BooleanType* Delta_bC_isZero,	/**< Output: Indicates if active constraints' bounds are to be shifted. */
-											BooleanType* Delta_bB_isZero	/**< Output: Indicates if active bounds are to be shifted. */
-											);
-
-/** Determines step direction of the homotopy path.
- *	\return SUCCESSFUL_RETURN \n
- 			RET_STEPDIRECTION_FAILED_TQ \n
-			RET_STEPDIRECTION_FAILED_CHOLESKY */
-returnValue QProblem_determineStepDirection(	QProblem* _THIS,
-												const real_t* const delta_g,	/**< Step direction of gradient vector. */
-												const real_t* const delta_lbA,	/**< Step direction of lower constraints' bounds. */
-												const real_t* const delta_ubA,	/**< Step direction of upper constraints' bounds. */
-												const real_t* const delta_lb,	/**< Step direction of lower bounds. */
-												const real_t* const delta_ub,	/**< Step direction of upper bounds. */
-												BooleanType Delta_bC_isZero, 	/**< Indicates if active constraints' bounds are to be shifted. */
-												BooleanType Delta_bB_isZero,	/**< Indicates if active bounds are to be shifted. */
-												real_t* const delta_xFX, 		/**< Output: Primal homotopy step direction of fixed variables. */
-												real_t* const delta_xFR,	 	/**< Output: Primal homotopy step direction of free variables. */
-												real_t* const delta_yAC, 		/**< Output: Dual homotopy step direction of active constraints' multiplier. */
-												real_t* const delta_yFX 		/**< Output: Dual homotopy step direction of fixed variables' multiplier. */
-												);
-
-/** Determines the maximum possible step length along the homotopy path
- *  and performs _THIS step (without changing working set).
- *	\return SUCCESSFUL_RETURN \n
- * 			RET_ERROR_IN_CONSTRAINTPRODUCT \n
- * 			RET_QP_INFEASIBLE */
-returnValue QProblem_performStep(	QProblem* _THIS,
-									const real_t* const delta_g,		/**< Step direction of gradient. */
-									const real_t* const delta_lbA,		/**< Step direction of lower constraints' bounds. */
-									const real_t* const delta_ubA,		/**< Step direction of upper constraints' bounds. */
-									const real_t* const delta_lb,	 	/**< Step direction of lower bounds. */
-									const real_t* const delta_ub,	 	/**< Step direction of upper bounds. */
-									const real_t* const delta_xFX, 		/**< Primal homotopy step direction of fixed variables. */
-									const real_t* const delta_xFR,		/**< Primal homotopy step direction of free variables. */
-									const real_t* const delta_yAC,		/**< Dual homotopy step direction of active constraints' multiplier. */
-									const real_t* const delta_yFX,		/**< Dual homotopy step direction of fixed variables' multiplier. */
-									int* BC_idx, 						/**< Output: Index of blocking constraint. */
-									SubjectToStatus* BC_status,			/**< Output: Status of blocking constraint. */
-									BooleanType* BC_isBound 			/**< Output: Indicates if blocking constraint is a bound. */
-									);
-
-/** Updates the active set.
- *	\return  SUCCESSFUL_RETURN \n
- 			 RET_REMOVE_FROM_ACTIVESET_FAILED \n
-			 RET_ADD_TO_ACTIVESET_FAILED */
-returnValue QProblem_changeActiveSet(	QProblem* _THIS,
-										int BC_idx, 				/**< Index of blocking constraint. */
-										SubjectToStatus BC_status,	/**< Status of blocking constraint. */
-										BooleanType BC_isBound 		/**< Indicates if blocking constraint is a bound. */
-										);
-
-
-/** Compute relative length of homotopy in data space for termination
- *  criterion.
- *  \return Relative length in data space. */
-real_t QProblem_getRelativeHomotopyLength(	QProblem* _THIS,
-											const real_t* const g_new,		/**< Final gradient. */
-											const real_t* const lb_new,		/**< Final lower variable bounds. */
-											const real_t* const ub_new,		/**< Final upper variable bounds. */
-											const real_t* const lbA_new,	/**< Final lower constraint bounds. */
-											const real_t* const ubA_new		/**< Final upper constraint bounds. */
-											);
-
-
-/** Ramping Strategy to avoid ties. Modifies homotopy start without
- *  changing current active set.
- *  \return SUCCESSFUL_RETURN */
-returnValue QProblem_performRamping( QProblem* _THIS );
-
-
-/** ... */
-returnValue QProblem_updateFarBounds(	QProblem* _THIS,
-										real_t curFarBound,				/**< ... */
-										int nRamp,						/**< ... */
-										const real_t* const lb_new,		/**< ... */
-										real_t* const lb_new_far,		/**< ... */
-										const real_t* const ub_new,		/**< ... */
-										real_t* const ub_new_far,		/**< ... */
-										const real_t* const lbA_new,	/**< ... */
-										real_t* const lbA_new_far,		/**< ... */
-										const real_t* const ubA_new,	/**< ... */
-										real_t* const ubA_new_far		/**< ... */
-										);
-
-/** ... */
-returnValue QProblemBCPY_updateFarBounds(	QProblem* _THIS,
-											real_t curFarBound,				/**< ... */
-											int nRamp,						/**< ... */
-											const real_t* const lb_new,		/**< ... */
-											real_t* const lb_new_far,		/**< ... */
-											const real_t* const ub_new,		/**< ... */
-											real_t* const ub_new_far		/**< ... */
-											);
-
-
-
-/** Performs robustified ratio test yield the maximum possible step length
- *  along the homotopy path.
- *	\return  SUCCESSFUL_RETURN */
-returnValue QProblem_performRatioTestC(	QProblem* _THIS,
-										int nIdx, 						/**< Number of ratios to be checked. */
-										const int* const idxList, 		/**< Array containing the indices of all ratios to be checked. */
-										Constraints* const subjectTo,	/**< Constraint object corresponding to ratios to be checked. */
-										const real_t* const num,	 	/**< Array containing all numerators for performing the ratio test. */
-										const real_t* const den,		/**< Array containing all denominators for performing the ratio test. */
-										real_t epsNum,					/**< Numerator tolerance. */
-										real_t epsDen,					/**< Denominator tolerance. */
-										real_t* t,						/**< Output: Maximum possible step length along the homotopy path. */
-										int* BC_idx 					/**< Output: Index of blocking constraint. */
-										);
-
-
-/** Drift correction at end of each active set iteration
- *  \return SUCCESSFUL_RETURN */
-returnValue QProblem_performDriftCorrection( QProblem* _THIS );
-
-
-/** Updates QP vectors, working sets and internal data structures in order to
-	start from an optimal solution corresponding to initial guesses of the working
-	set for bounds and constraints.
- *	\return SUCCESSFUL_RETURN \n
- *			RET_SETUP_AUXILIARYQP_FAILED \n
-			RET_INVALID_ARGUMENTS */
-returnValue QProblem_setupAuxiliaryQP(	QProblem* _THIS,
-										Bounds* const guessedBounds,			/**< Initial guess for working set of bounds. */
-										Constraints* const guessedConstraints	/**< Initial guess for working set of constraints. */
-										);
-
-/** Determines if it is more efficient to refactorise the matrices when
- *  hotstarting or not (i.e. better to update the existing factorisations).
- *	\return BT_TRUE iff matrices shall be refactorised afresh
- */
-BooleanType QProblem_shallRefactorise(	QProblem* _THIS,
-										Bounds* const guessedBounds,			/**< Guessed new working set of bounds. */
-										Constraints* const guessedConstraints	/**< Guessed new working set of constraints. */
-										);
-
-/** Setups internal QP data.
- *	\return SUCCESSFUL_RETURN \n
-			RET_INVALID_ARGUMENTS \n
-			RET_UNKNONW_BUG */
-returnValue QProblem_setupQPdataM(	QProblem* _THIS,
-									DenseMatrix *_H, 			/**< Hessian matrix. \n
-																	 If Hessian matrix is trivial,a NULL pointer can be passed. */
-									const real_t* const _g, 	/**< Gradient vector. */
-									DenseMatrix *_A, 			 /**< Constraint matrix. */
-									const real_t* const _lb,	/**< Lower bound vector (on variables). \n
-																	 If no lower bounds exist, a NULL pointer can be passed. */
-									const real_t* const _ub,	/**< Upper bound vector (on variables). \n
-																	 If no upper bounds exist, a NULL pointer can be passed. */
-									const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
-																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-									const real_t* const _ubA	/**< Upper constraints' bound vector. \n
-																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-									);
-
-
-/** Sets up dense internal QP data. If the current Hessian is trivial
- *  (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
- *  memory for Hessian is allocated and it is set to the given one.
- *	\return SUCCESSFUL_RETURN \n
-			RET_INVALID_ARGUMENTS \n
-			RET_UNKNONW_BUG */
-returnValue QProblem_setupQPdata(	QProblem* _THIS,
-									real_t* const _H, 			/**< Hessian matrix. \n
-																	 If Hessian matrix is trivial,a NULL pointer can be passed. */
-									const real_t* const _g, 	/**< Gradient vector. */
-									real_t* const _A,  			/**< Constraint matrix. */
-									const real_t* const _lb,	/**< Lower bound vector (on variables). \n
-																	 If no lower bounds exist, a NULL pointer can be passed. */
-									const real_t* const _ub,	/**< Upper bound vector (on variables). \n
-																	 If no upper bounds exist, a NULL pointer can be passed. */
-									const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
-																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-									const real_t* const _ubA	/**< Upper constraints' bound vector. \n
-																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-									);
-
-/** Sets up internal QP data by loading it from files. If the current Hessian
- *  is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
- *  memory for Hessian is allocated and it is set to the given one.
- *	\return SUCCESSFUL_RETURN \n
-			RET_UNABLE_TO_OPEN_FILE \n
-			RET_UNABLE_TO_READ_FILE \n
-			RET_INVALID_ARGUMENTS \n
-			RET_UNKNONW_BUG */
-returnValue QProblem_setupQPdataFromFile(	QProblem* _THIS,
-											const char* const H_file, 	/**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n
-														     				 If Hessian matrix is trivial,a NULL pointer can be passed. */
-											const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
-											const char* const A_file,	/**< Name of file where constraint matrix, of neighbouring QP to be solved, is stored. */
-											const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
-										 			 						 If no lower bounds exist, a NULL pointer can be passed. */
-											const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
-										 			 						 If no upper bounds exist, a NULL pointer can be passed. */
-											const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
-										 			 						 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-											const char* const ubA_file	/**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
-										 			 						 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-											);
-
-/** Loads new QP vectors from files (internal members are not affected!).
- *	\return SUCCESSFUL_RETURN \n
-			RET_UNABLE_TO_OPEN_FILE \n
-			RET_UNABLE_TO_READ_FILE \n
-			RET_INVALID_ARGUMENTS */
-returnValue QProblem_loadQPvectorsFromFile(	QProblem* _THIS,
-											const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
-											const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
-										 			 						 If no lower bounds exist, a NULL pointer can be passed. */
-											const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
-										 			 						 If no upper bounds exist, a NULL pointer can be passed. */
-											const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
-										 			 						 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-											const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
-										 			 						 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-											real_t* const g_new,		/**< Output: Gradient of neighbouring QP to be solved. */
-											real_t* const lb_new,		/**< Output: Lower bounds of neighbouring QP to be solved */
-											real_t* const ub_new,		/**< Output: Upper bounds of neighbouring QP to be solved */
-											real_t* const lbA_new,		/**< Output: Lower constraints' bounds of neighbouring QP to be solved */
-											real_t* const ubA_new		/**< Output: Upper constraints' bounds of neighbouring QP to be solved */
-											);
-
-
-/** Prints concise information on the current iteration.
- *	\return  SUCCESSFUL_RETURN \n */
-returnValue QProblem_printIteration(	QProblem* _THIS,
-										int iter,					/**< Number of current iteration. */
-										int BC_idx, 				/**< Index of blocking constraint. */
-										SubjectToStatus BC_status,	/**< Status of blocking constraint. */
-										BooleanType BC_isBound,		/**< Indicates if blocking constraint is a bound. */
-										real_t homotopyLength,		/**< Current homotopy distance. */
-										BooleanType isFirstCall		/**< Indicating whether this is the first call for current QP. */
- 										);
-
-
-/** Sets constraint matrix of the QP. \n
-	Note: Also internal vector Ax is recomputed!
- *	\return SUCCESSFUL_RETURN \n
- *			RET_INVALID_ARGUMENTS */
-static inline returnValue QProblem_setAM(	QProblem* _THIS,
-											DenseMatrix *A_new	/**< New constraint matrix. */
-											);
-
-/** Sets dense constraint matrix of the QP. \n
-	Note: Also internal vector Ax is recomputed!
- *	\return SUCCESSFUL_RETURN \n
- *			RET_INVALID_ARGUMENTS */
-static inline returnValue QProblem_setA(	QProblem* _THIS,
-											real_t* const A_new	/**< New dense constraint matrix (with correct dimension!). */
-											);
-
-
-/** Sets constraints' lower bound vector of the QP.
- *	\return SUCCESSFUL_RETURN \n
- *			RET_QPOBJECT_NOT_SETUP */
-static inline returnValue QProblem_setLBA(	QProblem* _THIS,
-											const real_t* const lbA_new	/**< New constraints' lower bound vector (with correct dimension!). */
-											);
-
-/** Changes single entry of lower constraints' bound vector of the QP.
- *	\return SUCCESSFUL_RETURN  \n
- *			RET_QPOBJECT_NOT_SETUP \n
- *			RET_INDEX_OUT_OF_BOUNDS */
-static inline returnValue QProblem_setLBAn(	QProblem* _THIS,
-											int number,		/**< Number of entry to be changed. */
-											real_t value	/**< New value for entry of lower constraints' bound vector (with correct dimension!). */
-											);
-
-/** Sets constraints' upper bound vector of the QP.
- *	\return SUCCESSFUL_RETURN \n
- *			RET_QPOBJECT_NOT_SETUP */
-static inline returnValue QProblem_setUBA(	QProblem* _THIS,
-											const real_t* const ubA_new	/**< New constraints' upper bound vector (with correct dimension!). */
-											);
-
-/** Changes single entry of upper constraints' bound vector of the QP.
- *	\return SUCCESSFUL_RETURN  \n
- *			RET_QPOBJECT_NOT_SETUP \n
- *			RET_INDEX_OUT_OF_BOUNDS */
-static inline returnValue QProblem_setUBAn(	QProblem* _THIS,
-											int number,		/**< Number of entry to be changed. */
-											real_t value	/**< New value for entry of upper constraints' bound vector (with correct dimension!). */
-											);
-
-
-/** Decides if lower bounds are smaller than upper bounds
- *
- * \return SUCCESSFUL_RETURN \n
- * 		   RET_QP_INFEASIBLE */
-returnValue QProblem_areBoundsConsistent(	QProblem* _THIS,
-											const real_t* const lb,		/**< Vector of lower bounds*/
-											const real_t* const ub,		/**< Vector of upper bounds*/
-											const real_t* const lbA,	/**< Vector of lower constraints*/
-											const real_t* const ubA		/**< Vector of upper constraints*/
-											);
-
-
-/** Drops the blocking bound/constraint that led to infeasibility, or finds another
- *  bound/constraint to drop according to drop priorities.
- *  \return SUCCESSFUL_RETURN \n
- */
-returnValue QProblem_dropInfeasibles ( 	QProblem* _THIS,
-										int BC_number,				/**< Number of the bound or constraint to be added */
-										SubjectToStatus BC_status, 	/**< New status of the bound or constraint to be added */
-										BooleanType BC_isBound,		/**< Whether a bound or a constraint is to be added */
-										real_t *xiB,
-										real_t *xiC
-										);
-
-
-/** If Hessian type has been set by the user, nothing is done.
- *  Otherwise the Hessian type is set to HST_IDENTITY, HST_ZERO, or
- *  HST_POSDEF (default), respectively.
- *	\return SUCCESSFUL_RETURN \n
-			RET_HESSIAN_INDEFINITE */
-returnValue QProblem_determineHessianType( QProblem* _THIS );
-
-/** Computes the Cholesky decomposition of the (simply projected) Hessian
- *  (i.e. R^T*R = Z^T*H*Z). It only works in the case where Z is a simple
- *  projection matrix!
- *  Note: If Hessian turns out not to be positive definite, the Hessian type
- *		  is set to HST_SEMIDEF accordingly.
- *	\return SUCCESSFUL_RETURN \n
- *			RET_HESSIAN_NOT_SPD \n
- *			RET_INDEXLIST_CORRUPTED */
-returnValue QProblemBCPY_computeCholesky( QProblem* _THIS );
-
-/** Obtains the desired working set for the auxiliary initial QP in
- *  accordance with the user specifications
- *	\return SUCCESSFUL_RETURN \n
-			RET_OBTAINING_WORKINGSET_FAILED \n
-			RET_INVALID_ARGUMENTS */
-returnValue QProblemBCPY_obtainAuxiliaryWorkingSet(	QProblem* _THIS,
-													const real_t* const xOpt,		/**< Optimal primal solution vector.
-																					 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
-													const real_t* const yOpt,		/**< Optimal dual solution vector.
-																					 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
-													Bounds* const guessedBounds,	/**< Guessed working set for solution (xOpt,yOpt). */
-													Bounds* auxiliaryBounds			/**< Input:  Allocated bound object. \n
-																					 *	 Output: Working set for auxiliary QP. */
-													);
-
-
-/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix.
- *	\return SUCCESSFUL_RETURN \n
-			RET_DIV_BY_ZERO */
-returnValue QProblem_backsolveR(	QProblem* _THIS,
-									const real_t* const b,	/**< Right hand side vector. */
-									BooleanType transposed,	/**< Indicates if the transposed system shall be solved. */
-									real_t* const a 		/**< Output: Solution vector */
-									);
-
-/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n
- *  Special variant for the case that _THIS function is called from within "removeBound()".
- *	\return SUCCESSFUL_RETURN \n
-			RET_DIV_BY_ZERO */
-returnValue QProblem_backsolveRrem(	QProblem* _THIS,
-									const real_t* const b,		/**< Right hand side vector. */
-									BooleanType transposed,		/**< Indicates if the transposed system shall be solved. */
-									BooleanType removingBound,	/**< Indicates if function is called from "removeBound()". */
-									real_t* const a 			/**< Output: Solution vector */
-									);
-
-
-/** Determines step direction of the shift of the QP data.
- *	\return SUCCESSFUL_RETURN */
-returnValue QProblemBCPY_determineDataShift(	QProblem* _THIS,
-												const real_t* const g_new,		/**< New gradient vector. */
-												const real_t* const lb_new,		/**< New lower bounds. */
-												const real_t* const ub_new,		/**< New upper bounds. */
-												real_t* const delta_g,	 		/**< Output: Step direction of gradient vector. */
-												real_t* const delta_lb,	 		/**< Output: Step direction of lower bounds. */
-												real_t* const delta_ub,	 		/**< Output: Step direction of upper bounds. */
-												BooleanType* Delta_bB_isZero	/**< Output: Indicates if active bounds are to be shifted. */
-												);
-
-
-/** Sets up internal QP data.
- *	\return SUCCESSFUL_RETURN \n
-			RET_INVALID_ARGUMENTS */
-returnValue QProblemBCPY_setupQPdataM(	QProblem* _THIS,
-										DenseMatrix *_H,	 		/**< Hessian matrix.*/
-										const real_t* const _g,		/**< Gradient vector. */
-										const real_t* const _lb,	/**< Lower bounds (on variables). \n
-																		 If no lower bounds exist, a NULL pointer can be passed. */
-										const real_t* const _ub		/**< Upper bounds (on variables). \n
-																		 If no upper bounds exist, a NULL pointer can be passed. */
-										);
-
-/** Sets up internal QP data. If the current Hessian is trivial
- *  (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
- *  memory for Hessian is allocated and it is set to the given one.
- *	\return SUCCESSFUL_RETURN \n
-			RET_INVALID_ARGUMENTS \n
-			RET_NO_HESSIAN_SPECIFIED */
-returnValue QProblemBCPY_setupQPdata(	QProblem* _THIS,
-										real_t* const _H, 			/**< Hessian matrix. \n
-																		 If Hessian matrix is trivial,a NULL pointer can be passed. */
-										const real_t* const _g,		/**< Gradient vector. */
-										const real_t* const _lb,	/**< Lower bounds (on variables). \n
-																		 If no lower bounds exist, a NULL pointer can be passed. */
-										const real_t* const _ub		/**< Upper bounds (on variables). \n
-																		 If no upper bounds exist, a NULL pointer can be passed. */
-										);
-
-/** Sets up internal QP data by loading it from files. If the current Hessian
- *  is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
- *  memory for Hessian is allocated and it is set to the given one.
- *	\return SUCCESSFUL_RETURN \n
-			RET_UNABLE_TO_OPEN_FILE \n
-			RET_UNABLE_TO_READ_FILE \n
-			RET_INVALID_ARGUMENTS \n
-			RET_NO_HESSIAN_SPECIFIED */
-returnValue QProblemBCPY_setupQPdataFromFile(	QProblem* _THIS,
-												const char* const H_file, 	/**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n
-														     					 If Hessian matrix is trivial,a NULL pointer can be passed. */
-												const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
-												const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
-										 			 							 If no lower bounds exist, a NULL pointer can be passed. */
-												const char* const ub_file 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
-										 			 							 If no upper bounds exist, a NULL pointer can be passed. */
-												);
-
-/** Loads new QP vectors from files (internal members are not affected!).
- *	\return SUCCESSFUL_RETURN \n
-			RET_UNABLE_TO_OPEN_FILE \n
-			RET_UNABLE_TO_READ_FILE \n
-			RET_INVALID_ARGUMENTS */
-returnValue QProblemBCPY_loadQPvectorsFromFile(	QProblem* _THIS,
-												const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
-												const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
-										 			 							 If no lower bounds exist, a NULL pointer can be passed. */
-												const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
-										 			 							 If no upper bounds exist, a NULL pointer can be passed. */
-												real_t* const g_new,		/**< Output: Gradient of neighbouring QP to be solved. */
-												real_t* const lb_new,		/**< Output: Lower bounds of neighbouring QP to be solved */
-												real_t* const ub_new		/**< Output: Upper bounds of neighbouring QP to be solved */
-												);
-
-
-/** Sets internal infeasibility flag and throws given error in case the far bound
- *	strategy is not enabled (as QP might actually not be infeasible in _THIS case).
- *	\return RET_HOTSTART_STOPPED_INFEASIBILITY \n
-			RET_ENSURELI_FAILED_CYCLING \n
-			RET_ENSURELI_FAILED_NOINDEX */
-returnValue QProblem_setInfeasibilityFlag(	QProblem* _THIS,
-											returnValue returnvalue,	/**< Returnvalue to be tunneled. */
-											BooleanType doThrowError	/**< Flag forcing to throw an error. */
-											);
-
-
-/** Determines if next QP iteration can be performed within given CPU time limit.
- *	\return BT_TRUE: CPU time limit is exceeded, stop QP solution. \n
-			BT_FALSE: Sufficient CPU time for next QP iteration. */
-BooleanType QProblem_isCPUtimeLimitExceeded(	QProblem* _THIS,
-												const real_t* const cputime,	/**< Maximum CPU time allowed for QP solution. */
-												real_t starttime,				/**< Start time of current QP solution. */
-												int nWSR						/**< Number of working set recalculations performed so far. */
-												);
-
-
-/** Regularise Hessian matrix by adding a scaled identity matrix to it.
- *	\return SUCCESSFUL_RETURN \n
-			RET_HESSIAN_ALREADY_REGULARISED */
-returnValue QProblem_regulariseHessian( QProblem* _THIS );
-
-
-/** Sets Hessian matrix of the QP.
- *	\return SUCCESSFUL_RETURN */
-static inline returnValue QProblem_setHM(	QProblem* _THIS,
-											DenseMatrix* H_new	/**< New Hessian matrix. */
-											);
-
-/** Sets dense Hessian matrix of the QP.
- *  If a null pointer is passed and
- *  a) hessianType is HST_IDENTITY, nothing is done,
- *  b) hessianType is not HST_IDENTITY, Hessian matrix is set to zero.
- *	\return SUCCESSFUL_RETURN */
-static inline returnValue QProblem_setH(	QProblem* _THIS,
-											real_t* const H_new	/**< New dense Hessian matrix (with correct dimension!). */
-											);
-
-/** Changes gradient vector of the QP.
- *	\return SUCCESSFUL_RETURN \n
- *			RET_INVALID_ARGUMENTS */
-static inline returnValue QProblem_setG(	QProblem* _THIS,
-											const real_t* const g_new	/**< New gradient vector (with correct dimension!). */
-											);
-
-/** Changes lower bound vector of the QP.
- *	\return SUCCESSFUL_RETURN \n
- *			RET_INVALID_ARGUMENTS */
-static inline returnValue QProblem_setLB(	QProblem* _THIS,
-											const real_t* const lb_new	/**< New lower bound vector (with correct dimension!). */
-											);
-
-/** Changes single entry of lower bound vector of the QP.
- *	\return SUCCESSFUL_RETURN  \n
-			RET_INDEX_OUT_OF_BOUNDS */
-static inline returnValue QProblem_setLBn(	QProblem* _THIS,
-											int number,		/**< Number of entry to be changed. */
-											real_t value	/**< New value for entry of lower bound vector. */
-											);
-
-/** Changes upper bound vector of the QP.
- *	\return SUCCESSFUL_RETURN \n
- *			RET_INVALID_ARGUMENTS */
-static inline returnValue QProblem_setUB(	QProblem* _THIS,
-											const real_t* const ub_new	/**< New upper bound vector (with correct dimension!). */
-											);
-
-/** Changes single entry of upper bound vector of the QP.
- *	\return SUCCESSFUL_RETURN  \n
-			RET_INDEX_OUT_OF_BOUNDS */
-static inline returnValue QProblem_setUBn(	QProblem* _THIS,
-											int number,		/**< Number of entry to be changed. */
-											real_t value	/**< New value for entry of upper bound vector. */
-											);
-
-
-
-/** Compute relative length of homotopy in data space for termination
- *  criterion.
- *  \return Relative length in data space. */
-real_t QProblemBCPY_getRelativeHomotopyLength(	QProblem* _THIS,
-												const real_t* const g_new,	/**< Final gradient. */
-												const real_t* const lb_new,	/**< Final lower variable bounds. */
-												const real_t* const ub_new	/**< Final upper variable bounds. */
-												);
-
-
-
-/** Performs robustified ratio test yield the maximum possible step length
- *  along the homotopy path.
- *	\return  SUCCESSFUL_RETURN */
-returnValue QProblem_performRatioTestB(	QProblem* _THIS,
-										int nIdx, 					/**< Number of ratios to be checked. */
-										const int* const idxList, 	/**< Array containing the indices of all ratios to be checked. */
-										Bounds* const subjectTo,	/**< Bound object corresponding to ratios to be checked. */
-										const real_t* const num,	/**< Array containing all numerators for performing the ratio test. */
-										const real_t* const den,	/**< Array containing all denominators for performing the ratio test. */
-										real_t epsNum,				/**< Numerator tolerance. */
-										real_t epsDen,				/**< Denominator tolerance. */
-										real_t* t,					/**< Output: Maximum possible step length along the homotopy path. */
-										int* BC_idx 				/**< Output: Index of blocking constraint. */
-										);
-
-/** Checks whether given ratio is blocking, i.e. limits the maximum step length
- *  along the homotopy path to a value lower than given one.
- *	\return  SUCCESSFUL_RETURN */
-static inline BooleanType QProblem_isBlocking(	QProblem* _THIS,
-												real_t num,			/**< Numerator for performing the ratio test. */
-												real_t den,			/**< Denominator for performing the ratio test. */
-												real_t epsNum,		/**< Numerator tolerance. */
-												real_t epsDen,		/**< Denominator tolerance. */
-												real_t* t			/**< Input:  Current maximum step length along the homotopy path,
-																	 *   Output: Updated maximum possible step length along the homotopy path. */
-												);
-
-
-/** ...
- *	\return SUCCESSFUL_RETURN  \n
-			RET_UNABLE_TO_OPEN_FILE */
-returnValue QProblem_writeQpDataIntoMatFile(	QProblem* _THIS,
-												const char* const filename	/**< Mat file name. */
-												);
-
-/** ...
-*	\return SUCCESSFUL_RETURN  \n
-			RET_UNABLE_TO_OPEN_FILE */
-returnValue QProblem_writeQpWorkspaceIntoMatFile(	QProblem* _THIS,
-													const char* const filename	/**< Mat file name. */
-													);
-
-
-/*
- *	g e t B o u n d s
- */
-static inline returnValue QProblem_getBounds( QProblem* _THIS, Bounds* _bounds )
-{
-	int nV = QProblem_getNV( _THIS );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	_bounds = _THIS->bounds;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t N V
- */
-static inline int QProblem_getNV( QProblem* _THIS )
-{
-	return Bounds_getNV( _THIS->bounds );
-}
-
-
-/*
- *	g e t N F R
- */
-static inline int QProblem_getNFR( QProblem* _THIS )
-{
-	return Bounds_getNFR( _THIS->bounds );
-}
-
-
-/*
- *	g e t N F X
- */
-static inline int QProblem_getNFX( QProblem* _THIS )
-{
-	return Bounds_getNFX( _THIS->bounds );
-}
-
-
-/*
- *	g e t N F V
- */
-static inline int QProblem_getNFV( QProblem* _THIS )
-{
-	return Bounds_getNFV( _THIS->bounds );
-}
-
-
-/*
- *	g e t S t a t u s
- */
-static inline QProblemStatus QProblem_getStatus( QProblem* _THIS )
-{
-	return _THIS->status;
-}
-
-
-/*
- *	i s I n i t i a l i s e d
- */
-static inline BooleanType QProblem_isInitialised( QProblem* _THIS )
-{
-	if ( _THIS->status == QPS_NOTINITIALISED )
-		return BT_FALSE;
-	else
-		return BT_TRUE;
-}
-
-
-/*
- *	i s S o l v e d
- */
-static inline BooleanType QProblem_isSolved( QProblem* _THIS )
-{
-	if ( _THIS->status == QPS_SOLVED )
-		return BT_TRUE;
-	else
-		return BT_FALSE;
-}
-
-
-/*
- *	i s I n f e a s i b l e
- */
-static inline BooleanType QProblem_isInfeasible( QProblem* _THIS )
-{
-	return _THIS->infeasible;
-}
-
-
-/*
- *	i s U n b o u n d e d
- */
-static inline BooleanType QProblem_isUnbounded( QProblem* _THIS )
-{
-	return _THIS->unbounded;
-}
-
-
-/*
- *	g e t H e s s i a n T y p e
- */
-static inline HessianType QProblem_getHessianType( QProblem* _THIS )
-{
-	return _THIS->hessianType;
-}
-
-
-/*
- *	s e t H e s s i a n T y p e
- */
-static inline returnValue QProblem_setHessianType( QProblem* _THIS, HessianType _hessianType )
-{
-	_THIS->hessianType = _hessianType;
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	u s i n g R e g u l a r i s a t i o n
- */
-static inline BooleanType QProblem_usingRegularisation( QProblem* _THIS )
-{
-	if ( _THIS->regVal > QPOASES_ZERO )
-		return BT_TRUE;
-	else
-		return BT_FALSE;
-}
-
-
-/*
- *	g e t O p t i o n s
- */
-static inline Options QProblem_getOptions( QProblem* _THIS )
-{
-	return _THIS->options;
-}
-
-
-/*
- *	s e t O p t i o n s
- */
-static inline returnValue QProblem_setOptions(	QProblem* _THIS,
-												Options _options
-												)
-{
-	OptionsCPY( &_options,&(_THIS->options) );
-	Options_ensureConsistency( &(_THIS->options) );
-
-	QProblem_setPrintLevel( _THIS,_THIS->options.printLevel );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t P r i n t L e v e l
- */
-static inline PrintLevel QProblem_getPrintLevel( QProblem* _THIS )
-{
-	return _THIS->options.printLevel;
-}
-
-
-/*
- *	g e t C o u n t
- */
-static inline unsigned int QProblem_getCount( QProblem* _THIS )
-{
-	return _THIS->count;
-}
-
-
-/*
- *	r e s e t C o u n t e r
- */
-static inline returnValue QProblem_resetCounter( QProblem* _THIS )
-{
-	_THIS->count = 0;
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-
-/*
- *	s e t H
- */
-static inline returnValue QProblem_setHM( QProblem* _THIS, DenseMatrix* H_new )
-{
-	if ( H_new == 0 )
-		return QProblem_setH( _THIS,(real_t*)0 );
-	else
-		return QProblem_setH( _THIS,DenseMatrix_getVal(H_new) );
-}
-
-
-/*
- *	s e t H
- */
-static inline returnValue QProblem_setH( QProblem* _THIS, real_t* const H_new )
-{
-	/* if null pointer is passed, Hessian is set to zero matrix
-	 *                            (or stays identity matrix) */
-	if ( H_new == 0 )
-	{
-		if ( _THIS->hessianType == HST_IDENTITY )
-			return SUCCESSFUL_RETURN;
-
-		_THIS->hessianType = HST_ZERO;
-
-		_THIS->H = 0;
-	}
-	else
-	{
-		DenseMatrixCON( _THIS->H,QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),H_new );
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t G
- */
-static inline returnValue QProblem_setG( QProblem* _THIS, const real_t* const g_new )
-{
-	unsigned int nV = (unsigned int)QProblem_getNV( _THIS );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( g_new == 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	memcpy( _THIS->g,g_new,nV*sizeof(real_t) );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t L B
- */
-static inline returnValue QProblem_setLB( QProblem* _THIS, const real_t* const lb_new )
-{
-	unsigned int i;
-	unsigned int nV = (unsigned int)QProblem_getNV( _THIS );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( lb_new != 0 )
-	{
-		memcpy( _THIS->lb,lb_new,nV*sizeof(real_t) );
-	}
-	else
-	{
-		/* if no lower bounds are specified, set them to -infinity */
-		for( i=0; i<nV; ++i )
-			_THIS->lb[i] = -QPOASES_INFTY;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t L B
- */
-static inline returnValue QProblem_setLBn( QProblem* _THIS, int number, real_t value )
-{
-	int nV = QProblem_getNV( _THIS );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( ( number >= 0 ) && ( number < nV ) )
-	{
-		_THIS->lb[number] = value;
-		return SUCCESSFUL_RETURN;
-	}
-	else
-	{
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-	}
-}
-
-
-/*
- *	s e t U B
- */
-static inline returnValue QProblem_setUB( QProblem* _THIS, const real_t* const ub_new )
-{
-	unsigned int i;
-	unsigned int nV = (unsigned int)QProblem_getNV( _THIS );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( ub_new != 0 )
-	{
-		memcpy( _THIS->ub,ub_new,nV*sizeof(real_t) );
-	}
-	else
-	{
-		/* if no upper bounds are specified, set them to infinity */
-		for( i=0; i<nV; ++i )
-			_THIS->ub[i] = QPOASES_INFTY;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t U B
- */
-static inline returnValue QProblem_setUBn( QProblem* _THIS, int number, real_t value )
-{
-	int nV = QProblem_getNV( _THIS );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( ( number >= 0 ) && ( number < nV ) )
-	{
-		_THIS->ub[number] = value;
-
-		return SUCCESSFUL_RETURN;
-	}
-	else
-	{
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-	}
-}
-
-
-
-/*
- * i s B l o c k i n g
- */
-static inline BooleanType QProblem_isBlocking(	QProblem* _THIS,
-												real_t num,
-												real_t den,
-												real_t epsNum,
-												real_t epsDen,
-												real_t* t
-												)
-{
-	if ( ( den >= epsDen ) && ( num >= epsNum ) )
-	{
-		if ( num < (*t)*den )
-			return BT_TRUE;
-	}
-
-	return BT_FALSE;
-}
-
-
-
-/*
- *	g e t C o n s t r a i n t s
- */
-static inline returnValue QProblem_getConstraints( QProblem* _THIS, Constraints* _constraints )
-{
-	int nV = QProblem_getNV( _THIS );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	ConstraintsCPY( _THIS->constraints,_constraints );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	g e t N C
- */
-static inline int QProblem_getNC( QProblem* _THIS )
-{
-	return Constraints_getNC( _THIS->constraints );
-}
-
-
-/*
- *	g e t N E C
- */
-static inline int QProblem_getNEC( QProblem* _THIS )
-{
-	return Constraints_getNEC( _THIS->constraints );
-}
-
-
-/*
- *	g e t N A C
- */
-static inline int QProblem_getNAC( QProblem* _THIS )
-{
-	return Constraints_getNAC( _THIS->constraints );
-}
-
-
-/*
- *	g e t N I A C
- */
-static inline int QProblem_getNIAC( QProblem* _THIS )
-{
-	return Constraints_getNIAC( _THIS->constraints );
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-
-/*
- *	s e t A
- */
-static inline returnValue QProblem_setAM( QProblem* _THIS, DenseMatrix *A_new )
-{
-	if ( A_new == 0 )
-		return QProblem_setA( _THIS,(real_t*)0 );
-	else
-		return QProblem_setA( _THIS,DenseMatrix_getVal(A_new) );
-}
-
-
-/*
- *	s e t A
- */
-static inline returnValue QProblem_setA( QProblem* _THIS, real_t* const A_new )
-{
-	int j;
-	int nV = QProblem_getNV( _THIS );
-	int nC = QProblem_getNC( _THIS );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( A_new == 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	DenseMatrixCON( _THIS->A,QProblem_getNC( _THIS ),QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),A_new );
-
-	DenseMatrix_times( _THIS->A,1, 1.0, _THIS->x, nV, 0.0, _THIS->Ax, nC);
-
-	for( j=0; j<nC; ++j )
-	{
-		_THIS->Ax_u[j] = _THIS->ubA[j] - _THIS->Ax[j];
-		_THIS->Ax_l[j] = _THIS->Ax[j] - _THIS->lbA[j];
-
-		/* (ckirches) disable constraints with empty rows */
-		if ( qpOASES_isZero( DenseMatrix_getRowNorm( _THIS->A,j,2 ),QPOASES_ZERO ) == BT_TRUE )
-			Constraints_setType( _THIS->constraints,j,ST_DISABLED );
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t L B A
- */
-static inline returnValue QProblem_setLBA( QProblem* _THIS, const real_t* const lbA_new )
-{
-	unsigned int i;
-	unsigned int nV = (unsigned int)QProblem_getNV( _THIS );
-	unsigned int nC = (unsigned int)QProblem_getNC( _THIS );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( lbA_new != 0 )
-	{
-		memcpy( _THIS->lbA,lbA_new,nC*sizeof(real_t) );
-	}
-	else
-	{
-		/* if no lower constraints' bounds are specified, set them to -infinity */
-		for( i=0; i<nC; ++i )
-			_THIS->lbA[i] = -QPOASES_INFTY;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t L B A
- */
-static inline returnValue QProblem_setLBAn( QProblem* _THIS, int number, real_t value )
-{
-	int nV = QProblem_getNV( _THIS );
-	int nC = QProblem_getNC( _THIS );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( ( number >= 0 ) && ( number < nC ) )
-	{
-		_THIS->lbA[number] = value;
-		return SUCCESSFUL_RETURN;
-	}
-	else
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-}
-
-
-/*
- *	s e t U B A
- */
-static inline returnValue QProblem_setUBA( QProblem* _THIS, const real_t* const ubA_new )
-{
-	unsigned int i;
-	unsigned int nV = (unsigned int)QProblem_getNV( _THIS );
-	unsigned int nC = (unsigned int)QProblem_getNC( _THIS );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( ubA_new != 0 )
-	{
-		memcpy( _THIS->ubA,ubA_new,nC*sizeof(real_t) );
-	}
-	else
-	{
-		/* if no upper constraints' bounds are specified, set them to infinity */
-		for( i=0; i<nC; ++i )
-			_THIS->ubA[i] = QPOASES_INFTY;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t U B A
- */
-static inline returnValue QProblem_setUBAn( QProblem* _THIS, int number, real_t value )
-{
-	int nV = QProblem_getNV( _THIS );
-	int nC = QProblem_getNC( _THIS );
-
-	if ( nV == 0 )
-		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
-
-	if ( ( number >= 0 ) && ( number < nC ) )
-	{
-		_THIS->ubA[number] = value;
-		return SUCCESSFUL_RETURN;
-	}
-	else
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-}
-
-
-END_NAMESPACE_QPOASES
-
-
-#endif	/* QPOASES_QPROBLEM_H */
-
-
-/*
- *	end of file
- */
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/QProblem.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ *
+ *	Declaration of the QProblem class which is able to use the newly
+ *	developed online active set strategy for parametric quadratic programming.
+ */
+
+
+
+#ifndef QPOASES_QPROBLEM_H
+#define QPOASES_QPROBLEM_H
+
+
+#include <qpOASES_e/Bounds.h>
+#include <qpOASES_e/Options.h>
+#include <qpOASES_e/Matrices.h>
+#include <qpOASES_e/Constraints.h>
+#include <qpOASES_e/Flipper.h>
+#include <qpOASES_e/ConstraintProduct.h>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+typedef struct {
+	Bounds *auxiliaryBounds;
+	Constraints *auxiliaryConstraints;
+
+	real_t *ub_new_far;
+	real_t *lb_new_far;
+	real_t *ubA_new_far;
+	real_t *lbA_new_far;
+
+	real_t *g_new;
+	real_t *lb_new;
+	real_t *ub_new;
+	real_t *lbA_new;
+	real_t *ubA_new;
+
+	real_t *g_new2;
+	real_t *lb_new2;
+	real_t *ub_new2;
+	real_t *lbA_new2;
+	real_t *ubA_new2;
+
+	real_t *delta_xFX5;
+	real_t *delta_xFR5;
+	real_t *delta_yAC5;
+	real_t *delta_yFX5;
+
+	real_t *Hx;
+
+	real_t *_H;
+
+	real_t *g_original;
+	real_t *lb_original;
+	real_t *ub_original;
+	real_t *lbA_original;
+	real_t *ubA_original;
+
+	real_t *delta_xFR;
+	real_t *delta_xFX;
+	real_t *delta_yAC;
+	real_t *delta_yFX;
+	real_t *delta_g;
+	real_t *delta_lb;
+	real_t *delta_ub;
+	real_t *delta_lbA;
+	real_t *delta_ubA;
+
+	real_t *gMod;
+
+	real_t *aFR;
+	real_t *wZ;
+
+	real_t *delta_g2;
+	real_t *delta_xFX2;
+	real_t *delta_xFR2;
+	real_t *delta_yAC2;
+	real_t *delta_yFX2;
+	real_t *nul;
+	real_t *Arow;
+
+	real_t *xiC;
+	real_t *xiC_TMP;
+	real_t *xiB;
+	real_t *Arow2;
+	real_t *num;
+
+	real_t *w;
+	real_t *tmp;
+
+	real_t *delta_g3;
+	real_t *delta_xFX3;
+	real_t *delta_xFR3;
+	real_t *delta_yAC3;
+	real_t *delta_yFX3;
+	real_t *nul2;
+
+	real_t *xiC2;
+	real_t *xiC_TMP2;
+	real_t *xiB2;
+	real_t *num2;
+
+	real_t *Hz;
+	real_t *z;
+	real_t *ZHz;
+	real_t *r;
+
+	real_t *tmp2;
+	real_t *Hz2;
+	real_t *z2;
+	real_t *r2;
+	real_t *rhs;
+
+	real_t *delta_xFX4;
+	real_t *delta_xFR4;
+	real_t *delta_yAC4;
+	real_t *delta_yFX4;
+	real_t *nul3;
+	real_t *ek;
+	real_t *x_W;
+	real_t *As;
+	real_t *Ax_W;
+
+	real_t *num3;
+	real_t *den;
+	real_t *delta_Ax_l;
+	real_t *delta_Ax_u;
+	real_t *delta_Ax;
+	real_t *delta_x;
+
+	real_t *_A;
+
+	real_t *grad;
+	real_t *AX;
+} QProblem_ws;
+
+int QProblem_ws_calculateMemorySize( unsigned int nV, unsigned int nC );
+
+char *QProblem_ws_assignMemory( unsigned int nV, unsigned int nC, QProblem_ws **mem, void *raw_memory );
+
+QProblem_ws *QProblem_ws_createMemory( unsigned int nV, unsigned int nC );
+
+/**
+ *	\brief Implements the online active set strategy for QPs with general constraints.
+ *
+ *	A class for setting up and solving quadratic programs. The main feature is
+ *	the possibily to use the newly developed online active set strategy for
+ * 	parametric quadratic programming.
+ *
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ */
+typedef struct
+{
+	QProblem_ws *ws;				/**< Workspace */
+	Bounds *bounds;					/**< Data structure for problem's bounds. */
+	Constraints *constraints;		/**< Data structure for problem's constraints. */
+	Flipper *flipper;				/**< Struct for making a temporary copy of the matrix factorisations. */
+
+	DenseMatrix* H;					/**< Hessian matrix pointer. */
+	DenseMatrix* A;					/**< Constraint matrix pointer. */
+
+	Options options;				/**< Struct containing all user-defined options for solving QPs. */
+	TabularOutput tabularOutput;	/**< Struct storing information for tabular output (printLevel == PL_TABULAR). */
+
+	real_t *g;						/**< Gradient. */
+
+	real_t *lb;						/**< Lower bound vector (on variables). */
+	real_t *ub;						/**< Upper bound vector (on variables). */
+	real_t *lbA;					/**< Lower constraints' bound vector. */
+	real_t *ubA;					/**< Upper constraints' bound vector. */
+
+	real_t *R;						/**< Cholesky factor of H (i.e. H = R^T*R). */
+
+	real_t *T;						/**< Reverse triangular matrix, A = [0 T]*Q'. */
+	real_t *Q;						/**< Orthonormal quadratic matrix, A = [0 T]*Q'. */
+
+	real_t *Ax;						/**< Stores the current A*x \n
+									 *	 (for increased efficiency only). */
+	real_t *Ax_l;					/**< Stores the current distance to lower constraints' bounds A*x-lbA \n
+									 *	 (for increased efficiency only). */
+	real_t *Ax_u;					/**< Stores the current distance to lower constraints' bounds ubA-A*x \n
+									 *	 (for increased efficiency only). */
+
+	real_t *x;						/**< Primal solution vector. */
+	real_t *y;						/**< Dual solution vector. */
+
+	real_t *delta_xFR_TMP;			/**< Temporary for determineStepDirection */
+	real_t *tempA;					/**< Temporary for determineStepDirection. */
+	real_t *tempB;					/**< Temporary for determineStepDirection. */
+	real_t *ZFR_delta_xFRz;			/**< Temporary for determineStepDirection. */
+	real_t *delta_xFRy;				/**< Temporary for determineStepDirection. */
+	real_t *delta_xFRz;				/**< Temporary for determineStepDirection. */
+	real_t *delta_yAC_TMP;			/**< Temporary for determineStepDirection. */
+
+	ConstraintProduct constraintProduct;	/**< Pointer to user-defined constraint product function. */
+
+	real_t tau;						/**< Last homotopy step length. */
+	real_t regVal;					/**< Holds the offset used to regularise Hessian matrix (zero by default). */
+
+	real_t ramp0;					/**< Start value for Ramping Strategy. */
+	real_t ramp1;					/**< Final value for Ramping Strategy. */
+
+	QProblemStatus status;			/**< Current status of the solution process. */
+	HessianType hessianType;		/**< Type of Hessian matrix. */
+
+	BooleanType haveCholesky;		/**< Flag indicating whether Cholesky decomposition has already been setup. */
+	BooleanType infeasible;			/**< QP infeasible? */
+	BooleanType unbounded;			/**< QP unbounded? */
+
+	int rampOffset;					/**< Offset index for Ramping. */
+	unsigned int count;				/**< Counts the number of hotstart function calls (internal usage only!). */
+
+	int sizeT;						/**< Matrix T is stored in a (sizeT x sizeT) array. */
+} QProblem;
+
+int QProblem_calculateMemorySize( unsigned int nV, unsigned int nC );
+
+char *QProblem_assignMemory( unsigned int nV, unsigned int nC, QProblem **mem, void *raw_memory );
+
+QProblem *QProblem_createMemory( unsigned int nV, unsigned int nC );
+
+
+/** Constructor which takes the QP dimension and Hessian type
+ *  information. If the Hessian is the zero (i.e. HST_ZERO) or the
+ *  identity matrix (i.e. HST_IDENTITY), respectively, no memory
+ *  is allocated for it and a NULL pointer can be passed for it
+ *  to the init() functions. */
+void QProblemCON(	QProblem* _THIS,
+					int _nV,	  					/**< Number of variables. */
+					int _nC,		  				/**< Number of constraints. */
+					HessianType _hessianType 		/**< Type of Hessian matrix. */
+					);
+
+/** Copies all members from given rhs object.
+ *  \return SUCCESSFUL_RETURN */
+void QProblemCPY(	QProblem* FROM,
+					QProblem* TO
+					);
+
+
+/** Clears all data structures of QProblem except for QP data.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_RESET_FAILED */
+returnValue QProblem_reset( QProblem* _THIS );
+
+
+/** Initialises a QP problem with given QP data and tries to solve it
+ *	using at most nWSR iterations.
+ *
+ *  Note: This function internally calls solveInitialQP for initialisation!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_TQ \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblem_initM(	QProblem* _THIS,
+							DenseMatrix *_H,			/**< Hessian matrix. */
+							const real_t* const _g, 	/**< Gradient vector. */
+							DenseMatrix *_A,  			/**< Constraint matrix. */
+							const real_t* const _lb,	/**< Lower bound vector (on variables). \n
+															 If no lower bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ub,	/**< Upper bound vector (on variables). \n
+															 If no upper bounds exist, a NULL pointer can be passed. */
+							const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
+															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ubA,	/**< Upper constraints' bound vector. \n
+															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							int* nWSR,					/**< Input: Maximum number of working set recalculations when using initial homotopy.
+															 Output: Number of performed working set recalculations. */
+							real_t* const cputime 		/**< Input: Maximum CPU time allowed for QP initialisation. \n
+															 Output: CPU time spent for QP initialisation (if pointer passed). */
+							);
+
+
+/** Initialises a QP problem with given QP data and tries to solve it
+ *	using at most nWSR iterations.
+ *
+ *  Note: This function internally calls solveInitialQP for initialisation!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_TQ \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblem_init(	QProblem* _THIS,
+							real_t* const _H, 			/**< Hessian matrix. \n
+															 If Hessian matrix is trivial, a NULL pointer can be passed. */
+							const real_t* const _g, 	/**< Gradient vector. */
+							real_t* const _A,  			/**< Constraint matrix. */
+							const real_t* const _lb,	/**< Lower bound vector (on variables). \n
+															 If no lower bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ub,	/**< Upper bound vector (on variables). \n
+															 If no upper bounds exist, a NULL pointer can be passed. */
+							const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
+															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ubA,	/**< Upper constraints' bound vector. \n
+															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							int* nWSR,					/**< Input: Maximum number of working set recalculations when using initial homotopy.
+															 Output: Number of performed working set recalculations. */
+							real_t* const cputime 		/**< Input: Maximum CPU time allowed for QP initialisation. \n
+															 Output: CPU time spent for QP initialisation (if pointer passed). */
+							);
+
+/** Initialises a QP problem with given QP data to be read from files and tries to solve it
+ *	using at most nWSR iterations.
+ *
+ *  Note: This function internally calls solveInitialQP for initialisation!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_TQ \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED \n
+			RET_UNABLE_TO_READ_FILE */
+returnValue QProblem_initF(	QProblem* _THIS,
+							const char* const H_file,	/**< Name of file where Hessian matrix is stored. \n
+															 If Hessian matrix is trivial, a NULL pointer can be passed. */
+							const char* const g_file,	/**< Name of file where gradient vector is stored. */
+							const char* const A_file,	/**< Name of file where constraint matrix is stored. */
+							const char* const lb_file,	/**< Name of file where lower bound vector. \n
+															If no lower bounds exist, a NULL pointer can be passed. */
+							const char* const ub_file,	/**< Name of file where upper bound vector. \n
+															If no upper bounds exist, a NULL pointer can be passed. */
+							const char* const lbA_file,	/**< Name of file where lower constraints' bound vector. \n
+															If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							const char* const ubA_file,	/**< Name of file where upper constraints' bound vector. \n
+															 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+							int* nWSR,					/**< Input: Maximum number of working set recalculations when using initial homotopy.
+															 Output: Number of performed working set recalculations. */
+							real_t* const cputime	 	/**< Input: Maximum CPU time allowed for QP initialisation. \n
+															 Output: CPU time spent for QP initialisation (if pointer passed). */
+							);
+
+/** Initialises a QP problem with given QP data and tries to solve it
+ *	using at most nWSR iterations. Depending on the parameter constellation it: \n
+ *	1. 0,    0,    0    : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n
+ *	2. xOpt, 0,    0    : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n
+ *	3. 0,    yOpt, 0    : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n
+ *	4. 0,    0,    gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n
+ *	5. xOpt, yOpt, 0    : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n
+ *	6. xOpt, 0,    gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n
+ *	7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!)
+ *
+ *  Note: This function internally calls solveInitialQP for initialisation!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_TQ \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblem_initMW(	QProblem* _THIS,
+								DenseMatrix *_H, 						/**< Hessian matrix. \n
+														    				 If Hessian matrix is trivial, a NULL pointer can be passed. */
+								const real_t* const _g, 				/**< Gradient vector. */
+								DenseMatrix *_A, 			 			/**< Constraint matrix. */
+								const real_t* const _lb,				/**< Lower bound vector (on variables). \n
+																			 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const _ub,				/**< Upper bound vector (on variables). \n
+																			 If no upper bounds exist, a NULL pointer can be passed. */
+								const real_t* const _lbA,				/**< Lower constraints' bound vector. \n
+																			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const real_t* const _ubA,				/**< Upper constraints' bound vector. \n
+																			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								int* nWSR,								/**< Input: Maximum number of working set recalculations when using initial homotopy.
+																		 *	 Output: Number of performed working set recalculations. */
+								real_t* const cputime,					/**< Input: Maximum CPU time allowed for QP initialisation. \n
+																			 Output: CPU time spent for QP initialisation. */
+								const real_t* const xOpt,				/**< Optimal primal solution vector. \n
+																			 (If a null pointer is passed, the old primal solution is kept!) */
+								const real_t* const yOpt,				/**< Optimal dual solution vector. \n
+																			 (If a null pointer is passed, the old dual solution is kept!) */
+								Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). */
+								Constraints* const guessedConstraints,	/**< Optimal working set of constraints for solution (xOpt,yOpt). */
+								const real_t* const _R					/**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix.
+																	 		 The Cholesky factor must be stored in a real_t array of size nV*nV
+																			 in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n
+																			 (If a null pointer is passed, Cholesky decomposition is computed internally!) */
+								);
+
+/** Initialises a QP problem with given QP data and tries to solve it
+ *	using at most nWSR iterations. Depending on the parameter constellation it: \n
+ *	1. 0,    0,    0    : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n
+ *	2. xOpt, 0,    0    : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n
+ *	3. 0,    yOpt, 0    : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n
+ *	4. 0,    0,    gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n
+ *	5. xOpt, yOpt, 0    : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n
+ *	6. xOpt, 0,    gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n
+ *	7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!)
+ *
+ *  Note: This function internally calls solveInitialQP for initialisation!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_TQ \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblem_initW(	QProblem* _THIS,
+							real_t* const _H, 						/**< Hessian matrix. \n
+														    			 If Hessian matrix is trivial, a NULL pointer can be passed. */
+							const real_t* const _g, 				/**< Gradient vector. */
+							real_t* const _A,  						/**< Constraint matrix. */
+							const real_t* const _lb,				/**< Lower bound vector (on variables). \n
+																		 If no lower bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ub,				/**< Upper bound vector (on variables). \n
+																		 If no upper bounds exist, a NULL pointer can be passed. */
+							const real_t* const _lbA,				/**< Lower constraints' bound vector. \n
+																		 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ubA,				/**< Upper constraints' bound vector. \n
+																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							int* nWSR,								/**< Input:  Maximum number of working set recalculations when using initial homotopy.
+																	 *	 Output: Number of performed working set recalculations. */
+							real_t* const cputime,					/**< Input:  Maximum CPU time allowed for QP initialisation. \n
+																		 Output: CPU time spent for QP initialisation. */
+							const real_t* const xOpt,				/**< Optimal primal solution vector. \n
+																		 (If a null pointer is passed, the old primal solution is kept!) */
+							const real_t* const yOpt,				/**< Optimal dual solution vector. \n
+																		 (If a null pointer is passed, the old dual solution is kept!) */
+							Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). */
+							Constraints* const guessedConstraints,	/**< Optimal working set of constraints for solution (xOpt,yOpt). */
+							const real_t* const _R					/**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix.
+																	 	 The Cholesky factor must be stored in a real_t array of size nV*nV
+																		 in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n
+																		 (If a null pointer is passed, Cholesky decomposition is computed internally!) */
+							);
+
+/** Initialises a QP problem with given QP data to be ream from files and tries to solve it
+ *	using at most nWSR iterations. Depending on the parameter constellation it: \n
+ *	1. 0,    0,    0    : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n
+ *	2. xOpt, 0,    0    : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n
+ *	3. 0,    yOpt, 0    : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n
+ *	4. 0,    0,    gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n
+ *	5. xOpt, yOpt, 0    : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n
+ *	6. xOpt, 0,    gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n
+ *	7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!)
+ *
+ *  Note: This function internally calls solveInitialQP for initialisation!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_TQ \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED \n
+			RET_UNABLE_TO_READ_FILE */
+returnValue QProblem_initFW(	QProblem* _THIS,
+								const char* const H_file,				/**< Name of file where Hessian matrix is stored. \n
+														     				 If Hessian matrix is trivial, a NULL pointer can be passed. */
+								const char* const g_file,				/**< Name of file where gradient vector is stored. */
+								const char* const A_file,				/**< Name of file where constraint matrix is stored. */
+								const char* const lb_file,				/**< Name of file where lower bound vector. \n
+																			 If no lower bounds exist, a NULL pointer can be passed. */
+								const char* const ub_file,				/**< Name of file where upper bound vector. \n
+																			 If no upper bounds exist, a NULL pointer can be passed. */
+								const char* const lbA_file,				/**< Name of file where lower constraints' bound vector. \n
+																			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const char* const ubA_file,				/**< Name of file where upper constraints' bound vector. \n
+																			 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int* nWSR,								/**< Input:  Maximum number of working set recalculations when using initial homotopy.
+																			 Output: Number of performed working set recalculations. */
+								real_t* const cputime,					/**< Input:  Maximum CPU time allowed for QP initialisation. \n
+																			 Output: CPU time spent for QP initialisation. */
+								const real_t* const xOpt,				/**< Optimal primal solution vector. \n
+																			 (If a null pointer is passed, the old primal solution is kept!) */
+								const real_t* const yOpt,				/**< Optimal dual solution vector. \n
+																			 (If a null pointer is passed, the old dual solution is kept!) */
+								Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). */
+								Constraints* const guessedConstraints,	/**< Optimal working set of constraints for solution (xOpt,yOpt). */
+								const char* const R_file				/**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix.
+																		 	 The Cholesky factor must be stored in a real_t array of size nV*nV
+																			 in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n
+																			 (If a null pointer is passed, Cholesky decomposition is computed internally!) */
+								);
+
+/** Solves an initialised QP sequence using the online active set strategy.
+ *	QP solution is started from previous solution.
+ *
+ *  Note: This function internally calls solveQP/solveRegularisedQP
+ *        for solving an initialised QP!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MAX_NWSR_REACHED \n
+ 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+			RET_HOTSTART_FAILED \n
+			RET_SHIFT_DETERMINATION_FAILED \n
+			RET_STEPDIRECTION_DETERMINATION_FAILED \n
+			RET_STEPLENGTH_DETERMINATION_FAILED \n
+			RET_HOMOTOPY_STEP_FAILED \n
+			RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+returnValue QProblem_hotstart(	QProblem* _THIS,
+								const real_t* const g_new,		/**< Gradient of neighbouring QP to be solved. */
+								const real_t* const lb_new,		/**< Lower bounds of neighbouring QP to be solved. \n
+											 			 			 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const ub_new,		/**< Upper bounds of neighbouring QP to be solved. \n
+											 			 			 If no upper bounds exist, a NULL pointer can be passed. */
+								const real_t* const lbA_new,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+											 			 			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const real_t* const ubA_new,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n
+											 			 			 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int* nWSR,						/**< Input:  Maximum number of working set recalculations; \n
+													 				 Output: Number of performed working set recalculations. */
+								real_t* const cputime 			/**< Input:  Maximum CPU time allowed for QP solution. \n
+														 			 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+								);
+
+/** Solves an initialised QP sequence using the online active set strategy,
+ *	where QP data is read from files. QP solution is started from previous solution.
+ *
+ *  Note: This function internally calls solveQP/solveRegularisedQP
+ *        for solving an initialised QP!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MAX_NWSR_REACHED \n
+ 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+			RET_HOTSTART_FAILED \n
+			RET_SHIFT_DETERMINATION_FAILED \n
+			RET_STEPDIRECTION_DETERMINATION_FAILED \n
+			RET_STEPLENGTH_DETERMINATION_FAILED \n
+			RET_HOMOTOPY_STEP_FAILED \n
+			RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+			RET_UNABLE_TO_READ_FILE \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblem_hotstartF(	QProblem* _THIS,
+								const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+								const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+											 					 If no lower bounds exist, a NULL pointer can be passed. */
+								const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+											 					 If no upper bounds exist, a NULL pointer can be passed. */
+								const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
+											 					 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
+											 					 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int* nWSR, 					/**< Input:  Maximum number of working set recalculations; \n
+																 Output: Number of performed working set recalculations. */
+								real_t* const cputime 		/**< Input:  Maximum CPU time allowed for QP solution. \n
+														 		 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+								);
+
+/** Solves an initialised QP sequence using the online active set strategy.
+ *	By default, QP solution is started from previous solution. If a guess
+ *	for the working set is provided, an initialised homotopy is performed.
+ *
+ *  Note: This function internally calls solveQP/solveRegularisedQP
+ *        for solving an initialised QP!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MAX_NWSR_REACHED \n
+ 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+			RET_HOTSTART_FAILED \n
+			RET_SHIFT_DETERMINATION_FAILED \n
+			RET_STEPDIRECTION_DETERMINATION_FAILED \n
+			RET_STEPLENGTH_DETERMINATION_FAILED \n
+			RET_HOMOTOPY_STEP_FAILED \n
+			RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+			RET_SETUP_AUXILIARYQP_FAILED */
+returnValue QProblem_hotstartW(	QProblem* _THIS,
+								const real_t* const g_new,				/**< Gradient of neighbouring QP to be solved. */
+								const real_t* const lb_new,				/**< Lower bounds of neighbouring QP to be solved. \n
+											 						 		 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const ub_new,				/**< Upper bounds of neighbouring QP to be solved. \n
+											 						 		 If no upper bounds exist, a NULL pointer can be passed. */
+								const real_t* const lbA_new,			/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+											 						 		 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const real_t* const ubA_new,			/**< Upper constraints' bounds of neighbouring QP to be solved. \n
+											 								 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int* nWSR,								/**< Input: Maximum number of working set recalculations; \n
+																			 Output: Number of performed working set recalculations. */
+								real_t* const cputime,					/**< Input: Maximum CPU time allowed for QP solution. \n
+																	 		 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+								Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). \n
+																			 (If a null pointer is passed, the previous working set of bounds is kept!) */
+								Constraints* const guessedConstraints	/**< Optimal working set of constraints for solution (xOpt,yOpt). \n
+																			 (If a null pointer is passed, the previous working set of constraints is kept!) */
+								);
+
+/** Solves an initialised QP sequence using the online active set strategy,
+ *  where QP data is read from files.
+ *	By default, QP solution is started from previous solution. If a guess
+ *	for the working set is provided, an initialised homotopy is performed.
+ *
+ *  Note: This function internally calls solveQP/solveRegularisedQP
+ *        for solving an initialised QP!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MAX_NWSR_REACHED \n
+ 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+			RET_HOTSTART_FAILED \n
+			RET_SHIFT_DETERMINATION_FAILED \n
+			RET_STEPDIRECTION_DETERMINATION_FAILED \n
+			RET_STEPLENGTH_DETERMINATION_FAILED \n
+			RET_HOMOTOPY_STEP_FAILED \n
+			RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+			RET_SETUP_AUXILIARYQP_FAILED \n
+			RET_UNABLE_TO_READ_FILE \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblem_hotstartFW(	QProblem* _THIS,
+									const char* const g_file, 				/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+									const char* const lb_file, 				/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+											 									 If no lower bounds exist, a NULL pointer can be passed. */
+									const char* const ub_file, 				/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+											 									 If no upper bounds exist, a NULL pointer can be passed. */
+									const char* const lbA_file, 			/**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
+											 									 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+									const char* const ubA_file, 			/**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
+											 									 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+									int* nWSR,								/**< Input: Maximum number of working set recalculations; \n
+																 				 Output: Number of performed working set recalculations. */
+									real_t* const cputime,					/**< Input: Maximum CPU time allowed for QP solution. \n
+																	 			 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+									Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). \n
+																				 (If a null pointer is passed, the previous working set of bounds is kept!) */
+									Constraints* const guessedConstraints	/**< Optimal working set of constraints for solution (xOpt,yOpt). \n
+																				 (If a null pointer is passed, the previous working set of constraints is kept!) */
+									);
+
+
+/** Solves using the current working set
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_STEPDIRECTION_FAILED_TQ \n
+ *			RET_STEPDIRECTION_FAILED_CHOLESKY \n
+ *			RET_INVALID_ARGUMENTS */
+returnValue QProblem_solveCurrentEQP (	QProblem* _THIS,
+										const int n_rhs,		/**< Number of consecutive right hand sides */
+										const real_t* g_in,		/**< Gradient of neighbouring QP to be solved. */
+										const real_t* lb_in,	/**< Lower bounds of neighbouring QP to be solved. \n
+																	 If no lower bounds exist, a NULL pointer can be passed. */
+										const real_t* ub_in,	/**< Upper bounds of neighbouring QP to be solved. \n
+																	 If no upper bounds exist, a NULL pointer can be passed. */
+										const real_t* lbA_in,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+										const real_t* ubA_in,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n */
+										real_t* x_out,			/**< Output: Primal solution */
+										real_t* y_out			/**< Output: Dual solution */
+										);
+
+
+
+/** Returns current constraints object of the QP (deep copy).
+  *	\return SUCCESSFUL_RETURN \n
+  			RET_QPOBJECT_NOT_SETUP */
+static inline returnValue QProblem_getConstraints(	QProblem* _THIS,
+													Constraints* _constraints	/** Output: Constraints object. */
+													);
+
+
+/** Returns the number of constraints.
+ *	\return Number of constraints. */
+static inline int QProblem_getNC( QProblem* _THIS );
+
+/** Returns the number of (implicitly defined) equality constraints.
+ *	\return Number of (implicitly defined) equality constraints. */
+static inline int QProblem_getNEC( QProblem* _THIS );
+
+/** Returns the number of active constraints.
+ *	\return Number of active constraints. */
+static inline int QProblem_getNAC( QProblem* _THIS );
+
+/** Returns the number of inactive constraints.
+ *	\return Number of inactive constraints. */
+static inline int QProblem_getNIAC( QProblem* _THIS );
+
+/** Returns the dimension of null space.
+ *	\return Dimension of null space. */
+int QProblem_getNZ( QProblem* _THIS );
+
+
+/** Returns the dual solution vector (deep copy).
+ *	\return SUCCESSFUL_RETURN \n
+			RET_QP_NOT_SOLVED */
+returnValue QProblem_getDualSolution(	QProblem* _THIS,
+										real_t* const yOpt	/**< Output: Dual solution vector (if QP has been solved). */
+										);
+
+
+/** Defines user-defined routine for calculating the constraint product A*x
+ *	\return  SUCCESSFUL_RETURN \n */
+returnValue QProblem_setConstraintProduct(	QProblem* _THIS,
+											ConstraintProduct _constraintProduct
+											);
+
+
+/** Prints concise list of properties of the current QP.
+ *	\return  SUCCESSFUL_RETURN \n */
+returnValue QProblem_printProperties( QProblem* _THIS );
+
+
+
+/** Writes a vector with the state of the working set
+*	\return SUCCESSFUL_RETURN */
+returnValue QProblem_getWorkingSet(	QProblem* _THIS,
+									real_t* workingSet	/** Output: array containing state of the working set. */
+									);
+
+/** Writes a vector with the state of the working set of bounds
+ *	\return SUCCESSFUL_RETURN \n
+ *	        RET_INVALID_ARGUMENTS */
+returnValue QProblem_getWorkingSetBounds(	QProblem* _THIS,
+											real_t* workingSetB	/** Output: array containing state of the working set of bounds. */
+											);
+
+/** Writes a vector with the state of the working set of constraints
+ *	\return SUCCESSFUL_RETURN \n
+ *	        RET_INVALID_ARGUMENTS */
+returnValue QProblem_getWorkingSetConstraints(	QProblem* _THIS,
+												real_t* workingSetC	/** Output: array containing state of the working set of constraints. */
+												);
+
+
+/** Returns current bounds object of the QP (deep copy).
+  *	\return SUCCESSFUL_RETURN \n
+  			RET_QPOBJECT_NOT_SETUP */
+static inline returnValue QProblem_getBounds(	QProblem* _THIS,
+												Bounds* _bounds		/** Output: Bounds object. */
+												);
+
+
+/** Returns the number of variables.
+ *	\return Number of variables. */
+static inline int QProblem_getNV( QProblem* _THIS );
+
+/** Returns the number of free variables.
+ *	\return Number of free variables. */
+static inline int QProblem_getNFR( QProblem* _THIS );
+
+/** Returns the number of fixed variables.
+ *	\return Number of fixed variables. */
+static inline int QProblem_getNFX( QProblem* _THIS );
+
+/** Returns the number of implicitly fixed variables.
+ *	\return Number of implicitly fixed variables. */
+static inline int QProblem_getNFV( QProblem* _THIS );
+
+
+/** Returns the optimal objective function value.
+ *	\return finite value: Optimal objective function value (QP was solved) \n
+ 			+infinity:	  QP was not yet solved */
+real_t QProblem_getObjVal( QProblem* _THIS );
+
+/** Returns the objective function value at an arbitrary point x.
+ *	\return Objective function value at point x */
+real_t QProblem_getObjValX(	QProblem* _THIS,
+							const real_t* const _x	/**< Point at which the objective function shall be evaluated. */
+							);
+
+/** Returns the primal solution vector.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_QP_NOT_SOLVED */
+returnValue QProblem_getPrimalSolution(	QProblem* _THIS,
+										real_t* const xOpt	/**< Output: Primal solution vector (if QP has been solved). */
+										);
+
+
+/** Returns status of the solution process.
+ *	\return Status of solution process. */
+static inline QProblemStatus QProblem_getStatus( QProblem* _THIS );
+
+
+/** Returns if the QProblem object is initialised.
+ *	\return BT_TRUE:  QProblem initialised \n
+ 			BT_FALSE: QProblem not initialised */
+static inline BooleanType QProblem_isInitialised( QProblem* _THIS );
+
+/** Returns if the QP has been solved.
+ *	\return BT_TRUE:  QProblem solved \n
+ 			BT_FALSE: QProblem not solved */
+static inline BooleanType QProblem_isSolved( QProblem* _THIS );
+
+/** Returns if the QP is infeasible.
+ *	\return BT_TRUE:  QP infeasible \n
+ 			BT_FALSE: QP feasible (or not known to be infeasible!) */
+static inline BooleanType QProblem_isInfeasible( QProblem* _THIS );
+
+/** Returns if the QP is unbounded.
+ *	\return BT_TRUE:  QP unbounded \n
+ 			BT_FALSE: QP unbounded (or not known to be unbounded!) */
+static inline BooleanType QProblem_isUnbounded( QProblem* _THIS );
+
+
+/** Returns Hessian type flag (type is not determined due to _THIS call!).
+ *	\return Hessian type. */
+static inline HessianType QProblem_getHessianType( QProblem* _THIS );
+
+/** Changes the print level.
+ *	\return SUCCESSFUL_RETURN */
+static inline returnValue QProblem_setHessianType(	QProblem* _THIS,
+													HessianType _hessianType /**< New Hessian type. */
+													);
+
+/** Returns if the QP has been internally regularised.
+ *	\return BT_TRUE:  Hessian is internally regularised for QP solution \n
+ 			BT_FALSE: No internal Hessian regularisation is used for QP solution */
+static inline BooleanType QProblem_usingRegularisation( QProblem* _THIS );
+
+/** Returns current options struct.
+ *	\return Current options struct. */
+static inline Options QProblem_getOptions( QProblem* _THIS );
+
+/** Overrides current options with given ones.
+ *	\return SUCCESSFUL_RETURN */
+static inline returnValue QProblem_setOptions(	QProblem* _THIS,
+												Options _options	/**< New options. */
+												);
+
+/** Returns the print level.
+ *	\return Print level. */
+static inline PrintLevel QProblem_getPrintLevel( QProblem* _THIS );
+
+/** Changes the print level.
+ *	\return SUCCESSFUL_RETURN */
+returnValue QProblem_setPrintLevel(	QProblem* _THIS,
+									PrintLevel _printlevel	/**< New print level. */
+									);
+
+
+/** Returns the current number of QP problems solved.
+ *	\return Number of QP problems solved. */
+static inline unsigned int QProblem_getCount( QProblem* _THIS );
+
+/** Resets QP problem counter (to zero).
+ *	\return SUCCESSFUL_RETURN. */
+static inline returnValue QProblem_resetCounter( QProblem* _THIS );
+
+
+/** Prints a list of all options and their current values.
+ *	\return  SUCCESSFUL_RETURN \n */
+returnValue QProblem_printOptions( QProblem* _THIS );
+
+
+/** Solves a QProblem whose QP data is assumed to be stored in the member variables.
+ *  A guess for its primal/dual optimal solution vectors and the corresponding
+ *  working sets of bounds and constraints can be provided.
+ *  Note: This function is internally called by all init functions!
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_TQ \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED */
+returnValue QProblem_solveInitialQP(	QProblem* _THIS,
+										const real_t* const xOpt,				/**< Optimal primal solution vector.*/
+										const real_t* const yOpt,				/**< Optimal dual solution vector. */
+										Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). */
+										Constraints* const guessedConstraints,	/**< Optimal working set of constraints for solution (xOpt,yOpt). */
+										const real_t* const _R,					/**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. */
+										int* nWSR, 								/**< Input:  Maximum number of working set recalculations; \n
+														 						 *	 Output: Number of performed working set recalculations. */
+										real_t* const cputime					/**< Input:  Maximum CPU time allowed for QP solution. \n
+																	 			 *	 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+										);
+
+/** Solves QProblem using online active set strategy.
+ *  Note: This function is internally called by all hotstart functions!
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MAX_NWSR_REACHED \n
+ 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+			RET_HOTSTART_FAILED \n
+			RET_SHIFT_DETERMINATION_FAILED \n
+			RET_STEPDIRECTION_DETERMINATION_FAILED \n
+			RET_STEPLENGTH_DETERMINATION_FAILED \n
+			RET_HOMOTOPY_STEP_FAILED \n
+			RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+returnValue QProblem_solveQP(	QProblem* _THIS,
+								const real_t* const g_new,		/**< Gradient of neighbouring QP to be solved. */
+								const real_t* const lb_new,		/**< Lower bounds of neighbouring QP to be solved. \n
+											 			 			 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const ub_new,		/**< Upper bounds of neighbouring QP to be solved. \n
+											 			 			 If no upper bounds exist, a NULL pointer can be passed. */
+								const real_t* const lbA_new,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+											 			 			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const real_t* const ubA_new,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n
+											 			 			 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int* nWSR,						/**< Input: Maximum number of working set recalculations; \n
+													 				 Output: Number of performed working set recalculations. */
+								real_t* const cputime,			/**< Input: Maximum CPU time allowed for QP solution. \n
+														 			 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+								int  nWSRperformed,				/**< Number of working set recalculations already performed to solve
+																	 this QP within previous solveQP() calls. This number is
+																	 always zero, except for successive calls from solveRegularisedQP()
+																	 or when using the far bound strategy. */
+								BooleanType isFirstCall			/**< Indicating whether this is the first call for current QP. */
+								);
+
+
+/** Solves QProblem using online active set strategy.
+ *  Note: This function is internally called by all hotstart functions!
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MAX_NWSR_REACHED \n
+ 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+			RET_HOTSTART_FAILED \n
+			RET_SHIFT_DETERMINATION_FAILED \n
+			RET_STEPDIRECTION_DETERMINATION_FAILED \n
+			RET_STEPLENGTH_DETERMINATION_FAILED \n
+			RET_HOMOTOPY_STEP_FAILED \n
+			RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+returnValue QProblem_solveRegularisedQP(	QProblem* _THIS,
+											const real_t* const g_new,		/**< Gradient of neighbouring QP to be solved. */
+											const real_t* const lb_new,		/**< Lower bounds of neighbouring QP to be solved. \n
+													 			 				 If no lower bounds exist, a NULL pointer can be passed. */
+											const real_t* const ub_new,		/**< Upper bounds of neighbouring QP to be solved. \n
+													 			 				 If no upper bounds exist, a NULL pointer can be passed. */
+											const real_t* const lbA_new,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+													 			 				 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+											const real_t* const ubA_new,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n
+													 			 				 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+											int* nWSR,						/**< Input: Maximum number of working set recalculations; \n
+															 					 Output: Number of performed working set recalculations. */
+											real_t* const cputime,			/**< Input: Maximum CPU time allowed for QP solution. \n
+																 				 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+											int  nWSRperformed,				/**< Number of working set recalculations already performed to solve
+																				 this QP within previous solveRegularisedQP() calls. This number is
+																				 always zero, except for successive calls when using the far bound strategy. */
+											BooleanType isFirstCall			/**< Indicating whether this is the first call for current QP. */
+											);
+
+
+/** Determines type of existing constraints and bounds (i.e. implicitly fixed, unbounded etc.).
+ *	\return SUCCESSFUL_RETURN \n
+			RET_SETUPSUBJECTTOTYPE_FAILED */
+returnValue QProblem_setupSubjectToType( QProblem* _THIS );
+
+/** Determines type of new constraints and bounds (i.e. implicitly fixed, unbounded etc.).
+ *	\return SUCCESSFUL_RETURN \n
+			RET_SETUPSUBJECTTOTYPE_FAILED */
+returnValue QProblem_setupSubjectToTypeNew(	QProblem* _THIS,
+											const real_t* const lb_new,		/**< New lower bounds. */
+											const real_t* const ub_new,		/**< New upper bounds. */
+											const real_t* const lbA_new,	/**< New lower constraints' bounds. */
+											const real_t* const ubA_new		/**< New upper constraints' bounds. */
+											);
+
+/** Computes the Cholesky decomposition of the projected Hessian (i.e. R^T*R = Z^T*H*Z).
+ *  Note: If Hessian turns out not to be positive definite, the Hessian type
+ *		  is set to HST_SEMIDEF accordingly.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_HESSIAN_NOT_SPD \n
+ *			RET_INDEXLIST_CORRUPTED */
+returnValue QProblem_computeProjectedCholesky( QProblem* _THIS );
+
+/** Computes initial Cholesky decomposition of the projected Hessian making
+ *  use of the function setupCholeskyDecomposition() or setupCholeskyDecompositionProjected().
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_HESSIAN_NOT_SPD \n
+ *			RET_INDEXLIST_CORRUPTED */
+returnValue QProblem_setupInitialCholesky( QProblem* _THIS );
+
+/** Initialises TQ factorisation of A (i.e. A*Q = [0 T]) if NO constraint is active.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INDEXLIST_CORRUPTED */
+returnValue QProblem_setupTQfactorisation( QProblem* _THIS );
+
+
+/** Obtains the desired working set for the auxiliary initial QP in
+ *  accordance with the user specifications
+ *  (assumes that member AX has already been initialised!)
+ *	\return SUCCESSFUL_RETURN \n
+			RET_OBTAINING_WORKINGSET_FAILED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblem_obtainAuxiliaryWorkingSet(	QProblem* _THIS,
+												const real_t* const xOpt,					/**< Optimal primal solution vector.
+																							 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
+												const real_t* const yOpt,					/**< Optimal dual solution vector.
+																							 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
+												Bounds* const guessedBounds,				/**< Guessed working set of bounds for solution (xOpt,yOpt). */
+												Constraints* const guessedConstraints,		/**< Guessed working set for solution (xOpt,yOpt). */
+												Bounds* auxiliaryBounds,					/**< Input: Allocated bound object. \n
+																							 *	 Ouput: Working set of constraints for auxiliary QP. */
+												Constraints* auxiliaryConstraints			/**< Input: Allocated bound object. \n
+																							 *	 Ouput: Working set for auxiliary QP. */
+												);
+
+/** Sets up bound and constraints data structures according to auxiliaryBounds/Constraints.
+ *  (If the working set shall be setup afresh, make sure that
+ *  bounds and constraints data structure have been resetted
+ *  and the TQ factorisation has been initialised!)
+ *	\return SUCCESSFUL_RETURN \n
+			RET_SETUP_WORKINGSET_FAILED \n
+			RET_INVALID_ARGUMENTS \n
+			RET_UNKNOWN_BUG */
+returnValue QProblem_setupAuxiliaryWorkingSet(	QProblem* _THIS,
+												Bounds* const auxiliaryBounds,				/**< Working set of bounds for auxiliary QP. */
+												Constraints* const auxiliaryConstraints,	/**< Working set of constraints for auxiliary QP. */
+												BooleanType setupAfresh						/**< Flag indicating if given working set shall be
+																							 *    setup afresh or by updating the current one. */
+												);
+
+/** Sets up the optimal primal/dual solution of the auxiliary initial QP.
+ *	\return SUCCESSFUL_RETURN */
+returnValue QProblem_setupAuxiliaryQPsolution(	QProblem* _THIS,
+												const real_t* const xOpt,	/**< Optimal primal solution vector.
+																		 	 *	 If a NULL pointer is passed, all entries are set to zero. */
+												const real_t* const yOpt	/**< Optimal dual solution vector.
+																			 *	 If a NULL pointer is passed, all entries are set to zero. */
+												);
+
+/** Sets up gradient of the auxiliary initial QP for given
+ *  optimal primal/dual solution and given initial working set
+ *  (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
+ *	\return SUCCESSFUL_RETURN */
+returnValue QProblem_setupAuxiliaryQPgradient( QProblem* _THIS );
+
+/** Sets up (constraints') bounds of the auxiliary initial QP for given
+ *  optimal primal/dual solution and given initial working set
+ *  (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
+ *	\return SUCCESSFUL_RETURN \n
+			RET_UNKNOWN_BUG */
+returnValue QProblem_setupAuxiliaryQPbounds(	QProblem* _THIS,
+												Bounds* const auxiliaryBounds,				/**< Working set of bounds for auxiliary QP. */
+												Constraints* const auxiliaryConstraints,	/**< Working set of constraints for auxiliary QP. */
+												BooleanType useRelaxation					/**< Flag indicating if inactive (constraints') bounds shall be relaxed. */
+												);
+
+
+/** Adds a constraint to active set.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_ADDCONSTRAINT_FAILED \n
+			RET_ADDCONSTRAINT_FAILED_INFEASIBILITY \n
+			RET_ENSURELI_FAILED */
+returnValue QProblem_addConstraint(	QProblem* _THIS,
+									int number,					/**< Number of constraint to be added to active set. */
+									SubjectToStatus C_status,	/**< Status of new active constraint. */
+									BooleanType updateCholesky,	/**< Flag indicating if Cholesky decomposition shall be updated. */
+									BooleanType ensureLI		/**< Ensure linear independence by exchange rules by default. */
+									);
+
+/** Checks if new active constraint to be added is linearly dependent from
+ *	from row of the active constraints matrix.
+ *	\return	 RET_LINEARLY_DEPENDENT \n
+ 			 RET_LINEARLY_INDEPENDENT \n
+			 RET_INDEXLIST_CORRUPTED */
+returnValue QProblem_addConstraint_checkLI(	QProblem* _THIS,
+											int number			/**< Number of constraint to be added to active set. */
+											);
+
+/** Ensures linear independence of constraint matrix when a new constraint is added.
+ * 	To _THIS end a bound or constraint is removed simultaneously if necessary.
+ *	\return	 SUCCESSFUL_RETURN \n
+ 			 RET_LI_RESOLVED \n
+			 RET_ENSURELI_FAILED \n
+			 RET_ENSURELI_FAILED_TQ \n
+			 RET_ENSURELI_FAILED_NOINDEX \n
+			 RET_REMOVE_FROM_ACTIVESET */
+returnValue QProblem_addConstraint_ensureLI(	QProblem* _THIS,
+												int number,					/**< Number of constraint to be added to active set. */
+												SubjectToStatus C_status	/**< Status of new active bound. */
+												);
+
+/** Adds a bound to active set.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_ADDBOUND_FAILED \n
+			RET_ADDBOUND_FAILED_INFEASIBILITY \n
+			RET_ENSURELI_FAILED */
+returnValue QProblem_addBound(	QProblem* _THIS,
+								int number,					/**< Number of bound to be added to active set. */
+								SubjectToStatus B_status,	/**< Status of new active bound. */
+								BooleanType updateCholesky,	/**< Flag indicating if Cholesky decomposition shall be updated. */
+								BooleanType ensureLI 		/**< Ensure linear independence by exchange rules by default. */
+								);
+
+/** Checks if new active bound to be added is linearly dependent from
+ *	from row of the active constraints matrix.
+ *	\return	 RET_LINEARLY_DEPENDENT \n
+ 			 RET_LINEARLY_INDEPENDENT */
+returnValue QProblem_addBound_checkLI(	QProblem* _THIS,
+										int number			/**< Number of bound to be added to active set. */
+										);
+
+/** Ensures linear independence of constraint matrix when a new bound is added.
+ *	To _THIS end a bound or constraint is removed simultaneously if necessary.
+ *	\return	 SUCCESSFUL_RETURN \n
+ 			 RET_LI_RESOLVED \n
+			 RET_ENSURELI_FAILED \n
+			 RET_ENSURELI_FAILED_TQ \n
+			 RET_ENSURELI_FAILED_NOINDEX \n
+			 RET_REMOVE_FROM_ACTIVESET */
+returnValue QProblem_addBound_ensureLI(	QProblem* _THIS,
+										int number,					/**< Number of bound to be added to active set. */
+										SubjectToStatus B_status	/**< Status of new active bound. */
+										);
+
+/** Removes a constraint from active set.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_CONSTRAINT_NOT_ACTIVE \n
+			RET_REMOVECONSTRAINT_FAILED \n
+			RET_HESSIAN_NOT_SPD */
+returnValue QProblem_removeConstraint(	QProblem* _THIS,
+										int number,						/**< Number of constraint to be removed from active set. */
+										BooleanType updateCholesky,		/**< Flag indicating if Cholesky decomposition shall be updated. */
+										BooleanType allowFlipping,		/**< Flag indicating if flipping bounds are allowed. */
+										BooleanType ensureNZC			/**< Flag indicating if non-zero curvature is ensured by exchange rules. */
+										);
+
+/** Removes a bounds from active set.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_BOUND_NOT_ACTIVE \n
+			RET_HESSIAN_NOT_SPD \n
+			RET_REMOVEBOUND_FAILED */
+returnValue QProblem_removeBound(	QProblem* _THIS,
+									int number,						/**< Number of bound to be removed from active set. */
+									BooleanType updateCholesky,		/**< Flag indicating if Cholesky decomposition shall be updated. */
+									BooleanType allowFlipping,		/**< Flag indicating if flipping bounds are allowed. */
+									BooleanType ensureNZC			/**< Flag indicating if non-zero curvature is ensured by exchange rules. */
+									);
+
+
+/** Performs robustified ratio test yield the maximum possible step length
+ *  along the homotopy path.
+ *	\return  SUCCESSFUL_RETURN */
+returnValue QProblem_performPlainRatioTest(	QProblem* _THIS,
+											int nIdx, 					/**< Number of ratios to be checked. */
+											const int* const idxList, 	/**< Array containing the indices of all ratios to be checked. */
+											const real_t* const num,	/**< Array containing all numerators for performing the ratio test. */
+											const real_t* const den,	/**< Array containing all denominators for performing the ratio test. */
+											real_t epsNum,				/**< Numerator tolerance. */
+											real_t epsDen,				/**< Denominator tolerance. */
+											real_t* t,					/**< Output: Maximum possible step length along the homotopy path. */
+											int* BC_idx 				/**< Output: Index of blocking constraint. */
+											);
+
+
+/** Ensure non-zero curvature by primal jump.
+ *  \return SUCCESSFUL_RETURN \n
+ *          RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+returnValue QProblem_ensureNonzeroCurvature(	QProblem* _THIS,
+												BooleanType removeBoundNotConstraint,	/**< SubjectTo to be removed is a bound. */
+												int remIdx,								/**< Index of bound/constraint to be removed. */
+												BooleanType* exchangeHappened,			/**< Output: Exchange was necessary to ensure. */
+												BooleanType* addBoundNotConstraint,		/**< SubjectTo to be added is a bound. */
+												int* addIdx,							/**< Index of bound/constraint to be added. */
+												SubjectToStatus* addStatus				/**< Status of bound/constraint to be added. */
+												);
+
+
+/** Solves the system Ta = b or T^Ta = b where T is a reverse upper triangular matrix.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_DIV_BY_ZERO */
+returnValue QProblem_backsolveT(	QProblem* _THIS,
+									const real_t* const b,	/**< Right hand side vector. */
+									BooleanType transposed,	/**< Indicates if the transposed system shall be solved. */
+									real_t* const a 		/**< Output: Solution vector */
+									);
+
+
+/** Determines step direction of the shift of the QP data.
+ *	\return SUCCESSFUL_RETURN */
+returnValue QProblem_determineDataShift(	QProblem* _THIS,
+											const real_t* const g_new,		/**< New gradient vector. */
+											const real_t* const lbA_new,	/**< New lower constraints' bounds. */
+											const real_t* const ubA_new,	/**< New upper constraints' bounds. */
+											const real_t* const lb_new,		/**< New lower bounds. */
+											const real_t* const ub_new,		/**< New upper bounds. */
+											real_t* const delta_g,	 		/**< Output: Step direction of gradient vector. */
+											real_t* const delta_lbA,		/**< Output: Step direction of lower constraints' bounds. */
+											real_t* const delta_ubA,		/**< Output: Step direction of upper constraints' bounds. */
+											real_t* const delta_lb,	 		/**< Output: Step direction of lower bounds. */
+											real_t* const delta_ub,	 		/**< Output: Step direction of upper bounds. */
+											BooleanType* Delta_bC_isZero,	/**< Output: Indicates if active constraints' bounds are to be shifted. */
+											BooleanType* Delta_bB_isZero	/**< Output: Indicates if active bounds are to be shifted. */
+											);
+
+/** Determines step direction of the homotopy path.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_STEPDIRECTION_FAILED_TQ \n
+			RET_STEPDIRECTION_FAILED_CHOLESKY */
+returnValue QProblem_determineStepDirection(	QProblem* _THIS,
+												const real_t* const delta_g,	/**< Step direction of gradient vector. */
+												const real_t* const delta_lbA,	/**< Step direction of lower constraints' bounds. */
+												const real_t* const delta_ubA,	/**< Step direction of upper constraints' bounds. */
+												const real_t* const delta_lb,	/**< Step direction of lower bounds. */
+												const real_t* const delta_ub,	/**< Step direction of upper bounds. */
+												BooleanType Delta_bC_isZero, 	/**< Indicates if active constraints' bounds are to be shifted. */
+												BooleanType Delta_bB_isZero,	/**< Indicates if active bounds are to be shifted. */
+												real_t* const delta_xFX, 		/**< Output: Primal homotopy step direction of fixed variables. */
+												real_t* const delta_xFR,	 	/**< Output: Primal homotopy step direction of free variables. */
+												real_t* const delta_yAC, 		/**< Output: Dual homotopy step direction of active constraints' multiplier. */
+												real_t* const delta_yFX 		/**< Output: Dual homotopy step direction of fixed variables' multiplier. */
+												);
+
+/** Determines the maximum possible step length along the homotopy path
+ *  and performs _THIS step (without changing working set).
+ *	\return SUCCESSFUL_RETURN \n
+ * 			RET_ERROR_IN_CONSTRAINTPRODUCT \n
+ * 			RET_QP_INFEASIBLE */
+returnValue QProblem_performStep(	QProblem* _THIS,
+									const real_t* const delta_g,		/**< Step direction of gradient. */
+									const real_t* const delta_lbA,		/**< Step direction of lower constraints' bounds. */
+									const real_t* const delta_ubA,		/**< Step direction of upper constraints' bounds. */
+									const real_t* const delta_lb,	 	/**< Step direction of lower bounds. */
+									const real_t* const delta_ub,	 	/**< Step direction of upper bounds. */
+									const real_t* const delta_xFX, 		/**< Primal homotopy step direction of fixed variables. */
+									const real_t* const delta_xFR,		/**< Primal homotopy step direction of free variables. */
+									const real_t* const delta_yAC,		/**< Dual homotopy step direction of active constraints' multiplier. */
+									const real_t* const delta_yFX,		/**< Dual homotopy step direction of fixed variables' multiplier. */
+									int* BC_idx, 						/**< Output: Index of blocking constraint. */
+									SubjectToStatus* BC_status,			/**< Output: Status of blocking constraint. */
+									BooleanType* BC_isBound 			/**< Output: Indicates if blocking constraint is a bound. */
+									);
+
+/** Updates the active set.
+ *	\return  SUCCESSFUL_RETURN \n
+ 			 RET_REMOVE_FROM_ACTIVESET_FAILED \n
+			 RET_ADD_TO_ACTIVESET_FAILED */
+returnValue QProblem_changeActiveSet(	QProblem* _THIS,
+										int BC_idx, 				/**< Index of blocking constraint. */
+										SubjectToStatus BC_status,	/**< Status of blocking constraint. */
+										BooleanType BC_isBound 		/**< Indicates if blocking constraint is a bound. */
+										);
+
+
+/** Compute relative length of homotopy in data space for termination
+ *  criterion.
+ *  \return Relative length in data space. */
+real_t QProblem_getRelativeHomotopyLength(	QProblem* _THIS,
+											const real_t* const g_new,		/**< Final gradient. */
+											const real_t* const lb_new,		/**< Final lower variable bounds. */
+											const real_t* const ub_new,		/**< Final upper variable bounds. */
+											const real_t* const lbA_new,	/**< Final lower constraint bounds. */
+											const real_t* const ubA_new		/**< Final upper constraint bounds. */
+											);
+
+
+/** Ramping Strategy to avoid ties. Modifies homotopy start without
+ *  changing current active set.
+ *  \return SUCCESSFUL_RETURN */
+returnValue QProblem_performRamping( QProblem* _THIS );
+
+
+/** ... */
+returnValue QProblem_updateFarBounds(	QProblem* _THIS,
+										real_t curFarBound,				/**< ... */
+										int nRamp,						/**< ... */
+										const real_t* const lb_new,		/**< ... */
+										real_t* const lb_new_far,		/**< ... */
+										const real_t* const ub_new,		/**< ... */
+										real_t* const ub_new_far,		/**< ... */
+										const real_t* const lbA_new,	/**< ... */
+										real_t* const lbA_new_far,		/**< ... */
+										const real_t* const ubA_new,	/**< ... */
+										real_t* const ubA_new_far		/**< ... */
+										);
+
+/** ... */
+returnValue QProblemBCPY_updateFarBounds(	QProblem* _THIS,
+											real_t curFarBound,				/**< ... */
+											int nRamp,						/**< ... */
+											const real_t* const lb_new,		/**< ... */
+											real_t* const lb_new_far,		/**< ... */
+											const real_t* const ub_new,		/**< ... */
+											real_t* const ub_new_far		/**< ... */
+											);
+
+
+
+/** Performs robustified ratio test yield the maximum possible step length
+ *  along the homotopy path.
+ *	\return  SUCCESSFUL_RETURN */
+returnValue QProblem_performRatioTestC(	QProblem* _THIS,
+										int nIdx, 						/**< Number of ratios to be checked. */
+										const int* const idxList, 		/**< Array containing the indices of all ratios to be checked. */
+										Constraints* const subjectTo,	/**< Constraint object corresponding to ratios to be checked. */
+										const real_t* const num,	 	/**< Array containing all numerators for performing the ratio test. */
+										const real_t* const den,		/**< Array containing all denominators for performing the ratio test. */
+										real_t epsNum,					/**< Numerator tolerance. */
+										real_t epsDen,					/**< Denominator tolerance. */
+										real_t* t,						/**< Output: Maximum possible step length along the homotopy path. */
+										int* BC_idx 					/**< Output: Index of blocking constraint. */
+										);
+
+
+/** Drift correction at end of each active set iteration
+ *  \return SUCCESSFUL_RETURN */
+returnValue QProblem_performDriftCorrection( QProblem* _THIS );
+
+
+/** Updates QP vectors, working sets and internal data structures in order to
+	start from an optimal solution corresponding to initial guesses of the working
+	set for bounds and constraints.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_SETUP_AUXILIARYQP_FAILED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblem_setupAuxiliaryQP(	QProblem* _THIS,
+										Bounds* const guessedBounds,			/**< Initial guess for working set of bounds. */
+										Constraints* const guessedConstraints	/**< Initial guess for working set of constraints. */
+										);
+
+/** Determines if it is more efficient to refactorise the matrices when
+ *  hotstarting or not (i.e. better to update the existing factorisations).
+ *	\return BT_TRUE iff matrices shall be refactorised afresh
+ */
+BooleanType QProblem_shallRefactorise(	QProblem* _THIS,
+										Bounds* const guessedBounds,			/**< Guessed new working set of bounds. */
+										Constraints* const guessedConstraints	/**< Guessed new working set of constraints. */
+										);
+
+/** Setups internal QP data.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INVALID_ARGUMENTS \n
+			RET_UNKNONW_BUG */
+returnValue QProblem_setupQPdataM(	QProblem* _THIS,
+									DenseMatrix *_H, 			/**< Hessian matrix. \n
+																	 If Hessian matrix is trivial,a NULL pointer can be passed. */
+									const real_t* const _g, 	/**< Gradient vector. */
+									DenseMatrix *_A, 			 /**< Constraint matrix. */
+									const real_t* const _lb,	/**< Lower bound vector (on variables). \n
+																	 If no lower bounds exist, a NULL pointer can be passed. */
+									const real_t* const _ub,	/**< Upper bound vector (on variables). \n
+																	 If no upper bounds exist, a NULL pointer can be passed. */
+									const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
+																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+									const real_t* const _ubA	/**< Upper constraints' bound vector. \n
+																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+									);
+
+
+/** Sets up dense internal QP data. If the current Hessian is trivial
+ *  (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
+ *  memory for Hessian is allocated and it is set to the given one.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INVALID_ARGUMENTS \n
+			RET_UNKNONW_BUG */
+returnValue QProblem_setupQPdata(	QProblem* _THIS,
+									real_t* const _H, 			/**< Hessian matrix. \n
+																	 If Hessian matrix is trivial,a NULL pointer can be passed. */
+									const real_t* const _g, 	/**< Gradient vector. */
+									real_t* const _A,  			/**< Constraint matrix. */
+									const real_t* const _lb,	/**< Lower bound vector (on variables). \n
+																	 If no lower bounds exist, a NULL pointer can be passed. */
+									const real_t* const _ub,	/**< Upper bound vector (on variables). \n
+																	 If no upper bounds exist, a NULL pointer can be passed. */
+									const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
+																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+									const real_t* const _ubA	/**< Upper constraints' bound vector. \n
+																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+									);
+
+/** Sets up internal QP data by loading it from files. If the current Hessian
+ *  is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
+ *  memory for Hessian is allocated and it is set to the given one.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_UNABLE_TO_OPEN_FILE \n
+			RET_UNABLE_TO_READ_FILE \n
+			RET_INVALID_ARGUMENTS \n
+			RET_UNKNONW_BUG */
+returnValue QProblem_setupQPdataFromFile(	QProblem* _THIS,
+											const char* const H_file, 	/**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n
+														     				 If Hessian matrix is trivial,a NULL pointer can be passed. */
+											const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+											const char* const A_file,	/**< Name of file where constraint matrix, of neighbouring QP to be solved, is stored. */
+											const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+										 			 						 If no lower bounds exist, a NULL pointer can be passed. */
+											const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+										 			 						 If no upper bounds exist, a NULL pointer can be passed. */
+											const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
+										 			 						 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+											const char* const ubA_file	/**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
+										 			 						 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+											);
+
+/** Loads new QP vectors from files (internal members are not affected!).
+ *	\return SUCCESSFUL_RETURN \n
+			RET_UNABLE_TO_OPEN_FILE \n
+			RET_UNABLE_TO_READ_FILE \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblem_loadQPvectorsFromFile(	QProblem* _THIS,
+											const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+											const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+										 			 						 If no lower bounds exist, a NULL pointer can be passed. */
+											const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+										 			 						 If no upper bounds exist, a NULL pointer can be passed. */
+											const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
+										 			 						 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+											const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
+										 			 						 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+											real_t* const g_new,		/**< Output: Gradient of neighbouring QP to be solved. */
+											real_t* const lb_new,		/**< Output: Lower bounds of neighbouring QP to be solved */
+											real_t* const ub_new,		/**< Output: Upper bounds of neighbouring QP to be solved */
+											real_t* const lbA_new,		/**< Output: Lower constraints' bounds of neighbouring QP to be solved */
+											real_t* const ubA_new		/**< Output: Upper constraints' bounds of neighbouring QP to be solved */
+											);
+
+
+/** Prints concise information on the current iteration.
+ *	\return  SUCCESSFUL_RETURN \n */
+returnValue QProblem_printIteration(	QProblem* _THIS,
+										int iter,					/**< Number of current iteration. */
+										int BC_idx, 				/**< Index of blocking constraint. */
+										SubjectToStatus BC_status,	/**< Status of blocking constraint. */
+										BooleanType BC_isBound,		/**< Indicates if blocking constraint is a bound. */
+										real_t homotopyLength,		/**< Current homotopy distance. */
+										BooleanType isFirstCall		/**< Indicating whether this is the first call for current QP. */
+ 										);
+
+
+/** Sets constraint matrix of the QP. \n
+	Note: Also internal vector Ax is recomputed!
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_INVALID_ARGUMENTS */
+static inline returnValue QProblem_setAM(	QProblem* _THIS,
+											DenseMatrix *A_new	/**< New constraint matrix. */
+											);
+
+/** Sets dense constraint matrix of the QP. \n
+	Note: Also internal vector Ax is recomputed!
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_INVALID_ARGUMENTS */
+static inline returnValue QProblem_setA(	QProblem* _THIS,
+											real_t* const A_new	/**< New dense constraint matrix (with correct dimension!). */
+											);
+
+
+/** Sets constraints' lower bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_QPOBJECT_NOT_SETUP */
+static inline returnValue QProblem_setLBA(	QProblem* _THIS,
+											const real_t* const lbA_new	/**< New constraints' lower bound vector (with correct dimension!). */
+											);
+
+/** Changes single entry of lower constraints' bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN  \n
+ *			RET_QPOBJECT_NOT_SETUP \n
+ *			RET_INDEX_OUT_OF_BOUNDS */
+static inline returnValue QProblem_setLBAn(	QProblem* _THIS,
+											int number,		/**< Number of entry to be changed. */
+											real_t value	/**< New value for entry of lower constraints' bound vector (with correct dimension!). */
+											);
+
+/** Sets constraints' upper bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_QPOBJECT_NOT_SETUP */
+static inline returnValue QProblem_setUBA(	QProblem* _THIS,
+											const real_t* const ubA_new	/**< New constraints' upper bound vector (with correct dimension!). */
+											);
+
+/** Changes single entry of upper constraints' bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN  \n
+ *			RET_QPOBJECT_NOT_SETUP \n
+ *			RET_INDEX_OUT_OF_BOUNDS */
+static inline returnValue QProblem_setUBAn(	QProblem* _THIS,
+											int number,		/**< Number of entry to be changed. */
+											real_t value	/**< New value for entry of upper constraints' bound vector (with correct dimension!). */
+											);
+
+
+/** Decides if lower bounds are smaller than upper bounds
+ *
+ * \return SUCCESSFUL_RETURN \n
+ * 		   RET_QP_INFEASIBLE */
+returnValue QProblem_areBoundsConsistent(	QProblem* _THIS,
+											const real_t* const lb,		/**< Vector of lower bounds*/
+											const real_t* const ub,		/**< Vector of upper bounds*/
+											const real_t* const lbA,	/**< Vector of lower constraints*/
+											const real_t* const ubA		/**< Vector of upper constraints*/
+											);
+
+
+/** Drops the blocking bound/constraint that led to infeasibility, or finds another
+ *  bound/constraint to drop according to drop priorities.
+ *  \return SUCCESSFUL_RETURN \n
+ */
+returnValue QProblem_dropInfeasibles ( 	QProblem* _THIS,
+										int BC_number,				/**< Number of the bound or constraint to be added */
+										SubjectToStatus BC_status, 	/**< New status of the bound or constraint to be added */
+										BooleanType BC_isBound,		/**< Whether a bound or a constraint is to be added */
+										real_t *xiB,
+										real_t *xiC
+										);
+
+
+/** If Hessian type has been set by the user, nothing is done.
+ *  Otherwise the Hessian type is set to HST_IDENTITY, HST_ZERO, or
+ *  HST_POSDEF (default), respectively.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_HESSIAN_INDEFINITE */
+returnValue QProblem_determineHessianType( QProblem* _THIS );
+
+/** Computes the Cholesky decomposition of the (simply projected) Hessian
+ *  (i.e. R^T*R = Z^T*H*Z). It only works in the case where Z is a simple
+ *  projection matrix!
+ *  Note: If Hessian turns out not to be positive definite, the Hessian type
+ *		  is set to HST_SEMIDEF accordingly.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_HESSIAN_NOT_SPD \n
+ *			RET_INDEXLIST_CORRUPTED */
+returnValue QProblemBCPY_computeCholesky( QProblem* _THIS );
+
+/** Obtains the desired working set for the auxiliary initial QP in
+ *  accordance with the user specifications
+ *	\return SUCCESSFUL_RETURN \n
+			RET_OBTAINING_WORKINGSET_FAILED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblemBCPY_obtainAuxiliaryWorkingSet(	QProblem* _THIS,
+													const real_t* const xOpt,		/**< Optimal primal solution vector.
+																					 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
+													const real_t* const yOpt,		/**< Optimal dual solution vector.
+																					 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
+													Bounds* const guessedBounds,	/**< Guessed working set for solution (xOpt,yOpt). */
+													Bounds* auxiliaryBounds			/**< Input:  Allocated bound object. \n
+																					 *	 Output: Working set for auxiliary QP. */
+													);
+
+
+/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_DIV_BY_ZERO */
+returnValue QProblem_backsolveR(	QProblem* _THIS,
+									const real_t* const b,	/**< Right hand side vector. */
+									BooleanType transposed,	/**< Indicates if the transposed system shall be solved. */
+									real_t* const a 		/**< Output: Solution vector */
+									);
+
+/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n
+ *  Special variant for the case that _THIS function is called from within "removeBound()".
+ *	\return SUCCESSFUL_RETURN \n
+			RET_DIV_BY_ZERO */
+returnValue QProblem_backsolveRrem(	QProblem* _THIS,
+									const real_t* const b,		/**< Right hand side vector. */
+									BooleanType transposed,		/**< Indicates if the transposed system shall be solved. */
+									BooleanType removingBound,	/**< Indicates if function is called from "removeBound()". */
+									real_t* const a 			/**< Output: Solution vector */
+									);
+
+
+/** Determines step direction of the shift of the QP data.
+ *	\return SUCCESSFUL_RETURN */
+returnValue QProblemBCPY_determineDataShift(	QProblem* _THIS,
+												const real_t* const g_new,		/**< New gradient vector. */
+												const real_t* const lb_new,		/**< New lower bounds. */
+												const real_t* const ub_new,		/**< New upper bounds. */
+												real_t* const delta_g,	 		/**< Output: Step direction of gradient vector. */
+												real_t* const delta_lb,	 		/**< Output: Step direction of lower bounds. */
+												real_t* const delta_ub,	 		/**< Output: Step direction of upper bounds. */
+												BooleanType* Delta_bB_isZero	/**< Output: Indicates if active bounds are to be shifted. */
+												);
+
+
+/** Sets up internal QP data.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblemBCPY_setupQPdataM(	QProblem* _THIS,
+										DenseMatrix *_H,	 		/**< Hessian matrix.*/
+										const real_t* const _g,		/**< Gradient vector. */
+										const real_t* const _lb,	/**< Lower bounds (on variables). \n
+																		 If no lower bounds exist, a NULL pointer can be passed. */
+										const real_t* const _ub		/**< Upper bounds (on variables). \n
+																		 If no upper bounds exist, a NULL pointer can be passed. */
+										);
+
+/** Sets up internal QP data. If the current Hessian is trivial
+ *  (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
+ *  memory for Hessian is allocated and it is set to the given one.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INVALID_ARGUMENTS \n
+			RET_NO_HESSIAN_SPECIFIED */
+returnValue QProblemBCPY_setupQPdata(	QProblem* _THIS,
+										real_t* const _H, 			/**< Hessian matrix. \n
+																		 If Hessian matrix is trivial,a NULL pointer can be passed. */
+										const real_t* const _g,		/**< Gradient vector. */
+										const real_t* const _lb,	/**< Lower bounds (on variables). \n
+																		 If no lower bounds exist, a NULL pointer can be passed. */
+										const real_t* const _ub		/**< Upper bounds (on variables). \n
+																		 If no upper bounds exist, a NULL pointer can be passed. */
+										);
+
+/** Sets up internal QP data by loading it from files. If the current Hessian
+ *  is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
+ *  memory for Hessian is allocated and it is set to the given one.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_UNABLE_TO_OPEN_FILE \n
+			RET_UNABLE_TO_READ_FILE \n
+			RET_INVALID_ARGUMENTS \n
+			RET_NO_HESSIAN_SPECIFIED */
+returnValue QProblemBCPY_setupQPdataFromFile(	QProblem* _THIS,
+												const char* const H_file, 	/**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n
+														     					 If Hessian matrix is trivial,a NULL pointer can be passed. */
+												const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+												const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+										 			 							 If no lower bounds exist, a NULL pointer can be passed. */
+												const char* const ub_file 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+										 			 							 If no upper bounds exist, a NULL pointer can be passed. */
+												);
+
+/** Loads new QP vectors from files (internal members are not affected!).
+ *	\return SUCCESSFUL_RETURN \n
+			RET_UNABLE_TO_OPEN_FILE \n
+			RET_UNABLE_TO_READ_FILE \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblemBCPY_loadQPvectorsFromFile(	QProblem* _THIS,
+												const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+												const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+										 			 							 If no lower bounds exist, a NULL pointer can be passed. */
+												const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+										 			 							 If no upper bounds exist, a NULL pointer can be passed. */
+												real_t* const g_new,		/**< Output: Gradient of neighbouring QP to be solved. */
+												real_t* const lb_new,		/**< Output: Lower bounds of neighbouring QP to be solved */
+												real_t* const ub_new		/**< Output: Upper bounds of neighbouring QP to be solved */
+												);
+
+
+/** Sets internal infeasibility flag and throws given error in case the far bound
+ *	strategy is not enabled (as QP might actually not be infeasible in _THIS case).
+ *	\return RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_ENSURELI_FAILED_CYCLING \n
+			RET_ENSURELI_FAILED_NOINDEX */
+returnValue QProblem_setInfeasibilityFlag(	QProblem* _THIS,
+											returnValue returnvalue,	/**< Returnvalue to be tunneled. */
+											BooleanType doThrowError	/**< Flag forcing to throw an error. */
+											);
+
+
+/** Determines if next QP iteration can be performed within given CPU time limit.
+ *	\return BT_TRUE: CPU time limit is exceeded, stop QP solution. \n
+			BT_FALSE: Sufficient CPU time for next QP iteration. */
+BooleanType QProblem_isCPUtimeLimitExceeded(	QProblem* _THIS,
+												const real_t* const cputime,	/**< Maximum CPU time allowed for QP solution. */
+												real_t starttime,				/**< Start time of current QP solution. */
+												int nWSR						/**< Number of working set recalculations performed so far. */
+												);
+
+
+/** Regularise Hessian matrix by adding a scaled identity matrix to it.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_HESSIAN_ALREADY_REGULARISED */
+returnValue QProblem_regulariseHessian( QProblem* _THIS );
+
+
+/** Sets Hessian matrix of the QP.
+ *	\return SUCCESSFUL_RETURN */
+static inline returnValue QProblem_setHM(	QProblem* _THIS,
+											DenseMatrix* H_new	/**< New Hessian matrix. */
+											);
+
+/** Sets dense Hessian matrix of the QP.
+ *  If a null pointer is passed and
+ *  a) hessianType is HST_IDENTITY, nothing is done,
+ *  b) hessianType is not HST_IDENTITY, Hessian matrix is set to zero.
+ *	\return SUCCESSFUL_RETURN */
+static inline returnValue QProblem_setH(	QProblem* _THIS,
+											real_t* const H_new	/**< New dense Hessian matrix (with correct dimension!). */
+											);
+
+/** Changes gradient vector of the QP.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_INVALID_ARGUMENTS */
+static inline returnValue QProblem_setG(	QProblem* _THIS,
+											const real_t* const g_new	/**< New gradient vector (with correct dimension!). */
+											);
+
+/** Changes lower bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_INVALID_ARGUMENTS */
+static inline returnValue QProblem_setLB(	QProblem* _THIS,
+											const real_t* const lb_new	/**< New lower bound vector (with correct dimension!). */
+											);
+
+/** Changes single entry of lower bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN  \n
+			RET_INDEX_OUT_OF_BOUNDS */
+static inline returnValue QProblem_setLBn(	QProblem* _THIS,
+											int number,		/**< Number of entry to be changed. */
+											real_t value	/**< New value for entry of lower bound vector. */
+											);
+
+/** Changes upper bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_INVALID_ARGUMENTS */
+static inline returnValue QProblem_setUB(	QProblem* _THIS,
+											const real_t* const ub_new	/**< New upper bound vector (with correct dimension!). */
+											);
+
+/** Changes single entry of upper bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN  \n
+			RET_INDEX_OUT_OF_BOUNDS */
+static inline returnValue QProblem_setUBn(	QProblem* _THIS,
+											int number,		/**< Number of entry to be changed. */
+											real_t value	/**< New value for entry of upper bound vector. */
+											);
+
+
+
+/** Compute relative length of homotopy in data space for termination
+ *  criterion.
+ *  \return Relative length in data space. */
+real_t QProblemBCPY_getRelativeHomotopyLength(	QProblem* _THIS,
+												const real_t* const g_new,	/**< Final gradient. */
+												const real_t* const lb_new,	/**< Final lower variable bounds. */
+												const real_t* const ub_new	/**< Final upper variable bounds. */
+												);
+
+
+
+/** Performs robustified ratio test yield the maximum possible step length
+ *  along the homotopy path.
+ *	\return  SUCCESSFUL_RETURN */
+returnValue QProblem_performRatioTestB(	QProblem* _THIS,
+										int nIdx, 					/**< Number of ratios to be checked. */
+										const int* const idxList, 	/**< Array containing the indices of all ratios to be checked. */
+										Bounds* const subjectTo,	/**< Bound object corresponding to ratios to be checked. */
+										const real_t* const num,	/**< Array containing all numerators for performing the ratio test. */
+										const real_t* const den,	/**< Array containing all denominators for performing the ratio test. */
+										real_t epsNum,				/**< Numerator tolerance. */
+										real_t epsDen,				/**< Denominator tolerance. */
+										real_t* t,					/**< Output: Maximum possible step length along the homotopy path. */
+										int* BC_idx 				/**< Output: Index of blocking constraint. */
+										);
+
+/** Checks whether given ratio is blocking, i.e. limits the maximum step length
+ *  along the homotopy path to a value lower than given one.
+ *	\return  SUCCESSFUL_RETURN */
+static inline BooleanType QProblem_isBlocking(	QProblem* _THIS,
+												real_t num,			/**< Numerator for performing the ratio test. */
+												real_t den,			/**< Denominator for performing the ratio test. */
+												real_t epsNum,		/**< Numerator tolerance. */
+												real_t epsDen,		/**< Denominator tolerance. */
+												real_t* t			/**< Input:  Current maximum step length along the homotopy path,
+																	 *   Output: Updated maximum possible step length along the homotopy path. */
+												);
+
+
+/** ...
+ *	\return SUCCESSFUL_RETURN  \n
+			RET_UNABLE_TO_OPEN_FILE */
+returnValue QProblem_writeQpDataIntoMatFile(	QProblem* _THIS,
+												const char* const filename	/**< Mat file name. */
+												);
+
+/** ...
+*	\return SUCCESSFUL_RETURN  \n
+			RET_UNABLE_TO_OPEN_FILE */
+returnValue QProblem_writeQpWorkspaceIntoMatFile(	QProblem* _THIS,
+													const char* const filename	/**< Mat file name. */
+													);
+
+
+/*
+ *	g e t B o u n d s
+ */
+static inline returnValue QProblem_getBounds( QProblem* _THIS, Bounds* _bounds )
+{
+	int nV = QProblem_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	_bounds = _THIS->bounds;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	g e t N V
+ */
+static inline int QProblem_getNV( QProblem* _THIS )
+{
+	return Bounds_getNV( _THIS->bounds );
+}
+
+
+/*
+ *	g e t N F R
+ */
+static inline int QProblem_getNFR( QProblem* _THIS )
+{
+	return Bounds_getNFR( _THIS->bounds );
+}
+
+
+/*
+ *	g e t N F X
+ */
+static inline int QProblem_getNFX( QProblem* _THIS )
+{
+	return Bounds_getNFX( _THIS->bounds );
+}
+
+
+/*
+ *	g e t N F V
+ */
+static inline int QProblem_getNFV( QProblem* _THIS )
+{
+	return Bounds_getNFV( _THIS->bounds );
+}
+
+
+/*
+ *	g e t S t a t u s
+ */
+static inline QProblemStatus QProblem_getStatus( QProblem* _THIS )
+{
+	return _THIS->status;
+}
+
+
+/*
+ *	i s I n i t i a l i s e d
+ */
+static inline BooleanType QProblem_isInitialised( QProblem* _THIS )
+{
+	if ( _THIS->status == QPS_NOTINITIALISED )
+		return BT_FALSE;
+	else
+		return BT_TRUE;
+}
+
+
+/*
+ *	i s S o l v e d
+ */
+static inline BooleanType QProblem_isSolved( QProblem* _THIS )
+{
+	if ( _THIS->status == QPS_SOLVED )
+		return BT_TRUE;
+	else
+		return BT_FALSE;
+}
+
+
+/*
+ *	i s I n f e a s i b l e
+ */
+static inline BooleanType QProblem_isInfeasible( QProblem* _THIS )
+{
+	return _THIS->infeasible;
+}
+
+
+/*
+ *	i s U n b o u n d e d
+ */
+static inline BooleanType QProblem_isUnbounded( QProblem* _THIS )
+{
+	return _THIS->unbounded;
+}
+
+
+/*
+ *	g e t H e s s i a n T y p e
+ */
+static inline HessianType QProblem_getHessianType( QProblem* _THIS )
+{
+	return _THIS->hessianType;
+}
+
+
+/*
+ *	s e t H e s s i a n T y p e
+ */
+static inline returnValue QProblem_setHessianType( QProblem* _THIS, HessianType _hessianType )
+{
+	_THIS->hessianType = _hessianType;
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	u s i n g R e g u l a r i s a t i o n
+ */
+static inline BooleanType QProblem_usingRegularisation( QProblem* _THIS )
+{
+	if ( _THIS->regVal > QPOASES_ZERO )
+		return BT_TRUE;
+	else
+		return BT_FALSE;
+}
+
+
+/*
+ *	g e t O p t i o n s
+ */
+static inline Options QProblem_getOptions( QProblem* _THIS )
+{
+	return _THIS->options;
+}
+
+
+/*
+ *	s e t O p t i o n s
+ */
+static inline returnValue QProblem_setOptions(	QProblem* _THIS,
+												Options _options
+												)
+{
+	OptionsCPY( &_options,&(_THIS->options) );
+	Options_ensureConsistency( &(_THIS->options) );
+
+	QProblem_setPrintLevel( _THIS,_THIS->options.printLevel );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	g e t P r i n t L e v e l
+ */
+static inline PrintLevel QProblem_getPrintLevel( QProblem* _THIS )
+{
+	return _THIS->options.printLevel;
+}
+
+
+/*
+ *	g e t C o u n t
+ */
+static inline unsigned int QProblem_getCount( QProblem* _THIS )
+{
+	return _THIS->count;
+}
+
+
+/*
+ *	r e s e t C o u n t e r
+ */
+static inline returnValue QProblem_resetCounter( QProblem* _THIS )
+{
+	_THIS->count = 0;
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*****************************************************************************
+ *  P R O T E C T E D                                                        *
+ *****************************************************************************/
+
+
+/*
+ *	s e t H
+ */
+static inline returnValue QProblem_setHM( QProblem* _THIS, DenseMatrix* H_new )
+{
+	if ( H_new == 0 )
+		return QProblem_setH( _THIS,(real_t*)0 );
+	else
+		return QProblem_setH( _THIS,DenseMatrix_getVal(H_new) );
+}
+
+
+/*
+ *	s e t H
+ */
+static inline returnValue QProblem_setH( QProblem* _THIS, real_t* const H_new )
+{
+	/* if null pointer is passed, Hessian is set to zero matrix
+	 *                            (or stays identity matrix) */
+	if ( H_new == 0 )
+	{
+		if ( _THIS->hessianType == HST_IDENTITY )
+			return SUCCESSFUL_RETURN;
+
+		_THIS->hessianType = HST_ZERO;
+
+		_THIS->H = 0;
+	}
+	else
+	{
+		DenseMatrixCON( _THIS->H,QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),H_new );
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t G
+ */
+static inline returnValue QProblem_setG( QProblem* _THIS, const real_t* const g_new )
+{
+	unsigned int nV = (unsigned int)QProblem_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( g_new == 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	memcpy( _THIS->g,g_new,nV*sizeof(real_t) );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t L B
+ */
+static inline returnValue QProblem_setLB( QProblem* _THIS, const real_t* const lb_new )
+{
+	unsigned int i;
+	unsigned int nV = (unsigned int)QProblem_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( lb_new != 0 )
+	{
+		memcpy( _THIS->lb,lb_new,nV*sizeof(real_t) );
+	}
+	else
+	{
+		/* if no lower bounds are specified, set them to -infinity */
+		for( i=0; i<nV; ++i )
+			_THIS->lb[i] = -QPOASES_INFTY;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t L B
+ */
+static inline returnValue QProblem_setLBn( QProblem* _THIS, int number, real_t value )
+{
+	int nV = QProblem_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ( number >= 0 ) && ( number < nV ) )
+	{
+		_THIS->lb[number] = value;
+		return SUCCESSFUL_RETURN;
+	}
+	else
+	{
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+	}
+}
+
+
+/*
+ *	s e t U B
+ */
+static inline returnValue QProblem_setUB( QProblem* _THIS, const real_t* const ub_new )
+{
+	unsigned int i;
+	unsigned int nV = (unsigned int)QProblem_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ub_new != 0 )
+	{
+		memcpy( _THIS->ub,ub_new,nV*sizeof(real_t) );
+	}
+	else
+	{
+		/* if no upper bounds are specified, set them to infinity */
+		for( i=0; i<nV; ++i )
+			_THIS->ub[i] = QPOASES_INFTY;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t U B
+ */
+static inline returnValue QProblem_setUBn( QProblem* _THIS, int number, real_t value )
+{
+	int nV = QProblem_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ( number >= 0 ) && ( number < nV ) )
+	{
+		_THIS->ub[number] = value;
+
+		return SUCCESSFUL_RETURN;
+	}
+	else
+	{
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+	}
+}
+
+
+
+/*
+ * i s B l o c k i n g
+ */
+static inline BooleanType QProblem_isBlocking(	QProblem* _THIS,
+												real_t num,
+												real_t den,
+												real_t epsNum,
+												real_t epsDen,
+												real_t* t
+												)
+{
+	if ( ( den >= epsDen ) && ( num >= epsNum ) )
+	{
+		if ( num < (*t)*den )
+			return BT_TRUE;
+	}
+
+	return BT_FALSE;
+}
+
+
+
+/*
+ *	g e t C o n s t r a i n t s
+ */
+static inline returnValue QProblem_getConstraints( QProblem* _THIS, Constraints* _constraints )
+{
+	int nV = QProblem_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	ConstraintsCPY( _THIS->constraints,_constraints );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	g e t N C
+ */
+static inline int QProblem_getNC( QProblem* _THIS )
+{
+	return Constraints_getNC( _THIS->constraints );
+}
+
+
+/*
+ *	g e t N E C
+ */
+static inline int QProblem_getNEC( QProblem* _THIS )
+{
+	return Constraints_getNEC( _THIS->constraints );
+}
+
+
+/*
+ *	g e t N A C
+ */
+static inline int QProblem_getNAC( QProblem* _THIS )
+{
+	return Constraints_getNAC( _THIS->constraints );
+}
+
+
+/*
+ *	g e t N I A C
+ */
+static inline int QProblem_getNIAC( QProblem* _THIS )
+{
+	return Constraints_getNIAC( _THIS->constraints );
+}
+
+
+
+/*****************************************************************************
+ *  P R O T E C T E D                                                        *
+ *****************************************************************************/
+
+
+/*
+ *	s e t A
+ */
+static inline returnValue QProblem_setAM( QProblem* _THIS, DenseMatrix *A_new )
+{
+	if ( A_new == 0 )
+		return QProblem_setA( _THIS,(real_t*)0 );
+	else
+		return QProblem_setA( _THIS,DenseMatrix_getVal(A_new) );
+}
+
+
+/*
+ *	s e t A
+ */
+static inline returnValue QProblem_setA( QProblem* _THIS, real_t* const A_new )
+{
+	int j;
+	int nV = QProblem_getNV( _THIS );
+	int nC = QProblem_getNC( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( A_new == 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	DenseMatrixCON( _THIS->A,QProblem_getNC( _THIS ),QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),A_new );
+
+	DenseMatrix_times( _THIS->A,1, 1.0, _THIS->x, nV, 0.0, _THIS->Ax, nC);
+
+	for( j=0; j<nC; ++j )
+	{
+		_THIS->Ax_u[j] = _THIS->ubA[j] - _THIS->Ax[j];
+		_THIS->Ax_l[j] = _THIS->Ax[j] - _THIS->lbA[j];
+
+		/* (ckirches) disable constraints with empty rows */
+		if ( qpOASES_isZero( DenseMatrix_getRowNorm( _THIS->A,j,2 ),QPOASES_ZERO ) == BT_TRUE )
+			Constraints_setType( _THIS->constraints,j,ST_DISABLED );
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t L B A
+ */
+static inline returnValue QProblem_setLBA( QProblem* _THIS, const real_t* const lbA_new )
+{
+	unsigned int i;
+	unsigned int nV = (unsigned int)QProblem_getNV( _THIS );
+	unsigned int nC = (unsigned int)QProblem_getNC( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( lbA_new != 0 )
+	{
+		memcpy( _THIS->lbA,lbA_new,nC*sizeof(real_t) );
+	}
+	else
+	{
+		/* if no lower constraints' bounds are specified, set them to -infinity */
+		for( i=0; i<nC; ++i )
+			_THIS->lbA[i] = -QPOASES_INFTY;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t L B A
+ */
+static inline returnValue QProblem_setLBAn( QProblem* _THIS, int number, real_t value )
+{
+	int nV = QProblem_getNV( _THIS );
+	int nC = QProblem_getNC( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ( number >= 0 ) && ( number < nC ) )
+	{
+		_THIS->lbA[number] = value;
+		return SUCCESSFUL_RETURN;
+	}
+	else
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+}
+
+
+/*
+ *	s e t U B A
+ */
+static inline returnValue QProblem_setUBA( QProblem* _THIS, const real_t* const ubA_new )
+{
+	unsigned int i;
+	unsigned int nV = (unsigned int)QProblem_getNV( _THIS );
+	unsigned int nC = (unsigned int)QProblem_getNC( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ubA_new != 0 )
+	{
+		memcpy( _THIS->ubA,ubA_new,nC*sizeof(real_t) );
+	}
+	else
+	{
+		/* if no upper constraints' bounds are specified, set them to infinity */
+		for( i=0; i<nC; ++i )
+			_THIS->ubA[i] = QPOASES_INFTY;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t U B A
+ */
+static inline returnValue QProblem_setUBAn( QProblem* _THIS, int number, real_t value )
+{
+	int nV = QProblem_getNV( _THIS );
+	int nC = QProblem_getNC( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ( number >= 0 ) && ( number < nC ) )
+	{
+		_THIS->ubA[number] = value;
+		return SUCCESSFUL_RETURN;
+	}
+	else
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_QPROBLEM_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/include/qpOASES_e/Types.h b/third_party/acados/include/qpOASES_e/Types.h
index 1e452097f8..fc042aed82 100644
--- a/third_party/acados/include/qpOASES_e/Types.h
+++ b/third_party/acados/include/qpOASES_e/Types.h
@@ -1,310 +1,310 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *	See the GNU Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES_e/Types.h
- *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
- *	\version 3.1embedded
- *	\date 2007-2015
- *
- *	Declaration of all non-built-in types (except for classes).
- */
-
-
-#ifndef QPOASES_TYPES_H
-#define QPOASES_TYPES_H
-
-#ifdef USE_ACADOS_TYPES
-#include "acados/utils/types.h"
-#endif
-
-/* If your compiler does not support the snprintf() function,
- * uncomment the following line and try to compile again. */
-/* #define __NO_SNPRINTF__ */
-
-
-/* Uncomment the following line for setting the __DSPACE__ flag. */
-/* #define __DSPACE__ */
-
-/* Uncomment the following line for setting the __XPCTARGET__ flag. */
-/* #define __XPCTARGET__ */
-
-
-/* Uncomment the following line for setting the __NO_FMATH__ flag. */
-/* #define __NO_FMATH__ */
-
-/* Uncomment the following line to enable debug information. */
-/* #define __DEBUG__ */
-
-/* Uncomment the following line to enable suppress any kind of console output. */
-/* #define __SUPPRESSANYOUTPUT__ */
-
-
-/** Forces to always include all implicitly fixed bounds and all equality constraints
- *  into the initial working set when setting up an auxiliary QP. */
-#define __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
-
-/* Uncomment the following line to activate the use of an alternative Givens
- * plane rotation requiring only three multiplications. */
-/* #define __USE_THREE_MULTS_GIVENS__ */
-
-/* Uncomment the following line to activate the use of single precision arithmetic. */
-/* #define __USE_SINGLE_PRECISION__ */
-
-/* The inline keyword is skipped by default as it is not part of the C90 standard.
- * However, by uncommenting the following line, use of the inline keyword can be enforced. */
-/* #define __USE_INLINE__ */
-
-
-/* Work-around for Borland BCC 5.5 compiler. */
-#ifdef __BORLANDC__
-#if __BORLANDC__ < 0x0561
-  #define __STDC__ 1
-#endif
-#endif
-
-
-/* Work-around for Microsoft compilers. */
-#ifdef _MSC_VER
-  #define __NO_SNPRINTF__
-  #pragma warning( disable : 4061 4100 4250 4514 4996 )
-#endif
-
-
-/* Apply pre-processor settings when using qpOASES within auto-generated code. */
-#ifdef __CODE_GENERATION__
-  #define __NO_COPYRIGHT__
-  #define __EXTERNAL_DIMENSIONS__
-#endif /* __CODE_GENERATION__ */
-
-
-/* Avoid using static variables declaration within functions. */
-#ifdef __NO_STATIC__
-  #define myStatic
-#else
-  #define myStatic static
-#endif /* __NO_STATIC__ */
-
-
-/* Skip inline keyword if not specified otherwise. */
-#ifndef __USE_INLINE__
-  #define inline
-#endif
-
-
-/* Avoid any printing on embedded platforms. */
-#if defined(__DSPACE__) || defined(__XPCTARGET__)
-  #define __SUPPRESSANYOUTPUT__
-  #define __NO_SNPRINTF__
-#endif
-
-
-#ifdef __NO_SNPRINTF__
-  #if (!defined(_MSC_VER)) || defined(__DSPACE__) || defined(__XPCTARGET__)
-    /* If snprintf is not available, provide an empty implementation... */
-    int snprintf( char* s, size_t n, const char* format, ... );
-  #else
-	/* ... or substitute snprintf by _snprintf for Microsoft compilers. */
-    #define snprintf _snprintf
-  #endif
-#endif /* __NO_SNPRINTF__ */
-
-
-/** Macro for switching on/off the beginning of the qpOASES namespace definition. */
-#define BEGIN_NAMESPACE_QPOASES
-
-/** Macro for switching on/off the end of the qpOASES namespace definition. */
-#define END_NAMESPACE_QPOASES
-
-/** Macro for switching on/off the use of the qpOASES namespace. */
-#define USING_NAMESPACE_QPOASES
-
-/** Macro for switching on/off references to the qpOASES namespace. */
-#define REFER_NAMESPACE_QPOASES /*::*/
-
-
-/** Macro for accessing the Cholesky factor R. */
-#define RR( I,J )  _THIS->R[(I)+nV*(J)]
-
-/** Macro for accessing the orthonormal matrix Q of the QT factorisation. */
-#define QQ( I,J )  _THIS->Q[(I)+nV*(J)]
-
-/** Macro for accessing the triangular matrix T of the QT factorisation. */
-#define TT( I,J )  _THIS->T[(I)*nVC_min+(J)]
-
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-/** Defines real_t for facilitating switching between double and float. */
-
-#ifndef USE_ACADOS_TYPES
-#ifndef __CODE_GENERATION__
-
-  #ifdef __USE_SINGLE_PRECISION__
-  typedef float real_t;
-  #else
-  typedef double real_t;
-  #endif /* __USE_SINGLE_PRECISION__ */
-
-#endif /* __CODE_GENERATION__ */
-#endif /* USE_ACADOS_TYPES */
-
-/** Summarises all possible logical values. */
-typedef enum
-{
-	BT_FALSE = 0,				/**< Logical value for "false". */
-	BT_TRUE						/**< Logical value for "true". */
-} BooleanType;
-
-
-/** Summarises all possible print levels. Print levels are used to describe
- *	the desired amount of output during runtime of qpOASES. */
-typedef enum
-{
-	PL_DEBUG_ITER = -2,			/**< Full tabular debugging output. */
-	PL_TABULAR,					/**< Tabular output. */
-	PL_NONE,					/**< No output. */
-	PL_LOW,						/**< Print error messages only. */
-	PL_MEDIUM,					/**< Print error and warning messages as well as concise info messages. */
-	PL_HIGH						/**< Print all messages with full details. */
-} PrintLevel;
-
-
-/** Defines visibility status of a message. */
-typedef enum
-{
-	VS_HIDDEN,					/**< Message not visible. */
-	VS_VISIBLE					/**< Message visible. */
-} VisibilityStatus;
-
-
-/** Summarises all possible states of the (S)QProblem(B) object during the
-solution process of a QP sequence. */
-typedef enum
-{
-	QPS_NOTINITIALISED,			/**< QProblem object is freshly instantiated or reset. */
-	QPS_PREPARINGAUXILIARYQP,	/**< An auxiliary problem is currently setup, either at the very beginning
-								 *   via an initial homotopy or after changing the QP matrices. */
-	QPS_AUXILIARYQPSOLVED,		/**< An auxilary problem was solved, either at the very beginning
-								 *   via an initial homotopy or after changing the QP matrices. */
-	QPS_PERFORMINGHOMOTOPY,		/**< A homotopy according to the main idea of the online active
-								 *   set strategy is performed. */
-	QPS_HOMOTOPYQPSOLVED,		/**< An intermediate QP along the homotopy path was solved. */
-	QPS_SOLVED					/**< The solution of the actual QP was found. */
-} QProblemStatus;
-
-
-/** Summarises all possible types of the QP's Hessian matrix. */
-typedef enum
-{
-	HST_ZERO,				/**< Hessian is zero matrix (i.e. LP formulation). */
-	HST_IDENTITY,			/**< Hessian is identity matrix. */
-	HST_POSDEF,				/**< Hessian is (strictly) positive definite. */
-	HST_POSDEF_NULLSPACE,	/**< Hessian is positive definite on null space of active bounds/constraints. */
-	HST_SEMIDEF,			/**< Hessian is positive semi-definite. */
-	HST_INDEF,				/**< Hessian is indefinite. */
-	HST_UNKNOWN				/**< Hessian type is unknown. */
-} HessianType;
-
-
-/** Summarises all possible types of bounds and constraints. */
-typedef enum
-{
-	ST_UNBOUNDED,		/**< Bound/constraint is unbounded. */
-	ST_BOUNDED,			/**< Bound/constraint is bounded but not fixed. */
-	ST_EQUALITY,		/**< Bound/constraint is fixed (implicit equality bound/constraint). */
-	ST_DISABLED,		/**< Bound/constraint is disabled (i.e. ignored when solving QP). */
-	ST_UNKNOWN			/**< Type of bound/constraint unknown. */
-} SubjectToType;
-
-
-/** Summarises all possible states of bounds and constraints. */
-typedef enum
-{
-	ST_LOWER = -1,			/**< Bound/constraint is at its lower bound. */
-	ST_INACTIVE,			/**< Bound/constraint is inactive. */
-	ST_UPPER,				/**< Bound/constraint is at its upper bound. */
-	ST_INFEASIBLE_LOWER,	/**< (to be documented) */
-	ST_INFEASIBLE_UPPER,	/**< (to be documented) */
-	ST_UNDEFINED			/**< Status of bound/constraint undefined. */
-} SubjectToStatus;
-
-
-/**
- *	\brief Stores internal information for tabular (debugging) output.
- *
- *	Struct storing internal information for tabular (debugging) output
- *	when using the (S)QProblem(B) objects.
- *
- *	\author Hans Joachim Ferreau
- *	\version 3.1embedded
- *	\date 2013-2015
- */
-typedef struct
-{
-	int idxAddB;		/**< Index of bound that has been added to working set. */
-	int idxRemB;		/**< Index of bound that has been removed from working set. */
-	int idxAddC;		/**< Index of constraint that has been added to working set. */
-	int idxRemC;		/**< Index of constraint that has been removed from working set. */
-	int excAddB;		/**< Flag indicating whether a bound has been added to working set to keep a regular projected Hessian. */
-	int excRemB;		/**< Flag indicating whether a bound has been removed from working set to keep a regular projected Hessian. */
-	int excAddC;		/**< Flag indicating whether a constraint has been added to working set to keep a regular projected Hessian. */
-	int excRemC;		/**< Flag indicating whether a constraint has been removed from working set to keep a regular projected Hessian. */
-} TabularOutput;
-
-/**
- *	\brief Struct containing the variable header for mat file.
- *
- *	Struct storing the header of a variable to be stored in
- *	Matlab's binary format (using the outdated Level 4 variant
- *  for simplictiy).
- *
- *  Note, this code snippet has been inspired from the document
- *  "Matlab(R) MAT-file Format, R2013b" by MathWorks
- *
- *	\author Hans Joachim Ferreau
- *	\version 3.1embedded
- *	\date 2013-2015
- */
-typedef struct
-{
-	long numericFormat;		/**< Flag indicating numerical format. */
-	long nRows;				/**< Number of rows. */
-	long nCols;				/**< Number of rows. */
-	long imaginaryPart;		/**< (to be documented) */
-	long nCharName;			/**< Number of character in name. */
-} MatMatrixHeader;
-
-
-END_NAMESPACE_QPOASES
-
-
-#endif	/* QPOASES_TYPES_H */
-
-
-/*
- *	end of file
- */
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/Types.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ *
+ *	Declaration of all non-built-in types (except for classes).
+ */
+
+
+#ifndef QPOASES_TYPES_H
+#define QPOASES_TYPES_H
+
+#ifdef USE_ACADOS_TYPES
+#include "acados/utils/types.h"
+#endif
+
+/* If your compiler does not support the snprintf() function,
+ * uncomment the following line and try to compile again. */
+/* #define __NO_SNPRINTF__ */
+
+
+/* Uncomment the following line for setting the __DSPACE__ flag. */
+/* #define __DSPACE__ */
+
+/* Uncomment the following line for setting the __XPCTARGET__ flag. */
+/* #define __XPCTARGET__ */
+
+
+/* Uncomment the following line for setting the __NO_FMATH__ flag. */
+/* #define __NO_FMATH__ */
+
+/* Uncomment the following line to enable debug information. */
+/* #define __DEBUG__ */
+
+/* Uncomment the following line to enable suppress any kind of console output. */
+/* #define __SUPPRESSANYOUTPUT__ */
+
+
+/** Forces to always include all implicitly fixed bounds and all equality constraints
+ *  into the initial working set when setting up an auxiliary QP. */
+#define __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
+
+/* Uncomment the following line to activate the use of an alternative Givens
+ * plane rotation requiring only three multiplications. */
+/* #define __USE_THREE_MULTS_GIVENS__ */
+
+/* Uncomment the following line to activate the use of single precision arithmetic. */
+/* #define __USE_SINGLE_PRECISION__ */
+
+/* The inline keyword is skipped by default as it is not part of the C90 standard.
+ * However, by uncommenting the following line, use of the inline keyword can be enforced. */
+/* #define __USE_INLINE__ */
+
+
+/* Work-around for Borland BCC 5.5 compiler. */
+#ifdef __BORLANDC__
+#if __BORLANDC__ < 0x0561
+  #define __STDC__ 1
+#endif
+#endif
+
+
+/* Work-around for Microsoft compilers. */
+#ifdef _MSC_VER
+  #define __NO_SNPRINTF__
+  #pragma warning( disable : 4061 4100 4250 4514 4996 )
+#endif
+
+
+/* Apply pre-processor settings when using qpOASES within auto-generated code. */
+#ifdef __CODE_GENERATION__
+  #define __NO_COPYRIGHT__
+  #define __EXTERNAL_DIMENSIONS__
+#endif /* __CODE_GENERATION__ */
+
+
+/* Avoid using static variables declaration within functions. */
+#ifdef __NO_STATIC__
+  #define myStatic
+#else
+  #define myStatic static
+#endif /* __NO_STATIC__ */
+
+
+/* Skip inline keyword if not specified otherwise. */
+#ifndef __USE_INLINE__
+  #define inline
+#endif
+
+
+/* Avoid any printing on embedded platforms. */
+#if defined(__DSPACE__) || defined(__XPCTARGET__)
+  #define __SUPPRESSANYOUTPUT__
+  #define __NO_SNPRINTF__
+#endif
+
+
+#ifdef __NO_SNPRINTF__
+  #if (!defined(_MSC_VER)) || defined(__DSPACE__) || defined(__XPCTARGET__)
+    /* If snprintf is not available, provide an empty implementation... */
+    int snprintf( char* s, size_t n, const char* format, ... );
+  #else
+	/* ... or substitute snprintf by _snprintf for Microsoft compilers. */
+    #define snprintf _snprintf
+  #endif
+#endif /* __NO_SNPRINTF__ */
+
+
+/** Macro for switching on/off the beginning of the qpOASES namespace definition. */
+#define BEGIN_NAMESPACE_QPOASES
+
+/** Macro for switching on/off the end of the qpOASES namespace definition. */
+#define END_NAMESPACE_QPOASES
+
+/** Macro for switching on/off the use of the qpOASES namespace. */
+#define USING_NAMESPACE_QPOASES
+
+/** Macro for switching on/off references to the qpOASES namespace. */
+#define REFER_NAMESPACE_QPOASES /*::*/
+
+
+/** Macro for accessing the Cholesky factor R. */
+#define RR( I,J )  _THIS->R[(I)+nV*(J)]
+
+/** Macro for accessing the orthonormal matrix Q of the QT factorisation. */
+#define QQ( I,J )  _THIS->Q[(I)+nV*(J)]
+
+/** Macro for accessing the triangular matrix T of the QT factorisation. */
+#define TT( I,J )  _THIS->T[(I)*nVC_min+(J)]
+
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** Defines real_t for facilitating switching between double and float. */
+
+#ifndef USE_ACADOS_TYPES
+#ifndef __CODE_GENERATION__
+
+  #ifdef __USE_SINGLE_PRECISION__
+  typedef float real_t;
+  #else
+  typedef double real_t;
+  #endif /* __USE_SINGLE_PRECISION__ */
+
+#endif /* __CODE_GENERATION__ */
+#endif /* USE_ACADOS_TYPES */
+
+/** Summarises all possible logical values. */
+typedef enum
+{
+	BT_FALSE = 0,				/**< Logical value for "false". */
+	BT_TRUE						/**< Logical value for "true". */
+} BooleanType;
+
+
+/** Summarises all possible print levels. Print levels are used to describe
+ *	the desired amount of output during runtime of qpOASES. */
+typedef enum
+{
+	PL_DEBUG_ITER = -2,			/**< Full tabular debugging output. */
+	PL_TABULAR,					/**< Tabular output. */
+	PL_NONE,					/**< No output. */
+	PL_LOW,						/**< Print error messages only. */
+	PL_MEDIUM,					/**< Print error and warning messages as well as concise info messages. */
+	PL_HIGH						/**< Print all messages with full details. */
+} PrintLevel;
+
+
+/** Defines visibility status of a message. */
+typedef enum
+{
+	VS_HIDDEN,					/**< Message not visible. */
+	VS_VISIBLE					/**< Message visible. */
+} VisibilityStatus;
+
+
+/** Summarises all possible states of the (S)QProblem(B) object during the
+solution process of a QP sequence. */
+typedef enum
+{
+	QPS_NOTINITIALISED,			/**< QProblem object is freshly instantiated or reset. */
+	QPS_PREPARINGAUXILIARYQP,	/**< An auxiliary problem is currently setup, either at the very beginning
+								 *   via an initial homotopy or after changing the QP matrices. */
+	QPS_AUXILIARYQPSOLVED,		/**< An auxilary problem was solved, either at the very beginning
+								 *   via an initial homotopy or after changing the QP matrices. */
+	QPS_PERFORMINGHOMOTOPY,		/**< A homotopy according to the main idea of the online active
+								 *   set strategy is performed. */
+	QPS_HOMOTOPYQPSOLVED,		/**< An intermediate QP along the homotopy path was solved. */
+	QPS_SOLVED					/**< The solution of the actual QP was found. */
+} QProblemStatus;
+
+
+/** Summarises all possible types of the QP's Hessian matrix. */
+typedef enum
+{
+	HST_ZERO,				/**< Hessian is zero matrix (i.e. LP formulation). */
+	HST_IDENTITY,			/**< Hessian is identity matrix. */
+	HST_POSDEF,				/**< Hessian is (strictly) positive definite. */
+	HST_POSDEF_NULLSPACE,	/**< Hessian is positive definite on null space of active bounds/constraints. */
+	HST_SEMIDEF,			/**< Hessian is positive semi-definite. */
+	HST_INDEF,				/**< Hessian is indefinite. */
+	HST_UNKNOWN				/**< Hessian type is unknown. */
+} HessianType;
+
+
+/** Summarises all possible types of bounds and constraints. */
+typedef enum
+{
+	ST_UNBOUNDED,		/**< Bound/constraint is unbounded. */
+	ST_BOUNDED,			/**< Bound/constraint is bounded but not fixed. */
+	ST_EQUALITY,		/**< Bound/constraint is fixed (implicit equality bound/constraint). */
+	ST_DISABLED,		/**< Bound/constraint is disabled (i.e. ignored when solving QP). */
+	ST_UNKNOWN			/**< Type of bound/constraint unknown. */
+} SubjectToType;
+
+
+/** Summarises all possible states of bounds and constraints. */
+typedef enum
+{
+	ST_LOWER = -1,			/**< Bound/constraint is at its lower bound. */
+	ST_INACTIVE,			/**< Bound/constraint is inactive. */
+	ST_UPPER,				/**< Bound/constraint is at its upper bound. */
+	ST_INFEASIBLE_LOWER,	/**< (to be documented) */
+	ST_INFEASIBLE_UPPER,	/**< (to be documented) */
+	ST_UNDEFINED			/**< Status of bound/constraint undefined. */
+} SubjectToStatus;
+
+
+/**
+ *	\brief Stores internal information for tabular (debugging) output.
+ *
+ *	Struct storing internal information for tabular (debugging) output
+ *	when using the (S)QProblem(B) objects.
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.1embedded
+ *	\date 2013-2015
+ */
+typedef struct
+{
+	int idxAddB;		/**< Index of bound that has been added to working set. */
+	int idxRemB;		/**< Index of bound that has been removed from working set. */
+	int idxAddC;		/**< Index of constraint that has been added to working set. */
+	int idxRemC;		/**< Index of constraint that has been removed from working set. */
+	int excAddB;		/**< Flag indicating whether a bound has been added to working set to keep a regular projected Hessian. */
+	int excRemB;		/**< Flag indicating whether a bound has been removed from working set to keep a regular projected Hessian. */
+	int excAddC;		/**< Flag indicating whether a constraint has been added to working set to keep a regular projected Hessian. */
+	int excRemC;		/**< Flag indicating whether a constraint has been removed from working set to keep a regular projected Hessian. */
+} TabularOutput;
+
+/**
+ *	\brief Struct containing the variable header for mat file.
+ *
+ *	Struct storing the header of a variable to be stored in
+ *	Matlab's binary format (using the outdated Level 4 variant
+ *  for simplictiy).
+ *
+ *  Note, this code snippet has been inspired from the document
+ *  "Matlab(R) MAT-file Format, R2013b" by MathWorks
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.1embedded
+ *	\date 2013-2015
+ */
+typedef struct
+{
+	long numericFormat;		/**< Flag indicating numerical format. */
+	long nRows;				/**< Number of rows. */
+	long nCols;				/**< Number of rows. */
+	long imaginaryPart;		/**< (to be documented) */
+	long nCharName;			/**< Number of character in name. */
+} MatMatrixHeader;
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_TYPES_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/include/qpOASES_e/UnitTesting.h b/third_party/acados/include/qpOASES_e/UnitTesting.h
index 3fb31129a5..dbff201039 100644
--- a/third_party/acados/include/qpOASES_e/UnitTesting.h
+++ b/third_party/acados/include/qpOASES_e/UnitTesting.h
@@ -1,79 +1,79 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
- *	Christian Kirches et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *	See the GNU Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file include/qpOASES_e/UnitTesting.h
- *	\author Hans Joachim Ferreau
- *	\version 3.1embedded
- *	\date 2014-2015
- *
- *	Definition of auxiliary functions/macros for unit testing.
- */
-
-
-#ifndef QPOASES_UNIT_TESTING_H
-#define QPOASES_UNIT_TESTING_H
-
-
-#ifndef TEST_TOL_FACTOR
-#define TEST_TOL_FACTOR 1
-#endif
-
-
-/** Return value for tests that passed. */
-#define TEST_PASSED 0
-
-/** Return value for tests that failed. */
-#define TEST_FAILED 1
-
-/** Return value for tests that could not run due to missing external data. */
-#define TEST_DATA_NOT_FOUND 99
-
-
-/** Macro verifying that two numerical values are equal in order to pass unit test. */
-#define QPOASES_TEST_FOR_EQUAL( x,y ) if ( REFER_NAMESPACE_QPOASES isEqual( (x),(y) ) == BT_FALSE ) { return TEST_FAILED; }
-
-/** Macro verifying that two numerical values are close to each other in order to pass unit test. */
-#define QPOASES_TEST_FOR_NEAR( x,y )  if ( REFER_NAMESPACE_QPOASES getAbs((x)-(y)) / REFER_NAMESPACE_QPOASES getMax( 1.0,REFER_NAMESPACE_QPOASES getAbs(x) ) >= 1e-10 ) { return TEST_FAILED; }
-
-/** Macro verifying that first quantity is lower or equal than second one in order to pass unit test. */
-#define QPOASES_TEST_FOR_TOL( x,tol )  if ( (x) > (tol)*(TEST_TOL_FACTOR) ) { return TEST_FAILED; }
-
-/** Macro verifying that a logical expression holds in order to pass unit test. */
-#define QPOASES_TEST_FOR_TRUE( x )  if ( (x) == 0 ) { return TEST_FAILED; }
-
-
-
-BEGIN_NAMESPACE_QPOASES
-
-
-END_NAMESPACE_QPOASES
-
-
-#endif	/* QPOASES_UNIT_TESTING_H */
-
-
-/*
- *	end of file
- */
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/UnitTesting.h
+ *	\author Hans Joachim Ferreau
+ *	\version 3.1embedded
+ *	\date 2014-2015
+ *
+ *	Definition of auxiliary functions/macros for unit testing.
+ */
+
+
+#ifndef QPOASES_UNIT_TESTING_H
+#define QPOASES_UNIT_TESTING_H
+
+
+#ifndef TEST_TOL_FACTOR
+#define TEST_TOL_FACTOR 1
+#endif
+
+
+/** Return value for tests that passed. */
+#define TEST_PASSED 0
+
+/** Return value for tests that failed. */
+#define TEST_FAILED 1
+
+/** Return value for tests that could not run due to missing external data. */
+#define TEST_DATA_NOT_FOUND 99
+
+
+/** Macro verifying that two numerical values are equal in order to pass unit test. */
+#define QPOASES_TEST_FOR_EQUAL( x,y ) if ( REFER_NAMESPACE_QPOASES isEqual( (x),(y) ) == BT_FALSE ) { return TEST_FAILED; }
+
+/** Macro verifying that two numerical values are close to each other in order to pass unit test. */
+#define QPOASES_TEST_FOR_NEAR( x,y )  if ( REFER_NAMESPACE_QPOASES getAbs((x)-(y)) / REFER_NAMESPACE_QPOASES getMax( 1.0,REFER_NAMESPACE_QPOASES getAbs(x) ) >= 1e-10 ) { return TEST_FAILED; }
+
+/** Macro verifying that first quantity is lower or equal than second one in order to pass unit test. */
+#define QPOASES_TEST_FOR_TOL( x,tol )  if ( (x) > (tol)*(TEST_TOL_FACTOR) ) { return TEST_FAILED; }
+
+/** Macro verifying that a logical expression holds in order to pass unit test. */
+#define QPOASES_TEST_FOR_TRUE( x )  if ( (x) == 0 ) { return TEST_FAILED; }
+
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_UNIT_TESTING_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/larch64/lib/libacados.so b/third_party/acados/larch64/lib/libacados.so
index 35f83d4159..9a28b4c9d6 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 6b0e26de79..b97ed876e2 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/larch64/lib/libhpipm.so b/third_party/acados/larch64/lib/libhpipm.so
index bebd5e8911..cd4b1f1b5b 100644
Binary files a/third_party/acados/larch64/lib/libhpipm.so and b/third_party/acados/larch64/lib/libhpipm.so differ
diff --git a/third_party/acados/larch64/lib/libqpOASES_e.so.3.1 b/third_party/acados/larch64/lib/libqpOASES_e.so.3.1
index 0611c8a939..19e7a69bd5 100644
Binary files a/third_party/acados/larch64/lib/libqpOASES_e.so.3.1 and b/third_party/acados/larch64/lib/libqpOASES_e.so.3.1 differ
diff --git a/third_party/acados/larch64/t_renderer b/third_party/acados/larch64/t_renderer
index b4ff8bc319..b6f70bde06 100755
Binary files a/third_party/acados/larch64/t_renderer and b/third_party/acados/larch64/t_renderer differ
diff --git a/third_party/acados/x86_64/lib/libacados.so b/third_party/acados/x86_64/lib/libacados.so
index b8a6280d68..ae5fc363f2 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 e4a19caf13..676ff67e98 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 428c76a117..8d026b1185 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 cf3604688c..c0c8b66650 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 5ced89c28d..d84fc762c8 100755
Binary files a/third_party/acados/x86_64/t_renderer and b/third_party/acados/x86_64/t_renderer differ
diff --git a/third_party/cluster/LICENSE b/third_party/cluster/LICENSE
deleted file mode 100644
index ab8b4db7d7..0000000000
--- a/third_party/cluster/LICENSE
+++ /dev/null
@@ -1,13 +0,0 @@
-Copyright:
-  * fastcluster_dm.cpp & fastcluster_R_dm.cpp:
-     © 2011 Daniel Müllner <http://danifold.net>
-  * fastcluster.(h|cpp) & demo.cpp & plotresult.r:
-     © 2018 Christoph Dalitz <http://www.hsnr.de/ipattern/>
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
-
-  * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
-  * 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.
diff --git a/third_party/cluster/README b/third_party/cluster/README
deleted file mode 100644
index acb18bc72b..0000000000
--- a/third_party/cluster/README
+++ /dev/null
@@ -1,79 +0,0 @@
-C++ interface to fast hierarchical clustering algorithms
-========================================================
-
-This is a simplified C++ interface to fast implementations of hierarchical
-clustering by Daniel Müllner. The original library with interfaces to R
-and Python is described in:
-
-Daniel Müllner: "fastcluster: Fast Hierarchical, Agglomerative Clustering
-Routines for R and Python." Journal of Statistical Software 53 (2013),
-no. 9, pp. 1–18, http://www.jstatsoft.org/v53/i09/
-
-
-Usage of the library
---------------------
-
-For using the library, the following source files are needed:
-
-fastcluster_dm.cpp, fastcluster_R_dm.cpp
-   original code by Daniel Müllner
-   these are included by fastcluster.cpp via #include, and therefore
-   need not be compiled to object code
-
-fastcluster.[h|cpp]
-   simplified C++ interface
-   fastcluster.cpp is the only file that must be compiled
-
-The library provides the clustering function *hclust_fast* for
-creating the dendrogram information in an encoding as used by the
-R function *hclust*. For a description of the parameters, see fastcluster.h.
-Its parameter *method* can be one of
-
-HCLUST_METHOD_SINGLE
-  single link with the minimum spanning tree algorithm (Rohlf, 1973)
-
-HHCLUST_METHOD_COMPLETE
-  complete link with the nearest-neighbor-chain algorithm (Murtagh, 1984)
-
-HCLUST_METHOD_AVERAGE
-  complete link with the nearest-neighbor-chain algorithm (Murtagh, 1984)
-
-HCLUST_METHOD_MEDIAN
-  median link with the generic algorithm (Müllner, 2011)
-
-For splitting the dendrogram into clusters, the two functions *cutree_k*
-and *cutree_cdist* are provided.
-
-Note that output parameters must be allocated beforehand, e.g.
-  int* merge = new int[2*(npoints-1)];
-For a complete usage example, see lines 135-142 of demo.cpp.
-
-
-Demonstration program
----------------------
-
-A simple demo is implemented in demo.cpp, which can be compiled and run with
-
-   make
-   ./hclust-demo -m complete lines.csv
-
-It creates two clusters of line segments such that the segment angle between
-line segments of different clusters have a maximum (cosine) dissimilarity.
-For visualizing the result, plotresult.r can be used as follows
-(requires R <https://r-project.org> to be installed):
-
-  ./hclust-demo -m complete lines.csv | Rscript plotresult.r
-
-
-Authors & Copyright
--------------------
-
-Daniel Müllner, 2011, <http://danifold.net>
-Christoph Dalitz, 2018, <http://www.hsnr.de/ipattern/>
-
-
-License
--------
-
-This code is provided under a BSD-style license.
-See the file LICENSE for details.
diff --git a/third_party/cluster/SConscript b/third_party/cluster/SConscript
deleted file mode 100644
index 97eb4300d4..0000000000
--- a/third_party/cluster/SConscript
+++ /dev/null
@@ -1,8 +0,0 @@
-Import('env')
-
-fc = env.SharedLibrary("fastcluster", "fastcluster.cpp")
-
-# TODO: how do I gate on test
-#env.Program("test", ["test.cpp"], LIBS=[fc])
-#valgrind --leak-check=full ./test
-
diff --git a/third_party/cluster/__init__.py b/third_party/cluster/__init__.py
deleted file mode 100644
index e69de29bb2..0000000000
diff --git a/third_party/cluster/fastcluster.cpp b/third_party/cluster/fastcluster.cpp
deleted file mode 100644
index d2b557d6f5..0000000000
--- a/third_party/cluster/fastcluster.cpp
+++ /dev/null
@@ -1,218 +0,0 @@
-//
-// C++ standalone verion of fastcluster by Daniel Müllner
-//
-// Copyright: Christoph Dalitz, 2018
-//            Daniel Müllner, 2011
-// License:   BSD style license
-//            (see the file LICENSE for details)
-//
-
-
-#include <vector>
-#include <algorithm>
-#include <cmath>
-
-
-extern "C" {
-#include "fastcluster.h"
-}
-
-// Code by Daniel Müllner
-// workaround to make it usable as a standalone version (without R)
-bool fc_isnan(double x) { return false; }
-#include "fastcluster_dm.cpp"
-#include "fastcluster_R_dm.cpp"
-
-extern "C" {
-//
-// Assigns cluster labels (0, ..., nclust-1) to the n points such
-// that the cluster result is split into nclust clusters.
-//
-// Input arguments:
-//   n      = number of observables
-//   merge  = clustering result in R format
-//   nclust = number of clusters
-// Output arguments:
-//   labels = allocated integer array of size n for result
-//
-  void cutree_k(int n, const int* merge, int nclust, int* labels) {
-
-    int k,m1,m2,j,l;
-
-    if (nclust > n || nclust < 2) {
-      for (j=0; j<n; j++) labels[j] = 0;
-      return;
-    }
-
-    // assign to each observable the number of its last merge step
-    // beware: indices of observables in merge start at 1 (R convention)
-    std::vector<int> last_merge(n, 0);
-    for (k=1; k<=(n-nclust); k++) {
-      // (m1,m2) = merge[k,]
-      m1 = merge[k-1];
-      m2 = merge[n-1+k-1];
-      if (m1 < 0 && m2 < 0) { // both single observables
-        last_merge[-m1-1] = last_merge[-m2-1] = k;
-      }
-      else if (m1 < 0 || m2 < 0) { // one is a cluster
-        if(m1 < 0) { j = -m1; m1 = m2; } else j = -m2;
-        // merging single observable and cluster
-        for(l = 0; l < n; l++)
-          if (last_merge[l] == m1)
-            last_merge[l] = k;
-        last_merge[j-1] = k;
-      }
-      else { // both cluster
-        for(l=0; l < n; l++) {
-          if( last_merge[l] == m1 || last_merge[l] == m2 )
-            last_merge[l] = k;
-        }
-      }
-    }
-
-    // assign cluster labels
-    int label = 0;
-    std::vector<int> z(n,-1);
-    for (j=0; j<n; j++) {
-      if (last_merge[j] == 0) { // still singleton
-        labels[j] = label++;
-      } else {
-        if (z[last_merge[j]] < 0) {
-          z[last_merge[j]] = label++;
-        }
-        labels[j] = z[last_merge[j]];
-      }
-    }
-  }
-
-  //
-  // Assigns cluster labels (0, ..., nclust-1) to the n points such
-  // that the hierarchical clustering is stopped when cluster distance >= cdist
-  //
-  // Input arguments:
-  //   n      = number of observables
-  //   merge  = clustering result in R format
-  //   height = cluster distance at each merge step
-  //   cdist  = cutoff cluster distance
-  // Output arguments:
-  //   labels = allocated integer array of size n for result
-  //
-  void cutree_cdist(int n, const int* merge, double* height, double cdist, int* labels) {
-
-    int k;
-
-    for (k=0; k<(n-1); k++) {
-      if (height[k] >= cdist) {
-        break;
-      }
-    }
-    cutree_k(n, merge, n-k, labels);
-  }
-
-
-  //
-  // Hierarchical clustering with one of Daniel Muellner's fast algorithms
-  //
-  // Input arguments:
-  //   n       = number of observables
-  //   distmat = condensed distance matrix, i.e. an n*(n-1)/2 array representing
-  //             the upper triangle (without diagonal elements) of the distance
-  //             matrix, e.g. for n=4:
-  //               d00 d01 d02 d03
-  //               d10 d11 d12 d13   ->  d01 d02 d03 d12 d13 d23
-  //               d20 d21 d22 d23
-  //               d30 d31 d32 d33
-  //   method  = cluster metric (see enum method_code)
-  // Output arguments:
-  //   merge   = allocated (n-1)x2 matrix (2*(n-1) array) for storing result.
-  //             Result follows R hclust convention:
-  //              - observabe indices start with one
-  //              - merge[i][] contains the merged nodes in step i
-  //              - merge[i][j] is negative when the node is an atom
-  //   height  = allocated (n-1) array with distances at each merge step
-  // Return code:
-  //   0 = ok
-  //   1 = invalid method
-  //
-  int hclust_fast(int n, double* distmat, int method, int* merge, double* height) {
-
-    // call appropriate culstering function
-    cluster_result Z2(n-1);
-    if (method == HCLUST_METHOD_SINGLE) {
-      // single link
-      MST_linkage_core(n, distmat, Z2);
-    }
-    else if (method == HCLUST_METHOD_COMPLETE) {
-      // complete link
-      NN_chain_core<METHOD_METR_COMPLETE, t_float>(n, distmat, NULL, Z2);
-    }
-    else if (method == HCLUST_METHOD_AVERAGE) {
-      // best average distance
-      double* members = new double[n];
-      for (int i=0; i<n; i++) members[i] = 1;
-      NN_chain_core<METHOD_METR_AVERAGE, t_float>(n, distmat, members, Z2);
-      delete[] members;
-    }
-    else if (method == HCLUST_METHOD_MEDIAN) {
-      // best median distance (beware: O(n^3))
-      generic_linkage<METHOD_METR_MEDIAN, t_float>(n, distmat, NULL, Z2);
-    }
-    else if (method == HCLUST_METHOD_CENTROID) {
-      // best centroid distance (beware: O(n^3))
-      double* members = new double[n];
-      for (int i=0; i<n; i++) members[i] = 1;
-      generic_linkage<METHOD_METR_CENTROID, t_float>(n, distmat, members, Z2);
-      delete[] members;
-    }
-    else {
-      return 1;
-    }
-
-    int* order = new int[n];
-    if (method == HCLUST_METHOD_MEDIAN || method == HCLUST_METHOD_CENTROID) {
-      generate_R_dendrogram<true>(merge, height, order, Z2, n);
-    } else {
-      generate_R_dendrogram<false>(merge, height, order, Z2, n);
-    }
-    delete[] order; // only needed for visualization
-
-    return 0;
-  }
-
-
-  // Build condensed distance matrix
-  // Input arguments:
-  //   n  = number of observables
-  //   m  = dimension of observable
-  // Output arguments:
-  //   out = allocated integer array of size n * (n - 1) / 2 for result
-  void hclust_pdist(int n, int m, double* pts, double* out) {
-    int ii = 0;
-    for (int i = 0; i < n; i++) {
-      for (int j = i + 1; j < n; j++) {
-        // Compute euclidian distance
-        double d = 0;
-        for (int k = 0; k < m; k ++) {
-          double error = pts[i * m + k] - pts[j * m + k];
-          d += (error * error);
-        }
-        out[ii] = d;//sqrt(d);
-        ii++;
-      }
-    }
-  }
-
-  void cluster_points_centroid(int n, int m, double* pts, double dist, int* idx) {
-    double* pdist = new double[n * (n - 1) / 2];
-    int* merge = new int[2 * (n - 1)];
-    double* height = new double[n - 1];
-
-    hclust_pdist(n, m, pts, pdist);
-    hclust_fast(n, pdist, HCLUST_METHOD_CENTROID, merge, height);
-    cutree_cdist(n, merge, height, dist, idx);
-
-    delete[] pdist;
-    delete[] merge;
-    delete[] height;
-  }
-}
diff --git a/third_party/cluster/fastcluster.h b/third_party/cluster/fastcluster.h
deleted file mode 100644
index 56c63b6a5e..0000000000
--- a/third_party/cluster/fastcluster.h
+++ /dev/null
@@ -1,77 +0,0 @@
-//
-// C++ standalone verion of fastcluster by Daniel Muellner
-//
-// Copyright: Daniel Muellner, 2011
-//            Christoph Dalitz, 2018
-// License:   BSD style license
-//            (see the file LICENSE for details)
-//
-
-#ifndef fastclustercpp_H
-#define fastclustercpp_H
-
-//
-// Assigns cluster labels (0, ..., nclust-1) to the n points such
-// that the cluster result is split into nclust clusters.
-//
-// Input arguments:
-//   n      = number of observables
-//   merge  = clustering result in R format
-//   nclust = number of clusters
-// Output arguments:
-//   labels = allocated integer array of size n for result
-//
-void cutree_k(int n, const int* merge, int nclust, int* labels);
-
-//
-// Assigns cluster labels (0, ..., nclust-1) to the n points such
-// that the hierarchical clsutering is stopped at cluster distance cdist
-//
-// Input arguments:
-//   n      = number of observables
-//   merge  = clustering result in R format
-//   height = cluster distance at each merge step
-//   cdist  = cutoff cluster distance
-// Output arguments:
-//   labels = allocated integer array of size n for result
-//
-void cutree_cdist(int n, const int* merge, double* height, double cdist, int* labels);
-
-//
-// Hierarchical clustering with one of Daniel Muellner's fast algorithms
-//
-// Input arguments:
-//   n       = number of observables
-//   distmat = condensed distance matrix, i.e. an n*(n-1)/2 array representing
-//             the upper triangle (without diagonal elements) of the distance
-//             matrix, e.g. for n=4:
-//               d00 d01 d02 d03
-//               d10 d11 d12 d13   ->  d01 d02 d03 d12 d13 d23
-//               d20 d21 d22 d23
-//               d30 d31 d32 d33
-//   method  = cluster metric (see enum method_code)
-// Output arguments:
-//   merge   = allocated (n-1)x2 matrix (2*(n-1) array) for storing result.
-//             Result follows R hclust convention:
-//              - observabe indices start with one
-//              - merge[i][] contains the merged nodes in step i
-//              - merge[i][j] is negative when the node is an atom
-//   height  = allocated (n-1) array with distances at each merge step
-// Return code:
-//   0 = ok
-//   1 = invalid method
-//
-int hclust_fast(int n, double* distmat, int method, int* merge, double* height);
-enum hclust_fast_methods {
-  HCLUST_METHOD_SINGLE = 0,
-  HCLUST_METHOD_COMPLETE = 1,
-  HCLUST_METHOD_AVERAGE = 2,
-  HCLUST_METHOD_MEDIAN = 3,
-  HCLUST_METHOD_CENTROID = 5,
-};
-
-void hclust_pdist(int n, int m, double* pts, double* out);
-void cluster_points_centroid(int n, int m, double* pts, double dist, int* idx);
-
-
-#endif
diff --git a/third_party/cluster/fastcluster_R_dm.cpp b/third_party/cluster/fastcluster_R_dm.cpp
deleted file mode 100644
index cbe126c15e..0000000000
--- a/third_party/cluster/fastcluster_R_dm.cpp
+++ /dev/null
@@ -1,115 +0,0 @@
-//
-// Excerpt from fastcluster_R.cpp
-//
-// Copyright: Daniel Müllner, 2011 <http://danifold.net>
-//
-
-struct pos_node {
-  t_index pos;
-  int node;
-};
-
-void order_nodes(const int N, const int * const merge, const t_index * const node_size, int * const order) {
-  /* Parameters:
-     N         : number of data points
-     merge     : (N-1)×2 array which specifies the node indices which are
-                 merged in each step of the clustering procedure.
-                 Negative entries -1...-N point to singleton nodes, while
-                 positive entries 1...(N-1) point to nodes which are themselves
-                 parents of other nodes.
-     node_size : array of node sizes - makes it easier
-     order     : output array of size N
-
-     Runtime: Θ(N)
-  */
-  auto_array_ptr<pos_node> queue(N/2);
-
-  int parent;
-  int child;
-  t_index pos = 0;
-
-  queue[0].pos = 0;
-  queue[0].node = N-2;
-  t_index idx = 1;
-
-  do {
-    --idx;
-    pos = queue[idx].pos;
-    parent = queue[idx].node;
-
-    // First child
-    child = merge[parent];
-    if (child<0) { // singleton node, write this into the 'order' array.
-      order[pos] = -child;
-      ++pos;
-    }
-    else { /* compound node: put it on top of the queue and decompose it
-              in a later iteration. */
-      queue[idx].pos = pos;
-      queue[idx].node = child-1; // convert index-1 based to index-0 based
-      ++idx;
-      pos += node_size[child-1];
-    }
-    // Second child
-    child = merge[parent+N-1];
-    if (child<0) {
-      order[pos] = -child;
-    }
-    else {
-      queue[idx].pos = pos;
-      queue[idx].node = child-1;
-      ++idx;
-    }
-  } while (idx>0);
-}
-
-#define size_(r_) ( ((r_<N) ? 1 : node_size[r_-N]) )
-
-template <const bool sorted>
-void generate_R_dendrogram(int * const merge, double * const height, int * const order, cluster_result & Z2, const int N) {
-  // The array "nodes" is a union-find data structure for the cluster
-  // identites (only needed for unsorted cluster_result input).
-  union_find nodes(sorted ? 0 : N);
-  if (!sorted) {
-    std::stable_sort(Z2[0], Z2[N-1]);
-  }
-
-  t_index node1, node2;
-  auto_array_ptr<t_index> node_size(N-1);
-
-  for (t_index i=0; i<N-1; ++i) {
-    // Get two data points whose clusters are merged in step i.
-    // Find the cluster identifiers for these points.
-    if (sorted) {
-      node1 = Z2[i]->node1;
-      node2 = Z2[i]->node2;
-    }
-    else {
-      node1 = nodes.Find(Z2[i]->node1);
-      node2 = nodes.Find(Z2[i]->node2);
-      // Merge the nodes in the union-find data structure by making them
-      // children of a new node.
-      nodes.Union(node1, node2);
-    }
-    // Sort the nodes in the output array.
-    if (node1>node2) {
-      t_index tmp = node1;
-      node1 = node2;
-      node2 = tmp;
-    }
-    /* Conversion between labeling conventions.
-       Input:  singleton nodes 0,...,N-1
-               compound nodes  N,...,2N-2
-       Output: singleton nodes -1,...,-N
-               compound nodes  1,...,N
-    */
-    merge[i]     = (node1<N) ? -static_cast<int>(node1)-1
-                              : static_cast<int>(node1)-N+1;
-    merge[i+N-1] = (node2<N) ? -static_cast<int>(node2)-1
-                              : static_cast<int>(node2)-N+1;
-    height[i] = Z2[i]->dist;
-    node_size[i] = size_(node1) + size_(node2);
-  }
-
-  order_nodes(N, merge, node_size, order);
-}
diff --git a/third_party/cluster/fastcluster_dm.cpp b/third_party/cluster/fastcluster_dm.cpp
deleted file mode 100644
index ee6670c204..0000000000
--- a/third_party/cluster/fastcluster_dm.cpp
+++ /dev/null
@@ -1,1794 +0,0 @@
-/*
-  fastcluster: Fast hierarchical clustering routines for R and Python
-
-  Copyright © 2011 Daniel Müllner
-  <http://danifold.net>
-
-  This library implements various fast algorithms for hierarchical,
-  agglomerative clustering methods:
-
-  (1) Algorithms for the "stored matrix approach": the input is the array of
-      pairwise dissimilarities.
-
-      MST_linkage_core: single linkage clustering with the "minimum spanning
-      tree algorithm (Rohlfs)
-
-      NN_chain_core: nearest-neighbor-chain algorithm, suitable for single,
-      complete, average, weighted and Ward linkage (Murtagh)
-
-      generic_linkage: generic algorithm, suitable for all distance update
-      formulas (Müllner)
-
-  (2) Algorithms for the "stored data approach": the input are points in a
-      vector space.
-
-      MST_linkage_core_vector: single linkage clustering for vector data
-
-      generic_linkage_vector: generic algorithm for vector data, suitable for
-      the Ward, centroid and median methods.
-
-      generic_linkage_vector_alternative: alternative scheme for updating the
-      nearest neighbors. This method seems faster than "generic_linkage_vector"
-      for the centroid and median methods but slower for the Ward method.
-
-  All these implementation treat infinity values correctly. They throw an
-  exception if a NaN distance value occurs.
-*/
-
-// Older versions of Microsoft Visual Studio do not have the fenv header.
-#ifdef _MSC_VER
-#if (_MSC_VER == 1500 || _MSC_VER == 1600)
-#define NO_INCLUDE_FENV
-#endif
-#endif
-// NaN detection via fenv might not work on systems with software
-// floating-point emulation (bug report for Debian armel).
-#ifdef __SOFTFP__
-#define NO_INCLUDE_FENV
-#endif
-#ifdef NO_INCLUDE_FENV
-#pragma message("Do not use fenv header.")
-#else
-//#pragma message("Use fenv header. If there is a warning about unknown #pragma STDC FENV_ACCESS, this can be ignored.")
-//#pragma STDC FENV_ACCESS on
-#include <fenv.h>
-#endif
-
-#include <cmath> // for std::pow, std::sqrt
-#include <cstddef> // for std::ptrdiff_t
-#include <limits> // for std::numeric_limits<...>::infinity()
-#include <algorithm> // for std::fill_n
-#include <stdexcept> // for std::runtime_error
-#include <string> // for std::string
-
-#include <cfloat> // also for DBL_MAX, DBL_MIN
-#ifndef DBL_MANT_DIG
-#error The constant DBL_MANT_DIG could not be defined.
-#endif
-#define T_FLOAT_MANT_DIG DBL_MANT_DIG
-
-#ifndef LONG_MAX
-#include <climits>
-#endif
-#ifndef LONG_MAX
-#error The constant LONG_MAX could not be defined.
-#endif
-#ifndef INT_MAX
-#error The constant INT_MAX could not be defined.
-#endif
-
-#ifndef INT32_MAX
-#ifdef _MSC_VER
-#if _MSC_VER >= 1600
-#define __STDC_LIMIT_MACROS
-#include <stdint.h>
-#else
-typedef __int32 int_fast32_t;
-typedef __int64 int64_t;
-#endif
-#else
-#define __STDC_LIMIT_MACROS
-#include <stdint.h>
-#endif
-#endif
-
-#define FILL_N std::fill_n
-#ifdef _MSC_VER
-#if _MSC_VER < 1600
-#undef FILL_N
-#define FILL_N stdext::unchecked_fill_n
-#endif
-#endif
-
-// Suppress warnings about (potentially) uninitialized variables.
-#ifdef _MSC_VER
-	#pragma warning (disable:4700)
-#endif
-
-#ifndef HAVE_DIAGNOSTIC
-#if __GNUC__ > 4 || (__GNUC__ == 4 && (__GNUC_MINOR__ >= 6))
-#define HAVE_DIAGNOSTIC 1
-#endif
-#endif
-
-#ifndef HAVE_VISIBILITY
-#if __GNUC__ >= 4
-#define HAVE_VISIBILITY 1
-#endif
-#endif
-
-/* Since the public interface is given by the Python respectively R interface,
- * we do not want other symbols than the interface initalization routines to be
- * visible in the shared object file. The "visibility" switch is a GCC concept.
- * Hiding symbols keeps the relocation table small and decreases startup time.
- * See http://gcc.gnu.org/wiki/Visibility
- */
-#if HAVE_VISIBILITY
-#pragma GCC visibility push(hidden)
-#endif
-
-typedef int_fast32_t t_index;
-#ifndef INT32_MAX
-#define MAX_INDEX 0x7fffffffL
-#else
-#define MAX_INDEX INT32_MAX
-#endif
-#if (LONG_MAX < MAX_INDEX)
-#error The integer format "t_index" must not have a greater range than "long int".
-#endif
-#if (INT_MAX > MAX_INDEX)
-#error The integer format "int" must not have a greater range than "t_index".
-#endif
-typedef double t_float;
-
-/* Method codes.
-
-   These codes must agree with the METHODS array in fastcluster.R and the
-   dictionary mthidx in fastcluster.py.
-*/
-enum method_codes {
-  // non-Euclidean methods
-  METHOD_METR_SINGLE           = 0,
-  METHOD_METR_COMPLETE         = 1,
-  METHOD_METR_AVERAGE          = 2,
-  METHOD_METR_WEIGHTED         = 3,
-  METHOD_METR_WARD             = 4,
-  METHOD_METR_WARD_D           = METHOD_METR_WARD,
-  METHOD_METR_CENTROID         = 5,
-  METHOD_METR_MEDIAN           = 6,
-  METHOD_METR_WARD_D2          = 7,
-
-  MIN_METHOD_CODE              = 0,
-  MAX_METHOD_CODE              = 7
-};
-
-enum method_codes_vector {
-  // Euclidean methods
-  METHOD_VECTOR_SINGLE         = 0,
-  METHOD_VECTOR_WARD           = 1,
-  METHOD_VECTOR_CENTROID       = 2,
-  METHOD_VECTOR_MEDIAN         = 3,
-
-  MIN_METHOD_VECTOR_CODE       = 0,
-  MAX_METHOD_VECTOR_CODE       = 3
-};
-
-// self-destructing array pointer
-template <typename type>
-class auto_array_ptr{
-private:
-  type * ptr;
-  auto_array_ptr(auto_array_ptr const &); // non construction-copyable
-  auto_array_ptr& operator=(auto_array_ptr const &); // non copyable
-public:
-  auto_array_ptr()
-    : ptr(NULL)
-  { }
-  template <typename index>
-  auto_array_ptr(index const size)
-    : ptr(new type[size])
-  { }
-  template <typename index, typename value>
-  auto_array_ptr(index const size, value const val)
-    : ptr(new type[size])
-  {
-    FILL_N(ptr, size, val);
-  }
-  ~auto_array_ptr() {
-    delete [] ptr; }
-  void free() {
-    delete [] ptr;
-    ptr = NULL;
-  }
-  template <typename index>
-  void init(index const size) {
-    ptr = new type [size];
-  }
-  template <typename index, typename value>
-  void init(index const size, value const val) {
-    init(size);
-    FILL_N(ptr, size, val);
-  }
-  inline operator type *() const { return ptr; }
-};
-
-struct node {
-  t_index node1, node2;
-  t_float dist;
-};
-
-inline bool operator< (const node a, const node b) {
-  return (a.dist < b.dist);
-}
-
-class cluster_result {
-private:
-  auto_array_ptr<node> Z;
-  t_index pos;
-
-public:
-  cluster_result(const t_index size)
-    : Z(size)
-    , pos(0)
-  {}
-
-  void append(const t_index node1, const t_index node2, const t_float dist) {
-    Z[pos].node1 = node1;
-    Z[pos].node2 = node2;
-    Z[pos].dist  = dist;
-    ++pos;
-  }
-
-  node * operator[] (const t_index idx) const { return Z + idx; }
-
-  /* Define several methods to postprocess the distances. All these functions
-     are monotone, so they do not change the sorted order of distances. */
-
-  void sqrt() const {
-    for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) {
-      ZZ->dist = std::sqrt(ZZ->dist);
-    }
-  }
-
-  void sqrt(const t_float) const { // ignore the argument
-    sqrt();
-  }
-
-  void sqrtdouble(const t_float) const { // ignore the argument
-    for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) {
-      ZZ->dist = std::sqrt(2*ZZ->dist);
-    }
-  }
-
-  #ifdef R_pow
-  #define my_pow R_pow
-  #else
-  #define my_pow std::pow
-  #endif
-
-  void power(const t_float p) const {
-    t_float const q = 1/p;
-    for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) {
-      ZZ->dist = my_pow(ZZ->dist,q);
-    }
-  }
-
-  void plusone(const t_float) const { // ignore the argument
-    for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) {
-      ZZ->dist += 1;
-    }
-  }
-
-  void divide(const t_float denom) const {
-    for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) {
-      ZZ->dist /= denom;
-    }
-  }
-};
-
-class doubly_linked_list {
-  /*
-    Class for a doubly linked list. Initially, the list is the integer range
-    [0, size]. We provide a forward iterator and a method to delete an index
-    from the list.
-
-    Typical use: for (i=L.start; L<size; i=L.succ[I])
-    or
-    for (i=somevalue; L<size; i=L.succ[I])
-  */
-public:
-  t_index start;
-  auto_array_ptr<t_index> succ;
-
-private:
-  auto_array_ptr<t_index> pred;
-  // Not necessarily private, we just do not need it in this instance.
-
-public:
-  doubly_linked_list(const t_index size)
-    // Initialize to the given size.
-    : start(0)
-    , succ(size+1)
-    , pred(size+1)
-  {
-    for (t_index i=0; i<size; ++i) {
-      pred[i+1] = i;
-      succ[i] = i+1;
-    }
-    // pred[0] is never accessed!
-    //succ[size] is never accessed!
-  }
-
-  ~doubly_linked_list() {}
-
-  void remove(const t_index idx) {
-    // Remove an index from the list.
-    if (idx==start) {
-      start = succ[idx];
-    }
-    else {
-      succ[pred[idx]] = succ[idx];
-      pred[succ[idx]] = pred[idx];
-    }
-    succ[idx] = 0; // Mark as inactive
-  }
-
-  bool is_inactive(t_index idx) const {
-    return (succ[idx]==0);
-  }
-};
-
-// Indexing functions
-// D is the upper triangular part of a symmetric (NxN)-matrix
-// We require r_ < c_ !
-#define D_(r_,c_) ( D[(static_cast<std::ptrdiff_t>(2*N-3-(r_))*(r_)>>1)+(c_)-1] )
-// Z is an ((N-1)x4)-array
-#define Z_(_r, _c) (Z[(_r)*4 + (_c)])
-
-/*
-  Lookup function for a union-find data structure.
-
-  The function finds the root of idx by going iteratively through all
-  parent elements until a root is found. An element i is a root if
-  nodes[i] is zero. To make subsequent searches faster, the entry for
-  idx and all its parents is updated with the root element.
- */
-class union_find {
-private:
-  auto_array_ptr<t_index> parent;
-  t_index nextparent;
-
-public:
-  union_find(const t_index size)
-    : parent(size>0 ? 2*size-1 : 0, 0)
-    , nextparent(size)
-  { }
-
-  t_index Find (t_index idx) const {
-    if (parent[idx] != 0 ) { // a → b
-      t_index p = idx;
-      idx = parent[idx];
-      if (parent[idx] != 0 ) { // a → b → c
-        do {
-          idx = parent[idx];
-        } while (parent[idx] != 0);
-        do {
-          t_index tmp = parent[p];
-          parent[p] = idx;
-          p = tmp;
-        } while (parent[p] != idx);
-      }
-    }
-    return idx;
-  }
-
-  void Union (const t_index node1, const t_index node2) {
-    parent[node1] = parent[node2] = nextparent++;
-  }
-};
-
-class nan_error{};
-#ifdef FE_INVALID
-class fenv_error{};
-#endif
-
-static void MST_linkage_core(const t_index N, const t_float * const D,
-                             cluster_result & Z2) {
-/*
-    N: integer, number of data points
-    D: condensed distance matrix N*(N-1)/2
-    Z2: output data structure
-
-    The basis of this algorithm is an algorithm by Rohlf:
-
-    F. James Rohlf, Hierarchical clustering using the minimum spanning tree,
-    The Computer Journal, vol. 16, 1973, p. 93–95.
-*/
-  t_index i;
-  t_index idx2;
-  doubly_linked_list active_nodes(N);
-  auto_array_ptr<t_float> d(N);
-
-  t_index prev_node;
-  t_float min;
-
-  // first iteration
-  idx2 = 1;
-  min = std::numeric_limits<t_float>::infinity();
-  for (i=1; i<N; ++i) {
-    d[i] = D[i-1];
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wfloat-equal"
-#endif
-    if (d[i] < min) {
-      min = d[i];
-      idx2 = i;
-    }
-    else if (fc_isnan(d[i]))
-      throw (nan_error());
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic pop
-#endif
-  }
-  Z2.append(0, idx2, min);
-
-  for (t_index j=1; j<N-1; ++j) {
-    prev_node = idx2;
-    active_nodes.remove(prev_node);
-
-    idx2 = active_nodes.succ[0];
-    min = d[idx2];
-    for (i=idx2; i<prev_node; i=active_nodes.succ[i]) {
-      t_float tmp = D_(i, prev_node);
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wfloat-equal"
-#endif
-      if (tmp < d[i])
-        d[i] = tmp;
-      else if (fc_isnan(tmp))
-        throw (nan_error());
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic pop
-#endif
-      if (d[i] < min) {
-        min = d[i];
-        idx2 = i;
-      }
-    }
-    for (; i<N; i=active_nodes.succ[i]) {
-      t_float tmp = D_(prev_node, i);
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wfloat-equal"
-#endif
-      if (d[i] > tmp)
-        d[i] = tmp;
-      else if (fc_isnan(tmp))
-        throw (nan_error());
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic pop
-#endif
-      if (d[i] < min) {
-        min = d[i];
-        idx2 = i;
-      }
-    }
-    Z2.append(prev_node, idx2, min);
-  }
-}
-
-/* Functions for the update of the dissimilarity array */
-
-inline static void f_single( t_float * const b, const t_float a ) {
-  if (*b > a) *b = a;
-}
-inline static void f_complete( t_float * const b, const t_float a ) {
-  if (*b < a) *b = a;
-}
-inline static void f_average( t_float * const b, const t_float a, const t_float s, const t_float t) {
-  *b = s*a + t*(*b);
-  #ifndef FE_INVALID
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wfloat-equal"
-#endif
-  if (fc_isnan(*b)) {
-    throw(nan_error());
-  }
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic pop
-#endif
-  #endif
-}
-inline static void f_weighted( t_float * const b, const t_float a) {
-  *b = (a+*b)*.5;
-  #ifndef FE_INVALID
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wfloat-equal"
-#endif
-  if (fc_isnan(*b)) {
-    throw(nan_error());
-  }
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic pop
-#endif
-  #endif
-}
-inline static void f_ward( t_float * const b, const t_float a, const t_float c, const t_float s, const t_float t, const t_float v) {
-  *b = ( (v+s)*a - v*c + (v+t)*(*b) ) / (s+t+v);
-  //*b = a+(*b)-(t*a+s*(*b)+v*c)/(s+t+v);
-  #ifndef FE_INVALID
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wfloat-equal"
-#endif
-  if (fc_isnan(*b)) {
-    throw(nan_error());
-  }
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic pop
-#endif
-  #endif
-}
-inline static void f_centroid( t_float * const b, const t_float a, const t_float stc, const t_float s, const t_float t) {
-  *b = s*a - stc + t*(*b);
-  #ifndef FE_INVALID
-  if (fc_isnan(*b)) {
-    throw(nan_error());
-  }
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic pop
-#endif
-  #endif
-}
-inline static void f_median( t_float * const b, const t_float a, const t_float c_4) {
-  *b = (a+(*b))*.5 - c_4;
-  #ifndef FE_INVALID
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wfloat-equal"
-#endif
-  if (fc_isnan(*b)) {
-    throw(nan_error());
-  }
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic pop
-#endif
-  #endif
-}
-
-template <method_codes method, typename t_members>
-static void NN_chain_core(const t_index N, t_float * const D, t_members * const members, cluster_result & Z2) {
-/*
-    N: integer
-    D: condensed distance matrix N*(N-1)/2
-    Z2: output data structure
-
-    This is the NN-chain algorithm, described on page 86 in the following book:
-
-    Fionn Murtagh, Multidimensional Clustering Algorithms,
-    Vienna, Würzburg: Physica-Verlag, 1985.
-*/
-  t_index i;
-
-  auto_array_ptr<t_index> NN_chain(N);
-  t_index NN_chain_tip = 0;
-
-  t_index idx1, idx2;
-
-  t_float size1, size2;
-  doubly_linked_list active_nodes(N);
-
-  t_float min;
-
-  for (t_float const * DD=D; DD!=D+(static_cast<std::ptrdiff_t>(N)*(N-1)>>1);
-       ++DD) {
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wfloat-equal"
-#endif
-    if (fc_isnan(*DD)) {
-      throw(nan_error());
-    }
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic pop
-#endif
-  }
-
-  #ifdef FE_INVALID
-  if (feclearexcept(FE_INVALID)) throw fenv_error();
-  #endif
-
-  for (t_index j=0; j<N-1; ++j) {
-    if (NN_chain_tip <= 3) {
-      NN_chain[0] = idx1 = active_nodes.start;
-      NN_chain_tip = 1;
-
-      idx2 = active_nodes.succ[idx1];
-      min = D_(idx1,idx2);
-      for (i=active_nodes.succ[idx2]; i<N; i=active_nodes.succ[i]) {
-        if (D_(idx1,i) < min) {
-          min = D_(idx1,i);
-          idx2 = i;
-        }
-      }
-    }  // a: idx1   b: idx2
-    else {
-      NN_chain_tip -= 3;
-      idx1 = NN_chain[NN_chain_tip-1];
-      idx2 = NN_chain[NN_chain_tip];
-      min = idx1<idx2 ? D_(idx1,idx2) : D_(idx2,idx1);
-    }  // a: idx1   b: idx2
-
-    do {
-      NN_chain[NN_chain_tip] = idx2;
-
-      for (i=active_nodes.start; i<idx2; i=active_nodes.succ[i]) {
-        if (D_(i,idx2) < min) {
-          min = D_(i,idx2);
-          idx1 = i;
-        }
-      }
-      for (i=active_nodes.succ[idx2]; i<N; i=active_nodes.succ[i]) {
-        if (D_(idx2,i) < min) {
-          min = D_(idx2,i);
-          idx1 = i;
-        }
-      }
-
-      idx2 = idx1;
-      idx1 = NN_chain[NN_chain_tip++];
-
-    } while (idx2 != NN_chain[NN_chain_tip-2]);
-
-    Z2.append(idx1, idx2, min);
-
-    if (idx1>idx2) {
-      t_index tmp = idx1;
-      idx1 = idx2;
-      idx2 = tmp;
-    }
-
-    if (method==METHOD_METR_AVERAGE ||
-        method==METHOD_METR_WARD) {
-      size1 = static_cast<t_float>(members[idx1]);
-      size2 = static_cast<t_float>(members[idx2]);
-      members[idx2] += members[idx1];
-    }
-
-    // Remove the smaller index from the valid indices (active_nodes).
-    active_nodes.remove(idx1);
-
-    switch (method) {
-    case METHOD_METR_SINGLE:
-      /*
-      Single linkage.
-
-      Characteristic: new distances are never longer than the old distances.
-      */
-      // Update the distance matrix in the range [start, idx1).
-      for (i=active_nodes.start; i<idx1; i=active_nodes.succ[i])
-        f_single(&D_(i, idx2), D_(i, idx1) );
-      // Update the distance matrix in the range (idx1, idx2).
-      for (; i<idx2; i=active_nodes.succ[i])
-        f_single(&D_(i, idx2), D_(idx1, i) );
-      // Update the distance matrix in the range (idx2, N).
-      for (i=active_nodes.succ[idx2]; i<N; i=active_nodes.succ[i])
-        f_single(&D_(idx2, i), D_(idx1, i) );
-      break;
-
-    case METHOD_METR_COMPLETE:
-      /*
-      Complete linkage.
-
-      Characteristic: new distances are never shorter than the old distances.
-      */
-      // Update the distance matrix in the range [start, idx1).
-      for (i=active_nodes.start; i<idx1; i=active_nodes.succ[i])
-        f_complete(&D_(i, idx2), D_(i, idx1) );
-      // Update the distance matrix in the range (idx1, idx2).
-      for (; i<idx2; i=active_nodes.succ[i])
-        f_complete(&D_(i, idx2), D_(idx1, i) );
-      // Update the distance matrix in the range (idx2, N).
-      for (i=active_nodes.succ[idx2]; i<N; i=active_nodes.succ[i])
-        f_complete(&D_(idx2, i), D_(idx1, i) );
-      break;
-
-    case METHOD_METR_AVERAGE: {
-      /*
-      Average linkage.
-
-      Shorter and longer distances can occur.
-      */
-      // Update the distance matrix in the range [start, idx1).
-      t_float s = size1/(size1+size2);
-      t_float t = size2/(size1+size2);
-      for (i=active_nodes.start; i<idx1; i=active_nodes.succ[i])
-        f_average(&D_(i, idx2), D_(i, idx1), s, t );
-      // Update the distance matrix in the range (idx1, idx2).
-      for (; i<idx2; i=active_nodes.succ[i])
-        f_average(&D_(i, idx2), D_(idx1, i), s, t );
-      // Update the distance matrix in the range (idx2, N).
-      for (i=active_nodes.succ[idx2]; i<N; i=active_nodes.succ[i])
-        f_average(&D_(idx2, i), D_(idx1, i), s, t );
-      break;
-    }
-
-    case METHOD_METR_WEIGHTED:
-      /*
-      Weighted linkage.
-
-      Shorter and longer distances can occur.
-      */
-      // Update the distance matrix in the range [start, idx1).
-      for (i=active_nodes.start; i<idx1; i=active_nodes.succ[i])
-        f_weighted(&D_(i, idx2), D_(i, idx1) );
-      // Update the distance matrix in the range (idx1, idx2).
-      for (; i<idx2; i=active_nodes.succ[i])
-        f_weighted(&D_(i, idx2), D_(idx1, i) );
-      // Update the distance matrix in the range (idx2, N).
-      for (i=active_nodes.succ[idx2]; i<N; i=active_nodes.succ[i])
-        f_weighted(&D_(idx2, i), D_(idx1, i) );
-      break;
-
-    case METHOD_METR_WARD:
-      /*
-      Ward linkage.
-
-      Shorter and longer distances can occur, not smaller than min(d1,d2)
-      but maybe bigger than max(d1,d2).
-      */
-      // Update the distance matrix in the range [start, idx1).
-      //t_float v = static_cast<t_float>(members[i]);
-      for (i=active_nodes.start; i<idx1; i=active_nodes.succ[i])
-        f_ward(&D_(i, idx2), D_(i, idx1), min,
-               size1, size2, static_cast<t_float>(members[i]) );
-      // Update the distance matrix in the range (idx1, idx2).
-      for (; i<idx2; i=active_nodes.succ[i])
-        f_ward(&D_(i, idx2), D_(idx1, i), min,
-               size1, size2, static_cast<t_float>(members[i]) );
-      // Update the distance matrix in the range (idx2, N).
-      for (i=active_nodes.succ[idx2]; i<N; i=active_nodes.succ[i])
-        f_ward(&D_(idx2, i), D_(idx1, i), min,
-               size1, size2, static_cast<t_float>(members[i]) );
-      break;
-
-    default:
-      throw std::runtime_error(std::string("Invalid method."));
-    }
-  }
-  #ifdef FE_INVALID
-  if (fetestexcept(FE_INVALID)) throw fenv_error();
-  #endif
-}
-
-class binary_min_heap {
-  /*
-  Class for a binary min-heap. The data resides in an array A. The elements of
-  A are not changed but two lists I and R of indices are generated which point
-  to elements of A and backwards.
-
-  The heap tree structure is
-
-     H[2*i+1]     H[2*i+2]
-         \            /
-          \          /
-           ≤        ≤
-            \      /
-             \    /
-              H[i]
-
-  where the children must be less or equal than their parent. Thus, H[0]
-  contains the minimum. The lists I and R are made such that H[i] = A[I[i]]
-  and R[I[i]] = i.
-
-  This implementation is not designed to handle NaN values.
-  */
-private:
-  t_float * const A;
-  t_index size;
-  auto_array_ptr<t_index> I;
-  auto_array_ptr<t_index> R;
-
-  // no default constructor
-  binary_min_heap();
-  // noncopyable
-  binary_min_heap(binary_min_heap const &);
-  binary_min_heap & operator=(binary_min_heap const &);
-
-public:
-  binary_min_heap(t_float * const A_, const t_index size_)
-    : A(A_), size(size_), I(size), R(size)
-  { // Allocate memory and initialize the lists I and R to the identity. This
-    // does not make it a heap. Call heapify afterwards!
-    for (t_index i=0; i<size; ++i)
-      R[i] = I[i] = i;
-  }
-
-  binary_min_heap(t_float * const A_, const t_index size1, const t_index size2,
-                  const t_index start)
-    : A(A_), size(size1), I(size1), R(size2)
-  { // Allocate memory and initialize the lists I and R to the identity. This
-    // does not make it a heap. Call heapify afterwards!
-    for (t_index i=0; i<size; ++i) {
-      R[i+start] = i;
-      I[i] = i + start;
-    }
-  }
-
-  ~binary_min_heap() {}
-
-  void heapify() {
-    // Arrange the indices I and R so that H[i] := A[I[i]] satisfies the heap
-    // condition H[i] < H[2*i+1] and H[i] < H[2*i+2] for each i.
-    //
-    // Complexity: Θ(size)
-    // Reference: Cormen, Leiserson, Rivest, Stein, Introduction to Algorithms,
-    // 3rd ed., 2009, Section 6.3 “Building a heap”
-    t_index idx;
-    for (idx=(size>>1); idx>0; ) {
-      --idx;
-      update_geq_(idx);
-    }
-  }
-
-  inline t_index argmin() const {
-    // Return the minimal element.
-    return I[0];
-  }
-
-  void heap_pop() {
-    // Remove the minimal element from the heap.
-    --size;
-    I[0] = I[size];
-    R[I[0]] = 0;
-    update_geq_(0);
-  }
-
-  void remove(t_index idx) {
-    // Remove an element from the heap.
-    --size;
-    R[I[size]] = R[idx];
-    I[R[idx]] = I[size];
-    if ( H(size)<=A[idx] ) {
-      update_leq_(R[idx]);
-    }
-    else {
-      update_geq_(R[idx]);
-    }
-  }
-
-  void replace ( const t_index idxold, const t_index idxnew,
-                 const t_float val) {
-    R[idxnew] = R[idxold];
-    I[R[idxnew]] = idxnew;
-    if (val<=A[idxold])
-      update_leq(idxnew, val);
-    else
-      update_geq(idxnew, val);
-  }
-
-  void update ( const t_index idx, const t_float val ) const {
-    // Update the element A[i] with val and re-arrange the indices to preserve
-    // the heap condition.
-    if (val<=A[idx])
-      update_leq(idx, val);
-    else
-      update_geq(idx, val);
-  }
-
-  void update_leq ( const t_index idx, const t_float val ) const {
-    // Use this when the new value is not more than the old value.
-    A[idx] = val;
-    update_leq_(R[idx]);
-  }
-
-  void update_geq ( const t_index idx, const t_float val ) const {
-    // Use this when the new value is not less than the old value.
-    A[idx] = val;
-    update_geq_(R[idx]);
-  }
-
-private:
-  void update_leq_ (t_index i) const {
-    t_index j;
-    for ( ; (i>0) && ( H(i)<H(j=(i-1)>>1) ); i=j)
-      heap_swap(i,j);
-  }
-
-  void update_geq_ (t_index i) const {
-    t_index j;
-    for ( ; (j=2*i+1)<size; i=j) {
-      if ( H(j)>=H(i) ) {
-        ++j;
-        if ( j>=size || H(j)>=H(i) ) break;
-      }
-      else if ( j+1<size && H(j+1)<H(j) ) ++j;
-      heap_swap(i, j);
-    }
-  }
-
-  void heap_swap(const t_index i, const t_index j) const {
-    // Swap two indices.
-    t_index tmp = I[i];
-    I[i] = I[j];
-    I[j] = tmp;
-    R[I[i]] = i;
-    R[I[j]] = j;
-  }
-
-  inline t_float H(const t_index i) const {
-    return A[I[i]];
-  }
-
-};
-
-template <method_codes method, typename t_members>
-static void generic_linkage(const t_index N, t_float * const D, t_members * const members, cluster_result & Z2) {
-  /*
-    N: integer, number of data points
-    D: condensed distance matrix N*(N-1)/2
-    Z2: output data structure
-  */
-
-  const t_index N_1 = N-1;
-  t_index i, j; // loop variables
-  t_index idx1, idx2; // row and column indices
-
-  auto_array_ptr<t_index> n_nghbr(N_1); // array of nearest neighbors
-  auto_array_ptr<t_float> mindist(N_1); // distances to the nearest neighbors
-  auto_array_ptr<t_index> row_repr(N); // row_repr[i]: node number that the
-                                       // i-th row represents
-  doubly_linked_list active_nodes(N);
-  binary_min_heap nn_distances(&*mindist, N_1); // minimum heap structure for
-                        // the distance to the nearest neighbor of each point
-  t_index node1, node2; // node numbers in the output
-  t_float size1, size2; // and their cardinalities
-
-  t_float min; // minimum and row index for nearest-neighbor search
-  t_index idx;
-
-  for (i=0; i<N; ++i)
-    // Build a list of row ↔ node label assignments.
-    // Initially i ↦ i
-    row_repr[i] = i;
-
-  // Initialize the minimal distances:
-  // Find the nearest neighbor of each point.
-  // n_nghbr[i] = argmin_{j>i} D(i,j) for i in range(N-1)
-  t_float const * DD = D;
-  for (i=0; i<N_1; ++i) {
-    min = std::numeric_limits<t_float>::infinity();
-    for (idx=j=i+1; j<N; ++j, ++DD) {
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wfloat-equal"
-#endif
-      if (*DD<min) {
-        min = *DD;
-        idx = j;
-      }
-      else if (fc_isnan(*DD))
-        throw(nan_error());
-    }
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic pop
-#endif
-    mindist[i] = min;
-    n_nghbr[i] = idx;
-  }
-
-  // Put the minimal distances into a heap structure to make the repeated
-  // global minimum searches fast.
-  nn_distances.heapify();
-
-  #ifdef FE_INVALID
-  if (feclearexcept(FE_INVALID)) throw fenv_error();
-  #endif
-
-  // Main loop: We have N-1 merging steps.
-  for (i=0; i<N_1; ++i) {
-    /*
-      Here is a special feature that allows fast bookkeeping and updates of the
-      minimal distances.
-
-      mindist[i] stores a lower bound on the minimum distance of the point i to
-      all points of higher index:
-
-          mindist[i] ≥ min_{j>i} D(i,j)
-
-      Normally, we have equality. However, this minimum may become invalid due
-      to the updates in the distance matrix. The rules are:
-
-      1) If mindist[i] is equal to D(i, n_nghbr[i]), this is the correct
-         minimum and n_nghbr[i] is a nearest neighbor.
-
-      2) If mindist[i] is smaller than D(i, n_nghbr[i]), this might not be the
-         correct minimum. The minimum needs to be recomputed.
-
-      3) mindist[i] is never bigger than the true minimum. Hence, we never
-         miss the true minimum if we take the smallest mindist entry,
-         re-compute the value if necessary (thus maybe increasing it) and
-         looking for the now smallest mindist entry until a valid minimal
-         entry is found. This step is done in the lines below.
-
-      The update process for D below takes care that these rules are
-      fulfilled. This makes sure that the minima in the rows D(i,i+1:)of D are
-      re-calculated when necessary but re-calculation is avoided whenever
-      possible.
-
-      The re-calculation of the minima makes the worst-case runtime of this
-      algorithm cubic in N. We avoid this whenever possible, and in most cases
-      the runtime appears to be quadratic.
-    */
-    idx1 = nn_distances.argmin();
-    if (method != METHOD_METR_SINGLE) {
-      while ( mindist[idx1] < D_(idx1, n_nghbr[idx1]) ) {
-        // Recompute the minimum mindist[idx1] and n_nghbr[idx1].
-        n_nghbr[idx1] = j = active_nodes.succ[idx1]; // exists, maximally N-1
-        min = D_(idx1,j);
-        for (j=active_nodes.succ[j]; j<N; j=active_nodes.succ[j]) {
-          if (D_(idx1,j)<min) {
-            min = D_(idx1,j);
-            n_nghbr[idx1] = j;
-          }
-        }
-        /* Update the heap with the new true minimum and search for the
-           (possibly different) minimal entry. */
-        nn_distances.update_geq(idx1, min);
-        idx1 = nn_distances.argmin();
-      }
-    }
-
-    nn_distances.heap_pop(); // Remove the current minimum from the heap.
-    idx2 = n_nghbr[idx1];
-
-    // Write the newly found minimal pair of nodes to the output array.
-    node1 = row_repr[idx1];
-    node2 = row_repr[idx2];
-
-    if (method==METHOD_METR_AVERAGE ||
-        method==METHOD_METR_WARD ||
-        method==METHOD_METR_CENTROID) {
-      size1 = static_cast<t_float>(members[idx1]);
-      size2 = static_cast<t_float>(members[idx2]);
-      members[idx2] += members[idx1];
-    }
-    Z2.append(node1, node2, mindist[idx1]);
-
-    // Remove idx1 from the list of active indices (active_nodes).
-    active_nodes.remove(idx1);
-    // Index idx2 now represents the new (merged) node with label N+i.
-    row_repr[idx2] = N+i;
-
-    // Update the distance matrix
-    switch (method) {
-    case METHOD_METR_SINGLE:
-      /*
-        Single linkage.
-
-        Characteristic: new distances are never longer than the old distances.
-      */
-      // Update the distance matrix in the range [start, idx1).
-      for (j=active_nodes.start; j<idx1; j=active_nodes.succ[j]) {
-        f_single(&D_(j, idx2), D_(j, idx1));
-        if (n_nghbr[j] == idx1)
-          n_nghbr[j] = idx2;
-      }
-      // Update the distance matrix in the range (idx1, idx2).
-      for (; j<idx2; j=active_nodes.succ[j]) {
-        f_single(&D_(j, idx2), D_(idx1, j));
-        // If the new value is below the old minimum in a row, update
-        // the mindist and n_nghbr arrays.
-        if (D_(j, idx2) < mindist[j]) {
-          nn_distances.update_leq(j, D_(j, idx2));
-          n_nghbr[j] = idx2;
-        }
-      }
-      // Update the distance matrix in the range (idx2, N).
-      // Recompute the minimum mindist[idx2] and n_nghbr[idx2].
-      if (idx2<N_1) {
-        min = mindist[idx2];
-        for (j=active_nodes.succ[idx2]; j<N; j=active_nodes.succ[j]) {
-          f_single(&D_(idx2, j), D_(idx1, j) );
-          if (D_(idx2, j) < min) {
-            n_nghbr[idx2] = j;
-            min = D_(idx2, j);
-          }
-        }
-        nn_distances.update_leq(idx2, min);
-      }
-      break;
-
-    case METHOD_METR_COMPLETE:
-      /*
-        Complete linkage.
-
-        Characteristic: new distances are never shorter than the old distances.
-      */
-      // Update the distance matrix in the range [start, idx1).
-      for (j=active_nodes.start; j<idx1; j=active_nodes.succ[j]) {
-        f_complete(&D_(j, idx2), D_(j, idx1) );
-        if (n_nghbr[j] == idx1)
-          n_nghbr[j] = idx2;
-      }
-      // Update the distance matrix in the range (idx1, idx2).
-      for (; j<idx2; j=active_nodes.succ[j])
-        f_complete(&D_(j, idx2), D_(idx1, j) );
-      // Update the distance matrix in the range (idx2, N).
-      for (j=active_nodes.succ[idx2]; j<N; j=active_nodes.succ[j])
-        f_complete(&D_(idx2, j), D_(idx1, j) );
-      break;
-
-    case METHOD_METR_AVERAGE: {
-      /*
-        Average linkage.
-
-        Shorter and longer distances can occur.
-      */
-      // Update the distance matrix in the range [start, idx1).
-      t_float s = size1/(size1+size2);
-      t_float t = size2/(size1+size2);
-      for (j=active_nodes.start; j<idx1; j=active_nodes.succ[j]) {
-        f_average(&D_(j, idx2), D_(j, idx1), s, t);
-        if (n_nghbr[j] == idx1)
-          n_nghbr[j] = idx2;
-      }
-      // Update the distance matrix in the range (idx1, idx2).
-      for (; j<idx2; j=active_nodes.succ[j]) {
-        f_average(&D_(j, idx2), D_(idx1, j), s, t);
-        if (D_(j, idx2) < mindist[j]) {
-          nn_distances.update_leq(j, D_(j, idx2));
-          n_nghbr[j] = idx2;
-        }
-      }
-      // Update the distance matrix in the range (idx2, N).
-      if (idx2<N_1) {
-        n_nghbr[idx2] = j = active_nodes.succ[idx2]; // exists, maximally N-1
-        f_average(&D_(idx2, j), D_(idx1, j), s, t);
-        min = D_(idx2,j);
-        for (j=active_nodes.succ[j]; j<N; j=active_nodes.succ[j]) {
-          f_average(&D_(idx2, j), D_(idx1, j), s, t);
-          if (D_(idx2,j) < min) {
-            min = D_(idx2,j);
-            n_nghbr[idx2] = j;
-          }
-        }
-        nn_distances.update(idx2, min);
-      }
-      break;
-    }
-
-    case METHOD_METR_WEIGHTED:
-      /*
-        Weighted linkage.
-
-        Shorter and longer distances can occur.
-      */
-      // Update the distance matrix in the range [start, idx1).
-      for (j=active_nodes.start; j<idx1; j=active_nodes.succ[j]) {
-        f_weighted(&D_(j, idx2), D_(j, idx1) );
-        if (n_nghbr[j] == idx1)
-          n_nghbr[j] = idx2;
-      }
-      // Update the distance matrix in the range (idx1, idx2).
-      for (; j<idx2; j=active_nodes.succ[j]) {
-        f_weighted(&D_(j, idx2), D_(idx1, j) );
-        if (D_(j, idx2) < mindist[j]) {
-          nn_distances.update_leq(j, D_(j, idx2));
-          n_nghbr[j] = idx2;
-        }
-      }
-      // Update the distance matrix in the range (idx2, N).
-      if (idx2<N_1) {
-        n_nghbr[idx2] = j = active_nodes.succ[idx2]; // exists, maximally N-1
-        f_weighted(&D_(idx2, j), D_(idx1, j) );
-        min = D_(idx2,j);
-        for (j=active_nodes.succ[j]; j<N; j=active_nodes.succ[j]) {
-          f_weighted(&D_(idx2, j), D_(idx1, j) );
-          if (D_(idx2,j) < min) {
-            min = D_(idx2,j);
-            n_nghbr[idx2] = j;
-          }
-        }
-        nn_distances.update(idx2, min);
-      }
-      break;
-
-    case METHOD_METR_WARD:
-      /*
-        Ward linkage.
-
-        Shorter and longer distances can occur, not smaller than min(d1,d2)
-        but maybe bigger than max(d1,d2).
-      */
-      // Update the distance matrix in the range [start, idx1).
-      for (j=active_nodes.start; j<idx1; j=active_nodes.succ[j]) {
-        f_ward(&D_(j, idx2), D_(j, idx1), mindist[idx1],
-               size1, size2, static_cast<t_float>(members[j]) );
-        if (n_nghbr[j] == idx1)
-          n_nghbr[j] = idx2;
-      }
-      // Update the distance matrix in the range (idx1, idx2).
-      for (; j<idx2; j=active_nodes.succ[j]) {
-        f_ward(&D_(j, idx2), D_(idx1, j), mindist[idx1], size1, size2,
-               static_cast<t_float>(members[j]) );
-        if (D_(j, idx2) < mindist[j]) {
-          nn_distances.update_leq(j, D_(j, idx2));
-          n_nghbr[j] = idx2;
-        }
-      }
-      // Update the distance matrix in the range (idx2, N).
-      if (idx2<N_1) {
-        n_nghbr[idx2] = j = active_nodes.succ[idx2]; // exists, maximally N-1
-        f_ward(&D_(idx2, j), D_(idx1, j), mindist[idx1],
-               size1, size2, static_cast<t_float>(members[j]) );
-        min = D_(idx2,j);
-        for (j=active_nodes.succ[j]; j<N; j=active_nodes.succ[j]) {
-          f_ward(&D_(idx2, j), D_(idx1, j), mindist[idx1],
-                 size1, size2, static_cast<t_float>(members[j]) );
-          if (D_(idx2,j) < min) {
-            min = D_(idx2,j);
-            n_nghbr[idx2] = j;
-          }
-        }
-        nn_distances.update(idx2, min);
-      }
-      break;
-
-    case METHOD_METR_CENTROID: {
-      /*
-        Centroid linkage.
-
-        Shorter and longer distances can occur, not bigger than max(d1,d2)
-        but maybe smaller than min(d1,d2).
-      */
-      // Update the distance matrix in the range [start, idx1).
-      t_float s = size1/(size1+size2);
-      t_float t = size2/(size1+size2);
-      t_float stc = s*t*mindist[idx1];
-      for (j=active_nodes.start; j<idx1; j=active_nodes.succ[j]) {
-        f_centroid(&D_(j, idx2), D_(j, idx1), stc, s, t);
-        if (D_(j, idx2) < mindist[j]) {
-          nn_distances.update_leq(j, D_(j, idx2));
-          n_nghbr[j] = idx2;
-        }
-        else if (n_nghbr[j] == idx1)
-          n_nghbr[j] = idx2;
-      }
-      // Update the distance matrix in the range (idx1, idx2).
-      for (; j<idx2; j=active_nodes.succ[j]) {
-        f_centroid(&D_(j, idx2), D_(idx1, j), stc, s, t);
-        if (D_(j, idx2) < mindist[j]) {
-          nn_distances.update_leq(j, D_(j, idx2));
-          n_nghbr[j] = idx2;
-        }
-      }
-      // Update the distance matrix in the range (idx2, N).
-      if (idx2<N_1) {
-        n_nghbr[idx2] = j = active_nodes.succ[idx2]; // exists, maximally N-1
-        f_centroid(&D_(idx2, j), D_(idx1, j), stc, s, t);
-        min = D_(idx2,j);
-        for (j=active_nodes.succ[j]; j<N; j=active_nodes.succ[j]) {
-          f_centroid(&D_(idx2, j), D_(idx1, j), stc, s, t);
-          if (D_(idx2,j) < min) {
-            min = D_(idx2,j);
-            n_nghbr[idx2] = j;
-          }
-        }
-        nn_distances.update(idx2, min);
-      }
-      break;
-    }
-
-    case METHOD_METR_MEDIAN: {
-      /*
-        Median linkage.
-
-        Shorter and longer distances can occur, not bigger than max(d1,d2)
-        but maybe smaller than min(d1,d2).
-      */
-      // Update the distance matrix in the range [start, idx1).
-      t_float c_4 = mindist[idx1]*.25;
-      for (j=active_nodes.start; j<idx1; j=active_nodes.succ[j]) {
-        f_median(&D_(j, idx2), D_(j, idx1), c_4 );
-        if (D_(j, idx2) < mindist[j]) {
-          nn_distances.update_leq(j, D_(j, idx2));
-          n_nghbr[j] = idx2;
-        }
-        else if (n_nghbr[j] == idx1)
-          n_nghbr[j] = idx2;
-      }
-      // Update the distance matrix in the range (idx1, idx2).
-      for (; j<idx2; j=active_nodes.succ[j]) {
-        f_median(&D_(j, idx2), D_(idx1, j), c_4 );
-        if (D_(j, idx2) < mindist[j]) {
-          nn_distances.update_leq(j, D_(j, idx2));
-          n_nghbr[j] = idx2;
-        }
-      }
-      // Update the distance matrix in the range (idx2, N).
-      if (idx2<N_1) {
-        n_nghbr[idx2] = j = active_nodes.succ[idx2]; // exists, maximally N-1
-        f_median(&D_(idx2, j), D_(idx1, j), c_4 );
-        min = D_(idx2,j);
-        for (j=active_nodes.succ[j]; j<N; j=active_nodes.succ[j]) {
-          f_median(&D_(idx2, j), D_(idx1, j), c_4 );
-          if (D_(idx2,j) < min) {
-            min = D_(idx2,j);
-            n_nghbr[idx2] = j;
-          }
-        }
-        nn_distances.update(idx2, min);
-      }
-      break;
-    }
-
-    default:
-      throw std::runtime_error(std::string("Invalid method."));
-    }
-  }
-  #ifdef FE_INVALID
-  if (fetestexcept(FE_INVALID)) throw fenv_error();
-  #endif
-}
-
-/*
-  Clustering methods for vector data
-*/
-
-template <typename t_dissimilarity>
-static void MST_linkage_core_vector(const t_index N,
-                                    t_dissimilarity & dist,
-                                    cluster_result & Z2) {
-/*
-    N: integer, number of data points
-    dist: function pointer to the metric
-    Z2: output data structure
-
-    The basis of this algorithm is an algorithm by Rohlf:
-
-    F. James Rohlf, Hierarchical clustering using the minimum spanning tree,
-    The Computer Journal, vol. 16, 1973, p. 93–95.
-*/
-  t_index i;
-  t_index idx2;
-  doubly_linked_list active_nodes(N);
-  auto_array_ptr<t_float> d(N);
-
-  t_index prev_node;
-  t_float min;
-
-  // first iteration
-  idx2 = 1;
-  min = std::numeric_limits<t_float>::infinity();
-  for (i=1; i<N; ++i) {
-    d[i] = dist(0,i);
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wfloat-equal"
-#endif
-    if (d[i] < min) {
-      min = d[i];
-      idx2 = i;
-    }
-    else if (fc_isnan(d[i]))
-      throw (nan_error());
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic pop
-#endif
-  }
-
-  Z2.append(0, idx2, min);
-
-  for (t_index j=1; j<N-1; ++j) {
-    prev_node = idx2;
-    active_nodes.remove(prev_node);
-
-    idx2 = active_nodes.succ[0];
-    min = d[idx2];
-
-    for (i=idx2; i<N; i=active_nodes.succ[i]) {
-      t_float tmp = dist(i, prev_node);
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wfloat-equal"
-#endif
-      if (d[i] > tmp)
-        d[i] = tmp;
-      else if (fc_isnan(tmp))
-        throw (nan_error());
-#if HAVE_DIAGNOSTIC
-#pragma GCC diagnostic pop
-#endif
-      if (d[i] < min) {
-        min = d[i];
-        idx2 = i;
-      }
-    }
-    Z2.append(prev_node, idx2, min);
-  }
-}
-
-template <method_codes_vector method, typename t_dissimilarity>
-static void generic_linkage_vector(const t_index N,
-                                   t_dissimilarity & dist,
-                                   cluster_result & Z2) {
-  /*
-    N: integer, number of data points
-    dist: function pointer to the metric
-    Z2: output data structure
-
-    This algorithm is valid for the distance update methods
-    "Ward", "centroid" and "median" only!
-  */
-  const t_index N_1 = N-1;
-  t_index i, j; // loop variables
-  t_index idx1, idx2; // row and column indices
-
-  auto_array_ptr<t_index> n_nghbr(N_1); // array of nearest neighbors
-  auto_array_ptr<t_float> mindist(N_1); // distances to the nearest neighbors
-  auto_array_ptr<t_index> row_repr(N); // row_repr[i]: node number that the
-                                       // i-th row represents
-  doubly_linked_list active_nodes(N);
-  binary_min_heap nn_distances(&*mindist, N_1); // minimum heap structure for
-                        // the distance to the nearest neighbor of each point
-  t_index node1, node2;     // node numbers in the output
-  t_float min; // minimum and row index for nearest-neighbor search
-
-  for (i=0; i<N; ++i)
-    // Build a list of row ↔ node label assignments.
-    // Initially i ↦ i
-    row_repr[i] = i;
-
-  // Initialize the minimal distances:
-  // Find the nearest neighbor of each point.
-  // n_nghbr[i] = argmin_{j>i} D(i,j) for i in range(N-1)
-  for (i=0; i<N_1; ++i) {
-    min = std::numeric_limits<t_float>::infinity();
-    t_index idx;
-    for (idx=j=i+1; j<N; ++j) {
-      t_float tmp;
-      switch (method) {
-      case METHOD_VECTOR_WARD:
-        tmp = dist.ward_initial(i,j);
-        break;
-      default:
-        tmp = dist.template sqeuclidean<true>(i,j);
-      }
-      if (tmp<min) {
-        min = tmp;
-        idx = j;
-      }
-    }
-    switch (method) {
-    case METHOD_VECTOR_WARD:
-      mindist[i] = t_dissimilarity::ward_initial_conversion(min);
-      break;
-    default:
-      mindist[i] = min;
-    }
-    n_nghbr[i] = idx;
-  }
-
-  // Put the minimal distances into a heap structure to make the repeated
-  // global minimum searches fast.
-  nn_distances.heapify();
-
-  // Main loop: We have N-1 merging steps.
-  for (i=0; i<N_1; ++i) {
-    idx1 = nn_distances.argmin();
-
-    while ( active_nodes.is_inactive(n_nghbr[idx1]) ) {
-      // Recompute the minimum mindist[idx1] and n_nghbr[idx1].
-      n_nghbr[idx1] = j = active_nodes.succ[idx1]; // exists, maximally N-1
-      switch (method) {
-      case METHOD_VECTOR_WARD:
-        min = dist.ward(idx1,j);
-        for (j=active_nodes.succ[j]; j<N; j=active_nodes.succ[j]) {
-          t_float const tmp = dist.ward(idx1,j);
-          if (tmp<min) {
-            min = tmp;
-            n_nghbr[idx1] = j;
-          }
-        }
-        break;
-      default:
-        min = dist.template sqeuclidean<true>(idx1,j);
-        for (j=active_nodes.succ[j]; j<N; j=active_nodes.succ[j]) {
-          t_float const tmp = dist.template sqeuclidean<true>(idx1,j);
-          if (tmp<min) {
-            min = tmp;
-            n_nghbr[idx1] = j;
-          }
-        }
-      }
-      /* Update the heap with the new true minimum and search for the (possibly
-         different) minimal entry. */
-      nn_distances.update_geq(idx1, min);
-      idx1 = nn_distances.argmin();
-    }
-
-    nn_distances.heap_pop(); // Remove the current minimum from the heap.
-    idx2 = n_nghbr[idx1];
-
-    // Write the newly found minimal pair of nodes to the output array.
-    node1 = row_repr[idx1];
-    node2 = row_repr[idx2];
-
-    Z2.append(node1, node2, mindist[idx1]);
-
-    switch (method) {
-    case METHOD_VECTOR_WARD:
-    case METHOD_VECTOR_CENTROID:
-      dist.merge_inplace(idx1, idx2);
-      break;
-    case METHOD_VECTOR_MEDIAN:
-      dist.merge_inplace_weighted(idx1, idx2);
-      break;
-    default:
-      throw std::runtime_error(std::string("Invalid method."));
-    }
-
-    // Index idx2 now represents the new (merged) node with label N+i.
-    row_repr[idx2] = N+i;
-    // Remove idx1 from the list of active indices (active_nodes).
-    active_nodes.remove(idx1);  // TBD later!!!
-
-    // Update the distance matrix
-    switch (method) {
-    case METHOD_VECTOR_WARD:
-      /*
-        Ward linkage.
-
-        Shorter and longer distances can occur, not smaller than min(d1,d2)
-        but maybe bigger than max(d1,d2).
-      */
-      // Update the distance matrix in the range [start, idx1).
-      for (j=active_nodes.start; j<idx1; j=active_nodes.succ[j]) {
-        if (n_nghbr[j] == idx2) {
-          n_nghbr[j] = idx1; // invalidate
-        }
-      }
-      // Update the distance matrix in the range (idx1, idx2).
-      for ( ; j<idx2; j=active_nodes.succ[j]) {
-        t_float const tmp = dist.ward(j, idx2);
-        if (tmp < mindist[j]) {
-          nn_distances.update_leq(j, tmp);
-          n_nghbr[j] = idx2;
-        }
-        else if (n_nghbr[j]==idx2) {
-          n_nghbr[j] = idx1; // invalidate
-        }
-      }
-      // Find the nearest neighbor for idx2.
-      if (idx2<N_1) {
-        n_nghbr[idx2] = j = active_nodes.succ[idx2]; // exists, maximally N-1
-        min = dist.ward(idx2,j);
-        for (j=active_nodes.succ[j]; j<N; j=active_nodes.succ[j]) {
-          t_float const tmp = dist.ward(idx2,j);
-          if (tmp < min) {
-            min = tmp;
-            n_nghbr[idx2] = j;
-          }
-        }
-        nn_distances.update(idx2, min);
-      }
-      break;
-
-    default:
-      /*
-        Centroid and median linkage.
-
-        Shorter and longer distances can occur, not bigger than max(d1,d2)
-        but maybe smaller than min(d1,d2).
-      */
-      for (j=active_nodes.start; j<idx2; j=active_nodes.succ[j]) {
-        t_float const tmp = dist.template sqeuclidean<true>(j, idx2);
-        if (tmp < mindist[j]) {
-          nn_distances.update_leq(j, tmp);
-          n_nghbr[j] = idx2;
-        }
-        else if (n_nghbr[j] == idx2)
-          n_nghbr[j] = idx1; // invalidate
-      }
-      // Find the nearest neighbor for idx2.
-      if (idx2<N_1) {
-        n_nghbr[idx2] = j = active_nodes.succ[idx2]; // exists, maximally N-1
-        min = dist.template sqeuclidean<true>(idx2,j);
-        for (j=active_nodes.succ[j]; j<N; j=active_nodes.succ[j]) {
-          t_float const tmp = dist.template sqeuclidean<true>(idx2, j);
-          if (tmp < min) {
-            min = tmp;
-            n_nghbr[idx2] = j;
-          }
-        }
-        nn_distances.update(idx2, min);
-      }
-    }
-  }
-}
-
-template <method_codes_vector method, typename t_dissimilarity>
-static void generic_linkage_vector_alternative(const t_index N,
-                                               t_dissimilarity & dist,
-                                               cluster_result & Z2) {
-  /*
-    N: integer, number of data points
-    dist: function pointer to the metric
-    Z2: output data structure
-
-    This algorithm is valid for the distance update methods
-    "Ward", "centroid" and "median" only!
-  */
-  const t_index N_1 = N-1;
-  t_index i, j=0; // loop variables
-  t_index idx1, idx2; // row and column indices
-
-  auto_array_ptr<t_index> n_nghbr(2*N-2); // array of nearest neighbors
-  auto_array_ptr<t_float> mindist(2*N-2); // distances to the nearest neighbors
-
-  doubly_linked_list active_nodes(N+N_1);
-  binary_min_heap nn_distances(&*mindist, N_1, 2*N-2, 1); // minimum heap
-      // structure for the distance to the nearest neighbor of each point
-
-  t_float min; // minimum for nearest-neighbor searches
-
-  // Initialize the minimal distances:
-  // Find the nearest neighbor of each point.
-  // n_nghbr[i] = argmin_{j>i} D(i,j) for i in range(N-1)
-  for (i=1; i<N; ++i) {
-    min = std::numeric_limits<t_float>::infinity();
-    t_index idx;
-    for (idx=j=0; j<i; ++j) {
-      t_float tmp;
-      switch (method) {
-      case METHOD_VECTOR_WARD:
-        tmp = dist.ward_initial(i,j);
-        break;
-      default:
-        tmp = dist.template sqeuclidean<true>(i,j);
-      }
-      if (tmp<min) {
-        min = tmp;
-        idx = j;
-      }
-    }
-    switch (method) {
-    case METHOD_VECTOR_WARD:
-      mindist[i] = t_dissimilarity::ward_initial_conversion(min);
-      break;
-    default:
-      mindist[i] = min;
-    }
-    n_nghbr[i] = idx;
-  }
-
-  // Put the minimal distances into a heap structure to make the repeated
-  // global minimum searches fast.
-  nn_distances.heapify();
-
-  // Main loop: We have N-1 merging steps.
-  for (i=N; i<N+N_1; ++i) {
-    /*
-      The bookkeeping is different from the "stored matrix approach" algorithm
-      generic_linkage.
-
-      mindist[i] stores a lower bound on the minimum distance of the point i to
-      all points of *lower* index:
-
-          mindist[i] ≥ min_{j<i} D(i,j)
-
-      Moreover, new nodes do not re-use one of the old indices, but they are
-      given a new, unique index (SciPy convention: initial nodes are 0,…,N−1,
-      new nodes are N,…,2N−2).
-
-      Invalid nearest neighbors are not recognized by the fact that the stored
-      distance is smaller than the actual distance, but the list active_nodes
-      maintains a flag whether a node is inactive. If n_nghbr[i] points to an
-      active node, the entries nn_distances[i] and n_nghbr[i] are valid,
-      otherwise they must be recomputed.
-    */
-    idx1 = nn_distances.argmin();
-    while ( active_nodes.is_inactive(n_nghbr[idx1]) ) {
-      // Recompute the minimum mindist[idx1] and n_nghbr[idx1].
-      n_nghbr[idx1] = j = active_nodes.start;
-      switch (method) {
-      case METHOD_VECTOR_WARD:
-        min = dist.ward_extended(idx1,j);
-        for (j=active_nodes.succ[j]; j<idx1; j=active_nodes.succ[j]) {
-          t_float tmp = dist.ward_extended(idx1,j);
-          if (tmp<min) {
-            min = tmp;
-            n_nghbr[idx1] = j;
-          }
-        }
-        break;
-      default:
-        min = dist.sqeuclidean_extended(idx1,j);
-        for (j=active_nodes.succ[j]; j<idx1; j=active_nodes.succ[j]) {
-          t_float const tmp = dist.sqeuclidean_extended(idx1,j);
-          if (tmp<min) {
-            min = tmp;
-            n_nghbr[idx1] = j;
-          }
-        }
-      }
-      /* Update the heap with the new true minimum and search for the (possibly
-         different) minimal entry. */
-      nn_distances.update_geq(idx1, min);
-      idx1 = nn_distances.argmin();
-    }
-
-    idx2 = n_nghbr[idx1];
-    active_nodes.remove(idx1);
-    active_nodes.remove(idx2);
-
-    Z2.append(idx1, idx2, mindist[idx1]);
-
-    if (i<2*N_1) {
-      switch (method) {
-      case METHOD_VECTOR_WARD:
-      case METHOD_VECTOR_CENTROID:
-        dist.merge(idx1, idx2, i);
-        break;
-
-      case METHOD_VECTOR_MEDIAN:
-        dist.merge_weighted(idx1, idx2, i);
-        break;
-
-      default:
-        throw std::runtime_error(std::string("Invalid method."));
-      }
-
-      n_nghbr[i] = active_nodes.start;
-      if (method==METHOD_VECTOR_WARD) {
-        /*
-          Ward linkage.
-
-          Shorter and longer distances can occur, not smaller than min(d1,d2)
-          but maybe bigger than max(d1,d2).
-        */
-        min = dist.ward_extended(active_nodes.start, i);
-        for (j=active_nodes.succ[active_nodes.start]; j<i;
-             j=active_nodes.succ[j]) {
-          t_float tmp = dist.ward_extended(j, i);
-          if (tmp < min) {
-            min = tmp;
-            n_nghbr[i] = j;
-          }
-        }
-      }
-      else {
-        /*
-          Centroid and median linkage.
-
-          Shorter and longer distances can occur, not bigger than max(d1,d2)
-          but maybe smaller than min(d1,d2).
-        */
-        min = dist.sqeuclidean_extended(active_nodes.start, i);
-        for (j=active_nodes.succ[active_nodes.start]; j<i;
-             j=active_nodes.succ[j]) {
-          t_float tmp = dist.sqeuclidean_extended(j, i);
-          if (tmp < min) {
-            min = tmp;
-            n_nghbr[i] = j;
-          }
-        }
-      }
-      if (idx2<active_nodes.start)  {
-        nn_distances.remove(active_nodes.start);
-      } else {
-        nn_distances.remove(idx2);
-      }
-      nn_distances.replace(idx1, i, min);
-    }
-  }
-}
-
-#if HAVE_VISIBILITY
-#pragma GCC visibility pop
-#endif
diff --git a/third_party/cluster/fastcluster_py.py b/third_party/cluster/fastcluster_py.py
deleted file mode 100644
index 1c9ecee369..0000000000
--- a/third_party/cluster/fastcluster_py.py
+++ /dev/null
@@ -1,28 +0,0 @@
-import os
-import numpy as np
-
-from cffi import FFI
-from common.ffi_wrapper import suffix
-
-cluster_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)))
-cluster_fn = os.path.join(cluster_dir, "libfastcluster"+suffix())
-
-ffi = FFI()
-ffi.cdef("""
-int hclust_fast(int n, double* distmat, int method, int* merge, double* height);
-void cutree_cdist(int n, const int* merge, double* height, double cdist, int* labels);
-void hclust_pdist(int n, int m, double* pts, double* out);
-void cluster_points_centroid(int n, int m, double* pts, double dist, int* idx);
-""")
-
-hclust = ffi.dlopen(cluster_fn)
-
-
-def cluster_points_centroid(pts, dist):
-  pts = np.ascontiguousarray(pts, dtype=np.float64)
-  pts_ptr = ffi.cast("double *", pts.ctypes.data)
-  n, m = pts.shape
-
-  labels_ptr = ffi.new("int[]", n)
-  hclust.cluster_points_centroid(n, m, pts_ptr, dist**2, labels_ptr)
-  return list(labels_ptr)
diff --git a/third_party/cluster/test.cpp b/third_party/cluster/test.cpp
deleted file mode 100644
index d4e4fa5361..0000000000
--- a/third_party/cluster/test.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-#include <cassert>
-
-extern "C" {
-#include "fastcluster.h"
-}
-
-
-int main(int argc, const char* argv[]) {
-  const int n = 11;
-  const int m = 3;
-  double* pts = new double[n*m]{59.26000137, -9.35999966, -5.42500019,
-                                91.61999817, -0.31999999, -2.75,
-                                31.38000031, 0.40000001, -0.2,
-                                89.57999725, -8.07999992, -18.04999924,
-                                53.42000122, 0.63999999, -0.175,
-                                31.38000031, 0.47999999, -0.2,
-                                36.33999939, 0.16, -0.2,
-                                53.33999939, 0.95999998, -0.175,
-                                59.26000137, -9.76000023, -5.44999981,
-                                33.93999977, 0.40000001, -0.22499999,
-                                106.74000092, -5.76000023, -18.04999924};
-
-  int * idx = new int[n];
-  int * correct_idx = new int[n]{0, 1, 2, 3, 4, 2, 5, 4, 0, 5, 6};
-
-  cluster_points_centroid(n, m, pts, 2.5 * 2.5, idx);
-
-  for (int i = 0; i < n; i++) {
-    assert(idx[i] == correct_idx[i]);
-  }
-
-  delete[] idx;
-  delete[] correct_idx;
-  delete[] pts;
-}
diff --git a/third_party/libyuv/lib/libyuv.a b/third_party/libyuv/lib/libyuv.a
deleted file mode 100644
index 228de4d03e..0000000000
Binary files a/third_party/libyuv/lib/libyuv.a and /dev/null differ
diff --git a/third_party/libyuv/x64/include b/third_party/libyuv/x86_64/include
similarity index 100%
rename from third_party/libyuv/x64/include
rename to third_party/libyuv/x86_64/include
diff --git a/third_party/libyuv/x64/lib/libyuv.a b/third_party/libyuv/x86_64/lib/libyuv.a
similarity index 100%
rename from third_party/libyuv/x64/lib/libyuv.a
rename to third_party/libyuv/x86_64/lib/libyuv.a
diff --git a/third_party/snpe/include/DlSystem/IOBufferDataTypeMap.hpp b/third_party/snpe/include/DlSystem/IOBufferDataTypeMap.hpp
index e36b1fbd7e..ac33c84af8 100644
--- a/third_party/snpe/include/DlSystem/IOBufferDataTypeMap.hpp
+++ b/third_party/snpe/include/DlSystem/IOBufferDataTypeMap.hpp
@@ -1,127 +1,127 @@
-//=============================================================================
-//
-//  Copyright (c) 2021-2022 Qualcomm Technologies, Inc.
-//  All Rights Reserved.
-//  Confidential and Proprietary - Qualcomm Technologies, Inc.
-//
-//=============================================================================
-
-
-#ifndef DL_SYSTEM_IOBUFFER_DATATYPE_MAP_HPP
-#define DL_SYSTEM_IOBUFFER_DATATYPE_MAP_HPP
-
-#include <cstddef>
-#include <memory>
-#include "DlSystem/DlEnums.hpp"
-
-namespace DlSystem
-{
-   // Forward declaration of IOBufferDataTypeMapImpl implementation.
-   class IOBufferDataTypeMapImpl;
-}
-
-namespace zdl
-{
-namespace DlSystem
-{
-/** @addtogroup c_plus_plus_apis C++
-@{ */
-
-/**
- * @brief .
- *
- * The IoBufferDataTypeMap class definition
- */
-class ZDL_EXPORT IOBufferDataTypeMap final
-{
-public:
-
-    /**
-    * @brief .
-    *
-    * Creates a new Buffer Data type map
-    *
-    */
-   IOBufferDataTypeMap();
-
-   /**
-    * @brief Adds a name and the corresponding buffer data type
-    *        to the map
-    *
-    * @param[name] name The name of the buffer
-    * @param[bufferDataType] buffer Data Type of the buffer
-    *
-    * @note If a buffer with the same name already exists, no new
-    *       buffer is added.
-    */
-   void add(const char* name, zdl::DlSystem::IOBufferDataType_t bufferDataType);
-
-   /**
-    * @brief Removes a buffer name from the map
-    *
-    * @param[name] name The name of the buffer
-    *
-    */
-   void remove(const char* name);
-
-   /**
-    * @brief Returns the type of the named buffer
-    *
-    * @param[name] name The name of the buffer
-    *
-    * @return The type of the buffer, or UNSPECIFIED if the buffer does not exist
-    *
-    */
-   zdl::DlSystem::IOBufferDataType_t getBufferDataType(const char* name);
-
-   /**
-    * @brief Returns the type of the first buffer
-    *
-    * @return The type of the first buffer, or UNSPECIFIED if the map is empty.
-    *
-    */
-   zdl::DlSystem::IOBufferDataType_t getBufferDataType();
-
-   /**
-    * @brief Returns the size of the buffer type map.
-    *
-    * @return The size of the map
-    *
-    */
-   size_t size();
-
-   /**
-    * @brief Checks the existence of the named buffer in the map
-    *
-    * @return True if the named buffer exists, false otherwise.
-    *
-    */
-   bool find(const char* name);
-
-   /**
-    * @brief Resets the map
-    *
-    */
-   void clear();
-
-   /**
-    * @brief Checks whether the map is empty
-    *
-    * @return True if the map is empty, false otherwise.
-    *
-    */
-   bool empty();
-
-   /**
-    * @brief Destroys the map
-    *
-    */
-   ~IOBufferDataTypeMap();
-
-private:
-   std::shared_ptr<::DlSystem::IOBufferDataTypeMapImpl> m_IOBufferDataTypeMapImpl;
-};
-}
-
-}
-#endif
+//=============================================================================
+//
+//  Copyright (c) 2021-2022 Qualcomm Technologies, Inc.
+//  All Rights Reserved.
+//  Confidential and Proprietary - Qualcomm Technologies, Inc.
+//
+//=============================================================================
+
+
+#ifndef DL_SYSTEM_IOBUFFER_DATATYPE_MAP_HPP
+#define DL_SYSTEM_IOBUFFER_DATATYPE_MAP_HPP
+
+#include <cstddef>
+#include <memory>
+#include "DlSystem/DlEnums.hpp"
+
+namespace DlSystem
+{
+   // Forward declaration of IOBufferDataTypeMapImpl implementation.
+   class IOBufferDataTypeMapImpl;
+}
+
+namespace zdl
+{
+namespace DlSystem
+{
+/** @addtogroup c_plus_plus_apis C++
+@{ */
+
+/**
+ * @brief .
+ *
+ * The IoBufferDataTypeMap class definition
+ */
+class ZDL_EXPORT IOBufferDataTypeMap final
+{
+public:
+
+    /**
+    * @brief .
+    *
+    * Creates a new Buffer Data type map
+    *
+    */
+   IOBufferDataTypeMap();
+
+   /**
+    * @brief Adds a name and the corresponding buffer data type
+    *        to the map
+    *
+    * @param[name] name The name of the buffer
+    * @param[bufferDataType] buffer Data Type of the buffer
+    *
+    * @note If a buffer with the same name already exists, no new
+    *       buffer is added.
+    */
+   void add(const char* name, zdl::DlSystem::IOBufferDataType_t bufferDataType);
+
+   /**
+    * @brief Removes a buffer name from the map
+    *
+    * @param[name] name The name of the buffer
+    *
+    */
+   void remove(const char* name);
+
+   /**
+    * @brief Returns the type of the named buffer
+    *
+    * @param[name] name The name of the buffer
+    *
+    * @return The type of the buffer, or UNSPECIFIED if the buffer does not exist
+    *
+    */
+   zdl::DlSystem::IOBufferDataType_t getBufferDataType(const char* name);
+
+   /**
+    * @brief Returns the type of the first buffer
+    *
+    * @return The type of the first buffer, or UNSPECIFIED if the map is empty.
+    *
+    */
+   zdl::DlSystem::IOBufferDataType_t getBufferDataType();
+
+   /**
+    * @brief Returns the size of the buffer type map.
+    *
+    * @return The size of the map
+    *
+    */
+   size_t size();
+
+   /**
+    * @brief Checks the existence of the named buffer in the map
+    *
+    * @return True if the named buffer exists, false otherwise.
+    *
+    */
+   bool find(const char* name);
+
+   /**
+    * @brief Resets the map
+    *
+    */
+   void clear();
+
+   /**
+    * @brief Checks whether the map is empty
+    *
+    * @return True if the map is empty, false otherwise.
+    *
+    */
+   bool empty();
+
+   /**
+    * @brief Destroys the map
+    *
+    */
+   ~IOBufferDataTypeMap();
+
+private:
+   std::shared_ptr<::DlSystem::IOBufferDataTypeMapImpl> m_IOBufferDataTypeMapImpl;
+};
+}
+
+}
+#endif
diff --git a/tinygrad_repo/extra/onnx.py b/tinygrad_repo/extra/onnx.py
index c64f1bb7da..54bd3905e5 100644
--- a/tinygrad_repo/extra/onnx.py
+++ b/tinygrad_repo/extra/onnx.py
@@ -1,9 +1,11 @@
-import functools
+from __future__ import annotations
+from google.protobuf.internal.containers import RepeatedCompositeFieldContainer
 import importlib
 import numpy as np
 from tinygrad.tensor import Tensor
-from tinygrad.helpers import prod
-from tinygrad.helpers import getenv, DEBUG
+from tinygrad.helpers import prod, getenv, DEBUG, dtypes
+from typing import List,Dict
+from onnx.onnx_pb import AttributeProto, ModelProto, TensorProto, TypeProto
 try:
   from onnx.helper import tensor_dtype_to_np_dtype
 except ImportError:
@@ -13,46 +15,70 @@ except ImportError:
 
 # global numpy cache for parameters
 numpy_cache = {}
-def safe_numpy(t):
+def safe_numpy(t) -> np.ndarray:
   if not isinstance(t, Tensor): return t
   global numpy_cache
   if t not in numpy_cache:
-    if DEBUG >= 1:
-      print("numpy cache miss", t)
-    numpy_cache[t] = t.numpy()
+    if DEBUG >= 3: print("numpy cache miss", t)
+    tmp = t.numpy()
+    numpy_cache[t] = tmp if len(tmp.shape) else tmp.reshape(1)
+  assert len(numpy_cache[t].shape) > 0
   return numpy_cache[t]
 
 onnx_ops = importlib.import_module('extra.onnx_ops')
 
 ONNXLIMIT = getenv("ONNXLIMIT", -1)
 
-def get_run_onnx(onnx_model):
-  def shape_to_tuple(s): return tuple(x.dim_value for x in s.dim)
-  def buffer_parse(inp):
+def get_run_onnx(onnx_model: ModelProto):
+  def type_parse(type_proto: TypeProto):
+    ret = []
+    while True:
+      attr = type_proto.WhichOneof('value')
+      if attr == 'tensor_type':
+        if "dim_value" not in getattr(type_proto, attr).shape.dim.__dir__(): return () # variable type, unable to determine shape
+        elif not ret:
+          return tuple([x.dim_value for x in getattr(type_proto, attr).shape.dim])
+        else:
+          ret.extend([(x.dim_value,) for x in getattr(type_proto, attr).shape.dim])
+          return tuple(ret)
+      elif attr == 'sequence_type':
+        type_proto = getattr(type_proto, attr).elem_type
+        ret.append(1)
+      elif attr == 'map_type': raise NotImplementedError(f"map_type is not implemented: {type_proto}")
+      elif attr == 'opaque_type': raise NotImplementedError(f"opaque_type is not implemented: {type_proto}")
+      elif attr == 'sparse_tensor_type': raise NotImplementedError(f"sparse_tensor_type is not implemented: {type_proto}")
+      elif attr == 'optional_type': type_proto = getattr(type_proto, attr).elem_type
+      else: raise Exception(f"unknown attr: {attr}, {type_proto}")
+
+  def buffer_parse(inp: TensorProto) -> Tensor:
     if inp.data_type in (1,10,6,7):
       # TODO: this is shared with below
       if len(inp.float_data) > 0:
         ret = Tensor(np.array(inp.float_data, dtype=np.float32).reshape(inp.dims), requires_grad=False)
       elif len(inp.int64_data) > 0:
-        ret = Tensor(np.array(inp.int64_data, dtype=np.float32).reshape(inp.dims), requires_grad=False)
+        ret = Tensor(np.array(inp.int64_data, dtype=np.int64).reshape(inp.dims), requires_grad=False)
       elif len(inp.int32_data) > 0:
-        ret = Tensor(np.array(inp.int32_data, dtype=np.float32).reshape(inp.dims), requires_grad=False)
+        ret = Tensor(np.array(inp.int32_data, dtype=np.int32).reshape(inp.dims), requires_grad=False)
       else:
         ret = Tensor(np.frombuffer(inp.raw_data, dtype=tensor_dtype_to_np_dtype(inp.data_type)).reshape(inp.dims).astype(np.float32).copy(), requires_grad=False)
     else:
       raise Exception(f"bad data type {inp.name} {inp.dims} {inp.data_type}")
     return ret
 
-  def attribute_parse(a):
-    if a.type in [6,7]: return tuple([int(x) for x in a.ints])
-    elif a.type == 4: return buffer_parse(a.t)  # TENSOR
-    elif a.type == 3: return str(a.s)
-    elif a.type == 2: return int(a.i)
-    elif a.type == 1: return float(a.f)
+  def attribute_parse(a: AttributeProto) -> float | int | str | Tensor | tuple[float] | tuple[int]:
+    # TODO: this is not complete, see onnx/onnx_ml_pb2.pyi for a complete list
+    if a.type == AttributeProto.FLOAT: return float(a.f)
+    elif a.type == AttributeProto.INT: return int(a.i)
+    elif a.type == AttributeProto.STRING: return a.s.decode("utf-8")
+    elif a.type == AttributeProto.TENSOR: return buffer_parse(a.t) # TENSOR
+    elif a.type == AttributeProto.FLOATS: return tuple(float(x) for x in a.floats)
+    elif a.type == AttributeProto.INTS: return tuple(int(x) for x in a.ints)
+    elif a.type == AttributeProto.STRINGS: return tuple(x.decode("utf-8") for x in a.strings)
+    elif a.type == AttributeProto.GRAPH: raise Exception(f"graph not implemented: {a.g}")
     else: raise Exception(f"can't parse {a.type} {a}")
-  def attribute_to_dict(a): return {x.name:attribute_parse(x) for x in a}
+  def attribute_to_dict(a: RepeatedCompositeFieldContainer[AttributeProto]): return {x.name:attribute_parse(x) for x in a}
 
-  tensors = {}
+  tensors: Dict[str, Tensor] = {}
 
   # get weights and biases
   for inp in onnx_model.graph.initializer:
@@ -61,114 +87,106 @@ def get_run_onnx(onnx_model):
     elif len(inp.float_data) > 0:
       tensors[inp.name] = Tensor(np.array(inp.float_data, dtype=np.float32).reshape(inp.dims), requires_grad=False)
     elif len(inp.int64_data) > 0:
-      tensors[inp.name] = Tensor(np.array(inp.int64_data, dtype=np.float32).reshape(inp.dims), requires_grad=False)
+      tensors[inp.name] = Tensor(np.array(inp.int64_data, dtype=np.int64).reshape(inp.dims), requires_grad=False)
+    elif len(inp.raw_data) == 0:
+      tensors[inp.name] = Tensor(np.array([], dtype=np.float32), requires_grad=False)
     else:
       print(inp.name, inp.dims, inp.data_type, len(inp.raw_data))
       print(inp)
       raise Exception("no data")
-    if DEBUG >= 1:
-      print("realize", inp.name)
-    tensors[inp.name].realize()
 
   # preparse the attributes
   attribute_dict = {}
+  domain = ""
   for num,n in enumerate(onnx_model.graph.node):
     attribute_dict[num] = attribute_to_dict(n.attribute)
-  
-  onnx_version = onnx_model.opset_import[0].version
+    if n.domain: domain = n.domain
 
-  def run_onnx(inputs={}, debug=False):
-    if getenv("DEBUGONNX"): debug = True
-    input_tensors = {}
-    intermediate_tensors = {}
+  onnx_model_version = onnx_model.opset_import[0].version
+
+  def run_onnx(inputs={}, debug=0):
+    debug = getenv("DEBUGONNX") or debug
+    input_tensors: Dict[str,Tensor] = {}
+    intermediate_tensors: Dict[str,Tensor] = {}
     output_tensor_names = [x.name for x in onnx_model.graph.output]
 
     # get inputs
     for inp in onnx_model.graph.input:
       if inp.name in tensors: continue
-      shape = shape_to_tuple(inp.type.tensor_type.shape)
-      if len(shape) >= 1 and shape[0] == 0: shape = tuple([1]+list(shape[1:]))   # 1 batch size
+      shape = type_parse(inp.type)
       if inp.name in inputs:
-        input_shape = inputs[inp.name].shape
-        if input_shape == (0,): raise NotImplementedError("empty tensors aren't supported in tinygrad")
-        assert input_shape == shape, f"wrong shape for input {inp.name}, {input_shape} isn't {shape}"
         if isinstance(inputs[inp.name], Tensor):
           input_tensors[inp.name] = inputs[inp.name]
+        elif isinstance(inputs[inp.name], list):
+          input_tensors[inp.name] = [Tensor(i, requires_grad=False) for i in inputs[inp.name]]
+        elif domain == "ai.onnx.preview.training": # not sure if in real use the domain is "ai.onnx.preview.training"
+          input_tensors[inp.name] = Tensor(inputs[inp.name], requires_grad=True) # TODO there isn't a good way to parse which inp requires_grad, some are manually turned off in optimizer ops
         else:
           input_tensors[inp.name] = Tensor(inputs[inp.name], requires_grad=False)
-        for _,v in input_tensors.items(): v.realize()
+        if shape: # if only input_tensor is not variable type
+          input_shape = input_tensors[inp.name].shape if isinstance(input_tensors[inp.name], Tensor) else (1, *[i.shape for i in input_tensors[inp.name]])
+          assert input_shape == shape, f"wrong shape for input {inp.name}, {input_shape} isn't {shape}"
       else:
         raise Exception(f"no data for {inp.name} with shape {shape}")
 
-    for num,n in enumerate(onnx_model.graph.node):
-      inp = [tensors[x] if x in tensors else (intermediate_tensors[x] if x in intermediate_tensors else (input_tensors[x] if x != str() else None)) for x in n.input]
-      opt = attribute_dict[num]
-      if debug: print(f"{num}: op {n.op_type} shape {[x.shape if isinstance(x, Tensor) else x for x in inp]} opt {opt}")
+    def fetch_tensor(x: str):
+      if x in tensors: return tensors[x]
+      if x in intermediate_tensors: return intermediate_tensors[x]
+      if x != str(): return input_tensors[x]
+      return None
 
-      # free ones
-      if n.op_type == "Relu": ret = inp[0].relu()
-      elif n.op_type == "Sigmoid": ret = inp[0].sigmoid()
-      elif n.op_type == "Tanh": ret = inp[0].tanh()
-      elif n.op_type == "MatMul": ret = inp[0].matmul(inp[1])
-      # one liners
-      elif n.op_type == "Elu": ret = inp[0].elu(alpha=opt.get('alpha', 1.0))
-      elif n.op_type == "Concat": ret = inp[0].cat(*inp[1:], dim=opt['axis'])
-      elif n.op_type == "Transpose": ret = inp[0].permute(order=opt.get('perm', list(range(len(inp[0].shape))[::-1])))
-      elif n.op_type == "Squeeze": ret = inp[0].reshape([s for i,s in enumerate(inp[0].shape) if i not in opt['axes']])
-      elif n.op_type == "Div":
-        # in openpilot, due to SHUFFLE_PAD_OPS issues, we are spending an extra kernel
-        ret = inp[0].div(inp[1])
-      elif n.op_type == "Constant": ret = opt['value'] if 'value' in opt else opt['value_float']
-      elif n.op_type == "Reshape": ret = inp[0].reshape([int(x) if x != 0 else inp[0].shape[i] for i,x in enumerate(safe_numpy(inp[1]))])
-      elif n.op_type == "Resize":
-        # TODO: this is handcoded for YOLOv8
-        scales = safe_numpy(inp[2])
-        assert all([int(x) == x and x >= 1 for x in scales])
-        ret = inp[0].reshape([val for pair in zip(inp[0].shape, [1] * len(scales)) for val in pair])
-        ret = ret.expand([val for pair in zip(inp[0].shape, [int(x) for x in scales]) for val in pair])
-        ret = ret.reshape([x*y for x,y in zip(inp[0].shape, [int(x) for x in scales])])
-      elif n.op_type == "Gather":
-        # TODO: is this correct? seems to work for simple gather ops
-        axis = opt['axis']
-        shape = list(inp[0].shape)
-        indices = [shape[axis]+int(x) if x<0 else int(x) for x in safe_numpy(inp[1])]
-        args = [[(0,x) if j != axis else (i,i+1) for j, x in enumerate(shape)] for i in indices]
-        ret = inp[0].slice(arg=args[0]).cat(*[inp[0].slice(arg=arg) for arg in args[1:]], dim=axis)
-        ret = ret.reshape([s for i,s in enumerate(shape) if i != axis]) if len(indices) == 1 else ret # squeeze if needed
-      elif n.op_type in ["Add", "Sub", "Mul", "Pow"]:
-        # TODO: add this to tinygrad? i don't think it's in torch
-        if len(inp[0].shape) != len(inp[1].shape) and prod(inp[0].shape) == prod(inp[1].shape):
-          inp[1] = inp[1].reshape(inp[0].shape)
-        # TODO: is this right?
-        if 'broadcast' in opt: inp[1] = inp[1].reshape([-1 if i == opt['broadcast'] else 1 for i in range(len(inp[0].shape))])
-        if n.op_type == "Add": ret = inp[0] + inp[1]
-        if n.op_type == "Sub": ret = inp[0] - inp[1]
-        if n.op_type == "Mul": ret = inp[0] * inp[1]
-        if n.op_type == "Pow": ret = inp[0] ** inp[1]
-      elif n.op_type == "Split":
-        if 'split' not in opt: opt['split'] = [int(x) for x in safe_numpy(inp[1])]  # split can be a tensor
-        i = 0
+    for num,n in enumerate(onnx_model.graph.node):
+      inp: List[Tensor] = []
+      if debug >= 3: print("inputs:")
+      for x in n.input:
+        t = fetch_tensor(x)
+        if debug >= 3: print(f"\t{x} - {t}")
+        inp.append(t)
+      opt: Dict = attribute_dict[num]
+      if debug >= 1: print(f"{num}: op {n.op_type} shape {[x.shape if isinstance(x, Tensor) else x for x in inp]} opt {opt}")
+      # some ops live here because they require some local variables
+      if n.op_type == "Split": # have to use n.output for cases when num_outputs is absent
+        axis = opt.get("axis", 0)
+        split = None if len(inp) == 1 else [int(x) for x in safe_numpy(inp[1])]
+        if split is None:
+          split = [inp[0].shape[axis] // len(n.output)] * len(n.output)
+          for i in range(inp[0].shape[axis] % len(n.output)):
+            split[i] += 1
+        i, ret = 0, []
         arg = [(0,x) for x in inp[0].shape]
-        for o,s in zip(n.output, opt['split']):
-          arg[opt['axis']] = (i,i+s)
-          intermediate_tensors[o] = inp[0].slice(arg=arg)
+        for s in split:
+          arg[axis] = (i,i+s)
+          ret.append(inp[0].shrink(arg=tuple(arg)))
           i = i+s
-        continue
-      elif n.op_type == "Slice":
-        assert onnx_version == 10
-        arg = [(0,x) for x in inp[0].shape]
-        starts, ends, axes = inp[1:4]
-        assert axes.shape == (1,)
-        axis, starts, ends  = int(safe_numpy(axes)[0]), int(safe_numpy(starts)[0]), int(safe_numpy(ends)[0])
-        ends = min(ends, inp[0].shape[axis])
-        starts = starts + inp[0].shape[axis] if starts < 0 else starts
-        arg[axis] = (starts, ends)
-        ret = inp[0].slice(arg=arg)
+        ret = tuple(ret)
+      elif n.op_type == "Slice": # need to check onnx_model_version
+        if onnx_model_version < 10:
+          axes, ends, starts, steps = list(opt.get("axes", range(inp[0].ndim))), list(opt["ends"]), list(opt["starts"]), [1]*inp[0].ndim
+        else:
+          starts, ends = inp[1:3]
+          axes = safe_numpy(Tensor.arange(inp[0].ndim, dtype=dtypes.int32) if len(inp) <= 3 else inp[3]).tolist()
+          steps = safe_numpy(inp[4]) if len(inp) > 4 else [1]*inp[0].ndim
+          starts, ends = safe_numpy(starts.ceil().cast(dtypes.int32)).tolist(), safe_numpy(ends.ceil().cast(dtypes.int32)).tolist()
+        arg = [(0,x,1) for x in inp[0].shape]
+        for i, axis in enumerate(axes):
+          axis = int(axis) + inp[0].ndim if axis < 0 else int(axis)
+          starts[i], ends[i] = starts[i] + inp[0].shape[axis] if starts[i] < 0 else starts[i], ends[i] + inp[0].shape[axis] if ends[i] < 0 else ends[i]
+          starts[i], ends[i] = max(0, min(starts[i], inp[0].shape[axis])), max(0, min(ends[i], inp[0].shape[axis]))
+          if starts[i] > ends[i] and steps[i] >= 0: steps[i] = -steps[i]
+          arg[axis] = (starts[i], ends[i], steps[i])
+        new_shape = tuple((s, e) if st > 0 else (e+1, s+1) for s, e, st in arg)
+        if any(s==e for s,e in new_shape): ret = inp[0].shrink(new_shape)
+        else: ret = inp[0].__getitem__(tuple([slice(s,e,st) for s,e,st in arg]))
+      elif n.op_type == "Gradient": # need to call backward on intermediate_tensors
+        assert len(opt["xs"]) == len(inp), f"len(opt['xs']):{len(opt['xs'])}, len(inp):{len(inp)} output and input has to match"
+        y = opt["y"]
+        intermediate_tensors[y].backward()
+        ret = tuple([t.grad for t in inp])
       elif hasattr(onnx_ops, n.op_type):
         fxn = getattr(onnx_ops, n.op_type)
         if isinstance(fxn, dict):
           for k in sorted(fxn.keys()):
-            if k < onnx_version:
+            if k <= onnx_model_version:
               real_fxn = fxn[k]
         else:
           real_fxn = fxn
@@ -178,9 +196,11 @@ def get_run_onnx(onnx_model):
         raise Exception(f"op_type {n.op_type} not supported")
       if not isinstance(ret, tuple): ret = (ret, )
       assert len(n.output) <= len(ret), f"expected output size must be less than {len(ret)}, it's {n.output}"
-      if debug: print([x.shape if isinstance(x, Tensor) else None for x in ret])
-      for i in range(len(n.output)): intermediate_tensors[n.output[i]] = ret[i]
-      #print(ret[0].numpy().mean())
+      if debug >= 2: print([x.shape if isinstance(x, Tensor) else None for x in ret])
+      if debug >= 2: print("outputs:")
+      for i in range(len(n.output)):
+        if debug >= 2: print(f"\t{n.output[i]} - {ret[i]}")
+        intermediate_tensors[n.output[i]] = ret[i]
       if num == ONNXLIMIT:
         output_tensor_names = n.output
         break
diff --git a/tinygrad_repo/extra/onnx_ops.py b/tinygrad_repo/extra/onnx_ops.py
index 35c1d92ebe..08bbfc0850 100644
--- a/tinygrad_repo/extra/onnx_ops.py
+++ b/tinygrad_repo/extra/onnx_ops.py
@@ -1,28 +1,201 @@
 from tinygrad.tensor import Tensor
-from tinygrad.helpers import prod
+from tinygrad.helpers import prod, dtypes, ImageDType
 from extra.onnx import safe_numpy
+from onnx.helper import tensor_dtype_to_np_dtype
+from onnx.onnx_pb import TensorProto
+import os
 import numpy as np
 import functools
+from typing import Union, Tuple, Optional, List, Any
+import math
 
-def Unsqueeze(data, axes):
+# **************** Free Ops ****************
+
+def Identity(input: Tensor): return input
+def Neg(input: Tensor): return -input
+def Add(input: Tensor, other: Tensor, broadcast=None): return input + other if input.dtype == dtypes.float or isinstance(input.dtype, ImageDType) else (input + other).cast(input.dtype)
+def Sub(input: Union[Tensor, Any], other: Tensor): return input - other # some test has input as int
+def Mul(input: Tensor, other: Tensor): return (input * other) if input.dtype == dtypes.float or isinstance(input.dtype, ImageDType) else (input * other).cast(input.dtype)
+# in openpilot, due to SHUFFLE_PAD_OPS issues, we are spending an extra kernel
+def Div(input: Tensor, other: Tensor): return input / other if input.dtype == dtypes.float or isinstance(input.dtype, ImageDType) else input.div(other).floor()
+def Pow(input: Tensor, other: Tensor): return (input.float() ** other.float()).cast(input.dtype)
+def Reciprocal(input: Tensor): return input.reciprocal()
+def Sqrt(input: Tensor): return input.sqrt()
+def Sign(input: Tensor): return input.sign()
+def Abs(input: Tensor): return input.abs()
+def Exp(input: Tensor): return input.exp()
+def Log(input: Tensor): return input.log()
+def Mish(input: Tensor): return input.mish()
+def Sin(x: Tensor): return x.sin()
+def Cos(x: Tensor): return x.cos()
+def Tan(x: Tensor): return x.tan()
+def Relu(input: Tensor): return input.relu()
+def Sigmoid(input: Tensor): return input.sigmoid()
+def Tanh(input: Tensor): return input.tanh()
+def MatMul(input: Tensor, other: Tensor): return input.matmul(other)
+def Floor(x:Tensor): return x.floor()
+def Ceil(x:Tensor): return x.ceil()
+def Less(x:Tensor,y:Tensor): return (x<y).cast(dtypes.bool)
+def LessOrEqual(x:Tensor,y:Tensor): return (x<=y).cast(dtypes.bool)
+def Greater(x:Tensor,y:Tensor): return (x>y).cast(dtypes.bool)
+def GreaterOrEqual(x:Tensor,y:Tensor): return (x>=y).cast(dtypes.bool)
+def Equal(x:Tensor,y:Tensor): return (x==y).cast(dtypes.bool)
+def Max(*data_0): return functools.reduce(Tensor.maximum, data_0)
+def Min(*data_0): return functools.reduce(Tensor.minimum, data_0)
+def Sum(*data_0): return functools.reduce(Tensor.__add__, data_0)
+def Mean(*data_0): return functools.reduce(Tensor.__add__, data_0) / len(data_0)
+def Where(condition:Tensor,X:Tensor,Y:Tensor): return condition.where(X, Y).cast(X.dtype)
+def Cast(input: Tensor, to): return input.cast(dtypes.from_np(tensor_dtype_to_np_dtype(to)))
+
+# **************** Simple Ops ****************
+
+def Constant(value: Tensor=None, value_float=None, value_floats=None, value_int=None, value_ints=None, value_string=None, value_strings=None):
+  if value: return value
+  elif value_float: return Tensor(value_float, dtype=dtypes.float32, requires_grad=False)
+  elif value_floats: return Tensor(list(value_floats), dtype=dtypes.float32, requires_grad=False)
+  elif value_int: return Tensor(value_int, dtype=dtypes.int64, requires_grad=False)
+  elif value_ints: return Tensor(list(value_ints), dtype=dtypes.int64, requires_grad=False)
+  elif value_string or value_strings: raise NotImplementedError(f'value_string or value_strings not implemented for Constant op')
+
+def Softsign(input: Tensor): return input / (1+input.abs())
+def Cosh(x): return (math.e ** x + math.e ** -x) / 2
+def Sinh(x): return (math.e ** x - math.e ** -x) / 2
+def Tanh(x): return x.tanh()
+
+def HardSigmoid(input: Tensor, alpha=0.2, beta=0.5): return (alpha*input + beta).clip(0, 1)
+def HardSwish(input: Tensor): return input * HardSigmoid(input, 1/6, 0.5)
+def Celu(X: Tensor, alpha=1.0): return X.relu() - (-alpha*(X/alpha).exp()+1).relu()
+def Selu(X: Tensor, alpha=1.67326319217681884765625, gamma=1.05070102214813232421875): return gamma * (X.relu() - (-alpha*X.exp()+alpha).relu())
+def Softplus(X: Tensor): return X.softplus()
+def PRelu(X:Tensor, slope:Tensor):
+  slope = slope[0] if slope.shape[-1] != X.shape[-1] else slope # HACK OnnxBackendPyTorchConvertedModelTest HAS WEIRD SLOPE WHERE IT'S [0.25, 0.25, 0.25] FOR ANY X.SHAPE
+  return X.clip(0, float("inf")) + X.clip(float("-inf"), 0) * slope
+def LeakyRelu(X: Tensor, alpha=0.01): return X.leakyrelu(alpha)
+def ThresholdedRelu(X: Tensor, alpha=1.0): return (X-alpha).relu() + (X-alpha).relu().sign() * alpha
+def Softmax_1(input: Tensor, axis=1): return input.softmax(axis)
+def Softmax_13(input: Tensor, axis=-1): return input.softmax(axis)
+Softmax = {1: Softmax_1, 13: Softmax_13}   # Softmax default axis changed
+def LogSoftmax(input: Tensor, axis=-1): return input.log_softmax(axis)
+def Clip(input: Tensor, min=None, max=None): return input.clip(float('-inf') if min is None else min, float('inf') if max is None else max)
+
+# NOTE ReduceProd would require a new llop
+def _axes(axes, noop_with_empty_axes): return [int(x) for x in safe_numpy(axes)] if axes is not None and not (isinstance(axes, Tensor) and axes.shape == (0,)) else ([] if noop_with_empty_axes else None)
+def ReduceMax(data: Tensor, axes=None, keepdims=1, noop_with_empty_axes=0): return data.max(_axes(axes, noop_with_empty_axes), keepdim=keepdims)
+def ReduceMin(data: Tensor, axes=None, keepdims=1, noop_with_empty_axes=0): return data.min(_axes(axes, noop_with_empty_axes), keepdim=keepdims)
+def ReduceSum(data: Tensor, axes=None, keepdims=1, noop_with_empty_axes=0): return data.sum(_axes(axes, noop_with_empty_axes), keepdim=keepdims)
+def ReduceMean(data: Tensor, axes=None, keepdims=1, noop_with_empty_axes=0): return data.mean(_axes(axes, noop_with_empty_axes), keepdim=keepdims)
+def ReduceSumSquare(data: Tensor, axes=None, keepdims=1, noop_with_empty_axes=0): return data.square().sum(_axes(axes, noop_with_empty_axes), keepdim=keepdims)
+def ReduceL1(data: Tensor, axes=None, keepdims=1, noop_with_empty_axes=0): return data.abs().sum(_axes(axes, noop_with_empty_axes), keepdim=keepdims)
+def ReduceL2(data: Tensor, axes=None, keepdims=1, noop_with_empty_axes=0): return data.square().sum(_axes(axes, noop_with_empty_axes), keepdim=keepdims).sqrt()
+def ReduceLogSum(data: Tensor, axes=None, keepdims=1, noop_with_empty_axes=0): return data.sum(_axes(axes, noop_with_empty_axes), keepdim=keepdims).log()
+def ReduceLogSumExp(data: Tensor, axes=None, keepdims=1, noop_with_empty_axes=0): return data.exp().sum(_axes(axes, noop_with_empty_axes), keepdim=keepdims).log()
+
+def GlobalAveragePool(X: Tensor): return X.mean(axis=tuple(range(2, len(X.shape))), keepdim=True)
+def GlobalMaxPool(X: Tensor): return X.max(axis=tuple(range(2, len(X.shape))), keepdim=True)
+def OptionalHasElement(x: Tensor=None): return Tensor(x is not None and x.numel() > 0, dtype=dtypes.bool)
+def OptionalGetElement(x: Tensor=None): return x if x is not None else Tensor([], dtype=dtypes.float32)
+
+def Tile(input: Tensor, repeats): return input.repeat([int(x) for x in safe_numpy(repeats)])
+def Range(start: Tensor, limit, delta): return Tensor.arange(start=int(safe_numpy(start)), stop=int(safe_numpy(limit)), step=int(safe_numpy(delta))).cast(dtype=start.dtype)
+def Shape(data: Tensor, end=None, start=0): return Tensor(list(data.shape)[start:end], dtype=dtypes.int32 if os.path.isfile("/TICI") else dtypes.int64)  # TODO: really?
+def Size(data: Tensor): return prod(data if isinstance(data, list) else data.shape)
+def Flatten(input: Tensor, axis=1): return input.reshape(prod((1,) + input.shape[0:axis]), -1)
+def Reshape(data: Tensor, shape: Tensor, allowzero=None): return data.reshape([int(x) if x != 0 else data.shape[i] for i,x in enumerate(safe_numpy(shape))])
+def Shrink(input: Tensor, bias=0.0, lambd=0.5): return (input < -lambd)*(input+bias) + (input > lambd)*(input-bias)
+def And(x:Tensor, y:Tensor): return Where((x==y), x, Tensor.zeros(*x.shape)).cast(dtypes.bool)
+def Or(x:Tensor, y:Tensor): return Where((x==y), x, Tensor.ones(*x.shape)).cast(dtypes.bool)
+def Xor(x:Tensor, y:Tensor): return Where((x==y), Tensor.zeros(*x.shape), Tensor.ones(*x.shape)).cast(dtypes.bool)
+def Not(x:Tensor): return Where((x==1), Tensor.zeros(*x.shape), Tensor.ones(*x.shape)).cast(dtypes.bool)
+
+def Asin(x): return Atan(x / Tensor.sqrt(1 - x * x))
+def Asinh(x): return Tensor.log(x + Tensor.sqrt(x * x + 1))
+def Acosh(x): return Tensor.log(x + Tensor.sqrt(x * x - 1))
+def Atanh(x): return 0.5 * Tensor.log((1 + x)/(1 - x))
+def Acos(x: Tensor):
+  negate = (x < 0)
+  x = x.abs()
+  ret = ((((-0.0187293 * x) + 0.0742610)*x - 0.2121144) * x + 1.5707288) * Tensor.sqrt(1.0 - x)
+  ret = ret - 2 * negate * ret
+  return negate * 3.14159265358979 + ret
+def Atan(y: Tensor):
+  x = Tensor.ones(y.shape)
+  t3 = x
+  t1 = y.abs()
+  t0 = (t3 > t1).where(t3, t1)
+  t1 = (t3 < t1).where(t3, t1)
+  t3 = t1 / t0
+  t4 = t3 * t3
+  t0 = ((((-0.013480470 * t4 + 0.057477314) * t4 - 0.121239071) * t4 + 0.195635925) * t4 - 0.332994597) * t4 + 0.999995630
+  t3 = t0 * t3
+  t3 = (y.abs() > x.abs()).where(1.570796327 - t3, t3)
+  return (y < 0).where(-t3, t3)
+
+def Trilu(x: Tensor, k: Union[Tensor, int]=0, upper=1):
+  k = int(k.numpy().item()) if k != 0 else 0 # onnx passes k as a tensor int64 with one element, default is 0
+  return x.triu(k) if upper else x.tril(k)
+
+def Squeeze(input: Tensor, axes):
+  if isinstance(axes, Tensor): axes = safe_numpy(axes)
+  axes = [int(x) if x >= 0 else int(x+input.ndim) for x in axes]
+  return input.reshape([s for i,s in enumerate(input.shape) if i not in axes])
+def Unsqueeze(data: Tensor, axes):
   axes = [len(data.shape) + int(x) if x < 0 else int(x) for x in safe_numpy(axes)]
-  ptr = 0
-  new_shape = []
-  for i in range(len(data.shape) + len(axes)):
-    if i in axes: new_shape.append(1)
-    else:
-      new_shape.append(data.shape[ptr])
-      ptr += 1
+  new_shape = [1] * (len(data.shape) + len(axes))
+  ptr = iter(data.shape)
+  for i in range(len(new_shape)):
+    if i not in axes:
+      new_shape[i] = next(ptr)
   return data.reshape(new_shape)
 
-def Gemm(A, B, C=None, alpha=1.0, beta=1.0, transA=0, transB=0):
-  ret = alpha * ((A.transpose() if transA == 1 else A) @ (B.transpose() if transB == 1 else B))
-  if C is not None: ret += beta * C
+def Binarizer(input, threshold=0.0): return input > threshold
+
+def ArgMax(x: Tensor, axis=0, keepdims=1, select_last_index=0):
+  axis = axis + x.ndim if axis < 0 else axis
+  m = x == (x.max(axis=axis, keepdim=keepdims) if keepdims else x.max(axis=axis, keepdim=keepdims).unsqueeze(axis))
+  c = Tensor.arange(x.shape[axis]).reshape(*[1]*(axis), x.shape[axis], *[1]*(x.ndim - axis-1)) * m
+  return c.max(axis=axis,keepdim=keepdims).cast(dtypes.int64)
+def ArgMin(x, axis=0, keepdims=1, select_last_index=0): return ArgMax(-x, axis=axis, keepdims=keepdims, select_last_index=select_last_index)
+
+def Elu(input: Tensor, alpha=1.0): return input.elu(alpha=alpha)
+def Concat(*inputs: List[Tensor], axis): return inputs[0].cat(*inputs[1:], dim=axis)
+def Transpose(input: Tensor, perm=None): return input.permute(order=list(range(len(input.shape))[::-1]) if perm is None else perm)
+
+# NOTE: since we only have one type, this is valid!
+def CastLike(input, target_type):
+  assert isinstance(target_type, Tensor), "can only CastLike Tensor"
+  return input
+
+def ConstantOfShape(input, value:Tensor=None):
+  if value is None: value=Tensor([0.0])
+  shape = [int(x) for x in safe_numpy(input)]
+  return Tensor.ones(*shape, dtype=value.dtype) * (value if shape[0]!=0 else 1)
+
+# TODO: abstract out the broadcast logic in tensor
+def Expand(input: Tensor, shape):
+  x_shape, y_shape = input.shape, [int(x) for x in safe_numpy(shape)]
+  # copied from _broadcasted
+  x_shape, y_shape = [([1]*(max(len(x_shape), len(y_shape))-len(t_shape)) + list(t_shape)) for t_shape in [x_shape, y_shape]]
+  shape_ret = tuple(max(sx, sy) for sx,sy in zip(x_shape, y_shape))
+  return input.reshape(x_shape).expand(shape_ret)
+
+# **************** Complex Ops ****************
+
+def Gemm(A: Tensor, B: Tensor, C: Tensor=None, alpha=1.0, beta=1.0, transA=0, transB=0, broadcast=0):
+  ret = alpha * (A.transpose(transA) @ B.transpose(transB))
+  if C is not None: ret += beta * (C if broadcast == 0 else C.reshape([-1 if i <  len(C.shape) else 1 for i in range(len(ret.shape))][::-1]))
   return ret
 
+# works with Tensors.ndim != 4
+def _batchnorm(self:Tensor, weight:Optional[Tensor], bias:Optional[Tensor], mean:Tensor, invstd:Tensor):
+  shape = [1, -1] + [1] * (self.ndim-2)
+  x = (self - mean.reshape(shape=shape))
+  if weight: x = x * weight.reshape(shape=shape)
+  ret = x.mul(invstd.reshape(shape=shape) if len(invstd.shape) == 1 else invstd)
+  return (ret + bias.reshape(shape=shape)) if bias else ret
+
 # TODO: this is copied from tinygrad/nn/__init__.py
 # spatial is from opset 7 and has since been removed
-def BatchNormalization(X, scale, B, input_mean, input_var, epsilon=1e-05, momentum=0.9, training_mode=0, spatial=1):
+def BatchNormalization(X: Tensor, scale, B, input_mean, input_var, epsilon=1e-05, momentum=0.9, training_mode=0, spatial=1, is_test=0):
   if training_mode:
     x_detached = X.detach()
     current_mean = x_detached.mean(axis=(0,2,3))
@@ -33,11 +206,17 @@ def BatchNormalization(X, scale, B, input_mean, input_var, epsilon=1e-05, moment
     running_mean = input_mean * momentum + current_mean * (1 - momentum)
     running_var = input_var * momentum + current_var * (1 - momentum)
 
-    return X.batchnorm(scale, B, current_mean, current_invstd), running_mean, running_var
+    return _batchnorm(X, scale, B, current_mean, current_invstd), running_mean, running_var
   else:
     invstd = (input_var + epsilon)**-0.5
-    return X.batchnorm(scale, B, input_mean, invstd)
-  
+    return _batchnorm(X, scale, B, input_mean, invstd)
+
+def InstanceNormalization(x: Tensor, scale: Tensor, bias: Tensor, epsilon=1e-05):
+  axis = tuple(range(2, len(x.shape)))
+  mean = x.mean(axis=axis, keepdim=True)
+  invstd = x.sub(mean).pow(2).mean(axis=axis, keepdim=True).add(epsilon).pow(-0.5)
+  return x.sub(mean).mul(scale.reshape(shape=[-1, 1, 1])).mul(invstd).add(bias.reshape(shape=[-1, 1, 1]))
+
 def LayerNormalization(x: Tensor, scale, bias, axis=-1, epsilon=1e-05, stash_type=1):
   assert stash_type == 1, "only float32 is supported"
   axis = tuple(i for i in range(axis if axis >= 0 else len(x.shape) + axis, len(x.shape)))
@@ -46,133 +225,496 @@ def LayerNormalization(x: Tensor, scale, bias, axis=-1, epsilon=1e-05, stash_typ
 
 def GroupNormalization(x: Tensor, scale: Tensor, bias: Tensor, num_groups, epsilon=1e-05):
   return x.reshape(x.shape[0], num_groups, -1).layernorm(axis=-1, eps=epsilon).mul(scale.unsqueeze(-1)).add(bias.unsqueeze(-1)).reshape(x.shape)
-  
-# TODO: expand to N-D
-def _padding(X, pads=None, auto_pad="NOTSET"):
-  assert auto_pad == "NOTSET"  # TODO: write this
-  if pads is not None:
-    return X.pad2d((pads[1], pads[3], pads[0], pads[2]))
-  else:
-    return X
 
-def AveragePool(X, kernel_shape, auto_pad="NOTSET", ceil_mode=0, count_include_pad=0, dilations=1, pads=None, strides=1):
-  # TODO: the padding shouldn't be counted in the average! this is causing a test failure
-  assert ceil_mode == 0 and dilations == 1
-  padding_included = _padding(X, pads, auto_pad).avg_pool2d(kernel_shape, stride=strides)
+# onnx: [x1_begin, x2_begin, ..., x1_end, x2_end, ...]
+# numpy.pad: ((x1_begin, x1_end), (x2_begin, x2_end), ...)
+def _format_padding(onnx_pads, ndims=None, axes=None):
+  if ndims and len(onnx_pads)//2 != ndims:  onnx_pads = onnx_pads * ndims # for OnnxBackendPyTorchConvertedModelTest the len(onnx_pads) == 2
+  if ndims is None: ndims = len(onnx_pads) // 2
+  if axes is None: axes = list(range(ndims))
+  num_axes = len(axes)
+  np_pads = [(0,0)] * ndims
+  for i in range(num_axes):
+    np_pads[axes[i]] = (onnx_pads[i], onnx_pads[i + num_axes])
+  return np_pads
+
+def _padding(X: Tensor, pads=None, auto_pad="NOTSET", axes=None, constant_value=0., strides=None, kernel_shape=None, dilations=None):
+  if auto_pad != "NOTSET": pads = _auto_pad(X, auto_pad, strides, kernel_shape, dilations)
+  if pads is None: return X
+  pads = _format_padding(pads, ndims=len(X.shape), axes=axes)
+  return X.pad(tuple(pads), value=constant_value)
+
+def _auto_pad(X, auto_pad, strides, kernel_shape, dilations):
+  strides = [strides]*len(kernel_shape) if isinstance(strides, int) else strides if strides else [1]*len(kernel_shape)
+  dilations = [1]*len(kernel_shape) if dilations == 1 else dilations
+  pad_shape = [(math.ceil(sh/st)-1)*st+((ks-1)*di+1)-sh for sh, st, ks, di in zip(X.shape[-len(strides):], strides, kernel_shape, dilations)]
+  if auto_pad == "SAME_UPPER": return [pad_shape[0]//2, pad_shape[1]//2, pad_shape[0]-pad_shape[0]//2, pad_shape[1]-pad_shape[1]//2]
+  elif auto_pad == "SAME_LOWER": return [pad_shape[0]-pad_shape[0]//2, pad_shape[1]-pad_shape[1]//2, pad_shape[0]//2,  pad_shape[1]//2]
+  else: raise NotImplementedError(f"auto_pad={auto_pad} not implemented, yet")
+
+def Pad(x: Tensor, pads: Union[Tensor, Tuple[int, ...]], constant_value: Tensor=None, axes: Tensor=None, mode="constant", value: float=0.):
+  constant_value = value if constant_value is None else float(safe_numpy(constant_value)[0])
+  seq_pads = list(pads) if isinstance(pads, tuple) else safe_numpy(pads)
+  seq_pads = [math.ceil(i) for i in seq_pads]
+  seq_axes = safe_numpy(axes).astype(np.int32).tolist() if axes is not None else None
+  base_shape = x.shape
+  pads = _format_padding(seq_pads, ndims=len(x.shape), axes=seq_axes)
+  if mode == "wrap":
+    repeat_args = [math.ceil(dim[0]/sh) + math.ceil(dim[1]/sh) + 1 for dim, sh in zip(pads, base_shape)]
+    new_shape = [s*r for s,r in zip(base_shape, repeat_args)]
+    shrink_args = [(sh-dim[0]%sh if dim[0]%sh != 0 else 0, nsh-(sh-dim[1]%sh) if dim[1]%sh != 0 else nsh) for dim, sh, nsh in zip(pads, base_shape, new_shape)]
+    return x.repeat(tuple(repeat_args)).shrink(tuple(shrink_args))
+  elif mode == "reflect":
+    for i,s in enumerate(x.shape):
+      if pads[i] == (0,0): continue
+      elif pads[i][0] and not pads[i][1]:
+        x = x.flip(i).shrink(tuple([(0,s_) if i_ != i else (s-pads[i][0]-1, s_-1) for i_,s_ in enumerate(x.shape)])).pad(tuple([(0,0) if i_ != i else (0,s) for i_ in range(x.ndim)])) + \
+            x.pad(tuple([(0,0) if i_ != i else pads[i] for i_ in range(x.ndim)]))
+      elif not pads[i][0] and pads[i][1]:
+        x = x.flip(i).shrink(tuple([(0,s_) if i_ != i else (1, pads[i][1]+1) for i_,s_ in enumerate(x.shape)])).pad(tuple([(0,0) if i_ != i else (s,0) for i_ in range(x.ndim)])) + \
+            x.pad(tuple([(0,0) if i_ != i else pads[i] for i_ in range(x.ndim)]))
+      else:
+        x = x.flip(i).shrink(tuple([(0,s_) if i_ != i else (s-pads[i][0]-1, s_-1) for i_,s_ in enumerate(x.shape)])).pad(tuple([(0,0) if i_ != i else (0,s+pads[i][1]) for i_ in range(x.ndim)])) + \
+            x.flip(i).shrink(tuple([(0,s_) if i_ != i else (1, pads[i][1]+1) for i_,s_ in enumerate(x.shape)])).pad(tuple([(0,0) if i_ != i else (s+pads[i][0],0) for i_ in range(x.ndim)])) + \
+            x.pad(tuple([(0,0) if i_ != i else pads[i] for i_ in range(x.ndim)]))
+    return x
+  elif mode == "edge":
+    for i,s in enumerate(x.shape):
+      if pads[i] == (0,0): continue
+      elif pads[i][0] and not pads[i][1]:
+        x = x.shrink(tuple([(0,s_) if i_ != i else (0,1) for i_,s_ in enumerate(x.shape)])).expand([pads[i][0] if i_ == i else s_ for i_,s_ in enumerate(x.shape)]).pad(tuple([(0,0) if i_ != i else (0,s) for i_ in range(x.ndim)])) + \
+            x.pad(tuple([(0,0) if i_ != i else pads[i] for i_ in range(x.ndim)]))
+      elif not pads[i][0] and pads[i][1]:
+        x = x.shrink(tuple([(0,s_) if i_ != i else (s_-1, s_) for i_,s_ in enumerate(x.shape)])).expand([pads[i][0] if i_ == i else s_ for i_,s_ in enumerate(x.shape)]).pad(tuple([(0,0) if i_ != i else (s+pads[i][0],0) for i_ in range(x.ndim)])) + \
+            x.pad(tuple([(0,0) if i_ != i else pads[i] for i_ in range(x.ndim)]))
+      else:
+        x = x.shrink(tuple([(0,s_) if i_ != i else (0,1) for i_,s_ in enumerate(x.shape)])).expand([pads[i][0] if i_ == i else s_ for i_,s_ in enumerate(x.shape)]).pad(tuple([(0,0) if i_ != i else (0,s+pads[i][1]) for i_ in range(x.ndim)])) + \
+            x.shrink(tuple([(0,s_) if i_ != i else (s_-1, s_) for i_,s_ in enumerate(x.shape)])).expand([pads[i][1] if i_ == i else s_ for i_,s_ in enumerate(x.shape)]).pad(tuple([(0,0) if i_ != i else (s+pads[i][0],0) for i_ in range(x.ndim)])) + \
+            x.pad(tuple([(0,0) if i_ != i else pads[i] for i_ in range(x.ndim)]))
+    return x
+  elif mode == "constant":
+    return _padding(x, seq_pads, axes=seq_axes, constant_value=constant_value)
+
+def AveragePool(X: Tensor, kernel_shape, auto_pad="NOTSET", ceil_mode=0, count_include_pad=0, dilations=1, pads=None, strides=1):
+  if dilations != 1: raise NotImplementedError(f"dilations != 1 not supported, dilations:{dilations}")
+  pixel_axes = tuple(range(len(X.shape)))[-2:]
+  if ceil_mode: auto_pad = "SAME_UPPER"
+  padding_included = _padding(X, pads, auto_pad, axes=pixel_axes, strides=strides, kernel_shape=kernel_shape, dilations=dilations).avg_pool2d(kernel_shape, stride=strides)
   if count_include_pad:
     return padding_included
   else:
-    div = _padding(Tensor.ones(*X.shape), pads, auto_pad).avg_pool2d(kernel_shape, stride=strides)
+    div = _padding(Tensor.ones(*X.shape), pads, auto_pad, axes=pixel_axes, strides=strides, kernel_shape=kernel_shape, dilations=dilations).avg_pool2d(kernel_shape, stride=strides)
     return padding_included / div
 
-def MaxPool(X, kernel_shape, auto_pad="NOTSET", ceil_mode=0, dilations=1, pads=None, storage_order=0, strides=1):
-  # TODO: the padding should be infinity, not 0!
-  assert ceil_mode == 0 and storage_order == 0 and dilations == 1
-  return _padding(X, pads, auto_pad).max_pool2d(kernel_shape, stride=strides)
-
-def Conv(X, W, B=None, auto_pad="NOTSET", dilations=1, group=1, kernel_shape=None, pads=None, strides=1):
-  return X.conv2d(W, B, stride=strides, groups=group, dilation=dilations, padding=(pads[1], pads[3], pads[0], pads[2]) if pads is not None else 0)
-
-# TODO: copied from tensor.py
-def Dropout(data, ratio=0.5, training_mode=False, seed=None):
-  # TODO: mask should be a boolean tensor
-  if not training_mode: return data, Tensor.ones(*data.shape)  # if mask is requested as output it will contain all ones.
-  if seed is not None: Tensor.manual_seed(seed)
-  _mask : np.ndarray = np.asarray(Tensor._rng.binomial(1, 1.0-ratio, size=data.shape), dtype=data.dtype)
-  mask = Tensor(_mask, requires_grad=False, device=data.device)
-  return data * mask * (1/(1.0 - ratio)), mask
+def MaxPool(X: Tensor, kernel_shape, auto_pad="NOTSET", ceil_mode=0, dilations=1, pads=None, storage_order=0, strides=1):
+  if ceil_mode: auto_pad = "SAME_UPPER"
+  ret = _padding(X, pads, auto_pad, constant_value=-np.inf, axes=tuple(range(len(X.shape)))[-len(kernel_shape):], strides=strides, kernel_shape=kernel_shape, dilations=dilations)
+  ret = ret.max_pool2d(kernel_shape, stride=strides, dilation=dilations)
+  ret_len, X_len = ret.numel(), X.numel()
+  indices = ((ret.flatten().unsqueeze(1).expand(ret_len, X_len) == X.flatten().reshape(1, X_len).expand(ret_len, X_len)) * Tensor.arange(X_len).reshape(1, X_len).expand(ret_len, X_len)).sum(1).reshape(ret.shape).cast(dtypes.int64)
+  if storage_order: indices = indices.transpose(indices.ndim-2, indices.ndim-1)
+  return ret, indices
 
-def Shape(data, end=None, start=0): return list(data.shape)[start:end]
-def Size(data): return prod(data.shape)
+def MaxUnpool(xT: Tensor, xI: Tensor, outshape: Tensor=None, kernel_shape=None, pads=None, strides=None):
+  out_sh = [(ks//2)*2 + st * inps for inps, st, ks in zip(xI.shape, strides, kernel_shape)]
+  outlength = prod(out_sh)
+  xI = xI.flatten().unsqueeze(1).expand(prod(xT.shape), outlength)
+  arange = Tensor.arange(outlength, requires_grad=False).reshape(1, outlength).expand(xI.shape)
+  xT = xT.flatten().unsqueeze(1).expand(prod(xT.shape), outlength)
+  ret = ((xI == arange) * xT).sum(0).reshape([1, 1] + out_sh)
+  if outshape is not None:
+    outshape = safe_numpy(outshape).tolist()
+    if outshape != ret.shape:
+      diff = [outshape[2] - ret.shape[2], outshape[3] - ret.shape[3]]
+      pad_args = [diff[0]//2, diff[1]//2, diff[0]-diff[0]//2, diff[1]-diff[1]//2]
+      ret = ret.pad2d((pad_args[1], pad_args[3], pad_args[0], pad_args[2]))
+  return ret
 
-# TODO: this doesn't match Tensor.flatten behavior
-def Flatten(input, axis=1):
-  new_shape = (1, -1) if axis == 0 else (prod(input.shape[0:axis]), -1)
-  return input.reshape(new_shape)
+def Conv(X: Tensor, W: Tensor, B=None, auto_pad="NOTSET", dilations=1, group=1, kernel_shape=None, pads=None, strides=1):
+  if auto_pad != "NOTSET": padding = _auto_pad(X, auto_pad, strides, kernel_shape, dilations)
+  else: padding = [p for ps in zip(pads[:len(pads)//2][::-1], pads[len(pads)//2:][::-1]) for p in ps] if pads is not None else 0 # reorder padding
+  return X.conv2d(W, B, stride=strides, groups=group, dilation=dilations, padding=padding)
 
-# TODO: abstract out the broadcast logic in tensor
-def Expand(input, shape):
-  x_shape, y_shape = input.shape, [int(x) for x in safe_numpy(shape)]
-  # copied from _broadcasted
-  x_shape, y_shape = [([1]*(max(len(x_shape), len(y_shape))-len(t_shape)) + list(t_shape)) for t_shape in [x_shape, y_shape]]
-  shape_ret = tuple(max(sx, sy) for sx,sy in zip(x_shape, y_shape))
-  return input.reshape(x_shape).expand(shape_ret)
+def ConvTranspose(X: Tensor, W: Tensor, B=None, auto_pad="NOTSET", dilations=1, group=1, kernel_shape=None, pads=None, output_shape=None, output_padding=0, strides=1):
+  if not kernel_shape: kernel_shape = W.shape
+  if pads is None and auto_pad != "NOTSET": pads = _auto_pad(X, auto_pad, strides, kernel_shape, dilations)
+  elif pads is None and auto_pad == "NOTSET": pads = [0,0] * (X.ndim - 2)
+  strides_ = [1]*(W.ndim-1) + [strides] if isinstance(strides, int) else [1]*(W.ndim-len(strides)) + list(strides)
+  dilations_ = [1]*(W.ndim-1) + [dilations] if isinstance(dilations, int) else [1]*(W.ndim-len(dilations)) + list(dilations)
+  if output_shape and not output_padding:
+    out_sh = [st*(xs-1) + (ks-1)*di+1 if n < 2 else st*(xs-1) + (ks-1)*di+1 - pads[n-2] - pads[n-1] for n, (st, xs, ks, di) in enumerate(zip(strides_, X.shape, kernel_shape, dilations_))]
+    output_padding = [os - rs for os, rs in zip(output_shape, out_sh[-len(output_shape):])]
+  return X.conv_transpose2d(W, B, stride=strides, groups=group, dilation=dilations, padding=pads if pads is not None else 0, output_padding=output_padding)
+
+# Reimplemented here because you need legacy RNG for passing ONNX tests.
+def Dropout(data: Tensor, ratio=0.5, training_mode=False, seed=None):
+  if isinstance(ratio, Tensor) and not ratio.shape: ratio = safe_numpy(ratio) # ratio and tensor is passed in as Tensor with shape: ()
+  if isinstance(training_mode, Tensor) and not training_mode.shape: training_mode = safe_numpy(training_mode)
+  if not training_mode: return data, Tensor.ones(*data.shape, dtype=dtypes.bool)  # if mask is requested as output it will contain all True's.
+  rng = np.random.RandomState(seed)
+  ratio = ratio.lazydata.realize().toCPU()[0] if isinstance(ratio, Tensor) else ratio
+  mask = Tensor((rng.random(data.shape) >= ratio), requires_grad=False, device=data.device)
+  return data * mask * (1/(1.0 - ratio)), mask
 
-def LRN(input, size, alpha=1e-4, beta=0.75, bias=1.0):
-  bs, c, iy, ix = input.shape 
+def LRN(input: Tensor, size, alpha=1e-4, beta=0.75, bias=1.0):
+  bs, c, iy, ix = input.shape
   return input / input.mul(input).reshape(bs,1,c,iy*ix).pad2d((0,0,(size-1)//2, size//2)).avg_pool2d((size, 1), 1).reshape(bs,c,iy,ix).mul(alpha).add(bias).pow(beta)
 
-def Identity(input): return input
-def Neg(input): return -input
-def Reciprocal(input): return input.reciprocal()
-def Sqrt(input): return input.sqrt()
-def Sign(input): return input.sign()
-def Softsign(input): return input / (1+input.abs())
-def Abs(input): return input.abs()
-def Exp(input): return input.exp()
-def Log(input): return input.log()
-def Mish(input): return input.mish()
-def HardSigmoid(input, alpha=0.2, beta=0.5): return (alpha*input + beta).clip(0, 1)
-def HardSwish(input): return input * HardSigmoid(input, 1/6, 0.5)
-def Celu(X, alpha=1.0): return X.relu() - (-alpha*(X/alpha).exp()+1).relu()
-def Selu(X, alpha=1.67326319217681884765625, gamma=1.05070102214813232421875): return gamma * (X.relu() - (-alpha*X.exp()+alpha).relu())
-def Softplus(X): return X.softplus()
-def PRelu(X, slope): return X.leakyrelu(slope)
-def LeakyRelu(X, alpha=0.01): return X.leakyrelu(alpha)
-def ThresholdedRelu(X, alpha=1.0): return (X-alpha).relu() + (X-alpha).relu().sign() * alpha
-def Softmax_1(input, axis=1): return input.softmax(axis)
-def Softmax_13(input, axis=-1): return input.softmax(axis)
-Softmax = {1: Softmax_1, 13: Softmax_13}   # Softmax default axis changed
-def LogSoftmax(input, axis=-1): return input.log_softmax(axis)
-def Clip(input, min=-3.4e38, max=3.4e38): return input.clip(min, max)
+def MeanVarianceNormalization(input: Tensor, axis=(0, 2, 3)):
+  data_mean = input.mean(axis=axis, keepdim=True)
+  std = ((input**2).mean(axis=axis, keepdim=True) - data_mean**2).sqrt()
+  return (input - data_mean) / (std + 1e-9)
 
-def Less(x, y): return (x<y).numpy().astype(bool)
-def LessOrEqual(x, y): return (x<=y).numpy().astype(bool)
-def Greater(x, y): return (x>y).numpy().astype(bool)
-def GreaterOrEqual(x, y): return (x>=y).numpy().astype(bool)
-def Equal(x, y): return (x.eq(y)).numpy().astype(bool)
+def NegativeLogLikelihoodLoss(input: Tensor, target: Tensor, weight=None, ignore_index=None, reduction="mean"):
+  target = target.cast(dtypes.float32)
+  N, C, i_shape = input.shape[0], input.shape[1], input.shape
+  t_shape = target.shape
+  if len(input.shape) != 3:
+    input = input.reshape((N, C, -1))
+    target = target.reshape((N, -1))
+  if weight is not None:
+    mask = target.unsqueeze(-1) == Tensor.arange(C).repeat((N, 1, 1))
+    weight = (mask * weight).sum(axis=-1)
+  if ignore_index is not None:
+    cond = target == ignore_index
+    weight = cond.where(0, weight) if weight is not None else cond.where(Tensor.zeros(*target.shape), 1)
+  mask = target[:, None, :] ==  Tensor.arange(C).reshape([1, C] + [1]*(len(input.shape) -2))
+  loss = (-mask * input).sum(axis=1) * (1 if weight is None else weight)
+  if reduction == "mean": return loss.mean() if weight is None else loss.sum() / weight.sum()
+  elif reduction == "sum": return loss.sum()
+  return loss.reshape(t_shape) if len(i_shape) != 3 else loss
 
-def Max(*data_0): return functools.reduce(Tensor.maximum, data_0)
-def Min(*data_0): return -functools.reduce(Tensor.maximum, [-x for x in data_0])
-def Sum(*data_0): return functools.reduce(Tensor.__add__, data_0)
-def Mean(*data_0): return functools.reduce(Tensor.__add__, data_0) / len(data_0)
+def SoftmaxCrossEntropyLoss(scores: Tensor, labels: Tensor, weights=None, ignore_index=None, reduction="mean"):
+  N, C, *s_dimensions = scores.shape
+  if ignore_index is not None: labels = (labels == ignore_index).where(C+1, labels)
+  mask = labels.unsqueeze(1) == Tensor.arange(C).reshape(1, C, *[1]*len(s_dimensions))
+  y = scores.log_softmax(axis=1)
+  if weights is not None: weights = weights.__getitem__(tuple([labels, *[slice(None)]*(weights.ndim-1)]))
+  loss = (mask * -y).sum(1) if weights is None else (mask * -y).sum(1) * weights
+  if reduction == "mean": loss = loss.sum() / (loss == 0).where(0, 1).sum() if weights is None else loss.sum() / weights.sum()
+  elif reduction == "sum": loss = loss.sum()
+  return loss, y
 
-def _axes(axes, noop_with_empty_axes): return [int(x) for x in safe_numpy(axes)] if axes is not None else ([] if noop_with_empty_axes else None)
-
-# ReduceProd would require a new llop
-def ReduceMax(data, axes=None, keepdims=1, noop_with_empty_axes=0): return data.max(_axes(axes, noop_with_empty_axes), keepdim=keepdims)
-def ReduceMin(data, axes=None, keepdims=1, noop_with_empty_axes=0): return data.min(_axes(axes, noop_with_empty_axes), keepdim=keepdims)
-def ReduceSum(data, axes=None, keepdims=1, noop_with_empty_axes=0): return data.sum(_axes(axes, noop_with_empty_axes), keepdim=keepdims)
-def ReduceMean(data, axes=None, keepdims=1, noop_with_empty_axes=0): return data.mean(_axes(axes, noop_with_empty_axes), keepdim=keepdims)
-def ReduceSumSquare(data, axes=None, keepdims=1, noop_with_empty_axes=0): return data.square().sum(_axes(axes, noop_with_empty_axes), keepdim=keepdims)
-def ReduceL1(data, axes=None, keepdims=1, noop_with_empty_axes=0): return data.abs().sum(_axes(axes, noop_with_empty_axes), keepdim=keepdims)
-def ReduceL2(data, axes=None, keepdims=1, noop_with_empty_axes=0): return data.square().sum(_axes(axes, noop_with_empty_axes), keepdim=keepdims).sqrt()
-def ReduceLogSum(data, axes=None, keepdims=1, noop_with_empty_axes=0): return data.sum(_axes(axes, noop_with_empty_axes), keepdim=keepdims).log()
-def ReduceLogSumExp(data, axes=None, keepdims=1, noop_with_empty_axes=0): return data.exp().sum(_axes(axes, noop_with_empty_axes), keepdim=keepdims).log()
-
-def GlobalAveragePool(X): return X.mean(axis=tuple(range(2, len(X.shape))), keepdim=True)
-def GlobalMaxPool(X): return X.max(axis=tuple(range(2, len(X.shape))), keepdim=True)
-
-def Tile(input, repeats):
-  repeats_ = [int(x) for x in safe_numpy(repeats)]
-  new_shape = [x for i in range(len(input.shape)) for x in [1,input.shape[i]]]
-  expand_shape = [x for r,s in zip(repeats_, input.shape) for x in [r,s]]
-  final_shape = [r*s for r,s in zip(repeats_, input.shape)]
-  return input.reshape(new_shape).expand(expand_shape).reshape(final_shape)
-
-def Range(start, limit, delta): return Tensor.arange(safe_numpy(limit)[0], safe_numpy(start)[0], safe_numpy(delta)[0])
-def Where(condition, X, Y): return condition*X + (1-condition)*Y
-
-def ConstantOfShape(input, value=0.0):
-  shape = [int(x) for x in safe_numpy(input)]
-  return Tensor.ones(*shape) * value
+def ArrayFeatureExtractor(input: Tensor, indices: Tensor): return input.__getitem__(tuple([slice(None) if i != (input.ndim-1) else indices for i in range(input.ndim)]))
+def Gather(input: Tensor, indices: Tensor, axis=0):
+  if indices.numel() < 9: # NOTE lessor kernels for smaller indices but kernel number increases depending on size of indices
+    input_sh = list(input.shape)
+    ret_shape = input_sh[:axis] + list(indices.shape) + input_sh[axis+1:]
+    if indices.ndim > 1: indices = indices.flatten()
+    indices = [int(safe_numpy(indices))] if indices.shape == () else [input_sh[axis]+int(x) if x<0 else int(x) for x in safe_numpy(indices)]
+    args = [[(0,x) if j != axis else (i,i+1) for j, x in enumerate(input_sh)] for i in indices]
+    return input.shrink(arg=tuple(args[0])).cat(*[input.shrink(arg=tuple(arg)) for arg in args[1:]], dim=axis).reshape(ret_shape)
+  else: # NOTE faster gather, fixed number of kernels, but exceeds limited kernels for openpilot
+    return input.__getitem__(tuple([slice(None) if i != axis else indices for i in range(input.ndim)]))
 
-# this is obviously wrong, but since we don't have types, it's better than nothing
-def Cast(input, to):
-  print(f"WARNING: attempting to cast to {to}")
-  return input
+def GatherElements(input: Tensor, indices: Tensor, axis):
+  indices = indices.sign().contiguous().__neg__().contiguous().relu() * input.shape[axis] + indices
+  return input.gather(indices, axis)
 
-# NOTE: since we only have one type, this is valid!
-def CastLike(input, target_type):
-  assert isinstance(target_type, Tensor), "can only CastLike Tensor"
-  return input
+def _round(x:Tensor, n:float, equidistant_case = "round_down") -> Tensor:
+  def _and(cond1, cond2): return ((cond1 + cond2) == 2).where(1, 0)
+  assert n <= 1, f"n:{n} shouldn't be larger than 1"
+  b = x.cast(dtypes.int32).contiguous().cast(x.dtype)
+  b = (b >= 0).where(b+n, b-n)
+  if equidistant_case == "round_down":
+    return (x > b).where(b+1-n, b-n)
+  elif equidistant_case == "round_up":
+    return (x >= b).where(b+1-n, b-n)
+  elif equidistant_case == "round_to_even":
+    x_ceil_fraction = x.ceil()/2
+    cond_ceil_even = x_ceil_fraction.ceil() == x_ceil_fraction
+    x = (_and(x == b, cond_ceil_even)).where(x+1-n, x)
+    x = (x > b).where(b+1-n, b-n)
+    return x
+
+def Round(X:Tensor): return _round(X, 0.5, "round_to_even")
+
+def Resize(X:Tensor, roi=None, scales=None, sizes=None, antialias=0, axes=None, coordinate_transformation_mode='half_pixel', cubic_coeff_a=-0.75, exclude_outside=0, extrapolation_value=0.0, keep_aspect_ratio_policy='stretch', mode='nearest', nearest_mode='round_prefer_floor'):
+  def _nearest_gather(X: Tensor, x_out, y_out): return X[:,:,y_out,:][:,:,:,x_out]
+  def _nearest_mode(x_resized: Tensor, nearest_mode: str, x_len):
+    if nearest_mode == "round_prefer_floor": ret = _round(x_resized, 0.5, "round_down")
+    elif nearest_mode == "round_prefer_ceil": ret = _round(x_resized, 0.5, "round_up")
+    elif nearest_mode == "floor": ret = x_resized.floor()
+    elif nearest_mode == "ceil": ret = x_resized.ceil()
+    return ret.clip(0, x_len-1)
+  def _coordinate_transformation(x_out, y_out, output_shape, scales_lol, roi=None):
+    if coordinate_transformation_mode == "half_pixel":
+      x_out = (x_out + 0.5)/Tensor(scales_lol[-1]) - 0.5 # TODO Tensor() because try (((Tensor([0,1,2,3,4,5])+0.5)/3.5 - 0.5)) with LLVM or METAL, inaccuacy.
+      y_out = (y_out + 0.5)/Tensor(scales_lol[-2]) - 0.5
+    elif coordinate_transformation_mode == "align_corners":
+      x_out = x_out * (X.shape[-1] - 1) / (output_shape[-1] - 1)
+      y_out = y_out * (X.shape[-2] - 1) / (output_shape[-2] - 1)
+    elif coordinate_transformation_mode == "asymmetric":
+      x_out = x_out/scales_lol[-1]
+      y_out = y_out/scales_lol[-2]
+    elif coordinate_transformation_mode == "half_pixel_symmetric":
+      x_out = X.shape[-1] / 2 * (1 - int(output_shape[-1]) / output_shape[-1]) + (x_out + 0.5) / scales_lol[-1] - 0.5
+      y_out = X.shape[-2] / 2 * (1 - int(output_shape[-2]) / output_shape[-2]) + (y_out + 0.5) / scales_lol[-2] - 0.5
+    elif coordinate_transformation_mode == "pytorch_half_pixel":
+      x_out = (x_out + 0.5)/scales_lol[-1] - 0.5 if output_shape[-1] > 1 else Tensor([0])
+      y_out = (y_out + 0.5)/scales_lol[-2] - 0.5 if output_shape[-2] > 1 else Tensor([0])
+    elif coordinate_transformation_mode == "tf_crop_and_resize":
+      x_out = roi[-1][0] * (X.shape[-1] - 1) + x_out * ((roi[-1][1] - roi[-1][0]) * (X.shape[-1] - 1) / (output_shape[-1] - 1))  if output_shape[-1] > 1 else Tensor([0.5 * (roi[-1][0] + roi[-1][1]) * (X.shape[-1] - 1)])
+      y_out = roi[-2][0] * (X.shape[-2] - 1) + y_out * ((roi[-2][1] - roi[-2][0]) * (X.shape[-2] - 1) / (output_shape[-2] - 1))  if output_shape[-2] > 1 else Tensor([0.5 * (roi[-2][0] + roi[-2][1]) * (X.shape[-2] - 1)])
+    return x_out.clip(0, X.shape[-1]-1), y_out.clip(0, X.shape[-2]-1)
+  if roi is not None:
+    roi = safe_numpy(roi)
+    roi = [(st,ed) for st, ed in zip(roi[:len(roi)//2], roi[len(roi)//2:])]
+    roi_ = [(1,1)] * 4
+    if axes is not None:
+      for a,r in zip(axes, roi):
+        roi_[a] = r
+      roi = roi_
+  if scales is not None:
+    scales = safe_numpy(scales).tolist()
+    if axes is not None:
+      scales_ = [1]*X.ndim
+      for a,s in zip(axes, scales):
+        scales_[a] = s
+      scales = scales_
+  elif sizes is not None:
+    sizes = [int(i) for i in safe_numpy(sizes)]
+    scales = []
+    if axes is not None:
+      sizes_ = [1]*X.ndim
+      for a,s in zip(axes, sizes):
+        sizes_[a] = s
+        scales.append(s/X.shape[a])
+      sizes = sizes_
+    else: scales = [si/xs for xs, si in zip(X.shape, sizes)]
+    if keep_aspect_ratio_policy == "not_larger":
+      scale = min(scales)
+      sizes = _round(Tensor(list(X.shape[-2:]))*scale, 0.5, "round_up")
+      sizes = list(X.shape[:-2]) + [int(i) for i in safe_numpy(sizes)]
+    elif keep_aspect_ratio_policy == "not_smaller":
+      scale = max(scales)
+      sizes = _round(Tensor(list(X.shape[-2:]))*scale, 0.5, "round_up")
+      sizes = list(X.shape[:-2]) + [int(i) for i in safe_numpy(sizes)]
+  output_shape = sizes if sizes else [math.floor(x*s) for x,s in zip(X.shape, scales)]
+  output_shape_ = sizes if sizes else [x*s for x,s in zip(X.shape, scales)]
+  scales_lol = [os/xs for xs, os in zip(X.shape, output_shape)]
+  x_out = Tensor.arange(output_shape[-1])
+  y_out = Tensor.arange(output_shape[-2])
+  if mode == "nearest":
+    x_out, y_out = _coordinate_transformation(x_out, y_out, output_shape, scales_lol, roi)
+    x_out = _nearest_mode(x_out, nearest_mode, X.shape[-1])
+    y_out = _nearest_mode(y_out, nearest_mode, X.shape[-1])
+    return _nearest_gather(X, x_out, y_out)
+  elif mode == "linear":
+    x_out, y_out = _coordinate_transformation(x_out, y_out, output_shape_, scales, roi)
+    ret = []
+    for y in safe_numpy(y_out):
+      for x in safe_numpy(x_out):
+        x_floor, y_floor = int(x), int(y)
+        y_shrink = (0, X.shape[2]) if X.shape[2] == 1 else (y_floor, y_floor+2) if y != y_floor else (y_floor, y_floor+1)
+        x_shrink = (x_floor, x_floor+2) if x != x_floor else (x_floor, x_floor+1)
+        shrink_args = ((0, X.shape[0]), (0, X.shape[1]), y_shrink, x_shrink)
+        corners = safe_numpy(X.shrink(shrink_args))
+        x1, x2, y1, y2 = x_floor, x_floor+1, y_floor, y_floor+1
+        if x == x_floor and y == y_floor: # TODO https://en.wikipedia.org/wiki/Bilinear_interpolation#Weighted_mean maybe do weighted mean?
+          ret.append(corners[0,0,0,0])
+        elif x == x_floor:
+          ret.append((corners[0,0,0,0] * (y2 - y) + corners[0,0,1,0] * (y - y1)) / (y2 - y1))
+        elif y == y_floor:
+          ret.append((corners[0,0,0,0] * (x2 - x) + corners[0,0,0,1] * (x - x1)) / (x2 - x1))
+        else:
+          ret.append((corners[0,0,0,0] * (x2 - x) * (y2 - y) + corners[0,0,0,1] * (x - x1) * (y2 - y) + corners[0,0,1,0] * (x2 - x) * (y - y1) + corners[0,0,1,1] * (x - x1) * (y - y1)) / ((x2 - x1) * (y2 - y1)))
+    return Tensor(ret).reshape(output_shape)
+  elif mode == "cubic":
+    raise Exception("cubic interpolation is not implemented")
+
+def CenterCropPad(input: Tensor, shape: Tensor, axes=None):
+  if not axes: axes = list(range(input.ndim))
+  shrink_arg = [(0,i) for i in input.shape]
+  pad_arg = [(0,0) for _ in range(input.ndim)]
+  shape = safe_numpy(shape).tolist()
+  for s, x in zip(shape, axes):
+    if s < input.shape[x]: shrink_arg[x] = (input.shape[x]//2 - s//2, input.shape[x]//2 + s//2) if s%2 == 0 else (input.shape[x]//2 - s//2 - 1, input.shape[x]//2 + s//2)
+    elif s > input.shape[x]: pad_arg[x] = ((s - input.shape[x])//2, (s - input.shape[x])//2)  if (s - input.shape[x])% 2 == 0 else ((s - input.shape[x])//2, (s - input.shape[x])//2 + 1)
+  return input.shrink(tuple(shrink_arg)).pad(tuple(pad_arg))
+
+def OneHot(indices: Tensor, depth: Tensor, values: Tensor, axis=-1):
+  depth = int(safe_numpy(depth).item())
+  indices, rank = (indices < 0).where(indices+depth, indices), len(indices.shape)
+  if axis < 0: axis += rank + 1
+  ls, rs = indices.shape[0:axis], indices.shape[axis: rank]
+  cond = indices[:,None] == Tensor.arange(depth).reshape((1,) * len(ls) + (depth,) + (1,) * len(rs))
+  return cond.where(values[1], values[0]).cast(values.dtype)
+
+def Erf(x: Tensor):
+  sign = x.sign()
+  x = x.abs()
+  t = 1.0 / (1.0 + 0.3275911 * x)
+  term1 = 0.254829592 * t
+  term2 = -0.284496736 * t ** 2
+  term3 = 1.421413741 * t ** 3
+  term4 = -1.453152027 * t ** 4
+  term5 = 1.061405429 * t ** 5
+  y = (term1 + term2 + term3 + term4 + term5)
+  return sign * (1.0 - y * Tensor.exp(-x * x))
+
+def Compress(inp: Tensor, condition: Tensor, axis=None):
+  if axis is None:
+    inp = inp.flatten()
+    axis = 0
+
+  axis = axis + inp.ndim if axis < 0 else axis
+
+  con_np = safe_numpy(condition)
+  con = Tensor(np.arange(condition.shape[0])[con_np]) # no boolean indexing in Tensor
+  return inp.__getitem__(tuple([slice(None) if i != axis else con for i in range(inp.ndim)]))
+
+type_map = {TensorProto.DOUBLE: dtypes.double, TensorProto.FLOAT: dtypes.float32}
+def EyeLike(x: Tensor, dtype=None, k=0):
+  if dtype is None: dtype = x.dtype
+  else: dtype = type_map[dtype]
+  shape = x.shape
+  dim = min(x.shape)
+  if shape[0] == shape[1]: return Tensor.eye(dim=dim, dtype=dtype)
+  else:
+    diff = (shape[0]-dim, shape[1]-dim)
+    padarg = tuple([(d, d) if d == 0 else (k, d-k) for d in diff])
+    return Tensor.eye(dim=dim, dtype=dtype).pad(padarg)
+
+def Upsample(X, scales, mode): return Resize(X=X, scales=scales, mode=mode)
+
+# Needs work
+def IsInf(x,detect_negative=1,detect_positive=1):
+  ret = (x == float("inf"))*detect_positive + (x == float("-inf"))*detect_negative + Tensor.zeros(*x.shape)
+  return ret.cast(dtypes.bool)
+
+# Needs work
+def DequantizeLinear(x: Tensor, x_scale: Tensor, x_zero_point=0, axis=1):
+  axis = axis + x.ndim if axis < 0 else axis
+  x_sc = x_scale.reshape(*[1]*axis, *x_scale.shape, *[1]*(x.ndim - axis - x_scale.ndim))
+  x_zer = x_zero_point.reshape(*[1]*axis, *x_scale.shape, *[1]*(x.ndim - axis - x_scale.ndim)) if isinstance(x_zero_point, Tensor) else x_zero_point
+  return (x - x_zer) * x_sc
+
+# Needs work
+def IsNaN(x):
+  return (x < float("-inf")).cast(dtypes.bool)
+
+# **************** com.microsoft Ops ****************
+
+def SkipLayerNormalization(input:Tensor, skip:Tensor, gamma, beta:Optional[Tensor]=None, bias:Optional[Tensor]=None, epsilon=None):
+  if epsilon is None: epsilon=1e-12
+  x = input + skip + bias
+  return x.layernorm(eps=epsilon) * gamma + beta, None, None, x
+
+def FastGelu(x:Tensor, bias:Optional[Tensor]=None):
+  x = x + bias
+  return 0.5 * x * (1 + (x * 0.797885 + 0.035677 * x ** 3).tanh())
+
+def EmbedLayerNormalization(input_ids: Tensor, segment_ids:Optional[Tensor]=None, word_embedding:Tensor=None, position_embedding:Tensor=None, segment_embedding:Optional[Tensor]=None, gamma=None, beta=None, mask:Optional[Tensor]=None, position_ids:Optional[Tensor]=None, epsilon=None, mask_index_type=None):
+  # https://github.com/microsoft/onnxruntime/blob/main/docs/ContribOperators.md#com.microsoft.EmbedLayerNormalization
+  assert (segment_ids is None) is (segment_embedding is None)
+  assert (mask is None) is (mask_index_type is None)
+  assert mask is None, "functionality not supported yet"  # TODO
+  input_shape = input_ids.shape
+  bsz, seq_length = input_shape[0], input_shape[1]
+  compute_seg_emb = (segment_embedding is not None and segment_ids is not None)
+  vocab_size, max_position_embeddings, type_vocab_size = word_embedding.shape[0], position_embedding.shape[0], (segment_embedding.shape[0] if compute_seg_emb else None)
+
+  def embedding(x:Tensor, vocab_size, weight:Tensor)->Tensor:  # TODO from nn.Embedding. Could probably upstream this to Tensor
+    vocab_counter = Tensor.arange(vocab_size, dtype=x.dtype, requires_grad=False).reshape(1, 1, vocab_size).expand(*x.shape, vocab_size)
+    return (vocab_counter == x.unsqueeze(2).expand(*x.shape, vocab_size)) @ weight
+
+  # bert embedding layer
+  if epsilon is None: epsilon = 1e-12
+  if position_ids is None: position_ids = Tensor.arange(seq_length, requires_grad=False).unsqueeze(0).expand(*input_shape)
+  wrd_embedding_res = embedding(input_ids, vocab_size, word_embedding)
+  pos_embedding_res = embedding(position_ids, max_position_embeddings, position_embedding)
+  seg_embedding_res = embedding(segment_ids, type_vocab_size, segment_embedding) if compute_seg_emb else None
+
+  embedding_sum = wrd_embedding_res + pos_embedding_res + seg_embedding_res
+  out = embedding_sum.layernorm(eps=epsilon) * gamma + beta
+  return out, None, embedding_sum
+
+def Attention(input:Tensor, weights, bias:Optional[Tensor]=None, mask_index:Optional[Tensor]=None, past:Optional[Tensor]=None, relative_position_bias:Optional[Tensor]=None, past_sequence_length:Optional[Tensor]=None, do_rotary=None, mask_filter_value=None, num_heads=None, past_present_share_buffer=None, qkv_hidden_sizes=None, scale=None, unidirectional=None):
+  # https://github.com/microsoft/onnxruntime/blob/main/docs/ContribOperators.md#com.microsoft.Attention
+  assert num_heads is not None  # required
+  assert (qkv_hidden_sizes is None and past is not None) or (qkv_hidden_sizes is not None)
+  assert relative_position_bias==do_rotary==past_sequence_length==mask_filter_value==past_present_share_buffer==scale==None, "functionality not supported yet"  # TODO strange params
+  hidden_size, v_hidden_size = qkv_hidden_sizes[1:] if qkv_hidden_sizes is not None else 2*(weights.shape[1] // 3,)
+
+  if unidirectional:  # gpt-style
+    assert hidden_size == v_hidden_size
+    xqkv = input.linear(weights, bias)
+    xq, xk, xv = [xqkv.slice([None, None, (i*hidden_size, (i+1)*hidden_size)]) for i in range(3)]
+  else:  # bert-style
+    wq, wk, wv = weights[:,:hidden_size], weights[:,hidden_size:hidden_size+v_hidden_size], weights[:,hidden_size+v_hidden_size:]
+    bq, bk, bv = (bias[:hidden_size], bias[hidden_size:hidden_size+v_hidden_size], bias[hidden_size+v_hidden_size]) if bias is not None else None
+    xq, xk, xv = [input.linear(w, b) for w, b in zip((wq, wk, wv), (bq, bk, bv))]
+  xq, xk, xv = [x.reshape(x.shape[0], x.shape[1], num_heads, -1).transpose(1, 2) for x in (xq, xk, xv)]
+
+  if past is not None:
+    xk, xv = Tensor.cat(past[0], xk, dim=-2), Tensor.cat(past[1], xv, dim=-2)
+    present = Tensor.cat(xk.unsqueeze(0), xv.unsqueeze(0))
+
+  def attn(query, key, value, attn_mask):
+    query_length, key_length = query.shape[-2], key.shape[-2]
+    cdim = max(query_length, key_length) + 1
+    attn_weights = query @ key.transpose(-1, -2) / math.sqrt(value.shape[-1])
+    # This is where Tensor.scaled_dot_product_attention differs:
+    causal_mask = Tensor.ones((cdim, cdim), requires_grad=False).cast(dtypes.bool).tril(0)[key_length - query_length : key_length, :key_length].cast(dtypes.bool)
+    return (Tensor.where(causal_mask, attn_weights, -float("inf")) + attn_mask).softmax(-1) @ value
+
+  bsz, _, seq_len, _ = xq.shape
+  out = attn(xq, xk, xv, mask_index).transpose(1, 2).reshape(bsz, seq_len, -1)
+  return out, present
+
+# **************** ai.onnx.preview.training Ops ****************
+
+# TODO not entirely sure these optimizers are correct
+def Adagrad(R, T, *inputs, decay_factor=0.0, epsilon=0.0, norm_coefficient=0.0):
+  groups = len(inputs) // 3
+  grouped_inputs = [inputs[i::groups] for i in range(groups)]
+  T, R = safe_numpy(T)[0], safe_numpy(R)[0]
+  r = R / (1 + T * decay_factor)
+  ret = []
+  for input in grouped_inputs:
+    X, G, H = input
+    X.grad = norm_coefficient * X + G
+    X.grad.requires_grad, H.requires_grad = False, False # TODO manually turning off requires_grad, see TODO under (domain == "ai.onnx.preview.training") in onnx.py
+    H.assign(H.detach() + X.grad * X.grad).realize()
+    H_adaptive = H.sqrt() + epsilon
+    X.assign(X.detach() - r * X.grad / H_adaptive)
+    ret.extend([X, H])
+  ret = ret[::2] + ret[1::2]
+  return tuple(ret)
+
+def Momentum(R, T, *inputs, alpha, beta, mode, norm_coefficient):
+  groups = len(inputs) // 3
+  grouped_inputs = [inputs[i::groups] for i in range(groups)]
+  T, R = safe_numpy(T)[0], safe_numpy(R)[0]
+  beta_adjusted = beta if T > 0 else 1
+  ret = []
+  for input in grouped_inputs:
+    X, G, V = input
+    X.grad = (norm_coefficient * X + G).realize()
+    X.grad.requires_grad, V.requires_grad = False, False
+    V.assign(alpha * V + beta_adjusted * X.grad).realize()
+    if mode == "standard": X.assign(X.detach() - R * V).realize()
+    elif mode == "nesterov": X.assign(X.detach() - R * (X.grad + alpha + V)).realize()
+    ret.extend([X, V])
+  ret = ret[::2] + ret[1::2]
+  return tuple(ret)
+
+# copied from tinygrad/nn/optim.py: LAMB with some edits
+def Adam(R, T, *inputs, alpha=0.9, beta=0.999, epsilon=0.0, norm_coefficient=0.0, norm_coefficient_post=0.0):
+  groups = len(inputs) // 4
+  grouped_inputs = [inputs[i::groups] for i in range(groups)]
+  T, R = safe_numpy(T)[0], safe_numpy(R)[0]
+  ret = []
+  for input in grouped_inputs:
+    X, G, V, H = input
+    X.grad = (norm_coefficient * X + G).realize()
+    V.requires_grad, H.requires_grad, X.grad.requires_grad = False, False, False
+    V.assign(alpha * V + (1.0 - alpha) * X.grad).realize()
+    H.assign(beta * H + (1.0 - beta) * (X.grad * X.grad)).realize()
+    up = (V / (1.0 - alpha**T)) / ((H / (1.0 - beta**T)).sqrt() + epsilon) if T > 0 else V / (H.sqrt() + epsilon)
+    X.assign(X.detach() - R * up).realize()
+    X = (1 - norm_coefficient_post) * X
+    ret.extend([X, V, H])
+  ret = ret[::3] + ret[1::3] + ret[2::3]
+  return tuple(ret)
diff --git a/tinygrad_repo/extra/thneed.py b/tinygrad_repo/extra/thneed.py
index b9b896ca69..a202e796cf 100644
--- a/tinygrad_repo/extra/thneed.py
+++ b/tinygrad_repo/extra/thneed.py
@@ -1,11 +1,11 @@
-# this can be constructed from a cl_cache or loaded from a thneed file 
+# this can be constructed from a cl_cache or loaded from a thneed file
 import time
 import struct
 import json
 import traceback
 import numpy as np
-from tinygrad.runtime.ops_gpu import CLProgram
-from tinygrad.helpers import prod, getenv
+from tinygrad.runtime.ops_gpu import CLProgram, compile_gpu
+from tinygrad.helpers import DEBUG, getenv
 from collections import defaultdict
 import pyopencl as cl
 from tinygrad.runtime.ops_gpu import CL, OSX_TIMING_RATIO
@@ -26,7 +26,7 @@ class Thneed:
       for a in args[3:]:
         nodes[a]['out_edges'].append(args[2])
         nodes[args[2]]['in_edges'].append(a)
-    
+
     # get buffers to save
     self.buffers_to_save = set()
     self.outputs = []
@@ -35,7 +35,7 @@ class Thneed:
         self.buffers_to_save.add(n)
       if len(nodes[n]['out_edges']) == 0:
         self.outputs.append(n)
-  
+
     fake_inputs = []
     for k,n in self.inputs.items():
       if n in self.buffers_to_save:
@@ -74,53 +74,43 @@ class Thneed:
         if o['arg_type'] == "image2d_t":
           if 'buffer_id' in o and o['height'] == 1 and not bufs_loaded[o['buffer_id']]:
             # hack: use a image1d since we can back that with a buffer
-            buf = cl.Image(CL.cl_ctx, mf.READ_WRITE, tfmt, shape=(o['width'],), buffer=bufs[o['buffer_id']])
+            buf = cl.Image(CL.cl_ctxs[0], mf.READ_WRITE, tfmt, shape=(o['width'],), buffer=bufs[o['buffer_id']])
           else:
             # buffer isn't supported in image2d, copy buffer into image
             if 'buffer_id' in o and bufs_loaded[o['buffer_id']]:
               arr = np.zeros(bufs[o['buffer_id']].size // 2, dtype=np.float16)
-              cl.enqueue_copy(q, arr, bufs[o['buffer_id']])
-              buf = cl.Image(CL.cl_ctx, mf.READ_WRITE | mf.COPY_HOST_PTR, tfmt,
+              cl.enqueue_copy(CL.cl_queue[0], arr, bufs[o['buffer_id']])
+              buf = cl.Image(CL.cl_ctxs[0], mf.READ_WRITE | mf.COPY_HOST_PTR, tfmt,
                 shape=(o['width'], o['height']), pitches=(o['row_pitch'],), hostbuf=arr)
             elif o['needs_load']:
-              buf = cl.Image(CL.cl_ctx, mf.READ_WRITE | mf.COPY_HOST_PTR, tfmt,
+              buf = cl.Image(CL.cl_ctxs[0], mf.READ_WRITE | mf.COPY_HOST_PTR, tfmt,
                 shape=(o['width'], o['height']), pitches=(o['row_pitch'],), hostbuf=o['data'])
             else:
-              buf = cl.Image(CL.cl_ctx, mf.READ_WRITE, tfmt, shape=(o['width'], o['height']))
+              buf = cl.Image(CL.cl_ctxs[0], mf.READ_WRITE, tfmt, shape=(o['width'], o['height']))
         if o['arg_type'] == "image1d_t":
           assert not o['needs_load']
           assert not bufs_loaded[o['buffer_id']]
-          buf = cl.Image(CL.cl_ctx, mf.READ_WRITE, tfmt, shape=(o['width'],), buffer=bufs[o['buffer_id']])
+          buf = cl.Image(CL.cl_ctxs[0], mf.READ_WRITE, tfmt, shape=(o['width'],), buffer=bufs[o['buffer_id']])
       else:
         if 'data' in o:
-          buf = cl.Buffer(CL.cl_ctx, mf.READ_WRITE | mf.COPY_HOST_PTR, hostbuf=o['data'])
+          buf = cl.Buffer(CL.cl_ctxs[0], mf.READ_WRITE | mf.COPY_HOST_PTR, hostbuf=o['data'])
         else:
           # zero out buffers
-          buf = cl.Buffer(CL.cl_ctx, mf.READ_WRITE | mf.COPY_HOST_PTR, hostbuf=b'\x00'*o['size'])
-        
+          buf = cl.Buffer(CL.cl_ctxs[0], mf.READ_WRITE | mf.COPY_HOST_PTR, hostbuf=b'\x00'*o['size'])
+
       bufs[o['id']] = buf
       bufs_loaded[o['id']] = 'data' in o
       # if it's loaded, it's saved
       if 'data' in o:
         self.buffers_to_save.add(buf)
 
-    # load in the programs (this isn't used)
-    prgs = {}
-    for k,v in jdat['programs'].items():
-      print("building", k)
-      try:
-        prgs[k] = CLProgram(k, v, rename=False)
-      except Exception:
-        print("FAILED", k)
-        traceback.print_exc()
-        exit(0)
-    
     # load binaries
+    prgs = {}
     for o in jdat['binaries']:
       nptr = ptr + o['length']
-      prgs[o['name']] = CLProgram(o['name'], weights[ptr:nptr], binary=True)
+      prgs[o['name']] = CLProgram(o['name'], weights[ptr:nptr])
       ptr = nptr
-  
+
     # populate the cl_cache
     for i,k in enumerate(jdat['kernels']):
       kernel = prgs[k['name']]
@@ -140,6 +130,8 @@ class Thneed:
         aaa.append(aa)
       self.cl_cache.append((kernel, [k['global_work_size'], k['local_work_size'], *aaa]))
 
+    if DEBUG >= 1: print(f"thneed: total bufs loaded: {len(bufs.keys())}")
+
     # load inputs
     for k in jdat['inputs']:
       self.inputs[k['name']] = bufs[k['buffer_id']]
@@ -161,12 +153,12 @@ class Thneed:
     for prg, args in self.cl_cache:
       # get binaries for saving
       if prg.name not in saved_binaries:
-        binary = prg.clprogram.get_info(cl.program_info.BINARIES)
+        binary = prg.clprograms[0].get_info(cl.program_info.BINARIES)
         assert len(binary) == 1
         jdat['binaries'].append({"name":prg.name, "length":len(binary[0])})
         binaries.append(binary[0])
         saved_binaries.add(prg.name)
-      
+
       # get the args from the kernel, some need the data saved
       targs, args_size = [], []
       argdtypes = prg.argdtypes if prg.argdtypes is not None else [None]*(len(args)-2)
@@ -193,19 +185,20 @@ class Thneed:
               })
               if needs_load:
                 data = np.empty(a.size//4, dtype=np.float32)
-                cl.enqueue_copy(CL.cl_queue, data, a, is_blocking=True)
+                cl.enqueue_copy(CL.cl_queue[0], data, a, is_blocking=True)
                 weights.append(data.tobytes())
             elif isinstance(a, cl.Image):
+              assert a.format == cl.ImageFormat(cl.channel_order.RGBA, cl.channel_type.HALF_FLOAT if FLOAT16 else cl.channel_type.FLOAT), "wrong type"
               needs_load = a in self.buffers_to_save
               row_pitch = (a.shape[0]*4*(2 if FLOAT16 else 4) + 63)//64 * 64
               size = row_pitch * a.shape[1]
               # this is *2 if float16 and *4 if float32
-              buf = cl.Buffer(CL.cl_ctx, cl.mem_flags.READ_WRITE, size=size * (2 if FLOAT16 else 1))
+              buf = cl.Buffer(CL.cl_ctxs[0], cl.mem_flags.READ_WRITE, size=size * (2 if FLOAT16 else 1))
 
               # zero out the buffer
-              cl.enqueue_copy(CL.cl_queue, buf, b'\x00'*buf.size, is_blocking=True)
+              cl.enqueue_copy(CL.cl_queue[0], buf, b'\x00'*buf.size, is_blocking=True)
 
-              CLProgram("from_image_strided", """
+              CLProgram("from_image_strided", compile_gpu("""
                 __kernel void from_image_strided(read_only image2d_t in, __global float4 *out, int row_pitch) {
                   const sampler_t smp = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST;
                   int2 l;
@@ -213,7 +206,7 @@ class Thneed:
                   l.x = get_global_id(0);
                   out[l.y*row_pitch + l.x] = read_imagef(in, smp, l);
                 }
-              """, argdtypes=(None, None, np.int32))(a.shape, None, a, buf, row_pitch//(4*(2 if FLOAT16 else 4)))
+              """), argdtypes=(None, None, np.int32))(a, buf, row_pitch//(4*(2 if FLOAT16 else 4)), global_size=a.shape)
 
               # multiple of 32 isn't enough
               jdat['objects'].append({
@@ -223,7 +216,7 @@ class Thneed:
 
               if needs_load:
                 data = np.empty(size//(2 if FLOAT16 else 4), dtype=np.float32)
-                cl.enqueue_copy(CL.cl_queue, data, buf, is_blocking=True)
+                cl.enqueue_copy(CL.cl_queue[0], data, buf, is_blocking=True)
                 if FLOAT16: data = data.astype(np.float16)
                 weights.append(data.tobytes())
             else:
@@ -244,7 +237,7 @@ class Thneed:
         "local_work_size": [1 for _ in args[0]] if args[1] is None else args[1],
         "num_args": len(args)-2,
         "args": targs,
-        "args_size": args_size 
+        "args_size": args_size
       })
 
     jdat['outputs'] = [{
@@ -270,20 +263,20 @@ class Thneed:
     events = []
     st = time.monotonic()
     for prg, args in self.cl_cache:
-      events.append(prg.clprg(CL.cl_queue, *args))
+      events.append(prg.clprgs[0](CL.cl_queue[0], *args))
     mt = time.monotonic()
-    CL.cl_queue.finish()
+    CL.synchronize()
     et = time.monotonic() - st
     print(f"submit in {(mt-st)*1000.0:.2f} ms, total runtime is {et*1000.0:.2f} ms")
 
     if DEBUGCL >= 2:
       for i, ((prg, args), e) in enumerate(zip(self.cl_cache, events)):
-        print(f"{i:3d} {prg.name:20s} " + "queued @ %5.2f ms, submit @ %5.2fms, start @ %5.2f ms, end @ %5.2f ms" % tuple((x*OSX_TIMING_RATIO - st*1e9)/1e6 for x in [e.profile.queued, e.profile.submit, e.profile.start, e.profile.end]))
+        print(f"{i:3d} {prg.name:25s} " + "queued @ %5.2f ms, submit @ %5.2fms, start @ %5.2f ms, end @ %5.2f ms" % tuple((x*OSX_TIMING_RATIO - st*1e9)/1e6 for x in [e.profile.queued, e.profile.submit, e.profile.start, e.profile.end]))
     if DEBUGCL >= 1:
       total_runtime = 0
       for i, ((prg, args), e) in enumerate(zip(self.cl_cache, events)):
         runtime = (e.profile.end - e.profile.start) * OSX_TIMING_RATIO
-        print(f"{i:3d} time {total_runtime/1e6:5.2f} ms running {prg.name:20s} with {str(args[0]):15s} {str(args[1]):15s} count {len(args)-2:2d} runtime {runtime/1e3:7.2f} us {(getattr(prg, 'op_estimate', float('nan')))/runtime:9.2f} GFLOPS -> {args[2].shape if hasattr(args[2], 'shape') else args[2].size}")
+        print(f"{i:3d} time {total_runtime/1e6:5.2f} ms running {prg.name:25s} with {str(args[0]):15s} {str(args[1]):15s} count {len(args)-2:2d} runtime {runtime/1e3:7.2f} us {(getattr(prg, 'op_estimate', float('nan')))/runtime:9.2f} GFLOPS -> {args[2].shape if hasattr(args[2], 'shape') else args[2].size}")
         if hasattr(prg, 'prg') and ((DEBUGCL >= 2 and getenv("PRINT_KERNEL", -1) == i) or DEBUGCL >= 3):
           print(prg.prg)
         total_runtime += runtime
diff --git a/tinygrad_repo/extra/utils.py b/tinygrad_repo/extra/utils.py
index dc124f3445..4c9ace2d15 100644
--- a/tinygrad_repo/extra/utils.py
+++ b/tinygrad_repo/extra/utils.py
@@ -1,70 +1,88 @@
-import pickle
+# type: ignore
+import pickle, hashlib, zipfile, io, requests, struct, tempfile, platform, concurrent.futures
 import numpy as np
 from tqdm import tqdm
-import tempfile
-from tinygrad.helpers import prod, getenv
+from pathlib import Path
+from collections import defaultdict
+from typing import Union
+
+from tinygrad.helpers import prod, getenv, DEBUG, dtypes
+from tinygrad.helpers import GlobalCounters
+from tinygrad.tensor import Tensor
+from tinygrad.lazy import LazyBuffer
+from tinygrad.ops import Device
+from tinygrad.shape.view import strides_for_shape
+OSX = platform.system() == "Darwin"
+WINDOWS = platform.system() == "Windows"
+
+def temp(x:str) -> str: return (Path(tempfile.gettempdir()) / x).as_posix()
 
 def fetch(url):
-  if url.startswith("/"):
+  if url.startswith("/") or url.startswith("."):
     with open(url, "rb") as f:
       return f.read()
-  import os, hashlib, tempfile
-  fp = os.path.join(tempfile.gettempdir(), hashlib.md5(url.encode('utf-8')).hexdigest())
+  fp = temp(hashlib.md5(url.encode('utf-8')).hexdigest())
   download_file(url, fp, skip_if_exists=not getenv("NOCACHE"))
   with open(fp, "rb") as f:
     return f.read()
 
-def download_file(url, fp, skip_if_exists=False):
-  import requests, os
-  if skip_if_exists and os.path.isfile(fp) and os.stat(fp).st_size > 0:
+def fetch_as_file(url):
+  if url.startswith("/") or url.startswith("."):
+    with open(url, "rb") as f:
+      return f.read()
+  fp = temp(hashlib.md5(url.encode('utf-8')).hexdigest())
+  download_file(url, fp, skip_if_exists=not getenv("NOCACHE"))
+  return fp
+
+def download_file(url, fp, skip_if_exists=True):
+  if skip_if_exists and Path(fp).is_file() and Path(fp).stat().st_size > 0:
     return
   r = requests.get(url, stream=True)
   assert r.status_code == 200
   progress_bar = tqdm(total=int(r.headers.get('content-length', 0)), unit='B', unit_scale=True, desc=url)
-  with tempfile.NamedTemporaryFile(delete=False) as f:
+  (path := Path(fp).parent).mkdir(parents=True, exist_ok=True)
+  with tempfile.NamedTemporaryFile(dir=path, delete=False) as f:
     for chunk in r.iter_content(chunk_size=16384):
       progress_bar.update(f.write(chunk))
     f.close()
-    os.rename(f.name, fp)
+    Path(f.name).rename(fp)
 
 def my_unpickle(fb0):
-  key_prelookup = {}
-  class HackTensor:
-    def __new__(cls, *args):
-      #print(args)
-      ident, storage_type, obj_key, location, obj_size = args[0][0:5]
-      assert ident == 'storage'
-
-      assert prod(args[2]) == obj_size
-      ret = np.zeros(args[2], dtype=storage_type)
-      key_prelookup[obj_key] = (storage_type, obj_size, ret, args[2], args[3])
-      return ret
-
-  class HackParameter:
-    def __new__(cls, *args):
-      #print(args)
-      pass
+  key_prelookup = defaultdict(list)
+  def _rebuild_tensor_v2(storage, storage_offset, size, stride, requires_grad, backward_hooks, metadata=None):
+    #print(storage, storage_offset, size, stride, requires_grad, backward_hooks, metadata)
+    ident, storage_type, obj_key, location, obj_size = storage[0:5]
+    assert ident == 'storage'
+    assert prod(size) <= (obj_size - storage_offset)
+
+    if storage_type not in [np.float16, np.float32]:
+      if DEBUG: print(f"unsupported type {storage_type} on {obj_key} with shape {size}")
+      ret = None
+    else:
+      ret = Tensor.empty(*size, dtype=dtypes.from_np(storage_type))
+    key_prelookup[obj_key].append((storage_type, obj_size, ret, size, stride, storage_offset))
+    return ret
 
-  class Dummy:
+  def _rebuild_parameter(*args):
+    #print(args)
     pass
 
+  class Dummy: pass
+
   class MyPickle(pickle.Unpickler):
     def find_class(self, module, name):
       #print(module, name)
-      if name == 'FloatStorage':
-        return np.float32
-      if name == 'LongStorage':
-        return np.int64
-      if name == 'HalfStorage':
-        return np.float16
+      if name == 'FloatStorage': return np.float32
+      if name == 'LongStorage': return np.int64
+      if name == 'IntStorage': return np.int32
+      if name == 'HalfStorage': return np.float16
       if module == "torch._utils":
-        if name == "_rebuild_tensor_v2":
-          return HackTensor
-        elif name == "_rebuild_parameter":
-          return HackParameter
+        if name == "_rebuild_tensor_v2": return _rebuild_tensor_v2
+        if name == "_rebuild_parameter": return _rebuild_parameter
       else:
+        if module.startswith('pytorch_lightning'): return Dummy
         try:
-          return pickle.Unpickler.find_class(self, module, name)
+          return super().find_class(module, name)
         except Exception:
           return Dummy
 
@@ -73,23 +91,78 @@ def my_unpickle(fb0):
 
   return MyPickle(fb0).load(), key_prelookup
 
-def fake_torch_load_zipped(fb0, load_weights=True):
-  import zipfile
+def load_single_weight(t:Tensor, myfile, shape, strides, dtype, storage_offset, mmap_allowed=False):
+  bytes_size = np.dtype(dtype).itemsize
+  if t is None:
+    myfile.seek(prod(shape) * bytes_size, 1)
+    return
+
+  bytes_offset = 0
+  if storage_offset is not None:
+    bytes_offset = storage_offset * bytes_size
+    myfile.seek(bytes_offset)
+
+  assert t.shape == shape or shape == tuple(), f"shape mismatch {t.shape} != {shape}"
+  assert t.dtype.np == dtype and t.dtype.itemsize == bytes_size
+  if any(s != 1 and st1 != st2 for s, st1, st2 in zip(shape, strides_for_shape(shape), strides)):
+    # slow path
+    buffer_size = sum(strides[i]*t.dtype.itemsize * (shape[i] - 1) for i in range(len(shape)))
+    buffer_size += t.dtype.itemsize
+    np_array = np.frombuffer(myfile.read(buffer_size), t.dtype.np)
+
+    np_array = np.lib.stride_tricks.as_strided(
+      np_array, shape=shape, strides=[i*t.dtype.itemsize for i in strides])
+
+    lna = t.lazydata.op.arg
+    lna.fxn = lambda _: np_array
+    t.realize()
+    return
+
+  # ["METAL", "CLANG", "LLVM"] support readinto for more speed
+  # ["GPU", "CUDA"] use _mmap since they have to copy in to the GPU anyway
+  # this needs real APIs
+  if t.device in ["METAL", "CLANG", "LLVM"]:
+    del t.lazydata.op
+    t.lazydata.realized = Device[t.lazydata.device].buffer(prod(t.shape), dtype=t.dtype)
+    myfile.readinto(t.lazydata.realized._buffer())
+  else:
+    def _mmap(lna):
+      assert myfile._compress_type == 0, "compressed data can't be mmaped"
+      return np.memmap(myfile._fileobj._file, dtype=lna.dtype, mode='r', offset=myfile._orig_compress_start + bytes_offset, shape=lna.shape)
+    def _read(lna):
+      ret = np.empty(lna.shape, dtype=lna.dtype)
+      myfile.readinto(ret.data)
+      return ret
+    if mmap_allowed and not OSX and t.device in ["GPU", "CUDA"]: t.lazydata.op.arg.fxn = _mmap
+    else: t.lazydata.op.arg.fxn = _read
+    t.realize()
+
+def fake_torch_load_zipped(fb0, load_weights=True, multithreaded=True):
+  if Device.DEFAULT in ["TORCH", "GPU", "CUDA"]: multithreaded = False  # multithreaded doesn't work with CUDA or TORCH. for GPU it's a wash with _mmap
   with zipfile.ZipFile(fb0, 'r') as myzip:
-    with myzip.open('archive/data.pkl') as myfile:
+    base_name = myzip.namelist()[0].split('/', 1)[0]
+    with myzip.open(f'{base_name}/data.pkl') as myfile:
       ret = my_unpickle(myfile)
     if load_weights:
-      for k,v in ret[1].items():
-        with myzip.open(f'archive/data/{k}') as myfile:
-          if v[2].dtype == "object":
-            print(f"issue assigning object on {k}")
-            continue
-          np.copyto(v[2], np.frombuffer(myfile.read(), v[2].dtype).reshape(v[3]))
+      def load_weight(k, vv):
+        with myzip.open(f'{base_name}/data/{k}') as myfile:
+          for v in vv:
+            load_single_weight(v[2], myfile, v[3], v[4], v[0], v[5], mmap_allowed=True)
+      if multithreaded:
+        # 2 seems fastest
+        with concurrent.futures.ThreadPoolExecutor(max_workers=2) as executor:
+          futures = {executor.submit(load_weight, k, v):k for k,v in ret[1].items()}
+          for future in (t:=tqdm(concurrent.futures.as_completed(futures), total=len(futures))):
+            if future.exception() is not None: raise future.exception()
+            k = futures[future]
+            t.set_description(f"loading {k} ram used: {GlobalCounters.mem_used/1e9:5.2f} GB")
+      else:
+        for k,v in (t := tqdm(ret[1].items())):
+          t.set_description(f"loading {k} ram used: {GlobalCounters.mem_used/1e9:5.2f} GB")
+          load_weight(k,v)
   return ret[0]
 
 def fake_torch_load(b0):
-  import io
-  import struct
 
   # convert it to a file
   fb0 = io.BytesIO(b0)
@@ -108,19 +181,15 @@ def fake_torch_load(b0):
   key_lookup = pickle.load(fb0)
   key_real = [None] * len(key_lookup)
   for k,v in key_prelookup.items():
-    key_real[key_lookup.index(k)] = v
+    assert len(v) == 1
+    key_real[key_lookup.index(k)] = v[0]
 
   # read in the actual data
-  for storage_type, obj_size, np_array, np_shape, np_strides in key_real:
+  for storage_type, obj_size, tensor, np_shape, np_strides, storage_offset in key_real:
     ll = struct.unpack("Q", fb0.read(8))[0]
-    assert ll == obj_size
-    bytes_size = {np.float32: 4, np.int64: 8}[storage_type]
-    mydat = fb0.read(ll * bytes_size)
-    np.copyto(np_array, np.frombuffer(mydat, storage_type).reshape(np_shape))
-
-    # numpy stores its strides in bytes
-    real_strides = tuple([x*bytes_size for x in np_strides])
-    np_array.strides = real_strides
+    assert ll == obj_size, f"size mismatch {ll} != {obj_size}"
+    assert storage_offset == 0, "not implemented"
+    load_single_weight(tensor, fb0, np_shape, np_strides, storage_type, None)
 
   return ret
 
diff --git a/tinygrad_repo/openpilot/compile.py b/tinygrad_repo/openpilot/compile.py
deleted file mode 100644
index e51f994579..0000000000
--- a/tinygrad_repo/openpilot/compile.py
+++ /dev/null
@@ -1,148 +0,0 @@
-#!/usr/bin/env python3
-import os, time, io, pathlib, sys, traceback
-sys.path.insert(0, str(pathlib.Path(__file__).parent.parent))
-
-if os.getenv("OPT", None) is None:
-  os.environ['OPT'] = '99'
-if os.getenv("GPU", None) is None:
-  os.environ['GPU'] = '1'
-if os.getenv("IMAGE", None) is None:
-  os.environ['IMAGE'] = '2'
-
-from tinygrad.helpers import getenv
-ALLOWED_KERNEL_COUNT = getenv("ALLOWED_KERNEL_COUNT", 0)
-DEBUGCL = getenv("DEBUGCL", 0)
-
-import onnx
-import numpy as np
-
-import tinygrad.graph as graph
-from tinygrad.ops import GlobalCounters
-
-import pyopencl as cl
-from tinygrad.runtime.ops_gpu import CL
-from extra.utils import fetch
-from extra.onnx import get_run_onnx
-from tinygrad.tensor import Tensor
-
-OPENPILOT_MODEL = "https://github.com/commaai/openpilot/raw/6c5693e965b9c63f8678f52b9e9b5abe35f23feb/selfdrive/modeld/models/supercombo.onnx"
-
-np.random.seed(1337)
-def get_random_input_tensors(input_shapes):
-  # this 16 is a random scale factor
-  inputs = {k:Tensor.randn(*shp, requires_grad=False)*8 for k,shp in input_shapes.items()}
-  np_inputs = {k:v.realize().numpy() for k,v in inputs.items()}
-  return inputs, np_inputs
-
-from tinygrad.jit import TinyJit
-
-@TinyJit
-def model_exec(run_onnx, using_graph, **inputs):
-  ret = next(iter(run_onnx(inputs).values()))
-  GlobalCounters.reset()
-  GlobalCounters.cache = []  # don't cache pre-realize
-  if using_graph: graph.GRAPH = True
-  print("realizing")
-  return ret.realize()
-
-def compile(dat, output_fn):
-  Tensor.manual_seed(1337)
-  Tensor.no_grad = True
-  using_graph = graph.GRAPH
-  graph.GRAPH = False
-
-  onnx_model = onnx.load(io.BytesIO(dat))
-  run_onnx = get_run_onnx(onnx_model)
-  input_shapes = {inp.name:tuple(x.dim_value for x in inp.type.tensor_type.shape.dim) for inp in onnx_model.graph.input}
-
-  inputs, np_inputs = get_random_input_tensors(input_shapes)
-  # run twice to trigger the JIT
-  for i in range(2): tinygrad_out = model_exec(run_onnx, i == 1 and using_graph, **inputs)
-  graph.GRAPH = False
-  print("kernel count:", len(model_exec.jit_cache))
-  assert len(model_exec.jit_cache) <= ALLOWED_KERNEL_COUNT or ALLOWED_KERNEL_COUNT == 0, "too many kernels!"
-
-  # pull out inputs and put them in the jit cache
-  input_rawbuffers = {k:inputs[k].lazydata.realized.raw() for k in inputs.keys()}
-  for (j,i),idx in model_exec.input_replace.items(): model_exec.jit_cache[j][1][i] = input_rawbuffers[idx]
-
-  # transform to CL.CACHE
-  used_ops = 0
-  cl_cache = []
-  for prg,args in model_exec.jit_cache:
-    # pass these to thneed
-    setattr(prg.clprg, 'op_estimate', prg.op_estimate)
-    setattr(prg.clprg, 'prg', prg.prg)
-    cl_cache.append((prg.clprg, [prg.global_size, prg.local_size, *[x._cl for x in args]]))
-    used_ops += prg.op_estimate
-
-  from extra.thneed import Thneed
-  t = Thneed(cl_cache, {k:v._cl for k,v in input_rawbuffers.items()})
-
-  # save thneed (before run)
-  t.save(output_fn)
-
-  print(f"buffers to save: {len(t.buffers_to_save)}, inputs: {list(t.inputs.keys())}, outputs: {t.outputs}")
-  runtime = t.run()
-  print(f"network using {used_ops/1e9:.2f} GOPS with runtime {runtime*1e3:.2f} ms that's {used_ops/runtime*1e-9:.2f} GFLOPS")
-
-  # confirm thneed found the right output
-  thneed_out = np.empty((t.outputs[0].size//4,), dtype=np.float32).reshape(tinygrad_out.shape)
-  cl.enqueue_copy(CL.cl_queue, thneed_out, t.outputs[0], is_blocking=True)
-  np.testing.assert_allclose(thneed_out, tinygrad_out.numpy())
-
-  # testing is float32 only (fix this)
-  FLOAT16 = getenv("FLOAT16", 0)
-  if FLOAT16 == 0:
-    try:
-      from test.models.test_onnx import run_onnx_torch
-      torch_out = run_onnx_torch(onnx_model, np_inputs).numpy()
-      print(thneed_out, torch_out, "mse", np.sum((thneed_out-torch_out)**2), "max err", np.max(np.abs((thneed_out-torch_out))))
-      np.testing.assert_allclose(torch_out, thneed_out, atol=1e-4, rtol=1e-2)
-
-      # test loading/run thneed
-      _, new_np_inputs = get_random_input_tensors(input_shapes)
-      new_torch_out = run_onnx_torch(onnx_model, new_np_inputs).numpy()
-
-      # try old thneed with a different input
-      for k,v in t.inputs.items():
-        cl.enqueue_copy(CL.cl_queue, v, new_np_inputs[k], is_blocking=True)
-
-      t.run()
-      old_thneed_out = np.empty((t.outputs[0].size//4,), dtype=np.float32).reshape(tinygrad_out.shape)
-      cl.enqueue_copy(CL.cl_queue, old_thneed_out, t.outputs[0], is_blocking=True)
-
-      # compare thneed (rerun) with torch
-      np.testing.assert_allclose(new_torch_out, old_thneed_out, atol=1e-4, rtol=1e-2)
-
-      # load thneed and try that
-      _, new_np_inputs = get_random_input_tensors(input_shapes)
-      new_torch_out = run_onnx_torch(onnx_model, new_np_inputs).numpy()
-      nt = Thneed()
-      nt.load(output_fn)
-
-      # inputs
-      for k,v in nt.inputs.items():
-        cl.enqueue_copy(CL.cl_queue, v, new_np_inputs[k], is_blocking=True)
-
-      nt.run()
-      new_thneed_out = np.empty((nt.outputs[0].size//4,), dtype=np.float32).reshape(tinygrad_out.shape)
-      cl.enqueue_copy(CL.cl_queue, new_thneed_out, nt.outputs[0], is_blocking=True)
-
-      # compare torch to thneed
-      np.testing.assert_allclose(new_torch_out, new_thneed_out, atol=1e-4, rtol=1e-2)
-      print("thneed self-test passed!")
-    except ModuleNotFoundError as e:
-      print(f"TEST NOT HAPPENING {e}")
-
-
-# UNSAFE_FLOAT4=1 DEBUGCL=1 FLOAT16=1 python3 openpilot/compile.py
-# 22.59 ms
-if __name__ == "__main__":
-  if len(sys.argv) >= 3:
-    with open(sys.argv[1], "rb") as f:
-      dat = f.read()
-    compile(dat, sys.argv[2])
-  else:
-    dat = fetch(OPENPILOT_MODEL)
-    compile(dat, "/tmp/output.thneed")
diff --git a/tinygrad_repo/openpilot/compile2.py b/tinygrad_repo/openpilot/compile2.py
new file mode 100644
index 0000000000..1ee84f91eb
--- /dev/null
+++ b/tinygrad_repo/openpilot/compile2.py
@@ -0,0 +1,166 @@
+#!/usr/bin/env python3
+import os, sys, io, pathlib
+sys.path.insert(0, str(pathlib.Path(__file__).parents[1]))
+
+if "FLOAT16" not in os.environ: os.environ["FLOAT16"] = "1"
+if "IMAGE" not in os.environ: os.environ["IMAGE"] = "2"
+if "NOLOCALS" not in os.environ: os.environ["NOLOCALS"] = "1"
+if "OPT" not in os.environ: os.environ["OPT"] = "99"
+os.environ["PREREALIZE"] = "0"
+
+OPENPILOT_MODEL = "https://github.com/commaai/openpilot/raw/v0.9.4/selfdrive/modeld/models/supercombo.onnx"
+
+import onnx
+from typing import Tuple, List
+from extra.utils import fetch
+from extra.onnx import get_run_onnx
+from tinygrad.graph import print_tree, log_schedule_item
+from tinygrad.tensor import Tensor
+from tinygrad.helpers import dtypes, partition, GlobalCounters, Context, DEBUG, getenv, ImageDType, GRAPH
+from tinygrad.realize import run_schedule
+from tinygrad.ops import LoadOps, Device, ScheduleItem
+from tinygrad.features.image import fix_schedule_for_images
+Device.DEFAULT = "GPU"
+
+def get_schedule(onnx_data) -> Tuple[List[ScheduleItem], List[ScheduleItem]]:
+  Tensor.no_grad = True
+  Tensor.training = False
+
+  # load the model
+  onnx_model = onnx.load(io.BytesIO(onnx_data))
+  run_onnx = get_run_onnx(onnx_model)
+  input_shapes = {inp.name:tuple(x.dim_value for x in inp.type.tensor_type.shape.dim) for inp in onnx_model.graph.input}
+
+  # run the model
+  inputs = {k:Tensor.empty(*shp) for k,shp in input_shapes.items()}
+  ret: Tensor = next(iter(run_onnx(inputs).values())).cast(dtypes.float32).contiguous()
+  schedule = ret.lazydata.schedule()
+
+  # filter schedule that don't depend on the inputs
+  input_lb = [x.lazydata.base for x in inputs.values()]
+  depends = set(input_lb)
+  for si in schedule:
+    if any(b in depends for b in si.inputs):
+      depends.add(si.out)
+
+  # run all kernels that don't depend on the inputs
+  # NOTE: there's two extra kernels due to fusions that now happen since the weights aren't realized
+  schedule, schedule_independent = partition(schedule, lambda si: si.out in depends)
+  print(f"{len(schedule)} schedule items depend on the input, {len(schedule_independent)} don't")
+
+  # confirm no loadops in the (non independent) schedule except for the ones that load the input buffers
+  assert all(si.ast.op not in LoadOps or si.out in input_lb for si in schedule), "has loadops, can't compile to Thneed"
+  return schedule, schedule_independent, inputs
+
+def schedule_to_thneed(schedule, output_fn):
+  from extra.thneed import Thneed
+
+  # transform to CL.CACHE
+  used_ops = 0
+  cl_cache = []
+  for si in schedule:
+    prg = Device["GPU"].method_cache[si.ast]
+    args = (si.out,) + si.inputs
+
+    # pass these to thneed
+    setattr(prg.clprg, 'op_estimate', prg.op_estimate)
+    setattr(prg.clprg, 'prg', prg.prg)
+
+    global_size = prg.global_size + [1]*(3-len(prg.global_size))
+    local_size = prg.local_size + [1]*(3-len(prg.local_size))
+    cl_cache.append((prg.clprg, [[int(g*l) for g,l in zip(global_size, local_size)], local_size, *[x.realized._buf for x in args]]))
+    used_ops += prg.op_estimate
+
+  from extra.thneed import Thneed
+  input_rawbuffers = {k:inputs[k].lazydata.realized for k in inputs.keys()}
+  t = Thneed(cl_cache, {k:v._buf for k,v in input_rawbuffers.items()})
+
+  # save thneed (before run)
+  t.save(output_fn)
+
+  print(f"buffers to save: {len(t.buffers_to_save)}, inputs: {list(t.inputs.keys())}, outputs: {t.outputs}")
+  runtime = t.run()
+  print(f"network using {used_ops/1e9:.2f} GOPS with runtime {runtime*1e3:.2f} ms that's {used_ops/runtime*1e-9:.2f} GFLOPS")
+
+def thneed_test_onnx(onnx_data, output_fn):
+  import onnx
+  import pyopencl as cl
+  from tinygrad.runtime.ops_gpu import CL
+  import numpy as np
+  from extra.thneed import Thneed
+  onnx_model = onnx.load(io.BytesIO(onnx_data))
+
+  input_shapes = {inp.name:tuple(x.dim_value for x in inp.type.tensor_type.shape.dim) for inp in onnx_model.graph.input}
+  inputs = {k:Tensor.randn(*shp, requires_grad=False)*8 for k,shp in input_shapes.items()}
+  new_np_inputs = {k:v.realize().numpy() for k,v in inputs.items()}
+
+  if getenv("ORT"):
+    # test with onnxruntime
+    import onnxruntime as ort
+    onnx_session = ort.InferenceSession(onnx_data)
+    onnx_output = onnx_session.run([onnx_model.graph.output[0].name], {k:v.astype(np.float16) for k,v in new_np_inputs.items()})
+    new_torch_out = onnx_output[0]
+  else:
+    # test with torch
+    from test.models.test_onnx import run_onnx_torch
+    new_torch_out = run_onnx_torch(onnx_model, new_np_inputs).numpy()
+
+  if output_fn is None:
+    # non thneed
+    run_onnx = get_run_onnx(onnx_model)
+    new_tinygrad_out = next(iter(run_onnx(inputs).values())).cast(dtypes.float32).numpy()
+    np.testing.assert_allclose(new_torch_out, new_tinygrad_out, atol=1e-4, rtol=1e-2)
+    print("classic self-test passed!")
+  else:
+    # load thneed and try that
+    nt = Thneed()
+    nt.load(output_fn)
+
+    # inputs
+    for k,v in nt.inputs.items():
+      cl.enqueue_copy(CL.cl_queue[0], v, new_np_inputs[k], is_blocking=True)
+
+    nt.run()
+    new_thneed_out = np.empty((nt.outputs[0].size//4,), dtype=np.float32).reshape(new_torch_out.shape)
+    cl.enqueue_copy(CL.cl_queue[0], new_thneed_out, nt.outputs[0], is_blocking=True)
+
+    # compare torch to thneed
+    np.testing.assert_allclose(new_torch_out, new_thneed_out, atol=1e-4, rtol=1e-2)
+    print("thneed self-test passed!")
+
+if __name__ == "__main__":
+  onnx_data = fetch(sys.argv[1] if len(sys.argv) > 1 else OPENPILOT_MODEL)
+
+  # quick test for ONNX issues
+  #thneed_test_onnx(onnx_data, None)
+  #exit(0)
+
+  schedule, schedule_independent, inputs = get_schedule(onnx_data)
+  schedule, schedule_input = partition(schedule, lambda x: x.ast.op not in LoadOps)
+  print(f"{len(schedule_input)} inputs")
+
+  run_schedule(schedule_independent, disable_logging=True)
+  run_schedule(schedule_input)
+  with Context(DEBUG=2, BEAM=getenv("LATEBEAM")):
+    schedule = fix_schedule_for_images(schedule)
+    image_count = sum(isinstance(si.out.dtype, ImageDType) for si in schedule)
+    print(f"**** running real kernels {image_count}/{len(schedule)} images ****")
+
+    if GRAPH:
+      for si in schedule_input: log_schedule_item(si)
+      for si in schedule: log_schedule_item(si)
+
+    GlobalCounters.reset()
+    run_schedule(schedule[:])
+
+  output_fn = sys.argv[2] if len(sys.argv) >= 3 else "/tmp/output.thneed"
+  schedule_to_thneed(schedule, output_fn)
+
+  FLOAT16 = getenv("FLOAT16", 0)
+  if FLOAT16 == 0:
+    try:
+      thneed_test_onnx(onnx_data, output_fn)
+    except ModuleNotFoundError as e:
+      print(f"TEST NOT HAPPENING {e}")
+
+
diff --git a/tinygrad_repo/tinygrad/codegen/ast.py b/tinygrad_repo/tinygrad/codegen/ast.py
deleted file mode 100644
index 270bc97915..0000000000
--- a/tinygrad_repo/tinygrad/codegen/ast.py
+++ /dev/null
@@ -1,192 +0,0 @@
-import itertools
-from enum import Enum, auto
-from typing import List, Tuple
-from tinygrad.helpers import prod, dedup, all_same, colored
-from tinygrad.ops import LazyOp, MovementOps, get_lazyop_info, get_buffers, ReduceOps, get_lazyops, map_buffers
-from tinygrad.shape import ShapeTracker, View, strides_for_shape
-
-def get_first_reduce(shapes):
-  for i in range(len(shapes[0])):
-    if not all_same([x[i] for x in shapes]): return i
-  return len(shapes[0])  # off the end
-
-# this will be removed soon anyway
-class Types(Enum): FLOAT = auto(); FLOAT4 = auto() # noqa: E702
-class Token:
-  def __init__(self, tok:str, typ:Types, ptr:bool=False):
-    assert isinstance(tok, str)
-    self.tok, self.typ, self.ptr = tok, typ, ptr
-    self.axis : List[Tuple[int, int, bool]] = []
-  def array(self, length, stride, reduce): self.axis.append((length, stride, reduce))
-  def size(self): return prod([x[0] for x in self.axis])
-  def offsets(self): return [sum(t) for t in itertools.product(*[[y*x[1] for y in range(x[0])] for x in self.axis[::-1]])] if len(self.axis) else [0]
-  def can_float4(self): return any(a[0:2] == (4,1) for a in self.axis)
-  # TODO: this is sort of a hack, it gets the accumulator indices
-  def acc_offsets(self):
-    if len(self.axis) == 0: return [0]
-    acc_strides = [x*(1-self.axis[::-1][i][2]) for i,x in enumerate(strides_for_shape(tuple(1 if r else s for s,_,r in self.axis[::-1])))]
-    return [sum(t) for t in itertools.product(*[[y*acc_strides[i] for y in range(x[0])] for i,x in enumerate(self.axis[::-1])])]
-  def decltype(self): return ('float' if self.typ == Types.FLOAT else 'float4') + ('*' if self.ptr else str())
-  def __repr__(self): return f"<{self.typ}{'*' if self.ptr else str()} {self.tok}{f'[{self.axis}]' if len(self.axis) else str()}>"
-
-# ast kernel can contain one ReduceOp with arbitrary Binary/Unary ops
-class ASTKernel:
-  def __init__(self, ast:LazyOp, output_buffer=None):
-    self.input_ast = ast
-
-    # if the AST ends with a RESHAPE, we remove it and create the buffer accordingly
-    if ast.op == MovementOps.RESHAPE:
-      output_shape = ast.arg
-      ast = ast.src[0]
-    else:
-      output_shape = None
-
-    self.info = get_lazyop_info(ast)
-    self.bufs = dedup(get_buffers(ast))
-    for b in self.bufs: b.st.simplify()
-    self.ast = ast
-
-    # check if the output buffer is allowed to be used
-    # if it's aliased, don't use it
-    if output_buffer is not None:
-      for a in self.bufs:
-        if a._buf == output_buffer._buf and not a.st.contiguous:
-          output_buffer = None
-          break
-
-    # create the buffer we are returning (as the same type as the input buffers) and add it as the first buffer
-    self.ret = output_buffer if output_buffer else type(self.bufs[0])(output_shape if output_shape else self.info.shape, force_create=True)
-    self.bufs = ([type(self.ret)(self.info.shape, hostbuf=self.ret)] if output_shape else [self.ret]) + self.bufs
-
-    # key for lookup in cache (can change, str might not be right)
-    # bufs are needed because kernels like f(x) = x + x and f(x, y) = x + y have the same str(ast), but are different kernels.
-    # mapping the buffers to integers is required because a-b != b-a (and how would you tell a and b apart?)
-    self.key = f"ASTKernelKey ast={str(map_buffers({x:i for i,x in enumerate(self.bufs)}, ast))} bufs={self.bufs}"
-
-  def process(self) -> None:
-    if hasattr(self, "sts"): return   # already processed
-
-    reduceops = [x for x in get_lazyops(self.ast) if x.op in ReduceOps]
-    assert len(dedup(reduceops)) <= 1, "max one reduce op in an ast"
-    self.reduceop = reduceops[0] if reduceops else None
-    self.earlybufs = dedup(get_buffers(self.reduceop)) if self.reduceop else []
-
-    self.buftokens = [Token(f"data{i}", Types.FLOAT, ptr=True) for i in range(len(self.bufs))]
-    self.group_for_reduce : List[int] = []
-
-    # check valid AST kernel
-    assert all_same([x.shape for x in self.earlybufs]), "all earlybufs must have the same shape"
-    assert all_same([x.shape for x in self.bufs if x not in self.earlybufs]), "all latebufs must have the same shape"
-    assert all_same([len(x.shape) for x in self.bufs]), "all bufs must have the same shape size"
-
-    # process
-    self.sts : List[ShapeTracker] = [x.st.copy() for x in self.bufs]   # create new shapetrackers inside this kernel
-    self.simplify_ones()
-    self.simplify_merge_adjacent()
-
-    # get full shape buf index (earlybufs if there are any, otherwise output)
-    self.full_buf_index : int = self.bufs.index(self.earlybufs[0]) if len(self.earlybufs) > 0 else 0
-
-  def print(self):
-    buf_count, op_count, cache = -1, -1, {}
-    def print_ast(x, name=None):
-      nonlocal buf_count, op_count
-      if x not in cache:
-        if not isinstance(x, LazyOp):
-          if name is None:
-            buf_count += 1
-            name = f"buf{buf_count}"
-          print(f"buf{buf_count} = {x}")
-          cache[x] = name
-        else:
-          srcs = [print_ast(y) for y in x.src]
-          if name is None:
-            op_count += 1
-            name = f"op{op_count}"
-          print(f"{name} = LazyOp({str(x.op)}, ({','.join(srcs)},), {x.arg})")
-          cache[x] = name
-      return cache[x]
-    print_ast(self.input_ast, "ast")
-
-  def printbufs(self, prefix="", print_shapetrackers=False):
-    print(f"first_reduce: {self.first_reduce} shape_len: {self.shape_len} group_for_reduce: {self.group_for_reduce}")
-    if print_shapetrackers:
-      for st in self.sts: print(st)
-    for i in range(len(self.sts)):
-      print(prefix, self.buftokens[i], f"early:{'T' if i < len(self.bufs) and self.bufs[i] in self.earlybufs else 'F'}", self.sts[i].shape, self.sts[i].views[-1].strides, len(self.sts[i].views), type(self.bufs[i]._buf) if self.bufs[i] is not None else "FAKE")
-
-  @property
-  def shape_len(self) -> int: return len(self.sts[0].shape)
-
-  @property
-  def full_shape(self) -> Tuple[int, ...]: return self.sts[self.full_buf_index].shape
-
-  @property
-  def upcast_in_mid_reduce_axes(self): return [j for j in range(self.first_reduce, self.first_reduce+len(self.group_for_reduce)) if self.full_shape[j] == self.sts[0].shape[j]]
-
-  def colorshape(self, pad=50) -> str:
-    axis = [(f"{rs:4d}", (("green" if i in self.upcast_in_mid_reduce_axes else "cyan") if i < self.first_reduce + len(self.group_for_reduce) else "red") if i >= self.first_reduce else "blue") for i, rs in enumerate(self.full_shape)]
-    axis += [(f"{s:4d}", 'magenta' if reduce else 'yellow') for s, _, reduce in self.buftokens[self.full_buf_index].axis[::-1]]
-    return ' '.join([colored(*x) for x in axis])+(" "*(pad-len(' '.join([x[0] for x in axis]))))
-
-  def simplify_ones(self):
-    # remove places where the shape is all ones
-    # TODO: this should be factored in to multi shape stride
-    all_ones = [all(st.shape[i]==1 for st in self.sts) for i in range(self.shape_len)]
-    # keep at least 1 one
-    if all(all_ones): all_ones[-1] = False
-    self.reshape_and_permute(lambda shape: [x for i,x in enumerate(shape) if not all_ones[i]], None)
-    # find first mismatch, don't reduce this
-    self.first_reduce = get_first_reduce([x.shape for x in self.sts])
-
-  def simplify_merge_adjacent(self):
-    shapes, strides = [x.shape for x in self.sts], [x.views[-1].strides for x in self.sts]
-
-    # merge dimensions if we can, multi get_shape_strides
-    # TODO: does this always preserve the reduce dimension, NO
-    # TODO: move this into shapetracker, with tests!
-    rets = [[(shapes[j][0], strides[j][0])] for j in range(len(shapes))]
-    for i in range(1, len(shapes[0])):
-      can_merge = []
-      for j in range(len(shapes)):
-        # TODO: added the always mergeability of 1s, is this right? if so, add to shapetracker in the 1 case
-        can_merge.append((strides[j][i] != 0 and rets[j][-1][1] == shapes[j][i]*strides[j][i]) or (strides[j][i] == 0 and rets[j][-1][1] == 0))
-      # more can merge than this
-      mergeable = all(can_merge) and i != self.first_reduce
-      for j in range(len(shapes)):
-        if mergeable: rets[j][-1] = (rets[j][-1][0] * shapes[j][i], strides[j][i])
-        else: rets[j].append((shapes[j][i], strides[j][i]))
-
-    for i,x in enumerate(rets): self.sts[i].reshape(tuple(y[0] for y in x))
-    self.first_reduce = get_first_reduce([x.shape for x in self.sts])
-
-  # this should be aware of the three parts to the shape
-  #  * the input/output dimensions
-  #  * the reduce dimensions
-  #  * the size outputted by each kernel
-  def reshape_and_permute(self, new_shape_fxn, axis):
-    for st in self.sts:
-      if new_shape_fxn is not None: st.reshape(tuple(new_shape_fxn(st.shape)))
-      if axis is not None: st.permute(tuple(axis))
-
-  # axis : the axis to pull from
-  # amount : the amount to take
-  # top : if you want to pull that amount from the top
-  # insert_before : place to insert the new stuff
-  def shift_to(self, axis, amount, top=False, insert_before=None):
-    if insert_before is None: insert_before = self.shape_len
-    move_axis = axis if top else axis+1
-    if move_axis < insert_before: insert_before += 1
-    self.reshape_and_permute(
-      lambda x: list(x[0:axis]) + (([amount, x[axis]//amount] if top else [x[axis]//amount, amount]) if x[axis] > 1 else [1,1]) + list(x[axis+1:]),
-      [i for i in range(insert_before) if i != move_axis] + [move_axis] + [i for i in range(insert_before, self.shape_len+1) if i != move_axis])
-
-  # drops the final dimension
-  def upcast(self):
-    upcasted = [x.shape[-1] for x in self.sts if x.shape[-1] != 1]
-    assert len(upcasted) >= 1 and all_same(upcasted), f"can't upcast mismatch {upcasted}"
-    for st,buftoken in zip(self.sts, self.buftokens):
-      # add last axis to the buftoken (if it's not a 1)
-      if st.shape[-1] == upcasted[0]: buftoken.array(st.shape[-1], st.views[-1].strides[-1], len(upcasted) != len(self.sts))
-      # remove the last axis (unless it's the only dimension, then make it a 1)
-      st.views[-1] = View(st.shape[0:-1], st.views[-1].strides[0:-1], st.views[-1].offset) if len(st.shape) > 1 else View((1,), (0,), st.views[-1].offset)
diff --git a/tinygrad_repo/tinygrad/codegen/gpu.py b/tinygrad_repo/tinygrad/codegen/gpu.py
deleted file mode 100644
index 38b33a2412..0000000000
--- a/tinygrad_repo/tinygrad/codegen/gpu.py
+++ /dev/null
@@ -1,340 +0,0 @@
-import math, itertools
-from collections import defaultdict
-from typing import Optional, List, Tuple, Dict, Set, Final, NamedTuple
-from tinygrad.ops import UnaryOps, BinaryOps, ReduceOps, LazyOp, Op, ASTRunner
-from tinygrad.codegen.ast import ASTKernel, Token, Types
-from tinygrad.shape.symbolic import Node, MulNode, DivNode, SumNode, Variable, render_python
-from tinygrad.shape import ShapeTracker, View
-from tinygrad.helpers import getenv, DEBUG, prod, partition, mnum, all_same, dedup
-
-# div is different in cl than python
-render_cl = render_python.copy()
-render_cl[DivNode] = lambda self,ops,ctx: f"({self.a.render(ops)}/{self.b})"
-
-VALIDHACKS = getenv("VALIDHACKS", 0)    # TODO: remove the need for this
-NATIVE_EXPLOG = getenv("NATIVE_EXPLOG", 0)  # this is needed as a switch for the tests to pass
-
-class GPULanguage(NamedTuple):
-  kernel_prefix : str = ""
-  buffer_prefix : str = ""
-  buffer_suffix : str = ""
-  smem_prefix : str = ""
-  barrier : str = ""
-  gid : List[str] = []
-  lid : List[str] = []
-  extra_args : List[str] = []
-  float4 : Optional[str] = None
-
-def to_image_idx(base_shape:Tuple[int, ...], idxy:Node, valid:Node, validhacks=False) -> Tuple[Node, Node]:
-  idy = (idxy//(4*base_shape[1]))
-  if validhacks and valid.min == 0:
-    idx = (idxy//4) + (idy*-base_shape[1])
-    # find the ones in idx that didn't factorize and remove them (TODO: this is not universal)
-    if isinstance(idx, SumNode):
-      unfactored, idx_nodes = partition(idx.nodes, lambda x: isinstance(x, MulNode) and x.b == -base_shape[1])
-      assert len(unfactored) <= 1
-      idx = Variable.sum(idx_nodes)
-      unfactored = (Variable.sum(unfactored) // base_shape[1])
-      idy += unfactored
-      # ugh really...handtuned garbage
-      if idx.min >= (base_shape[1]*3)//4:
-        idx -= base_shape[1]
-        idy += 1
-  else:
-    idx = (idxy//4)%base_shape[1]
-  #print(base_shape, idx.min, idx.max, idy.min, idy.max, idx, idy)
-  return idx, idy
-
-class GPUCodegen(ASTKernel):
-  lang : GPULanguage = GPULanguage()
-
-  # for renaming
-  kernel_cnt : Final[Dict[str, int]] = defaultdict(lambda: -1)
-  kernel_name_cache : Final[Dict[str, str]] = {}
-
-  code_for_op : Final[Dict[Op, str]] = {
-    UnaryOps.NOOP: "(A)", UnaryOps.NEG: "(-(A))", UnaryOps.NOT: "(1.0f-A)",
-    UnaryOps.EXP: "native_exp(A)" if NATIVE_EXPLOG else "exp(A)",
-    UnaryOps.LOG: "native_log(A)" if NATIVE_EXPLOG else "log(A)",
-    BinaryOps.ADD: "(A+B)", BinaryOps.SUB: "(A-B)", BinaryOps.MUL: "(A*B)",
-    BinaryOps.DIV: "(A/B)", BinaryOps.POW: "pow(A,B)", BinaryOps.CMPEQ: "(A==B)",
-    BinaryOps.MAX: "max(A,B)", ReduceOps.SUM: "A+=B", ReduceOps.MAX: "A=max(A,B)"
-  }
-  start_for_op : Final[Dict[Op, str]] = {ReduceOps.SUM: "0.0f", ReduceOps.MAX: "-INFINITY"}
-
-  def group_float4(self, grp:List[Token]) -> Token:
-    if all(g.tok.endswith(e) for g,e in zip(grp, [".x", ".y", ".z", ".w"])) and all_same([g.tok.split(".")[0] for g in grp]): return Token(grp[0].tok.split(".")[0], Types.FLOAT4)
-    else: return Token(f"{self.lang.float4}({','.join(g.tok for g in grp)})", Types.FLOAT4)
-
-  def store(self, buf_index:int, value:List[Token]) -> None:
-    assert len(value) == self.buftokens[buf_index].size(), f"size mismatch {len(value)} != {self.buftokens[buf_index].size()}"
-    assert len(self.sts[buf_index].views) == 1, "store has more than one view"
-
-    # all stores can merge, since they have one view and are valid
-    should_upcast = self.lang.float4 and self.buftokens[buf_index].can_float4()
-
-    to_store = {o:v for o,v in zip(self.buftokens[buf_index].offsets(), value)}
-    did_store = set()
-    for o,v in to_store.items():
-      if o in did_store: continue
-      idxy, valid = self.sts[buf_index].expr_idxs(o)
-      assert valid.min == 1, "store must always be valid"
-      if should_upcast:
-        for j in range(4): did_store.add(o+j)
-        v = self.group_float4([to_store[o+j] for j in range(4)])
-      if self.bufs[buf_index] is not None and hasattr(self.bufs[buf_index]._buf, "IMAGE"):
-        assert v.typ == Types.FLOAT4, "Image requires upcasting to FLOAT4"
-        idx, idy = to_image_idx(self.bufs[buf_index]._base_shape, idxy, valid)
-        self.kernel.append(f"write_imagef({self.buftokens[buf_index].tok}, (int2)({idx.render(render_cl)}, {idy.render(render_cl)}), {v.tok});  /* {self.bufs[buf_index]._base_shape} */\n")
-      elif v.typ == Types.FLOAT4:
-        self.kernel.append(f"(({self.lang.buffer_prefix if self.bufs[buf_index] is not None else self.lang.smem_prefix}float4*){self.buftokens[buf_index].tok})[{(idxy//4).render(render_cl)}] = {v.tok};\n")
-      else:
-        self.kernel.append(f"{self.buftokens[buf_index].tok}[{(idxy//(4 if v.typ == Types.FLOAT4 else 1)).render(render_cl)}] = {v.tok};\n")
-
-  def load(self, buf_index:int, idx_override:Optional[str]=None) -> List[Token]:
-    # constant folding
-    const = None
-    if self.bufs[buf_index] is not None and self.bufs[buf_index]._base_shape == (1,) and self.bufs[buf_index]._backing is not None:
-      if buf_index != 0: self.bufs_to_delete.add(buf_index)
-      val = self.bufs[buf_index]._backing[0]
-      assert not math.isnan(val)
-      const = Token(f"({val}f)", Types.FLOAT)
-    should_upcast = self.lang.float4 and const is None and self.buftokens[buf_index].can_float4()
-    tokens = []
-    test_idy = []
-    for o in self.buftokens[buf_index].offsets():
-      key = f"val{mnum(buf_index)}_{mnum(o)}"
-      if (buf_index, o) not in self.loaded_keys:
-        idxy, valid = self.sts[buf_index].expr_idxs(o) if idx_override is None else self.sts[buf_index].expr_node(idx_override, o)
-        if should_upcast:
-          float4_index = Variable("FLOAT4_INDEX", 0, 3)
-          idxy_test, valid_test = self.sts[buf_index].expr_idxs(float4_index+o) if idx_override is None else self.sts[buf_index].expr_node(idx_override, float4_index+o)
-          can_merge = idxy_test == float4_index or (isinstance(idxy_test, SumNode) and any(x == float4_index for x in idxy_test.nodes))   # float4_index must be in there without a multiply
-          can_merge = can_merge and "FLOAT4_INDEX" not in (idxy_test//4).render() and "FLOAT4_INDEX" not in valid_test.render()  # float4_index must not be in after divide or in valid (TODO: don't check render)
-        if const is not None:
-          ldr = const
-        elif self.bufs[buf_index] is not None and hasattr(self.bufs[buf_index]._buf, "IMAGE"):
-          assert should_upcast and can_merge, f"Image requires upcasting to FLOAT4 {self.buftokens[buf_index]}"
-          idx, idy = to_image_idx(self.bufs[buf_index]._base_shape, idxy, valid, VALIDHACKS)
-          ldr = Token(f"read_imagef({self.buftokens[buf_index].tok}, smp, (int2)({idx.render(render_cl)}, {idy.render(render_cl)})) /* {self.bufs[buf_index]._base_shape} */", Types.FLOAT4)
-          test_idy.append(idy.render(render_cl))
-        elif should_upcast and can_merge:
-          ldr = Token(f"(({self.lang.buffer_prefix if self.bufs[buf_index] is not None else self.lang.smem_prefix}float4*){self.buftokens[buf_index].tok})[{(idxy//4).render(render_cl)}]", Types.FLOAT4)
-        else:
-          ldr = Token(f"{self.buftokens[buf_index].tok}[{idxy.render(render_cl)}]", Types.FLOAT)
-        invalid = self.group_float4([Token("0.0f", Types.FLOAT)]*4) if ldr.typ == Types.FLOAT4 else Token("0.0f", Types.FLOAT)
-        ldr = ldr if valid.min == 1 or (VALIDHACKS and hasattr(self.bufs[buf_index]._buf, "IMAGE")) else (Token(f"({valid.render(render_cl)} ? {ldr.tok} : {invalid.tok})", ldr.typ) if valid.max == 1 else invalid)
-        if const is not None:
-          self.loaded_keys[(buf_index,o)] = ldr
-        else:
-          self.kernel.append(f"{ldr.decltype()} {key} = {ldr.tok};\n")
-          if should_upcast and can_merge:
-            for j in range(4):
-              self.loaded_keys[(buf_index,o+j)] = Token(key+f'.{"xyzw"[j]}', Types.FLOAT)
-          else:
-            self.loaded_keys[(buf_index,o)] = Token(key, Types.FLOAT)
-      tokens.append(self.loaded_keys[(buf_index,o)])
-    assert not VALIDHACKS or all_same(test_idy), f"idy changed! {test_idy}"
-    return tokens
-
-  def ast_parse(self, x, acc:List[Token], do_reduce=False) -> List[Token]:
-    if not isinstance(x, LazyOp): return self.load(self.bufs.index(x), "mid" if x is None else None)  # hack for local
-    if isinstance(x.op, ReduceOps) and not do_reduce: return acc
-    values : List[List[Token]] = ([acc] if isinstance(x.op, ReduceOps) else []) + [self.ast_parse(v, acc, do_reduce) for v in x.src]
-    code = GPUCodegen.code_for_op[x.op]  # TODO: replace this with a function
-    if len(values) == 2:
-      assert len(values[0]) == len(values[1]) and values[0][0].typ == values[1][0].typ, f"values mismatch {values}"
-      return [Token(code.replace("A", a.tok).replace("B", b.tok), a.typ) for a,b in zip(values[0], values[1])]
-    else:
-      return [Token(code.replace("A", a.tok), a.typ) for a in values[0]]
-
-  def required_optimizations(self, early_only=False):
-    for buf_index,buf in enumerate(self.bufs):
-      upcast_strides = [self.sts[buf_index].strides[i] for i in self.upcast_in_mid_reduce_axes]
-      if (not early_only or buf in self.earlybufs) and hasattr(buf._buf, "IMAGE") and not (self.buftokens[buf_index].can_float4() or (buf not in self.earlybufs and (1 in upcast_strides))):
-        axes = [i for i,x in enumerate(self.sts[buf_index].strides) if x == 1]
-        assert len(axes) == 1, f"wrong number of stride 1 axis : {axes}"
-        self.shift_to(axes[0], 4)
-        self.upcast()
-        assert self.buftokens[buf_index].can_float4()
-
-  def hand_coded_optimizations(self):
-    # if there's images in the earlybufs, we have to make an axis the 4 loading one
-    self.required_optimizations(early_only=True)
-
-    # simplify (sets first_reduce)
-    self.simplify_ones()
-
-    # are we grouping? (requires local shape support)
-    if len(self.lang.lid) and not self.buftokens[0].can_float4() and self.first_reduce <= 2 and self.first_reduce + 1 <= self.shape_len and prod(self.sts[0].shape[:self.first_reduce]) <= 2048:
-      # TODO: use 1024 if it's allowed in a smarter way
-      for sz in (([256, 16]) if prod(self.sts[0].shape[:self.first_reduce]) <= 32 else [16]):
-        if all([st.shape[self.first_reduce] % sz == 0 or st.shape[self.first_reduce] == 1 for st in self.sts]):
-          self.shift_to(self.first_reduce, sz, top=True, insert_before=self.first_reduce)
-          self.group_for_reduce.append(sz)
-          break
-
-    # are we upcasting in mid reduce?
-    if hasattr(self.bufs[0]._buf, "IMAGE") and not self.buftokens[0].can_float4() and self.group_for_reduce and self.first_reduce <= 2:
-      axes = [i for i,x in enumerate(self.sts[0].strides) if x == 1]
-      assert len(axes) == 1, f"wrong number of stride 1 axis : {axes}"
-      self.shift_to(axes[0], 4, insert_before=self.first_reduce + len(self.group_for_reduce))   # insert at the end of the grouped axis
-      self.group_for_reduce.append(4)
-
-    # now do everything required
-    self.required_optimizations()
-
-    # simplify (sets first_reduce)
-    self.simplify_ones()
-
-    # use more opencl indexing if the output buffer is an image and we have room
-    if hasattr(self.bufs[0]._buf, "IMAGE") and self.first_reduce+len(self.group_for_reduce) < 3:
-      base_shape = self.bufs[0]._base_shape
-      if (base_shape[0]*base_shape[1]) % self.sts[0].shape[0] == 0 and self.sts[0].shape[0]//base_shape[0] != 0:
-        if DEBUG >= 4: print("split opencl", base_shape, self.sts[0].shape)
-        self.reshape_and_permute(lambda x: [base_shape[0], x[0]//base_shape[0]]+list(x[1:]), None)
-        self.simplify_ones()
-
-    # no more opt if we are grouping
-    if self.group_for_reduce: return
-
-    # **** below this line need to be optional and benchmarked ****
-
-    # potentially do more upcasts of non reduce axes based on a heuristic
-    while prod(self.sts[0].shape[:self.first_reduce]) >= 1024:
-      xb_choices = []
-      for axis, upcast_amount in itertools.product(range(self.first_reduce), [3,4]):   # consider all the non reduce axes, and a 3 or 4 reduce
-        # if it mods, and some buffer has stride 0 on axis while having no stride 0 in the buftoken
-        if self.full_shape[axis]%upcast_amount == 0 and any(self.sts[buf_index].strides[axis] == 0 and not any(x[1] == 0 for x in self.buftokens[buf_index].axis) for buf_index in range(len(self.sts))):
-          xb_choices.append((sum(st.strides[axis]>0 for st in self.sts), sum(st.strides[axis] for st in self.sts), axis, upcast_amount))
-      if len(xb_choices):
-        xb_choices = sorted(xb_choices)
-        if DEBUG >= 4: print(f"float4 merging axis : {xb_choices}")
-        self.shift_to(xb_choices[0][2], amount=xb_choices[0][3])
-        self.upcast()
-        self.simplify_ones()
-      else:
-        break
-
-    # if last dim <= 5 and it's a reduce dim, upcast the reduce (loop unrolling). no simplify needed since it's just an upcast. NOTE: careful, this has broken VALIDHACKS
-    if self.first_reduce < self.shape_len and self.full_shape[-1] <= 5 and (max([x.size() for i,x in enumerate(self.buftokens) if self.bufs[i] in self.earlybufs]) <= 4 or not any(r for _,_,r in self.buftokens[self.full_buf_index].axis)):
-      self.upcast()
-
-  def get_accumulators(self, name="acc") -> List[Token]:
-    assert self.reduceop is not None, "no accumulators if you aren't reducing"
-    should_upcast = self.lang.float4 and self.buftokens[0].can_float4()
-    accumulators = [Token(f"{name}{i//4}.{'xyzw'[i%4]}" if should_upcast else f"{name}{i}", self.buftokens[0].typ) for i in self.buftokens[0].offsets()]
-    if should_upcast:
-      self.kernel += [f"float4 {tok} = {self.group_float4([Token(GPUCodegen.start_for_op[self.reduceop.op], Types.FLOAT)]*4).tok};\n" for tok in dedup([x.tok.split('.')[0] for x in accumulators])]
-    else:
-      self.kernel += [f"float {x.tok} = {GPUCodegen.start_for_op[self.reduceop.op]};\n" for x in accumulators]
-    return accumulators
-
-  # STOP WASTING TIME WITH DOING THE RESHAPES AND PERMUTES BY HAND. KERNEL SEARCH IS THE ONLY WAY IT WILL EVER BE GOOD
-  # group_for_reduce will have to be better first
-  def codegen(self) -> ASTRunner:
-    self.process()
-    if DEBUG >= 4: self.printbufs("old:", DEBUG>=5)
-
-    self.hand_coded_optimizations()
-
-    # fancy colored shape printer
-    if DEBUG >= 3: print(self.colorshape(), end="")
-
-    # add a local buffer for multistage reduce
-    if len(self.group_for_reduce):
-      self.bufs.append(None)
-      # TODO: the strides of this can be controlled
-      st = ShapeTracker(tuple([1] * self.first_reduce + self.group_for_reduce + [1] * (self.shape_len - len(self.group_for_reduce) - self.first_reduce) + [x[0] for x in self.buftokens[0].axis]))
-      buftoken = Token("temp", Types.FLOAT, ptr=True)
-      # manual upcast of the local
-      for _,_,r in self.buftokens[0].axis[::-1]:
-        buftoken.array(st.shape[-1], st.views[-1].strides[-1], r)
-        st.views[-1] = View(st.shape[0:-1], st.views[-1].strides[0:-1], st.views[-1].offset)
-      self.sts.append(st)
-      self.buftokens.append(buftoken)
-
-    self.output_shape : Tuple[int, ...] = self.sts[0].shape[:self.first_reduce] + tuple(self.group_for_reduce)
-    assert self.full_shape[:len(self.output_shape)] == self.output_shape, f"output shape mismatch : {self.full_shape[:len(self.output_shape)]} != {self.output_shape}"
-    if DEBUG >= 4:
-      print("output shape", self.output_shape)
-      self.printbufs("new:", DEBUG>=5)
-
-    self.bufs_to_delete : Set[int] = set()
-    self.loaded_keys : Dict[Tuple[int,int], Token] = {}
-    self.prekernel : Set[str] = set()
-    self.kernel : List[str] = ["const sampler_t smp = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST;\n"] if any(hasattr(buf._buf, "IMAGE") for buf in self.bufs if buf is not None) else []
-
-    if len(self.lang.gid) == 0:
-      self.kernel += [f"for (int idx{i} = 0; idx{i} < {self.output_shape[i]}; idx{i}++) {{\n" for i in range(0, len(self.output_shape))]
-    else:
-      # output_shape[-1] is get_global_id(0)
-      self.kernel += [f"int idx{len(self.output_shape)-1-i} = {self.lang.gid[i]}; /* {self.output_shape[-1-i]} */\n" for i in range(min(len(self.lang.gid), len(self.output_shape))) if self.output_shape[-1-i] != 1]
-      if len(self.output_shape) > len(self.lang.gid):
-        # sometimes, there's more dimensions. compact all the dimensions into the first one
-        # TODO: these compactions should be searchable (they sort of are with reshapes and permutes)
-        final_dimension = len(self.output_shape)-len(self.lang.gid)
-        for i in range(final_dimension-1, -1, -1):
-          self.kernel += [f"int idx{i} = idx{final_dimension} % {self.output_shape[i]};", f"idx{final_dimension} = idx{final_dimension} / {self.output_shape[i]};\n"]
-        self.output_shape = (prod(self.output_shape[0:final_dimension+1]), ) + self.output_shape[final_dimension+1:]
-        if DEBUG >= 4: print(f"replaced output shape with {self.output_shape}")
-
-    # early ast
-    accumulators : List[Token] = []
-    if self.reduceop is not None:
-      accumulators = self.get_accumulators()
-      self.kernel += [f"for (int idx{i} = 0; idx{i} < {self.full_shape[i]}; idx{i}++) {{\n" for i in range(self.first_reduce+len(self.group_for_reduce), self.shape_len)]
-      self.kernel += [f"{x.tok};\n" for x in self.ast_parse(self.reduceop, [accumulators[off] for off in self.buftokens[self.full_buf_index].acc_offsets()], do_reduce=True)]
-      self.kernel += ["}\n"] * (self.shape_len - (self.first_reduce + len(self.group_for_reduce)))
-
-      # second stage reduce
-      if self.group_for_reduce:
-        self.kernel.append(self.lang.smem_prefix + f"float {self.buftokens[-1].tok}[{self.sts[-1].size()*self.buftokens[-1].size()}];\n")
-        self.store(-1, accumulators)  # TODO: this is assuming the local size = global size. should use lidxs
-        self.kernel.append(self.lang.barrier+"\n")
-
-        # this is used to identify the thread doing the reducing (lidx == 0) and is repeated from store
-        # must happen before the upcast
-        lidx, lvalid = self.sts[-1].expr_idxs()
-        assert lvalid.min == 1, "local buffer must always be valid"
-
-        # if any group_for_reduce items aren't reduces, upcast them here
-        for j in self.upcast_in_mid_reduce_axes:
-          self.reshape_and_permute(None, [i for i in range(self.shape_len) if i != j] + [j])
-          self.upcast()
-          if DEBUG >= 4: print("upcast", self.colorshape()) # NOTE: colorshape is wrong here
-
-        self.kernel.append(f"if ({lidx.render(render_cl)} == 0) {{\n")   # lidx.max works here too
-
-        # second stage reduce with a new set of accumulators. TODO: do we need acc_offsets here?
-        accumulators = self.get_accumulators("output")
-        self.kernel.append(f"for (int mid = 0; mid < {self.sts[-1].size()}; mid++) {{\n")
-        self.kernel += [f"{x.tok};\n" for x in self.ast_parse(LazyOp(self.reduceop.op, (None,), self.sts[0].shape), accumulators, do_reduce=True)]
-        self.kernel.append("}\n")
-
-    # late ast
-    self.store(0, self.ast_parse(self.ast, accumulators))
-    if self.group_for_reduce: self.kernel.append("}")
-    if len(self.lang.gid) == 0: self.kernel += ["}"] * len(self.output_shape)
-    self.kernel.append("\n}")
-
-    # concat kernel into prg
-    buftypes = [f"{'read_only' if i > 0 else 'write_only'} image2d_t" if hasattr(x._buf, "IMAGE") else self.lang.buffer_prefix+self.buftokens[i].decltype()+self.lang.buffer_suffix for i,x in enumerate(self.bufs) if x is not None]
-    prg = ' '.join(list(self.prekernel) + [f"{self.lang.kernel_prefix} void KERNEL_NAME_PLACEHOLDER(",] +
-      [', '.join([f'{t} data{i}' for i,t in enumerate(buftypes) if i not in self.bufs_to_delete] + self.lang.extra_args)] +
-      [") {\n"] + self.kernel)
-
-    # kernel function definition
-    function_name = ("re_S" if self.reduceop else "ew_S") + '_'.join([str(x) for x in self.full_shape])
-
-    # painfully name the function
-    if prg in GPUCodegen.kernel_name_cache: function_name = GPUCodegen.kernel_name_cache[prg]
-    else:
-      GPUCodegen.kernel_cnt[function_name] += 1
-      if GPUCodegen.kernel_cnt[function_name]: function_name = f"{function_name}{'_N'+str(GPUCodegen.kernel_cnt[function_name])}"
-      GPUCodegen.kernel_name_cache[prg] = function_name
-
-    return ASTRunner(function_name, prg.replace("KERNEL_NAME_PLACEHOLDER", function_name), self.bufs_to_delete,
-      list(self.output_shape[::-1]) if len(self.output_shape) > 0 else [1],
-      (self.group_for_reduce[::-1] + [1]*(len(self.output_shape)-len(self.group_for_reduce))) if self.group_for_reduce else None,
-      op_estimate=self.info.flops, mem_estimate=sum(prod(x._base_shape) for x in self.bufs if x is not None))
diff --git a/tinygrad_repo/tinygrad/codegen/kernel.py b/tinygrad_repo/tinygrad/codegen/kernel.py
new file mode 100644
index 0000000000..f4b2ac6c62
--- /dev/null
+++ b/tinygrad_repo/tinygrad/codegen/kernel.py
@@ -0,0 +1,585 @@
+from __future__ import annotations
+import os, math, itertools
+from typing import NamedTuple, Optional, List, Tuple, cast, Dict, Union
+from tinygrad.ops import LazyOp, FlopCounter, get_lazyop_info, UnaryOps, BinaryOps, ReduceOps, MemBuffer, ConstBuffer, BufferOps, Device, Compiled
+from tinygrad.helpers import dedup, dtypes, colored, ImageDType, DType, all_int, ansilen, getenv, prod, DEBUG
+from tinygrad.shape.shapetracker import ShapeTracker, get_contraction
+from tinygrad.shape.symbolic import sint
+from tinygrad.shape.view import View, strides_for_shape
+from dataclasses import dataclass
+from enum import Enum, auto
+
+class OptOps(Enum):
+  UPCAST = auto(); UPCASTMID = auto(); UNROLL = auto(); LOCAL = auto(); LASTLOCAL = auto(); GROUP = auto(); GROUPTOP = auto(); NOLOCALS = auto() # noqa: E702
+  def __lt__(self, x:OptOps): return self.value < x.value
+
+@dataclass(frozen=True, order=True)
+class Opt:
+  op: OptOps
+  axis: Optional[int] = None
+  amt: Optional[int] = None
+  def __repr__(self): return f"Opt(op={self.op}, axis={self.axis}, amt={self.amt})"
+
+@dataclass(frozen=True)
+class TensorCore:
+  device: str
+  dims: List[int]
+  dtype_in: DType
+  dtype_out: DType
+  threads: List[Tuple[int,int]] # list of (TC dim,amt) that construct the warp thread structure
+  upcast_dim: int # which TC dim to upcast
+  thread_local_aliases: List[List[List[int]]] # a list of [threads_1, ..., threads_n, upcast_1(unrolled), upcast_2(upcast)] defining the alias (-1 is upcast, 1-n is warp threads) for each TC dim
+  thread_local_sizes: List[int] # in each thread, the number of elements stored in registers for each TC dim
+  arch: Optional[str] = None
+  def __str__(self): return f"tensor_core<{self.device}, {self.dims}, {self.dtype_in}, {self.dtype_out}>"
+
+tensor_cores: Dict[str, List[TensorCore]] = {
+  "METAL": [
+    TensorCore(device="METAL", dims=[8,8,8], dtype_in=dtypes.float, dtype_out=dtypes.float, upcast_dim=0, threads=[(0,2),(1,4),(0,2),(1,2)], thread_local_sizes=[2,2,2], thread_local_aliases= [ [[4],[0],[2],[0],[-1, 1, 3],[0]], [[0],[3],[0],[1],[2, 4],[-1]], [[4],[3],[2],[1],[0],[-1]] ], arch="arm64"),
+    TensorCore(device="METAL", dims=[8,8,8], dtype_in=dtypes.half,  dtype_out=dtypes.half,  upcast_dim=0, threads=[(0,2),(1,4),(0,2),(1,2)], thread_local_sizes=[2,2,2], thread_local_aliases= [ [[4],[0],[2],[0],[-1, 1, 3],[0]], [[0],[3],[0],[1],[2, 4],[-1]], [[4],[3],[2],[1],[0],[-1]] ], arch="arm64"),
+  ],
+  "HIP": [
+    TensorCore(device="HIP", dims=[16,16,16], dtype_in=dtypes.half, dtype_out=dtypes.float, upcast_dim=1, threads=[(0,16),(1,2)], thread_local_sizes=[16,16,8], thread_local_aliases=[ [[0],[0],[-1],[1]], [[0],[1],[-1],[0]], [[0],[1],[0],[2,-1]] ]),
+    TensorCore(device="HIP", dims=[16,16,16], dtype_in=dtypes.half, dtype_out=dtypes.half,  upcast_dim=1, threads=[(0,16),(1,2)], thread_local_sizes=[16,16,8], thread_local_aliases=[ [[0],[0],[-1],[1]], [[0],[1],[-1],[0]], [[0],[1],[0],[2,-1]] ]),
+  ]
+}
+
+class LocalBuffer(NamedTuple):
+  name: str
+  size: int
+  dtype: DType = dtypes.float32
+  realized: None = None
+  def __str__(self): return f"localbuffer<{self.name}[{self.size}]>"
+
+class LinearizerOptions(NamedTuple):
+  device: str = ""
+  # TODO: make this generic with a list of supported types
+  supports_float4: bool = True
+  supports_float4_alu: bool = True
+  has_local: bool = True
+  has_shared: bool = True
+  # NOTE: these two should be in z,y,x(reversed) order for cstyle backends, they are flipped when kernel is rendered
+  global_max: Optional[List[int]] = None
+  local_max: Optional[List[int]] = None
+
+class Kernel:
+  def __init__(self, ast:LazyOp, opts:Optional[LinearizerOptions]=None):
+    self.opts = opts if opts else (cast(Compiled, Device[Device.DEFAULT]).linearizer_opts if isinstance(Device[Device.DEFAULT], Compiled) else LinearizerOptions())
+    self.ast = ast
+
+    # fetch lazyop info
+    self.info: FlopCounter = get_lazyop_info(cast(LazyOp, self.ast))
+
+    # there's only allowed to be one reduceop
+    reduceops = [x for x in self.ast.get_lazyops() if x.op in ReduceOps]
+    assert len(dedup(reduceops)) <= 1, "max one reduce op in an ast"
+    self.reduceop = reduceops[0] if reduceops else None
+
+    # create new shapetrackers inside this kernel, we will permute them
+    self.bufs: List[Union[MemBuffer, ConstBuffer, LocalBuffer]] = [MemBuffer(0, self.info.dtype, ShapeTracker.from_shape(self.info.shape))] + dedup([x.arg for x in self.ast.get_lazyops() if x.op in BufferOps])
+
+    # get earlybufs, before the one reduce op
+    self.earlybufs = [x.arg for x in self.reduceop.get_lazyops() if x.op in BufferOps] if self.reduceop else []
+    self.full_buf_index: int = self.bufs.index(self.earlybufs[0]) if self.earlybufs else 0
+
+    # create the (permuted) shapetrackers
+    self.sts: List[ShapeTracker] = [x.st for x in cast(List[Union[MemBuffer, ConstBuffer]], self.bufs)]
+
+    # move all reduce axes to the end
+    reduce = list(enumerate(zip(self.full_shape, self.sts[0].shape)))
+    permute = tuple([i for i,(s,n) in reduce if s == n] + [i for i,(s,n) in reduce if s != n])
+    self.reshape_and_permute(None, permute)
+
+    # parameters for optimization
+    self.applied_opts: List[Opt] = []
+    self.group_for_reduce: List[int] = []
+    self.upcasted: int = 0
+    self.local_dims: int = 0
+    self.local_alias: Dict[int, LocalBuffer] = {}
+    self.tensor_core: Optional[TensorCore] = None
+    self.dont_use_locals: bool = False
+
+    # group simplifies
+    self.simplify_ones()
+    self.simplify_merge_adjacent()
+
+    # cache
+    self.applied_opts_cache: Optional[List[Opt]] = None
+
+  def copy(self):
+    ret = type(self).__new__(type(self))
+
+    # base linearizer params
+    ret.opts, ret.ast = self.opts, self.ast
+
+    # things downstream of the AST
+    # NOTE: we copy bufs for local buffers and sts for optimizations
+    ret.info, ret.reduceop, ret.bufs, ret.earlybufs, ret.full_buf_index, ret.sts = \
+      self.info, self.reduceop, self.bufs[:], self.earlybufs, self.full_buf_index, self.sts[:]
+
+    # parameters for optimizations
+    ret.applied_opts, ret.group_for_reduce, ret.upcasted, ret.local_dims, ret.local_alias, ret.tensor_core, ret.dont_use_locals = \
+      self.applied_opts[:], self.group_for_reduce[:], self.upcasted, self.local_dims, self.local_alias.copy(), self.tensor_core, self.dont_use_locals
+
+    # uncached since linearize didn't run
+    ret.applied_opts_cache = None
+
+    return ret
+
+  @property
+  def membufs(self) -> List[MemBuffer]: return [x for x in self.bufs if isinstance(x, MemBuffer)]
+
+  def has_variable_shape(self) -> bool:
+    for b in self.bufs:
+      if not isinstance(b, LocalBuffer) and not all_int(b.st.views[-1].shape): return True
+    return False
+
+  def shape_offsets(self, i): return itertools.product(*[list(range(s)) for s in self.sts[i].shape[self.shape_len-self.upcasted:][::-1]]) if self.upcasted > 0 else [tuple()]
+  def float4_axis(self, i): return [x-(self.shape_len-self.upcasted) for x in self.sts[i].unit_stride_axes() if x >= self.shape_len-self.upcasted and self.sts[i].shape[x]%4 == 0]
+
+  def upcasted_axis(self, i):
+    return list(zip(self.sts[i].shape[self.shape_len-self.upcasted:],
+                    self.sts[i].real_strides()[self.shape_len-self.upcasted:],
+                    [x!=y for x,y in zip(self.sts[0].shape[self.shape_len-self.upcasted:], self.full_shape[self.shape_len-self.upcasted:])]))
+
+  # TODO: is there a better way to write this?
+  def acc_offsets(self, i):
+    if self.upcasted == 0: return [0]
+    upcasted_i = self.upcasted_axis(i)
+    acc_strides = [x*(1-upcasted_i[::-1][i][2]) for i,x in enumerate(strides_for_shape(tuple(1 if r else s for s,_,r in upcasted_i[::-1])))]
+    return [sum(t) for t in itertools.product(*[[y*acc_strides[i] for y in range(x[0])] for i,x in enumerate(upcasted_i[::-1])])]
+
+  def get_upcast_dim(self, i) -> List[int]:
+    should_upcast = self.opts.supports_float4 and (self.bufs[i].dtype in [dtypes.float32, dtypes.float16] or isinstance(self.bufs[i].dtype, ImageDType))
+    return [x for x in self.sts[i].unit_stride_axes() if should_upcast and x >= self.shape_len-self.upcasted and self.sts[i].shape[x] > 1]
+
+  @property
+  def first_reduce(self) -> int: return [x!=y for x,y in zip(self.sts[0].shape[:self.shape_len-self.upcasted]+(0,), self.full_shape[:self.shape_len-self.upcasted]+(1,))].index(True)
+
+  @property
+  def output_shape(self) -> Tuple[sint, ...]: return self.sts[0].shape
+
+  @property
+  def full_shape(self) -> Tuple[sint, ...]: return self.sts[self.full_buf_index].shape
+
+  @property
+  def full_unupcasted_shape(self) -> Tuple[sint, ...]: return self.full_shape[:self.shape_len-self.upcasted]
+
+  @property
+  def shape_len(self) -> int: return len(self.sts[0].shape)
+
+  @property
+  def upcast_in_mid_reduce_axes(self) -> List[int]: return [j for j in range(self.first_reduce, self.first_reduce+len(self.group_for_reduce)) if self.full_shape[j] == self.sts[0].shape[j]]
+
+  @property
+  def global_dims(self) -> int: return self.first_reduce-self.local_dims
+
+  # there's eight chunks of the shape
+  # blue   -- global dims
+  # cyan   -- local dims (warp ones first)
+  #  *** self.first_reduce
+  # green  -- reduce-local dims
+  # white  -- reduce-late upcasted dim (self.upcast_in_mid_reduce_axes)
+  # red    -- reduce loops
+  #  *** self.upcasted
+  # purple -- reduce upcasted
+  # yellow -- normal upcasted dimensions
+  def colors(self) -> List[str]:
+    # first non local non reduce dims are global (blue)
+    colors = ["blue"] * self.global_dims if not self.dont_use_locals else ["BLUE"] * self.global_dims
+    # after global are local_dims; warp ones used in tensor cores must be closest to first_reduce (cyan)
+    colors += ["cyan"] * self.local_dims
+    # between first_reduce and first_reduce + group_for_reduce, they are either upcast mid reduce (white), or late upcasted (green)
+    colors += ["white" if i in self.upcast_in_mid_reduce_axes else "green" for i in range(self.first_reduce, self.first_reduce + len(self.group_for_reduce))]
+    # between first_reduce + group_for_reduce and upcasted, they are reduce (red)
+    colors += ["red"] * ((self.shape_len-self.upcasted) - (self.first_reduce + len(self.group_for_reduce)))
+    # upcasted dimensions are reduce (magenta) or normal (yellow)
+    colors += ["magenta" if self.full_shape[i] != self.sts[0].shape[i] else "yellow" for i in range(self.shape_len-self.upcasted, self.shape_len)]
+    assert len(colors) == self.shape_len, "colors size mismatch"
+    return colors
+
+  def colored_shape(self, pad=None, dense=False) -> str:
+    ret = ' '.join(colored(s, color) for s,color in zip([f"{s:4d}" if isinstance(s, int) and not dense else s for s in self.full_shape], self.colors()))
+    if pad: ret += ' '*(pad-ansilen(ret))
+    return ret
+
+  # ******************** base simplifiers ********************
+
+  # apply reshape and permute to all shapetrackers
+  def reshape_and_permute(self, new_shape_fxn, axis):
+    new_sts = []
+    for st in self.sts:
+      if new_shape_fxn is not None: st = st.reshape(tuple(new_shape_fxn(st.shape)))
+      if axis is not None: st = st.permute(tuple(axis))
+      new_sts.append(st)
+    self.sts = new_sts
+
+  # drops the final dimension
+  def upcast(self):
+    assert self.full_shape[-1] != 1, "can't upcast a dimension with size 1"
+    self.upcasted += 1
+
+  # axis : the axis to pull from
+  # amount : the amount to take
+  # top : if you want to pull that amount from the top
+  # insert_before : place to insert the new stuff
+  def shift_to(self, axis, amount, top=False, insert_before=None):
+    if insert_before is None: insert_before = self.shape_len
+    move_axis = axis if top else axis+1
+    if move_axis < insert_before: insert_before += 1
+    self.reshape_and_permute(
+      lambda x: list(x[0:axis]) + (([amount, x[axis]//amount] if top else [x[axis]//amount, amount]) if x[axis] > 1 else [1,1]) + list(x[axis+1:]),
+      [i for i in range(insert_before) if i != move_axis] + [move_axis] + [i for i in range(insert_before, self.shape_len+1) if i != move_axis])
+
+  # ******************** complex simplifiers ********************
+
+  def simplify_ones(self) -> bool:
+    # remove places where the shape is all ones
+    # TODO: this should be factored in to multi shape stride
+    if self.shape_len == 0: return False
+    all_ones = [s==1 for s in self.full_shape]
+    self.local_dims -= sum(all_ones[self.first_reduce-self.local_dims:self.first_reduce])
+    self.upcasted -= sum(all_ones[self.shape_len-self.upcasted:])
+    self.reshape_and_permute(lambda shape: [x for i,x in enumerate(shape) if not all_ones[i]], None)
+    return any(all_ones)
+
+  def simplify_merge_adjacent(self):
+    if self.shape_len == 0: return
+    shapes, strides = [x.shape for x in self.sts], [x.real_strides() for x in self.sts]
+
+    # if it's an image, insert fake strides such that this fusion doesn't happen across image axes
+    if isinstance(self.bufs[0].dtype, ImageDType):
+      base_shape = self.bufs[0].dtype.shape
+      if shape_idx_groups := get_contraction(self.output_shape, base_shape):
+        special_strides: Tuple[int, ...] = tuple()
+        for i,g in enumerate(shape_idx_groups):
+          shape_piece = tuple(self.output_shape[x] for x in g)
+          assert prod(shape_piece) == base_shape[i], f"get_contraction was wrong? {shape_piece} != {base_shape[i]}"
+          special_strides += strides_for_shape(shape_piece)
+        # adding the fake image shape
+        shapes.append(self.output_shape)
+        strides.append(special_strides)
+
+    # merge dimensions if we can, multi get_shape_strides
+    # TODO: does this always preserve the reduce dimension, NO
+    # TODO: move this into shapetracker, with tests!
+    rets = [[(shapes[j][0], strides[j][0])] for j in range(len(shapes))]
+    for i in range(1, len(shapes[0])):
+      can_merge = []
+      for j in range(len(shapes)):
+        # TODO: added the always mergeability of 1s, is this right? if so, add to shapetracker in the 1 case
+        can_merge.append(strides[j][i] is not None and ((strides[j][i] != 0 and rets[j][-1][1] == shapes[j][i]*cast(int, strides[j][i])) or (strides[j][i] == 0 and rets[j][-1][1] == 0)))
+      # more can merge than this
+      mergeable = all(can_merge) and i != self.first_reduce
+      for j in range(len(shapes)):
+        if mergeable: rets[j][-1] = (rets[j][-1][0] * shapes[j][i], strides[j][i])
+        else: rets[j].append((shapes[j][i], strides[j][i]))
+
+    # do the reshapes
+    for i,x in enumerate(rets[:len(self.sts)]): self.sts[i] = self.sts[i].reshape(tuple([y[0] for y in x]))
+
+  # ******************** GPU simplifiers ********************
+  def _limit_size(self, x: Tuple[int], max_size: List) -> Tuple[int, ...]:
+    new_shape,dims = list(x), len(x)
+    for i in range(dims):
+      next_idx = (i + 1) % dims
+      while new_shape[i] > max_size[i]:
+        new_shape[i] = new_shape[i] // 2
+        if (new_shape[next_idx] <= max_size[next_idx]):
+          new_shape[next_idx] = new_shape[next_idx] * 2
+        else:
+          next_idx = (next_idx + 1) % dims
+          new_shape[next_idx] = new_shape[next_idx] * 2
+    return tuple(new_shape)
+
+  def limit_dims_to_max(self, global_max: List[int], local_max: List[int]):
+    # Check the global allocation limit, current the global_size will be flipped during codegen
+    # and then padded right with 1s if its length < 3 which makes this part a bit awkward to write
+    global_dims = self.first_reduce-self.local_dims
+    if global_dims > 0:
+      if global_max:
+        tmp = global_max[:global_dims] + (local_max[:self.local_dims] if local_max else [])
+        if max(global_max) < max(self.full_shape[:global_dims]): self.reshape_and_permute(lambda x: self._limit_size(x, tmp + [math.inf] * (len(self.full_shape)-len(tmp))), None)
+        assert max(global_max) >= max(self.full_shape[:global_dims]), f"device max allocation {max(self.full_shape[:global_dims])} exceeds global dim maximum {max(global_max)}"
+      for i in range(global_dims-1):
+        if i < len(global_max) and self.full_shape[i] > global_max[i]:
+          order = list(range(len(self.full_shape)))
+          order[i], order[global_dims-1] = order[global_dims-1], order[i]
+          self.reshape_and_permute(None, order)
+          if DEBUG >= 3: print("permuted global dim", order, "due to allocation exceeds global limit")
+
+  def alias_buffer(self, i, pattern):
+    assert len(pattern) == len(self.sts[i].shape), f"must include a pattern for each shape {pattern} {self.sts[i].shape}"
+
+    bst = 1
+    real_strides = self.sts[i].real_strides()
+    shp, stride = [(s if p != 0 else 1) for s,p in zip(self.sts[i].shape, pattern)], [0]*len(pattern)
+    for priority in range(1, max(pattern)+1):  # priority. 0 is non local and ignored
+      for j,p in enumerate(pattern):
+        if priority == p and real_strides[j] != 0:
+          stride[j] = bst
+          bst *= shp[j]
+
+    self.sts.append(ShapeTracker((View.create(tuple(shp), tuple(stride)),)))
+    self.bufs.append(LocalBuffer(name=f"ldata{i}", size=self.sts[-1].size()))
+    if DEBUG >= 4: print("aliasing buffer", self.sts[i])
+    self.local_alias[i] = cast(LocalBuffer, self.bufs[-1])
+
+  # ******************** high level optimizers ********************
+
+  def apply_tensor_cores(self, use_tensor_cores=1, extra_opts:Optional[List[Opt]]=None):
+    if use_tensor_cores and self.opts.has_local and self.reduceop and self.reduceop.op == ReduceOps.SUM and self.opts.device in tensor_cores:
+      for tc in tensor_cores[self.opts.device]:
+        if not((tc.arch is None or tc.arch == os.uname().machine) and isinstance(self.reduceop.src[0], LazyOp)): continue
+        has_cast = tc.dtype_in != tc.dtype_out
+
+        if has_cast and not(isinstance(self.reduceop.src[0], LazyOp) and self.reduceop.src[0].op == UnaryOps.CAST and self.reduceop.src[0].arg[0] == tc.dtype_out): continue
+        mul_op = self.reduceop.src[0].src[0] if has_cast else self.reduceop.src[0]
+
+        if not(isinstance(mul_op, LazyOp) and mul_op.op == BinaryOps.MUL): continue
+        if not(isinstance(mul_op.src[0], LazyOp) and mul_op.src[0].op == BufferOps.MEM and mul_op.src[0].arg.dtype == tc.dtype_in): continue
+        if not(isinstance(mul_op.src[1], LazyOp) and mul_op.src[1].op == BufferOps.MEM and mul_op.src[1].arg.dtype == tc.dtype_in): continue
+        buf0, buf1 = self.bufs.index(cast(MemBuffer, mul_op.src[0].arg)), self.bufs.index(cast(MemBuffer, mul_op.src[1].arg))
+        buf0_strides, buf1_strides = self.sts[buf0].real_strides(), self.sts[buf1].real_strides()
+        axis_buf0 = [(i,self.full_shape[i],buf1_strides[i]) for i,s in enumerate(buf0_strides[:self.first_reduce]) if s == 0 and self.full_shape[i]%tc.dims[0] == 0]
+        axis_buf1 = [(i,self.full_shape[i],buf0_strides[i]) for i,s in enumerate(buf1_strides[:self.first_reduce]) if s == 0 and self.full_shape[i]%tc.dims[1] == 0]
+
+        if not(axis_buf0 and axis_buf1 and self.full_shape[self.first_reduce]%tc.dims[2] == 0 and self.full_shape[self.first_reduce] >= tc.dims[2] and (self.shape_len-self.first_reduce) == 1): continue
+
+        if DEBUG >= 3: print("TENSOR CORES", axis_buf0, axis_buf1, tc)
+
+        s0, s1 = axis_buf0[-1][0], axis_buf1[-1][0] # TODO: select axis in smart way
+        s0_exists, s1_exists = True, True
+        assert s0 != s1 and self.full_shape[s0]%tc.dims[0] == 0 and self.full_shape[s1]%tc.dims[1] == 0
+        def fix(needed, ax):
+          nonlocal s0, s1, s0_exists, s1_exists
+          if not needed: return
+          if s0_exists and ax == s0:
+            if s1_exists and s0 < s1: s1 -= 1
+            s0_exists = False
+          elif s1_exists and ax == s1:
+            if s0_exists and s1 < s0: s0 -= 1
+            s1_exists = False
+
+        # tensor core -- unroll the reduce dim, upcast input, then create the correct thread pattern
+        self.apply_opt(Opt(OptOps.UNROLL, 0, tc.dims[2]))
+        self.apply_opt(Opt(OptOps.UPCAST, s0 if tc.upcast_dim == 0 else s1, (tc.dims[0]*tc.dims[2])//prod([a[1] for a in tc.threads])))
+        for (tc_dim, tc_amt) in tc.threads:
+          fix(self.apply_opt(Opt(OptOps.LASTLOCAL, s0 if tc_dim == 0 else s1, tc_amt)), s0 if tc_dim == 0 else s1)
+
+        # assert tensor core and prevent extra_opts from altering the key shape structure
+        if use_tensor_cores == 1: self.tensor_core = tc # TC=2 will do the shape ops without the WMMA
+
+        if extra_opts is not None:
+          for opt in extra_opts:
+            self.apply_opt(opt)
+        else:
+          # hand-coded TC opts
+          if s1_exists:
+            s1_div = [upc for upc in [5,4,3,2,1] if self.full_shape[s1]%upc == 0][0]
+            if s1_div != 1: fix(self.apply_opt(Opt(OptOps.UPCAST, s1, s1_div)), s1)
+          if s0_exists:
+            s0_div = [upc for upc in [5,4,3,2,1] if self.full_shape[s0]%upc == 0][0]
+            if s0_div != 1: fix(self.apply_opt(Opt(OptOps.UPCAST, s0, s0_div)), s0)
+          if self.tensor_core and s0_exists:
+            for upc in [4,2]:
+              if self.full_shape[s0] % upc == 0:
+                self.apply_opt(Opt(OptOps.LASTLOCAL, s0, upc))
+                break
+
+        # alias buffer
+        alias_pattern = [0]*(self.global_dims+(self.local_dims-len(tc.threads))) + [2]*(len(tc.threads)) + [0]*(self.shape_len-self.upcasted-self.first_reduce) + [1,1] + [3]*(self.upcasted-2)
+        self.alias_buffer(buf0, alias_pattern)
+        self.alias_buffer(buf1, alias_pattern)
+        return True
+    return False
+
+  def apply_opt(self, opt:Opt):
+    assert not self.dont_use_locals or opt.op not in {OptOps.LOCAL, OptOps.LASTLOCAL, OptOps.GROUP, OptOps.GROUPTOP, OptOps.UPCASTMID}, "not using locals"
+    self.applied_opts.append(opt)
+    if opt.axis is not None:
+      axis = opt.axis + (self.first_reduce if opt.op == OptOps.UNROLL else (self.first_reduce+len(self.group_for_reduce) if opt.op == OptOps.GROUP or opt.op == OptOps.GROUPTOP else 0))
+    else:
+      axis = -1
+    if opt.amt is not None:
+      amt = opt.amt if opt.amt != 0 else self.full_shape[axis]
+      assert self.full_shape[axis] % amt == 0, "no longer valid shift"
+      assert isinstance(amt, int) and amt != 1, "shift of amt 1 or Node is meaningless"
+    else:
+      amt = -1
+    if opt.op == OptOps.LOCAL:        # cyan
+      assert axis < self.first_reduce, "can't local a reduce"
+      assert not(self.tensor_core), "can't local with tensor cores"
+      self.shift_to(axis, amt, insert_before=self.first_reduce)
+      self.local_dims += 1
+    elif opt.op == OptOps.LASTLOCAL:  # cyan
+      assert axis < self.first_reduce, "can't local a reduce"
+      self.shift_to(axis, amt, insert_before=self.first_reduce-self.local_dims)
+      self.local_dims += 1
+    elif opt.op == OptOps.GROUP:      # green
+      assert axis >= self.first_reduce + len(self.group_for_reduce) and axis < self.shape_len-self.upcasted, "must be reduce axis to group"
+      assert not(self.tensor_core), "can't group with tensor cores"
+      self.shift_to(axis, amt, insert_before=self.first_reduce + len(self.group_for_reduce))
+      self.group_for_reduce.append(amt)
+    elif opt.op == OptOps.GROUPTOP:   # green
+      assert axis >= self.first_reduce + len(self.group_for_reduce) and axis < self.shape_len-self.upcasted, "must be reduce axis to group"
+      assert not(self.tensor_core), "can't group with tensor cores"
+      self.shift_to(axis, amt, top=True, insert_before=self.first_reduce + len(self.group_for_reduce))
+      self.group_for_reduce.append(amt)
+    elif opt.op == OptOps.UNROLL:     # purple
+      assert axis < self.shape_len-self.upcasted, "can't upcasted already upcasted"
+      assert amt <= 32, "don't unroll more than 32"
+      self.shift_to(axis, amt, insert_before=None)
+      self.upcast()
+    elif opt.op == OptOps.UPCAST:     # yellow
+      assert axis < self.first_reduce, "upcast is for non-reduce"
+      assert amt <= 8, "don't upcast more than 8"
+      self.shift_to(axis, amt, insert_before=None)
+      self.upcast()
+    elif opt.op == OptOps.UPCASTMID:  # white
+      assert self.bufs[0].dtype.name.startswith('image') and not self.float4_axis(0) and self.group_for_reduce and self.first_reduce <= 2 and prod(self.sts[0].shape) > 1, "invalid upcast mid reduce"
+      axes = self.sts[0].unit_stride_axes()
+      assert len(axes) == 1, f"wrong number of stride 1 axis : {axes}"
+      assert axes[0] == axis, "wrong axis"
+      assert amt == 4, "don't upcast mid anything but 4"
+      self.shift_to(axis, amt, insert_before=self.first_reduce + len(self.group_for_reduce))
+      self.group_for_reduce.append(amt)
+    elif opt.op == OptOps.NOLOCALS:
+      assert self.local_dims == 0 and len(self.group_for_reduce) == 0, "can't have no locals with locals"
+      assert not self.dont_use_locals, "already not using locals"
+      self.dont_use_locals = True
+    return self.simplify_ones()
+
+  def required_optimizations(self, early_only=False):
+    for buf_index,buf in enumerate(self.bufs):
+      unit_stride_axes_mul_4 = [i for i in self.sts[buf_index].unit_stride_axes(ignore_valid=True) if self.sts[buf_index].shape[i]%4 == 0]
+      if (not early_only or buf in self.earlybufs) and self.bufs[buf_index].dtype.__class__ is ImageDType:
+        assert len(unit_stride_axes_mul_4) >= 1, f"needs a unit stride axis in {self.bufs[buf_index]}"
+        if all(x < (self.shape_len-self.upcasted) for x in unit_stride_axes_mul_4) and unit_stride_axes_mul_4[0] not in self.upcast_in_mid_reduce_axes:
+          if unit_stride_axes_mul_4[0] < self.first_reduce:
+            self.apply_opt(Opt(OptOps.UPCAST, unit_stride_axes_mul_4[0], 4))
+          else:
+            self.apply_opt(Opt(OptOps.UNROLL, unit_stride_axes_mul_4[0]-self.first_reduce, 4))
+
+  def hand_coded_optimizations(self):
+    # if there's images in the earlybufs, we have to make an axis the 4 loading one
+    self.required_optimizations(early_only=True)
+
+    # should use matvec - TODO: adjust/tune based on the wide vs tall/large vs small mat
+    MV_BLOCKSIZE, MV_THREADS_PER_ROW, MV_ROWS_PER_THREAD = getenv("MV_BLOCKSIZE", 4), getenv("MV_THREADS_PER_ROW", 8), getenv("MV_ROWS_PER_THREAD", 4)
+    if self.opts.has_local and getenv("MV",1) != 0 and (MV_BLOCKSIZE > 1 or MV_THREADS_PER_ROW > 1 or MV_ROWS_PER_THREAD > 1) and  \
+        self.reduceop and self.reduceop.op == ReduceOps.SUM and len(self.full_shape) >= 2 and self.opts.has_shared and \
+        isinstance(self.reduceop.src[0], LazyOp) and self.reduceop.src[0].op == BinaryOps.MUL and \
+        self.reduceop.src[0].src[0].op == BufferOps.MEM and self.reduceop.src[0].src[1].op == BufferOps.MEM:
+      buf0 = self.bufs.index(cast(LazyOp, self.reduceop.src[0].src[0]).arg)
+      buf1 = self.bufs.index(cast(LazyOp, self.reduceop.src[0].src[1]).arg)
+      buf0_strides = self.sts[buf0].real_strides()
+      buf1_strides = self.sts[buf1].real_strides()
+      def has_expanded_axis(s, st): return any(x > 1 and y == 0 for x,y in zip(s,st))
+      if buf0_strides[self.first_reduce] == 1 and not (has_expanded_axis(self.sts[buf0].shape, buf0_strides) and has_expanded_axis(self.sts[buf1].shape, buf1_strides)):
+        for global_idx in range(self.global_dims):
+          if self.full_shape[self.first_reduce]%MV_THREADS_PER_ROW == 0 and self.full_shape[global_idx]%(MV_BLOCKSIZE*MV_ROWS_PER_THREAD) == 0:
+            if DEBUG >= 3: print(f"MATVEC: full_shape={self.full_shape} first_reduce={self.first_reduce} buf0_strides={buf0_strides} blocksize={MV_BLOCKSIZE} threads_per_row={MV_THREADS_PER_ROW} rows_per_thread={MV_ROWS_PER_THREAD}")
+            if MV_THREADS_PER_ROW > 1:
+              self.apply_opt(Opt(OptOps.GROUP, 0, MV_THREADS_PER_ROW))
+            if MV_BLOCKSIZE > 1:
+              self.apply_opt(Opt(OptOps.LOCAL, global_idx, MV_BLOCKSIZE))
+            if MV_ROWS_PER_THREAD > 1:
+              self.apply_opt(Opt(OptOps.UPCAST, global_idx, MV_ROWS_PER_THREAD))
+            return
+
+    if self.opts.has_local and self.opts.has_shared and all(isinstance(s, int) for s in self.sts[0].shape[:self.first_reduce]):
+      # are we grouping? (requires local shape support)
+      if not self.float4_axis(0) and self.first_reduce <= 2 and self.first_reduce + 1 <= self.shape_len and prod(self.sts[0].shape[:self.first_reduce]) <= 2048:
+        # TODO: use 1024 if it's allowed in a smarter way
+        for sz in (([256, 16]) if prod(self.sts[0].shape[:self.first_reduce]) <= 32 else [16]):
+          if all(st.shape[self.first_reduce] % sz == 0 or st.shape[self.first_reduce] == 1 for st in self.sts):
+            self.apply_opt(Opt(OptOps.GROUPTOP, 0, sz))
+            break
+
+      # are we upcasting in mid reduce? (only for images)
+      if self.bufs[0].dtype.name.startswith('image') and not self.float4_axis(0) and self.group_for_reduce and self.first_reduce <= 2 and prod(self.sts[0].shape) > 1:
+        axes = self.sts[0].unit_stride_axes()
+        assert len(axes) == 1, f"wrong number of stride 1 axis : {axes}"
+        if self.sts[0].shape[axes[0]]%4 == 0:
+          self.apply_opt(Opt(OptOps.UPCASTMID, axes[0], 4))
+
+    # now do everything required
+    self.required_optimizations()
+
+    # no more opt if we are grouping
+    if self.group_for_reduce: return
+
+    # **** below this line need to be optional and benchmarked ****
+
+    # TODO: doing extra upcasts with images doesn't work for some reason (maybe has to do with to_image_idx)
+    # to trigger the above bug, remove prod(self.full_shape[self.shape_len - self.upcasted:]) from the below
+    # expression and run test/test_ops.py with IMAGE=2
+    # if there are small dims with lots of valid masks, upcast them (they might be from Tensor.stack)
+    # this can be made much smarter
+    to_upcast: List[int] = []
+    # upcast leading axes first (hack-ish for winograd; we actually want to upcast masked axes with low stride first)
+    for axis in range(self.first_reduce):
+      # we might want to be able to split axes that are masked, or refuse to merge them in simplify_merge_adjacent
+      # for now skip upcasting here if there is a symbolic axis
+      if isinstance(self.full_shape[axis], int) and self.full_shape[axis] <= 7 and any(st.axis_is_masked(axis) for st in self.sts) and \
+        prod(self.full_shape[self.shape_len - self.upcasted:]) * prod(self.full_shape[j] for j in to_upcast) * self.full_shape[axis] <= 7 * 7:
+        if DEBUG >= 4: print(f"upcasting masked axis : {axis}")
+        to_upcast.append(axis)
+    for axis in to_upcast[::-1]:
+      self.apply_opt(Opt(OptOps.UPCAST, axis, 0))
+
+    # potentially do more upcasts of non reduce axes based on a heuristic
+    upcasted_axis = set()
+    while prod(self.sts[0].shape[:self.first_reduce]) >= 1024:
+      xb_choices = []
+      for axis, upcast_amount in itertools.product(range(self.first_reduce), [3,4]):   # consider all the non reduce axes, and a 3 or 4 reduce
+        # if we haven't upcasted it, it's not symbolic, it mods, and some buffer has stride 0 on axis while having no stride 0 in the upcasted axis already
+        if axis not in upcasted_axis and isinstance(self.full_shape[axis], int) and self.full_shape[axis]%upcast_amount == 0 and any(st.views[-1].strides[axis] == 0 and not any(x[1] == 0 for x in self.upcasted_axis(buf_index)) for buf_index, st in enumerate(self.sts)):
+          xb_choices.append((sum(st.views[-1].strides[axis]>0 for st in self.sts), sum(st.views[-1].strides[axis] for st in self.sts), axis, upcast_amount))
+      if xb_choices:
+        xb_choices = sorted(xb_choices)
+        if DEBUG >= 4: print(f"float4 merging axis : {xb_choices}")
+        self.apply_opt(Opt(OptOps.UPCAST, xb_choices[0][2], xb_choices[0][3]))
+        upcasted_axis.add(xb_choices[0][2])
+      else:
+        break
+
+    # if last dim is small(ish) and it's a reduce dim, upcast the reduce (loop unrolling). no simplify needed since it's just an upcast. NOTE: careful, this has broken VALIDHACKS
+    if self.first_reduce < (self.shape_len-self.upcasted) and (len(list(self.shape_offsets(self.full_buf_index))) <= 4 or not any(r for _,_,r in self.upcasted_axis(self.full_buf_index))) and (self.upcasted == 0 or prod(self.full_shape[-self.upcasted:]) < 64):
+      if (s:=self.full_unupcasted_shape[-1]) <= 32 and isinstance(s, int):  # NOTE: cannot loop unroll symbolic axis
+        self.apply_opt(Opt(OptOps.UNROLL, len(self.full_unupcasted_shape)-1-self.first_reduce, 0))
+        # if it's small, upcast a second reduce dimension too
+        if self.first_reduce < (self.shape_len-self.upcasted) and s <= 3 and (s2:=self.full_unupcasted_shape[-1]) <= 3 and isinstance(s2, int):
+          self.apply_opt(Opt(OptOps.UNROLL, len(self.full_unupcasted_shape)-1-self.first_reduce, 0))
+      else:
+        for splits in [4]:
+          if self.full_unupcasted_shape[-1]%splits == 0:
+            self.apply_opt(Opt(OptOps.UNROLL, len(self.full_unupcasted_shape)-1-self.first_reduce, splits))
+            break
+
+    # if nothing at all is upcasted and it's easy to, do an upcast
+    # TODO: this is breaking the tests
+    for splits in [4]:
+      if self.upcasted == 0 and self.full_unupcasted_shape and self.full_unupcasted_shape[-1] % splits == 0:
+        self.apply_opt(Opt(OptOps.UPCAST, len(self.full_unupcasted_shape)-1, splits))
+
+    # **** local groups ****
+
+    if self.opts.has_local:
+      if getenv("NOLOCALS") and self.local_dims == 0 and not self.group_for_reduce:
+        self.apply_opt(Opt(OptOps.NOLOCALS))
+      else:
+        # prioritize making expand axes local
+        local_axis_ranking = [(any(self.sts[buf_index].views[-1].strides[axis] == 0 for buf_index in range(len(self.sts))), axis) for axis in range(len(self.full_shape[:self.first_reduce]))]
+        to_local: List[Tuple[int, int]] = []
+        for _, axis in sorted(local_axis_ranking, key=lambda x: (-x[0], -x[1])):
+          local_size = prod(sz for _, sz in to_local)
+          local_sz: Optional[int] = next((x for x in ([32] * (axis == 0) + [16, 8, 4, 3, 2]) if self.full_shape[axis] % x == 0 and local_size * x <= 128), None)
+          if local_sz is not None: to_local.append((axis, local_sz))
+        deleted_shape = 0
+        for axis, local_sz in sorted(to_local[:3]):
+          axis = axis - deleted_shape
+          will_delete_shape = local_sz == self.full_shape[axis]
+          self.apply_opt(Opt(OptOps.LOCAL, axis, local_sz))
+          if will_delete_shape: deleted_shape += 1
diff --git a/tinygrad_repo/tinygrad/codegen/linearizer.py b/tinygrad_repo/tinygrad/codegen/linearizer.py
new file mode 100644
index 0000000000..08880352a0
--- /dev/null
+++ b/tinygrad_repo/tinygrad/codegen/linearizer.py
@@ -0,0 +1,441 @@
+from __future__ import annotations
+from typing import List, Tuple, Any, Optional, cast, DefaultDict, NamedTuple, Dict, Union, Sequence, Final, Set
+import itertools, math, functools
+from collections import defaultdict
+from enum import Enum, auto
+
+from tinygrad.helpers import colored, ImageDType, DEBUG, dtypes, DType, prod, PtrDType, all_same
+from tinygrad.ops import LazyOp, UnaryOps, ConstBuffer, MemBuffer, BufferOps
+from tinygrad.ops import ReduceOps, BinaryOps, TernaryOps
+from tinygrad.shape.shapetracker import ShapeTracker
+from tinygrad.shape.symbolic import Variable, NumNode, VariableOrNum, Node, SumNode, MulNode, DivNode, ModNode, LtNode, AndNode, sym_rename
+from tinygrad.codegen.kernel import LocalBuffer, Kernel
+from tinygrad.lazy import vars_from_ast
+from tinygrad.features.image import to_image_idx
+
+# bottom ones are asm only
+class UOps(Enum):
+  LOOP = auto(); IF = auto(); END = auto(); SPECIAL = auto() # loops can be global, local, or other # noqa: E702
+  DEFINE_GLOBAL = auto(); DEFINE_LOCAL = auto(); DEFINE_ACC = auto() # this defines buffers # noqa: E702
+  LOAD = auto(); STORE = auto(); CONST = auto(); BARRIER = auto(); PHI = auto() # noqa: E702
+  ALU = auto(); WMMA = auto(); CAST = auto(); GEP = auto() # noqa: E702
+
+class UOp(NamedTuple):
+  uop: UOps
+  dtype: Optional[DType]
+  vin: Tuple[UOp, ...]
+  arg: Any
+  def __repr__(self): return f"{self.num:4d} {str(self.uop):20s}: {str(self.dtype) if self.dtype is not None else '':25s} {str([x.num for x in self.vin]):32s} {self.arg}"
+  #def __repr__(self): return f"{str(self.uop):20s}: {str(self.dtype) if self.dtype is not None else '':25s} {str(self.vin):32s} {self.arg}"
+
+  # UOps are unique
+  num: int
+  def __hash__(self): return self.num
+  def __eq__(self, x): return self.num == x.num
+
+def get_grouped_dims(prefix, start_dim, local_dims, maxdim:int=0):
+  local_idxs = loop_local_idxs = [Variable(f"{prefix}{start_dim+i}", 0, s-1) for i,s in enumerate(local_dims[0:maxdim-1] + (prod(local_dims[maxdim-1:]),) if len(local_dims) > maxdim else local_dims)]
+  if maxdim != 0 and len(local_dims) > maxdim:
+    dd = local_idxs[maxdim-1]
+    nli = []
+    for s in local_dims[maxdim-1:][::-1]:
+      nli.append(dd % s)
+      dd //= s
+    local_idxs = local_idxs[0:maxdim-1] + nli[::-1]
+  return local_idxs, [x for x in loop_local_idxs if not isinstance(x, NumNode)]
+
+class Linearizer(Kernel):
+  def uop_alu_idx(self, a:UOp, b, ops, ctx:Linearizer, op, dtype=dtypes.int32):
+    render_b:UOp = cast(UOp, (NumNode(b) if not isinstance(b, Node) else b).render(ops, ctx))
+    return self.uop(UOps.ALU, dtype, (a, render_b), op)
+
+  # NOTE: the consts have to be be cached for deduping of downstream uops to work
+  def const(self, b:Union[int,float], dtype=dtypes.int32) -> UOp: return self.uop(UOps.CONST, dtype, tuple(), b)
+
+  render_ops: Any = { Variable: lambda self, ops, ctx: ctx.loop_uops[self.expr], NumNode: lambda self, ops, ctx: ctx.const(self.b),
+                MulNode: lambda self, ops, ctx: ctx.uop_alu_idx(self.a.render(ops, ctx), self.b, ops, ctx, BinaryOps.MUL),
+                DivNode: lambda self, ops, ctx: ctx.uop_alu_idx(self.a.render(ops, ctx), self.b, ops, ctx, BinaryOps.DIV),
+                ModNode: lambda self, ops, ctx: ctx.uop_alu_idx(self.a.render(ops, ctx), self.b, ops, ctx, BinaryOps.MOD),
+                LtNode: lambda self, ops, ctx: ctx.uop_alu_idx(self.a.render(ops, ctx), self.b, ops, ctx, BinaryOps.CMPLT, dtype=dtypes.bool),
+    SumNode: lambda self,ops,ctx: functools.reduce(lambda a,b: ctx.uop_alu_idx(a, b, ops, ctx, BinaryOps.ADD), self.nodes[1:], self.nodes[0].render(ops,ctx)),
+    AndNode: lambda self,ops,ctx: functools.reduce(lambda a,b: ctx.uop_alu_idx(a, b, ops, ctx, BinaryOps.MUL, dtype=dtypes.bool), self.nodes[1:], self.nodes[0].render(ops,ctx)) }
+
+  def global_load(self, i:int, idxs:Sequence[Node], acc=None) -> List[UOp]:
+    buf = self.bufs[i]
+    const = buf.val if isinstance(buf, ConstBuffer) else acc
+
+    def rename_var(v: VariableOrNum, expr: str): return v if isinstance(v, NumNode) else Variable(expr, v.min, v.max)
+
+    amt, dim = 1, None
+    upcast_dim = self.get_upcast_dim(i)
+    if len(upcast_dim) == 1 and len(float4_expand := idxs[upcast_dim[0]].expand()) in [4,2]:
+      dim, amt = upcast_dim[0], len(float4_expand)
+
+    expand_vars = tuple([rename_var(idx.expand_idx(), f"_uidx{j}") for j, idx in enumerate(idxs)])
+    fake_idxs = [idx.substitute({idx.expand_idx(): ev}) for idx, ev in zip(idxs, expand_vars)]
+    if dim is not None:
+      g_idx, g_valid = self.sts[i].expr_idxs(fake_idxs[:dim] + [float4_expand[0]] + fake_idxs[dim+1:])
+      if (g_idx // amt * amt).render() != g_idx.render():
+        (g_idx, g_valid), amt, dim = self.sts[i].expr_idxs(fake_idxs), 1, None
+    else:
+      g_idx, g_valid = self.sts[i].expr_idxs(fake_idxs)
+    localtype = dtypes.float32 if amt == 1 else dtypes._float4 if amt == 4 else dtypes._float2
+
+    e_idxs, e_valids = g_idx.expand(expand_vars), g_valid.expand(expand_vars)
+
+    ret = []
+    invalid_value = 0 if dtypes.is_int(buf.dtype) else 0.0
+    for idx, valid, rep_idx in zip(e_idxs, e_valids, Node.iter_idxs(expand_vars)):
+      this_const, idx, valid = (invalid_value, Variable.num(0), Variable.num(1)) if valid.max == 0 else (const, idx, valid)
+      key = f"{acc}{localtype}{this_const if this_const is not None and acc is None else (buf.idx if isinstance(buf, MemBuffer) else cast(LocalBuffer, buf).name)}{idx.render()}{valid.render()}"
+      if key not in self.load_cache:
+        if acc is not None:
+          assert valid.min == 1
+          self.load_cache[key] = self.uop(UOps.DEFINE_ACC, localtype, (), this_const, cachable=False)
+        elif this_const is not None:
+          self.load_cache[key] = self.const(this_const, localtype)
+          if valid.min == 0 and valid.max == 1:
+            valid_rendered = valid.render(self.render_ops, self)
+            self.load_cache[key] = self.uop(UOps.ALU, localtype, (valid_rendered, self.load_cache[key], self.const(invalid_value, localtype)), TernaryOps.WHERE)
+        else:
+          buf_uop = self.buf_uops[i]
+          assert buf_uop is not None, f"buffer {i} wasn't UOped"
+          if isinstance(buf.dtype, ImageDType):
+            idx, valid = to_image_idx(buf.dtype.shape, idx, valid)
+            rendered_idx = self.uop(UOps.CAST, dtypes._int2, (idx[0].render(self.render_ops, self), idx[1].render(self.render_ops, self)))
+          else:
+            rendered_idx = idx.render(self.render_ops, self)
+
+          if valid.min == 0:
+            valid_rendered = valid.render(self.render_ops, self)
+            self.load_cache[key] = self.uop(UOps.LOAD, localtype, (buf_uop, rendered_idx, valid_rendered, self.const(invalid_value, localtype)))
+          else:
+            self.load_cache[key] = self.uop(UOps.LOAD, localtype, (buf_uop, rendered_idx))
+      ret.append(self.uop(UOps.GEP, dtypes.float32, (self.load_cache[key],), rep_idx[dim]) if dim is not None else self.load_cache[key])
+    return ret
+
+  def global_store(self, i:int, idxs:List[Node], store:List[UOp]) -> None:
+    buf = self.bufs[i]
+    buf_uop = self.buf_uops[i]
+    assert buf_uop is not None, f"buffer {i} wasn't UOped"
+
+    expanded_nodes = [idx.expand() for idx in idxs]
+    _idxs = [x[::-1] for x in itertools.product(*expanded_nodes[::-1])]
+    store_offset = dict(zip(_idxs, store))
+
+    # float4 grouping
+    upcast_dim = self.get_upcast_dim(i)
+    if len(upcast_dim) == 1 and len(expanded_nodes[upcast_dim[0]]) in [2,4]:
+      grouped_store_offset = defaultdict(list)
+      for k in store_offset:
+        _idx = k[:upcast_dim[0]] + (expanded_nodes[upcast_dim[0]][0],) + k[upcast_dim[0]+1:]
+        grouped_store_offset[_idx].append(store_offset[k])
+      store_offset_new = {}
+      for k,out_tokens in grouped_store_offset.items():
+        amt = len(out_tokens)
+        idx, valid = self.sts[i].expr_idxs(k)
+        assert idx.render() == ((idx//amt)*amt).render(), "float4 stores are always aligned"
+        assert valid.min == 1, "stores are always valid"
+        store_offset_new[k] = self.uop(UOps.CAST, dtypes._float4 if amt == 4 else dtypes._float2, tuple(out_tokens))
+      store_offset = store_offset_new
+
+    for idx, var in store_offset.items():
+      idx, valid = self.sts[i].expr_idxs(idx)
+      if isinstance(buf.dtype, ImageDType):
+        idx, valid = to_image_idx(buf.dtype.shape, idx, valid)
+        rendered_idx = self.uop(UOps.CAST, dtypes._int2, tuple(x.render(self.render_ops, self) for x in idx))
+      else:
+        rendered_idx = idx.render(self.render_ops, self)
+      self.uop(UOps.STORE, None, (buf_uop, rendered_idx, var))
+
+  kernel_cnt: Final[DefaultDict[str, int]] = defaultdict(int)
+  def linearize(self):
+    # no new opts and we already ran? skip relinearizing
+    if self.applied_opts == self.applied_opts_cache: return self
+
+    # save backups
+    sts_backup, gfr_backup, upc_backup = self.sts[:], self.group_for_reduce[:], self.upcasted
+
+    # global uop cache
+    self.saved_exprs: Dict[Tuple, UOp] = dict()
+
+    # limit dims if we need to
+    if self.opts.global_max and self.opts.local_max: self.limit_dims_to_max(self.opts.global_max, self.opts.local_max)
+
+    # uops
+    self.uops: List[UOp] = []
+    self.buf_uops: List[Optional[UOp]] = [None]*len(self.bufs)
+    self.loop_uops: Dict[str, UOp] = {}
+
+    # add global buffers
+    for i,buf in enumerate(self.bufs):
+      if isinstance(buf, MemBuffer):
+        self.buf_uops[i] = self.uop(UOps.DEFINE_GLOBAL, PtrDType(buf.dtype) if not isinstance(buf.dtype, ImageDType) else buf.dtype, (), (f"data{buf.idx}", buf.dtype))
+    # add var vals
+    for var in sorted(vars_from_ast(self.ast), key=lambda k: k.key):
+      assert var.expr is not None
+      self.loop_uops[var.expr] = self.uop(UOps.DEFINE_GLOBAL, dtypes.int32, (), (var.expr, dtypes._arg_int32))
+    # define local buffers
+    for lb in self.local_alias.values():
+      self.buf_uops[self.bufs.index(lb)] = self.uop(UOps.DEFINE_LOCAL, PtrDType(dtypes.float32), (), (lb.name, self.sts[self.bufs.index(lb)].size()))
+    # add a local buffer for multistage reduce. # TODO: use local alias
+    if self.group_for_reduce:
+      # TODO: the strides of this can be controlled
+      self.sts.append(ShapeTracker.from_shape(tuple([1] * self.global_dims + list(self.full_shape[self.global_dims:self.global_dims+self.local_dims+len(self.group_for_reduce)]) + [1] * (self.shape_len - self.upcasted - len(self.group_for_reduce) - self.first_reduce) + [x[0] for x in self.upcasted_axis(0)])))
+      self.bufs.append(LocalBuffer("temp", self.sts[-1].size()))
+      self.buf_uops.append(self.uop(UOps.DEFINE_LOCAL, PtrDType(dtypes.float32), (), ("temp", self.sts[-1].size())))
+
+    # kernel name (before late upcast)
+    self.function_name = ("r_" if self.reduceop else "E_") + '_'.join([str(x) if isinstance(x, int) else sym_rename(x) for x in self.full_shape])
+    self.display_name = ("r_" if self.reduceop else "E_") + colored('_', 'BLACK').join([colored(str(x), c) for x,c in zip(self.full_shape, self.colors())])
+
+    # name the function something unique
+    Linearizer.kernel_cnt[self.function_name] += 1
+    suffix = f"{'n'+str(Linearizer.kernel_cnt[self.function_name]-1)}" if Linearizer.kernel_cnt[self.function_name] > 1 else ""
+    self.function_name, self.display_name = self.function_name+suffix, self.display_name+colored(suffix, 'BLACK')
+
+    # define indexes
+    global_idxs, loop_global_idxs = get_grouped_dims("gidx", 0, self.full_shape[:self.global_dims], 3 if self.opts.has_local else 0)
+    local_idxs, loop_local_idxs = get_grouped_dims("lidx", self.global_dims, self.full_shape[self.global_dims:self.first_reduce+len(self.group_for_reduce)], 3 if self.opts.has_local else 0)
+    full_upcast_idxs = [Variable(None, 0, s-1) for s in self.full_shape[self.shape_len-self.upcasted:]]
+    upcast_idxs = [Variable(None, 0, s-1) for s in self.output_shape[self.shape_len-self.upcasted:]]
+
+    # global and local loops
+    def render_loop(xx:List[Variable]):
+      self.loop_uops.update({x.expr:self.uop(UOps.LOOP, dtypes.int32, (
+        self.const(x.min) if isinstance(x.min, int) else cast(Node, x.min).render(self.render_ops, self),
+        self.const(x.max+1) if isinstance(x.max, int) else cast(Node, x.max+1).render(self.render_ops, self)), cachable=False) for x in xx if not isinstance(x, NumNode) and x.expr is not None})
+    def end_loop(xx:List[Variable]):
+      for x in xx[::-1]:
+        if not isinstance(x, NumNode) and x.expr is not None:
+          loop_uop = self.loop_uops[x.expr]
+          if loop_uop.uop == UOps.LOOP: self.uop(UOps.END, None, (loop_uop,))
+
+    # set global/local size
+    self.global_size: Optional[List[int]] = None
+    self.local_size: Optional[List[int]] = None
+    if self.dont_use_locals:
+      self.global_size = [x.max+1 for x in loop_global_idxs][::-1]
+      self.loop_uops.update({x.expr:self.uop(UOps.SPECIAL, dtypes.int32, (), (len(loop_global_idxs)-1-i, x.expr.replace("gidx", "idx"), x.max+1)) for i,x in enumerate(loop_global_idxs)})
+    elif self.opts.has_local:
+      self.global_size, self.local_size = [x.max+1 for x in loop_global_idxs][::-1], [x.max+1 for x in loop_local_idxs][::-1]
+      self.global_size += [1]*(3-len(self.global_size))
+      self.local_size += [1]*(3-len(self.local_size))
+      self.loop_uops.update({x.expr:self.uop(UOps.SPECIAL, dtypes.int32, (), (len(loop_global_idxs)-1-i, x.expr, x.max+1)) for i,x in enumerate(loop_global_idxs)})
+      self.loop_uops.update({x.expr:self.uop(UOps.SPECIAL, dtypes.int32, (), (len(loop_local_idxs)-1-i, x.expr, x.max+1)) for i,x in enumerate(loop_local_idxs)})
+    else:
+      render_loop(loop_global_idxs+loop_local_idxs)
+
+    # parse AST
+    loaded_buffers = {}
+    acc = []
+    self.load_cache: Dict[str, UOp] = {}
+    if_gate: Optional[UOp] = None
+
+    # reduce op
+    fake_reduce_idxs: List[Variable] = []
+    if self.reduceop is not None:
+      # define indexes
+      reduce_idxs = [Variable(f"ridx{i}", 0, self.full_shape[i]-1) for i in range(self.first_reduce+len(self.group_for_reduce), self.shape_len-self.upcasted)]
+      fake_reduce_idxs = [x*0 for x in reduce_idxs]
+
+      # define accumulator
+      acc = self.global_load(0, global_idxs+local_idxs+fake_reduce_idxs+upcast_idxs, {ReduceOps.SUM: 0.0, ReduceOps.MAX: -math.inf}[cast(ReduceOps, self.reduceop.op)])
+
+      if self.tensor_core:
+        def calc_tc_idxs(local_size: int, aliases: List[List[int]]):
+          replace_idxs = []
+          for alias in aliases:
+            full_var, full_var_sz = Variable.num(0), 1
+            if alias[0] != 0:
+              for i in alias:
+                next_var = local_idxs[-i] if i > 0 else Variable(None, 0, local_size-1)
+                full_var += next_var * full_var_sz
+                full_var_sz *= next_var.max+1
+            replace_idxs.append(full_var)
+          return replace_idxs
+        replace_acc_idxs = calc_tc_idxs(self.tensor_core.thread_local_sizes[2], self.tensor_core.thread_local_aliases[2])
+        for n in range(len(self.tensor_core.threads)):
+          local_idxs[self.local_dims-len(self.tensor_core.threads)+n] = replace_acc_idxs[n] # replace locals
+        for n in range(len(replace_acc_idxs)-len(self.tensor_core.threads)):
+          upcast_idxs[n] = replace_acc_idxs[len(self.tensor_core.threads)+n] # replace upcasts
+
+      # reduce loop
+      render_loop(reduce_idxs)
+
+      # barrier for fast GEMM
+      if self.tensor_core: self.uop(UOps.BARRIER, None, (), cachable=False)
+
+      # compute local aliases
+      locals_to_store = []
+      for i in self.local_alias:
+        localbuf_idx = self.bufs.index(self.local_alias[i])
+        buf_idxs = [idx*0 if s == 0 else idx for idx,s in zip(global_idxs+local_idxs+reduce_idxs+full_upcast_idxs,self.sts[i].real_strides())]
+        if self.tensor_core:
+          min_alias_idx = min(self.local_alias.keys())
+          replace_input_idxs = calc_tc_idxs(self.tensor_core.thread_local_sizes[i-min_alias_idx], self.tensor_core.thread_local_aliases[i-min_alias_idx])
+          for n in range(len(self.tensor_core.threads)):
+            buf_idxs[self.first_reduce-len(self.tensor_core.threads)+n] = replace_input_idxs[n] # replace locals
+          for n in range(len(replace_input_idxs)-len(self.tensor_core.threads)):
+            buf_idxs[self.shape_len-self.upcasted+n] = replace_input_idxs[len(self.tensor_core.threads)+n] # replace upcasts
+        if DEBUG >= 3: print(f"{localbuf_idx} alias {i}: idxs=", buf_idxs)
+        ll = self.global_load(i, buf_idxs)
+        locals_to_store.append((localbuf_idx, buf_idxs, ll))
+
+      # copy in any global buffers
+      if self.tensor_core:
+        wmma_sz = self.tensor_core.thread_local_sizes
+        # calculate the number of local accumulator reduces and render WMMAs: this is bad... this needs to come from someplace else
+        nx, ny, nacc = (len(locals_to_store[0][2])//wmma_sz[0]), (len(locals_to_store[1][2])//wmma_sz[1]), (len(acc)//wmma_sz[2])
+        acc_reds = math.isqrt((nx*ny)//nacc)
+        i, bx, by = 0, nx//acc_reds, ny//acc_reds
+        for y in range(by):
+          for x in range(bx):
+            for j in range(acc_reds):
+              self.uop(UOps.WMMA, None, tuple(locals_to_store[0][2][(x+(j*bx))*wmma_sz[0]:(x+(j*bx)+1)*wmma_sz[0]]+locals_to_store[1][2][(y+(j*by))*wmma_sz[1]:(y+(j*by)+1)*wmma_sz[1]]+acc[i:i+wmma_sz[2]]), (self.opts.device, self.tensor_core.dtype_in, self.tensor_core.dtype_out,))
+            i += wmma_sz[2]
+      else:
+        if locals_to_store:
+          self.uop(UOps.BARRIER, None, (), cachable=False)
+          for i, idxs, ll in locals_to_store: self.global_store(i, idxs, ll)
+          self.uop(UOps.BARRIER, None, (), cachable=False)
+
+        # load earlybufs
+        loaded_buffers.update({b:self.global_load(self.bufs.index(self.local_alias[i]) if i in self.local_alias else i, global_idxs+local_idxs+reduce_idxs+full_upcast_idxs) for i,b in enumerate(self.bufs[1:], start=1) if b in self.earlybufs})
+
+        # run early AST (with reduce)
+        self.ast_parse(self.reduceop, acc, self.acc_offsets(self.full_buf_index), loaded_buffers, do_reduce=True)
+
+      # end the reduce loop
+      end_loop(reduce_idxs)
+      self.load_cache.clear()
+
+      # end the local loop, do the local reduce
+      if self.group_for_reduce:
+        fake_global_idxs = [x*0 for x in global_idxs]
+        self.global_store(-1, fake_global_idxs+local_idxs+fake_reduce_idxs+upcast_idxs, acc)  # store accumulators
+        self.uop(UOps.BARRIER, None, (), cachable=False)
+        end_loop(loop_local_idxs)  # TODO: this is ending too much, should only end what's in the if?
+        if self.opts.has_local:
+          fake_idxs = [Variable.num(0)]*len(self.sts[-1].shape)
+          fake_idxs[self.global_dims+self.local_dims:self.global_dims+len(local_idxs)] = local_idxs[self.local_dims:]
+          if_cond: UOp = (self.sts[-1].expr_idxs(fake_idxs)[0]<1).render(self.render_ops, self)
+          if_gate = self.uop(UOps.IF, None, (if_cond,), cachable=False)
+
+        # create new late reduce local loops and replace local_idxs that have been used
+        end_local_idxs = [Variable(f"tidx{i}", 0, self.full_shape[i]-1 if i >= self.first_reduce and i not in self.upcast_in_mid_reduce_axes else 0) for i in range(0, self.first_reduce+len(self.group_for_reduce))]
+        local_idxs = local_idxs[:self.local_dims] + end_local_idxs[self.global_dims + self.local_dims:]
+
+        # if any group_for_reduce items aren't reduces, upcast them here
+        for j in self.upcast_in_mid_reduce_axes:
+          self.reshape_and_permute(None, [i for i in range(self.shape_len) if i != j] + [j])
+          self.upcast()
+          self.group_for_reduce.pop()
+          local_idxs = local_idxs[:-1]
+          end_local_idxs = end_local_idxs[:-1]
+          # regenerate upcast_idxs
+          upcast_idxs = [Variable(None, 0, s-1) for s in self.output_shape[self.shape_len-self.upcasted:]]
+
+        # NOTE: this structure is the same as the reduce op above
+
+        # define late accumulator
+        acc = self.global_load(-1, fake_global_idxs+local_idxs+fake_reduce_idxs+upcast_idxs, {ReduceOps.SUM: 0.0, ReduceOps.MAX: -math.inf}[cast(ReduceOps, self.reduceop.op)])
+
+        # late reduce loop
+        render_loop(end_local_idxs)
+
+        # load localbufs
+        loaded_buffers[self.bufs[-1]] = self.global_load(-1, fake_global_idxs+local_idxs+fake_reduce_idxs+upcast_idxs)
+
+        # there's no AST here (and there's no shape for the reduce LazyOp)
+        self.ast_parse(LazyOp(self.reduceop.op, (self.bufs[-1],)), acc, self.acc_offsets(-1), loaded_buffers, do_reduce=True) # type: ignore
+
+        # end the late reduce loop
+        end_loop(end_local_idxs)
+        self.load_cache.clear()
+
+    # load latebufs
+    loaded_buffers.update({b:self.global_load(i, global_idxs+local_idxs+fake_reduce_idxs+upcast_idxs) for i,b in enumerate(self.bufs) if b not in self.earlybufs and i != 0 and b.__class__ is not LocalBuffer})
+
+    # run late AST
+    val = self.ast_parse(self.ast, acc, None, loaded_buffers)
+
+    # store
+    self.global_store(0, global_idxs+local_idxs+fake_reduce_idxs+upcast_idxs, val)
+
+    # end the global (and maybe local) loop
+    if if_gate: self.uop(UOps.END, None, (if_gate,))
+    end_loop(loop_global_idxs+loop_local_idxs if not self.group_for_reduce else loop_global_idxs)
+
+    # (recursively) remove childless uops
+    UOPS_W_SIDE_EFFECTS = {UOps.STORE, UOps.WMMA, UOps.END, UOps.BARRIER, UOps.DEFINE_GLOBAL}
+    while 1:
+      has_child: Set[UOp] = set()
+      for ru in self.uops:
+        for vu in ru.vin:
+          has_child.add(vu)
+      nu: List[UOp] = [x for x in self.uops if x in has_child or x.uop in UOPS_W_SIDE_EFFECTS]
+      if len(nu) == len(self.uops): break
+      if DEBUG >= 4: print(f"reduced UOp count from {len(self.uops)} to {len(nu)}")
+      self.uops = nu
+
+    # restore backups
+    self.sts, self.group_for_reduce, self.upcasted = sts_backup, gfr_backup, upc_backup
+
+    # set cache and return
+    self.applied_opts_cache = self.applied_opts[:]
+    return self
+
+  def uop(self, uop:UOps, dtype:Optional[DType], vin:Tuple[UOp, ...], arg:Any=None, cachable=True) -> UOp:
+    key = (uop, dtype, vin, arg)
+    if uop == UOps.PHI and len(vin) == 2 and vin[0] == vin[1]: return vin[0]   # self phi is noop
+    if uop == UOps.CAST and all(x.uop == UOps.GEP for x in vin) and all_same([x.vin[0] for x in vin]) and all(x.arg == i for i,x in enumerate(vin)): return vin[0].vin[0]
+    if uop == UOps.GEP and vin[0].uop == UOps.CONST: return self.const(vin[0].arg, dtype)
+    if uop == UOps.ALU:
+      # rewrites. NOTE: the rewritten NEG op is still around...
+      if arg == BinaryOps.ADD and vin[1].uop == UOps.ALU and vin[1].arg == UnaryOps.NEG: return self.uop(UOps.ALU, dtype, (vin[0], vin[1].vin[0]), BinaryOps.SUB, cachable=cachable)
+      # constant folding
+      if arg == UnaryOps.NEG and vin[0].uop == UOps.CONST: return self.const(-vin[0].arg, dtype)
+      # zero folding
+      for x in [0,1]:
+        if arg == BinaryOps.ADD and vin[x].uop == UOps.CONST and vin[x].arg == 0.0: return vin[1-x]
+        if arg == BinaryOps.MUL and vin[x].uop == UOps.CONST and vin[x].arg == 1.0: return vin[1-x]
+        if arg == BinaryOps.MUL and vin[x].uop == UOps.CONST and vin[x].arg == 0.0: return vin[x]
+      if arg == BinaryOps.SUB and vin[1].uop == UOps.CONST and vin[1].arg == 0.0: return vin[0]
+      if arg == BinaryOps.DIV and vin[1].uop == UOps.CONST and vin[1].arg == 1.0: return vin[0]
+    if cachable and key in self.saved_exprs: return self.saved_exprs[key]
+    self.uops.append(UOp(uop, dtype, vin, arg, len(self.uops)))
+    if DEBUG >= 5: print(self.uops[-1])
+    if cachable: self.saved_exprs[key] = self.uops[-1]
+    return self.uops[-1]
+
+  def ast_parse(self, x, acc, offs, loaded_buffers, do_reduce=False) -> List[UOp]:
+    if x.__class__ is not LazyOp: return loaded_buffers[x]    # for LOCAL_BUFFER
+    if x.op in BufferOps: return loaded_buffers[x.arg]
+    if x.op in [UnaryOps.NOOP, UnaryOps.CAST]: return self.ast_parse(x.src[0], acc, offs, loaded_buffers)  # cast isn't an ALU op
+    if x.op in ReduceOps and not do_reduce:
+      assert offs is None, "not available if we aren't doing reduce"
+      return acc
+    # MULACC fusion. TODO: this is copied from Interpreted
+    if x.op == ReduceOps.SUM and x.src[0].__class__ is LazyOp and x.src[0].op == BinaryOps.MUL:
+      x = LazyOp(TernaryOps.MULACC, x.src[0].src, x.arg)
+    if x.op == ReduceOps.SUM and x.src[0].__class__ is LazyOp and x.src[0].op == UnaryOps.CAST and x.src[0].src[0].__class__ is LazyOp and x.src[0].src[0].op == BinaryOps.MUL:
+      x = LazyOp(TernaryOps.MULACC, x.src[0].src[0].src, x.arg)
+    values = [self.ast_parse(v, acc, offs, loaded_buffers) for v in x.src]
+    ops = {ReduceOps.SUM:BinaryOps.ADD, ReduceOps.MAX:BinaryOps.MAX, TernaryOps.MULACC:TernaryOps.MULACC}
+    if x.op in ops:
+      ret = []
+      for idx, val, off in zip([[i] for i in range(len(values[0]))], zip(*values), offs):
+        new_val = self.uop(UOps.ALU, dtypes.float32, val+(acc[off],), ops[x.op])
+        # NOTE: we could apply the phi node to only the last change, but this breaks CLANG with nested max(x,y)
+        acc[off] = self.uop(UOps.PHI, dtypes.float32, (acc[off], new_val))
+        ret.append((idx, acc[off]))
+    else:
+      ret = [(idx, self.uop(UOps.ALU, dtypes.float32, val, x.op)) for idx, val in zip([[i] for i in range(len(values[0]))], zip(*values))]
+    ordered_ret: List[Optional[UOp]] = [None]*len(values[0])
+    # scatter
+    for i,j in ret:
+      for k in i:
+        ordered_ret[k] = j
+    assert all(isinstance(x, UOp) for x in ordered_ret), "some tokens didn't get scattered?"
+    return cast(List[UOp], ordered_ret)
diff --git a/tinygrad_repo/tinygrad/features/image.py b/tinygrad_repo/tinygrad/features/image.py
new file mode 100644
index 0000000000..7f75a124f0
--- /dev/null
+++ b/tinygrad_repo/tinygrad/features/image.py
@@ -0,0 +1,204 @@
+from typing import List, Tuple, Dict, Any
+from tinygrad.helpers import ImageDType, prod, IMAGE, getenv, dtypes, DEBUG, flatten
+
+# *** image Tensor function replacements ***
+
+from tinygrad.lazy import get_single_root
+
+def image_dot(self, w):
+  # NOTE: we use a 1x1 conv2d to do the matmul. mxk @ kxn = (1,k,m,1).conv2d(n,k,1,1)
+  n1, n2 = len(self.shape), len(w.shape)
+  assert n1 != 0 and n2 != 0, f"both arguments to matmul need to be at least 1D, but they are {n1}D and {n2}D"
+  assert self.shape[-1] == w.shape[-min(n2, 2)], f"Input Tensor shapes {self.shape} and {w.shape} cannot be multiplied ({self.shape[-1]} != {w.shape[-min(n2, 2)]})"
+  bs, groups = prod(self.shape[0:-2]), prod(w.shape[0:-2])
+  cin, cout = w.shape[-2], w.shape[-1]
+  out_shape_t = self.shape[0:-2] + (cout,-1)
+  if len(self.shape) > 1:
+    order = tuple(range(len(self.shape)-2)) + (len(self.shape)-1, len(self.shape)-2)
+  else:
+    order, out_shape_t = (0,), (cout, )
+  worder = tuple(range(len(w.shape)-2)) + (len(w.shape)-1, len(w.shape)-2)
+
+  # NOTE: with NHWC we can remove the transposes
+  # bs x groups*cin x H x W
+  cx = self.permute(order=order).reshape(shape=(bs//groups, groups*cin, -1, 1))
+  # groups*cout x cin x H, W
+  cw = w.permute(order=worder).reshape(shape=(groups*cout, cin, 1, 1))
+  return image_conv2d(cx, cw, groups=groups).reshape(shape=out_shape_t).permute(order=order)
+
+def image_conv2d(self, weight, bias=None, groups=1, stride=1, dilation=1, padding=0):
+  base_image_type = dtypes.imageh if getenv("FLOAT16", 0) else dtypes.imagef
+
+  (bs,_,iy,ix), (cout,cin,H,W) = self.shape, weight.shape
+  rcout = cout//groups
+  x, w = self, weight.reshape(groups, rcout, cin, H, W)
+
+  # hack for non multiples of 4 on cin
+  if cin % 4 != 0 and not (cin == 1 and groups%4 == 0):
+    x = x.reshape(bs, groups, cin, iy, ix)   # do this always?
+    added_input_channels = 4 - (cin % 4)
+    w = w.pad(tuple((0, added_input_channels) if i == 2 else (0, 0) for i in range(len(w.shape))))
+    x = x.pad(tuple((0, added_input_channels) if i == 2 else (0, 0) for i in range(len(x.shape))))
+    cin = cin + added_input_channels
+    x = x.reshape(bs, groups*cin, iy, ix)
+
+  # hack for non multiples of 4 on rcout
+  added_output_channels = 0
+  if rcout % 4 != 0 and not (rcout == 1 and groups%4 == 0):
+    added_output_channels = 4 - (rcout % 4)
+    rcout += added_output_channels
+    cout = groups * rcout
+    w = w.slice(tuple((0, rcout) if i == 1 else (0, s) for i,s in enumerate(w.shape)))
+
+  # packed (note: flipping bs and iy would make the auto-padding work)
+  x = x.permute(0,2,3,1)
+  cin_last = iy == 1 and ix == 1
+  if cin == 1: w = w.reshape(cout//4,4,H,W).permute(0,2,3,1)
+  elif cin_last: w = w.reshape(cout//4,4,cin//4,4,H,W).permute(0,4,2,5,1,3)
+  else: w = w.reshape(cout//4,4,cin//4,4,H,W).permute(0,4,2,5,3,1)
+
+  # contiguous creates the image, and early realize static weights (TODO: test for the static weight)
+  if IMAGE >= 2: x,w = x.cast(base_image_type((bs*iy, ix*groups*cin//4, 4))), w.cast(base_image_type((cout//4, H*W*cin, 4)))
+  x, w = x.contiguous(), w.contiguous()
+  if getenv("PREREALIZE", 1) and get_single_root(w.lazydata).realized: w.realize()
+
+  # expand out
+  rcin_hi, rcin_lo = cin//4 if cin >= 4 else 1, 4 if cin >= 4 else 1
+  cout_expand = [groups//4 if cin == 1 else groups, 4 if cin == 1 else 1, rcout//4 if rcout >= 4 else 1, 4 if rcout >= 4 else 1]
+  x = x.reshape(bs, iy, ix, groups, rcin_hi, rcin_lo)
+  if cin_last: w = w.reshape(cout//4, H, rcin_hi, W, 4, rcin_lo)
+  else: w = w.reshape(cout//4, H, rcin_hi, W, rcin_lo, 4).permute(0,1,2,3,5,4)
+
+  # padding
+  padding_ = [padding]*4 if isinstance(padding, int) else (padding if len(padding) == 4 else [padding[1], padding[1], padding[0], padding[0]])
+  x = x.slice((None, (-padding_[2], x.shape[1]+padding_[3]), (-padding_[0], x.shape[2]+padding_[1]), None, None, None))
+
+  # prepare input
+  x = x.permute(0,3,4,5,1,2)._pool((H, W), stride, dilation) # -> (bs, groups, rcin_hi, rcin_lo, oy, ox, H, W)
+  oy, ox = x.shape[4:6]
+  x = x.permute(0,4,5,1,2,3,6,7).reshape(bs, oy, ox, *cout_expand[0:2], 1, 1, rcin_hi, rcin_lo, H, W)
+  x = x.expand(bs, oy, ox, *cout_expand, rcin_hi, rcin_lo, H, W)
+
+  # prepare weights
+  w = w.permute(0,4,2,5,1,3)
+  w = w.reshape((1, 1, 1, *cout_expand, rcin_hi, rcin_lo, H, W)).expand(x.shape)
+
+  # the conv! (+ the bias)
+  ret = x*w
+  if IMAGE >= 2: ret = ret.cast(base_image_type((bs*oy, ox*cout//4, 4)))
+  ret = ret.sum((-4, -3, -2, -1))
+
+  # undo hack for non multiples of 4 on C.rcout
+  if added_output_channels != 0:
+    ret = ret.reshape(bs, oy, ox, groups, rcout)[:, :, :, :, :-added_output_channels]
+    rcout -= added_output_channels
+    cout = groups * rcout
+
+  # NCHW output
+  ret = ret.reshape(bs, oy, ox, cout).permute(0,3,1,2)
+  return ret if bias is None else ret.add(bias.reshape(1, -1, 1, 1))
+
+# *** schedules with images need to be fixed to be valid ***
+
+import dataclasses
+from tinygrad.ops import ScheduleItem, BufferOps, LazyOp, UnaryOps, LoadOps, MemBuffer, get_lazyop_info
+
+def fix_schedule_for_images(schedule:List[ScheduleItem]):
+  # this is the fundamental fix, find unwritable or unreadable images and convert them to normal float32 (TODO: should it be float16?)
+  replace_inputs = {}
+  for i, si in enumerate(schedule):
+    if isinstance(si.out.dtype, ImageDType) and (prod(si.out.shape) != prod(si.out.dtype.shape) or not any(si.out.shape[x]%4 == 0 for x in si.out.st.unit_stride_axes())):
+      if DEBUG >= 1: print(f"{i:3d}: rewrite output, output shape {prod(si.out.shape)}, image dtype {si.out.dtype} prod {prod(si.out.dtype.shape)}")
+      si.out.dtype = dtypes.float32
+    for b in si.ast.get_lazyops():
+      if b.op != BufferOps.MEM: continue
+      # TODO: unit_stride axes will fail if there's a mask, even if the mask is divisble by four. this is too aggressive
+      if isinstance(si.inputs[b.arg.idx-1].dtype, ImageDType) and (b.arg.st.real_offset() % 4 != 0 or not any(b.arg.st.shape[x]%4 == 0 for x in b.arg.st.unit_stride_axes())):
+        if DEBUG >= 1: print(f"{i:3d}: rewrite input, image dtype {si.inputs[b.arg.idx-1].dtype}, {b.arg.st.views}")
+        if si.inputs[b.arg.idx-1].realized:
+          # have to copy it
+          replace_inputs[si.inputs[b.arg.idx-1]] = si.inputs[b.arg.idx-1].cast(dtypes.float32)
+        else:
+          # change it before it's created
+          si.inputs[b.arg.idx-1].dtype = dtypes.float32
+
+  # now fix up the schedule to reflect the new dtypes
+  fixed_schedule:List[ScheduleItem] = []
+  for i,si in enumerate(schedule):
+    ast = si.ast
+    inputs = si.inputs
+
+    # replace inputs with casted versions
+    if any(x in replace_inputs for x in inputs):
+      fixed_schedule += flatten([replace_inputs[x].schedule() for x in inputs if x in replace_inputs])
+      inputs = tuple(replace_inputs.get(x, x) for x in inputs)
+
+    # fix input dtypes to match what they actually are
+    replacements = {}
+    for b in si.ast.get_lazyops():
+      if b.op != BufferOps.MEM: continue
+      if b.arg.dtype != inputs[b.arg.idx-1].dtype:
+        replacements[b] = LazyOp(BufferOps.MEM, (), MemBuffer(b.arg.idx, inputs[b.arg.idx-1].dtype, b.arg.st))
+    if replacements: ast = ast.map_buffers(replacements)
+
+    # fix the ops to create the output dtype
+    if ast.op not in LoadOps:
+      info = get_lazyop_info(ast)
+      if info.dtype != si.out.dtype:
+        if DEBUG >= 3: print(f"{i:3d}: info.dtype {info.dtype} != {si.out.dtype} -> {si.out.dtype}")
+        ast = LazyOp(UnaryOps.CAST, (ast,), (si.out.dtype, False))
+
+    # put this in the fixed schedule
+    fixed_schedule.append(dataclasses.replace(si, ast=ast, inputs=inputs))
+  return fixed_schedule
+
+# *** images have weird indexing requirements ***
+
+from tinygrad.shape.symbolic import Node, AndNode, Variable, NumNode, SumNode, LtNode
+
+def to_image_idx(base_shape:Tuple[int, ...], idxy:Node, valid:Node) -> Tuple[Tuple[Node, Node], Node]:
+  idx = (idxy // 4) % base_shape[1]
+  idy = (idxy // (4 * base_shape[1]))
+
+  if valid.min == 0 and isinstance(idxy, SumNode):
+    nodes = valid.nodes if isinstance(valid, AndNode) else [valid]
+    val_dict: Dict[Node, Any] = {}
+    idxy_flat_var = [(i, i.vars()[0]) for i in idxy.flat_components if not isinstance(i, NumNode)]
+
+    for node in nodes:
+      assert isinstance(node, LtNode)
+      node_flat, node_vars = node.a.flat_components if isinstance(node.a, SumNode) else [node.a], node.vars()
+      same_sym = [i for (i, var) in idxy_flat_var if var in node_vars]
+      if len(same_sym) == 0: continue
+      first, second = sorted(same_sym)[0], sorted(node_flat)[0]
+      f_b = 1 if isinstance(first, Variable) else first.b
+      s_b = 1 if isinstance(second, Variable) else second.b
+      sig = -1 if s_b < 0 else 1
+      key_node = sig*node.a
+      if key_node not in val_dict: val_dict[key_node] = [key_node.min, key_node.max, abs(f_b//s_b)]
+      val_dict[key_node][(sig + 1)//2] = sig*(node.b - 1)
+
+    fakes = {}
+    for cnt, (key_node, (mnn, mxn, multip)) in enumerate(val_dict.items()):
+      fake_var = Variable("fake_" + str(cnt), mnn, mxn)
+      fakes[fake_var] = key_node
+      idxy += multip*(fake_var - key_node)
+
+    idx = (idxy // 4) % base_shape[1]
+    idy = (idxy // (4 * base_shape[1]))
+
+    fake_rep = {fake: node for fake, node in fakes.items()}
+
+    idx = idx.substitute(fake_rep)
+    idy = idy.substitute(fake_rep)
+
+    idy_vars, idx_vars, ones = set(idy.vars()), set(idx.vars()), []
+    for node in nodes:
+      node_vars = set(node.vars())
+      if not node_vars & (idx_vars | idy_vars): continue #There is simplified NumNode which can not go outside the bounds
+      # NOTE: Why does only idy is problematic? and not the idx
+      if idy_vars == node_vars or idy_vars & node_vars == set(): ones.append(node)
+    valid = Variable.ands([i for i in nodes if i not in ones])
+
+  if DEBUG>=5: print("to_image_idx", base_shape, idx.min, idx.max, idy.min, idy.max, idx, idy, valid)
+  return (idx, idy), valid
diff --git a/tinygrad_repo/tinygrad/features/search.py b/tinygrad_repo/tinygrad/features/search.py
new file mode 100644
index 0000000000..c48a3de482
--- /dev/null
+++ b/tinygrad_repo/tinygrad/features/search.py
@@ -0,0 +1,151 @@
+from typing import Dict, List, cast, DefaultDict, Optional, Tuple, Callable
+import itertools, random
+from tinygrad.lazy import vars_from_ast
+from tinygrad.ops import Device, Compiled, MemBuffer
+from tinygrad.helpers import prod, ImageDType, flatten, DEBUG, CACHELEVEL, diskcache_get, diskcache_put, getenv, Context
+from tinygrad.codegen.linearizer import Linearizer
+from tinygrad.runtime.lib import RawBuffer
+from collections import defaultdict
+from tinygrad.tensor import Tensor
+
+from tinygrad.codegen.kernel import Opt, OptOps
+actions = flatten([[Opt(op=OptOps.UPCAST, axis=axis, amt=amt) for amt in [0,2,3,4,7]] for axis in range(6)])
+actions += flatten([[Opt(op=OptOps.UNROLL, axis=axis, amt=amt) for amt in [0,4]] for axis in range(4)])
+actions += flatten([[Opt(op=OptOps.LOCAL, axis=axis, amt=amt) for amt in [2,3,4,8,13,16,29]] for axis in range(5)])
+actions += flatten([[Opt(op=OptOps.GROUPTOP, axis=axis, amt=amt) for amt in [13,16,29,32,256]] for axis in range(3)])
+actions += [
+  Opt(op=OptOps.LOCAL, axis=0, amt=32),
+  Opt(op=OptOps.GROUP, axis=0, amt=4), Opt(op=OptOps.GROUP, axis=0, amt=8), Opt(op=OptOps.GROUP, axis=1, amt=8),
+  Opt(op=OptOps.UPCASTMID, axis=1, amt=4),
+  Opt(op=OptOps.NOLOCALS),
+]
+
+# returns time in seconds
+def time_linearizer(lin:Linearizer, rawbufs:List[RawBuffer], allow_test_size=True, max_global_size=65536, cnt=3, disable_cache=False, clear_l2=False) -> float:
+  key = {"ast": str(lin.ast), "opts": str(lin.applied_opts), "allow_test_size": allow_test_size, "max_global_size": max_global_size}
+  if not disable_cache and CACHELEVEL >= 2 and (val:=diskcache_get("time_linearizer", key)) is not None: return min(val)
+  var_vals = {k:k.min for k in vars_from_ast(lin.ast)}
+  try:
+    lin.linearize()
+    prg = cast(Compiled, Device[Device.DEFAULT]).to_program(lin)
+    real_global_size = prg.global_size
+    if allow_test_size and prg.global_size:
+      test_global_size = prg.global_size[:]
+      while prod(test_global_size) > max_global_size:
+        for j in range(2,-1,-1):
+          if test_global_size[j] > 16:
+            test_global_size[j] //= 2
+            break
+      factor = prod(prg.global_size) / prod(test_global_size)
+      prg.global_size = test_global_size
+      #print(real_global_size, test_global_size, factor)
+    else:
+      factor = 1
+    # TODO: this is super broken for var_vals
+    # TODO: this is copied from prg.__call__
+    global_size, local_size = prg.launch_dims(var_vals)
+    if global_size is not None and local_size is None:
+      local_size = prg.optimize_local_size(global_size, rawbufs)
+      global_size = [g//l if g%l == 0 else g/l for g,l in zip(global_size, local_size)]
+    tms = []
+    for _ in range(cnt):
+      if clear_l2:
+        # TODO: this is too small for many L2 caches
+        with Context(DEBUG=0): Tensor.rand(1024,1024).realize()
+      lra = prg.runtime_args.copy()
+      if global_size: lra['global_size'] = global_size
+      if local_size: lra['local_size'] = local_size
+      tms.append(prg.clprg(*rawbufs, *var_vals.values(), **lra, wait=True)*factor)
+    prg.global_size = real_global_size
+  except Exception:
+    if DEBUG >= 4:
+      import traceback
+      traceback.print_exc()
+      print("FAILED")
+      print(lin.ast)
+      print(lin.applied_opts)
+    tms = [float('inf')]
+  if CACHELEVEL >= 2: diskcache_put("time_linearizer", key, tms)
+  return min(tms)
+
+# get (scrap) buffers for timing the linearizer
+def bufs_from_lin(lin:Linearizer) -> List[RawBuffer]:
+  bufsts:DefaultDict[int, List[MemBuffer]] = defaultdict(list)
+  for x in lin.membufs: bufsts[x.idx].append(x)
+  rawbufs:List[Optional[RawBuffer]] = [None]*len(bufsts)
+  for k,lx in bufsts.items():
+    rawbufs[k] = cast(Compiled, Device[Device.DEFAULT]).buffer(prod(lx[0].dtype.shape) if isinstance(lx[0].dtype, ImageDType) else max(y.st.size() for y in lx), lx[0].dtype)
+  assert all(r is not None for r in rawbufs)
+  return cast(List[RawBuffer], rawbufs)
+
+# get dictionary of all possible actions
+def get_linearizer_actions(lin:Linearizer, include_0=True) -> Dict[int, Linearizer]:
+  acted_lins = {0:lin} if include_0 else {}
+  for i,a in enumerate(actions):
+    if a.axis is not None and a.axis >= lin.shape_len: continue
+    if a.axis is not None and lin.full_shape[a.axis] == a.amt and Opt(a.op, a.axis, 0) in actions: continue
+    lin2 = lin.copy()
+    try:
+      lin2.apply_opt(a)
+      up, lcl = 1, 1
+      for s,c in zip(lin2.full_shape, lin2.colors()):
+        if c in {"magenta", "yellow"}: up *= s
+        if c in {"cyan", "green", "white"}: lcl *= s
+      if up > 256 or lcl > 256: continue
+      acted_lins[i+1] = lin2
+    except Exception:
+      pass
+  return acted_lins
+
+def beam_search(lin:Linearizer, rawbufs, amt:int, allow_test_size=True) -> Linearizer:
+  key = {"ast": str(lin.ast), "amt": amt, "allow_test_size": allow_test_size}
+  if (val:=diskcache_get("beam_search", key)) is not None and not getenv("IGNORE_BEAM_CACHE") and CACHELEVEL >= 1:
+    ret = lin.copy()
+    for o in val[len(lin.applied_opts):]: ret.apply_opt(o)
+    return ret
+
+  # init the BEAM with the base linearizer
+  beam: List[Tuple[Linearizer, float]] = [(lin, time_linearizer(lin, rawbufs, allow_test_size=allow_test_size))]
+
+  # NOTE: real uops use a weird compare method that's only valid inside a linearizer
+  def tuplize_uops(uops): return tuple([(x.uop, x.dtype, tuple(x.num for x in x.vin), x.arg) for x in uops])
+  seen_uops = {tuplize_uops(lin.linearize().uops): tuple(lin.applied_opts)}
+
+  while 1:
+    acted_lins = lins = flatten([get_linearizer_actions(lin, include_0=False).values() for lin,_ in beam])
+
+    # dedup with uops (TODO: double linearize not needed)
+    acted_lins_dedup = []
+    for lin in acted_lins:
+      tuops = tuplize_uops(lin.linearize().uops)
+      if tuops in seen_uops:
+        #print(seen_uops[tuops], lin.applied_opts)
+        continue
+      seen_uops[tuops] = tuple(lin.applied_opts)
+      acted_lins_dedup.append(lin)
+    acted_lins = acted_lins_dedup
+
+    # time linearizers
+    timed_lins: List[Tuple[Linearizer, float]] = [(v,time_linearizer(v,rawbufs,allow_test_size=allow_test_size)) for v in acted_lins]
+    opts = sorted(timed_lins, key=lambda x: x[1])
+    if len(opts) == 0 or beam[0][1] <= opts[0][1]: break  # we didn't get faster
+
+    # keep the BEAM best
+    beam = opts[:amt]
+    if DEBUG >= 2: print(f"{opts[0][1]*1e6:12.2f} us from {len(lins):3d} -> {len(opts):3d} actions", beam[0][0].colored_shape())
+
+  if CACHELEVEL >= 1: diskcache_put("beam_search", key, beam[0][0].applied_opts)
+  if DEBUG >= 3: print(beam[0][0].applied_opts)
+  return beam[0][0]
+
+def optimize_local_size(clprg:Callable, global_size:List[int], rawbufs:List[RawBuffer]) -> List[int]:
+  test_rawbuffers = [type(rawbufs[0])(rawbufs[0].size, rawbufs[0].dtype), *rawbufs[1:]] if rawbufs[0] in rawbufs[1:] else rawbufs
+  MAX_WORKGROUP = clprg.max_work_group_size() if hasattr(clprg, 'max_work_group_size') else 1024
+  local_dims = [[x for x in set([sz, 1, 2, 4, 8, 16, 32, 64, 128, 256, MAX_WORKGROUP]) if x<=sz] for sz in global_size]
+  local_sizes = [list(x) for x in itertools.product(*local_dims) if prod(x) <= MAX_WORKGROUP] * 2  # try each valid size twice
+  def try_exec(local_size):
+    try:
+      return clprg(*test_rawbuffers, global_size=[g//l if g%l == 0 else g/l for g,l in zip(global_size, local_size)], local_size=local_size, wait=True)
+    except Exception:
+      return float('inf')
+  return min([(try_exec(local_size), local_size) for local_size in random.sample(local_sizes, len(local_sizes))])[1]
diff --git a/tinygrad_repo/tinygrad/graph.py b/tinygrad_repo/tinygrad/graph.py
index d86bc72285..e332e5f127 100644
--- a/tinygrad_repo/tinygrad/graph.py
+++ b/tinygrad_repo/tinygrad/graph.py
@@ -1,23 +1,27 @@
-import os, atexit, itertools
+import os, atexit, functools
 try:
   import networkx as nx  # type: ignore
 except ImportError:
   nx = None # graph won't work
 from collections import defaultdict
-from typing import Dict, List, Optional
-from tinygrad.ops import DeviceBuffer, UnaryOps, BinaryOps, ReduceOps, MovementOps, LoadOps, FusedOps, Op, OpType, LazyOp, get_buffers, get_lazyops
-from tinygrad.helpers import getenv, DEBUG
-
-GRAPH, PRUNEGRAPH, GRAPHPATH = getenv("GRAPH", 0), getenv("PRUNEGRAPH", 0), getenv("GRAPHPATH", "/tmp/net")
+from typing import Dict, List
+from tinygrad.ops import ScheduleItem, UnaryOps, BinaryOps, ReduceOps, MovementOps, LoadOps, BufferOps, TernaryOps, Op, OpType, LazyOp
+from tinygrad.helpers import GRAPH, GRAPHPATH, DEBUG, GlobalCounters, getenv, dedup
+from tinygrad.codegen.linearizer import UOps
 
 # **** debugging and graphing ****
 
 G = nx.DiGraph() if nx is not None else None
-cnts : Dict[OpType, int] = defaultdict(int)
+cnts: Dict[OpType, int] = defaultdict(int)
+if DEBUG >= 2:
+  def print_globalcounters():
+    if GlobalCounters.time_sum_s == 0: return
+    print(f"avg: {GlobalCounters.global_ops*1e-9/GlobalCounters.time_sum_s:8.2f} GFLOPS {GlobalCounters.global_mem*1e-9/GlobalCounters.time_sum_s:8.2f} GB/s",
+          f"{' '*10}total: {GlobalCounters.kernel_count:5d} kernels {GlobalCounters.global_ops*1e-9:8.2f} GOPS {GlobalCounters.global_mem*1e-9:8.2f} GB {GlobalCounters.time_sum_s*1e3:8.2f} ms")
+  atexit.register(print_globalcounters)
 if GRAPH:
   def save_graph_exit():
     for k,v in cnts.items(): print(k, v)
-    if PRUNEGRAPH: prune_graph()
     print("saving", G)
     nx.drawing.nx_pydot.write_dot(G, f'{GRAPHPATH}.dot')
     # -Gnslimit=100 can make it finish, but you won't like results
@@ -32,43 +36,82 @@ def nm(x):
     node_count += 1
   return x.node_id
 
-def get_sop(op : List[Op]):
+def get_sop(op: List[Op]):
+  op = [x for x in op if x not in BufferOps]
   if len(op) <= 2: return '.'.join([str(y).split(".")[1] for y in op][::-1])
-  if len(op) <= 4: return '.'.join([str(y).split(".")[1][0:2] for y in op][::-1])
+  if len(op) <= 6: return '.'.join([str(y).split(".")[1][0:3] for y in op][::-1])
   return str(len(op))
 
-def log_op(ret : DeviceBuffer, ast : LazyOp, show_graph : Optional[bool] = None):
-  if show_graph is None: show_graph = GRAPH
+def str_dtype(dtyp):
+  ret = str(dtyp)[7:]
+  return "" if ret == 'float' else f"\n{ret}"
+
+@functools.lru_cache(None)
+def add_st_node(nmx, nmo, label, st):
+  global node_count
+  inter_node = node_count
+  node_count += 1
+  G.add_node(inter_node, style='filled', fillcolor="#80ff8080", color="black", label=f"{st.shape}\n{st.real_strides()}" + (f"\n{st.real_offset()}" if st.real_offset() != 0 else ""))
+  G.add_edge(nmx, inter_node, color='#00000060')
+  G.add_edge(inter_node, nmo, label=label, color='#00000060')
+
+logops = open(getenv("LOGOPS", ""),"a") if getenv("LOGOPS", "") else None
+def log_schedule_item(si: ScheduleItem):
+  if logops and si.ast.op not in LoadOps: logops.write(str(si.ast)+"\n")
+  show_graph = bool(GRAPH)
   if not DEBUG and not show_graph: return
-  op : List[Op] = [x.op for x in get_lazyops(ast)]
-  inp : List[DeviceBuffer] = get_buffers(ast)
-  if len(inp) == 1 and inp[0] == ret:
-    if show_graph and nm(ret) in G.nodes: G.nodes[nm(ret)]['style'] += ', bold'
-    return   # don't log self loops
-  oporder = [LoadOps, FusedOps, ReduceOps, BinaryOps, UnaryOps, MovementOps]
+  if si.ast.op == LoadOps.CONTIGUOUS: setattr(si.out, 'node_id', nm(si.inputs[0].base))
+  if si.ast.op in {LoadOps.CONST, LoadOps.CONTIGUOUS}: return
+
+  op: List[Op] = [x.op for x in si.ast.get_lazyops()]
+  oporder = [LoadOps, TernaryOps, ReduceOps, BinaryOps, UnaryOps, MovementOps, BufferOps]
   optype = type(sorted(op, key=lambda x: oporder.index(type(x)))[0])
   cnts[optype] += 1
-  if DEBUG >= 4: print(f"{op} : {', '.join([f'{x.shape}-<{nm(x)}>' for x in inp])} -> {ret.shape}-<{nm(ret)}>")
   if show_graph:
-    top_colors = {LoadOps: '#FFFF80', UnaryOps: "#c0c0c0", ReduceOps: "#8080ff", BinaryOps: "#c0c0c0", MovementOps: "#80ff80", FusedOps: "#ff8080"}
-    dashed = (optype == LoadOps and hasattr(ret, "_backing")) or (hasattr(ret, "st") and not ret.st.contiguous)  # type: ignore
+    assert si.out.base == si.out, "all outputs based"
+    top_colors = {LoadOps: '#FFFFa0', UnaryOps: "#c0c0c0", ReduceOps: "#8080ff", BinaryOps: "#c0c0c0", MovementOps: "#80ff80", TernaryOps: "#c0c0c0", BufferOps: '#FF8080'}
 
-    for x in inp:
-      G.add_edge(nm(x), nm(ret), label=get_sop(op))
+    # get inputs for shapetrackers
+    input_to_st = defaultdict(list)
+    for lo in si.ast.get_lazyops():
+      if lo.op != BufferOps.MEM: continue
+      input_to_st[si.inputs[lo.arg.idx-1]].append(lo.arg.st)
+
+    # add them to the graph, potentially with a movement op seperating them
+    for x in input_to_st:
+      for st in dedup(input_to_st[x]):
+        if st.contiguous:
+          G.add_edge(nm(x), nm(si.out), label=get_sop(op), color='#00000060')
+        else:
+          add_st_node(nm(x), nm(si.out), get_sop(op), st)
       if 'label' not in G.nodes[nm(x)]:
-        G.nodes[nm(x)]['label'] = str(x.shape)
-    if nm(ret) not in G.nodes: G.add_node(nm(ret))
+        G.nodes[nm(x)]['label'] = str(x.shape)+str_dtype(si.out.dtype)
+
+    if nm(si.out) not in G.nodes: G.add_node(nm(si.out))
+
+    G.nodes[nm(si.out)]['label'] = (str(set(x.shape for x in si.inputs))+"\n"+str(si.out.shape) if optype == ReduceOps else str(si.out.shape))+str_dtype(si.out.dtype)+(f"\n{si.ast.op}" if si.ast.op in LoadOps else "")
+    G.nodes[nm(si.out)]['fillcolor'] = top_colors[optype]
+    G.nodes[nm(si.out)]['color'] = 'black'
+    G.nodes[nm(si.out)]['style'] = 'filled'
+
+def _tree(lazydata, prefix=""):
+  if type(lazydata).__name__ == "LazyBuffer": return [f"━━ realized {lazydata.dtype.name} {lazydata.shape}"] if (lazydata.realized) else _tree(lazydata.op, "LB ")
+  if len(lazydata.src) == 0: return [f"━━ {prefix}{lazydata.op.name} {lazydata.arg if lazydata.arg else ''}"]
+  lines = [f"━┳ {prefix}{lazydata.op.name} {lazydata.arg if lazydata.arg else ''}"]
+  childs = [_tree(c) for c in lazydata.src[:]]
+  for c in childs[:-1]: lines += [f" ┣{c[0]}"] + [f" ┃{l}" for l in c[1:]]
+  return lines + [" ┗"+childs[-1][0]] + ["  "+l for l in childs[-1][1:]]
 
-    G.nodes[nm(ret)]['label'] = str(set(x.shape for x in inp))+"\n"+str(ret.shape) if optype == ReduceOps else str(ret.shape)
-    G.nodes[nm(ret)]['fillcolor'] = (top_colors[optype] + ('80' if dashed else str())) if optype in top_colors else "#ffffff"
-    G.nodes[nm(ret)]['style'] = 'filled, dashed' if dashed else 'filled'
-    G.nodes[nm(ret)]['prunable'] = optype in [LoadOps, MovementOps]
+def print_tree(lazydata:LazyOp): print("\n".join([f"{str(i).rjust(3)} {s}" for i,s in enumerate(_tree(lazydata))]))
 
-# prune movementops and loadops
-def prune_graph():
-  dead_nodes = []
-  for n in G.nodes:
-    if 'prunable' in G.nodes[n] and G.nodes[n]['prunable']:
-      G.add_edges_from([(x, y) for (x,_),(_,y) in itertools.product(G.in_edges(n), G.out_edges(n))])
-      dead_nodes.append(n)
-  G.remove_nodes_from(dead_nodes)
+def graph_uops(uops):
+  colors = {UOps.ALU: "#ffffc0", UOps.LOAD: "#ffc0c0", UOps.STORE: "#c0ffc0", UOps.SPECIAL: "#c0c0ff", UOps.CONST: "#e0e0e0",
+            UOps.DEFINE_GLOBAL: "#ffe0b0", UOps.DEFINE_LOCAL: "#ffe0d0", UOps.DEFINE_ACC: "#f0ffe0",
+            UOps.LOOP: "#c8a0e0", UOps.PHI: "#e0ffc0"}
+  G = nx.DiGraph()
+  for u in uops:
+    G.add_node(u.num, label=f"{str(u.uop)[5:]}{(' '+str(u.arg)) if u.arg is not None else ''}\n{str(u.dtype)}", style="filled", fillcolor=colors.get(u.uop, "#ffffff"))
+    for v in u.vin: G.add_edge(v.num, u.num)
+  GRAPHPATH = "/tmp/uops"
+  nx.drawing.nx_pydot.write_dot(G, f'{GRAPHPATH}.dot')
+  os.system(f'dot -Grankdir=LR -Tsvg {GRAPHPATH}.dot -o {GRAPHPATH}.svg')
diff --git a/tinygrad_repo/tinygrad/helpers.py b/tinygrad_repo/tinygrad/helpers.py
index 006b376ff5..f676991944 100644
--- a/tinygrad_repo/tinygrad/helpers.py
+++ b/tinygrad_repo/tinygrad/helpers.py
@@ -1,18 +1,208 @@
-import os, math, functools
-from typing import Tuple, Union, List
+from __future__ import annotations
+import os, functools, platform, time, re, contextlib, operator, hashlib, pickle, sqlite3
+import numpy as np
+from typing import Dict, Tuple, Union, List, NamedTuple, Final, Iterator, ClassVar, Optional, Iterable, Any, TypeVar, TYPE_CHECKING
+if TYPE_CHECKING:  # TODO: remove this and import TypeGuard from typing once minimum python supported version is 3.10
+  from typing_extensions import TypeGuard
+
+T = TypeVar("T")
+# NOTE: it returns int 1 if x is empty regardless of the type of x
+def prod(x:Iterable[T]) -> Union[T,int]: return functools.reduce(operator.__mul__, x, 1)
+
+# NOTE: helpers is not allowed to import from anything else in tinygrad
+OSX = platform.system() == "Darwin"
+CI = os.getenv("CI", "") != ""
 
 def dedup(x): return list(dict.fromkeys(x))   # retains list order
-def prod(x:Union[List[int], Tuple[int, ...]]) -> int: return math.prod(x)
-def argfix(*x): return tuple() if len(x) == 0 else tuple(x[0]) if isinstance(x[0], (tuple, list)) else tuple(x)
-def argsort(x): return sorted(range(len(x)), key=x.__getitem__) # https://stackoverflow.com/questions/3382352/equivalent-of-numpy-argsort-in-basic-python
-def all_same(items): return all(x == items[0] for x in items) if len(items) > 0 else True
-def colored(st, color, background=False, bright=False): return f"\u001b[{10*background+60*bright+30+['black', 'red', 'green', 'yellow', 'blue', 'magenta', 'cyan', 'white'].index(color)}m{st}\u001b[0m"  # replace the termcolor library with one line
-def partition(lst, fxn): return [x for x in lst if fxn(x)], [x for x in lst if not fxn(x)]
+def argfix(*x): return tuple(x[0]) if x and x[0].__class__ in (tuple, list) else x
+def argsort(x): return type(x)(sorted(range(len(x)), key=x.__getitem__)) # https://stackoverflow.com/questions/3382352/equivalent-of-numpy-argsort-in-basic-python
+def all_same(items): return all(x == items[0] for x in items)
+def all_int(t: Tuple[Any, ...]) -> TypeGuard[Tuple[int, ...]]: return all(isinstance(s, int) for s in t)
+def colored(st, color, background=False): return f"\u001b[{10*background+60*(color.upper() == color)+30+['black', 'red', 'green', 'yellow', 'blue', 'magenta', 'cyan', 'white'].index(color.lower())}m{st}\u001b[0m" if color is not None else st  # replace the termcolor library with one line
+def ansistrip(s): return re.sub('\x1b\\[(K|.*?m)', '', s)
+def ansilen(s): return len(ansistrip(s))
 def make_pair(x:Union[int, Tuple[int, ...]], cnt=2) -> Tuple[int, ...]: return (x,)*cnt if isinstance(x, int) else x
-def flatten(l): return [item for sublist in l for item in sublist]
-def mnum(i) -> str: return str(i) if i >= 0 else f"m{-i}"
+def flatten(l:Union[List, Iterator]): return [item for sublist in l for item in sublist]
+def fromimport(mod, frm): return getattr(__import__(mod, fromlist=[frm]), frm)
+def strip_parens(fst): return fst[1:-1] if fst[0] == '(' and fst[-1] == ')' and fst[1:-1].find('(') <= fst[1:-1].find(')') else fst
+def merge_dicts(ds:Iterable[Dict]) -> Dict:
+  assert len(kvs:=set([(k,v) for d in ds for k,v in d.items()])) == len(set(kv[0] for kv in kvs)), f"cannot merge, {kvs} contains different values for the same key"
+  return {k:v for d in ds for k,v in d.items()}
+def partition(lst, fxn):
+  a: list[Any] = []
+  b: list[Any] = []
+  for s in lst: (a if fxn(s) else b).append(s)
+  return a,b
 
 @functools.lru_cache(maxsize=None)
 def getenv(key, default=0): return type(default)(os.getenv(key, default))
 
-DEBUG, IMAGE = getenv("DEBUG", 0), getenv("IMAGE", 0)
+class Context(contextlib.ContextDecorator):
+  stack: ClassVar[List[dict[str, int]]] = [{}]
+  def __init__(self, **kwargs): self.kwargs = kwargs
+  def __enter__(self):
+    Context.stack[-1] = {k:o.value for k,o in ContextVar._cache.items()} # Store current state.
+    for k,v in self.kwargs.items(): ContextVar._cache[k].value = v # Update to new temporary state.
+    Context.stack.append(self.kwargs) # Store the temporary state so we know what to undo later.
+  def __exit__(self, *args):
+    for k in Context.stack.pop(): ContextVar._cache[k].value = Context.stack[-1].get(k, ContextVar._cache[k].value)
+
+class ContextVar:
+  _cache: ClassVar[Dict[str, ContextVar]] = {}
+  value: int
+  def __new__(cls, key, default_value):
+    if key in ContextVar._cache: return ContextVar._cache[key]
+    instance = ContextVar._cache[key] = super().__new__(cls)
+    instance.value = getenv(key, default_value)
+    return instance
+  def __bool__(self): return bool(self.value)
+  def __ge__(self, x): return self.value >= x
+  def __gt__(self, x): return self.value > x
+  def __lt__(self, x): return self.value < x
+
+DEBUG, IMAGE, BEAM, NOOPT = ContextVar("DEBUG", 0), ContextVar("IMAGE", 0), ContextVar("BEAM", 0), ContextVar("NOOPT", 0)
+GRAPH, GRAPHPATH = getenv("GRAPH", 0), getenv("GRAPHPATH", "/tmp/net")
+
+class Timing(contextlib.ContextDecorator):
+  def __init__(self, prefix="", on_exit=None, enabled=True): self.prefix, self.on_exit, self.enabled = prefix, on_exit, enabled
+  def __enter__(self): self.st = time.perf_counter_ns()
+  def __exit__(self, exc_type, exc_val, exc_tb):
+    self.et = time.perf_counter_ns() - self.st
+    if self.enabled: print(f"{self.prefix}{self.et*1e-6:.2f} ms"+(self.on_exit(self.et) if self.on_exit else ""))
+
+# **** tinygrad now supports dtypes! *****
+
+class DType(NamedTuple):
+  priority: int  # this determines when things get upcasted
+  itemsize: int
+  name: str
+  np: Optional[type]  # TODO: someday this will be removed with the "remove numpy" project
+  sz: int = 1
+  def __repr__(self): return f"dtypes.{INVERSE_DTYPES_DICT[self]}"
+
+# dependent typing?
+class ImageDType(DType):
+  def __new__(cls, priority, itemsize, name, np, shape):
+    return super().__new__(cls, priority, itemsize, name, np)
+  def __init__(self, priority, itemsize, name, np, shape):
+    self.shape: Tuple[int, ...] = shape  # arbitrary arg for the dtype, used in image for the shape
+    super().__init__()
+  def __repr__(self): return f"dtypes.{self.name}({self.shape})"
+  # TODO: fix this to not need these
+  def __hash__(self): return hash((super().__hash__(), self.shape))
+  def __eq__(self, x): return super().__eq__(x) and self.shape == x.shape
+  def __ne__(self, x): return super().__ne__(x) or self.shape != x.shape
+
+class PtrDType(DType):
+  def __new__(cls, dt:DType): return super().__new__(cls, dt.priority, dt.itemsize, dt.name, dt.np, dt.sz)
+  def __repr__(self): return f"ptr.{super().__repr__()}"
+
+class dtypes:
+  @staticmethod # static methds on top, or bool in the type info will refer to dtypes.bool
+  def is_int(x: DType)-> bool: return x in (dtypes.int8, dtypes.int16, dtypes.int32, dtypes.int64, dtypes.uint8, dtypes.uint16, dtypes.uint32, dtypes.uint64)
+  @staticmethod
+  def is_float(x: DType) -> bool: return x in (dtypes.float16, dtypes.float32, dtypes.float64, dtypes._half4, dtypes._float2, dtypes._float4)
+  @staticmethod
+  def is_unsigned(x: DType) -> bool: return x in (dtypes.uint8, dtypes.uint16, dtypes.uint32, dtypes.uint64)
+  @staticmethod
+  def from_np(x) -> DType: return DTYPES_DICT[np.dtype(x).name]
+  @staticmethod
+  def fields() -> Dict[str, DType]: return DTYPES_DICT
+  bool: Final[DType] = DType(0, 1, "bool", np.bool_)
+  float16: Final[DType] = DType(0, 2, "half", np.float16)
+  half = float16
+  float32: Final[DType] = DType(4, 4, "float", np.float32)
+  float = float32
+  float64: Final[DType] = DType(0, 8, "double", np.float64)
+  double = float64
+  int8: Final[DType] = DType(0, 1, "char", np.int8)
+  int16: Final[DType] = DType(1, 2, "short", np.int16)
+  int32: Final[DType] = DType(2, 4, "int", np.int32)
+  int64: Final[DType] = DType(3, 8, "long", np.int64)
+  uint8: Final[DType] = DType(0, 1, "unsigned char", np.uint8)
+  uint16: Final[DType] = DType(1, 2, "unsigned short", np.uint16)
+  uint32: Final[DType] = DType(2, 4, "unsigned int", np.uint32)
+  uint64: Final[DType] = DType(3, 8, "unsigned long", np.uint64)
+
+  # NOTE: bfloat16 isn't supported in numpy
+  bfloat16: Final[DType] = DType(0, 2, "__bf16", None)
+
+  # NOTE: these are internal dtypes, should probably check for that
+  _int2: Final[DType] = DType(2, 4*2, "int2", None, 2)
+  _half4: Final[DType] = DType(0, 2*4, "half4", None, 4)
+  _float2: Final[DType] = DType(4, 4*2, "float2", None, 2)
+  _float4: Final[DType] = DType(4, 4*4, "float4", None, 4)
+  _arg_int32: Final[DType] = DType(2, 4, "_arg_int32", None)
+
+  # NOTE: these are image dtypes
+  @staticmethod
+  def imageh(shp): return ImageDType(100, 2, "imageh", np.float16, shp)
+  @staticmethod
+  def imagef(shp): return ImageDType(100, 4, "imagef", np.float32, shp)
+
+# HACK: staticmethods are not callable in 3.8 so we have to compare the class
+DTYPES_DICT = {k: v for k, v in dtypes.__dict__.items() if not k.startswith('__') and not callable(v) and not v.__class__ == staticmethod}
+INVERSE_DTYPES_DICT = {v:k for k,v in DTYPES_DICT.items()}
+
+class GlobalCounters:
+  global_ops: ClassVar[int] = 0
+  global_mem: ClassVar[int] = 0
+  time_sum_s: ClassVar[float] = 0.0
+  kernel_count: ClassVar[int] = 0
+  mem_used: ClassVar[int] = 0   # NOTE: this is not reset
+  mem_cached: ClassVar[int] = 0 # NOTE: this is not reset
+  @staticmethod
+  def reset(): GlobalCounters.global_ops, GlobalCounters.global_mem, GlobalCounters.time_sum_s, GlobalCounters.kernel_count = 0,0,0.0,0
+
+# *** universal database cache ***
+
+CACHEDB = getenv("CACHEDB", "/tmp/tinygrad_cache")
+CACHELEVEL = getenv("CACHELEVEL", 2)
+
+VERSION = 6
+_db_connection = None
+def db_connection():
+  global _db_connection
+  if _db_connection is None:
+    _db_connection = sqlite3.connect(CACHEDB)
+    if DEBUG >= 5: _db_connection.set_trace_callback(print)
+    if diskcache_get("meta", "version") != VERSION:
+      print("cache is out of date, clearing it")
+      os.unlink(CACHEDB)
+      _db_connection = sqlite3.connect(CACHEDB)
+      if DEBUG >= 5: _db_connection.set_trace_callback(print)
+      diskcache_put("meta", "version", VERSION)
+  return _db_connection
+
+def diskcache_get(table:str, key:Union[Dict, str, int]) -> Any:
+  if isinstance(key, (str,int)): key = {"key": key}
+  try:
+    res = db_connection().cursor().execute(f"SELECT val FROM {table} WHERE {' AND '.join([f'{x}=?' for x in key.keys()])}", tuple(key.values()))
+  except sqlite3.OperationalError:
+    return None  # table doesn't exist
+  if (val:=res.fetchone()) is not None:
+    return pickle.loads(val[0])
+  return None
+
+_db_tables = set()
+def diskcache_put(table:str, key:Union[Dict, str, int], val:Any):
+  if isinstance(key, (str,int)): key = {"key": key}
+  conn = db_connection()
+  cur = conn.cursor()
+  if table not in _db_tables:
+    TYPES = {str: "text", bool: "integer", int: "integer", float: "numeric", bytes: "blob"}
+    ltypes = ', '.join(f"{k} {TYPES[type(key[k])]}" for k in key.keys())
+    cur.execute(f"CREATE TABLE IF NOT EXISTS {table} ({ltypes}, val blob, PRIMARY KEY ({', '.join(key.keys())}))")
+    _db_tables.add(table)
+  cur.execute(f"REPLACE INTO {table} ({', '.join(key.keys())}, val) VALUES ({', '.join(['?']*len(key.keys()))}, ?)", tuple(key.values()) + (pickle.dumps(val), ))
+  conn.commit()
+  cur.close()
+  return val
+
+def diskcache(func):
+  def wrapper(*args, **kwargs) -> bytes:
+    table, key = f"cache_{func.__name__}", hashlib.sha256(pickle.dumps((args, kwargs))).hexdigest()
+    if (ret:=diskcache_get(table, key)): return ret
+    return diskcache_put(table, key, func(*args, **kwargs))
+  setattr(wrapper, "__wrapped__", func)
+  return wrapper
diff --git a/tinygrad_repo/tinygrad/image.py b/tinygrad_repo/tinygrad/image.py
deleted file mode 100644
index 068566a395..0000000000
--- a/tinygrad_repo/tinygrad/image.py
+++ /dev/null
@@ -1,74 +0,0 @@
-from tinygrad.helpers import IMAGE
-from tinygrad.lazy import get_single_root
-
-def image_conv2d_decorator(normal_conv):
-  if IMAGE == 0: return normal_conv
-
-  def image_conv2d(self, weight, bias=None, groups=1, stride=1, dilation=1, padding=0):
-    (bs,_,iy,ix), (cout,cin,H,W) = self.shape, weight.shape
-    rcout = cout//groups
-    x, w = self, weight.reshape(groups, rcout, cin, H, W)
-
-    # hack for non multiples of 4 on cin
-    if cin % 4 != 0 and not (cin == 1 and groups%4 == 0):
-      x = x.reshape(bs, groups, cin, iy, ix)   # do this always?
-      added_input_channels = 4 - (cin % 4)
-      w = w.pad(tuple((0, added_input_channels) if i == 2 else (0, 0) for i in range(len(w.shape))))
-      x = x.pad(tuple((0, added_input_channels) if i == 2 else (0, 0) for i in range(len(x.shape))))
-      cin = cin + added_input_channels
-      x = x.reshape(bs, groups*cin, iy, ix)
-
-    # hack for non multiples of 4 on rcout
-    added_output_channels = 0
-    if rcout % 4 != 0 and not (rcout == 1 and groups%4 == 0):
-      added_output_channels = 4 - (rcout % 4)
-      rcout += added_output_channels
-      cout = groups * rcout
-      w = w.slice(tuple((0, rcout) if i == 1 else (0, w.shape[i]) for i in range(len(w.shape))))
-
-    # packed (note: flipping bs and iy would make the auto-padding work)
-    x = x.permute(0,2,3,1).reshape(bs * iy, ix * groups * cin//4, 4)
-    cin_last = iy == 1 and ix == 1
-    if cin == 1: w = w.reshape(cout//4,4,H*W).permute(0,2,1)
-    elif cin_last: w = w.reshape(cout//4,4,cin//4,4,H,W).permute(0,4,2,5,1,3).reshape(cout//4, H*cin//4*W*4, 4)
-    else: w = w.reshape(cout//4,4,cin//4,4,H,W).permute(0,4,2,5,3,1).reshape(cout//4, H*cin//4*W*4, 4)
-
-    # contiguous creates the image, and early realize static weights (TODO: test for the static weight)
-    x, w = x.contiguous(), w.contiguous()
-    if get_single_root(w.lazydata).realized: w.realize()
-
-    # expand out
-    rcin_hi, rcin_lo = cin//4 if cin >= 4 else 1, 4 if cin >= 4 else 1
-    cout_expand = [groups//4 if cin == 1 else groups, 4 if cin == 1 else 1, rcout//4 if rcout >= 4 else 1, 4 if rcout >= 4 else 1]
-    x = x.reshape(bs, iy, ix, groups, rcin_hi, rcin_lo)
-    if cin_last: w = w.reshape(cout//4, H, rcin_hi, W, 4, rcin_lo)
-    else: w = w.reshape(cout//4, H, rcin_hi, W, rcin_lo, 4).permute(0,1,2,3,5,4)
-
-    # padding
-    padding_ = [padding]*4 if isinstance(padding, int) else (padding if len(padding) == 4 else [padding[1], padding[1], padding[0], padding[0]])
-    x = x.slice((None, (-padding_[2], x.shape[1]+padding_[3]), (-padding_[0], x.shape[2]+padding_[1]), None, None, None))
-
-    # prepare input
-    x = x.permute(0,3,4,5,1,2)._pool((H, W), stride, dilation) # -> (bs, groups, rcin_hi, rcin_lo, oy, ox, H, W)
-    oy, ox = x.shape[4:6]
-    x = x.permute(0,4,5,1,2,3,6,7).reshape(bs, oy, ox, *cout_expand[0:2], 1, 1, rcin_hi, rcin_lo, H, W)
-    x = x.expand(bs, oy, ox, *cout_expand, rcin_hi, rcin_lo, H, W)
-
-    # prepare weights
-    w = w.permute(0,4,2,5,1,3)
-    w = w.reshape((1, 1, 1, *cout_expand, rcin_hi, rcin_lo, H, W))
-
-    # the conv!
-    ret = (x*w).sum((-4, -3, -2, -1)).reshape(bs*oy, ox*cout//4, 4)
-    if IMAGE >= 3: ret = ret.contiguous()
-
-    # undo hack for non multiples of 4 on C.rcout
-    if added_output_channels != 0:
-      ret = ret.reshape(bs, oy, ox, groups, rcout)[:, :, :, :, :-added_output_channels]
-      rcout -= added_output_channels
-      cout = groups * rcout
-
-    # NCHW output
-    ret = ret.reshape(bs, oy, ox, cout).permute(0,3,1,2)
-    return ret if bias is None else ret.add(bias.reshape(1, -1, 1, 1))
-  return image_conv2d
diff --git a/tinygrad_repo/tinygrad/jit.py b/tinygrad_repo/tinygrad/jit.py
index 3cdb757c46..65270112d6 100644
--- a/tinygrad_repo/tinygrad/jit.py
+++ b/tinygrad_repo/tinygrad/jit.py
@@ -1,46 +1,77 @@
-from typing import Callable, List, Tuple, Any, Dict, cast, Union
-import itertools
-from tinygrad.helpers import DEBUG, colored
-
-from tinygrad.lazy import Device
+from typing import Callable, List, Tuple, Any, Dict, cast, Union, Optional
+from collections import defaultdict
+import functools, itertools
+from tinygrad.helpers import DEBUG, DType, merge_dicts
+from tinygrad.ops import RawBuffer, Device
 from tinygrad.tensor import Tensor
-from tinygrad.ops import GlobalCounters, CompiledBuffer, RawBuffer
+from tinygrad.shape.shapetracker import ShapeTracker
+from tinygrad.shape.symbolic import Variable
+
+JIT_SUPPORTED_DEVICE = ["GPU", "CLANG", "METAL", "CUDA", "HIP", "WEBGPU", "LLVM"]
 
 class TinyJit:
   def __init__(self, fxn:Callable):
-    self.fxn : Callable = fxn
-    self.cnt : int = 0
-    self.jit_cache : List[Tuple[Callable, Any]] = []  # TODO: Any should be List[RawBuffer], but this fails
-    self.ret : Any = None
-    self.input_replace : Dict[Tuple[int, int], Union[int, str]]= {}
+    self.fxn: Callable = fxn
+    self.cnt: int = 0
+    self.jit_cache: List[Tuple[Any, List[Optional[RawBuffer]], Dict[Variable, int]]] = []
+    self.ret: Any = None
+    self.input_replace: Dict[Tuple[int, int], Tuple[Union[int, str], ShapeTracker, DType]]= {}   # (kernel_number, buffer_number) -> (input_name, expected_shapetracker, expected_type)
+    self.updatable_entries: Dict[int, List[int]] = defaultdict(list) # (kernel_number) -> list(argument id). These are buffers from input + variables.
+
+  # add support for instance methods
+  def __get__(self, obj, objtype): return functools.partial(self.__call__, obj)
 
   def __call__(self, *args, **kwargs) -> Any:
-    if Device.DEFAULT not in ["GPU", "CLANG", "METAL", "CUDA"]: return self.fxn(*args, **kwargs)  # only jit on the GPU codegen
-    # NOTE: this cast is needed since although we know realize will create a ".realized" DeviceBuffer, the type checker doesn't
-    input_rawbuffers : Dict[Union[int, str], RawBuffer] = {cast(Union[int, str], k):cast(CompiledBuffer, v.realize().lazydata.realized).raw() for k,v in itertools.chain(enumerate(args), kwargs.items()) if isinstance(v, Tensor)}
+    if Device.DEFAULT.split(":")[0] not in JIT_SUPPORTED_DEVICE: return self.fxn(*args, **kwargs)  # only jit on supported device
+    # NOTE: this cast is needed since although we know realize will create a ".realized" RawBuffer, the type checker doesn't
+    input_rawbuffers: Dict[Union[int, str], Tuple[RawBuffer, ShapeTracker]] = {cast(Union[int, str], k):(cast(RawBuffer, v.realize().lazydata.realized), v.lazydata.st) for k,v in itertools.chain(enumerate(args), kwargs.items()) if v.__class__ is Tensor}
     assert len(input_rawbuffers) != 0, "no inputs to JIT"
+    assert len(set(input_rawbuffers.values())) == len(input_rawbuffers), "duplicate inputs to JIT"
     if self.cnt >= 2:
-      for (j,i),idx in self.input_replace.items(): self.jit_cache[j][1][i] = input_rawbuffers[idx]
-      if DEBUG >= 2: print(colored("TOK", 'magenta').split("TOK")[0], end="")
-      for prg, args in self.jit_cache: prg(args)
-      if DEBUG >= 2: print(colored("TOK", 'magenta').split("TOK")[1], end="")
-      for (j,i),idx in self.input_replace.items(): self.jit_cache[j][1][i] = None
+      try: var_vals: Dict[Variable, int] = kwargs["jit_ctx"]
+      except KeyError: var_vals = merge_dicts([arg.lazydata.st.var_vals for arg in args if arg.__class__ is Tensor])
+      if len(var_vals) > 1: var_vals = dict(sorted(var_vals.items(), key=lambda kv: kv[0].key))
+      for (j,i),(input_name, expected_st, expected_type) in self.input_replace.items():
+        assert input_rawbuffers[input_name][0].dtype == expected_type, f"type mismatch in JIT, {input_rawbuffers[input_name][0].dtype} != {expected_type}"
+        # NOTE: if we pass jit_ctx instead of using reshape to update the var_vals, we cannot compare the shapetracker directly
+        if "jit_ctx" not in kwargs: assert input_rawbuffers[input_name][1].unbind() == expected_st, f"ShapeTracker mismatch in JIT, {input_rawbuffers[input_name][1].unbind()} != {expected_st}"
+        self.jit_cache[j][1][i] = input_rawbuffers[input_name][0]
+      for j in self.updatable_entries.keys():
+        for k in self.jit_cache[j][2].keys():
+          try: self.jit_cache[j][2][k] = var_vals[k]
+          except KeyError: pass
+      for prg, pargs, variables in self.jit_cache: prg(pargs, variables, jit=True)
+      for (j,i) in self.input_replace.keys(): self.jit_cache[j][1][i] = None
     elif self.cnt == 1:
-      GlobalCounters.cache = []
+      CacheCollector.start()
       self.ret = self.fxn(*args, **kwargs)
-      self.jit_cache = GlobalCounters.cache
-      GlobalCounters.cache = None
+      self.jit_cache = CacheCollector.finish()
       assert len(self.jit_cache) != 0, "didn't JIT anything!"
       if DEBUG >= 1: print(f"JIT captured {len(self.jit_cache)} kernels with {len(input_rawbuffers)} inputs")
 
       # get the inputs for replacement
-      for j,(prg,args) in enumerate(self.jit_cache):  # pylint: disable=E1133
-        for i,a in enumerate(args):
-          if a in input_rawbuffers.values():
-            self.input_replace[(j,i)] = [k for k,v in input_rawbuffers.items() if v == a][0]
-      assert set(self.input_replace.values()) == set(input_rawbuffers.keys()), "some input tensors not found"
-      for (j,i),idx in self.input_replace.items(): self.jit_cache[j][1][i] = None
+      for j_,cache in enumerate(self.jit_cache): # type: Tuple[int, Tuple[Callable, List[Optional[RawBuffer]], Dict[Variable, int]]]
+        for i,a in enumerate(cache[1]):
+          if a in [v[0] for v in input_rawbuffers.values()]:
+            self.input_replace[(j_,i)] = [(k, v[1].unbind(), v[0].dtype) for k,v in input_rawbuffers.items() if v[0] == a][0]
+            self.updatable_entries[j_].append(i)
+        for i in range(len(cache[2])): self.updatable_entries[j_].append(len(cache[1])+i)
+      assert set([x[0] for x in self.input_replace.values()]) == set(input_rawbuffers.keys()), "some input tensors not found"
+      for (j,i) in self.input_replace.keys(): self.jit_cache[j][1][i] = None
     elif self.cnt == 0:
       self.ret = self.fxn(*args, **kwargs)
     self.cnt += 1
     return self.ret
+
+class _CacheCollector:
+  def __init__(self): self.cache: Optional[List[Tuple[Callable, List[Any], Dict[Any,Any]]]] = None
+  def start(self): self.cache = []
+  def add(self, prg, rawbufs, var_vals):
+    if self.cache is None: return
+    self.cache.append((prg, rawbufs, var_vals))
+  def finish(self):
+    if self.cache is None: return []
+    ret = self.cache
+    self.cache = None
+    return ret
+CacheCollector = _CacheCollector()
diff --git a/tinygrad_repo/tinygrad/lazy.py b/tinygrad_repo/tinygrad/lazy.py
index 7b397aa1c4..096cf7c9b1 100644
--- a/tinygrad_repo/tinygrad/lazy.py
+++ b/tinygrad_repo/tinygrad/lazy.py
@@ -1,248 +1,347 @@
 from __future__ import annotations
-from typing import Optional, Tuple, Union, List, Dict, Any, ClassVar, Type
-import os, sys, weakref, importlib, inspect, functools
-from weakref import WeakValueDictionary
-from tinygrad.helpers import prod, getenv
-from tinygrad.shape import ShapeTracker, get_contraction
-from tinygrad.ops import DeviceBuffer, UnaryOps, BinaryOps, ReduceOps, MovementOps, LoadOps, OpType, LazyOp, get_buffers, get_lazyops, map_buffers
-from tinygrad.graph import log_op
+import sys, operator, math, functools
+from typing import Callable, Optional, Tuple, Union, List, Dict, Any, cast, Mapping
+from weakref import ref, WeakSet, WeakValueDictionary
+
+import numpy as np
+from tinygrad.helpers import prod, getenv, DType, dtypes, flatten, dedup, merge_dicts, all_int
+from tinygrad.ops import ScheduleItem, UnaryOps, BinaryOps, TernaryOps, ReduceOps, MovementOps, LoadOps, OpType, LazyOp, MemBuffer, ConstBuffer, BufferOps
+from tinygrad.shape.shapetracker import ShapeTracker, get_contraction
+from tinygrad.shape.symbolic import Variable, sint
+
+from tinygrad.runtime.lib import RawBuffer
+from tinygrad.runtime.ops_cpu import RawNumpyBuffer
 
 # lazy can recurse a lot
 sys.setrecursionlimit(10000)
 
 OPT = getenv("OPT", 2)
-LAZY = getenv("LAZY", 1)
-
-def get_buffer(name, base='tinygrad.runtime'):
-  try:
-    return [cls for cname, cls in inspect.getmembers(importlib.import_module(f'{base}.ops_{name}'), inspect.isclass) if (cname.lower() == name + "buffer")][0]
-  except Exception as e:  # NOTE: this can't be put on one line due to mypy issue
-    print(name, "backend not available", e, file=sys.stderr)
-
-class _Device:
-  def __init__(self) -> None:
-    self._buffers : Dict[str, Type[DeviceBuffer]] = {x.upper():get_buffer(x) for x in
-      [os.path.splitext(x)[0][len("ops_"):] for x in sorted(os.listdir(os.path.join(os.path.dirname(os.path.realpath(__file__)), "runtime"))) if x.startswith("ops_")] if x is not None}
-    self.DEFAULT : str = "CPU"
-    for name in self._buffers:
-      if getenv(name) == 1: self.DEFAULT = name  # note: DEFAULT can be a Device that can't be imported. better than silent use of a different device
-      if self._buffers[name] is not None: self.__setattr__(name, name)
-Device = _Device()
+LAZYCACHE = getenv("LAZYCACHE", 1)
 
 # TODO: movement ops that only change shape are really nops. treat them as such
-REMOVE_MOVEMENT_NOPS, MERGE_UNARY_OPS, MERGE_ELEMENTWISE_INTO_REDUCE, SHUFFLE_MOVEMENT_OPS = OPT>=1, OPT>=1, OPT>=1, OPT>=1
-MERGE_ELEMENTWISE_OPS, MERGE_ONE_REDUCE_INTO_ELEMENTWISE = OPT>=2, OPT>=2
+REMOVE_MOVEMENT_NOPS, MERGE_ELEMENTWISE_INTO_REDUCE, SHUFFLE_MOVEMENT_OPS, MERGE_ELEMENTWISE_OPS = OPT>=1, OPT>=1, OPT>=1, OPT>=1
+MERGE_ONE_REDUCE_INTO_ELEMENTWISE, SHUFFLE_PAD_OPS = OPT>=2, OPT>=2
 PUSH_PERMUTES, PUSH_CONTIGUOUS = OPT>=3, OPT>=3
+PUSH_RESHAPES = OPT>=4
+
+# **** ast fixing functions ****
 
-# **** realize functions ****
-def _ast_reduceops(self:LazyBuffer) -> LazyOp:
+def _ast_reduceops(op:LazyOp) -> LazyOp:
   # TODO: this can also corealize a binary op after the reduce, not just before
-  src = self.op.src[0]
-  if MERGE_ELEMENTWISE_INTO_REDUCE and src.realized is None and src.optype == BinaryOps and len(src.children) <= 1:
-    src = src.op
-  return LazyOp(self.op.op, (src,), self.op.arg)
+  src = op.src[0]
+  if not src.realized:
+    assert isinstance(src.op, LazyOp), "if not src.realized, then src.op must be a LazyOp"
+    if MERGE_ELEMENTWISE_INTO_REDUCE and src.optype is BinaryOps and len(src.children) <= 1: src = src.op
+  return LazyOp(op.op, (src,), op.arg)
 
 # this supports late merging an upstream Reduce op and even an Elementwise op above that
-def _ast_binaryops(self:LazyBuffer) -> LazyOp:
-  real_srcs : Dict[LazyBuffer, Union[None, LazyOp, LazyBuffer]] = {x:None for x in get_buffers(self.op)}
+def _ast_binaryops(op:LazyOp, shape: Tuple[sint, ...]) -> LazyOp:
+  real_srcs: Dict[LazyBuffer, Optional[Union[LazyOp, LazyBuffer]]] = {x:None for x in op.buffers}
   # NOTE: contiguous does not always mean the same size with SHRINK. this is still mergeable but requires more thought how
-  psrcs : List[Tuple[LazyBuffer, LazyBuffer]] = [(k,x) for k,x in zip(real_srcs.keys(), map(get_movementroot_contiguous, real_srcs.keys())) if x.optype == ReduceOps and x.realized is None and prod(k.shape) == prod(x.shape) and len(x.children) <= 1 and len(k.children) <= 1]
-  intermediate_shape : Tuple[int, ...] = self.shape
-  if len(psrcs) == 1 and MERGE_ONE_REDUCE_INTO_ELEMENTWISE:
-    if psrcs[0][1].optype == ReduceOps:
-      top = _ast_reduceops(psrcs[0][1])
-    real_srcs[psrcs[0][0]] = top
-    real_srcs.update({x:x for x in get_buffers(top)})  # the reduce op buffers are not modified
+  # TODO: this can also support late fusion of BinaryOps, required for test_fold_conv_sgd
+  psrcs: List[Tuple[LazyBuffer, LazyBuffer]] = [(k,x) for k,x in zip(real_srcs.keys(), map(get_movementroot_contiguous, real_srcs.keys())) if x.optype == ReduceOps and not x.realized and prod(k.shape) == prod(x.shape) and len(x.children) <= 1 and len(k.children) <= 1]
+  intermediate_shape: Tuple[sint, ...] = shape
+  if MERGE_ONE_REDUCE_INTO_ELEMENTWISE and psrcs:
+    psrc = psrcs[0] # NOTE: right now we can't handle multiple, as we'd have to check for loop
+    if psrc[1].optype == ReduceOps:
+      top = _ast_reduceops(psrc[1].op)
+    real_srcs[psrc[0]] = top
+    real_srcs.update({x:x for x in top.buffers})  # the reduce op buffers are not modified
 
     # if the ReduceOp is followed by a reshape, we push this reshape before all the ElementwiseOp inputs
-    if psrcs[0][0].shape != psrcs[0][1].shape:
-      intermediate_shape = psrcs[0][1].shape
-      assert psrcs[0][0].shape == self.shape, f"shape mismatch {psrcs[0][0].shape} != {self.shape}"
+    if psrc[0].shape != psrc[1].shape:
+      intermediate_shape = psrc[1].shape
+      assert psrc[0].shape == shape, f"shape mismatch {psrc[0].shape} != {shape}"
 
   # reshape all the late ops into the output shape
   # NOTE: these RESHAPEs will return self if they don't change the shape
   for x in real_srcs.keys():
-    if real_srcs[x] is None: real_srcs[x] = x.movement_op(MovementOps.RESHAPE, intermediate_shape)
-  ast = map_buffers(real_srcs, self.op)
-  return LazyOp(MovementOps.RESHAPE, (ast, ), self.shape) if intermediate_shape != self.shape else ast
+    if real_srcs[x] is None: real_srcs[x] = x.reshape(intermediate_shape)
+  # NOTE: cast the type to remove the Optional
+  ast = op.map_buffers(cast(Dict[LazyBuffer, Union[LazyOp, LazyBuffer]], real_srcs))
+  return LazyOp(MovementOps.RESHAPE, (ast, ), shape) if intermediate_shape != shape else ast
+
+def _replace_bufferops(op:LazyOp) -> Tuple[LazyOp, List[LazyBuffer]]:
+  replacements:Dict[LazyBuffer, LazyOp] = {}
+  base_bufs = dedup([x.base for x in op.buffers if not x.is_unrealized_const()])
+  for x in op.buffers:
+    st = x.st.simplify().unbind()
+    if x.base in base_bufs:
+      replacements[x] = LazyOp(BufferOps.MEM, (), MemBuffer(base_bufs.index(x.base)+1, x.dtype, st))
+    elif not x.realized and x.base.op.op == LoadOps.CONST:
+      replacements[x] = LazyOp(BufferOps.CONST, (), ConstBuffer(float(x.base.op.arg), x.dtype, st))
+    else:
+      raise NotImplementedError(f"not handled {x}")
+  return (op.src[0] if op.op == MovementOps.RESHAPE else op).map_buffers(replacements), base_bufs
 
 # **** lazy operations ****
 
-def get_weakop(op:LazyOp) -> LazyOp: return LazyOp(op.op, tuple(get_weakop(x) if isinstance(x, LazyOp) else weakref.ref(x) for x in op.src), op.arg)
-def get_single_root(root:LazyBuffer) -> LazyBuffer: return get_single_root(root.op.src[0]) if getattr(root, 'op', None) and len(root.op.src) == 1 else root
-def get_movementroot(root:LazyBuffer, allow_contiguous=False) -> LazyBuffer: return get_movementroot(root.op.src[0], allow_contiguous) if root.realized is None and (root.optype == MovementOps or (root.op.op == LoadOps.CONTIGUOUS and allow_contiguous and root.op.src[0].st.contiguous)) else root
-def get_movementroot_contiguous(x:LazyBuffer) -> LazyBuffer: return get_movementroot_contiguous(x.op.src[0]) if x.realized is None and x.op.op == LoadOps.CONTIGUOUS else (get_movementroot(x, True) if x.optype == MovementOps and x.st.contiguous else x)
+def get_single_root(root:LazyBuffer) -> LazyBuffer: return get_single_root(cast(LazyBuffer, root.op.src[0])) if getattr(root, 'op', None) and len(root.op.src) == 1 and isinstance(root.op.src[0], LazyBuffer) else root
+def get_movementroot(root:LazyBuffer, allow_contiguous=False) -> LazyBuffer: return get_movementroot(cast(LazyBuffer, root.op.src[0]), allow_contiguous) if not root.realized and (root.optype == MovementOps or (root.op.op == LoadOps.CONTIGUOUS and allow_contiguous and root.op.src[0].st.contiguous)) else root
+def get_movementroot_contiguous(x:LazyBuffer) -> LazyBuffer: return get_movementroot_contiguous(cast(LazyBuffer, x.op.src[0])) if not x.realized and x.op.op == LoadOps.CONTIGUOUS else (get_movementroot(x, True) if x.optype == MovementOps and x.st.contiguous else x)
+
+def vars_from_ast(ast:LazyOp) -> List[Variable]: return dedup(functools.reduce(operator.add, [x.arg.st.vars() for x in ast.get_lazyops() if x.op in BufferOps], []))
+
+lazycache: WeakValueDictionary = WeakValueDictionary()
+def create_lazybuffer(device:str, st:ShapeTracker, optype:OpType, op:LazyOp, dtype:DType, base:Optional[LazyBuffer]=None):
+  # fromcpu aren't cached
+  if not LAZYCACHE or (optype is LoadOps and op.op in {LoadOps.EMPTY, LoadOps.RAND, LoadOps.CONST}): return LazyBuffer(device, st, optype, op, dtype, base=base)
 
-def replace_with_movement_op(y:Union[LazyOp, LazyBuffer], op:MovementOps, arg:Tuple[Any, ...]) -> LazyBuffer:
-  if isinstance(y, LazyBuffer): return y.movement_op(op, arg)
-  assert y.op in BinaryOps or y.op in UnaryOps
-  return elementwise_op(y.op, *[replace_with_movement_op(z, op, arg) for z in y.src])   # type: ignore
+  # wop is the deduping key. i feel this used to compare more deeply
+  wop = (device, dtype, optype, ref(op), ref(base) if base else None)
+  if wop in lazycache:
+    for x in op.buffers: x.children.add(lazycache[wop])
+    return lazycache[wop]
+
+  lazycache[wop] = ret = LazyBuffer(device, st, optype, op, dtype, base=base)
+  return ret
+
+UNSAFE_PAD_OPS = {BinaryOps.DIV, BinaryOps.CMPLT, UnaryOps.LOG2, UnaryOps.EXP2, UnaryOps.RECIP}
 
-def support_weakref(x): return x
-@support_weakref  # needed for mypyc, this prevents LazyBuffer from becoming a native class
 class LazyBuffer:
   __deletable__ = ('op',)
-  lazycache : ClassVar[WeakValueDictionary[Tuple[str, OpType, LazyOp], LazyBuffer]] = WeakValueDictionary()
-  def __new__(cls, device:str, shape:Union[ShapeTracker, Tuple[int, ...]], optype:OpType, op:LazyOp):
-    # fromcpu aren't cached
-    if optype == LoadOps and op.op == LoadOps.FROMCPU:
-      return super().__new__(cls)
-    wop = (device, optype, get_weakop(op))   # NOTE: shape should be deterministic. annoying to cache with the ShapeTracker
-    # NOTE: we need "ret" to prevent the new buffer from being immediately deleted
-    if wop not in LazyBuffer.lazycache: LazyBuffer.lazycache[wop] = ret = super().__new__(cls)
-    else: ret = LazyBuffer.lazycache[wop]
-    return ret
-
-  def __init__(self, device:str, shape:Union[ShapeTracker, Tuple[int, ...]], optype:OpType, op:LazyOp):
-    if hasattr(self, 'device'):
-      return  # cache hit, we return and don't reinit
-    self.st = shape if isinstance(shape, ShapeTracker) else ShapeTracker(tuple(shape))
-    self.shape, self.optype, self.op = self.st.shape, optype, op
-    self.realized : Optional[DeviceBuffer] = None
-    self.output_buffer : Optional[DeviceBuffer] = None
-    self.device, self.dbuffer = device, Device._buffers[device]
+  def __init__(self, device:str, st:ShapeTracker, optype:OpType, op:Optional[LazyOp], dtype:DType, src:Optional[RawBuffer]=None, base:Optional[LazyBuffer]=None):
+    self.st: ShapeTracker = st
+    self.device, self.shape, self.optype, self._dtype = device, self.st.shape, optype, dtype
+    self._realized: Optional[RawBuffer] = src
+    self.output_buffer: Optional[RawBuffer] = None   # TODO: do we really need this? or can we just use realized
     # TODO: does children have to be a ref count instead of a set? can a Buffer be a double child?
-    self.children : weakref.WeakSet[LazyBuffer] = weakref.WeakSet()
-    # NOTE: op should be read only after construction of LazyBuffer
-    for x in get_buffers(op): x.children.add(self)
-    if not LAZY: self.realize()
-
-  def __repr__(self): return f"<LB {self.shape} op:{self.op.op if self.realized is None else 'realized'}>"
-
-  # this produces a device buffer
-  def realize(self:LazyBuffer, required_device=None) -> DeviceBuffer:
-    assert required_device is None or required_device == self.device
-    if self.realized is None:
-      # get real ops first
-      if self.op.op == LoadOps.FROMCPU:
-        self.realized = Device._buffers[self.device].fromCPU(self.op.arg)
-        ast = LazyOp(self.op.op, tuple())
-      elif self.op.op == LoadOps.CONTIGUOUS:
-        real_src = self.op.src[0].realize(self.device)
-        self.realized = real_src.contiguous()
-        ast = LazyOp(self.op.op, (real_src, ))
-      elif self.optype == MovementOps:
-        src = self.op.src[0]
-
-        # fuse RESHAPE and ReduceOps
-        if src.realized is None and src.optype == ReduceOps and self.op.op == MovementOps.RESHAPE and len(src.children) <= 1:
-          # it's okay to add a RESHAPE to the ast here
-          ast = LazyOp(MovementOps.RESHAPE, (_ast_reduceops(src), ), self.op.arg)
-        else:
-          # movement ops aren't an AST, just run them
-          real_src = src.realize(self.device)
-          self.realized = real_src.movement_op(self.op.op, self.op.arg)
-          ast = LazyOp(self.op.op, (real_src, ))
-      elif self.optype == ReduceOps: ast = _ast_reduceops(self)
-      elif self.optype == BinaryOps: ast = _ast_binaryops(self)
-
-      # no need to keep the op after realization
-      del self.op
-
-      # run the ast if we still have to, and log the op
-      if self.realized is None:
-        ast = map_buffers({x:x.realize(self.device) for x in get_buffers(ast)}, ast)
-        self.realized = self.dbuffer.exec_ast(ast, output_buffer=self.output_buffer)
-      log_op(self.realized, ast)
-
-    assert self.realized.shape == self.shape, f"shape mismatch on realize {self.realized.shape} vs {self.shape}"
-    assert isinstance(self.realized, Device._buffers[self.device])
-    return self.realized
+    self.children: WeakSet = WeakSet()
+    self.views: WeakSet = WeakSet()
+    # NOTE: op should be read only after construction of LazyBuffer. it is now with schedule
+    if op is not None:
+      self.op: LazyOp = op
+      for x in op.buffers: x.children.add(self)
+    assert optype != MovementOps or (base is not None and base.optype != MovementOps), "MovementOps must be based"
+    self._base = base
+    if base: base.views.add(self)
+    else: assert st.contiguous, "unbased LazyBuffers must be contiguous"
+
+  @property
+  def base(self): return self._base if self._base is not None else self
+
+  def is_unrealized_const(self): return not self.realized and self.base.op.op == LoadOps.CONST
+
+  @property
+  def realized(self): return self.base._realized
+  @realized.setter
+  def realized(self, val):
+    assert self._base is None, "no setting realized of based LazyBuffers"
+    self._realized = val
+  @property
+  def dtype(self): return self.base._dtype
+  @dtype.setter
+  def dtype(self, val):
+    assert self._base is None, "no setting dtype of based LazyBuffers"
+    self._dtype = val
+
+  def __repr__(self): return f"<LB {self.shape} {self.dtype} op={self.op.op if hasattr(self, 'op') else self._realized} st={self.st}>"
+  @property
+  def key(self):
+    if self.realized: return (self.dtype, self.realized.key, self.st)
+    return (self.dtype, self.op.op, self.st)
+
+  def _device_extra_args(self) -> Dict[str, str]: return {"device": self.device.split(":", 1)[1]} if ":" in self.device else {}
+
+  @property
+  def buffers(self) -> Tuple[LazyBuffer, ...]: return (self,)
+  def map_buffers(self, real_srcs: Mapping[Any, Union[LazyBuffer, LazyOp]]): return real_srcs.get(self, self)
+  def get_lazyops(self) -> List[LazyOp]: return []
+
+  # *** scheduling ***
+
+  def schedule(self, seen=None) -> List[ScheduleItem]:
+    if seen is None: seen = set()
+    if self in seen or self.realized or self.is_unrealized_const(): return []
+    seen.add(self)
+    if self.base != self: return self.base.schedule(seen)
+
+    # rewrite unbased CONTIGUOUS into UnaryOps.NOOP
+    op = self.op if self.op.op != LoadOps.CONTIGUOUS else LazyOp(UnaryOps.NOOP, self.op.src)
+
+    if self.optype is BinaryOps: op = _ast_binaryops(op, self.shape)
+    elif self.optype is ReduceOps: op = _ast_reduceops(op)
+
+    # schedule the past
+    ret = []
+    for x in op.buffers: ret += x.schedule(seen)
+
+    var_vals = dict(sorted(merge_dicts([self.st.var_vals] + [buf.st.var_vals for buf in op.buffers]).items(), key=lambda kv:cast(Variable,kv[0]).key))
+
+    # run the ast and log the op
+    op, base_bufs = _replace_bufferops(op)
+    return ret + [ScheduleItem(op, self, tuple(base_bufs), {k:var_vals[k] for k in vars_from_ast(op)})]
+
+  # *** creation/special ops ***
+
+  @staticmethod
+  def loadop(op, shape, dtype, device, arg=None, src=None) -> LazyBuffer:
+    return create_lazybuffer(device, ShapeTracker.from_shape(tuple(shape)), LoadOps, LazyOp(op, tuple() if src is None else (src,), arg), dtype)
+
+  # create a constant with the shape and dtype of self
+  def const(self, val:Union[float, int]) -> LazyBuffer:
+    # NOTE: dtypes.from_np(self.dtype.np) to deal with image types
+    return self.loadop(LoadOps.CONST, tuple(), dtypes.from_np(self.dtype.np), self.device, arg=val).reshape((1,)*len(self.shape)).expand(self.shape)
+
+  def copy_to_device(self, device:str) -> LazyBuffer:
+    # back off a FROM if it's a double FROM
+    if not self.realized and self.op.op == LoadOps.FROM and cast(LazyBuffer, self.op.src[0]).device == device: return cast(LazyBuffer, self.op.src[0])
+    return LazyBuffer.loadop(LoadOps.FROM, self.shape, self.dtype, device, src=self.contiguous())
+
+  def contiguous(self:LazyBuffer) -> LazyBuffer:
+    if not self.realized and self.op.op in LoadOps and self.op.op != LoadOps.CONST: return self  # all LoadOps are already contiguous (except CONST)
+    if self.st.contiguous and self.st.size() == self.base.st.size() and not self.is_unrealized_const():
+      # this will turn into nothing, it's based and a copy
+      # TODO: based lazybuffers shouldn't take dtype or var_vals, same issue in movementops
+      return create_lazybuffer(self.device, ShapeTracker.from_shape(tuple(self.shape)), LoadOps, LazyOp(LoadOps.CONTIGUOUS, (self,), None), self.dtype, base=self.base)
+    # real contiguous, this will turn into a UnaryOps.NOOP
+    return self.loadop(LoadOps.CONTIGUOUS, self.shape, self.dtype, self.device, src=self)
 
   @staticmethod
-  def fromCPU(x, device) -> LazyBuffer: return LazyBuffer(device, x.shape, LoadOps, LazyOp(LoadOps.FROMCPU, tuple(), x.copy()))
-  def toCPU(self): return self.realize().toCPU()
+  def fromCPU(x: np.ndarray) -> LazyBuffer:
+    return LazyBuffer("CPU", ShapeTracker.from_shape(x.shape), LoadOps, None, dtypes.from_np(x.dtype), RawNumpyBuffer.fromCPU(x))
+
+  def cast(self, dtype:DType, bitcast:bool=False):
+    return self.e(UnaryOps.CAST, arg=(dtype, bitcast))
+
+  # *** elementwise ops ***
+
+  def e(self:LazyBuffer, op:Union[UnaryOps, BinaryOps, TernaryOps], *srcs:LazyBuffer, arg:Optional[Any]=None) -> LazyBuffer:
+    # srcs includes self
+    srcs = (self,)+srcs
+
+    # if we are separated from other binary ops by movement ops, we push those movement ops above those binaryops
+    if SHUFFLE_MOVEMENT_OPS: srcs = _push_movement_ops(srcs)
+
+    # get outputs now
+    out_device, out_shape, out_dtype = srcs[0].device, srcs[0].shape, max([x.dtype for x in srcs]) if op != UnaryOps.CAST else cast(Tuple[DType, bool], arg)[0]
+
+    # push all contiguous to the end of BinaryOps. kernels 198 -> 196
+    if PUSH_CONTIGUOUS and any(not x.realized and x.op.op == LoadOps.CONTIGUOUS and len(x.op.src[0].children) <= 1 for x in srcs):
+      new_srcs: List[LazyBuffer] = []
+      for x in srcs:
+        if not x.realized and x.op.op == LoadOps.CONTIGUOUS and len(x.op.src[0].children) <= 1:
+          x.op.src[0].children.discard(x)
+          new_srcs.append(cast(LazyBuffer, x.op.src[0]))
+        else:
+          new_srcs.append(x)
+      return new_srcs[0].e(op, *new_srcs[1:], arg=arg).contiguous()
+
+    if MERGE_ELEMENTWISE_OPS:
+      # remove the buffers from any (childless) BinaryOps that feed into this
+      _srcs = tuple([x.op if x.optype == BinaryOps and not x.children and not x.realized else x for x in srcs])  # type: ignore
+      # TODO: needs general merge limiting
+      if out_device != "WEBGPU" or len(dedup([x.base for _src in _srcs for x in _src.buffers if not x.is_unrealized_const()])) < 7: srcs = _srcs # type: ignore
+
+    return create_lazybuffer(out_device, ShapeTracker.from_shape(out_shape), BinaryOps, LazyOp(op, srcs, arg), out_dtype)
 
-  def unary_op(self:LazyBuffer, op:UnaryOps) -> LazyBuffer: return elementwise_op(op, self)
-  def binary_op(self:LazyBuffer, op:BinaryOps, y:LazyBuffer) -> LazyBuffer: return elementwise_op(op, self, y)
-  def contiguous(self:LazyBuffer) -> LazyBuffer: return LazyBuffer(self.device, self.shape, LoadOps, LazyOp(LoadOps.CONTIGUOUS, (self,)))
+  # *** reduce ops ***
 
-  def reduce_op(self:LazyBuffer, op:ReduceOps, new_shape:Tuple[int, ...]) -> LazyBuffer:
+  def _reduce_op(self:LazyBuffer, op:ReduceOps, new_shape:Tuple[sint, ...]) -> LazyBuffer:
     if self.shape == tuple(new_shape): return self
-    reduce = list(enumerate(zip(self.shape, new_shape)))
-    # move the reduce axes to the end
-    x = self.movement_op(MovementOps.PERMUTE, tuple([i for i,(s,n) in reduce if s == n] + [i for i,(s,n) in reduce if s != n]))
-    new_tmp_shape = tuple([n for _,(s,n) in reduce if s == n] + [n for _,(s,n) in reduce if s != n])
-    # NOTE: this reshape can only move around 1s
-    return LazyBuffer(x.device, new_tmp_shape, ReduceOps, LazyOp(op, (x,), new_tmp_shape)).movement_op(MovementOps.RESHAPE, new_shape)
-
-  def movement_op(self:LazyBuffer, op:MovementOps, arg:Tuple[Any, ...]) -> LazyBuffer:
-    # very instant nop
-    if op == MovementOps.RESHAPE and self.shape == arg: return self
-
-    # TODO: look into why that copy is needed
-    local_st = ShapeTracker(self.shape).movement_op(op, arg)
-
-    # instant nops
-    if local_st.contiguous and self.shape == local_st.shape: return self
-
-    # two ops in a row is one op. merge them if unresolved
-    if self.realized is None and self.op.op == op:
-      # TODO: why is deleting self from children needed? shouldn't GC do it?
-      self.op.src[0].children.discard(self)
-      if op in [MovementOps.RESHAPE, MovementOps.EXPAND, MovementOps.SHRINK]: return self.op.src[0].movement_op(op, arg)
-      if op == MovementOps.PERMUTE: return self.op.src[0].movement_op(op, tuple(self.op.arg[i] for i in arg))
-      if op == MovementOps.PAD: return self.op.src[0].movement_op(op, tuple((b1+b2, e1+e2) for (b1,e1),(b2,e2) in zip(self.op.arg, arg)))
-      if op == MovementOps.FLIP: return self.op.src[0].movement_op(op, tuple(i for i in arg+self.op.arg if not (i in arg and i in self.op.arg)))
-
-    # push permutes before reduce ops
-    if op == MovementOps.PERMUTE and PUSH_PERMUTES and self.realized is None and self.optype == ReduceOps:
-      # reduceops have one buffer input, permute it
-      narg = tuple(self.op.arg[arg[i]] for i in range(len(arg)))
-      src, rop = self.op.src[0], self.op.op
-      src.children.discard(self)
-      del self  # TODO: why doesn't this delete remove it from the children
-      return src.movement_op(op, arg).reduce_op(rop, narg)
-
-    # some permutes are actually just reshapes
-    if op == MovementOps.PERMUTE and local_st.contiguous: return self.movement_op(MovementOps.RESHAPE, tuple(self.shape[i] for i in arg))
-
-    # move permutes before expands
-    if op == MovementOps.PERMUTE and PUSH_PERMUTES and self.realized is None and self.op.op == MovementOps.EXPAND:
-      self.op.src[0].children.discard(self)
-      return self.op.src[0].movement_op(MovementOps.PERMUTE, arg).movement_op(MovementOps.EXPAND, tuple(self.op.arg[a] for a in arg))
-
-    # move permutes before reshapes if we can
-    if op == MovementOps.PERMUTE and PUSH_PERMUTES and self.realized is None and self.op.op == MovementOps.RESHAPE and isinstance(self.op.src[0], LazyBuffer):
-      if shape_idx_groups := get_contraction(self.op.src[0].shape, self.shape):
-        new_arg : List[int] = functools.reduce(lambda r, x: r + shape_idx_groups[x], arg, [])
-        self.op.src[0].children.discard(self)   # this changes nothing?
-        return self.op.src[0].movement_op(MovementOps.PERMUTE, tuple(new_arg)) \
-          .movement_op(MovementOps.RESHAPE, ShapeTracker(self.st).movement_op(op, arg).shape)
-
-    # if this MovementOp is being applied to a BinaryOp, apply the MovementOp to all the BinaryOp inputs instead. NOTE: UnaryOps is never an OpType
-    if SHUFFLE_MOVEMENT_OPS and self.optype == BinaryOps and self.realized is None and len(self.children) == 0 and op != MovementOps.EXPAND and (op != MovementOps.PAD or all(x.op != BinaryOps.DIV for x in get_lazyops(self.op))):
-      return replace_with_movement_op(self.op, op, arg)
-
-    # create the buffer
-    ret = LazyBuffer(self.device, ShapeTracker(self.st).movement_op(op, arg), MovementOps, LazyOp(op, (self,), arg))
-
-    # if the ShapeTracker becomes contiguous, replace the whole thing with a reshape (or nothing if shapes match)
-    # NOTE: if ret is in the cache, it can already be realized
-    if REMOVE_MOVEMENT_NOPS and ret.realized is None and self.realized is None and ret.st.contiguous:
+    srcs = _push_movement_ops((self,)) if SHUFFLE_MOVEMENT_OPS else (self,)
+    unbound_new_shape = tuple(s.unbind()[0] if not isinstance(s, int) else s for s in new_shape)
+    return create_lazybuffer(self.device, ShapeTracker.from_shape(new_shape), ReduceOps, LazyOp(op, srcs, unbound_new_shape), self.dtype)
+
+  def r(self:LazyBuffer, op:ReduceOps, new_shape:Tuple[sint, ...]) -> LazyBuffer:
+    if not all_int(self.shape) or prod(self.shape) // prod(new_shape) < getenv("REDUCEOP_SPLIT_THRESHOLD", 32768): return self._reduce_op(op, new_shape) # The amount of work should be big enough to take the benefit of "2 kernels" approach.
+    heuristic, divisor, dim_to_split = max(((divisor := math.gcd(256, old))/(stride or math.inf), divisor, i) for i, (old, new, stride) in enumerate(zip(self.shape, new_shape, self.st.real_strides())) if old != new) # type: ignore
+    if divisor < 16 or heuristic < 0.1: return self._reduce_op(op, new_shape) # Choose largest divisor (>=16) to split on, penalize large strides.
+    def splitted_shape(dim_aft_div): return self.shape[:dim_to_split] + (self.shape[dim_to_split]//divisor,) + dim_aft_div + self.shape[dim_to_split+1:]
+    return self.reshape(splitted_shape((divisor,)))._reduce_op(op, splitted_shape((1,))).reshape(splitted_shape(()))._reduce_op(op, new_shape)
+
+  # *** movement ops ***
+
+  def _movement_op(self, st: ShapeTracker, op: MovementOps, arg: Union[Tuple[sint, ...], Tuple[Tuple[sint, sint], ...]]) -> LazyBuffer:
+    if SHUFFLE_MOVEMENT_OPS and not self.realized and self.optype == BinaryOps and not self.children:
+      if op in {MovementOps.SHRINK, MovementOps.STRIDE, MovementOps.PERMUTE} or (op == MovementOps.RESHAPE and (self.op.op in UnaryOps or PUSH_RESHAPES)):
+        return self.op.replace_with_movement_ops([(op, arg)])
+    if REMOVE_MOVEMENT_NOPS and not self.realized and st.contiguous:
       # MovementOps aren't stacked any more, they each have one parent, find the root
       root = get_movementroot(self)
-      if root.st.contiguous and root != self and prod(ret.st.shape) == prod(root.shape):
-        return root.movement_op(MovementOps.RESHAPE, ret.st.shape)
-
-    return ret
-
-def elementwise_op(op:Union[UnaryOps, BinaryOps], *srcs:LazyBuffer) -> LazyBuffer:
-  out_device, out_shape = srcs[0].device, srcs[0].shape
-
-  # push all contiguous to the end of BinaryOps. kernels 198 -> 196
-  if PUSH_CONTIGUOUS and any(x.realized is None and x.op.op == LoadOps.CONTIGUOUS and len(x.op.src[0].children) <= 1 for x in srcs):
-    new_srcs = []
-    for x in srcs:
-      if x.realized is None and x.op.op == LoadOps.CONTIGUOUS and len(x.op.src[0].children) <= 1:
-        x.op.src[0].children.discard(x)
-        new_srcs.append(x.op.src[0])
-      else:
-        new_srcs.append(x)
-    return elementwise_op(op, *new_srcs).contiguous()
-
-  if MERGE_ELEMENTWISE_OPS or (MERGE_UNARY_OPS and len(set(srcs)) == 1):
-    # remove the buffers from any (childless) BinaryOps that feed into this
-    srcs = tuple(x.op if x.optype == BinaryOps and len(x.children) == 0 and x.realized is None else x for x in srcs)  # type: ignore
-
-  return LazyBuffer(out_device, out_shape, BinaryOps, LazyOp(op, srcs))
+      if root.st.contiguous and root != self and prod(st.shape) == prod(root.shape):
+        return root.reshape(st.shape)
+    return create_lazybuffer(self.device, st, MovementOps, LazyOp(op, (self,), arg), self.dtype, base=self.base)
+
+  def reshape(self:LazyBuffer, arg:Tuple[sint, ...]) -> LazyBuffer:
+    if self.shape == arg: return self
+    if not self.realized and self.op.op == MovementOps.RESHAPE:
+      assert isinstance(self.op.src[0], LazyBuffer)
+      self.op.src[0].children.discard(self) # NOTE: this is only required in reshape and when pushing permutes, why??
+      return self.op.src[0].reshape(arg)
+    return self._movement_op(self.st.reshape(arg), MovementOps.RESHAPE, arg)
+
+  def pad(self:LazyBuffer, arg:Tuple[Tuple[int, int], ...]) -> LazyBuffer:
+    if all(b == 0 and e == 0 for b,e in arg): return self
+    if not self.realized and self.op.op == MovementOps.PAD: return self.op.src[0].pad(tuple([(b1+b2, e1+e2) for (b1,e1),(b2,e2) in zip(self.op.arg, arg)]))
+    return self._movement_op(self.st.pad(arg), MovementOps.PAD, arg)
+
+  def expand(self: LazyBuffer, arg:Tuple[sint, ...]) -> LazyBuffer:
+    if self.shape == arg: return self
+    if not self.realized and self.op.op == MovementOps.EXPAND: return self.op.src[0].expand(arg)
+    return self._movement_op(self.st.expand(arg), MovementOps.EXPAND, arg)
+
+  def permute(self: LazyBuffer, arg:Tuple[int, ...]) -> LazyBuffer:
+    if arg == tuple(range(len(self.shape))): return self
+    if not self.realized and self.op.op == MovementOps.PERMUTE: return self.op.src[0].permute(tuple([self.op.arg[i] for i in arg]))
+    if SHUFFLE_MOVEMENT_OPS and not self.realized:
+      if PUSH_PERMUTES and self.optype == ReduceOps:
+        # reduceops have one buffer input, permute it
+        narg = tuple([self.op.arg[a] for a in arg])
+        src, rop = self.op.src[0], self.op.op
+        src.children.discard(self)
+        del self  # TODO: why doesn't this delete remove it from the children
+        return src.permute(arg).r(cast(ReduceOps, rop), narg)
+
+      # move permutes before expands (always, this is safe)
+      if self.op.op == MovementOps.EXPAND:
+        return self.op.src[0].permute(arg).expand(tuple([self.op.arg[a] for a in arg]))
+
+      # move permutes before reshapes if we can
+      if PUSH_PERMUTES and self.op.op == MovementOps.RESHAPE and isinstance(self.op.src[0], LazyBuffer):
+        if shape_idx_groups := get_contraction(self.op.src[0].shape, self.shape):
+          self.op.src[0].children.discard(self) # NOTE: this is only required in reshape and when pushing permutes, why??
+          return self.op.src[0].permute(tuple(flatten(shape_idx_groups[i] for i in arg))).reshape(self.st.permute(arg).shape)
+    return self._movement_op(self.st.permute(arg), MovementOps.PERMUTE, arg)
+
+  def shrink(self:LazyBuffer, arg:Tuple[Tuple[sint, sint], ...]) -> LazyBuffer:
+    if all(b - a == s for s, (a, b) in zip(self.shape, arg)): return self
+    if not self.realized and self.op.op == MovementOps.SHRINK: return self.op.src[0].shrink(tuple([(b1+b2, b1+e2) for (b1,_),(b2,e2) in zip(self.op.arg, arg)]))
+    return self._movement_op(self.st.shrink(arg), MovementOps.SHRINK, arg)
+
+  def stride(self:LazyBuffer, arg:Tuple[int, ...]) -> LazyBuffer:
+    if all(a == 1 for a in arg): return self
+    if not self.realized and self.op.op == MovementOps.STRIDE: return self.op.src[0].stride(tuple(map(operator.mul, arg, self.op.arg)))
+    return self._movement_op(self.st.stride(arg), MovementOps.STRIDE, arg)
+
+  def replace_with_movement_ops(self: LazyBuffer, ops:List[Tuple[MovementOps, Any]]) -> LazyBuffer:
+    y = self
+    for op, arg in ops: y = MOVEMENT_OPS_DISPATCHER[op](y, arg)
+    return y
+
+def _push_movement_ops(srcs:Tuple[LazyBuffer, ...]) -> Tuple[LazyBuffer, ...]:
+  new_srcs = []
+  for x in srcs:
+    mops: List[Tuple[MovementOps, Any]] = []
+    bx = x
+    # backwalk all the movement ops. don't push PAD or EXPAND
+    while not bx.realized and bx.optype is MovementOps and bx.op.op is not MovementOps.EXPAND and (SHUFFLE_PAD_OPS or bx.op.op is not MovementOps.PAD) and len(bx.children) <= 1:
+      assert isinstance(bx.op.op, MovementOps)
+      mops.append((bx.op.op, bx.op.arg))
+      assert isinstance(bx.op.src[0], LazyBuffer)
+      bx = bx.op.src[0]
+    # NOTE: can't push pads past anything where f(0, 0) != 0 or f(0) != 0
+    if mops and not bx.realized and bx.optype is BinaryOps and len(bx.children) <= 1 and (all(y[0] is not MovementOps.PAD for y in mops) or all(y.op not in UNSAFE_PAD_OPS for y in bx.op.get_lazyops())):
+      new_srcs.append(bx.op.replace_with_movement_ops(mops[::-1]))
+    else:
+      new_srcs.append(x)
+  return tuple(new_srcs)
+
+MOVEMENT_OPS_DISPATCHER: Dict[MovementOps, Callable] = {
+  MovementOps.RESHAPE: LazyBuffer.reshape,
+  MovementOps.EXPAND: LazyBuffer.expand,
+  MovementOps.SHRINK: LazyBuffer.shrink,
+  MovementOps.PERMUTE: LazyBuffer.permute,
+  MovementOps.PAD: LazyBuffer.pad,
+  MovementOps.STRIDE: LazyBuffer.stride,
+}
diff --git a/tinygrad_repo/tinygrad/mlops.py b/tinygrad_repo/tinygrad/mlops.py
index 2eba538442..ee0766fbe9 100644
--- a/tinygrad_repo/tinygrad/mlops.py
+++ b/tinygrad_repo/tinygrad/mlops.py
@@ -1,162 +1,211 @@
-from tinygrad.helpers import argsort
-from tinygrad.ops import UnaryOps, BinaryOps, ReduceOps, MovementOps
+import math
+from typing import Tuple, Optional, cast
+from tinygrad.helpers import argsort, DType
+from tinygrad.ops import UnaryOps, BinaryOps, TernaryOps, ReduceOps
 from tinygrad.tensor import Function
+from tinygrad.lazy import LazyBuffer
+from tinygrad.shape.symbolic import sint
 
 class Contiguous(Function):
-  def forward(self, x): return x.contiguous()
-  def backward(self, grad_output): return grad_output
+  def forward(self, x:LazyBuffer) -> LazyBuffer: return x.contiguous()
+  def backward(self, grad_output:LazyBuffer) -> LazyBuffer: return grad_output
+
+class ContiguousBackward(Function):
+  def forward(self, x:LazyBuffer) -> LazyBuffer: return x
+  def backward(self, grad_output:LazyBuffer) -> LazyBuffer: return grad_output.contiguous()
+
+class Cast(Function):
+  def forward(self, x:LazyBuffer, dtype:DType, bitcast:bool=False) -> LazyBuffer:
+    self.input_dtype, self.bitcast = x.dtype, bitcast
+    return x.cast(dtype, bitcast)
+
+  def backward(self, grad_output:LazyBuffer) -> LazyBuffer:
+    return grad_output.cast(self.input_dtype, self.bitcast)
 
 # ************* unary ops *************
 
-class Log(Function):
-  def forward(self, x):
+class Zero(Function):
+  def forward(self, x:LazyBuffer) -> LazyBuffer: return x.const(0)
+  def backward(self, grad:LazyBuffer) -> LazyBuffer: return grad.const(0)
+
+class Neg(Function):
+  def forward(self, x:LazyBuffer) -> LazyBuffer: return x.e(UnaryOps.NEG)
+  def backward(self, grad:LazyBuffer) -> LazyBuffer: return grad.e(UnaryOps.NEG)
+
+class Sin(Function):
+  def forward(self, x:LazyBuffer) -> LazyBuffer:
     self.x = x
-    return x.unary_op(UnaryOps.LOG)
+    return x.e(UnaryOps.SIN)
 
-  def backward(self, grad_output):
-    return grad_output.binary_op(BinaryOps.DIV, self.x)
+  def backward(self, grad:LazyBuffer) -> LazyBuffer:
+    return self.x.const(math.pi / 2).e(BinaryOps.SUB, self.x).e(UnaryOps.SIN).e(BinaryOps.MUL, grad)
 
-class Exp(Function):
-  def forward(self, x):
-    self.ret = x.unary_op(UnaryOps.EXP)
+# NOTE: maximum(x, 0) behaves differently where x=0
+class Relu(Function):
+  def forward(self, x:LazyBuffer) -> LazyBuffer:
+    self.ret = x.e(BinaryOps.MAX, x.const(0))
     return self.ret
 
-  def backward(self, grad_output):
-    return self.ret.binary_op(BinaryOps.MUL, grad_output)
+  def backward(self, grad_output:LazyBuffer) -> LazyBuffer:
+    return self.ret.const(0).e(BinaryOps.CMPLT, self.ret).e(BinaryOps.MUL, grad_output)
 
-# ************* reduce ops *************
-
-class Sum(Function):
-  def forward(self, x, new_shape):
-    self.input_shape = x.shape
-    return x.reduce_op(ReduceOps.SUM, new_shape)
+class Log(Function):
+  def forward(self, x:LazyBuffer) -> LazyBuffer:
+    self.x = x
+    return x.e(UnaryOps.LOG2).e(BinaryOps.MUL, x.const(math.log(2)))
 
-  def backward(self, grad_output):
-    return grad_output.movement_op(MovementOps.EXPAND, self.input_shape)
+  def backward(self, grad_output:LazyBuffer) -> LazyBuffer:
+    return grad_output.e(BinaryOps.DIV, self.x)
 
-class Max(Function):
-  def forward(self, x, new_shape):
-    self.x, self.ret = x, x.reduce_op(ReduceOps.MAX, new_shape)
+class Exp(Function):
+  def forward(self, x:LazyBuffer) -> LazyBuffer:
+    self.ret = x.e(BinaryOps.MUL, x.const(1/math.log(2))).e(UnaryOps.EXP2)
     return self.ret
 
-  def backward(self, grad_output):
-    # 1s in locations where the max was chosen (can be two locations)
-    max_is_1s = self.x.binary_op(BinaryOps.CMPEQ, self.ret.movement_op(MovementOps.EXPAND, self.x.shape))
+  def backward(self, grad_output:LazyBuffer) -> LazyBuffer:
+    return self.ret.e(BinaryOps.MUL, grad_output)
 
-    # sum of locations, averaged
-    div = max_is_1s.reduce_op(ReduceOps.SUM, grad_output.shape).movement_op(MovementOps.EXPAND, self.x.shape)
-    max_is_amount = max_is_1s.binary_op(BinaryOps.DIV, div)
+class Sqrt(Function):
+  def forward(self, x:LazyBuffer) -> LazyBuffer:
+    self.ret = x.e(UnaryOps.SQRT)
+    return self.ret
 
-    grad_output_expanded = grad_output.movement_op(MovementOps.EXPAND, self.x.shape)
-    return max_is_amount.binary_op(BinaryOps.MUL, grad_output_expanded)
+  def backward(self, grad_output:LazyBuffer) -> LazyBuffer:
+    return grad_output.e(BinaryOps.DIV, self.ret.e(BinaryOps.MUL, self.ret.const(2)))
 
-# ************* binary ops *************
+# NOTE: the implicit derivative of sigmoid is not stable
+# https://towardsdatascience.com/derivative-of-the-sigmoid-function-536880cf918e
+# TODO: have the backend automatically find this
+class Sigmoid(Function):
+  def forward(self, x:LazyBuffer) -> LazyBuffer:
+    self.ret = x.const(1).e(BinaryOps.DIV, x.const(1).e(BinaryOps.ADD, x.e(BinaryOps.MUL, x.const(-1/math.log(2))).e(UnaryOps.EXP2)))
+    return self.ret
 
-class Equal(Function):
-  def forward(self, x, y):
-    return x.binary_op(BinaryOps.CMPEQ, y)
+  def backward(self, grad_output:LazyBuffer) -> LazyBuffer:
+    return self.ret.e(BinaryOps.MUL, self.ret.const(1).e(BinaryOps.SUB, self.ret)).e(BinaryOps.MUL, grad_output)
 
-class Maximum(Function):
-  def forward(self, x, y):
-    self.y, self.ret = y, x.binary_op(BinaryOps.MAX, y)
-    return self.ret
+# ************* binary ops *************
 
-  def backward(self, grad_output):
-    mask = self.y.binary_op(BinaryOps.CMPEQ, self.ret)
-    # TODO: if they are equal, do they split the gradient?
-    return grad_output.binary_op(BinaryOps.MUL, mask.unary_op(UnaryOps.NOT)) if self.needs_input_grad[0] else None, \
-           grad_output.binary_op(BinaryOps.MUL, mask) if self.needs_input_grad[1] else None
+class Less(Function):
+  def forward(self, x:LazyBuffer, y:LazyBuffer) -> LazyBuffer:
+    return x.e(BinaryOps.CMPLT, y)
 
 class Add(Function):
-  def forward(self, x, y):
-    return x.binary_op(BinaryOps.ADD, y)
+  def forward(self, x:LazyBuffer, y:LazyBuffer) -> LazyBuffer:
+    return x.e(BinaryOps.ADD, y)
 
-  def backward(self, grad_output):
+  def backward(self, grad_output:LazyBuffer) -> Tuple[Optional[LazyBuffer], Optional[LazyBuffer]]:
     return grad_output if self.needs_input_grad[0] else None, \
            grad_output if self.needs_input_grad[1] else None
 
 class Sub(Function):
-  def forward(self, x, y):
-    return x.binary_op(BinaryOps.SUB, y)
+  def forward(self, x:LazyBuffer, y:LazyBuffer) -> LazyBuffer:
+    return x.e(BinaryOps.SUB, y)
 
-  def backward(self, grad_output):
+  def backward(self, grad_output:LazyBuffer) -> Tuple[Optional[LazyBuffer], Optional[LazyBuffer]]:
     return grad_output if self.needs_input_grad[0] else None, \
-           grad_output.unary_op(UnaryOps.NEG) if self.needs_input_grad[1] else None
+           grad_output.e(UnaryOps.NEG) if self.needs_input_grad[1] else None
 
 class Mul(Function):
-  def forward(self, x, y):
+  def forward(self, x:LazyBuffer, y:LazyBuffer) -> LazyBuffer:
     self.x, self.y = x, y
-    return x.binary_op(BinaryOps.MUL, y)
-
-  def backward(self, grad_output):
-    return self.y.binary_op(BinaryOps.MUL, grad_output) if self.needs_input_grad[0] else None, \
-           self.x.binary_op(BinaryOps.MUL, grad_output) if self.needs_input_grad[1] else None
+    return x.e(BinaryOps.MUL, y)
 
-class Pow(Function):
-  def forward(self, x, y):
-    self.x, self.y, self.ret = x, y, x.binary_op(BinaryOps.POW, y)
-    return self.ret
-
-  def backward(self, grad_output):
-    return grad_output.binary_op(BinaryOps.MUL, self.y.binary_op(BinaryOps.MUL, self.ret.binary_op(BinaryOps.DIV, self.x))) if self.needs_input_grad[0] else None, \
-           grad_output.binary_op(BinaryOps.MUL, self.x.unary_op(UnaryOps.LOG).binary_op(BinaryOps.MUL, self.ret)) if self.needs_input_grad[1] else None
+  def backward(self, grad_output:LazyBuffer) -> Tuple[Optional[LazyBuffer], Optional[LazyBuffer]]:
+    return self.y.e(BinaryOps.MUL, grad_output) if self.needs_input_grad[0] else None, \
+           self.x.e(BinaryOps.MUL, grad_output) if self.needs_input_grad[1] else None
 
 class Div(Function):
-  def forward(self, x, y):
+  def forward(self, x:LazyBuffer, y:LazyBuffer) -> LazyBuffer:
     self.x, self.y = x, y
-    return x.binary_op(BinaryOps.DIV, y)
+    return x.e(BinaryOps.DIV, y)
+
+  def backward(self, grad_output:LazyBuffer) -> Tuple[Optional[LazyBuffer], Optional[LazyBuffer]]:
+    return grad_output.e(BinaryOps.DIV, self.y) if self.needs_input_grad[0] else None, \
+           grad_output.e(UnaryOps.NEG).e(BinaryOps.MUL, self.x).e(BinaryOps.DIV, self.y.e(BinaryOps.MUL, self.y)) if self.needs_input_grad[1] else None
 
-  def backward(self, grad_output):
-    return grad_output.binary_op(BinaryOps.DIV, self.y) if self.needs_input_grad[0] else None, \
-           grad_output.unary_op(UnaryOps.NEG).binary_op(BinaryOps.MUL, self.x).binary_op(BinaryOps.DIV, self.y.binary_op(BinaryOps.MUL, self.y)) if self.needs_input_grad[1] else None
+# ************* ternary ops *************
+
+class Where(Function):
+  def forward(self, x:LazyBuffer, y:LazyBuffer, z:LazyBuffer) -> LazyBuffer:
+    self.x = x
+    return x.e(TernaryOps.WHERE, y, z)
+
+  def backward(self, grad_output:LazyBuffer) -> Tuple[None, Optional[LazyBuffer], Optional[LazyBuffer]]:
+    return None, \
+           self.x.e(TernaryOps.WHERE, grad_output, grad_output.const(0)) if self.needs_input_grad[1] else None, \
+           self.x.e(TernaryOps.WHERE, grad_output.const(0), grad_output) if self.needs_input_grad[2] else None
+
+# ************* reduce ops *************
+
+class Sum(Function):
+  def forward(self, x:LazyBuffer, new_shape:Tuple[int, ...]) -> LazyBuffer:
+    self.input_shape = x.shape
+    return x.r(ReduceOps.SUM, new_shape)
+
+  def backward(self, grad_output:LazyBuffer) -> LazyBuffer:
+    return grad_output.expand(self.input_shape)
+
+class Max(Function):
+  def forward(self, x:LazyBuffer, new_shape:Tuple[int, ...]) -> LazyBuffer:
+    self.x, self.ret = x, x.r(ReduceOps.MAX, new_shape)
+    return self.ret
+
+  def backward(self, grad_output:LazyBuffer) -> LazyBuffer:
+    # 1s in locations where the max was chosen (can be two locations)
+    max_is_1s = self.x.const(1.0).e(BinaryOps.SUB, self.x.e(BinaryOps.CMPLT, self.ret.expand(self.x.shape)))
+    div = max_is_1s.r(ReduceOps.SUM, grad_output.shape).expand(self.x.shape)
+    return max_is_1s.e(BinaryOps.DIV, div).e(BinaryOps.MUL, grad_output.expand(self.x.shape))
 
 # ************* movement ops *************
 
 # NOTE: this is sum in reverse
 class Expand(Function):
-  def forward(self, x, shape):
+  def forward(self, x:LazyBuffer, shape:Tuple[int, ...]) -> LazyBuffer:
     self.input_shape = x.shape
-    return x.movement_op(MovementOps.EXPAND, shape)
+    return x.expand(shape)
 
-  def backward(self, grad_output):
-    return grad_output.reduce_op(ReduceOps.SUM, self.input_shape)
+  def backward(self, grad_output:LazyBuffer) -> LazyBuffer:
+    return grad_output.r(ReduceOps.SUM, self.input_shape)
 
 class Reshape(Function):
-  def forward(self, x, shape):
+  def forward(self, x:LazyBuffer, shape:Tuple[int, ...]) -> LazyBuffer:
     self.input_shape = x.shape
-    return x.movement_op(MovementOps.RESHAPE, shape)
+    return x.reshape(shape)
 
-  def backward(self, grad_output):
-    return grad_output.movement_op(MovementOps.RESHAPE, self.input_shape)
+  def backward(self, grad_output:LazyBuffer) -> LazyBuffer:
+    return grad_output.reshape(self.input_shape)
 
 class Permute(Function):
-  def forward(self, x, order=(1,0)):
+  def forward(self, x:LazyBuffer, order:Tuple[int, ...]) -> LazyBuffer:
     self.input_order = order
-    return x.movement_op(MovementOps.PERMUTE, order)
+    return x.permute(order)
 
-  def backward(self, grad_output):
-    return grad_output.movement_op(MovementOps.PERMUTE, tuple(argsort(self.input_order)))
+  def backward(self, grad_output:LazyBuffer) -> LazyBuffer:
+    return grad_output.permute(argsort(self.input_order))
 
 class Pad(Function):
-  def forward(self, x, arg):
-    self.narg = tuple((p[0], s+p[0]) for s,p in zip(x.shape, arg))
-    return x.movement_op(MovementOps.PAD, arg)
+  def forward(self, x:LazyBuffer, arg:Tuple[Tuple[int, int], ...]) -> LazyBuffer:
+    self.narg = tuple([(p[0], s+p[0]) for s,p in zip(x.shape, arg)])
+    return x.pad(arg)
 
-  def backward(self, grad_output):
-    return grad_output.movement_op(MovementOps.SHRINK, self.narg)
+  def backward(self, grad_output:LazyBuffer) -> LazyBuffer:
+    return grad_output.shrink(self.narg)
 
 class Shrink(Function):
-  def forward(self, x, arg):
-    self.narg = tuple((p[0], s-p[1]) for s,p in zip(x.shape, arg))
-    return x.movement_op(MovementOps.SHRINK, arg)
+  def forward(self, x:LazyBuffer, arg:Tuple[Tuple[sint, sint], ...]) -> LazyBuffer:
+    self.narg = tuple([(p[0], s-p[1]) for s,p in zip(x.shape, arg)])
+    return x.shrink(arg)
 
-  def backward(self, grad_output):
-    return grad_output.movement_op(MovementOps.PAD, self.narg)
+  def backward(self, grad_output:LazyBuffer) -> LazyBuffer:
+    assert all(isinstance(x[0], int) and isinstance(x[1], int) for x in self.narg), "symbolic shrink does not support backward"
+    # need this cast because mypy cannot narrow the type even with assert
+    return grad_output.pad(cast(Tuple[Tuple[int, int], ...], self.narg))
 
 class Flip(Function):
-  def forward(self, x, axis):
-    self.axis = axis
-    return x.movement_op(MovementOps.FLIP, axis)
+  def forward(self, x:LazyBuffer, axis:Tuple[int, ...]) -> LazyBuffer:
+    self.arg = tuple([-1 if i in set(axis) else 1 for i in range(len(x.shape))])
+    return x.stride(self.arg)
 
-  def backward(self, grad_output):
-    return grad_output.movement_op(MovementOps.FLIP, self.axis)
+  def backward(self, grad_output:LazyBuffer) -> LazyBuffer:
+    return grad_output.stride(self.arg)
diff --git a/tinygrad_repo/tinygrad/nn/__init__.py b/tinygrad_repo/tinygrad/nn/__init__.py
index 31b9193040..fbf9597133 100644
--- a/tinygrad_repo/tinygrad/nn/__init__.py
+++ b/tinygrad_repo/tinygrad/nn/__init__.py
@@ -1,12 +1,14 @@
+import math
 from typing import Optional, Union, Tuple
 from tinygrad.tensor import Tensor
+from tinygrad.helpers import prod, all_int
 
 class BatchNorm2d:
   def __init__(self, sz, eps=1e-5, affine=True, track_running_stats=True, momentum=0.1):
-    assert affine, "BatchNorm2d is only supported with affine"
     self.eps, self.track_running_stats, self.momentum = eps, track_running_stats, momentum
 
-    self.weight, self.bias = Tensor.ones(sz), Tensor.zeros(sz)
+    if affine: self.weight, self.bias = Tensor.ones(sz), Tensor.zeros(sz)
+    else: self.weight, self.bias = None, None
 
     self.running_mean, self.running_var = Tensor.zeros(sz, requires_grad=False), Tensor.ones(sz, requires_grad=False)
     self.num_batches_tracked = Tensor.zeros(1, requires_grad=False)
@@ -16,53 +18,70 @@ class BatchNorm2d:
       # This requires two full memory accesses to x
       # https://github.com/pytorch/pytorch/blob/c618dc13d2aa23625cb0d7ada694137532a4fa33/aten/src/ATen/native/cuda/Normalization.cuh
       # There's "online" algorithms that fix this, like https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Welford's_Online_algorithm
-      x_detached = x.detach()
-      batch_mean = x_detached.mean(axis=(0,2,3))
-      y = (x_detached - batch_mean.reshape(shape=[1, -1, 1, 1]))
+      batch_mean = x.mean(axis=(0,2,3))
+      y = (x - batch_mean.reshape(shape=[1, -1, 1, 1]))
       batch_var = (y*y).mean(axis=(0,2,3))
       batch_invstd = batch_var.add(self.eps).pow(-0.5)
-      self.batch_invstd = None
 
       # NOTE: wow, this is done all throughout training in most PyTorch models
       if self.track_running_stats:
-        self.running_mean.assign((1 - self.momentum) * self.running_mean + self.momentum * batch_mean)
-        self.running_var.assign((1 - self.momentum) * self.running_var + self.momentum * batch_var)
+        self.running_mean.assign((1 - self.momentum) * self.running_mean + self.momentum * batch_mean.detach())
+        self.running_var.assign((1 - self.momentum) * self.running_var + self.momentum * prod(y.shape)/(prod(y.shape) - y.shape[1]) * batch_var.detach() )
         self.num_batches_tracked += 1
     else:
-      batch_mean, batch_var = self.running_mean, self.running_var
-      # NOTE: this can be precomputed for static inference. if you manually update running_var, you have to reset this
-      if not hasattr(self, "batch_invstd") or not self.batch_invstd:
-        self.batch_invstd = batch_var.add(self.eps).pow(-0.5)
-      batch_invstd = self.batch_invstd
+      batch_mean = self.running_mean
+      # NOTE: this can be precomputed for static inference. we expand it here so it fuses
+      batch_invstd = self.running_var.reshape(1, -1, 1, 1).expand(x.shape).add(self.eps).rsqrt()
 
     return x.batchnorm(self.weight, self.bias, batch_mean, batch_invstd)
 
-# TODO: is this good weight init?
+# TODO: these Conv lines are terrible
+def Conv1d(in_channels, out_channels, kernel_size, stride=1, padding=0, dilation=1, groups=1, bias=True):
+  return Conv2d(in_channels, out_channels, (kernel_size,), stride, padding, dilation, groups, bias)
+
 class Conv2d:
-  def __init__(self, in_channels, out_channels, kernel_size, stride=1, padding=0, bias=True):
-    self.kernel_size = (kernel_size, kernel_size) if isinstance(kernel_size, int) else (kernel_size[0], kernel_size[1])
-    self.stride = (stride, stride) if isinstance(stride, int) else (stride[0], stride[1])
-    self.padding = (padding, ) * 4 if isinstance(padding, int) else ((padding[0], padding[0], padding[1], padding[1]) if len(padding) == 2 else padding)
-    # TODO: why is this realize needed? shouldn't it realize on the first run?
-    self.weight = Tensor.glorot_uniform(out_channels, in_channels, self.kernel_size[0], self.kernel_size[1]).realize()
-    self.bias = Tensor.zeros(out_channels).contiguous().realize() if bias else None
+  def __init__(self, in_channels, out_channels, kernel_size, stride=1, padding=0, dilation=1, groups=1, bias=True):
+    self.kernel_size = (kernel_size, kernel_size) if isinstance(kernel_size, int) else tuple(kernel_size)
+    self.stride, self.padding, self.dilation, self.groups = stride, padding, dilation, groups
+    self.weight = self.initialize_weight(out_channels, in_channels, groups)
+    assert all_int(self.weight.shape), "does not support symbolic shape"
+    bound = 1 / math.sqrt(prod(self.weight.shape[1:]))
+    self.bias = Tensor.uniform(out_channels, low=-bound, high=bound) if bias else None
+
+  def __call__(self, x:Tensor):
+    return x.conv2d(self.weight, self.bias, padding=self.padding, stride=self.stride, dilation=self.dilation, groups=self.groups)
+
+  def initialize_weight(self, out_channels, in_channels, groups): return Tensor.kaiming_uniform(out_channels, in_channels//groups, *self.kernel_size, a=math.sqrt(5))
+
+def ConvTranspose1d(in_channels, out_channels, kernel_size, stride=1, padding=0, output_padding=0, dilation=1, groups=1, bias=True):
+  return ConvTranspose2d(in_channels, out_channels, (kernel_size,), stride, padding, output_padding, dilation, groups, bias)
+
+class ConvTranspose2d(Conv2d):
+  def __init__(self, in_channels, out_channels, kernel_size, stride=1, padding=0, output_padding=0, dilation=1, groups=1, bias=True):
+    super().__init__(in_channels, out_channels, kernel_size, stride, padding, dilation, groups, bias)
+    self.output_padding = output_padding
+
+  def __call__(self, x:Tensor):
+    return x.conv_transpose2d(self.weight, self.bias, padding=self.padding, output_padding=self.output_padding, stride=self.stride, dilation=self.dilation, groups=self.groups)
 
-  def __call__(self, x):
-    return x.conv2d(self.weight, self.bias, padding=self.padding, stride=self.stride)
+  def initialize_weight(self, out_channels, in_channels, groups): return Tensor.kaiming_uniform(in_channels, out_channels//groups, *self.kernel_size, a=math.sqrt(5))
 
 class Linear:
   def __init__(self, in_features, out_features, bias=True):
-    self.weight = Tensor.glorot_uniform(out_features, in_features).realize()
-    self.bias = Tensor.zeros(out_features).contiguous().realize() if bias else None
+    self.weight = Tensor.kaiming_uniform(out_features, in_features, a=math.sqrt(5))
+    # TODO: remove this once we can represent Tensor with int shape in typing
+    assert isinstance(self.weight.shape[1], int), "does not support symbolic shape"
+    bound = 1 / math.sqrt(self.weight.shape[1])
+    self.bias = Tensor.uniform(out_features, low=-bound, high=bound) if bias else None
 
-  def __call__(self, x):
+  def __call__(self, x:Tensor):
     return x.linear(self.weight.transpose(), self.bias)
 
 class GroupNorm:
   def __init__(self, num_groups:int, num_channels:int, eps:float=1e-5, affine:bool=True):
     self.num_groups, self.num_channels, self.eps = num_groups, num_channels, eps
-    self.weight : Optional[Tensor] = Tensor.ones(num_channels) if affine else None
-    self.bias : Optional[Tensor] = Tensor.zeros(num_channels) if affine else None
+    self.weight: Optional[Tensor] = Tensor.ones(num_channels) if affine else None
+    self.bias: Optional[Tensor] = Tensor.zeros(num_channels) if affine else None
 
   def __call__(self, x:Tensor):
     # reshape for layernorm to work as group norm
@@ -71,15 +90,39 @@ class GroupNorm:
 
     if self.weight is None or self.bias is None: return x
     # elementwise_affine on channels
-    return x * self.weight.reshape(1, -1, 1, 1) + self.bias.reshape(1, -1, 1, 1)
+    return x * self.weight.reshape(1, -1, *[1] * (len(x.shape)-2)) + self.bias.reshape(1, -1, *[1] * (len(x.shape)-2))
+
+class InstanceNorm:
+  def __init__(self, num_features:int, eps:float=1e-5, affine:bool=True):
+    self.num_features, self.eps = num_features, eps
+    self.weight: Optional[Tensor] = Tensor.ones(num_features) if affine else None
+    self.bias: Optional[Tensor] = Tensor.zeros(num_features) if affine else None
+
+  def __call__(self, x:Tensor):
+    x = x.reshape(x.shape[0], self.num_features, -1).layernorm(eps=self.eps).reshape(x.shape)
+    if self.weight is None or self.bias is None: return x
+    return x * self.weight.reshape(1, -1, *[1] * (len(x.shape)-2)) + self.bias.reshape(1, -1, *[1] * (len(x.shape)-2))
 
 class LayerNorm:
   def __init__(self, normalized_shape:Union[int, Tuple[int, ...]], eps:float=1e-5, elementwise_affine:bool=True):
-    normalized_shape = (normalized_shape,) if isinstance(normalized_shape, int) else tuple(normalized_shape)
-    self.axis, self.eps, self.elementwise_affine = tuple(-1-i for i in range(len(normalized_shape))), eps, elementwise_affine
-    self.weight, self.bias = (Tensor.ones(*normalized_shape), Tensor.zeros(*normalized_shape)) if elementwise_affine else (None, None)
+    self.normalized_shape = (normalized_shape,) if isinstance(normalized_shape, int) else tuple(normalized_shape)
+    self.axis, self.eps, self.elementwise_affine = tuple(-1-i for i in range(len(self.normalized_shape))), eps, elementwise_affine
+    self.weight, self.bias = (Tensor.ones(*self.normalized_shape), Tensor.zeros(*self.normalized_shape)) if elementwise_affine else (None, None)
 
   def __call__(self, x:Tensor):
+    assert self.normalized_shape == x.shape[-len(self.normalized_shape):], f"last dimensions of {x.shape} must match {self.normalized_shape}"
     x = x.layernorm(eps=self.eps, axis=self.axis)
     if not self.elementwise_affine: return x
     return x * self.weight + self.bias
+
+class LayerNorm2d(LayerNorm):
+  def __call__(self, x): return super().__call__(x.permute(0, 2, 3, 1)).permute(0, 3, 1, 2)
+
+class Embedding:
+  def __init__(self, vocab_size:int, embed_size:int):
+    self.vocab_size = vocab_size
+    self.weight = Tensor.glorot_uniform(vocab_size, embed_size)
+
+  def __call__(self, idx:Tensor) -> Tensor:
+    if not hasattr(self, 'vocab_counter'): self.vocab_counter = Tensor.arange(self.vocab_size, requires_grad=False).reshape(1, 1, self.vocab_size)
+    return (self.vocab_counter == idx.unsqueeze(2)).expand(*idx.shape, self.vocab_size) @ self.weight
diff --git a/tinygrad_repo/tinygrad/nn/optim.py b/tinygrad_repo/tinygrad/nn/optim.py
index 759886c04c..84948c7fcb 100644
--- a/tinygrad_repo/tinygrad/nn/optim.py
+++ b/tinygrad_repo/tinygrad/nn/optim.py
@@ -1,87 +1,68 @@
 # sorted in order of increasing complexity
 from typing import List
+from tinygrad.helpers import dedup
 from tinygrad.tensor import Tensor
 
 class Optimizer:
-  def __init__(self, params : List[Tensor]):
+  def __init__(self, params: List[Tensor], lr: float):
     # if it's None, but being put into an optimizer, set it to True
     for x in params:
       if x.requires_grad is None: x.requires_grad = True
 
-    self.params : List[Tensor] = [x for x in params if x.requires_grad]
-    self.buffers : List[Tensor] = [x for x in params if not x.requires_grad]   # buffers are still realized
-
-  # TODO: this probably shouldn't change the gradients, just the ones used by the optimizer
-  def clipnorm(self, amount=1):
-    for param in self.params:
-      assert param.grad is not None
-      # clipnorm is the L2 norm, not value: is this right?
-      param.grad.assign(param.grad.clip(-(amount**2), (amount**2)))
+    self.params: List[Tensor] = dedup([x for x in params if x.requires_grad])
+    self.buffers: List[Tensor] = dedup([x for x in params if not x.requires_grad])   # buffers are still realized
+    self.lr = Tensor([lr], requires_grad=False).contiguous()
 
   def zero_grad(self):
     for param in self.params: param.grad = None
 
   def realize(self, extra=None):
-    # TODO: corealize
-    for p in extra + self.params + self.buffers if extra is not None else self.params + self.buffers:
-      p.realize()
+    # NOTE: in extra is too late for most of the params due to issues with assign
+    Tensor.corealize(extra + self.params + self.buffers if extra is not None else self.params + self.buffers)
 
 class SGD(Optimizer):
-  def __init__(self, params : List[Tensor], lr=0.001, momentum=0, nesterov=False):
-    super().__init__(params)
-    self.lr, self.momentum, self.nesterov = lr, momentum, nesterov
-    self.b = [Tensor.zeros(*t.shape, device=params[0].device, requires_grad=False) for t in self.params] if self.momentum else []
+  def __init__(self, params: List[Tensor], lr=0.001, momentum=0, weight_decay=0.0, nesterov=False):
+    super().__init__(params, lr)
+    self.momentum, self.wd, self.nesterov = momentum, weight_decay, nesterov
+    self.b = [Tensor.zeros(*t.shape, device=t.device, requires_grad=False) for t in self.params] if self.momentum else []
 
   # https://pytorch.org/docs/stable/generated/torch.optim.SGD.html
   def step(self) -> None:
     for i, t in enumerate(self.params):
       assert t.grad is not None
-      g = t.grad
+      g = t.grad.realize() + self.wd * t.detach()
       if self.momentum:
-        self.b[i].assign(self.momentum * self.b[i] + g)
+        self.b[i].assign(self.momentum * self.b[i] + g).realize()  # NOTE: self.b[i] is zero on the first run, no if required
         g = (g + self.momentum * self.b[i]) if self.nesterov else self.b[i]
       t.assign(t.detach() - g * self.lr)
     self.realize(self.b)
 
-class RMSprop(Optimizer):
-  def __init__(self, params : List[Tensor], lr=0.001, decay=0.9, eps=1e-8):
-    super().__init__(params)
-    self.lr, self.decay, self.eps = lr, decay, eps
-
-    self.v = [Tensor.zeros(*t.shape, device=params[0].device, requires_grad=False) for t in self.params]
-
-  def step(self) -> None:
-    for i, t in enumerate(self.params):
-      assert t.grad is not None
-      self.v[i].assign(self.decay * self.v[i] + (1.0 - self.decay) * (t.grad * t.grad))
-      t.assign(t.detach() - (t.grad * self.lr).div(self.v[i].sqrt() + self.eps))
-    self.realize(self.v)
-
-class Adam(Optimizer):
-  def __init__(self, params : List[Tensor], lr=0.001, b1=0.9, b2=0.999, eps=1e-8):
-    super().__init__(params)
-    # NOTE: self.t is a tensor so Adam can be jitted
-    self.lr, self.b1, self.b2, self.eps, self.t = lr, b1, b2, eps, Tensor([0], requires_grad=False).realize()
+# LAMB is essentially just the trust ratio part of LARS applied to Adam/W so if we just set the trust ratio to 1.0 its just Adam/W.
+def AdamW(params: List[Tensor], lr=0.001, b1=0.9, b2=0.999, eps=1e-8, wd=0.01): return LAMB(params, lr, b1, b2, eps, wd, adam=True)
+def Adam(params: List[Tensor], lr=0.001, b1=0.9, b2=0.999, eps=1e-8): return LAMB(params, lr, b1, b2, eps, 0.0, adam=True)
 
-    self.m = [Tensor.zeros(*t.shape, device=params[0].device, requires_grad=False) for t in self.params]
-    self.v = [Tensor.zeros(*t.shape, device=params[0].device, requires_grad=False) for t in self.params]
+class LAMB(Optimizer):
+  def __init__(self, params: List[Tensor], lr=0.001, b1=0.9, b2=0.999, eps=1e-6, wd=0.0, adam=False):
+    super().__init__(params, lr)
+    self.b1, self.b2, self.eps, self.wd, self.adam, self.t = b1, b2, eps, wd, adam, Tensor([0], requires_grad=False).realize()
+    self.m = [Tensor.zeros(*t.shape, device=t.device, requires_grad=False) for t in self.params]
+    self.v = [Tensor.zeros(*t.shape, device=t.device, requires_grad=False) for t in self.params]
 
   def step(self) -> None:
-    self.t = self.t + 1
-    a = self.lr * ((1.0 - self.b2**self.t)**0.5) / (1.0 - self.b1**self.t)
+    self.t.assign(self.t + 1).realize()
     for i, t in enumerate(self.params):
       assert t.grad is not None
-      self.m[i].assign(self.b1 * self.m[i] + (1.0 - self.b1) * t.grad)
-      self.v[i].assign(self.b2 * self.v[i] + (1.0 - self.b2) * (t.grad * t.grad))
-      t.assign(t.detach() - a * self.m[i].div(self.v[i].sqrt() + self.eps))
+      g = t.grad.realize()
+      self.m[i].assign(self.b1 * self.m[i] + (1.0 - self.b1) * g).realize()
+      self.v[i].assign(self.b2 * self.v[i] + (1.0 - self.b2) * (g * g)).realize()
+      m_hat = self.m[i] / (1.0 - self.b1**self.t)
+      v_hat = self.v[i] / (1.0 - self.b2**self.t)
+      up = (m_hat / (v_hat.sqrt() + self.eps)) + self.wd * t.detach()
+      if not self.adam:
+        r1 = t.detach().square().sum().sqrt()
+        r2 = up.square().sum().sqrt()
+        r = Tensor.where(r1 > 0, Tensor.where(r2 > 0, r1 / r2, 1.0), 1.0)
+      else:
+        r = 1.0
+      t.assign(t.detach() - self.lr * r * up)
     self.realize([self.t] + self.m + self.v)
-
-def get_parameters(obj) -> List[Tensor]:
-  parameters : List[Tensor] = []
-  if isinstance(obj, Tensor):
-    parameters.append(obj)
-  elif isinstance(obj, (list, tuple)):
-    for x in obj: parameters.extend(get_parameters(x))
-  elif hasattr(obj, '__dict__'):
-    for v in obj.__dict__.values(): parameters.extend(get_parameters(v))
-  return parameters
diff --git a/tinygrad_repo/tinygrad/nn/state.py b/tinygrad_repo/tinygrad/nn/state.py
new file mode 100644
index 0000000000..beabe4bd9f
--- /dev/null
+++ b/tinygrad_repo/tinygrad/nn/state.py
@@ -0,0 +1,124 @@
+import os, json, pathlib, zipfile, pickle
+from tqdm import tqdm
+from typing import Dict, Union, List, Optional, Any, Tuple
+from tinygrad.tensor import Tensor
+from tinygrad.helpers import dtypes, prod, argsort, DEBUG, Timing, GlobalCounters, CI
+from tinygrad.shape.view import strides_for_shape
+from tinygrad.ops import Device
+
+safe_dtypes = {"F16": dtypes.float16, "F32": dtypes.float32, "U8": dtypes.uint8, "I8": dtypes.int8, "I32": dtypes.int32, "I64": dtypes.int64}
+inverse_safe_dtypes = {v:k for k,v in safe_dtypes.items()}
+
+def safe_load_metadata(fn:Union[Tensor,str]) -> Tuple[Tensor, int, Any]:
+  t = fn if isinstance(fn, Tensor) else Tensor.empty(os.stat(fn).st_size, dtype=dtypes.uint8, device=f"disk:{fn}")
+  json_len = t[0:1].cast(dtypes.int64).numpy()[0]
+  return (t, json_len, json.loads(t[8:8+json_len].numpy().tobytes()))
+
+def safe_load(fn:Union[Tensor,str]) -> Dict[str, Tensor]:
+  t, json_len, metadata = safe_load_metadata(fn)
+  return {k:t[8+json_len+v['data_offsets'][0]:].cast(safe_dtypes[v['dtype']])[:prod(v['shape'])].reshape(v['shape']) for k,v in metadata.items() if k != "__metadata__"}
+
+def safe_save(tensors:Dict[str, Tensor], fn:str, metadata:Optional[Dict[str, Any]]=None):
+  headers, offset = {}, 0
+  if metadata: headers['__metadata__'] = metadata
+  for k,v in tensors.items():
+    headers[k] = {'dtype': inverse_safe_dtypes[v.dtype], 'shape': list(v.shape), 'data_offsets':[offset, offset+v.nbytes()]}
+    offset += v.nbytes()
+  j = json.dumps(headers, separators=(',', ':'))
+  j += "\x20"*((8-len(j)%8)%8)
+  pathlib.Path(fn).unlink(missing_ok=True)
+  t = Tensor.empty(8+len(j)+offset, dtype=dtypes.uint8, device=f"disk:{fn}")
+  t[0:1].cast(dtypes.int64).assign([len(j)])
+  t[8:8+len(j)].assign(Tensor(list(j.encode('utf-8')), dtype=dtypes.uint8, device="cpu"))
+  for k,v in safe_load(t).items(): v.assign(tensors[k])
+
+# state dict
+
+from collections import OrderedDict
+def get_state_dict(obj, prefix:str='', tensor_type=Tensor) -> Dict[str, Tensor]:
+  if isinstance(obj, tensor_type): return {prefix.strip('.'):obj}
+  if hasattr(obj, '_asdict'): return get_state_dict(obj._asdict(), prefix, tensor_type)  # namedtuple
+  if isinstance(obj, OrderedDict): return get_state_dict(dict(obj), prefix, tensor_type)
+  if hasattr(obj, '__dict__'): return get_state_dict(obj.__dict__, prefix, tensor_type)
+  state_dict = {}
+  if isinstance(obj, (list, tuple)):
+    for i,x in enumerate(obj): state_dict.update(get_state_dict(x, f"{prefix}{str(i)}.", tensor_type))
+  elif isinstance(obj, dict):
+    for k,v in obj.items(): state_dict.update(get_state_dict(v, f"{prefix}{str(k)}.", tensor_type))
+  return state_dict
+def get_parameters(obj) -> List[Tensor]: return list(get_state_dict(obj).values())
+
+def load_state_dict(model, state_dict, strict=True, verbose=True):
+  with Timing("loaded weights in ", lambda et_ns: f", {GlobalCounters.mem_used/1e9:.2f} GB loaded at {GlobalCounters.mem_used/et_ns:.2f} GB/s"):
+    model_state_dict = get_state_dict(model)
+    if DEBUG >= 1 and len(state_dict) > len(model_state_dict): print("WARNING: unused weights in state_dict", sorted(list(state_dict.keys() - model_state_dict.keys())))
+    for k,v in (t := tqdm(model_state_dict.items(), disable=CI or not verbose)):
+      t.set_description(f"ram used: {GlobalCounters.mem_used/1e9:5.2f} GB, {k:50s}")
+      if k not in state_dict and not strict:
+        if DEBUG >= 1: print(f"WARNING: not loading {k}")
+        continue
+      v.assign(state_dict[k].to(v.device)).realize()
+
+# torch support!
+
+def torch_load(fn:str):
+  t = Tensor.empty(os.stat(fn).st_size, dtype=dtypes.uint8, device=f"disk:{fn}")
+
+  offsets: Dict[str, int] = {}
+  lens: Dict[str, int] = {}
+  def _rebuild_tensor_v2(storage, storage_offset, size, stride, requires_grad, backward_hooks, metadata=None):
+    #print(storage, storage_offset, size, stride, requires_grad, backward_hooks, metadata)
+    lens[storage[2]] = storage[4] * storage[1].itemsize
+    if storage[2] not in offsets: return None
+    byte_offset = offsets[storage[2]]+storage_offset*storage[1].itemsize
+    ret = t[byte_offset:byte_offset+prod(size)].cast(storage[1])
+    # convert bfloat16 -> float16 using LLVM for Llama 2
+    # upstream LLaMA also does this conversion:
+    # https://github.com/facebookresearch/llama/blob/6c7fe276574e78057f917549435a2554000a876d/llama/generation.py#L95
+    # TODO: should this be done in the example instead? or maybe we don't need this anymore with better bfloat16 support
+    if storage[1] == dtypes.bfloat16:
+      ret = ret.bitcast(dtypes.uint16).to("CPU").cast(dtypes.uint32).mul(1<<16).bitcast(dtypes.float32).to(Device.DEFAULT).half()
+      #ret = ret.to("LLVM").half().to(Device.DEFAULT)
+
+    # 7 lines to deal with permuted tensors. NOTE: this currently requires reading off the disk
+    shape_strides = [(s, st) for s,st in zip(size, stride) if s != 1]
+    permute_indexes = [len(shape_strides)-1-y for y in argsort([x[1] for x in shape_strides])]
+    if tuple(permute_indexes) != tuple(range(len(permute_indexes))):
+      intermediate_shape = tuple([shape_strides[x][0] for x in argsort(permute_indexes)])
+      assert tuple([shape_strides[i][1] for i in argsort(permute_indexes)]) == strides_for_shape(intermediate_shape), "nonpermutable strides"
+      if DEBUG >= 2: print(f"WARNING: this torch load is slow. CPU to permute {intermediate_shape} with {permute_indexes}")
+      # TODO: find a nice way to support all shapetracker on disktensors
+      ret = ret.cpu().reshape(intermediate_shape).permute(permute_indexes)
+
+    return ret.reshape(size)
+
+  intercept = {"HalfStorage": dtypes.float16, "FloatStorage": dtypes.float32, "BFloat16Storage": dtypes.bfloat16, "IntStorage": dtypes.int32, "LongStorage": dtypes.int64, "_rebuild_tensor_v2": _rebuild_tensor_v2}
+  whitelist = {"torch", "collections", "numpy", "_codecs"}  # NOTE: this is not for security, only speed
+  class Dummy: pass
+  class TorchPickle(pickle.Unpickler):
+    def find_class(self, module, name):
+      module_root = module.split(".")[0]
+      if module_root not in whitelist:
+        if DEBUG >= 2: print(f"WARNING: returning Dummy for {module} {name}")
+        return Dummy
+      return intercept[name] if module_root == "torch" else super().find_class(module, name)
+    def persistent_load(self, pid): return pid
+
+  if tuple(t[0:2].numpy()) == (0x50, 0x4b):
+    myzip = zipfile.ZipFile(fn, 'r')
+    base_name = myzip.namelist()[0].split('/', 1)[0]
+    for n in myzip.namelist():
+      if n.startswith(f'{base_name}/data/'):
+        with myzip.open(n) as myfile:
+          offsets[n.split("/")[-1]] = myfile._orig_compress_start # type: ignore
+    with myzip.open(f'{base_name}/data.pkl') as myfile:
+      return TorchPickle(myfile).load()
+  else:
+    with open(fn, "rb") as f:
+      pkl = TorchPickle(f)
+      _, _, _, rwd, _, ids, base_offset = pkl.load(), pkl.load(), pkl.load(), f.tell(), pkl.load(), pkl.load(), f.tell()
+      for i in ids:
+        offsets[i] = base_offset + 8
+        base_offset += 8 + lens[i]
+      f.seek(rwd)
+      return TorchPickle(f).load()
diff --git a/tinygrad_repo/tinygrad/ops.py b/tinygrad_repo/tinygrad/ops.py
index 6c0eb9bb07..bf877a9781 100644
--- a/tinygrad_repo/tinygrad/ops.py
+++ b/tinygrad_repo/tinygrad/ops.py
@@ -1,202 +1,300 @@
 from __future__ import annotations
-import functools, itertools, operator, random
-import numpy as np
+import importlib, inspect, functools, pathlib
 from enum import Enum, auto
-from typing import Union, Type, NamedTuple, Tuple, Any, List, ClassVar, Optional, Callable, Dict, TypeVar, Set
-from tinygrad.helpers import prod, DEBUG, getenv
-from tinygrad.shape import ShapeTracker
+from typing import TYPE_CHECKING, Union, Type, Tuple, Any, List, Optional, Dict, Callable, Mapping
+from tinygrad.helpers import ansilen, prod, DEBUG, getenv, GlobalCounters, DType, colored, BEAM, NOOPT
+from tinygrad.runtime.lib import RawBuffer
+from tinygrad.shape.symbolic import Variable, sym_infer
+from dataclasses import dataclass
 
 # these are the llops your accelerator must implement, along with toCpu
 # the Enum class doesn't work with mypy, this is static. sorry it's ugly
-class UnaryOps(Enum): NOOP = auto(); NEG = auto(); EXP = auto(); LOG = auto(); NOT = auto() # noqa: E702
-class BinaryOps(Enum): ADD = auto(); SUB = auto(); MUL = auto(); DIV = auto(); POW = auto(); CMPEQ = auto(); MAX = auto() # noqa: E702
+# NOTE: MOD, CMPLT don't have to be implemented on vectors, just scalars
+# NOTE: rdna3 only has RECIP and not DIV. DIV and POW are on the chopping block
+class UnaryOps(Enum): NOOP = auto(); EXP2 = auto(); LOG2 = auto(); CAST = auto(); SIN = auto(); SQRT = auto(); RECIP = auto(); NEG = auto() # noqa: E702
+class BinaryOps(Enum): ADD = auto(); SUB = auto(); MUL = auto(); DIV = auto(); MAX = auto(); MOD = auto(); CMPLT = auto() # noqa: E702
+class TernaryOps(Enum): MULACC = auto(); WHERE = auto() # noqa: E702
 class ReduceOps(Enum): SUM = auto(); MAX = auto() # noqa: E702
-class MovementOps(Enum): RESHAPE = auto(); PERMUTE = auto(); EXPAND = auto(); FLIP = auto(); PAD = auto(); SHRINK = auto() # noqa: E702
-class FusedOps(Enum): MULACC = auto() # noqa: E702
-class LoadOps(Enum): FROMCPU = auto(); CONTIGUOUS = auto() # noqa: E702
+class BufferOps(Enum): MEM = auto(); CONST = auto() # noqa: E702
+# Ops below this line are not allowed in ASTs
+class MovementOps(Enum): RESHAPE = auto(); PERMUTE = auto(); EXPAND = auto(); PAD = auto(); SHRINK = auto(); STRIDE = auto(); AS_STRIDED = auto() # noqa: E702
+class LoadOps(Enum): EMPTY = auto(); RAND = auto(); CONST = auto(); FROM = auto(); CONTIGUOUS = auto(); CUSTOM = auto() # noqa: E702
 
-Op = Union[UnaryOps, BinaryOps, ReduceOps, MovementOps, LoadOps, FusedOps]
-OpType = Union[Type[UnaryOps], Type[BinaryOps], Type[ReduceOps], Type[MovementOps], Type[LoadOps], Type[FusedOps]]
+Op = Union[UnaryOps, BinaryOps, ReduceOps, MovementOps, LoadOps, TernaryOps, BufferOps]
+OpType = Union[Type[UnaryOps], Type[BinaryOps], Type[ReduceOps], Type[MovementOps], Type[LoadOps], Type[TernaryOps], Type[BufferOps]]
 
-class LazyOp(NamedTuple):
+if TYPE_CHECKING:
+  from tinygrad.shape.shapetracker import ShapeTracker
+  from tinygrad.lazy import LazyBuffer
+
+@dataclass(frozen=True)
+class MemBuffer:
+  idx: int
+  dtype: DType
+  st: ShapeTracker
+
+@dataclass(frozen=True)
+class ConstBuffer:
+  val: Any
+  dtype: DType
+  st: ShapeTracker
+
+@dataclass(frozen=True)
+class ScheduleItem:
+  ast: LazyOp
+  out: LazyBuffer
+  inputs: Tuple[LazyBuffer, ...]
+  var_vals: Dict[Variable, int]
+
+@dataclass(frozen=True)
+class LazyOp:
   op: Op
-  # Any == Union[LazyOp, LazyBuffer, DeviceBuffer]
-  src: Tuple[Any, ...]  # type: ignore
+  src: Tuple[Union[LazyOp, LazyBuffer], ...]
   arg: Any = None
-  # TODO: add dest to support multiple outputs
-
-# Any == Union[LazyBuffer, DeviceBuffer]
-def get_buffers(op:LazyOp) -> List[Any]: return functools.reduce(operator.add, [get_buffers(x) if isinstance(x, LazyOp) else [x] for x in op.src], [])
-def get_lazyops(op:LazyOp) -> List[LazyOp]: return functools.reduce(operator.add, [get_lazyops(x) for x in op.src if isinstance(x, LazyOp)], [op])
-def map_buffers(real_srcs, x:LazyOp) -> LazyOp:
-  if x in real_srcs: return map_buffers(real_srcs, real_srcs[x]) if isinstance(real_srcs[x], LazyOp) else real_srcs[x]
-  return LazyOp(x.op, tuple((map_buffers(real_srcs, y) if isinstance(y, LazyOp) else real_srcs[y]) for y in x.src), x.arg)
-
-_T = TypeVar("_T")
-class RawBuffer:
-  def __init__(self, size): raise NotImplementedError("must be implemented")
-  @classmethod
-  def fromCPU(cls:Type[_T], x:np.ndarray) -> _T: raise NotImplementedError("must be implemented")
-  def toCPU(self:RawBuffer) -> np.ndarray: raise NotImplementedError("must be implemented")
-
-class RawBufferCopyIn(RawBuffer):
-  def copyin(self, x:np.ndarray) -> None: raise NotImplementedError("must be implemented")
-
-  @classmethod
-  def fromCPU(cls, x:np.ndarray):
-    ret = cls(4*prod(x.shape))
-    ret.copyin(x)
-    return ret
+  def __repr__(self): return f"LazyOp(op={self.op}, src={self.src}, arg={self.arg})"
+  @property
+  def buffers(self):
+    buffers: Tuple[Union[LazyOp, LazyBuffer], ...] = ()
+    try:  # NOTE: the linearizer's key function maps the buffers to ints, and LOCAL_BUFFER is used. we don't care about buffers in these cases
+      for x in self.src: buffers += x.buffers
+    except AttributeError: buffers = ()
+    return buffers
 
-class RawBufferCopyInOut(RawBufferCopyIn):
-  size : int
-  def copyout(self, x:np.ndarray) -> None: raise NotImplementedError("must be implemented")
+  @property
+  def key(self): return (self.op, tuple(map(lambda x: getattr(x, "key", x), self.src)), getattr(self.arg, "key", self.arg))
 
-  def toCPU(self) -> np.ndarray:
-    x = np.empty((self.size//4), dtype=np.float32)
-    self.copyout(x)
-    return x
+  def map_buffers(self, real_srcs: Mapping[Any, Union[LazyBuffer, LazyOp]]) -> LazyOp: return LazyOp(self.op, tuple([y.map_buffers(real_srcs) if y not in real_srcs else real_srcs[y] for y in self.src]), self.arg)
+  def get_lazyops(self) -> List[LazyOp]: return [self] + [item for x in self.src for item in x.get_lazyops()]
 
-# a placeholder class to extend by the exec classes
-class DeviceBuffer(RawBuffer):
-  _buf: Any                # underlying buffer
-  shape: Tuple[int, ...]
-  @classmethod
-  def exec_ast(cls, ast:LazyOp, output_buffer=None): raise NotImplementedError("must be implemented")
+  def replace_with_movement_ops(self:LazyOp, ops:List[Tuple[MovementOps, Tuple[Any, ...]]]) -> 'LazyBuffer':
+    assert self.op in BinaryOps or self.op in UnaryOps or self.op in TernaryOps
+    srcs = [z.replace_with_movement_ops(ops) for z in self.src]
+    return srcs[0].e(self.op, *srcs[1:], arg=self.arg)   # type: ignore
+
+  @property
+  def st(self): raise NotImplementedError
+  @property
+  def realized(self): raise NotImplementedError
+  @property
+  def children(self): raise NotImplementedError
+
+  # movement ops
+  def reshape(self, _): raise NotImplementedError
+  def pad(self, _): raise NotImplementedError
+  def expand(self, _): raise NotImplementedError
+  def permute(self, _): raise NotImplementedError
+  def shrink(self, _): raise NotImplementedError
+  def stride(self, _): raise NotImplementedError
+
+# **************** Device ****************
+
+class _Device:
+  def __init__(self) -> None: self._buffers: List[str] = [x.stem[len("ops_"):].upper() for x in (pathlib.Path(__file__).parent/"runtime").iterdir() if x.stem.startswith("ops_")]
+  def canonicalize(self, device:Optional[str]) -> str: return (device.split(":", 1)[0].upper() + ((":"+device.split(":", 1)[1]) if ':' in device else '')).replace(":0", "") if device is not None else self.DEFAULT
+  @functools.lru_cache(maxsize=None)  # this class is a singleton, pylint: disable=method-cache-max-size-none
+  def __getitem__(self, x:str) -> Union[Interpreted, Compiled]:
+    x = x.split(":")[0].upper()
+    return [cls for cname, cls in inspect.getmembers(importlib.import_module(f'tinygrad.runtime.ops_{x.lower()}')) if (cname.lower() == x.lower() + "buffer") and x in self._buffers][0]
+  @functools.cached_property
+  def DEFAULT(self) -> str:
+    device_from_env: Optional[str] = functools.reduce(lambda val, ele: ele if getenv(ele) == 1 else val, self._buffers, None)
+    if device_from_env: return device_from_env
+    for device in ["METAL", "CUDA", "GPU"]:
+      try:
+        if self[device]: return device
+      except Exception: pass
+    return "CPU"
+Device = _Device()
+
+# **************** for Interpreted Buffers ****************
+
+class Interpreted:
+  def __init__(self, buffer, fxn_for_op: Dict[Op, Callable], from_underlying=None):
+    self.buffer, self.fxn_for_op, self.from_underlying = buffer, fxn_for_op, from_underlying
+    self.synchronize = lambda: None
+    self.codegen = None
+    self.method_cache: Dict[LazyOp, Callable] = {}
+
+  def interpret_ast(self:Interpreted, ast:LazyOp) -> Callable:
+    tglob: Dict[str, Any] = {}
+    lines: List[str] = []
+    f = self.fxn_for_op
+
+    @functools.lru_cache(None)
+    def gstr(x:Any, nm=None) -> str:
+      ret = str(nm).replace(".", "_") if nm else f"m{len(tglob):04d}"
+      tglob[ret] = x
+      return ret
+
+    @functools.lru_cache(None)
+    def _interpret_ast(ast:LazyOp) -> str:
+      if TernaryOps.MULACC in f and ast.op == ReduceOps.SUM and isinstance(ast.src[0], LazyOp) and ast.src[0].op == BinaryOps.MUL:
+        ast = LazyOp(TernaryOps.MULACC, ast.src[0].src, ast.arg)
+
+      if MovementOps.AS_STRIDED in f and ast.op in BufferOps:
+        tmp = f"{gstr(f[ast.op], ast.op)}({gstr(ast.arg.val)}, {gstr(ast.arg.dtype)})" if ast.op == BufferOps.CONST else f"{gstr(f[ast.op], ast.op)}(inputs[{ast.arg.idx-1}])"
+        for mop,arg in ast.arg.st.to_movement_ops(): tmp = f"{gstr(f[mop], mop)}({tmp}, {gstr(arg)})"
+      else:
+        inp = [_interpret_ast(src) for src in ast.src]
+        tmp = f"{gstr(f[ast.op], ast.op)}({', '.join(inp + ([gstr(ast.arg)] if ast.arg else []))})"
+
+      ret = f"a{len(lines)}"
+      lines.append(f"  {ret} = {tmp}")
+      return ret
+
+    ret = _interpret_ast(ast)
+    src = '\n'.join(['def run(inputs):'] + lines + [f"  return {gstr(self.from_underlying, 'from_underlying')}({ret})" if self.from_underlying else f"  return {ret}"])
+    if DEBUG >= 4: print(functools.reduce(lambda x,y: (x.replace(y[0], str(y[1])) if y[0][0:2] == "m0" else x), tglob.items(), src))
+    exec(compile(src, "<ast>", "exec"), tglob) # pylint: disable=exec-used
+    return tglob['run']
+
+  def exec_ast(self, ast:LazyOp, output=None, inputs=None, var_vals=None, **kwargs):
+    if ast not in self.method_cache: self.method_cache[ast] = self.interpret_ast(ast)
+    ret = self.method_cache[ast]([x.realized for x in inputs] if inputs else None)
+    if output is not None and ret.dtype != output.dtype and UnaryOps.CAST in self.fxn_for_op:
+      ret = self.from_underlying(self.fxn_for_op[UnaryOps.CAST](self.fxn_for_op[BufferOps.MEM](ret), (output.dtype, False))) # Do manual casting of ret if it does not match the required output dtype.
+    # TODO: is this used?
+    if output is not None and output.output_buffer is not None:
+      assert output.output_buffer.dtype == ret.dtype
+      output.output_buffer._buf = ret._buf
+      return output.output_buffer
+    return ret
 
-# this is a quick "buffer" class for flop tracking and getting the output shape
-class GenericShape:
-  def __init__(self, shape:Tuple[int, ...], flops:int=0): self.shape, self.flops = shape, flops
+@dataclass
+class FlopCounter:
+  shape: Tuple[int, ...]
+  dtype: DType
+  flops: int
+  mem: Dict[int, int]
+  @property
+  def mem_estimate(self): return sum(self.mem.values()) + self.dtype.itemsize*prod(self.shape)
   def consume_flops(self):
     self.flops, ret = 0, self.flops
     return ret
-shape_fxn_for_op : Dict[Op, Callable] = {
-  **{op:lambda self: GenericShape(self.shape, self.consume_flops() + prod(self.shape)) for op in UnaryOps},
-  **{op:lambda self,y: GenericShape(self.shape, self.consume_flops() + y.consume_flops() + prod(self.shape)) for op in BinaryOps},
-  **{op:lambda self,new_shape: GenericShape(new_shape, self.consume_flops() + prod(self.shape)) for op in ReduceOps},
-  **{op:functools.partial(lambda mop,self,arg: GenericShape(ShapeTracker(self.shape).movement_op(mop, arg).shape, self.consume_flops()), op) for op in MovementOps}}
-
-# used in CPUBuffer and TorchBuffer
-class InterpretedBuffer(DeviceBuffer):  # pylint: disable=abstract-method
-  fxn_for_op : ClassVar = shape_fxn_for_op
-  # TODO: use generic types here to remove __init__ in specialized classes
-  def __init__(self, lbuf:Any): self._buf, self.shape = lbuf, tuple(lbuf.shape)
-  def contiguous(self): return type(self).exec_ast(LazyOp(op=UnaryOps.NOOP, src=(self,)))
-  def movement_op(self, op:MovementOps, arg=None): return type(self)(self.fxn_for_op[op](self._buf, arg)) if op in self.fxn_for_op else type(self)(getattr(self._buf, op.name.lower())(arg))
-  @classmethod
-  def exec_ast(cls, ast:LazyOp, output_buffer:Optional[InterpretedBuffer]=None, context=None):
-    if FusedOps.MULACC in cls.fxn_for_op and ast.op == ReduceOps.SUM and isinstance(ast.src[0], LazyOp) and ast.src[0].op == BinaryOps.MUL:
-      ast = LazyOp(FusedOps.MULACC, ast.src[0].src, ast.arg)
-    if context is None: context = dict()
-    if ast in context: return context[ast]
-    srcs = [cls.exec_ast(x, context=context) if isinstance(x, LazyOp) else x for x in ast.src]
-    if DEBUG >= 4: print("exec_ast", ast.op, [x.shape for x in srcs], ast.arg)
-    if ast.op in BinaryOps: assert srcs[0].shape == srcs[1].shape, f"BinaryOps shape mismatch {srcs[0].shape} != {srcs[1].shape}"
-    if ast.op in ReduceOps: assert all(r == n or n == 1 for r,n in zip(srcs[0].shape, ast.arg)), f"ReduceOps can't reduce {srcs[0].shape} -> {ast.arg}"
-    if ast.op in MovementOps: ret = srcs[0].movement_op(ast.op, ast.arg)
-    else: ret = cls(cls.fxn_for_op[ast.op](*([x._buf for x in srcs] + ([ast.arg] if ast.arg else []))))
-    context[ast] = ret
-    if output_buffer is not None:
-      assert output_buffer.shape == ret.shape
-      output_buffer._buf = ret._buf
-      return output_buffer
-    else:
-      return ret
-def get_lazyop_info(ast:LazyOp): return InterpretedBuffer.exec_ast(map_buffers({x:InterpretedBuffer(GenericShape(x.shape)) for x in get_buffers(ast)}, ast))._buf
+InterpretedFlopCounter = Interpreted(FlopCounter, {
+  BufferOps.MEM: lambda arg: FlopCounter(arg.st.shape, arg.dtype, 0, {arg.idx: arg.dtype.itemsize*arg.st.size()}), BufferOps.CONST: lambda arg: FlopCounter(arg.st.shape, arg.dtype, 0, {}),
+  UnaryOps.CAST: lambda self,arg: FlopCounter(self.shape, arg[0], self.consume_flops(), self.mem),   # cast uses no flops
+  **{op:lambda self: FlopCounter(self.shape, self.dtype, self.consume_flops() + prod(self.shape), self.mem) for op in UnaryOps if op != UnaryOps.CAST},
+  **{op:lambda self,y: FlopCounter(self.shape, max(self.dtype, y.dtype), self.consume_flops() + y.consume_flops() + prod(self.shape), {**self.mem, **y.mem}) for op in BinaryOps},
+  **{op:lambda self,new_shape: FlopCounter(new_shape, self.dtype, self.consume_flops() + prod(self.shape), self.mem) for op in ReduceOps},
+  TernaryOps.WHERE: lambda self,y,z: FlopCounter(self.shape, y.dtype, self.consume_flops() + y.consume_flops() + z.consume_flops() + prod(self.shape), {**self.mem, **y.mem, **z.mem})})
+
+@functools.lru_cache(None)
+def get_lazyop_info(ast:LazyOp) -> FlopCounter: return InterpretedFlopCounter.exec_ast(ast)
+
+# **************** for Compiled Buffers ****************
 
 class ASTRunner:
-  def __init__(self, name, prg, bufs_to_delete:Optional[Set[int]]=None, global_size:Optional[List[int]]=None, local_size:Optional[List[int]]=None, op_estimate=0, mem_estimate=0):
+  def __init__(self, name:str, prg:str, global_size:Optional[List[int]]=None, local_size:Optional[List[int]]=None, op_estimate=0, mem_estimate=0, display_name:Optional[str]=None, runtime_args:Optional[dict]=None):
     if DEBUG >= 4: print(prg)
-    self.name, self.prg, self.global_size, self.local_size, self.bufs_to_delete, self.op_estimate, self.mem_estimate = name, prg, global_size, local_size, bufs_to_delete if bufs_to_delete else set(), op_estimate, mem_estimate
-  def build(self, runtime):
-    self.clprg = runtime(self.name, self.prg)
+    self.name, self.prg, self.global_size, self.local_size, self.op_estimate, self.mem_estimate, self.display_name, self.runtime_args = name, prg, global_size, local_size, op_estimate, mem_estimate, display_name, runtime_args if runtime_args is not None else {}
+
+  def build(self, compiler, runtime):
+    self.lib = compiler.__wrapped__(self.prg) if getenv("DISABLE_COMPILER_CACHE") else compiler(self.prg)
+    self.clprg = runtime(self.name, self.lib)
     return self
-  def timeit(self, bufs, local_override=None) -> float:
-    try: return self.clprg(self.global_size, local_override if local_override is not None else self.local_size, *bufs, wait=True)
-    except Exception: return float('inf')
-  def optimize_local_size(self, bufs) -> List[int]:
-    assert self.global_size is not None, "needs a global size to optimize local size"
-    MAX_WORKGROUP = self.clprg.max_work_group_size() if hasattr(self.clprg, 'max_work_group_size') else 1024
-    local_dims = [[x for x in set([sz, 1, 2, 4, 8, 16, 32, 64, 128, 256, MAX_WORKGROUP]) if x<=sz] for sz in self.global_size]
-    local_sizes = [list(x) for x in itertools.product(*local_dims) if prod(x) <= MAX_WORKGROUP] * 2  # try each valid size twice
-    return min([(self.timeit(bufs, local_size), local_size) for local_size in random.sample(local_sizes, len(local_sizes))])[1]
-  def lower(self, bufs) -> List[RawBuffer]: return [x.raw() for i,x in enumerate(bufs) if x is not None and i not in self.bufs_to_delete]
-  def __call__(self, bufs):
-    if getenv("OPTLOCAL") and self.global_size is not None and self.local_size is None: self.local_size = self.optimize_local_size(bufs)
-    if et := self.clprg(self.global_size, self.local_size, *bufs, wait=DEBUG>=2): GlobalCounters.time_sum_s += et
-    if DEBUG >= 1:
-      print(f"**** {GlobalCounters.kernel_count:4d} {self.name:20s} args {len(bufs):5d}  kernels {str(self.global_size):18s} {str(self.local_size):12s} OPs {self.op_estimate/1e6:7.1f}M/{GlobalCounters.global_ops/1e9:7.2f}G  mem {GlobalCounters.mem_used/1e9:5.2f} GB " +
-            (str() if et is None else f"tm {et*1e6:9.2f}us/{GlobalCounters.time_sum_s*1e3:9.2f}ms ({self.op_estimate/(et*1e9):8.2f} GFLOPS)"))
-    GlobalCounters.log_kernel(self.op_estimate, self.mem_estimate)
-    return et
 
-# assumes you are using ShapeTracker
-# used in GPUBuffer and LLVMBuffer
-class CompiledBuffer(DeviceBuffer):  # pylint: disable=abstract-method
-  def __init__(self, shape:Union[ShapeTracker, Tuple[int, ...]], hostbuf:Optional[CompiledBuffer]=None, backing:Optional[np.ndarray]=None, force_create=False):
-    self.st = shape if isinstance(shape, ShapeTracker) else ShapeTracker(tuple(shape))
-    self.shape = self.st.shape
-    self._base_shape : Tuple[int, ...] = hostbuf._base_shape if hostbuf is not None else self.shape
-    self._buf = hostbuf._buf if hostbuf is not None else None
-    self._backing : Optional[np.ndarray] = hostbuf._backing if hostbuf is not None else backing
-    if (self._backing is not None and self._backing.shape != (1,)) or force_create: self.raw()
-
-  # TODO: not GPUBuffer, get name of class
-  def __repr__(self): return f"GPUBuffer(shape={self.st}, hostbuf=GPUBuffer(shape={self._base_shape}" + (f", backing=np.array({self._backing}, dtype=np.float32)))" if self._backing else ", force_create=True))")
-
-  raw_buffer_type : Type[RawBuffer]
-  @classmethod
-  def create_raw_buffer(cls, shape, backing) -> RawBuffer:
-    assert backing is None or prod(shape) == prod(backing.shape), "backing has the wrong shape"
-    assert backing is None or GlobalCounters.cache is None, f"can't copy in {backing.shape} while caching"
-    return cls.raw_buffer_type(4*prod(shape)) if backing is None else cls.raw_buffer_type.fromCPU(backing)
-  def raw(self) -> RawBuffer:
-    if self._buf is None:
-      self._buf = self.create_raw_buffer(self._base_shape, self._backing)
-      self._backing = None
-    return self._buf
-
-  @classmethod
-  def fromCPU(cls, x:np.ndarray) -> CompiledBuffer: return cls(x.shape, backing=x.view(np.ndarray).astype(np.float32).ravel())
-  def toCPU(self) -> np.ndarray:
-    assert GlobalCounters.cache is None, f"can't copy out {self} while caching"
-    return self.contiguous().raw().toCPU().reshape(self.shape)
-
-  codegen_type : Any
-  runtime_type : Type
-  method_cache : Dict[str, ASTRunner] = {}
-  @classmethod
-  def exec_ast(cls, ast:LazyOp, output_buffer:Optional[CompiledBuffer]=None):
-    k = cls.codegen_type(ast, output_buffer)
-    if getenv("ENABLE_METHOD_CACHE"):   # TODO: this breaks the ops test!
-      if k.key not in cls.method_cache: cls.method_cache[k.key] = k.codegen().build(cls.runtime_type)
-      elif DEBUG >= 4: print(f"method cache hit : {k.key}")
-      prg = cls.method_cache[k.key]
-    else:
-      prg = k.codegen().build(cls.runtime_type)
-    if getenv("PRINT_AST", "") == prg.name:
-      k.print()
-      print(prg.prg)
-    rawbufs = prg.lower(k.bufs)
-    if GlobalCounters.cache is not None: GlobalCounters.cache.append((prg, rawbufs))
-    prg(rawbufs)
-    return k.ret
-
-  # universal for shape tracked
-  def contiguous(self): return self if self.st.contiguous and prod(self._base_shape) == prod(self.shape) else type(self).exec_ast(LazyOp(op=UnaryOps.NOOP, src=(self,)))
-  def movement_op(self, op:MovementOps, arg): return type(self)(ShapeTracker(self.st).movement_op(op, arg), self)
-
-class GlobalCounters:
-  global_ops : ClassVar[int] = 0
-  global_mem : ClassVar[int] = 0
-  time_sum_s : ClassVar[float] = 0.0
-  kernel_count : ClassVar[int] = 0
-  mem_used : ClassVar[int] = 0   # NOTE: this is not reset
-  cache : ClassVar[Optional[List[Tuple[Callable, Any]]]] = None
-  @staticmethod
-  def reset(): GlobalCounters.global_ops, GlobalCounters.global_mem, GlobalCounters.time_sum_s, GlobalCounters.kernel_count, GlobalCounters.cache = 0,0,0.0,0,None
-  @staticmethod
-  def log_kernel(op_estimate:int, mem_estimate:int):
+  def exec(self, rawbufs, var_vals:Optional[Dict[Variable, int]]=None, force_wait=False) -> Optional[float]:
+    from tinygrad.jit import CacheCollector
+    CacheCollector.add(self, rawbufs, var_vals if var_vals is not None else {})
+    return self(rawbufs, var_vals, force_wait=force_wait)
+
+  def launch_dims(self, var_vals):
+    global_size = ([sym_infer(sz, var_vals) for sz in self.global_size] + [1]*(3-len(self.global_size))) if self.global_size is not None else self.global_size
+    local_size = ([sym_infer(sz, var_vals) for sz in self.local_size] + [1]*(3-len(self.local_size))) if self.local_size is not None else self.local_size
+    return global_size, local_size
+
+  def __call__(self, rawbufs:List[RawBuffer], var_vals:Optional[Dict[Variable, int]]=None, jit=False, force_wait=False) -> Optional[float]:
+    if var_vals is None: var_vals = {}
+    global_size, local_size = self.launch_dims(var_vals)
+    if global_size is not None and local_size is None:
+      # TODO: this is copied from get_program
+      from tinygrad.features.search import optimize_local_size
+      local_size = self.local_size = optimize_local_size(self.clprg, global_size, rawbufs)
+      global_size = self.global_size = [g//l if g%l == 0 else g/l for g,l in zip(global_size, local_size)]
+    lra = self.runtime_args.copy()
+    if global_size: lra['global_size'] = global_size
+    if local_size and 'local_size' not in lra: lra['local_size'] = local_size
+    if et := self.clprg(*rawbufs, *var_vals.values(), **lra, wait=force_wait or DEBUG>=2): GlobalCounters.time_sum_s += et
+    op_estimate = sym_infer(self.op_estimate, var_vals)
+    mem_estimate = sym_infer(self.mem_estimate, var_vals)
+    if DEBUG >= 2:
+      print(f"{colored(f'*** {GlobalCounters.kernel_count:4d}', 'magenta' if jit else None)} {(self.display_name+' '*(37-ansilen(self.display_name))) if self.display_name is not None else self.name:33s} arg {len(rawbufs):3d} sz {str(global_size):18s} {str(local_size):12s} OPs {int(op_estimate/1e6):6d}M/{GlobalCounters.global_ops/1e9:7.2f}G  mem {GlobalCounters.mem_used/1e9:5.2f} GB " +
+            (str() if et is None else f"tm {et*1e6:9.2f}us/{GlobalCounters.time_sum_s*1e3:9.2f}ms ({op_estimate/((et or 1e-20)*1e9):8.2f} GFLOPS, {mem_estimate/((et or 1e-20)*1e9):7.2f} GB/s)"))
     GlobalCounters.kernel_count += 1
     GlobalCounters.global_ops += op_estimate
-    GlobalCounters.global_mem += mem_estimate
\ No newline at end of file
+    GlobalCounters.global_mem += mem_estimate
+    return et
+
+class Compiled:
+  def __init__(self, buffer: Type[RawBuffer], linearizer_opts, renderer, compiler, runtime, synchronize=lambda: None):
+    self.buffer, self.linearizer_opts, self.renderer, self.compiler, self.runtime, self.synchronize = buffer, linearizer_opts, renderer, compiler, runtime, synchronize
+    self.method_cache: Dict[LazyOp, ASTRunner] = {}
+
+  def to_program(self, k):
+    k.linearize()
+    src, runtime_args = self.renderer(k.function_name, k.uops)
+    return ASTRunner(k.function_name, src, k.global_size, k.local_size,
+                     op_estimate=k.info.flops, mem_estimate=k.info.mem_estimate,
+                     display_name=k.display_name, runtime_args=runtime_args).build(self.compiler, self.runtime)
+
+  def exec_ast(self, ast:LazyOp, output, inputs, var_vals, **kwargs):
+    # check if we can reuse the output buffer
+    # if it's aliased, don't use it
+    # NOTE: this is pretty wrong actually, who knows where else this buffer is used?
+    output.realized = output.output_buffer
+    if output.realized:
+      for i,a in enumerate(inputs):
+        # TODO: if this is contiguous it's fine
+        if a.realized == output.realized:
+          if any(not x.arg.st.contiguous for x in ast.get_lazyops() if x.op == BufferOps.MEM and x.arg.idx == i+1):
+            output.realized = None
+            break
+
+    # we don't have an output buffer, we have to create it, and create to max size if it has symbolic shape
+    if not output.realized:
+      output.realized = self.buffer(prod((s if isinstance(s, int) else s.max for s in output.shape)), output.dtype, **kwargs)
+
+    # all the rawbuffers
+    rawbuffers = [output.realized] + [x.realized for x in inputs]
+
+    # extract real vars used in ast
+    from tinygrad.lazy import vars_from_ast
+    ast_vars = vars_from_ast(ast)
+    assert all(v.val is None for v in ast_vars), f"ast contains bound Variable {ast_vars}"
+
+    # compilation time
+    def get_program():
+      from tinygrad.codegen.linearizer import Linearizer
+      k = Linearizer(ast, self.linearizer_opts)
+      assert k.info.dtype == output.dtype, f"linearizer must match dtype. linearizer wants {k.info.dtype} but buffer is {output.dtype}"
+      if not NOOPT:
+        if not (used_tensor_cores:=k.apply_tensor_cores(getenv("TC", 1))): k.hand_coded_optimizations()
+        if BEAM >= 1 and not vars_from_ast(ast):
+          lins = [(("tc" if used_tensor_cores else "hc"), k)]
+          # allocate a scratch buffer if output buffer is also input
+          test_rawbuffers = [type(rawbuffers[0])(rawbuffers[0].size, rawbuffers[0].dtype), *rawbuffers[1:]] if rawbuffers[0] in rawbuffers[1:] else rawbuffers
+          kb = Linearizer(ast, self.linearizer_opts)
+          kb.required_optimizations()
+          from tinygrad.features.search import beam_search, time_linearizer
+          lins.append((f"beam{BEAM.value}", beam_search(kb, test_rawbuffers, BEAM.value, bool(getenv("BEAM_ESTIMATE", 1)))))
+          if used_tensor_cores:
+            lins.append(("hc", Linearizer(ast, self.linearizer_opts)))
+            lins[-1][1].hand_coded_optimizations()
+          timed = sorted([(nm, tk, time_linearizer(tk, test_rawbuffers, allow_test_size=False, disable_cache=True, clear_l2=True)) for nm, tk in lins], key=lambda x: x[2])
+          if DEBUG >= 1: print("  <  ".join(f"{nm:6s} : {lin.colored_shape(30, dense=True)} : {tm*1e6:8.2f} us" for nm, lin, tm in timed))
+          k = timed[0][1]
+      else:
+        k.required_optimizations()
+      return self.to_program(k)
+
+    if getenv("ENABLE_METHOD_CACHE", 1):
+      if ast not in self.method_cache: self.method_cache[ast] = get_program()
+      prg = self.method_cache[ast]
+    else:
+      prg = get_program()
+
+    if prg.name == getenv("PRINT_PRG", ''): print(prg.prg)
+
+    prg.exec(rawbuffers, var_vals={k:var_vals[k] for k in ast_vars})
+    return output.realized
diff --git a/tinygrad_repo/tinygrad/realize.py b/tinygrad_repo/tinygrad/realize.py
new file mode 100644
index 0000000000..9c8969a633
--- /dev/null
+++ b/tinygrad_repo/tinygrad/realize.py
@@ -0,0 +1,74 @@
+from typing import List, cast, Dict, Callable
+import numpy as np
+from tinygrad.ops import ScheduleItem, LazyOp, LoadOps, Device, BufferOps
+from tinygrad.graph import log_schedule_item, print_tree
+from tinygrad.lazy import LazyBuffer
+from tinygrad.helpers import DEBUG, prod, all_int, getenv, IMAGE
+
+from tinygrad.runtime.lib import RawBufferMapped, RawBufferTransfer
+from tinygrad.runtime.ops_disk import RawDiskBuffer
+from tinygrad.features.image import fix_schedule_for_images
+
+def run_schedule(schedule:List[ScheduleItem], disable_logging=False):
+  # HACK: images can be not usable due to shape
+  if IMAGE >= 2: schedule = fix_schedule_for_images(schedule)
+
+  # NOTE: if you for loop the schedule it's slow because nothing frees
+  while len(schedule):
+    si = schedule.pop(0)
+    if not disable_logging: log_schedule_item(si)
+    assert all(x.realized for x in si.inputs), "can't run schedule, some inputs aren't realized"
+    if DEBUG >= 3: print_tree(si.ast)
+    if si.ast.op in LoadOps:
+      # confirm the LoadOps are contiguous and in order
+      for i,s in enumerate(si.ast.src): assert isinstance(s, LazyOp) and s.op == BufferOps.MEM and s.arg.idx == i+1 and s.arg.st.contiguous, f"bad LoadOps src {i}: {s}"
+      LOAD_OPS_DISPATCHER[cast(LoadOps, si.ast.op)](si.out, *si.inputs)
+    else:
+      si.out.realized = Device[si.out.device].exec_ast(si.ast, output=si.out, inputs=si.inputs, var_vals=si.var_vals, **si.out._device_extra_args())
+    del si.out.op
+    for v in si.out.views: del v.op
+    assert si.out.realized and isinstance(si.out.realized, Device[si.out.device].buffer), f"device mismatch on realized got {type(si.out.realized)} expected {si.out.device}"
+    assert si.out.realized.dtype == si.out.dtype, "realized dtype is incorrect"
+
+# *** zero op LoadOps ***
+
+def _realize_empty(buffer: LazyBuffer) -> None:
+  assert all_int(buffer.shape), "does not support symbolic shape"
+  if DEBUG >= 2: print(f"***     empty {buffer.device}                              shape {str(buffer.shape):23s} dtype {buffer.dtype}")
+  buffer.realized = Device[buffer.device].buffer(prod(buffer.shape), buffer.dtype, **buffer._device_extra_args())
+
+def _realize_rand(buffer: LazyBuffer) -> None:
+  assert all_int(buffer.shape), "does not support symbolic shape"
+  if DEBUG >= 2: print(f"***      rand {buffer.device}    seed {buffer.op.arg:<10d}  shape {str(buffer.shape):23s} dtype {buffer.dtype}")
+  rng = np.random.default_rng(buffer.op.arg)
+  buffer.realized = Device[buffer.device].buffer.fromCPU(rng.random(size=prod(buffer.shape), dtype=np.float32).astype(dtype=buffer.dtype.np, copy=False), **buffer._device_extra_args())
+
+# *** one op LoadOps ***
+
+def _realize_from(buffer: LazyBuffer, src: LazyBuffer) -> None:
+  assert src.realized.size == buffer.st.size(), f"size mismatch on FROM {src.realized.size} != {buffer.st.size()}"
+  assert src.st.contiguous and buffer.st.contiguous, "all must be contiguous for from"
+  if DEBUG >= 2: print(f"***      copy {buffer.device} <- {src.device} size {src.realized.size:<16d} shape {str(buffer.shape):23s} dtype {src.realized.dtype}")
+  # TODO: make this generic
+  if isinstance(src.realized, RawDiskBuffer) and issubclass(Device[buffer.device].buffer, RawBufferMapped):
+    assert all_int(buffer.shape), "does not support symbolic shape"
+    buffer.realized = Device[buffer.device].buffer(prod(buffer.shape), buffer.dtype, **buffer._device_extra_args())
+    src.realized.readinto(cast(RawBufferMapped, buffer.realized)._buffer())
+  elif isinstance(src.realized, RawBufferTransfer) and issubclass(Device[buffer.device].buffer, RawBufferTransfer) and getenv("P2P", 0) >= 1:
+    buffer.realized = cast(RawBufferTransfer, Device[buffer.device].buffer).transfer(src.realized, buffer.shape, buffer.dtype, **buffer._device_extra_args())
+  else:
+    # TODO: schedule this as FROM to go to CPU, and a FROM to go to device
+    buffer.realized = Device[buffer.device].buffer.fromCPU(src.realized.toCPU(), **buffer._device_extra_args())
+
+# *** n op LoadOps ***
+
+def _realize_custom(buffer: LazyBuffer, *inputs: LazyBuffer) -> None:
+  if DEBUG >= 2: print(f"***    custom {buffer.device}                              shape {str(buffer.shape):23s} dtype {buffer.dtype}")
+  buffer.realized = buffer.op.arg(buffer, *inputs)
+
+LOAD_OPS_DISPATCHER: Dict[LoadOps, Callable] = {
+  LoadOps.EMPTY: _realize_empty,
+  LoadOps.RAND: _realize_rand,
+  LoadOps.FROM: _realize_from,
+  LoadOps.CUSTOM: _realize_custom,
+}
diff --git a/tinygrad_repo/tinygrad/renderer/cstyle.py b/tinygrad_repo/tinygrad/renderer/cstyle.py
new file mode 100644
index 0000000000..9ac119b908
--- /dev/null
+++ b/tinygrad_repo/tinygrad/renderer/cstyle.py
@@ -0,0 +1,212 @@
+from typing import Dict, List, Optional, NamedTuple, Tuple, Union, DefaultDict
+import math
+from collections import defaultdict
+from tinygrad.codegen.linearizer import UOps, UOp
+from tinygrad.ops import UnaryOps, BinaryOps, TernaryOps
+from tinygrad.helpers import ImageDType, dtypes, prod, DType, strip_parens
+
+class CStyleLanguage(NamedTuple):
+  size_prefix: str = "int"
+  generic_var_prefix: str = ""
+  kernel_prefix: str = ""
+  buffer_prefix: str = ""
+  buffer_suffix: str = ""
+  smem_align: str = ""
+  smem_prefix: str = ""
+  smem_prefix_for_cast: bool = True
+  arg_int_prefix: str = ""
+  barrier: str = ""
+  xid: List[str] = []
+  gid: List[str] = []
+  lid: List[str] = []
+  global_max: List[int] = []
+  local_max: List[int] = []
+  extra_args: List[str] = []
+  float4: Optional[str] = None
+  half_prekernel: Optional[str] = None
+  uses_vload: bool = False
+  external_local_bufs: bool = False
+  uses_ptr_arithmetic: bool = False
+  launch_bounds: bool = False
+  code_for_op: Dict = {
+    UnaryOps.NEG: lambda x: f"(-{x})",
+    UnaryOps.EXP2: lambda x: f"exp2({x})",
+    UnaryOps.LOG2: lambda x: f"log2({x})",
+    UnaryOps.SIN: lambda x: f"sin({x})",
+    UnaryOps.SQRT: lambda x: f"sqrt({x})",
+    BinaryOps.ADD: lambda a,b: f"({a}+{b})", BinaryOps.SUB: lambda a,b: f"({a}-{b})",
+    BinaryOps.MUL: lambda a,b: f"({a}*{b})", BinaryOps.DIV: lambda a,b: f"({a}/{b})",
+    BinaryOps.MAX: lambda a,b: f"max({a},{b})", BinaryOps.MOD: lambda a,b: f"({a}%{b})",
+    BinaryOps.CMPLT: lambda a,b: f"({a}<{b})", TernaryOps.MULACC: lambda a,b,c: f"(({a}*{b})+{c})",
+    TernaryOps.WHERE: lambda a,b,c: f"({a}!=0?{b}:{c})"
+  }
+
+  # returns a str expression of the casted xs with the given type
+  def render_cast(self, x:List[str], var_dtype:DType) -> str:
+    if len(x) == 1: return f"({var_dtype.name})({x[0]})"
+    assert len(x) == var_dtype.sz, f"cast is wrong size {len(x)} != {var_dtype.sz}"
+    assert self.float4 is not None, "cast is not supported on this platform"
+    if var_dtype == dtypes._float4: return f"{self.float4}({','.join(x)})"
+    if var_dtype == dtypes._float2: return f"{self.float4.replace('float4', 'float2')}({','.join(x)})"
+    if var_dtype == dtypes._int2: return f"{self.float4.replace('float4', 'int2')}({','.join(x)})"
+    raise NotImplementedError(f"no cast for {var_dtype}")
+
+  # returns a str expression of the const with the given type
+  def render_const(self, x:Union[float,int], var_dtype) -> str:
+    if math.isnan(x): val = "NAN"
+    elif math.isinf(x): val = ("-" if x < 0 else "") + "INFINITY"
+    else: val = f"{x}f" if dtypes.is_float(var_dtype) and isinstance(x, float) else f"{int(x)}"
+    return self.render_cast([val]*var_dtype.sz, var_dtype) if var_dtype.sz > 1 else val
+
+  # returns a str expression of the loaded value with the output type
+  def render_load(self, output_dtype, buf_name, buf_dtype, idx, local=False) -> str:
+    if isinstance(buf_dtype, ImageDType):
+      assert output_dtype == dtypes._float4, f"images must be float4, getting {output_dtype}"
+      return f"read_imagef({buf_name}, smp, {idx})"
+    if self.uses_vload and buf_dtype == dtypes.float16:
+      return f"vload_half{'' if output_dtype.sz == 1 else str(output_dtype.sz)}(0, {buf_name}+{idx})"
+    if output_dtype.sz > 1:
+      out_val = f"*(({self.smem_prefix if local and self.smem_prefix_for_cast else self.buffer_prefix}{buf_dtype.name}{output_dtype.sz}*)({buf_name}+{idx}))"
+    else:
+      out_val = f"*({buf_name}+{idx})" if self.uses_ptr_arithmetic else f"{buf_name}[{idx}]"
+
+    return self.render_cast([out_val], output_dtype) if output_dtype != buf_dtype else out_val
+
+  def render_local(self, name:str, size:int):
+    return self.smem_align + self.smem_prefix + f"float {name}[{size}];"
+
+  def render_for(self, expr: str, _min:Union[int,str], _max:Union[int,str]) -> str:
+    return f"for (int {expr} = {_min}; {expr} < {_max}; ++{expr}) {{"
+
+  def render_if(self, cond: str):
+    return f"if ({cond}) {{"
+
+  def render_conditional(self, cond: str, x:str, y:str) -> str:
+    return f"({cond})?({x}):{y}"
+
+  def render_kernel(self, function_name:str, kernel:List[str], bufs:List[Tuple[str,DType]], local_size:List[int], prekernel:List[str]) -> str:
+    tmp = "const sampler_t smp = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST;\n" if any(isinstance(dtype, ImageDType) for _,dtype in bufs) else ""
+    buftypes = [(name,f"{'read_only' if i > 0 else 'write_only'} image2d_t" if dtype.name.startswith('image') else
+                self.arg_int_prefix if dtype == dtypes._arg_int32 else
+                ("const " if i > 0 else "")+self.buffer_prefix+dtype.name+"*"+self.buffer_suffix) for i,(name,dtype) in enumerate(bufs)]
+    prg = ''.join([f"{self.kernel_prefix}void {f'__launch_bounds__ ({prod(local_size)}, 1) ' if self.launch_bounds else ''}{function_name}(",] +
+    [', '.join([f'{t} {name}' for name,t in buftypes] + self.extra_args)] +
+    [") {\n" + tmp] + ['\n'.join(kernel), "\n}"])
+    if self.half_prekernel and any(dtype == dtypes.float16 for _,dtype in bufs): prg = ''.join([f"{self.half_prekernel}", "\n", prg])
+    return prg
+
+  # returns a str statement that does the store
+  def render_store(self, buf_name:str, buf_dtype:DType, var_name:str, var_dtype:DType, idx:str, local=False) -> str:
+    if isinstance(buf_dtype, ImageDType):
+      assert var_dtype == dtypes._float4, "images must be float4"
+      return f"write_imagef({buf_name}, {idx}, {var_name});"
+    if self.uses_vload and buf_dtype == dtypes.float16:
+      return f"vstore_half{'' if var_dtype.sz == 1 else str(var_dtype.sz)}({var_name}, 0, {buf_name}+{idx});"
+    if var_dtype.sz > 1:
+      return f"*(({self.smem_prefix if local and self.smem_prefix_for_cast else self.buffer_prefix}{buf_dtype.name}{var_dtype.sz}*)({buf_name}+{idx})) = ({buf_dtype.name}{var_dtype.sz}){var_name};"
+    return f"*({buf_name}+{idx}) = {var_name};" if self.uses_ptr_arithmetic else f"{buf_name}[{idx}] = {var_name};"
+
+def uops_to_cstyle(lang:CStyleLanguage, function_name:str, uops:List[UOp]) -> Tuple[str, Dict]:
+  local_size: List[int] = []
+  kernel,prekernel,bufs = [],[],[]
+  #pend_close = None
+  depth = 1
+  def kk(s): kernel.append("  "*depth+s)
+
+  c: DefaultDict[str, int] = defaultdict(int)
+  r: Dict[UOp, str] = {}
+  def ssa(u, prefix="t"):
+    nonlocal c, r
+    c[prefix] += 1
+    r[u]=f"{prefix}{c[prefix]-1}"
+    return r[u]
+
+  child_count: DefaultDict[UOp, int] = defaultdict(int)
+  for ru in uops:
+    for v in ru.vin:
+      child_count[v] += 1
+
+  for u in uops:
+    uop,dtype,vin,args,_ = u
+    if uop == UOps.LOOP:
+      kk(lang.render_for(ssa(u,'ridx'), r[vin[0]], r[vin[1]]))
+      depth += 1
+    elif uop == UOps.IF:
+      kk(lang.render_if(r[vin[0]]))
+      depth += 1
+    elif uop == UOps.BARRIER:
+      kk(lang.barrier)
+    elif uop == UOps.END:
+      depth -= 1
+      kk("}")
+    elif uop == UOps.WMMA:
+      if args[0] == "METAL":
+        # ((lidx2*32)+(lidx3*4)+(lidx4*16)+(lidx5*8)+(lidx6*2))
+        kk("{ simdgroup_float8x8 a,b,c;")
+        kk(f"a.thread_elements()[0] = {r[vin[0]]}; a.thread_elements()[1] = {r[vin[1]]};")
+        kk(f"b.thread_elements()[0] = {r[vin[2]]}; b.thread_elements()[1] = {r[vin[3]]};")
+        kk(f"c.thread_elements()[0] = {r[vin[4]]}; c.thread_elements()[1] = {r[vin[5]]};")
+        kk("simdgroup_multiply_accumulate(c, a, b, c);")
+        kk(f"{r[vin[4]]} = c.thread_elements()[0]; {r[vin[5]]} = c.thread_elements()[1]; }}")
+      elif args[0] == "HIP":
+        kk("{")
+        kk(f"half16 a_frag = {{ {','.join(['(half)'+r[x] for x in vin[0:16]])} }};")
+        kk(f"half16 b_frag = {{ {','.join(['(half)'+r[x] for x in vin[16:32]])} }};")
+        kk(f"float8 c_frag = {{ {','.join([r[x] for x in vin[32:]])} }};")
+        kk("c_frag = __builtin_amdgcn_wmma_f32_16x16x16_f16_w32(a_frag, b_frag, c_frag);")
+        for i in range(8): kk(f"{r[vin[32+i]]} = c_frag[{i}];")
+        kk("}")
+      else:
+        raise NotImplementedError(f"WMMA not implemented for {args}")
+    elif uop == UOps.ALU:
+      assert dtype is not None
+      # remove parens if ALU types are the same. TODO: can do more here
+      if vin[0].uop == UOps.ALU and vin[0].arg == args and args in {BinaryOps.ADD, BinaryOps.SUB, BinaryOps.MUL}:
+        val = lang.code_for_op[args](strip_parens(r[vin[0]]), *[r[x] for x in vin[1:]])
+      else:
+        val = lang.code_for_op[args](*[r[x] for x in vin])
+      assert child_count[u] != 0, f"childless ALU op found {u}"
+      if child_count[u] <= 1 or dtypes.is_int(dtype):  # fix index rendering issue
+        r[u] = val
+      else:
+        kk(f"{lang.generic_var_prefix if lang.generic_var_prefix else dtype.name} {ssa(u,'alu')} = {val};")
+    elif uop == UOps.DEFINE_ACC:
+      assert dtype is not None
+      kk(f"{lang.generic_var_prefix if lang.generic_var_prefix else dtype.name} {ssa(u,'acc')} = {lang.render_const(args, dtype)};")
+    elif uop == UOps.SPECIAL:
+      xid = lang.gid if args[1].startswith("g") else (lang.xid if args[1].startswith("i") else lang.lid)
+      kk(f"{lang.size_prefix} {args[1]} = {xid[args[0]]}; /* {args[2]} */")
+      if args[1].startswith("l"): local_size.append(args[2])
+      r[u] = args[1]
+    elif uop == UOps.CONST:
+      r[u] = lang.render_const(args, dtype) if args >= 0 else f"({lang.render_const(args, dtype)})"
+    elif uop == UOps.LOAD:
+      assert dtype is not None
+      val = lang.render_load(dtype, r[vin[0]], vin[0].dtype, strip_parens(r[vin[1]]), vin[0].uop == UOps.DEFINE_LOCAL)
+      if len(vin) > 2: val = lang.render_conditional(r[vin[2]], val, r[vin[3]])
+      kk(f"{lang.generic_var_prefix if lang.generic_var_prefix else dtype.name} {ssa(u,'val')} = {val};")
+    elif uop == UOps.PHI:
+      kk(f"{r[vin[0]]} = {r[vin[1]]};")
+      r[u] = r[vin[0]]
+    elif uop == UOps.STORE:
+      assert vin[0].dtype is not None and vin[2].dtype is not None
+      kk(lang.render_store(r[vin[0]], vin[0].dtype, r[vin[2]], vin[2].dtype, strip_parens(r[vin[1]]), vin[0].uop == UOps.DEFINE_LOCAL))
+    elif uop == UOps.CAST and dtype is not None and dtype.sz > 1:
+      val = lang.render_cast([r[x] for x in vin], dtype)
+      if child_count[u] <= 1: r[u] = val
+      else: kk(f"{lang.generic_var_prefix if lang.generic_var_prefix else dtype.name} {ssa(u,'cast')} = {val};")
+    elif uop == UOps.DEFINE_LOCAL:
+      if lang.external_local_bufs:
+        prekernel.append(lang.render_local(args[0], args[1]))
+      else:
+        kk(lang.render_local(args[0], args[1]))
+      r[u] = args[0]
+    elif uop == UOps.DEFINE_GLOBAL:
+      bufs.append(args)
+      r[u] = args[0]
+    elif uop == UOps.GEP:
+      r[u] = f"({r[vin[0]]}).{'xyzw'[args]}"
+    else:
+      raise RuntimeError(f"failed to render {uop}")
+
+  return lang.render_kernel(function_name, kernel, bufs, local_size, prekernel), {}
diff --git a/tinygrad_repo/tinygrad/renderer/opencl.py b/tinygrad_repo/tinygrad/renderer/opencl.py
new file mode 100644
index 0000000000..bad72c9bb2
--- /dev/null
+++ b/tinygrad_repo/tinygrad/renderer/opencl.py
@@ -0,0 +1,23 @@
+import functools
+from tinygrad.helpers import dtypes
+from tinygrad.ops import TernaryOps
+from tinygrad.renderer.cstyle import uops_to_cstyle, CStyleLanguage
+
+type_map = { dtypes.uint8: "uchar", dtypes.uint32: "uint", dtypes.uint64: "ulong" }
+class OpenCLLanguage(CStyleLanguage):
+  kernel_prefix = "__kernel "
+  buffer_prefix = "__global "
+  smem_align = "__attribute__ ((aligned (16))) "
+  smem_prefix = "__local "
+  arg_int_prefix = "const int"
+  half_prekernel = "#pragma OPENCL EXTENSION cl_khr_fp16 : enable"
+  barrier = "barrier(CLK_LOCAL_MEM_FENCE);"
+  float4 = "(float4)"
+  gid = [f'get_group_id({i})' for i in range(3)]
+  lid = [f'get_local_id({i})' for i in range(3)]
+  xid = [f'get_global_id({i})' for i in range(3)]
+  uses_vload = True
+  # NOTE: mad is used so the loads aren't reordered into the math on 845
+  code_for_op = {**CStyleLanguage().code_for_op, TernaryOps.MULACC: lambda a,b,c: f"mad({a},{b},{c})"}
+
+OpenCLRenderer = functools.partial(uops_to_cstyle, OpenCLLanguage())
diff --git a/tinygrad_repo/tinygrad/runtime/lib.py b/tinygrad_repo/tinygrad/runtime/lib.py
new file mode 100644
index 0000000000..5b6379c911
--- /dev/null
+++ b/tinygrad_repo/tinygrad/runtime/lib.py
@@ -0,0 +1,111 @@
+import ctypes
+import numpy as np
+from collections import defaultdict, deque
+from typing import TypeVar, Type, Any, Dict, Deque, Tuple
+from tinygrad.helpers import DType, dtypes, prod, GlobalCounters, ImageDType
+
+_T = TypeVar("_T")
+class RawBuffer:  # pylint: disable=abstract-method
+  def __init__(self, size:int, dtype:DType, buf:Any=None, allocator:Any=None, **kwargs):
+    self.size: int = size
+    self.dtype: DType = dtype
+    self._buf = buf if buf is not None else (allocator.alloc(size, dtype, **kwargs) if allocator else None) # If buf is provided, use it. Otherwise try to allocate from the allocator.
+    self._memsz: int = size*dtype.itemsize
+    self._allocator = allocator
+    self._device = kwargs.get('device', None)
+    GlobalCounters.mem_used += self._memsz
+  def __del__(self):  # NOTE: if it fails on init (bad dtype), it won't have a _memsz
+    if hasattr(self, '_memsz'): GlobalCounters.mem_used -= self._memsz
+    if hasattr(self, '_allocator') and self._allocator: self._allocator.free(self._buf)
+  def __repr__(self): return f"buffer<{self.size}, {self.dtype}, {id(self)}>"
+  @property
+  def key(self): return (self.size, self.dtype)
+
+  # NOTE: this interface allows for 0 copy
+  @classmethod
+  def fromCPU(cls:Type[_T], x:np.ndarray) -> _T: raise NotImplementedError("must be implemented")
+  def toCPU(self) -> np.ndarray: raise NotImplementedError("must be implemented")
+
+class RawBufferCopyIn(RawBuffer):
+  def _copyin(self, x:np.ndarray) -> None: raise NotImplementedError("must be implemented")
+
+  @classmethod
+  def fromCPU(cls, x:np.ndarray, **kwargs):
+    ret = cls(prod(x.shape), dtypes.from_np(x.dtype), **kwargs)
+    if x.size > 0: ret._copyin(x)
+    return ret
+
+class RawBufferMapped(RawBufferCopyIn):
+  def _buffer(self) -> memoryview: raise NotImplementedError("must be implemented")
+  # NOTE: this metadata prevents the backing buffer from being freed. hack can be removed with PEP688
+  def buffer_view(self) -> np.ndarray: return np.frombuffer(self._buffer(), dtype=np.dtype(self.dtype.np, metadata={"backing": self}), count=self.size)  # type: ignore
+  def toCPU(self) -> np.ndarray: return self.buffer_view().copy() # Need a copy, since jit will write to the same buffer.
+  def _copyin(self, x:np.ndarray) -> None: np.copyto(self.buffer_view(), x.reshape(-1))
+
+# this one is simple enough that i moved it out of the runtimes
+class RawMallocBuffer(RawBufferMapped):
+  def __init__(self, size, dtype: DType): super().__init__(size, dtype, ({dtypes.float64:ctypes.c_double, dtypes.float32: ctypes.c_float, dtypes.float16: ctypes.c_int16, dtypes.bfloat16: ctypes.c_int16, dtypes.int8: ctypes.c_int8, dtypes.uint8: ctypes.c_uint8, dtypes.bool: ctypes.c_uint8, dtypes.int32: ctypes.c_int32, dtypes.uint32: ctypes.c_uint32, dtypes.int64: ctypes.c_int64, dtypes.uint64: ctypes.c_uint64, dtypes.int16: ctypes.c_int16, dtypes.uint16: ctypes.c_uint16}[dtype] * size)())
+  def _buffer(self): return memoryview(self._buf)
+
+class RawBufferCopyInOut(RawBufferCopyIn):
+  def _copyout(self, x:np.ndarray) -> None: raise NotImplementedError("must be implemented")
+
+  def toCPU(self) -> np.ndarray:
+    x: np.ndarray = np.empty(self.size, dtype=self.dtype.np)
+    if x.size > 0: self._copyout(x)
+    return x
+
+class RawBufferTransfer(RawBuffer):
+  def _transfer(self, x) -> None: raise NotImplementedError("must be implemented")
+
+  @classmethod
+  def transfer(cls, x, shape, dtype, **kwargs):
+    ret = cls(prod(shape), dtype, **kwargs)
+    ret._transfer(x)
+    return ret
+
+class LRUAllocator:
+  def __init__(self, dev_memsz=(4<<30)):
+    self.epoch = 0
+    self.free_space: Dict[Any, int] = defaultdict(lambda: dev_memsz)
+    self.buffer_info: Dict[Any, Tuple[int, DType, str]] = dict()
+    self.cached_buffers: Dict[Tuple[int, ...], Deque[Tuple[Any, int]]] = defaultdict(deque) # Cached buffer storage, splitted by type and size, newest first.
+    self.aging_order: Dict[Any, Deque[Tuple[Tuple[int, ...], int]]] = defaultdict(deque) # Keys of cached_buffers, ordered from oldest to newest updates.
+
+  def _cache_reuse_buffer(self, rawbufs: Deque[Tuple[Any, int]]): # The newest cached buffer is reused.
+    GlobalCounters.mem_cached -= self._underlying_buf_memsz(rawbufs[0][0])
+    return rawbufs.popleft()[0]
+
+  def ensure_has_free_space(self, size, dtype, device):
+    while len(self.aging_order[device]) and (self.free_space[device]-size*dtype.itemsize) < 0: # When OOM removing lru buffers.
+      bucket, epoch = self.aging_order[device].popleft()
+      if self.cached_buffers[bucket] and self.cached_buffers[bucket][-1][1] == epoch: self._free_buffer(self.cached_buffers[bucket].pop()[0]) # Free cached buffer if it is still in cache.
+
+  def _alloc_buffer(self, size, dtype, device, **kwargs):
+    self.ensure_has_free_space(size, dtype, device)
+    self.free_space[device] -= size*dtype.itemsize
+    newbuf = self._do_alloc(max(1, size), dtype, device, **kwargs)
+    self.buffer_info[newbuf] = (size, dtype, device)
+    return newbuf
+
+  def _free_buffer(self, buf_to_free):
+    self.free_space[self.buffer_info[buf_to_free][2]] += self._underlying_buf_memsz(buf_to_free)
+    GlobalCounters.mem_cached -= self._underlying_buf_memsz(buf_to_free)
+    self.buffer_info.pop(buf_to_free)
+    self._do_free(buf_to_free)
+
+  def alloc(self, size, dtype, device='0', **kwargs):
+    rawbufs = self.cached_buffers.get(self._cached_bufkey(size, dtype, device), None)
+    return self._cache_reuse_buffer(rawbufs) if rawbufs else self._alloc_buffer(size, dtype, device, **kwargs)
+
+  def free(self, buf): # free() just caches buffer. It might be freed later when OOM during allocation.
+    self.epoch += 1
+    size, dtype, device = self.buffer_info[buf]
+    self.cached_buffers[self._cached_bufkey(size, dtype, device)].appendleft((buf, self.epoch))
+    self.aging_order[device].append((self._cached_bufkey(size, dtype, device), self.epoch))
+    GlobalCounters.mem_cached += self._underlying_buf_memsz(buf)
+
+  def _underlying_buf_memsz(self, buf): return self.buffer_info[buf][0] * self.buffer_info[buf][1].itemsize
+  def _cached_bufkey(self, size, dtype, device) -> Tuple[int, ...]: return (device, size, dtype, dtype.shape) if isinstance(dtype, ImageDType) else (device, size, dtype) # Provides a key for reusing device buffers with identical keys.
+  def _do_alloc(self, size, dtype, device, **kwargs): raise NotImplementedError("must be implemented")
+  def _do_free(self, buf): pass
diff --git a/tinygrad_repo/tinygrad/runtime/ops_cpu.py b/tinygrad_repo/tinygrad/runtime/ops_cpu.py
new file mode 100644
index 0000000000..81b64bde62
--- /dev/null
+++ b/tinygrad_repo/tinygrad/runtime/ops_cpu.py
@@ -0,0 +1,52 @@
+import numpy as np
+import operator
+from typing import Callable, Dict, Tuple, Optional
+from tinygrad.helpers import dtypes, DType
+from tinygrad.ops import BufferOps, UnaryOps, BinaryOps, MovementOps, ReduceOps, TernaryOps, Op, Interpreted
+from tinygrad.runtime.lib import RawBuffer
+
+def shape_to_axis(old_shape:Tuple[int, ...], new_shape:Tuple[int, ...]) -> Tuple[int, ...]:
+  assert len(old_shape) == len(new_shape), "reduce shapes must have same dimensions"
+  return tuple(i for i,(a,b) in enumerate(zip(old_shape, new_shape)) if a != b)
+
+base_fxn_for_op: Dict[Op, Callable] = {
+  BufferOps.MEM: lambda x: x._buf, UnaryOps.NEG: operator.neg, BinaryOps.ADD: operator.add, BinaryOps.SUB: operator.sub, BinaryOps.MUL: operator.mul, BinaryOps.DIV: operator.truediv,
+  ReduceOps.SUM: lambda x, new_shape: x.sum(shape_to_axis(x.shape, new_shape), keepdims=True) if tuple(x.shape) != tuple(new_shape) else x[:],
+  ReduceOps.MAX: lambda x, new_shape: (x.amax if hasattr(x, 'amax') else x.max)(shape_to_axis(x.shape, new_shape), keepdims=True) if tuple(x.shape) != tuple(new_shape) else x[:],
+  MovementOps.RESHAPE: lambda x, arg: x.reshape(arg), MovementOps.SHRINK: lambda x, arg: x[tuple(slice(p[0], p[1], None) for p in arg)],
+}
+
+def match_types(x, y):
+  up = x.dtype if dtypes.from_np(x.dtype).priority > dtypes.from_np(y.dtype).priority else y.dtype
+  return x.astype(up, copy=False), y.astype(up, copy=False)
+
+def einsum_mulacc(einsum, get_strides, expand):
+  def einscripts(x): return ''.join(["abcdefghijklmnopqrstuvwxyz"[i] for i in x])
+  def axes_slice(strides): return [i for i,s in enumerate(strides) if s != 0], tuple([slice(None) if s != 0 else 0 for i,s in enumerate(strides)])
+  def mulacc(a, b, new_shape):
+    (a_axes, a_slices), (b_axes, b_slices) = axes_slice(get_strides(a)), axes_slice(get_strides(b))
+    out = [i for i in range(len(new_shape)) if a.shape[i] == new_shape[i] and (i in a_axes or i in b_axes)]
+    ret = einsum(f"{einscripts(a_axes)}, {einscripts(b_axes)} -> {einscripts(out)}", a[a_slices], b[b_slices])
+    return expand(ret.reshape([(1 if i not in a_axes and i not in b_axes else s) for i,s in enumerate(new_shape)]), new_shape)
+  return mulacc
+
+numpy_fxn_for_op: Dict[Op, Callable] = {**base_fxn_for_op, **{
+  BufferOps.CONST: lambda val, dtype: np.array(val, dtype=dtype.np),
+  UnaryOps.NOOP: lambda x: np.require(x, requirements='C'), UnaryOps.EXP2: np.exp2, UnaryOps.LOG2: np.log2, UnaryOps.SIN: np.sin,
+  UnaryOps.CAST: lambda x,y: x.view(y[0].np) if y[1] else x.astype(y[0].np, copy=False),
+  BinaryOps.MAX: np.maximum, BinaryOps.CMPLT: lambda x,y: (x<y).astype(np.promote_types(x.dtype,y.dtype)), BinaryOps.ADD: lambda x, y: np.add(*match_types(x, y)),
+  BinaryOps.SUB: lambda x, y: np.subtract(*match_types(x, y)), BinaryOps.MUL: lambda x, y: np.multiply(*match_types(x, y)),
+  BinaryOps.DIV: lambda x, y: np.divide(*match_types(x, y)), UnaryOps.SQRT: np.sqrt,
+  MovementOps.PERMUTE: lambda x, order: x.transpose(order), MovementOps.PAD: np.pad, MovementOps.EXPAND: np.broadcast_to,
+  MovementOps.STRIDE: lambda x, arg: x[tuple(slice(None, None, i) for i in arg)],
+  MovementOps.AS_STRIDED: lambda x, arg: np.ndarray(arg[0], buffer=np.require(x, requirements='C'), dtype=x.dtype, offset=arg[2]*x.dtype.itemsize, strides=tuple(y*x.dtype.itemsize for y in arg[1])),
+  TernaryOps.MULACC: einsum_mulacc(lambda s,a,b: np.einsum(s, *match_types(a.copy(), b.copy()), optimize=True), lambda x: x.strides, np.broadcast_to),
+  TernaryOps.WHERE: np.where,
+}}
+
+class RawNumpyBuffer(RawBuffer):
+  def __init__(self, size:int, dtype:DType, buf:Optional[np.ndarray]=None): super().__init__(size, dtype, buf if buf is not None else np.empty([size], dtype.np))
+  @classmethod
+  def fromCPU(cls, x): return cls(x.size, dtypes.from_np(x.dtype), x)
+  def toCPU(self): return self._buf
+CPUBuffer = Interpreted(RawNumpyBuffer, numpy_fxn_for_op, from_underlying=RawNumpyBuffer.fromCPU)
diff --git a/tinygrad_repo/tinygrad/runtime/ops_disk.py b/tinygrad_repo/tinygrad/runtime/ops_disk.py
new file mode 100644
index 0000000000..583fec05f5
--- /dev/null
+++ b/tinygrad_repo/tinygrad/runtime/ops_disk.py
@@ -0,0 +1,41 @@
+import os, mmap
+from typing import Optional
+from typing import Callable, Dict, Tuple
+from tinygrad.helpers import prod, DType
+from tinygrad.runtime.lib import RawBufferMapped
+from tinygrad.ops import Interpreted, Op, MovementOps, UnaryOps, BufferOps
+
+class RawDiskBuffer(RawBufferMapped):
+  def __init__(self, size, dtype:DType, device:Optional[str]=None, buf=None, shape=None, offset=0):  # pylint: disable=super-init-not-called
+    self.shape = (size, ) if shape is None else shape
+    self.offset = offset  # this is an offset in bytes
+    assert device is not None or buf is not None, "disk tensor needs a path or a buf"
+    if device is not None:
+      f = open(device, "a+b")
+      if os.path.getsize(device) < size * dtype.itemsize: os.ftruncate(f.fileno(), size * dtype.itemsize)
+      buf = [f, mmap.mmap(f.fileno(), size * dtype.itemsize), 1]
+    else:
+      buf[2] += 1
+    # NOTE: we don't call super since disk tensors don't use RAM
+    self.size, self.dtype, self._buf = size, dtype, buf
+  def __del__(self):
+    self._buf[2] -= 1
+    if self._buf[2] == 0: self._buf[0].close()
+  def cast(self, arg:Tuple[DType, bool]): return RawDiskBuffer(self.size, arg[0], buf=self._buf, shape=self.shape, offset=self.offset)
+  def reshape(self, arg): return RawDiskBuffer(self.size, self.dtype, buf=self._buf, shape=arg, offset=self.offset)
+  def shrink(self, arg):
+    assert arg[1:] == tuple([(0,x) for x in self.shape[1:]]), f"can only slice the first dim of disk tensor {arg}"
+    offset = arg[0][0]*prod(self.shape[1:])*self.dtype.itemsize
+    size = (arg[0][1]-arg[0][0]) * prod(self.shape[1:])
+    return RawDiskBuffer(size, self.dtype, buf=self._buf, offset=self.offset+offset, shape=(arg[0][1]-arg[0][0],)+self.shape[1:])
+
+  def as_strided(self, arg):
+    return RawDiskBuffer(prod(arg[0]), self.dtype, buf=self._buf, offset=self.offset+arg[2]*self.dtype.itemsize, shape=arg[0])
+
+  def _buffer(self): return memoryview(self._buf[1])[self.offset:self.offset+self.size*self.dtype.itemsize]
+  def readinto(self, buf):
+    self._buf[0].seek(self.offset)
+    self._buf[0].readinto(buf)
+
+disk_fxn_for_op: Dict[Op, Callable] = { BufferOps.MEM: lambda x: x, UnaryOps.NOOP: lambda x: x, UnaryOps.CAST: RawDiskBuffer.cast, MovementOps.AS_STRIDED: RawDiskBuffer.as_strided }
+DiskBuffer = Interpreted(RawDiskBuffer, disk_fxn_for_op, from_underlying=lambda x:x)
\ No newline at end of file
diff --git a/tinygrad_repo/tinygrad/runtime/ops_gpu.py b/tinygrad_repo/tinygrad/runtime/ops_gpu.py
index 6c84e4f199..b8d252838c 100644
--- a/tinygrad_repo/tinygrad/runtime/ops_gpu.py
+++ b/tinygrad_repo/tinygrad/runtime/ops_gpu.py
@@ -1,103 +1,110 @@
 from __future__ import annotations
-import platform, functools
+import os
+os.environ['PYOPENCL_NO_CACHE'] = '1'
+import pathlib
 import numpy as np
 import pyopencl as cl  # type: ignore
-from typing import Dict, Optional, List, ClassVar, Final
-from collections import defaultdict
-from tinygrad.helpers import IMAGE, DEBUG, getenv
-from tinygrad.ops import CompiledBuffer, GlobalCounters, RawBufferCopyInOut, RawBuffer
-from tinygrad.codegen.gpu import GPUCodegen, GPULanguage
+from typing import Optional, List, Tuple
+from tinygrad.helpers import DEBUG, getenv, prod, ImageDType, OSX, fromimport, diskcache
+from tinygrad.ops import Compiled
+from tinygrad.renderer.opencl import OpenCLRenderer
+from tinygrad.runtime.lib import RawBufferCopyInOut, LRUAllocator, RawBufferTransfer
+from tinygrad.codegen.kernel import LinearizerOptions
 
-OSX = platform.system() == "Darwin"
-OSX_TIMING_RATIO = (125/3) if OSX else 1.0   # see test/external_osx_profiling.py to determine this ratio. it's in like GPU clocks or something
-CLCACHE = getenv("CLCACHE", 1)
-FLOAT16 = getenv("FLOAT16", 0)
+OSX_TIMING_RATIO = (125/3) if OSX else 1.0   # see test/external/external_osx_profiling.py to determine this ratio. it's in like GPU clocks or something
 
-class _CL:
-  @functools.cached_property
-  def cl_ctx(self) -> cl.Context:
-    devices : List[cl.Device] = sum([x.get_devices(device_type=cl.device_type.GPU) for x in cl.get_platforms()], [])
-    if len(devices) == 0: devices = sum([x.get_devices(device_type=cl.device_type.CPU) for x in cl.get_platforms()], []) # settle for CPU
-    if len(devices) > 1 or DEBUG >= 1: print(f"using {devices[getenv('CL_DEVICE', 0)]}")
-    return cl.Context(devices=[devices[getenv("CL_DEVICE", 0)]])
-
-  @functools.cached_property
-  def cl_queue(self) -> cl.CommandQueue:
-    return cl.CommandQueue(CL.cl_ctx, properties=cl.command_queue_properties.PROFILING_ENABLE)  # this is an in-order command queue
-CL = _CL()
+# TODO: if you fork and exit the child process after creating anything with cl on AMD, it hangs on e.wait()
+ROCM_LLVM_PATH = pathlib.Path("/opt/rocm/llvm/bin")
+#ROCM_LLVM_PATH = pathlib.Path(__file__).parents[3] / "extra/rocm/build/llvm-project/bin"
+if DEBUG >= 5:
+  early_exec = fromimport("extra.helpers", "enable_early_exec")()
 
-class CLBuffer(RawBufferCopyInOut):
-  # TODO: this can be in RawBuffer generically
-  BUFFER_CACHE : ClassVar[Dict[int, List[cl.Buffer]]] = defaultdict(list)
-
-  def __init__(self, size):
-    self.size = size
-    if len(CLBuffer.BUFFER_CACHE[size]) > 0:
-      self._cl = CLBuffer.BUFFER_CACHE[size].pop()
+class CLAllocator(LRUAllocator):
+  def _do_alloc(self, size, dtype, device, **kwargs):
+    if isinstance(dtype, ImageDType):
+      # NOTE: the memory is a bit off here due to padding, it's buf.row_pitch * buf.height * 4 * dtype.itemsize
+      assert size == prod(dtype.shape), f"image size mismatch {size} != {dtype.shape}"
+      fmt = cl.ImageFormat(cl.channel_order.RGBA, {2: cl.channel_type.HALF_FLOAT, 4: cl.channel_type.FLOAT}[dtype.itemsize])
+      buf = cl.Image(CL.cl_ctxs[int(device)], cl.mem_flags.READ_WRITE, fmt, shape=(dtype.shape[1], dtype.shape[0]))
     else:
-      # TODO: on GPU OOM, clear the cache
-      self._cl = cl.Buffer(CL.cl_ctx, cl.mem_flags.READ_WRITE, size)
-      GlobalCounters.mem_used += self._cl.size
-
-  def __del__(self):
-    if CLCACHE: CLBuffer.BUFFER_CACHE[self._cl.size].append(self._cl)
-    else: GlobalCounters.mem_used -= self._cl.size
+      buf = cl.Buffer(CL.cl_ctxs[int(device)], cl.mem_flags.READ_WRITE, size * dtype.itemsize)
+    setattr(buf, 'device', int(device)) # device is tracked on the underlying buffer
+    return buf
 
-  def copyin(self, x:np.ndarray): cl.enqueue_copy(CL.cl_queue, self._cl, x, is_blocking=False)
-  def copyout(self, x:np.ndarray): cl.enqueue_copy(CL.cl_queue, x, self._cl, is_blocking=True)
-
-class CLImage(RawBuffer):  # pylint: disable=abstract-method
-  fmt : Final = cl.ImageFormat(cl.channel_order.RGBA, cl.channel_type.HALF_FLOAT if FLOAT16 else cl.channel_type.FLOAT)
-  IMAGE : Final = True
+class _CL:
+  def __init__(self):
+    cl_platforms = cl.get_platforms()
+    platform_devices: List[List[cl.Device]] = [y for y in ([x.get_devices(device_type=cl.device_type.GPU) for x in cl_platforms] + [x.get_devices(device_type=cl.device_type.CPU) for x in cl_platforms]) if y]
+    self.devices = [device for device in platform_devices[getenv('CL_PLATFORM', 0)] if device.name not in getenv('CL_EXCLUDE', "").split(",")]
+    self.cl_platform = self.devices[0].platform
+  def post_init(self, device=None):
+    self.cl_ctxs: List[cl.Context] = [cl.Context(devices=[x]) for x in self.devices] if device is None else [cl.Context(devices=[self.devices[device]])]
+    if DEBUG >= 1: print(f"using devices: {[ctx.devices[0].hashable_model_and_version_identifier for ctx in self.cl_ctxs]}")
+    self.cl_queue: List[cl.CommandQueue] = [cl.CommandQueue(ctx, device=ctx.devices[0], properties=cl.command_queue_properties.PROFILING_ENABLE) for ctx in self.cl_ctxs]
+    self.cl_allocator = CLAllocator(CL.cl_ctxs[0].devices[0].get_info(cl.device_info.GLOBAL_MEM_SIZE))
+  def synchronize(self):
+    for q in self.cl_queue: q.finish()
+CL = _CL()
+if not getenv("DELAYED_RUNTIME_INIT", False): CL.post_init()
 
-  def __init__(self, shape):
-    self._cl = cl.Image(CL.cl_ctx, cl.mem_flags.READ_WRITE, CLImage.fmt, shape=(shape[1], shape[0]))
-    GlobalCounters.mem_used += self._cl.row_pitch * self._cl.height
+class CLBuffer(RawBufferCopyInOut, RawBufferTransfer):
+  def __init__(self, size, dtype, device='0'): super().__init__(size, dtype, allocator=CL.cl_allocator, **{'device': device})
+  def _copyin(self, x:np.ndarray):
+    assert not self.dtype.name.startswith("image"), f"can't copyin images {self.dtype}"
+    self.event = cl.enqueue_copy(CL.cl_queue[self._buf.device], self._buf, np.require(x, requirements=['C', 'A']), is_blocking=False)
+  def _copyout(self, x:np.ndarray):
+    assert not self.dtype.name.startswith("image"), f"can't copyout images {self.dtype}"
+    CL.cl_allocator.ensure_has_free_space(self.size, self.dtype, self._device)
+    buf = cl.Buffer(CL.cl_ctxs[self._buf.device], cl.mem_flags.WRITE_ONLY | cl.mem_flags.USE_HOST_PTR, 0, hostbuf=x.data)
+    mapped, event = cl.enqueue_map_buffer(CL.cl_queue[self._buf.device], buf, cl.map_flags.WRITE, 0, self.size, dtype=self.dtype.np, is_blocking=False)
+    with mapped.base: cl.enqueue_copy(CL.cl_queue[self._buf.device], mapped, self._buf, is_blocking=True, wait_for=[event] + ([self.event] if hasattr(self, "event") else []))
+  def _transfer(self, x):
+    if "gfx" in CL.cl_ctxs[x._buf.device].devices[0].name:
+      cl.enqueue_copy_buffer_p2p_amd(CL.cl_platform, CL.cl_queue[x._buf.device], x._buf, self._buf, x.size * x.dtype.itemsize).wait()
+    else: raise NotImplementedError("p2p transfer between devices not implemented on non-amd")
 
-  def __del__(self): GlobalCounters.mem_used -= self._cl.row_pitch * self._cl.height
+@diskcache
+def compile_gpu(prg:str) -> bytes:
+  clprg = cl.Program(CL.cl_ctxs[0], prg)
+  clprg.build()
+  return clprg.get_info(cl.program_info.BINARIES)[0]
 
-@functools.lru_cache(maxsize=None)
 class CLProgram:
-  def __init__(self, name:str, prg:str, binary=False, argdtypes=None):
-    self.name, self.argdtypes, self.clprogram = name, argdtypes, cl.Program(CL.cl_ctx, CL.cl_ctx.devices, [prg]) if binary else cl.Program(CL.cl_ctx, prg)  # type: ignore
-    try:
-      self._clprg = self.clprogram.build()
-    except cl.RuntimeError as e:
-      if DEBUG >= 3: print("FAILED TO BUILD", prg)
-      raise e
-    self.clprg = self._clprg.__getattr__(name)
+  def __init__(self, name:str, prg:bytes, argdtypes=None, options=None):
+    self.name, self.clprograms = name, [cl.Program(ctx, ctx.devices, [prg]*len(ctx.devices)) for ctx in CL.cl_ctxs]  # type: ignore
+    self._clprgs = [clprogram.build(options=options) for clprogram in self.clprograms]
+    self.clprgs = [clprg.__getattr__(name) for clprg in self._clprgs]
     if DEBUG >= 5 and not OSX:
-      binary = self.clprogram.get_info(cl.program_info.BINARIES)[0]
-      if 'Adreno' in CL.cl_ctx.devices[0].name:
-        from disassemblers.adreno import disasm
-        disasm(binary)
+      if 'Adreno' in CL.cl_ctxs[0].devices[0].name:
+        fromimport('disassemblers.adreno', 'disasm')(prg)
+      elif CL.cl_ctxs[0].devices[0].name.startswith('gfx'):
+        asm = early_exec(([ROCM_LLVM_PATH / "llvm-objdump", '-d', '-'], prg))
+        print('\n'.join([x for x in asm.decode('utf-8').split("\n") if 's_code_end' not in x]))
       else:
         # print the PTX for NVIDIA. TODO: probably broken for everything else
-        print(binary.decode('utf-8'))
-    if self.argdtypes is not None: self.clprg.set_scalar_arg_dtypes(self.argdtypes)
+        print(prg.decode('utf-8'))
+    if argdtypes is not None: self.set_argdtypes(argdtypes)
+
+  def set_argdtypes(self, argdtypes): self.argdtypes, _ = argdtypes, [clprg.set_scalar_arg_dtypes(argdtypes) for clprg in self.clprgs]
 
   @staticmethod
-  def max_work_group_size(): return CL.cl_ctx.devices[0].max_work_group_size
+  def max_work_group_size(): return CL.cl_ctxs[0].devices[0].max_work_group_size
 
-  def __call__(self, global_size, local_size, *bufs, wait=False) -> Optional[float]:
-    e = self.clprg(CL.cl_queue, global_size, local_size, *[x._cl if isinstance(x, (CLBuffer, CLImage)) else x for x in bufs])
+  def __call__(self, *bufs, global_size:Tuple[int,int,int], local_size:Optional[Tuple[int,int,int]]=None, wait=False) -> Optional[float]:
+    if not hasattr(self, 'argdtypes'): self.set_argdtypes(tuple(None if x.__class__ is CLBuffer else np.int32 for x in bufs))
+    cl_bufs, wait_for = [], []
+    for x in bufs:
+      if x.__class__ is CLBuffer:
+        cl_bufs.append(x._buf)
+        if hasattr(x, "event"): wait_for.append(x.event)
+      else: cl_bufs.append(x)
+    e = self.clprgs[cl_bufs[0].device](CL.cl_queue[cl_bufs[0].device], [int(g*l) for g,l in zip(global_size, local_size)] if local_size is not None else global_size, local_size, *cl_bufs, wait_for=wait_for)
     if wait:
-      CL.cl_queue.finish()
-      return ((e.profile.end - e.profile.start) * OSX_TIMING_RATIO) * 1e-9
+      e.wait()
+      try:
+        return ((e.profile.end - e.profile.start) * OSX_TIMING_RATIO) * 1e-9
+      except cl.RuntimeError:   # no profiling info available
+        return None
     return None
 
-class CLCodegen(GPUCodegen):
-  lang = GPULanguage(
-    kernel_prefix = "__kernel", buffer_prefix = "__global ", smem_prefix = "__local ",
-    barrier = "barrier(CLK_LOCAL_MEM_FENCE);", float4 = "(float4)",
-    gid = [f'get_global_id({i})' for i in range(3)], lid = [f'get_local_id({i})' for i in range(3)])
-
-class GPUBuffer(CompiledBuffer):
-  raw_buffer_type = CLBuffer
-  # override this method for image
-  @classmethod
-  def create_raw_buffer(cls, shape, backing) -> RawBuffer:
-    if len(shape) == 3 and shape[2] == 4 and IMAGE >= 2 and backing is None: return CLImage(shape)
-    else: return super().create_raw_buffer(shape, backing)
-  codegen_type = CLCodegen
-  runtime_type = CLProgram
+GPUBuffer = Compiled(CLBuffer, LinearizerOptions(), OpenCLRenderer, compile_gpu, CLProgram, CL.synchronize)
diff --git a/tinygrad_repo/tinygrad/shape/__init__.py b/tinygrad_repo/tinygrad/shape/__init__.py
deleted file mode 100644
index 53468d15ca..0000000000
--- a/tinygrad_repo/tinygrad/shape/__init__.py
+++ /dev/null
@@ -1,215 +0,0 @@
-# ShapeTracker allows movement operations to a buffer that don't require a copy to be made.
-from __future__ import annotations
-import functools
-from typing import Tuple, Union, List, Optional, cast
-from tinygrad.helpers import prod, DEBUG
-from tinygrad.shape.symbolic import Variable, MulNode, NumNode, Node
-
-@functools.lru_cache(maxsize=None)
-def to_shape_strides(shape:Tuple[int, ...], strides:Tuple[int, ...]) -> List[Tuple[int, int]]:
-  assert len(shape) == len(strides)
-  ret = [(shape[0], strides[0])] if len(shape) > 0 else []
-  for i in range(1, len(shape)):
-    if (strides[i] != 0 and ret[-1][1] == shape[i]*strides[i]) or ret[-1][0] == 1 or (strides[i] == 0 and ret[-1][1] == 0):
-      ret[-1] = (ret[-1][0] * shape[i], strides[i])
-    else:
-      ret.append((shape[i], strides[i]))
-  return ret
-
-class View:
-  def __init__(self, shape:Tuple[int, ...], strides:Tuple[int, ...], offset:int=0):
-    self.shape, self.strides, self.offset = shape, tuple(stride if shp != 1 else 0 for stride,shp in zip(strides, shape)), offset
-    self.shape_strides = to_shape_strides(self.shape, self.strides)
-    self.contiguous : bool = self.offset == 0 and all(s1 == s2 or s == 1 for s,s1,s2 in zip(self.shape, self.strides, strides_for_shape(self.shape)))
-
-  def __repr__(self): return f"View({self.shape}, {self.strides}, {self.offset})"
-
-  def expr_node(self, idx=None, offset:Union[Node, int]=0):
-    if idx is None: idx = Variable('idx', 0, prod(self.shape))
-    ret = [Variable.num(self.offset)+offset]
-    acc = 1
-    for d,s in self.shape_strides[::-1]:
-      ret.append(((idx//acc)%d)*s)
-      acc *= d
-    return Variable.sum(ret)
-
-  # generate an expression if you have a variable or expression for each index
-  def expr_idxs(self, idxs, offset:Union[Node, int]=0):
-    return Variable.sum([Variable.num(self.offset)+offset] + [Variable(idx, 0, sh-1)*st for idx,sh,st in zip(idxs, self.shape, self.strides) if sh != 1 and st != 0])
-
-class ZeroView:
-  def __init__(self, old_shape:Tuple[int, ...], arg):
-    self.old_shape, self.arg = old_shape, arg
-    self.shape : Tuple[int, ...] = tuple([y-x for x,y in self.arg])
-    # fake properties
-    self.strides, self.contiguous, self.offset = strides_for_shape(self.shape), False, 0
-
-  def __repr__(self): return f"ZeroView({self.old_shape}, {self.arg})"
-
-  def expr_node(self, idx=None, valid=None):
-    if idx is None: idx = Variable('idx', 0, prod([y-x for x,y in self.arg]))
-    expr, acc = [valid] if valid is not None else [], 1
-    for s,ns,(x,y) in list(zip(self.old_shape, self.shape, self.arg))[::-1]:
-      base = ((idx//acc) % ns) + x
-      expr += ([base >= 0] if x < 0 else []) + ([base < s] if y > s else [])
-      acc *= ns
-    return Variable.ands(expr)
-
-  def expr_idxs(self, idxs, offset=0): raise NotImplementedError("ZeroView doesn't support expr_idxs")
-
-ViewTypes = Union[View, ZeroView]
-
-@functools.lru_cache(maxsize=None)
-def strides_for_shape(shape:Tuple[int, ...]) -> Tuple[int, ...]:
-  strides = [1]
-  for d in shape[::-1][:-1]: strides = [d*strides[0]] + strides
-  return tuple(st if s != 1 else 0 for st, s in zip(strides, shape))
-
-@functools.lru_cache(maxsize=None)
-def view_from_shape(shape:Tuple[int, ...]) -> View:
-  assert all(isinstance(x, int) for x in shape) and len(shape) != 0
-  return View(tuple(shape), strides_for_shape(shape))
-
-def merge_views(vm2:View, vm1:View) -> Optional[View]:
-  new_strides, new_offset = [], vm2.expr_node(Variable.num(vm1.offset))
-  assert isinstance(new_offset, NumNode), "new_offset wasn't a number?!?"
-  for s,st in zip(vm1.shape, vm1.strides):
-    this_dim = View(vm2.shape, vm2.strides).expr_node(Variable('idx', 0, s-1)*st)
-    if s == 1:
-      new_strides.append(0)   # all shape 1 can have stride 0
-    elif isinstance(this_dim, NumNode) and this_dim.b == 0:
-      new_strides.append(0)
-    elif isinstance(this_dim, Variable):
-      new_strides.append(1)
-    elif isinstance(this_dim, MulNode) and isinstance(this_dim.a, Variable):
-      new_strides.append(this_dim.b)
-    else:
-      if DEBUG >= 4: print("can't simplify", s, this_dim.render())
-      break
-  return View(vm1.shape, tuple(new_strides), new_offset.b) if len(new_strides) == len(vm1.strides) else None
-
-class ShapeTracker:
-  def __init__(self, shape:Union[ShapeTracker, Tuple[int, ...]], views:Optional[List[ViewTypes]]=None):
-    self.views : List[ViewTypes] = views if views is not None else (shape.views[:] if isinstance(shape, ShapeTracker) else [view_from_shape(shape)])
-  def __repr__(self): return f"ShapeTracker(shape={self.shape}, views={self.views})"
-  def copy(self) -> ShapeTracker: return ShapeTracker(self.shape, self.views[:])
-
-  @property
-  def contiguous(self) -> bool: return len(self.views) == 1 and self.views[-1].contiguous
-
-  @property
-  def shape(self) -> Tuple[int, ...]: return self.views[-1].shape
-
-  @property
-  def strides(self) -> Tuple[int, ...]: return self.views[-1].strides
-
-  @property
-  def offset(self) -> int: return self.views[-1].offset
-
-  # this is the real size
-  def size(self): return prod([s for s,st in zip(self.shape, self.strides) if st != 0])
-
-  def _expr_idx(self, idx):
-    valid = Variable.num(1)
-    for v in self.views[0:-1][::-1]:
-      if isinstance(v, ZeroView): valid = v.expr_node(idx, valid)
-      else: idx = v.expr_node(idx)
-    return idx, valid
-
-  def simplify(self):
-    if len(self.views) >= 2 and isinstance(self.views[-2], View) and isinstance(self.views[-1], View):
-      new_view = merge_views(self.views[-2], self.views[-1])
-      if new_view:
-        if DEBUG >= 4: print(f"st simplify : {self.views[-2]} + {self.views[-1]} = {new_view}")
-        self.views = self.views[:-2] + [new_view]
-        self.simplify()
-
-  # TODO: arg order is reversed here
-  def expr_idxs(self, offset=0, idxs=None):
-    if idxs is None: idxs = [f"idx{i}" for i in range(len(self.shape))]
-    return self._expr_idx(self.views[-1].expr_idxs(idxs, offset))
-
-  def expr_node(self, idx='idx', offset=0):
-    return self._expr_idx(self.views[-1].expr_node(Variable(idx, 0, prod(self.shape)-1), offset))
-
-  def movement_op(self, op, arg:Union[Tuple[int, ...], Tuple[Tuple[int, int], ...]]) -> ShapeTracker:
-    return getattr(self, str(op).split(".")[1].lower())(arg)
-  def needs_valid(self) -> bool:
-    return any(isinstance(v, ZeroView) for v in self.views)
-
-  def reshape(self, new_shape : Tuple[int, ...]) -> ShapeTracker:
-    assert isinstance(new_shape, tuple)
-    if self.shape == new_shape: return self
-    assert all(isinstance(x, int) and x != 0 for x in new_shape), f"shape must be ints and can't contain 0 {new_shape}"
-    assert prod(self.shape) == prod(new_shape), f"can't reshape {self.shape} -> {new_shape}"
-
-    view = View(new_shape, strides_for_shape(new_shape))
-    if self.contiguous: self.views[-1] = view   # NOTE: if it's contiguous it can't have an offset
-    else:
-      # NOTE: the last view in self.views is never a ZeroView
-      if (merged_view := merge_views(cast(View, self.views[-1]), view)) is not None: self.views[-1] = merged_view
-      else: self.views.append(view)
-    return self
-
-  def permute(self, axis : Tuple[int, ...]) -> ShapeTracker:
-    assert isinstance(axis, tuple)
-    assert all(isinstance(x, int) and x >= 0 and x < len(self.shape) for x in axis), f"invalid permute {axis} for {self.shape}"
-    assert len(set(axis)) == len(axis) and len(axis) == len(self.shape), f"can't permute {self.shape} with {axis}"
-    self.views[-1] = View(tuple(self.shape[a] for a in axis), tuple(self.strides[a] for a in axis), self.offset)
-    return self
-
-  # TODO: this is a special case of slice with strides, remove it
-  # though it's nice that it can't change size
-  def flip(self, axis : Tuple[int, ...]) -> ShapeTracker:
-    return self.stride(tuple(-1 if i in axis else 1 for i in range(len((self.shape)))))
-
-  # *** under this line are not invertible ***
-
-  # TODO: take this functionality out of slice
-  def pad(self, arg : Tuple[Tuple[int, int], ...]) -> ShapeTracker:
-    assert isinstance(arg, tuple)
-    assert all((b>=0 and e>=0) for b,e in arg) and len(arg) == len(self.shape)
-    return self.shrink(tuple((-b,s+e) for s,(b,e) in zip(self.shape, arg)))
-
-  # TODO: take the pad functionality out of shrink
-  def shrink(self, arg : Tuple[Tuple[int, int], ...]) -> ShapeTracker:
-    assert isinstance(arg, tuple)
-    assert len(arg) == len(self.shape)
-    offset = sum([self.strides[i]*x for i,(x,_) in enumerate(arg)])
-    zeroview = ZeroView(self.shape, arg)
-    self.views[-1] = View(tuple(y-x for x,y in arg), self.strides, self.offset+offset)
-    if zeroview.expr_node().min == 0:  # may be invalid
-      # if we add a ZeroView, we add another (stock) view also for modding
-      self.views += [zeroview, View(self.shape, strides_for_shape(self.shape))]
-    return self
-
-  def expand(self, new_shape : Tuple[int, ...]) -> ShapeTracker:
-    assert isinstance(new_shape, tuple)
-    assert all(isinstance(x, int) for x in new_shape), f"non ints for expand in {new_shape}"
-    assert all(x == y or x == 1 for x,y in zip(self.shape, new_shape)), f"can't expand {self.shape} into {new_shape}"
-    strides : Tuple[int, ...] = tuple(s if x == y else 0 for s,(x,y) in zip(self.strides, zip(self.shape, new_shape)))
-    self.views[-1] = View(new_shape, strides, self.offset)
-    return self
-
-  # TODO: combine with slice? this doesn't require a ZeroView, though slice shouldn't always either
-  def stride(self, mul : Tuple[int, ...]) -> ShapeTracker:
-    assert isinstance(mul, tuple)
-    assert all(isinstance(x, int) for x in mul)
-    strides = tuple(z*m for z,m in zip(self.strides, mul))
-    new_shape = tuple((s+(abs(m)-1))//abs(m) for s,m in zip(self.shape, mul))
-    offset = sum([(s-1)*z for s,z,m in zip(self.shape, self.strides, mul) if m < 0])
-    self.views[-1] = View(new_shape, strides, self.offset + offset)
-    return self
-
-# returns the axes to create new_shape if new_shape can be created by combining axis from old_shape
-def get_contraction(old_shape:Tuple[int, ...], new_shape:Tuple[int, ...]):
-  if len(new_shape) > len(old_shape): return None
-  new_shape_i : int = 0
-  shape_idx_groups : List[List[int]] = [[] for _ in range(len(new_shape))]
-  for old_shape_i, t in enumerate(old_shape):
-    if new_shape[new_shape_i] % t != 0 or prod([old_shape[x] for x in shape_idx_groups[new_shape_i]]) * t > new_shape[new_shape_i]:
-      return None
-    shape_idx_groups[new_shape_i].append(old_shape_i)
-    if prod([old_shape[x] for x in shape_idx_groups[new_shape_i]]) == new_shape[new_shape_i] and new_shape_i < len(new_shape) - 1:
-      new_shape_i += 1
-  return shape_idx_groups
diff --git a/tinygrad_repo/tinygrad/shape/shapetracker.py b/tinygrad_repo/tinygrad/shape/shapetracker.py
new file mode 100644
index 0000000000..a4a6b65978
--- /dev/null
+++ b/tinygrad_repo/tinygrad/shape/shapetracker.py
@@ -0,0 +1,221 @@
+# ShapeTracker allows movement operations to a buffer that don't require a copy to be made.
+from __future__ import annotations
+import functools, operator
+from dataclasses import dataclass
+from typing import Tuple, List, Optional, Dict, cast
+from tinygrad.ops import MovementOps
+from tinygrad.helpers import prod, DEBUG, dedup
+from tinygrad.shape.symbolic import Variable, MulNode, NumNode, Node, SumNode, sint
+from tinygrad.shape.view import View
+
+@functools.lru_cache(maxsize=None)
+def to_shape_strides(shape:Tuple[int, ...], strides:Tuple[int, ...]) -> Tuple[Tuple[int, int], ...]:
+  assert len(shape) == len(strides)
+  ret = [(shape[0], strides[0])] if shape else []
+  for i in range(1, len(shape)):
+    if ret[-1][1] == shape[i]*strides[i] or ret[-1][0] == 1:
+      ret[-1] = (ret[-1][0] * shape[i], strides[i])
+    elif shape[i] == 1:
+      continue
+    else:
+      ret.append((shape[i], strides[i]))
+  return tuple(ret)
+
+def expr_node_mask(view:View, idx, valid=None) -> Node:
+  expr = [valid] if valid is not None else []
+  if view.mask is not None:
+    acc = 1
+    for ns,(x,y) in reversed(list(zip(view.shape, view.mask))):
+      if x != 0 or y != ns:
+        base = ((idx//acc) % ns)
+        expr += [base >= x, base < y]
+      acc *= ns
+  return Variable.ands(expr)
+
+# generate an expression if you have a single idx variable
+def expr_node(view:View, idx=None) -> Node:
+  if idx is None: idx = Variable('idx', 0, prod(view.shape)-1)
+  ret: List[Node] = [Variable.num(view.offset) if isinstance(view.offset, int) else view.offset] if view.offset else []
+  acc = 1
+  for d,s in reversed(to_shape_strides(view.shape, view.strides)):
+    ret.append(((idx//acc)%d)*s)
+    acc *= d
+  return Variable.sum(ret)
+
+# generate an expression if you have a variable or expression for each index
+def expr_idxs(view:View, idxs) -> Node:
+  assert len(idxs) == len(view.shape), f"need an idx for all dimensions {idxs} vs {view.shape}"
+  return Variable.sum([Variable.num(view.offset) if isinstance(view.offset, int) else view.offset] + [idx*st for idx,sh,st in zip(idxs, view.shape, view.strides) if sh != 1 and st != 0])
+
+@functools.lru_cache(maxsize=None)
+def merge_views(vm2:View, vm1:View) -> Optional[View]:
+  if vm2.mask: return None  # this isn't supported yet
+  mst = ShapeTracker((vm2, vm1))
+  strides = mst.real_strides()
+  if None in strides: return None
+  return View.create(vm1.shape, cast(Tuple[sint, ...], strides), mst.real_offset(), vm1.mask)
+
+@functools.lru_cache(maxsize=None)
+def idxs_to_idx(shape:Tuple[int, ...], idxs) -> Node:
+  assert len(idxs) == len(shape), "need an idx for all dimensions"
+  acc = 1
+  ret = []
+  for tidx,d in reversed(list(zip(idxs, shape))):
+    ret.append(tidx * acc)
+    acc *= d
+  return Variable.sum(ret)
+
+@dataclass(frozen=True)
+class ShapeTracker:
+  views: Tuple[View, ...]
+  def __post_init__(self): assert isinstance(self.views, tuple) and all(isinstance(v, View) for v in self.views), "ShapeTracker must be created with a tuple of Views"
+
+  @staticmethod
+  def from_shape(shape:Tuple[sint, ...]): return ShapeTracker((View.create(shape),))
+
+  @property
+  def contiguous(self) -> bool: return len(self.views) == 1 and self.views[0].contiguous
+
+  @property
+  def shape(self) -> Tuple[sint, ...]: return self.views[-1].shape
+
+  # this is the real size (ish)
+  def size(self): return self.views[-1].size()
+
+  def vars(self) -> List[Variable]: return dedup(functools.reduce(operator.add, [v.vars() for v in self.views], []))
+
+  @property
+  def var_vals(self) -> Dict[Variable, int]:
+    ret:Dict[Variable, int] = {}
+    for v in self.vars():
+      var, val = v.unbind()
+      assert var not in ret or ret[var] == val, f"{var} has conflicted values {val} and {ret[var]}"
+      ret[var] = val
+    return ret
+
+  def unbind(self) -> ShapeTracker: return ShapeTracker(tuple(v.unbind() for v in self.views))
+
+  def to_movement_ops(self) -> List[Tuple[MovementOps, Tuple]]:
+    to_apply:List[Tuple[MovementOps, Tuple]] = []
+    for v in self.views:
+      real_shape = tuple(y-x for x,y in v.mask) if v.mask else v.shape
+      real_offset = v.offset + (sum(x*st for (x,_),st in zip(v.mask, v.strides)) if v.mask else 0)
+      # first, we apply the offset
+      # then, we make it the correct shape
+      # then, we apply permutations
+      # TODO: don't use as_strided
+      to_apply.append((MovementOps.AS_STRIDED, (tuple([s if st != 0 else 1 for s,st in zip(real_shape, v.strides)]), v.strides, real_offset)))
+      # then, we apply pre expand pads
+      if v.mask is not None:
+        pre_expand_pads = tuple((x,s-y) if st != 0 else (0,0) for (x,y),s,st in zip(v.mask, v.shape, v.strides))
+        post_expand_pads = tuple((x,s-y) if st == 0 else (0,0) for (x,y),s,st in zip(v.mask, v.shape, v.strides))
+        if any(x != (0,0) for x in pre_expand_pads):
+          to_apply.append((MovementOps.PAD, pre_expand_pads))
+          real_shape = tuple(x+s[0]+s[1] for x,s in zip(real_shape, pre_expand_pads))
+      # then, we do any expands
+      if any(s != 1 and st == 0 for s,st in zip(real_shape, v.strides)): to_apply.append((MovementOps.EXPAND, real_shape))
+      # lastly, we apply post expand pads
+      if v.mask is not None and any(x != (0,0) for x in post_expand_pads): to_apply.append((MovementOps.PAD, post_expand_pads))
+    return to_apply
+
+  # these are multiview strides, value is None if it's not a simple strided dimension
+  # TODO: this can be shared code between simplify and merge_views
+  def real_offset(self) -> sint:
+    real_offset, _ = self.expr_node(Variable('zero', 0, 0))
+    return real_offset.b if isinstance(real_offset, NumNode) else real_offset
+
+  # NOTE: if a stride is not always valid, it will be None
+  def real_strides(self, ignore_valid=False) -> Tuple[Optional[sint], ...]:
+    if len(self.views) == 1 and self.views[-1].mask is None: return self.views[-1].strides
+    idxs = [Variable(f"idx{i}", 0, s-1) for i,s in enumerate(self.shape)]
+    idx, valid = self.expr_idxs(idxs)
+    ret: List[Optional[sint]] = [None] * len(self.views[-1].shape)
+    for this_dim in (idx.nodes if isinstance(idx, SumNode) else [idx]):
+      if isinstance(this_dim, MulNode) and isinstance(this_dim.a, Variable) and this_dim.a in idxs:
+        ret[idxs.index(this_dim.a)] = this_dim.b
+      elif isinstance(this_dim, Variable) and this_dim in idxs:
+        ret[idxs.index(this_dim)] = 1
+    idx_vars, valid_vars = idx.vars(), valid.vars()
+    for i,tidx in enumerate(idxs):
+      if tidx in valid_vars and not ignore_valid: ret[i] = None
+      elif tidx not in idx_vars: ret[i] = 0
+    return tuple(ret)
+  def unit_stride_axes(self, ignore_valid=False) -> List[int]: return [i for i,st in enumerate(self.real_strides(ignore_valid)) if st == 1]
+
+  def _expr_idx(self, idx, valid) -> Tuple[Node, Node]:
+    for v in reversed(self.views[0:-1]):
+      if valid.max == 0: return Variable.num(-1), valid
+      valid = expr_node_mask(v, idx, valid)
+      idx = expr_node(v, idx)
+    return idx, valid
+
+  def simplify(self) -> ShapeTracker:
+    if len(self.views) >= 2:
+      new_view = merge_views(self.views[-2], self.views[-1])
+      if new_view:
+        if DEBUG >= 4: print(f"st simplify : {self.views[-2]} + {self.views[-1]} = {new_view}")
+        return ShapeTracker(self.views[:-2] + (new_view,)).simplify()
+    return self
+
+  def expr_idxs(self, idxs=None):
+    if idxs is None: idxs = [Variable(f"idx{i}", 0, s-1) for i,s in enumerate(self.shape)]
+    idx = expr_idxs(self.views[-1], tuple(idxs))
+    valid = expr_node_mask(self.views[-1], idxs_to_idx(self.views[-1].shape, tuple(idxs)))
+    return self._expr_idx(idx, valid)
+
+  def expr_node(self, idx='idx'):
+    if idx.__class__ is str: idx = Variable(idx, 0, prod(self.shape)-1)
+    return self._expr_idx(expr_node(self.views[-1], idx), expr_node_mask(self.views[-1], idx))
+
+  def axis_is_masked(self, axis) -> bool:
+    _, valid = self.expr_idxs()
+    return f'idx{axis}' in [v.expr for v in valid.vars()]
+
+  # *** under this line are the movement ops ***
+
+  def pad(self, arg: Tuple[Tuple[int, int], ...]) -> ShapeTracker:
+    return ShapeTracker(self.views[0:-1] + (self.views[-1].pad(arg), ))
+
+  def shrink(self, arg: Tuple[Tuple[sint, sint], ...]) -> ShapeTracker:
+    return ShapeTracker(self.views[0:-1] + (self.views[-1].shrink(arg), ))
+
+  def expand(self, new_shape: Tuple[sint, ...]) -> ShapeTracker:
+    return ShapeTracker(self.views[0:-1] + (self.views[-1].expand(new_shape), ))
+
+  def permute(self, axis: Tuple[int, ...]) -> ShapeTracker:
+    return ShapeTracker(self.views[0:-1] + (self.views[-1].permute(axis), ))
+
+  def stride(self, mul: Tuple[int, ...]) -> ShapeTracker:
+    return ShapeTracker(self.views[0:-1] + (self.views[-1].stride(mul), ))
+
+  def reshape(self, new_shape: Tuple[sint, ...]) -> ShapeTracker:
+    new_view = self.views[-1].reshape(new_shape)
+    if new_view is None:
+      extra_view = View.create(new_shape)
+      # last chance to merge. TODO: move into View
+      if (merged_view := merge_views(self.views[-1], extra_view)) is not None:
+        return ShapeTracker(self.views[0:-1] + (merged_view,))
+      return ShapeTracker(self.views + (extra_view, ))
+    return ShapeTracker(self.views[0:-1] + (new_view,))
+
+# returns the axes to create new_shape if new_shape can be created by combining axis from old_shape
+# TODO: if we remove movementops from lazy.py we can delete this
+def get_contraction(old_shape:Tuple[sint, ...], new_shape:Tuple[sint, ...]) -> Optional[List[List[int]]]:
+  # Pre-allocate all groups.
+  axis_groups: List[List[int]] = [[] for _ in range(len(new_shape))]
+  # Index for new_shape and axis_groups.
+  i: int = 0
+  old_shape_i: int = 0
+  while old_shape_i < len(old_shape):
+    # 1s exist in new_shape only will lead to empty axes group creations.
+    if new_shape[i] == 1 and old_shape[old_shape_i] != 1:
+      if i < len(new_shape) - 1: i += 1
+    else:
+      axis_groups[i].append(old_shape_i)
+      axis_group_size = prod([old_shape[x] for x in axis_groups[i]])
+      # Move to next axes group if total size of all dimensions match.
+      if axis_group_size == new_shape[i]:
+        if i < len(new_shape) - 1: i += 1
+      elif axis_group_size > new_shape[i]: return None
+      old_shape_i += 1
+  return axis_groups
diff --git a/tinygrad_repo/tinygrad/shape/symbolic.py b/tinygrad_repo/tinygrad/shape/symbolic.py
index be06ae5f69..0f27e76664 100644
--- a/tinygrad_repo/tinygrad/shape/symbolic.py
+++ b/tinygrad_repo/tinygrad/shape/symbolic.py
@@ -1,191 +1,352 @@
 from __future__ import annotations
-import math, itertools, functools
-from typing import List, Dict, Callable, Type, Union
-from tinygrad.helpers import partition, all_same
+from abc import abstractmethod
+import functools
+from math import gcd
+from itertools import product
+from tinygrad.helpers import partition
+from typing import List, Dict, Callable, Tuple, Type, Union, Optional, Any, Iterator
 
 # NOTE: Python has different behavior for negative mod and floor div than c
 # symbolic matches the Python behavior, but the code output is agnostic, and will never have negative numbers in div or mod
 
-def create_node(typ:Type[Node], *args):
-  ret = typ(*args)
-  assert ret.min <= ret.max, f"min greater than max! {ret.min} {ret.max} when creating {typ} {args}"
-  if ret.min == ret.max: return NumNode(ret.min)
-  return ret
+def is_sym_int(x: Any) -> bool: return isinstance(x, (int, Node))
 
 class Node:
-  b: int
+  b: Union[Node, int]
   min: int
   max: int
-  def render(self, ops=None, ctx=None) -> str:
+  def render(self, ops=None, ctx=None) -> Any:
     if ops is None: ops = render_python
-    assert isinstance(self, NumNode) or self.min != self.max
+    assert self.__class__ in (Variable, NumNode) or self.min != self.max
     return ops[type(self)](self, ops, ctx)
+  def vars(self): return []
+
+  def expand_idx(self) -> VariableOrNum: return next((v for v in self.vars() if v.expr is None), NumNode(0))
+  # expand a Node into List[Node] that enumerates the underlying Variables from min to max
+  # expand increments earlier variables faster than later variables (as specified in the argument)
+  @functools.lru_cache(maxsize=None)  # pylint: disable=method-cache-max-size-none
+  def expand(self, idxs:Optional[Tuple[VariableOrNum, ...]]=None) -> List[Node]:
+    if idxs is None: idxs = (self.expand_idx(),)
+    return [self.substitute(dict(zip(idxs, (NumNode(x) for x in rep)))) for rep in Node.iter_idxs(idxs)]
+  @staticmethod
+  def iter_idxs(idxs:Tuple[VariableOrNum, ...]) -> Iterator[Tuple[int,...]]:
+    yield from (x[::-1] for x in product(*[[x for x in range(v.min, v.max + 1)] for v in idxs[::-1]]))
+  # substitute Variables with the values in var_vals
+  def substitute(self, var_vals: Dict[VariableOrNum, Node]) -> Node: raise RuntimeError(self.__class__.__name__)
+  def unbind(self) -> Tuple[Node, Optional[int]]: return self.substitute({v: v.unbind()[0] for v in self.vars() if v.val is not None}), None
+
   @functools.cached_property
   def key(self) -> str: return self.render(ctx="DEBUG")
-  def __repr__(self): return "<"+self.key+">"
+  @functools.cached_property
+  def hash(self) -> int: return hash(self.key)
+  def __repr__(self): return self.render(ctx="REPR")
+  def __str__(self): return "<"+self.key+">"
+  def __hash__(self): return self.hash
+  def __bool__(self): return not (self.max == self.min == 0)
   def __eq__(self, other:object) -> bool:
     if not isinstance(other, Node): return NotImplemented
     return self.key == other.key
   def __neg__(self): return self*-1
-  def __add__(self, b:Union[Node, int]): return Variable.sum([self, b if isinstance(b, Node) else Variable.num(b)])
-  def __sub__(self, b:Union[Node, int]): return self+-b
-  def __ge__(self, b:int): return create_node(GeNode, self, b)
-  def __lt__(self, b:int): return create_node(LtNode, self, b)
-  def __mul__(self, b:int):
+  def __add__(self, b:Union[Node,int]): return Variable.sum([self, b if isinstance(b, Node) else Variable.num(b)])
+  def __radd__(self, b:int): return self+b
+  def __sub__(self, b:Union[Node,int]): return self+-b
+  def __rsub__(self, b:int): return -self+b
+  def __le__(self, b:Union[Node,int]): return self < (b+1)
+  def __gt__(self, b:Union[Node,int]): return (-self) < (-b)
+  def __ge__(self, b:Union[Node,int]): return (-self) < (-b+1)
+  def __lt__(self, b:Union[Node,int]): return create_node(LtNode(self, b))
+  def __mul__(self, b:Union[Node, int]):
     if b == 0: return NumNode(0)
-    elif b == 1: return self
-    if isinstance(self, MulNode): return self.a*(self.b*b) # two muls is one mul
-    if isinstance(self, SumNode): return Variable.sum([x*b for x in self.nodes]) # distribute mul into sum
-    return create_node(MulNode, self, b)
+    if b == 1: return self
+    if self.__class__ is NumNode: return NumNode(self.b*b) if isinstance(b, int) else b*self.b
+    return create_node(MulNode(self, b.b)) if isinstance(b, NumNode) else create_node(MulNode(self, b))
+  def __rmul__(self, b:int): return self*b
 
   # *** complex ops ***
 
-  def __floordiv__(self, b:int):
+  def __rfloordiv__(self, b:int):
+    if self.min > b >= 0: return NumNode(0)
+    if isinstance(self, NumNode): return NumNode(b // self.b)
+    raise RuntimeError(f"not supported: {b} // {self}")
+  def __floordiv__(self, b:Union[Node,int], factoring_allowed=True):
+    if isinstance(b, Node):
+      if b.__class__ is NumNode: return self // b.b
+      if self == b: return NumNode(1)
+      if (b - self).min > 0 and self.min >= 0: return NumNode(0) # b - self simplifies the node
+      raise RuntimeError(f"not supported: {self} // {b}")
     assert b != 0
     if b < 0: return (self//-b)*-1
     if b == 1: return self
-    if isinstance(self, DivNode): return self.a//(self.b*b) # two divs is one div
-    if isinstance(self, MulNode) and self.b % b == 0: return self.a*(self.b//b)
-    if isinstance(self, MulNode) and b % self.b == 0: return self.a//(b//self.b)
-    if isinstance(self, SumNode):
-      factors, tmp_nofactor = partition(self.nodes, lambda x: (isinstance(x, (MulNode, NumNode))) and x.b%b == 0)
-      nofactor = []
-      # ugh, i doubt this is universally right
-      for x in tmp_nofactor:
-        if isinstance(x, NumNode):
-          if (x.b%b) != x.b:
-            factors.append(Variable.num(x.b - (x.b%b)))  # python does floor division
-          nofactor.append(Variable.num(x.b%b))
-        else:
-          nofactor.append(x)
-      gcd = [math.gcd(x.b, b) if isinstance(x, (MulNode, NumNode)) else None for x in nofactor]
-      if len(factors) > 0:
-        # these don't have to be the same, just having a common factor
-        if len(gcd) > 0 and all_same(gcd) and gcd[0] is not None and gcd[0] > 1:
-          nofactor_term = Variable.sum([(x.a * (x.b//gcd[0])) if isinstance(x, MulNode) else Variable.num(x.b//gcd[0]) for x in nofactor])//(b//gcd[0])
-        else:
-          nofactor_term = Variable.sum(nofactor)//b
-        return Variable.sum([(x.a * (x.b//b)) if isinstance(x, MulNode) else Variable.num(x.b//b) for x in factors] + [nofactor_term])
-      else:
-        muls = [x.b for x in nofactor if isinstance(x, MulNode)]
-        for m in muls:
-          if m > 1 and b%m == 0:
-            return (self//m)//(b//m)
+
+    # the numerator of div is not allowed to be negative
     if self.min < 0:
       offset = self.min//b
-      return (self+offset*b)//b - offset
-    return create_node(DivNode, self, b)
+      # factor out an "offset" to make the numerator positive. don't allowing factoring again
+      return (self + -offset*b).__floordiv__(b, factoring_allowed=False) + offset
+    return create_node(DivNode(self, b))
 
-  def __mod__(self, b:int):
+  def __rmod__(self, b:int):
+    if self.min > b >= 0: return NumNode(b)
+    if isinstance(self, NumNode): return NumNode(b % self.b)
+    raise RuntimeError(f"not supported: {b} % {self}")
+  def __mod__(self, b:Union[Node,int]):
+    if isinstance(b, Node):
+      if b.__class__ is NumNode: return self % b.b
+      if self == b: return NumNode(0)
+      if (b - self).min > 0 and self.min >= 0: return self # b - self simplifies the node
+      raise RuntimeError(f"not supported: {self} % {b}")
     assert b > 0
     if b == 1: return NumNode(0)
-    if isinstance(self, SumNode):
-      new_nodes = []
-      for x in self.nodes:
-        if isinstance(x, NumNode): new_nodes.append(Variable.num(x.b%b))
-        elif isinstance(x, MulNode): new_nodes.append(x.a * (x.b%b))
-        else: new_nodes.append(x)
-      a = Variable.sum(new_nodes)
-    elif isinstance(self, MulNode):
-      a = self.a * (self.b%b)
-    else:
-      a = self
-    if a.min >= 0 and a.max < b: return a
-    if a.min < 0: return (a + ((a.min//b)*b)) % b
-    return create_node(ModNode, a, b)
+    if self.min >= 0 and self.max < b: return self
+    if (self.min//b) == (self.max//b): return self - (b*(self.min//b))
+    if self.min < 0: return (self - ((self.min//b)*b)) % b
+    return create_node(ModNode(self, b))
+
+  @staticmethod
+  def num(num:int) -> NumNode: return NumNode(num)
 
   @staticmethod
-  def num(num:int) -> Node: return NumNode(num)
+  def factorize(nodes:List[Node]) -> List[Node]:
+    mul_groups: Dict[Node, int] = {}
+    for x in nodes:
+      a,b = (x.a,x.b) if isinstance(x, MulNode) else (x,1)
+      mul_groups[a] = mul_groups.get(a, 0) + b
+    return [MulNode(a, b_sum) if b_sum != 1 else a for a, b_sum in mul_groups.items() if b_sum != 0]
 
   @staticmethod
   def sum(nodes:List[Node]) -> Node:
-    # expand any sums inside one sum
-    if any([isinstance(x, SumNode) for x in nodes]):
-      nodes, sum_nodes = partition(nodes, lambda x: not isinstance(x, SumNode))
-      for x in sum_nodes: nodes += x.nodes
-      return Variable.sum(nodes)
-
-    # combine any numbers inside a sum
-    nodes, num_nodes = partition(nodes, lambda x: not isinstance(x, NumNode))
-    nodes.append(NumNode(sum([x.b for x in num_nodes])))
-
-    # combine any MulNodes that factorize (big hack sticking the MulNode(x, 1) on things)
-    nodes, mul_nodes = partition(nodes, lambda x: not isinstance(x, MulNode))
-    mul_nodes += [MulNode(x, 1) for x in nodes]
-    mul_nodes = sorted(mul_nodes, key=lambda x: x.a.render()) # group by equality (ugh, uses render!)
-    new_nodes = [k * sum(x.b for x in g) for k, g in itertools.groupby(mul_nodes, key=lambda x: x.a)]
-    nodes = [x if not isinstance(x, MulNode) or x.b != 1 else x.a for x in new_nodes]
-
-    # filter 0s
-    nodes = [x for x in nodes if x.min != 0 or x.max != 0]
-    return create_node(SumNode, nodes) if len(nodes) > 1 else (nodes[0] if len(nodes) == 1 else NumNode(0))
+    nodes = [x for x in nodes if x.max or x.min]
+    if not nodes: return NumNode(0)
+    if len(nodes) == 1: return nodes[0]
+
+    new_nodes: List[Node] = []
+    num_node_sum = 0
+    for node in SumNode(nodes).flat_components:
+      if node.__class__ is NumNode: num_node_sum += node.b
+      else: new_nodes.append(node)
+
+    if len(new_nodes) > 1 and len(set([x.a if isinstance(x, MulNode) else x for x in new_nodes])) < len(new_nodes):
+      new_nodes = Node.factorize(new_nodes)
+    if num_node_sum: new_nodes.append(NumNode(num_node_sum))
+    return create_rednode(SumNode, new_nodes) if len(new_nodes) > 1 else new_nodes[0] if len(new_nodes) == 1 else NumNode(0)
 
   @staticmethod
   def ands(nodes:List[Node]) -> Node:
-    if any((x.min == 0 and x.max == 0) for x in nodes): return NumNode(0)
+    if not nodes: return NumNode(1)
+    if len(nodes) == 1: return nodes[0]
+    if any(not x for x in nodes): return NumNode(0)
 
     # filter 1s
     nodes = [x for x in nodes if x.min != x.max]
-    return create_node(AndNode, nodes) if len(nodes) > 1 else (nodes[0] if len(nodes) == 1 else NumNode(1))
+    return create_rednode(AndNode, nodes) if len(nodes) > 1 else (nodes[0] if len(nodes) == 1 else NumNode(1))
 
 # 4 basic node types
 
 class Variable(Node):
-  def __new__(cls, expr:str, nmin:int, nmax:int):
+  def __new__(cls, expr:Optional[str], nmin:int, nmax:int):
     assert nmin >= 0 and nmin <= nmax
     if nmin == nmax: return NumNode(nmin)
     return super().__new__(cls)
 
-  def __init__(self, expr:str, nmin:int, nmax:int):
+  def __init__(self, expr:Optional[str], nmin:int, nmax:int):
     self.expr, self.min, self.max = expr, nmin, nmax
+    self.val:Optional[int] = None
+  def bind(self, val):
+    assert self.val is None and self.min<=val<=self.max, f"cannot bind {val} to {self}"
+    self.val = val
+    return self
+  def unbind(self) -> Tuple[Variable, int]:
+    assert self.val is not None, f"cannot unbind {self}"
+    return Variable(self.expr, self.min, self.max), self.val
+  def vars(self): return [self]
+  def substitute(self, var_vals: Dict[VariableOrNum, Node]) -> Node: return var_vals[self] if self in var_vals else self
 
 class NumNode(Node):
   def __init__(self, num:int):
-    self.b, self.min, self.max = num, num, num
+    assert isinstance(num, int), f"{num} is not an int"
+    self.b:int = num
+    self.min, self.max = num, num
+  def bind(self, val):
+    assert self.b == val, f"cannot bind {val} to {self}"
+    return self
+  def __eq__(self, other): return self.b == other
+  def __hash__(self): return self.hash  # needed with __eq__ override
+  def substitute(self, var_vals: Dict[VariableOrNum, Node]) -> Node: return self
+
+def create_node(ret:Node):
+  assert ret.min <= ret.max, f"min greater than max! {ret.min} {ret.max} when creating {type(ret)} {ret}"
+  if ret.min == ret.max: return NumNode(ret.min)
+  return ret
 
 class OpNode(Node):
-  def __init__(self, a:Node, b:int):
+  def __init__(self, a:Node, b:Union[Node, int]):
     self.a, self.b = a, b
-    self.min, self.max = self.minmax(a,b)
-  minmax = staticmethod(lambda a,b: (1//0, 1//0))
+    self.min, self.max = self.get_bounds()
+  def vars(self): return self.a.vars() + (self.b.vars() if isinstance(self.b, Node) else [])
+  @abstractmethod
+  def get_bounds(self) -> Tuple[int, int]: pass
 
-class RedNode(Node):
-  def __init__(self, nodes:List[Node]):
-    self.nodes = nodes
-    self.min, self.max = self.minmax(nodes)
-  minmax = staticmethod(lambda nodes: (1//0, 1//0))
+class LtNode(OpNode):
+  def __floordiv__(self, b: Union[Node, int], _=False): return (self.a//b) < (self.b//b)
+  def get_bounds(self) -> Tuple[int, int]:
+    if isinstance(self.b, int):
+      return (1, 1) if self.a.max < self.b else (0, 0) if self.a.min >= self.b else (0, 1)
+    return (1, 1) if self.a.max < self.b.min else (0, 0) if self.a.min >= self.b.max else (0, 1)
+  def substitute(self, var_vals: Dict[VariableOrNum, Node]) -> Node: return self.a.substitute(var_vals) < (self.b if isinstance(self.b, int) else self.b.substitute(var_vals))
 
-# operation nodes
+class MulNode(OpNode):
+  def __lt__(self, b: Union[Node, int]):
+    if isinstance(b, Node) or isinstance(self.b, Node) or self.b == -1: return Node.__lt__(self, b)
+    sgn = 1 if self.b > 0 else -1
+    return Node.__lt__(self.a*sgn, (b + abs(self.b) - 1)//abs(self.b))
+  def __mul__(self, b: Union[Node, int]): return self.a*(self.b*b) # two muls in one mul
+  def __floordiv__(self, b: Union[Node, int], factoring_allowed=False): # NOTE: mod negative isn't handled right
+    if self.b % b == 0: return self.a*(self.b//b)
+    if b % self.b == 0 and self.b > 0: return self.a//(b//self.b)
+    return Node.__floordiv__(self, b, factoring_allowed)
+  def __mod__(self, b: Union[Node, int]):
+    a = (self.a * (self.b%b))
+    return Node.__mod__(a, b)
+  def get_bounds(self) -> Tuple[int, int]:
+    return (self.a.min*self.b, self.a.max*self.b) if self.b >= 0 else (self.a.max*self.b, self.a.min*self.b)
+  def substitute(self, var_vals: Dict[VariableOrNum, Node]) -> Node: return self.a.substitute(var_vals) * (self.b if isinstance(self.b, int) else self.b.substitute(var_vals))
 
-class GeNode(OpNode): minmax = staticmethod(lambda a,b: (int(a.min >= b), int(a.max >= b)))
-class LtNode(OpNode): minmax = staticmethod(lambda a,b: (int(a.max < b), int(a.min < b)))
-class MulNode(OpNode): minmax = staticmethod(lambda a,b: (a.min*b, a.max*b) if b >= 0 else (a.max*b, a.min*b))
 class DivNode(OpNode):
-  @staticmethod
-  def minmax(a, b):
-    assert a.min >= 0
-    return a.min//b, a.max//b
+  def __floordiv__(self, b: Union[Node, int], _=False): return self.a//(self.b*b) # two divs is one div
+  def get_bounds(self) -> Tuple[int, int]:
+    assert self.a.min >= 0 and isinstance(self.b, int)
+    return self.a.min//self.b, self.a.max//self.b
+  def substitute(self, var_vals: Dict[VariableOrNum, Node]) -> Node: return self.a.substitute(var_vals) // self.b
 
 class ModNode(OpNode):
-  @staticmethod
-  def minmax(a, b):
-    assert a.min >= 0
-    if a.max - a.min >= b or (a.min != a.max and a.min%b >= a.max%b): return (0, b-1)
-    return a.min%b, a.max%b
+  def __mod__(self, b: Union[Node, int]):
+    if isinstance(b, Node) or isinstance(self.b, Node): return Node.__mod__(self, b)
+    return self.a % b if gcd(self.b, b) == b else Node.__mod__(self, b)
+  def __floordiv__(self, b: Union[Node, int], factoring_allowed=True):
+    if (self.b % b == 0): return (self.a//b) % (self.b//b) # put the div inside mod
+    return Node.__floordiv__(self, b, factoring_allowed)
+  def get_bounds(self) -> Tuple[int, int]:
+    assert self.a.min >= 0 and isinstance(self.b, int)
+    return (0, self.b-1) if self.a.max - self.a.min >= self.b or (self.a.min != self.a.max and self.a.min%self.b >= self.a.max%self.b) else (self.a.min%self.b, self.a.max%self.b)
+  def substitute(self, var_vals: Dict[VariableOrNum, Node]) -> Node: return self.a.substitute(var_vals) % self.b
+
+class RedNode(Node):
+  def __init__(self, nodes:List[Node]): self.nodes = nodes
+  def vars(self): return functools.reduce(lambda l,x: l+x.vars(), self.nodes, [])
+
+class SumNode(RedNode):
+  @functools.lru_cache(maxsize=None)  # pylint: disable=method-cache-max-size-none
+  def __mul__(self, b: Union[Node, int]): return Node.sum([x*b for x in self.nodes]) # distribute mul into sum
+  @functools.lru_cache(maxsize=None)  # pylint: disable=method-cache-max-size-none
+  def __floordiv__(self, b: Union[Node, int], factoring_allowed=True):
+    fully_divided: List[Node] = []
+    rest: List[Node] = []
+    if isinstance(b, SumNode):
+      nu_num = sum(node.b for node in self.flat_components if node.__class__ is NumNode)
+      de_num = sum(node.b for node in b.flat_components if node.__class__ is NumNode)
+      if nu_num > 0 and de_num and (d:=nu_num//de_num) > 0: return NumNode(d) + (self-b*d) // b
+    if isinstance(b, Node):
+      for x in self.flat_components:
+        if x % b == 0: fully_divided.append(x // b)
+        else: rest.append(x)
+      if (sum_fully_divided:=create_rednode(SumNode, fully_divided)) != 0: return sum_fully_divided + create_rednode(SumNode, rest) // b
+      return Node.__floordiv__(self, b, False)
+    if b == 1: return self
+    if not factoring_allowed: return Node.__floordiv__(self, b, factoring_allowed)
+    fully_divided, rest = [], []
+    _gcd = b
+    divisor = 1
+    for x in self.flat_components:
+      if x.__class__ in (NumNode, MulNode):
+        if x.b%b == 0: fully_divided.append(x//b)
+        else:
+          rest.append(x)
+          _gcd = gcd(_gcd, x.b)
+          if x.__class__ == MulNode and divisor == 1 and b%x.b == 0: divisor = x.b
+      else:
+        rest.append(x)
+        _gcd = 1
+    if _gcd > 1: return Node.sum(fully_divided) + Node.sum(rest).__floordiv__(_gcd) // (b//_gcd)
+    if divisor > 1: return Node.sum(fully_divided) + Node.sum(rest).__floordiv__(divisor) // (b//divisor)
+    return Node.sum(fully_divided) + Node.__floordiv__(Node.sum(rest), b)
+
+  @functools.lru_cache(maxsize=None)  # pylint: disable=method-cache-max-size-none
+  def __mod__(self, b: Union[Node, int]):
+    if isinstance(b, SumNode):
+      nu_num = sum(node.b for node in self.flat_components if node.__class__ is NumNode)
+      de_num = sum(node.b for node in b.flat_components if node.__class__ is NumNode)
+      if nu_num > 0 and de_num and (d:=nu_num//de_num) > 0: return (self-b*d) % b
+    if isinstance(b, Node) and (b - self).min > 0: return self # b - self simplifies the node
+    new_nodes: List[Node] = []
+    for x in self.nodes:
+      if x.__class__ is NumNode: new_nodes.append(Variable.num(x.b%b))
+      elif isinstance(x, MulNode): new_nodes.append(x.a * (x.b%b))
+      else: new_nodes.append(x)
+    return Node.__mod__(Node.sum(new_nodes), b)
+
+  def __lt__(self, b:Union[Node,int]):
+    lhs: Node = self
+    if isinstance(b, int):
+      new_sum = []
+      for x in self.nodes:
+        # TODO: should we just force the last one to always be the number
+        if isinstance(x, NumNode): b -= x.b
+        else: new_sum.append(x)
+      lhs = Node.sum(new_sum)
+      nodes = lhs.nodes if isinstance(lhs, SumNode) else [lhs]
+      muls, others = partition(nodes, lambda x: isinstance(x, MulNode) and x.b > 0 and x.max >= b)
+      if muls:
+        # NOTE: gcd in python 3.8 takes exactly 2 args
+        mul_gcd = b
+        for x in muls: mul_gcd = gcd(mul_gcd, x.b)  # type: ignore  # mypy cannot tell x.b is int here
+        all_others = Variable.sum(others)
+        if all_others.min >= 0 and all_others.max < mul_gcd:
+          lhs, b = Variable.sum([mul//mul_gcd for mul in muls]), b//mul_gcd
+    return Node.__lt__(lhs, b)
+
+  def substitute(self, var_vals: Dict[VariableOrNum, Node]) -> Node: return Variable.sum([node.substitute(var_vals) for node in self.nodes])
+
+  @property
+  def flat_components(self): # recursively expand sumnode components
+    new_nodes = []
+    for x in self.nodes: new_nodes += (x.flat_components if isinstance(x, SumNode) else [x])
+    return new_nodes
+
+class AndNode(RedNode):
+  def __floordiv__(self, b: Union[Node, int], _=True): return Variable.ands([x//b for x in self.nodes])
+  def substitute(self, var_vals: Dict[VariableOrNum, Node]) -> Node:
+    subed = []
+    for node in self.nodes:
+      if not (sub:=node.substitute(var_vals)): return NumNode(0)
+      subed.append(sub)
+    return Variable.ands(subed)
+
+def create_rednode(typ:Type[RedNode], nodes:List[Node]):
+  ret = typ(nodes)
+  if typ == SumNode: ret.min, ret.max = (sum([x.min for x in nodes]), sum([x.max for x in nodes]))
+  elif typ == AndNode: ret.min, ret.max = (min([x.min for x in nodes]), max([x.max for x in nodes]))
+  return create_node(ret)
 
-# reduce nodes
+@functools.lru_cache(maxsize=None)
+def sym_rename(s) -> str: return f"s{sym_rename.cache_info().currsize}"
+def sym_render(a: Union[Node, int], ops=None, ctx=None) -> str: return str(a) if isinstance(a, int) else a.render(ops, ctx)
+def sym_infer(a: Union[Node, int], var_vals: Dict[Variable, int]) -> int:
+  if isinstance(a, (int, float)): return a
+  ret = a.substitute({k:Variable.num(v) for k, v in var_vals.items()})
+  assert isinstance(ret, NumNode), f"sym_infer didn't produce NumNode from {a} with {var_vals}"
+  return ret.b
 
-class SumNode(RedNode): minmax = staticmethod(lambda nodes: (sum([x.min for x in nodes]), sum([x.max for x in nodes])))
-class AndNode(RedNode): minmax = staticmethod(lambda nodes: (min([x.min for x in nodes]), max([x.max for x in nodes])))
+# symbolic int
+sint = Union[Node, int]
+VariableOrNum = Union[Variable, NumNode]
 
-render_python : Dict[Type, Callable] = {
-  Variable: lambda self,ops,ctx: f"{self.expr}<{self.min},{self.max}>" if ctx == "DEBUG" else f"{self.expr}",
+render_python: Dict[Type, Callable] = {
+  Variable: lambda self,ops,ctx: f"{self.expr}[{self.min}-{self.max}{'='+str(self.val) if self.val is not None else ''}]" if ctx == "DEBUG" else (f"Variable('{self.expr}', {self.min}, {self.max})" if ctx == "REPR" else f"{self.expr}"),
   NumNode: lambda self,ops,ctx: f"{self.b}",
-  MulNode: lambda self,ops,ctx: f"({self.a.render(ops,ctx)}*{self.b})",
+  MulNode: lambda self,ops,ctx: f"({self.a.render(ops,ctx)}*{sym_render(self.b,ops,ctx)})",
   DivNode: lambda self,ops,ctx: f"({self.a.render(ops,ctx)}//{self.b})",
   ModNode: lambda self,ops,ctx: f"({self.a.render(ops,ctx)}%{self.b})",
-  GeNode: lambda self,ops,ctx: f"({self.a.render(ops,ctx)}>={self.b})",
-  LtNode: lambda self,ops,ctx: f"({self.a.render(ops,ctx)}<{self.b})",
+  LtNode: lambda self,ops,ctx: f"({self.a.render(ops,ctx)}<{sym_render(self.b,ops,ctx)})",
   SumNode: lambda self,ops,ctx: f"({'+'.join(sorted([x.render(ops,ctx) for x in self.nodes]))})",
-  AndNode: lambda self,ops,ctx: f"({'&&'.join(sorted([x.render(ops,ctx) for x in self.nodes]))})"
+  AndNode: lambda self,ops,ctx: f"({' and '.join(sorted([x.render(ops,ctx) for x in self.nodes]))})"
 }
\ No newline at end of file
diff --git a/tinygrad_repo/tinygrad/shape/view.py b/tinygrad_repo/tinygrad/shape/view.py
new file mode 100644
index 0000000000..960db7ca75
--- /dev/null
+++ b/tinygrad_repo/tinygrad/shape/view.py
@@ -0,0 +1,132 @@
+from __future__ import annotations
+import functools, operator
+from dataclasses import dataclass
+from typing import Tuple, List, Optional, Dict, cast
+from tinygrad.helpers import prod, all_int, dedup
+from tinygrad.shape.symbolic import Node, NumNode, Variable, VariableOrNum, is_sym_int, sint
+
+@functools.lru_cache(maxsize=None)
+def filter_strides(shape:Tuple[int, ...], strides:Tuple[int, ...]) -> Tuple[int, ...]:
+  return tuple(stride if shp != 1 else 0 for stride, shp in zip(strides, shape))
+
+@functools.lru_cache(maxsize=None)
+def strides_for_shape(shape:Tuple[int, ...]) -> Tuple[int, ...]:
+  strides = [1] if shape else []
+  for d in shape[::-1][:-1]: strides = [d*strides[0]] + strides
+  return filter_strides(shape, tuple(strides))
+
+@dataclass(frozen=True)
+class View:
+  shape:Tuple[sint, ...]
+  strides:Tuple[sint, ...]
+  offset:sint
+  mask:Optional[Tuple[Tuple[sint, sint], ...]]
+  contiguous:bool
+
+  @staticmethod
+  @functools.lru_cache(maxsize=None)
+  def create(shape:Tuple[sint, ...], strides:Optional[Tuple[sint, ...]]=None, offset:sint=0, mask:Optional[Tuple[Tuple[sint, sint], ...]]=None):
+    strides = filter_strides(shape, strides) if strides else strides_for_shape(shape)
+    contiguous = offset == 0 and mask is None and all(s1 == s2 for s1,s2 in zip(strides, strides_for_shape(shape)))
+    return View(shape, strides, offset, mask, contiguous)
+
+  @functools.lru_cache(maxsize=None)  # pylint: disable=method-cache-max-size-none
+  def size(self): return prod([s.max if isinstance(s, Node) else s for s,st in zip(self.shape, self.strides) if st != 0])
+
+  def vars(self) -> List[Variable]:
+    flatten_mask = tuple(x for m in self.mask for x in m) if self.mask is not None else tuple()
+    return dedup(functools.reduce(operator.add, [x.vars() for x in self.shape+self.strides+(self.offset,)+flatten_mask if isinstance(x, Node)], []))
+
+  def unbind(self) -> View:
+    unbound_vars:Dict[VariableOrNum,Node] = {v: v.unbind()[0] for v in self.vars() if v.val is not None}
+    new_shape = tuple([s if isinstance(s, int) else s.substitute(unbound_vars) for s in self.shape])
+    new_strides = tuple([s if isinstance(s, int) else s.substitute(unbound_vars) for s in self.strides])
+    new_offset = self.offset if isinstance(self.offset, int) else self.offset.substitute(unbound_vars)
+    new_mask = tuple((a if isinstance(a, int) else a.substitute(unbound_vars), b if isinstance(b, int) else b.substitute(unbound_vars)) for (a, b) in self.mask) if self.mask is not None else None
+    return View.create(new_shape, new_strides, new_offset, new_mask)
+
+  # MovementOps live here now
+
+  def __unsafe_resize(self, arg: Tuple[Tuple[sint, sint], ...], mask=None) -> View:
+    offset = sum([s * x[0] for s, x in zip(self.strides,arg)])
+    if self.mask:
+      # move the old mask
+      nmask = tuple([(max(mx-ax, 0), min(my-ax, ay-ax)) for (mx,my),(ax,ay) in zip(self.mask, arg)])
+      # merge the masks if we have two
+      mask = tuple([(max(mx1, mx2), min(my1, my2)) for (mx1, my1), (mx2, my2) in zip(nmask, mask)]) if mask is not None else nmask
+    shape = [y-x for x,y in arg]
+    return View.create(tuple(s.b if isinstance(s, NumNode) else s for s in shape), self.strides, self.offset+offset, mask)
+
+  @functools.lru_cache(maxsize=None)  # pylint: disable=method-cache-max-size-none
+  def pad(self, arg: Tuple[Tuple[int, int], ...]) -> View:
+    assert all((b>=0 and e>=0) for b,e in arg) and len(arg) == len(self.shape)
+    if any(b or e for b, e in arg):
+      zvarg = tuple([(-b,s+e) for s,(b,e) in zip(self.shape, arg)])
+      mask = tuple([(b,s+b) for s,(b,_) in zip(self.shape, arg)])
+      return self.__unsafe_resize(zvarg, mask=mask)
+    return self
+
+  @functools.lru_cache(maxsize=None)  # pylint: disable=method-cache-max-size-none
+  def shrink(self, arg: Tuple[Tuple[sint, sint], ...]) -> View:
+    assert all((b>=0 and e<=s) for s,(b,e) in zip(self.shape,arg)) and len(arg) == len(self.shape)
+    return self.__unsafe_resize(arg)
+
+  @functools.lru_cache(maxsize=None)  # pylint: disable=method-cache-max-size-none
+  def expand(self, new_shape: Tuple[sint, ...]) -> View:
+    assert len(new_shape) == len(self.shape)
+    assert all(is_sym_int(x) and (s == x or (s == 1 and st == 0)) for s,x,st in zip(self.shape, new_shape, self.strides)), f"can't expand {self.shape} into {new_shape}"
+    # NOTE: can the mask ever be (0,0)?
+    mask = tuple([(((0,0) if m != (0,1) else (0,ns)) if s != ns else m) for m,s,ns in zip(self.mask, self.shape, new_shape)]) if self.mask else None
+    return View.create(new_shape, self.strides, self.offset, mask)
+
+  @functools.lru_cache(maxsize=None)  # pylint: disable=method-cache-max-size-none
+  def permute(self, axis: Tuple[int, ...]) -> View:
+    assert all(isinstance(x, int) and x >= 0 and x < len(self.shape) for x in axis), f"invalid permute {axis} for {self.shape}"
+    assert len(set(axis)) == len(axis) and len(axis) == len(self.shape), f"can't permute {self.shape} with {axis}"
+    return View.create(tuple([self.shape[a] for a in axis]), tuple([self.strides[a] for a in axis]), self.offset, tuple([self.mask[a] for a in axis]) if self.mask is not None else None)
+
+  @functools.lru_cache(maxsize=None)  # pylint: disable=method-cache-max-size-none
+  def stride(self, mul: Tuple[int, ...]) -> View:
+    # except for the negative case, you can build this from the others. invertible in the negative case
+    assert all(isinstance(x, int) and x != 0 for x in mul), f"invalid stride {mul} for {self.shape}"
+    strides = tuple([z*m for z,m in zip(self.strides, mul)])
+    new_shape = tuple([(s+(abs(m)-1))//abs(m) for s,m in zip(self.shape, mul)])
+    offset = sum([(s-1)*z for s,z,m in zip(self.shape, self.strides, mul) if m < 0])
+    mask = tuple([(((mx if m > 0 else s-my)+(abs(m)-1))//abs(m), ((my if m > 0 else s-mx)+(abs(m)-1))//abs(m)) for (mx,my),s,m in zip(self.mask, self.shape, mul)]) if self.mask is not None else None
+    return View.create(new_shape, strides, self.offset + offset, mask)
+
+  @functools.lru_cache(maxsize=None)  # pylint: disable=method-cache-max-size-none
+  def reshape(self, new_shape: Tuple[sint, ...]) -> Optional[View]:
+    if self.shape == new_shape: return self
+
+    assert all(is_sym_int(x) and x > 0 for x in new_shape), f"shape must be symbolic ints and can't contain 0 or negative numbers {new_shape}"
+    # check for the same size
+    if all_int(self.shape):
+      if all_int(new_shape):
+        assert prod(self.shape) == prod(new_shape), f"size mismatched, can't reshape {self.shape=} -> {new_shape=}"
+      else:
+        assert all(isinstance(s, (int, Variable)) for s in new_shape), f"{self.shape=} -> {new_shape=} contains non (int, Variable) dim"
+        assert prod(self.shape) == prod([s if isinstance(s, int) else cast(Variable,s).val for s in new_shape]), f"size mismatched, can't reshape {self.shape=} -> {new_shape=}"
+
+    # after the asserts, it's okay to check contiguous
+    if self.contiguous: return View.create(new_shape)
+
+    # check if this is adding or removing 1s (only)
+    # NOTE: this is optional, but removes most calls to (expensive!) merge_views (with mask, not optional)
+    if [x for x in self.shape if x != 1] == [x for x in new_shape if x != 1]:
+      new_strides: List[sint] = [y for x,y in zip(self.shape, self.strides) if x != 1]
+      new_strides_tuple: Tuple[sint, ...] = tuple([0 if x == 1 else new_strides.pop(0) for x in new_shape])
+      new_mask_tuple: Optional[Tuple[Tuple[sint, sint], ...]] = None
+      if self.mask:
+        for x,y in zip(self.shape, self.mask):
+          if x == 1 and y != (0, 1):
+            new_mask_tuple = ((0,0),) * len(new_shape)
+            break
+        else:
+          new_mask: List[Tuple[sint, sint]] = [y for x,y in zip(self.shape, self.mask) if x != 1]
+          new_mask_tuple = tuple([(0,1) if x == 1 else new_mask.pop(0) for x in new_shape])
+      return View.create(new_shape, new_strides_tuple, self.offset, new_mask_tuple)
+
+    # TODO: bring the merge_views logic here for more caching
+
+    return None
diff --git a/tinygrad_repo/tinygrad/tensor.py b/tinygrad_repo/tinygrad/tensor.py
index b7bd2146be..f0c3280b11 100644
--- a/tinygrad_repo/tinygrad/tensor.py
+++ b/tinygrad_repo/tinygrad/tensor.py
@@ -1,18 +1,25 @@
 # inspired by https://github.com/karpathy/micrograd/blob/master/micrograd/engine.py
 from __future__ import annotations
-import math, functools, itertools
+import time, math
+from collections import defaultdict
+from functools import partialmethod, reduce
+from itertools import accumulate
 import numpy as np
-from typing import List, Tuple, Callable, Optional, ClassVar, Type, Union, Sequence
-from tinygrad.helpers import prod, argfix, make_pair, getenv, DEBUG, flatten
-from tinygrad.lazy import Device, LazyBuffer
-from tinygrad.image import image_conv2d_decorator
+from typing import List, Tuple, Callable, Optional, ClassVar, Type, Union, Sequence, Any, Iterable, Set
+
+from tinygrad.helpers import ImageDType, argfix, make_pair, getenv, IMAGE, DEBUG, flatten, DType, dtypes, prod, all_int
+from tinygrad.lazy import LazyBuffer
+from tinygrad.ops import Device, LoadOps
+from tinygrad.shape.symbolic import sint
+from tinygrad.realize import run_schedule
 
 # An instantiation of the Function is the Context
 class Function:
   def __init__(self, device:str, *tensors:Tensor):
-    self.device, self.parents = device, tensors
-    self.needs_input_grad = [t.requires_grad for t in self.parents]
-    self.requires_grad = True if any(self.needs_input_grad) else (None if any(x is None for x in self.needs_input_grad) else False)
+    self.device = device
+    self.needs_input_grad = [t.requires_grad for t in tensors]
+    self.requires_grad = True if any(self.needs_input_grad) else None if None in self.needs_input_grad else False
+    if self.requires_grad: self.parents = tensors
 
   def forward(self, *args, **kwargs): raise NotImplementedError(f"forward not implemented for {type(self)}")
   def backward(self, *args, **kwargs): raise RuntimeError(f"backward not implemented for {type(self)}")
@@ -29,133 +36,191 @@ import tinygrad.mlops as mlops
 # **** start with two base classes, Tensor and Function ****
 
 class Tensor:
+  __slots__ = "lazydata", "requires_grad", "grad", "_ctx"
   __deletable__ = ('_ctx',)
-  training : ClassVar[bool] = False
-  no_grad : ClassVar[bool] = False
-
-  def __init__(self, data, device=Device.DEFAULT, requires_grad:Optional[bool]=None):
-    if isinstance(data, list):
-      data = np.array(data, dtype=np.float32)
-    elif isinstance(data, LazyBuffer) and data.device != device:
-      # TODO: this has to realize, it shouldn't have to
-      data = data.realize().toCPU()
-
-    if isinstance(data, np.ndarray):
-      data = data if data.shape else data.reshape((1,))
-      self.lazydata = LazyBuffer.fromCPU(data.astype(np.float32), device)
-    elif isinstance(data, LazyBuffer):
-      self.lazydata = data
-    else:
-      raise RuntimeError(f"can't create Tensor from {data}")
-
+  training: ClassVar[bool] = False
+  class train:
+    def __init__(self, val=True): self.val = val
+    def __enter__(self):
+      self.prev = Tensor.training
+      Tensor.training = self.val
+    def __exit__(self, exc_type: Any, exc_value: Any, traceback: Any): Tensor.training = self.prev
+
+  no_grad: ClassVar[bool] = False
+  default_type: ClassVar[DType] = dtypes.float32
+  def __init__(self, data:Union[int, float, list, LazyBuffer, np.ndarray], device:Optional[str]=None, dtype:Optional[DType]=None, requires_grad:Optional[bool]=None):
+    assert dtype is None or isinstance(dtype, DType), f"invalid dtype {dtype}"
+    device = Device.canonicalize(device)
     # tensors have gradients, buffers do not
-    self.grad : Optional[Tensor] = None
+    self.grad: Optional[Tensor] = None
 
     # NOTE: this can be in three states. False and None: no gradient, True: gradient
     # None (the default) will be updated to True if it's put in an optimizer
-    self.requires_grad : Optional[bool] = requires_grad
+    self.requires_grad: Optional[bool] = requires_grad
 
     # internal variables used for autograd graph construction
-    self._ctx : Optional[Function] = None
+    self._ctx: Optional[Function] = None
+    if isinstance(data, LazyBuffer): assert dtype is None or dtype == data.dtype, "dtype doesn't match, and casting isn't supported"
+    elif isinstance(data, (int, float)):
+      data = LazyBuffer.loadop(LoadOps.CONST, tuple(), dtype or Tensor.default_type, device, data)
+    elif data.__class__ is list:
+      assert dtype is None or dtype.np is not None, f"{dtype} doesn't have a numpy dtype"
+      data = LazyBuffer.fromCPU(np.array(data, dtype=(dtype or Tensor.default_type).np))
+    elif isinstance(data, np.ndarray):
+      assert dtype is None or dtype.np is not None, f"{dtype} doesn't have a numpy dtype"
+      if data.shape == ():
+        data = LazyBuffer.loadop(LoadOps.CONST, tuple(), dtype or dtypes.from_np(data.dtype), device, data.item())
+      else:
+        data = LazyBuffer.fromCPU(data.astype(dtype.np) if dtype is not None and dtype.np is not None else data)
+    else: raise RuntimeError(f"can't create Tensor from {data}")
+
+    # data is a LazyBuffer, but it might be on the wrong device
+    self.lazydata = data if data.device == device else data.copy_to_device(device)
 
   def __repr__(self):
-    return f"<Tensor {self.lazydata if self.lazydata.realized is None else self.lazydata.realized!r} with grad {(self.grad.lazydata if self.grad else None)!r}>"
+    return f"<Tensor {self.lazydata!r} on {self.device} with grad {(self.grad.lazydata if self.grad else None)!r}>"
+
+  # Python has a non moving GC, so this should be okay
+  def __hash__(self): return id(self)
 
   @property
-  def shape(self) -> Tuple[int, ...]: return self.lazydata.shape
+  def device(self) -> str: return self.lazydata.device
 
-  # dtype handling was very broken. it's always float32 now
   @property
-  def dtype(self) -> type: return np.float32
+  def shape(self) -> Tuple[sint, ...]: return self.lazydata.shape
 
   @property
-  def device(self) -> str: return self.lazydata.device
+  def dtype(self) -> DType: return self.lazydata.dtype
 
   # ***** data handlers ****
 
+  @staticmethod
+  def corealize(lst:Iterable[Tensor]):
+    seen:Set[LazyBuffer] = set()
+    sched = []
+    for t in lst: sched += t.lazydata.schedule(seen)
+    run_schedule(sched)
+
   def realize(self) -> Tensor:
-    self.lazydata.realize()
+    run_schedule(self.lazydata.schedule())
     return self
 
   def assign(self, x) -> Tensor:
-    if not isinstance(x, Tensor): x = Tensor(x)
-    assert self.shape == x.shape
+    # TODO: this is a hack for writing to DISK
+    if self.device.startswith("DISK"):
+      if x.__class__ is not Tensor: x = Tensor(x, device="CPU", dtype=self.dtype)
+      self.contiguous().realize().lazydata.realized._copyin(x.numpy())  # type: ignore
+      return self
+    if x.__class__ is not Tensor: x = Tensor(x, device=self.device, dtype=self.dtype)
+    assert self.shape == x.shape and self.device == x.device, f"assign shape mismatch {self.shape} != {x.shape} or device mismatch {self.device} != {x.device}"
     assert not x.requires_grad  # self requires_grad is okay?
     if DEBUG >= 4: print(f"assign {self.lazydata} <- {x.lazydata}")
-    if self.lazydata.realized is not None and not getenv("DISALLOW_ASSIGN"): x.lazydata.output_buffer = self.lazydata.realized
+    if self.dtype == x.dtype and self.lazydata.realized is not None and not getenv("DISALLOW_ASSIGN"): x.lazydata.output_buffer = self.lazydata.realized
     self.lazydata = x.lazydata
     return self
 
-  def detach(self): return Tensor(self.lazydata, device=self.device, requires_grad=False)
-  def numpy(self) -> np.ndarray: return self.lazydata.toCPU()
+  def detach(self) -> Tensor: return Tensor(self.lazydata, device=self.device, requires_grad=False)
+  def numpy(self) -> np.ndarray:
+    assert all_int(self.shape), f"no numpy if shape is symbolic, {self.shape=}"
+    assert self.dtype.np is not None, f"no numpy dtype for {self.dtype}"
+    return self.detach().cast(dtypes.from_np(self.dtype.np)).contiguous().to('CPU').realize().lazydata.realized.toCPU().reshape(self.shape)
 
   # TODO: if things are realized this won't work
   def to_(self, device:str):
     assert self.lazydata.realized is None
     self.lazydata.device = device
-    if self.grad:
-      self.grad.lazydata.device = device
+    if self.grad: self.grad.to_(device)
 
-  def to(self, device:str):
+  def to(self, device:str) -> Tensor:
     ret = Tensor(self.lazydata, device)
-    if self.grad:
-      ret.grad = self.grad.to(device)
+    if self.grad: ret.grad = self.grad.to(device)
     return ret
 
-  # ***** creation helper functions *****
+  # ***** creation llop entrypoint *****
 
   @staticmethod
-  def zeros(*shape, **kwargs): return Tensor([0], **kwargs).reshape([1]*len(shape)).expand(shape).contiguous()
+  def _loadop(op, sz, device:Optional[str]=None, dtype:Optional[DType]=None, arg=None, **kwargs):
+    return Tensor(LazyBuffer.loadop(op, (sz,), Tensor.default_type if dtype is None else dtype, Device.canonicalize(device), arg), dtype=dtype, device=device, **kwargs)
 
   @staticmethod
-  def ones(*shape, **kwargs): return Tensor([1], **kwargs).reshape([1]*len(shape)).expand(shape).contiguous()
+  def empty(*shape, **kwargs):
+    assert all_int(shape), f"cannot create with symbolic shape {shape}"
+    return Tensor._loadop(LoadOps.EMPTY, prod(shape), **kwargs).reshape(shape)
 
+  _seed: int = int(time.time())
   @staticmethod
-  def zeros_like(tensor, **kwargs): return Tensor.zeros(*tensor.shape, **kwargs)
+  def manual_seed(seed=0): Tensor._seed = seed
 
   @staticmethod
-  def empty(*shape, **kwargs): return Tensor.zeros(*shape, **kwargs)
+  def rand(*shape, **kwargs):
+    assert all_int(shape), f"cannot create with symbolic shape {shape}"
+    Tensor._seed += 1
+    return Tensor._loadop(LoadOps.RAND, prod(shape), arg=Tensor._seed, **kwargs).reshape(shape)
 
-  @staticmethod
-  def eye(dim, **kwargs): return Tensor([1], **kwargs).slice(((0,dim+1),)).reshape(1, dim+1).expand(dim, dim+1).reshape(dim*(dim+1)).slice(((0,dim*dim),)).reshape(dim, dim)
+  # ***** creation helper functions *****
 
-  # TODO: below line, remove use of numpy here and make lazy
-  # TODO: requires cumsum to remove numpy
   @staticmethod
-  def arange(stop, start=0, step=1, **kwargs): return Tensor(np.arange(start=start, stop=stop, step=step, dtype=np.float32), **kwargs)
+  def full(shape:Tuple[sint, ...], fill_value, **kwargs): return Tensor(fill_value, **kwargs).reshape([1]*len(new_shape := argfix(shape))).expand(new_shape)
 
-  # ***** (numpy) rng helper functions *****
-  # TODO: move randomness generation out of numpy
+  @staticmethod
+  def zeros(*shape, **kwargs): return Tensor.full(argfix(*shape), 0, **kwargs)
 
-  _rng : ClassVar[np.random.Generator] = np.random.default_rng()
   @staticmethod
-  def manual_seed(seed=None): Tensor._rng = np.random.default_rng(seed=seed)
+  def ones(*shape, **kwargs): return Tensor.full(argfix(*shape), 1, **kwargs)
 
   @staticmethod
-  def rand(*shape, **kwargs) -> Tensor: return Tensor(Tensor._rng.random(size=shape, dtype=np.float32), **kwargs)
+  def arange(start, stop=None, step=1, **kwargs):
+    if stop is None: stop, start = start, 0
+    return Tensor.full((math.ceil((stop-start)/step),), step, **kwargs).cumsum() + (start - step)
 
-  # TODO: replace with a transformation from uniform -> gaussian
   @staticmethod
-  def randn(*shape, **kwargs) -> Tensor: return Tensor(Tensor._rng.standard_normal(size=shape, dtype=np.float32), **kwargs)
+  def eye(dim:int, **kwargs): return Tensor.full((dim,1),1,**kwargs).pad(((0,0),(0,dim))).reshape(dim*(dim+1)).shrink(((0,dim*dim),)).reshape(dim, dim)
+
+  def full_like(self, fill_value, **kwargs):
+    return Tensor.full(self.shape, fill_value=fill_value, dtype=kwargs.pop("dtype", self.dtype), device=kwargs.pop("device", self.device), **kwargs)
+  def zeros_like(self, **kwargs): return self.full_like(0, **kwargs)
+  def ones_like(self, **kwargs): return self.full_like(1, **kwargs)
 
   # ***** rng hlops *****
 
   @staticmethod
-  def uniform(*shape, **kwargs) -> Tensor: return Tensor.rand(*shape, **kwargs) * 2 - 1
+  def randn(*shape, dtype:Optional[DType]=None, **kwargs) -> Tensor:
+    # https://en.wikipedia.org/wiki/Box%E2%80%93Muller_transform
+    src = Tensor.rand(2, *shape, **kwargs)
+    return src[0].mul(2*math.pi).cos().mul((1 - src[1]).log().mul(-2).sqrt()).cast(Tensor.default_type if dtype is None else dtype)
+
+  @staticmethod
+  def normal(*shape, mean=0.0, std=1.0, **kwargs) -> Tensor: return (std * Tensor.randn(*shape, **kwargs)) + mean
 
   @staticmethod
-  def scaled_uniform(*shape, **kwargs) -> Tensor: return Tensor.uniform(*shape, **kwargs).mul(prod(shape)**-0.5)
+  def uniform(*shape, low=0.0, high=1.0, **kwargs) -> Tensor:
+    dtype = kwargs.pop("dtype", Tensor.default_type)
+    return ((high-low) * Tensor.rand(*shape, **kwargs)).cast(dtype) + low
+
+  @staticmethod
+  def scaled_uniform(*shape, **kwargs) -> Tensor: return Tensor.uniform(*shape, low=-1.0, high=1.0, **kwargs).mul(prod(shape)**-0.5)
 
   # https://www.tensorflow.org/api_docs/python/tf/keras/initializers/GlorotUniform
   @staticmethod
-  def glorot_uniform(*shape, **kwargs) -> Tensor: return Tensor.uniform(*shape, **kwargs).mul((6/(shape[0]+prod(shape[1:])))**0.5)
+  def glorot_uniform(*shape, **kwargs) -> Tensor: return Tensor.uniform(*shape, low=-1.0, high=1.0, **kwargs).mul((6/(shape[0]+prod(shape[1:])))**0.5)
 
-  # ***** toposort and backward pass *****
+  # https://pytorch.org/docs/stable/_modules/torch/nn/init.html#kaiming_uniform_
+  @staticmethod
+  def kaiming_uniform(*shape, a:float = 0.01, **kwargs) -> Tensor:
+    bound = math.sqrt(3.0) * math.sqrt(2.0 / (1 + a ** 2)) / math.sqrt(prod(shape[1:]))
+    return Tensor.uniform(*shape, low=-bound, high=bound, **kwargs)
 
+  # https://pytorch.org/docs/stable/_modules/torch/nn/init.html#kaiming_normal_
+  @staticmethod
+  def kaiming_normal(*shape, a:float = 0.01, **kwargs) -> Tensor:
+    std = math.sqrt(2.0 / (1 + a ** 2)) / math.sqrt(prod(shape[1:]))
+    return Tensor.normal(*shape, mean=0.0, std=std, **kwargs)
+
+  # ***** toposort and backward pass *****
   def deepwalk(self):
     def _deepwalk(node, visited, nodes):
       visited.add(node)
-      if node._ctx:
+      if getattr(node, "_ctx", None):
         for i in node._ctx.parents:
           if i not in visited: _deepwalk(i, visited, nodes)
         nodes.append(node)
@@ -163,116 +228,223 @@ class Tensor:
     return _deepwalk(self, set(), [])
 
   def backward(self):
-    assert self.shape == (1,)
+    assert self.shape == tuple(), f"backward can only be called for scalar tensors, but it has shape {self.shape})"
 
-    # fill in the first grad with one
+    # fill in the first grad with one. don't use Tensor.ones because we don't need contiguous
     # this is "implicit gradient creation"
-    self.grad = Tensor.ones(*self.shape, device=self.device, requires_grad=False)
+    self.grad = Tensor(1, device=self.device, requires_grad=False)
 
     for t0 in reversed(self.deepwalk()):
-      if not any(x.requires_grad for x in t0._ctx.parents):
-        continue
       assert (t0.grad is not None)
       grads = t0._ctx.backward(t0.grad.lazydata)
       grads = [Tensor(g, device=self.device, requires_grad=False) if g is not None else None
         for g in ([grads] if len(t0._ctx.parents) == 1 else grads)]
       for t, g in zip(t0._ctx.parents, grads):
         if g is not None and t.requires_grad:
-          assert g.shape == t.shape, f"grad shape must match tensor shape in {self._ctx!r}, {g.shape!r} != {t.shape!r}"
+          assert g.shape == t.shape, f"grad shape must match tensor shape, {g.shape!r} != {t.shape!r}"
           t.grad = g if t.grad is None else (t.grad + g)
       del t0._ctx
 
   # ***** movement mlops *****
-
   def reshape(self, shape, *args) -> Tensor:
     new_shape = argfix(shape, *args)
-    assert len(new_shape) > 0 and all(x != 0 for x in new_shape), f"zeros not allowed in shape {new_shape}"
-    return mlops.Reshape.apply(self, shape=tuple(-prod(self.shape) // prod(new_shape) if s == -1 else s for s in new_shape))
-  def expand(self, shape, *args) -> Tensor: return mlops.Expand.apply(self, shape=tuple(x if x != -1 else s for s,x in zip(self.shape, argfix(shape, *args))))
+    assert 0 not in new_shape, f"zeros not allowed in shape {new_shape}"
+    return mlops.Reshape.apply(self, shape=tuple([-prod(self.shape) // prod(new_shape) if s == -1 else s for s in new_shape]))
+  def expand(self, shape, *args) -> Tensor: return mlops.Expand.apply(self, shape=tuple([x if x != -1 else s for s,x in zip(self.shape, argfix(shape, *args))]))
   def permute(self, order, *args) -> Tensor: return mlops.Permute.apply(self, order=argfix(order, *args))
-  def flip(self, axis, *args) -> Tensor: return mlops.Flip.apply(self, axis=argfix(axis, *args))
-  def pad(self, arg:Tuple[Tuple[int, int], ...]) -> Tensor: return mlops.Pad.apply(self, arg=arg) if any(x != (0,0) for x in arg) else self
-  def shrink(self, arg:Tuple[Tuple[int, int], ...]) -> Tensor: return mlops.Shrink.apply(self, arg=arg) if any(x != (0,s) for x,s in zip(arg, self.shape)) else self
+  def flip(self, axis, *args) -> Tensor: return mlops.Flip.apply(self, axis=[x if x >= 0 else x+len(self.shape) for x in argfix(axis, *args)])
+  def shrink(self, arg:Tuple[Tuple[sint, sint], ...]) -> Tensor: return mlops.Shrink.apply(self, arg=arg) if any(x != (0,s) for x,s in zip(arg, self.shape)) else self
+  def pad(self, arg: Tuple[Tuple[int, int], ...], value:float=0) -> Tensor:
+    ret = mlops.Pad.apply(self, arg=arg) if any(x != (0, 0) for x in arg) else self
+    return ret if 0 == value else ret + mlops.Pad.apply(Tensor.ones_like(self), arg=arg).where(0, value)
 
   # ***** movement hlops *****
 
-  # NOTE: using slice is discouraged and things should migrate to pad and shrink
-  def slice(self, arg:Sequence[Optional[Tuple[int, int]]]) -> Tensor:
-    arg_ = tuple(a if a is not None else (0,s) for s,a in zip(self.shape, arg))
-    padding = tuple((max(0, -p[0]), max(0, p[1]-self.shape[i])) for i,p in enumerate(arg_))
-    return self.pad(padding).shrink(tuple((p[0] + padding[i][0], p[1] + padding[i][0]) for i,p in enumerate(arg_)))
-
-  # Tensors mostly follow the normal python indexing / slicing behavior for sequences
   # - Negative indices are taken relative to the end of the sequence, so X[-2] returns the 2nd-to-last element
   # - A slice i:j returns the elements with indices in [i, j)
-  #   - If omitted, i and j will default to 0 and N, respectively, where N is the length of the sequence
-  #   - Negative values for i and j are taken relative to the end of the sequence
-  #   - Both i and j will be clamped to the range (-N, N], where N in the length of the sequence
-  # - Indexing with np.newaxis or None on a given axis will add a new dimension of size one before that axis
-  # - Empty slices are not allowed
-  # - Strides other than 1 are not allowedå
-  def __getitem__(self, val):
-    def slcfix(i, sz, default): return default if i is None else max(0, min(sz, sz+i if i < 0 else i))  # Fix negative idxs, clamp to [0,N]
-    new_slice, new_shape = [], []
-    val = [val] if not isinstance(val, (list, tuple)) else val
-    assert sum(s is not None for s in val) <= len(self.shape)
-    assert all(s.step is None or s.step == 1 for s in val if isinstance(s, slice))
-    for i,(sz,s) in enumerate(zip(self.shape, [v for v in val if v is not None])):  # Slicing only depends on ints + slices
-      if isinstance(s, int) and not (-sz <= s < sz):
-        raise IndexError(f"index {s} is out of bounds for dimension {i} with size {sz}")
-      new_slice.append((s%sz, s%sz+1) if isinstance(s, int) else (slcfix(s.start, sz, 0), slcfix(s.stop, sz, sz)))
-    for s,sz in zip(val, [self.shape[i-1] for i in itertools.accumulate([s is not None for s in val])]):  # Shape depends on slices + positions of Nones
-      if not isinstance(s, int):
-        new_shape.append(1 if s is None else slcfix(s.stop, sz, sz) - slcfix(s.start, sz, 0))
-    new_shape += [self.shape[i] for i in range(len(new_slice), len(self.shape))]
-    new_slice += [(0,self.shape[i]) for i in range(len(new_slice), len(self.shape))]
-    return self.slice(new_slice).reshape(new_shape if len(new_shape) else (1,))
+  #    - If omitted, i and j will default to 0 and N, respectively, where N is the length of the sequence
+  #    - Negative values for i and j are taken relative to the end of the sequence
+  #    - Both i and j will be clamped to the range (-N, N], where N in the length of the sequence
+  # - Indexing with None on a given axis will add a new dimension of size one before that axis
+  # - Empty slices are not allowed (tensors with 0s in shape have to be supported first, for all backends).
+  # - For a slice [i:j:k] finding the correct indices is delegated to slice.indices(len).
+  # - Strides > 1 and < 0 are now allowed!:
+  #    - This works by applying Shrink -> [[Flip -> ] Pad -> Reshape -> Shrink] -> Reshape (ops in brackets are optional)
+  #    - Idea of stride < 0 support:
+  #        - Do the slice first, flip the axes were slice.step is negative, do slice.step -> -slice.step. Go to steps below.
+  #    - Idea of stride `s` > 1 support (Pad -> Reshape -> Shrink):
+  #        - Instead of doing [::s] on axis [dim_sz], do [:, 0] on axes [dim_sz_padded // s, s].
+  #        - So pad dim_sz with as many zeros as needed (dim_sz -> dim_sz_padded) so that reshape to [dim_sz_padded // s, s]
+  #          is possible.
+  #        - Apply Shrink to do the slice [:, 0] on axes of shapes [dim_sz_padded // s, s].
+  # - Fancy indexing and combined indexing is supported
+  #    - Combined indexing works by letting regular slicing finish first -> computing the resulting dims w.r.t to Tensors passed in -> fancy indexing
+  #    - Any Tensors passed in __getitem__ will perform (CMPEQ with arange -> MUL with self -> SUM_REDUCE) iteratively
+  #        - The first iteration will expand the dim of self while consecutive iterations will reduce the dim
+  #    - There's a special case where a permute is needed at the end:
+  #        - if first Tensor passed in (expand dims) is not at dim 0
+  #        - and following Tensors does not follow consecutively to the end of fancy indexing's dims
+  def __getitem__(self, val): # val: Union[int, slice, Tensor, None, Ellipsis, Tuple[Union[int, slice, Tensor, None, Ellipsis], ...]]
+    def normalize_int(e, i, dim_sz):
+      if -dim_sz <= e < dim_sz: return e if e != -1 else dim_sz-1
+      raise IndexError(f"index {e} is out of bounds for dimension {i} with size {self.shape[i]}")
+
+    orig_slices = list(val) if isinstance(val, tuple) else [val]
+    count = defaultdict(list)
+    for i,v in enumerate(orig_slices): count[type(v)].append(i)
+
+    if (num_slices := len(count[int]) + len(count[slice]) + len(count[Tensor])) > len(self.shape): raise IndexError(f"too many indices for tensor of dimension {len(self.shape)}")
+    if len(ellipsis_found := count[type(Ellipsis)]) > 1: raise IndexError("an index can only have a single ellipsis ('...')")
+
+    ellipsis_idx = ellipsis_found[0] if ellipsis_found else len(orig_slices)
+    orig_slices[ellipsis_idx:ellipsis_idx+1] = [slice(None)] * (len(self.shape) - num_slices)
+
+    valid_slices = [v for v in orig_slices if v is not None]
+    valid_slices = [v if isinstance(v, slice) else slice(y_ := normalize_int(v, i, dim_sz), y_+1) if isinstance(v, int) else slice(None) for i, (v, dim_sz) in enumerate(zip(valid_slices, self.shape))]
+
+    start, stop, strides = zip(*y) if (y := [s.indices(dim_sz) for s, dim_sz in zip(valid_slices, self.shape)]) else ((), (), ())
+    new_slice = tuple((s, e) if st > 0 else (e+1, s+1) for s, e, st in zip(start, stop, strides))
+    sliced_tensor = self.shrink(new_slice).flip(axis=[i for i, s in enumerate(strides) if s < 0])
+    new_shape = sliced_tensor.shape
+    if any(abs(s) != 1 for s in strides):
+      strides = tuple(abs(s) for s in strides)
+      # Pad: add pad at the end: [dim_sz] -> [dim_sz_padded]
+      padded_tensor = sliced_tensor.pad(tuple((0, s-(dim_sz % s) if dim_sz % s != 0 else 0) for s, dim_sz in zip(strides, sliced_tensor.shape)))
+      # Reshape: [dim_sz_padded] -> [dim_sz_padded // s, s]
+      reshaped_tensor = padded_tensor.reshape(flatten([sh // s, s] for sh, s in zip(padded_tensor.shape, strides)))
+      new_shape = reshaped_tensor.shape[::2]
+      # Shrink: do [:, 0]
+      sliced_tensor = reshaped_tensor.shrink(tuple(flatten(((0, sh), (0, 1)) for sh in new_shape)))
+
+    final_shape, it_shape, dim, tensors, dim_collapsed = [], iter(new_shape), [], [], 0
+    for i,s in enumerate(orig_slices):
+      if s is None: final_shape.append(1)
+      else: # s is int or slice or Tensor
+        dim_shape = next(it_shape)
+        if isinstance(s, int):
+          dim_collapsed += 1
+        else:
+          assert isinstance(dim_shape, int), f"does not support symbolic shape {dim_shape}"
+          final_shape.append(dim_shape)
+          if isinstance(s, Tensor):
+            tensors.append(s)
+            dim.append(i-dim_collapsed)
+    ret = sliced_tensor.reshape(tuple(final_shape))
+
+    if tensors: # Fancy/tensor indexing
+      # normalize idx
+      # TODO: first contiguous fixes torch+cpu_only CI, but it causes llvm to fail. Second one fixes llvm
+      idx = [t.sign().contiguous().__neg__().contiguous().relu() * ret.shape[d] + t for d,t in zip(dim, tensors)]
+      max_dim = max(i.ndim for i in idx)
+      # compute sum_dim, arange, and idx
+      sum_dim = [d if n==0 else d+max_dim-n for n,d in enumerate(dim)]
+      arange = [Tensor.arange(ret.shape[d], dtype=dtypes.int32, requires_grad=False, device=self.device).reshape(*[1]*sd, ret.shape[d], *[1]*(ret.ndim + max_dim - n - sd - 1)) for n,(sd,d) in enumerate(zip(sum_dim, dim))]
+      first_idx = [idx[0].reshape(*[1]*dim[0], *[1]*(1 + max_dim - idx[0].ndim), *idx[0].shape, *[1]*(ret.ndim - dim[0] - 1))]
+      rest_idx = [i.reshape(*[1]*dim[0], *[1]*(max_dim - i.ndim), *i.shape, *[1]*(ret.ndim - dim[0] - n)) for n,i in enumerate(idx[1:], 1)]
+      idx = first_idx + rest_idx
+      ret = ret.reshape(*ret.shape[:sum_dim[0]+1], *[1]*max_dim, *ret.shape[sum_dim[0]+1:])
+      # iteratively fancy index
+      for a,i,sd in zip(arange, idx, sum_dim): ret = (a==i).mul(ret).sum(sd)
+      # special permute case
+      if dim[0] != 0 and len(dim) != 1 and dim != list(range(dim[0], dim[-1]+1)):
+        ret_dims = list(range(ret.ndim))
+        ret = ret.permute(ret_dims[dim[0]:dim[0]+max_dim] + ret_dims[:dim[0]] + ret_dims[dim[0]+max_dim:])
+    return ret
+
+  def __setitem__(self,s,v): return self.__getitem__(s).assign(v)
+
+  # NOTE: using slice is discouraged and things should migrate to pad and shrink
+  def slice(self, arg:Sequence[Optional[Tuple[int, sint]]], value:float=0) -> Tensor:
+    arg_ = tuple([a if a is not None else (0,s) for s,a in zip(self.shape, arg)])
+    padding = tuple([(max(0, -p[0]), max(0, p[1]-self.shape[i])) for i,p in enumerate(arg_)])
+    return self.pad(padding, value=value).shrink(tuple([(p[0] + padding[i][0], p[1] + padding[i][0]) for i,p in enumerate(arg_)]))
+
+  def gather(self: Tensor, idx: Tensor, dim: int):
+    assert idx.ndim == self.ndim, "self.ndim must equal idx.ndim"
+    assert all(s >= i for s,i in zip(self.shape, idx.shape)), "all dim of idx.shape must be smaller than self.shape"
+    if dim < 0: dim += self.ndim
+    idx = idx.transpose(ax1=dim, ax2=0).unsqueeze(-1)
+    permarg = list(range(self.ndim))
+    permarg = permarg[1:dim] + [permarg[0]] + permarg[dim+1:] + [permarg[dim]] if dim != 0 else permarg[1:] + [permarg[0]]
+    return ((idx == Tensor.arange(self.shape[dim], dtype=dtypes.int32, requires_grad=False, device=self.device)) * self.permute(*permarg).shrink(tuple([*[(0,sh) for sh in idx.shape[1:-1]], (0,self.shape[dim])])).unsqueeze(0)).sum(-1).transpose(ax1=0, ax2=dim)
 
   def cat(self, *args, dim=0):
     dim = (dim + len(self.shape)) if dim < 0 else dim
-    for y in args:
-      assert len(y.shape) == len(self.shape) and all(y.shape[i] == s for i,s in enumerate(self.shape) if i != dim)
-    catargs = [self] + list(args)
-    shape_cumsum = [0, *itertools.accumulate([y.shape[dim] for y in catargs])]
-    slc = [[(0, s) for s in self.shape] for _ in catargs]
-    for s,k in zip(slc, shape_cumsum):
-      s[dim] = (-k, shape_cumsum[-1]-k)
-    return functools.reduce(Tensor.__add__, [arg.slice(s) for arg,s in zip(catargs, slc)])
-
-  # TODO: make this nicer with syntactic sugar in slice
-  def chunk(self, num, dim):
-    slice_params = [[(0, s) for s in self.shape] for _ in range(num)]
-    for i,k in enumerate(range(0, self.shape[dim], self.shape[dim]//num)):
-      slice_params[i][dim] = (k, min(self.shape[dim], k+self.shape[dim]//num))
-    return [self.slice(p) for p in slice_params]
+    assert all(len(y.shape) == len(self.shape) and all(y.shape[i] == s for i,s in enumerate(self.shape) if i != dim) for y in args)
+    catargs = [self, *args]
+    assert all(t.shape for t in catargs), "zero-dimensional tensor cannot be concatenated"
+    shapes = [s.shape[dim] for s in catargs]
+    shape_cumsum = [0, *accumulate(shapes)]
+    slc = [[(0, 0) for _ in self.shape] for _ in catargs]
+    for shp,k,s in zip(shapes, shape_cumsum[:-1], slc):
+      s[dim] = (k, shape_cumsum[-1] - k - shp)
+    return reduce(Tensor.__add__, [arg.pad(tuple(s)) for arg,s in zip(catargs, slc)])
+
+  @staticmethod
+  def stack(tensors, dim=0):
+    first = tensors[0].unsqueeze(dim)
+    unsqueezed_tensors = [tensor.unsqueeze(dim) for tensor in tensors[1:]]
+    # checks for shapes and number of dimensions delegated to cat
+    return first.cat(*unsqueezed_tensors, dim=dim)
+
+  def repeat(self, repeats):
+    base_shape = (1,) * (len(repeats) - self.ndim) + self.shape
+    new_shape = [x for b in base_shape for x in [1, b]]
+    expand_shape = [x for rs in zip(repeats, base_shape) for x in rs]
+    final_shape = [r*s for r,s in zip(repeats, base_shape)]
+    return self.reshape(new_shape).expand(expand_shape).reshape(final_shape)
+
+  def chunk(self, num:int, dim:int) -> List[Tensor]:
+    assert all_int(self.shape), f"does not support symbolic shape {self.shape}"
+    dim, step = dim + self.ndim if dim < 0 else dim, math.ceil(self.shape[dim]/num)
+    slice_params = [[slice(None)]*dim + [slice(k, k + step)] for k in range(0, self.shape[dim], step)]
+    return [self[tuple(sl)] for sl in slice_params]
+
+  def squeeze(self, dim=None):
+    if dim is None: return self if 1 not in self.shape else self.reshape(*[size for size in self.shape if size != 1])
+    if dim <= 0 and self.ndim == 0: return self # This is to match PyTorch behavior
+    if not -self.ndim <= dim < self.ndim: raise IndexError(f"Dimension out of range (expected to be in range of [{-self.ndim if self.ndim > 0 else self.ndim-1}, {self.ndim-1 if self.ndim > 0 else self.ndim}], but got {dim})")
+    if dim < 0: dim += self.ndim
+    return self if self.shape[dim] != 1 else self.reshape(*[size for idx, size in enumerate(self.shape) if idx != dim])
 
   def unsqueeze(self, dim):
     if dim < 0: dim = len(self.shape) + dim + 1
     return self.reshape(self.shape[:dim] + (1,) + self.shape[dim:])
 
   # (padding_left, padding_right, padding_top, padding_bottom)
-  def pad2d(self, padding:Tuple[int, ...]): return self.slice(((0,self.shape[0]), (0,self.shape[1]), (-padding[2],self.shape[2]+padding[3]), (-padding[0],self.shape[3]+padding[1])))
-  # TODO: this is totally not transpose
-  def transpose(self, order=(1,0)) -> Tensor: return self.permute(order=order)
-  def flatten(self, start_dim=0): return self.reshape(shape=tuple(list(self.shape[0:start_dim]) + [-1]))
+  def pad2d(self, padding:Union[List[int], Tuple[int, ...]], value:float=0):
+    slc = [(-p0, s+p1) for p0,p1,s in zip(padding[::2], padding[1::2], self.shape[::-1])][::-1]
+    return self.slice([(0,s) for s in self.shape[:-(len(padding)//2)]] + slc, value=value)
+
+  @property
+  def T(self) -> Tensor: return self.transpose()
+  def transpose(self, ax1=1, ax2=0) -> Tensor:
+    order = list(range(len(self.shape)))
+    order[ax1], order[ax2] = order[ax2], order[ax1]
+    return self.permute(order)
+  def flatten(self, start_dim=0): return self.reshape(shape=self.shape[:start_dim] + (-1,))
 
   # ***** reduce ops *****
 
-  def _reduce(self, fxn:Type[Function], axis:Optional[Union[int, Tuple[int, ...]]]=None, keepdim=False):
-    axis_ : List[int] = list(range(len(self.shape))) if axis is None else ([axis] if isinstance(axis, int) else list(axis))
+  def _reduce(self, fxn:Type[Function], axis:Optional[Union[int, Tuple[int, ...]]]=None, keepdim=False) -> Tensor:
+    axis_: List[int] = list(range(len(self.shape))) if axis is None else ([axis] if axis.__class__ is int else list(axis)) # type: ignore
     axis_ = [x if x >= 0 else x+len(self.shape) for x in axis_]
-    shape = [self.shape[i] for i in range(len(self.shape)) if i not in axis_]
-    ret = fxn.apply(self, new_shape=tuple(1 if i in axis_ else self.shape[i] for i in range(len(self.shape))))
-    return ret if keepdim else ret.reshape(shape=[1] if shape == [] else shape)
+    shape = [s for i,s in enumerate(self.shape) if i not in axis_]
+    ret = fxn.apply(self, new_shape=tuple([1 if i in axis_ else s for i,s in enumerate(self.shape)]))
+    return ret if keepdim else ret.reshape(shape=shape)
 
   def sum(self, axis=None, keepdim=False): return self._reduce(mlops.Sum, axis, keepdim)
   def max(self, axis=None, keepdim=False): return self._reduce(mlops.Max, axis, keepdim)
   def min(self, axis=None, keepdim=False): return -((-self).max(axis=axis, keepdim=keepdim))
 
   def mean(self, axis=None, keepdim=False):
+    assert all_int(self.shape), "does not support symbolic shape"
     out = self.sum(axis=axis, keepdim=keepdim)
-    return out * (prod(out.shape)/prod(self.shape))
-
+    return out.mul(prod(out.shape)/prod(self.shape))
+  def std(self, axis=None, keepdim=False, correction=1):
+    assert all_int(self.shape), "does not support symbolic shape"
+    square_sum = ((self - self.mean(axis=axis, keepdim=True)).square()).sum(axis=axis, keepdim=keepdim)
+    return square_sum.div(prod(self.shape)/prod(square_sum.shape)-correction).sqrt()
   def _softmax(self, axis):
     m = self - self.max(axis=axis, keepdim=True)
     e = m.exp()
@@ -286,10 +458,21 @@ class Tensor:
     m, _, ss = self._softmax(axis)
     return m - ss.log()
 
+  def argmax(self, axis=None, keepdim=False):
+    if axis is None:
+      idx = (self == self.max(axis)) * Tensor.arange(prod(self.shape)-1,-1,-1, dtype=dtypes.int32, requires_grad=False, device=self.device).reshape(self.shape)
+      return prod(self.shape) - idx.max() - 1
+    axis = axis + len(self.shape) if axis < 0 else axis
+    m = self == self.max(axis=axis, keepdim=True)
+    idx = m * Tensor.arange(self.shape[axis]-1,-1,-1, dtype=dtypes.int32, requires_grad=False, device=self.device).reshape(self.shape[axis], *[1]*(self.ndim-axis-1))
+    return self.shape[axis]-idx.max(axis=axis, keepdim=keepdim)-1
+  def argmin(self, axis=None, keepdim=False): return (-self).argmax(axis=axis, keepdim=keepdim)
+
   # ***** processing ops *****
 
   def _pool(self, k_:Tuple[int, ...], stride:Union[Tuple[int, ...], int]=1, dilation:Union[Tuple[int, ...], int]=1) -> Tensor:
     assert len(self.shape) >= len(k_), f"can't pool {self.shape} with {k_}"
+    assert all_int(self.shape), f"does not support symbolic shape {self.shape}"
     s_, d_ = make_pair(stride, len(k_)), make_pair(dilation, len(k_))
     assert len(k_) == len(s_) and len(k_) == len(d_), f"stride/dilation mismatch kernel:{k_} stride:{s_} dilation:{d_}"
     slc_prefix, prefix, i_ = [(0,x) for x in self.shape[0:-len(k_)]], self.shape[0:-len(k_)], self.shape[-len(k_):]
@@ -306,103 +489,203 @@ class Tensor:
       xup = xup.slice(slc_prefix + flatten(((0,k), (0,o), (0,1)) for k,o in zip(k_, o_)))
       xup = xup.reshape(*prefix, *flatten((k,o) for k,o in zip(k_, o_)))
       return xup.permute(*range(len(prefix)), *[len(prefix)+i*2+1 for i in range(len(k_))], *[len(prefix)+i*2 for i in range(len(k_))])
-    else:
-      # TODO: once the shapetracker can optimize well, remove this alternative implementation. or not if the CPU implementation doesn't use ShapeTracker
-      o_ = [(i+(s-k))//s for i,s,k in zip(i_, s_, k_)]
-      xup = self.slice(slc_prefix + [(0,o*s) for o,s in zip(o_, s_)])
-      xup = xup.reshape(*prefix, *flatten(((o, s) for o,s in zip(o_, s_))))
-      xup = xup.slice(slc_prefix + flatten(((0,o), (0,k)) for o,k in zip(o_, k_)))
-      return xup.permute(*range(len(prefix)), *[len(prefix)+i*2 for i in range(len(k_))], *[len(prefix)+i*2+1 for i in range(len(k_))])
+    # TODO: once the shapetracker can optimize well, remove this alternative implementation. or not if the CPU implementation doesn't use ShapeTracker
+    o_ = [(i+(s-k))//s for i,s,k in zip(i_, s_, k_)]
+    xup = self.slice(slc_prefix + [(0,o*s) for o,s in zip(o_, s_)])
+    xup = xup.reshape(*prefix, *flatten(((o, s) for o,s in zip(o_, s_))))
+    xup = xup.slice(slc_prefix + flatten(((0,o), (0,k)) for o,k in zip(o_, k_)))
+    return xup.permute(*range(len(prefix)), *[len(prefix)+i*2 for i in range(len(k_))], *[len(prefix)+i*2+1 for i in range(len(k_))])
 
   # NOTE: these work for more than 2D
   def avg_pool2d(self, kernel_size=(2,2), stride=None): return self._pool(make_pair(kernel_size), stride if stride is not None else kernel_size).mean(axis=tuple(range(0-len(make_pair(kernel_size)), 0)))
-  def max_pool2d(self, kernel_size=(2,2), stride=None): return self._pool(make_pair(kernel_size), stride if stride is not None else kernel_size).max(axis=tuple(range(0-len(make_pair(kernel_size)), 0)))
-
-  @image_conv2d_decorator
+  def max_pool2d(self, kernel_size=(2,2), stride=None, dilation=1): return self._pool(make_pair(kernel_size), stride if stride is not None else kernel_size, dilation).max(axis=tuple(range(0-len(make_pair(kernel_size)), 0)))
+
+  def conv_transpose2d(self, weight:Tensor, bias:Optional[Tensor]=None, groups=1, stride=1, dilation=1, padding=0, output_padding=0) -> Tensor:
+    HW, trailing = weight.shape[2:], list(range(3, len(weight.shape)+1))
+    x, w = self, weight.reshape(groups, weight.shape[0]//groups, weight.shape[1], *weight.shape[2:]).permute(0,2,1,*trailing).flip(trailing)
+    stride = make_pair(stride, len(HW))
+    if any(s>1 for s in stride):
+      x = x.reshape(*x.shape[:2], *flatten((k,1) for k in x.shape[2:]))
+      x = x.pad(((0,0), (0,0), *flatten(((0,0),(0,s-1)) for s in stride)))
+      x = x.reshape(*x.shape[:2], *[k*s for k,s in zip(x.shape[2::2], stride)])
+      x = x.shrink(((0,x.shape[0]), (0,x.shape[1]), *[(0,k-(s-1)) for k,s in zip(x.shape[2:], stride)]))
+    padding = flatten((((k-1)*d-p,(k-1)*d-p+op) for k,d,p,op in reversed(list(zip(HW, make_pair(dilation, len(HW)), make_pair(padding, len(HW)), make_pair(output_padding, len(HW)))))))
+    return x.conv2d(w.reshape(w.shape[0]*w.shape[1],*w.shape[2:]), groups=groups, bias=bias, dilation=dilation, padding=padding)
+
+  wino = int(getenv("WINO", "0"))
   def conv2d(self, weight:Tensor, bias:Optional[Tensor]=None, groups=1, stride=1, dilation=1, padding=0) -> Tensor:
-    (bs,cin_,_,_), (cout,cin,H,W) = self.shape, weight.shape
-    assert cin*groups == cin_, f"Input Tensor shape {self.shape} does not match the shape of the weights {weight.shape}. ({cin*groups} vs. {cin_})"
-    padding_ = [padding]*4 if isinstance(padding, int) else (padding if len(padding) == 4 else [padding[1], padding[1], padding[0], padding[0]])
+    (bs,cin_), (cout,cin), HW = self.shape[:2], weight.shape[:2], weight.shape[2:]
+    assert groups*cin == cin_ and len(self.shape) == len(weight.shape), f"Input Tensor shape {self.shape} does not match the shape of the weights {weight.shape}. ({groups*cin} vs. {cin_})"
+    if isinstance(padding, (tuple,list)): assert len(padding) == 2*len(HW) or len(padding) == len(HW), f"Expected padding of length {2*len(HW)} or {len(HW)}, but got {len(padding)} for tensor of shape {self.shape}"
+    padding_ = [padding]*2*len(HW) if isinstance(padding, int) else (padding if len(padding) == 2*len(HW) else [p for p in padding for _ in range(2)][::-1])
 
     # conv2d is a pooling op (with padding)
-    x = self.pad2d(padding_)._pool((H,W),stride, dilation)
+    x = self.pad2d(padding_)._pool(HW, stride, dilation)   # (bs, groups*cin, oy, ox, H, W)
+    rcout, oyx = cout//groups, x.shape[2:-len(HW)]
+    if not all(x == 3 for x in HW) or stride != 1 or dilation != 1 or not Tensor.wino:
+      # normal conv
+      x = x.reshape(bs, groups, cin, 1, *oyx, *HW).expand(bs, groups, cin, rcout, *oyx, *HW).permute(0,1,3,*[4+i for i in range(len(oyx))],2,*[4+len(oyx)+i for i in range(len(HW))])
 
-    oy, ox, rcout = x.shape[2], x.shape[3], cout//groups
-    # NOTE: we do this expand explicitly so the permute isn't pushed in the binop
-    x = x.reshape(bs, groups, 1, cin, oy, ox, H, W).expand(bs, groups, rcout, cin, oy, ox, H, W).permute(0,1,2,4,5,3,6,7)
+      # conv! broadcasted to (bs, groups, rcout, *oyx, cin, *HW)
+      ret = (x * weight.reshape(1, groups, rcout, *[1] * len(oyx), cin, *HW)).sum([-1-i for i in range(1+len(oyx))], keepdim=True).reshape(bs, cout, *oyx)
+      return ret if bias is None else ret.add(bias.reshape(1, -1, *[1] * len(HW)))
 
-    # conv! broadcasted to (bs, groups, rcout, oy, ox, cin, H, W)
-    ret = (x * weight.reshape(1, groups, rcout, 1, 1, cin, H, W)).sum((-3, -2, -1)).reshape(bs, cout, oy, ox)
-    return ret if bias is None else ret.add(bias.reshape(1, -1, 1, 1))
+    # winograd conv 3 kernel f(4x4,3x3) see: http://arxiv.org/abs/1509.09308
+    def apply_matrix(mat, t, dim=0): return t if dim == len(HW) else Tensor.stack([apply_matrix(mat, sum(mm*t[j] for j,mm in enumerate(m) if mm), dim=dim+1) for m in mat])
+    HWI, HWO = (6,) * len(HW), (4,) * len(HW)  # F(4x4,3x3) winograd tiles
+    winograd_Bt = [[4, 0, -5, 0, 1, 0], [0, -4, -4, 1, 1, 0], [0, 4, -4, -1, 1, 0], [0, -2, -1, 2, 1, 0], [0, 2, -1, -2, 1, 0], [0, 4, 0, -5, 0, 1]]
+    winograd_G = [[1/4, 0, 0], [-1/6, -1/6, -1/6], [-1/6, 1/6, -1/6], [1/24, 1/12, 1/6], [1/24, -1/12, 1/6], [0, 0, 1]]
+    winograd_At = [[1, 1, 1, 1, 1, 0], [0, 1, -1, 2, -2, 0], [0, 1, 1, 4, 4, 0], [0, 1, -1, 8, -8, 1]]  # applying At in pre-order almost doubles compilation time
+
+    # todo: stride == dilation
+    # use padding to round up to 4x4 output tiles
+    d = self.pad2d(sum([[padding_[i*2], padding_[i*2+1] + (-(dim + sum(padding_[i * 2:(i + 1) * 2]) - 2) % 4)] for i, dim in enumerate(self.shape[-len(HW):])], []))._pool(HWI, HWO)  # (bs, cin_, tyx, HWI)
+    d = d.permute(*range(len(d.shape)-len(HW),len(d.shape)), *range(len(d.shape)-len(HW))).contiguous_backward()  # move HW to the front: # (HWI, bs, cin_, tyx)
+    tyx = d.shape[-len(HWI):]  # dim of tiling
+
+    g = weight.permute(*range(len(weight.shape)-len(HW),len(weight.shape)), *range(len(weight.shape)-len(HW)))  # move HW to the front
+
+    # compute 6x6 winograd tiles: GgGt, BtdB
+    gfactors = apply_matrix(winograd_G, g).contiguous().reshape(*HWI, 1, groups, rcout, cin, *([1]*len(tyx)))  # (HWI, groups * rcout, cin) -> (HWI, bs=1, groups, rcout, cin, tyx=(1,1))
+    dfactors = apply_matrix(winograd_Bt, d).contiguous().reshape(*HWI, bs, groups, 1, cin, *tyx)  # (HWI, bs, cin_, tyx) -> (HWI, bs, groups, 1 ,cin, *tyx)
+
+    ret = apply_matrix(winograd_At, (gfactors * dfactors).sum(axis=-1-len(HW)))  # matmul; sum across cin: (HWI, bs, groups, rcout, *tyx); then HWI -> HWO: (HWO, bs, groups, rcout, *tyx)
+
+    ret = ret.permute([*range(len(HW), len(ret.shape)-len(HW)), *[i+o for i in range(len(HW)) for o in [len(ret.shape)-len(HW),0]]])  # interleave tyx and HWO: (bs, groups, rcout, oy, HO, ox, WO)
+    ret = ret.reshape(bs, cout, *[c * HWO[i] for i, c in enumerate(tyx)]).shrink(tuple((0, s) for s in [bs, cout, *oyx]))  # merge groups and rcout, tyx and HWO: (bs, groups, cout, *yx), shrink to final
+
+    return (ret if bias is None else ret.add(bias.reshape(1, -1, *[1 for _ in range(len(HW))]))).contiguous().contiguous_backward()
 
   def dot(self, w:Tensor) -> Tensor:
-    # NOTE: we use a 1x1 conv2d to do the matmul. mxk @ kxn = (1,k,m,1).conv2d(n,k,1,1)
-    bs, groups = prod(self.shape[0:-2]), prod(w.shape[0:-2])
-    cin, cout = w.shape[-2], w.shape[-1]
-    out_shape_t = self.shape[0:-2] + (cout,-1)
-    if len(self.shape) > 1:
-      order = tuple(range(len(self.shape)-2)) + (len(self.shape)-1, len(self.shape)-2)
-    else:
-      order, out_shape_t = (0,), (cout, )
-    worder = tuple(range(len(w.shape)-2)) + (len(w.shape)-1, len(w.shape)-2)
-
-    # NOTE: with NHWC we can remove the transposes
-    # bs x groups*cin x H x W
-    cx = self.transpose(order=order).reshape(shape=(bs//groups, groups*cin, -1, 1))
-    # groups*cout x cin x H, W
-    cw = w.transpose(order=worder).reshape(shape=(groups*cout, cin, 1, 1))
-    return cx.conv2d(cw, groups=groups).reshape(shape=out_shape_t).transpose(order=order)
+    n1, n2 = len(self.shape), len(w.shape)
+    assert n1 != 0 and n2 != 0, f"both arguments to matmul need to be at least 1D, but they are {n1}D and {n2}D"
+    assert self.shape[-1] == w.shape[-min(n2, 2)], f"Input Tensor shapes {self.shape} and {w.shape} cannot be multiplied ({self.shape[-1]} != {w.shape[-min(n2, 2)]})"
+    x = self.reshape(*self.shape[0:-1], *[1]*min(n1-1, n2-1, 1), self.shape[-1])
+    w = w.reshape(*w.shape[0:-2], *[1]*min(n1-1, n2-1, 1), *w.shape[-min(n2, 2):]).transpose(-1, -min(n2, 2))
+    return (x*w).sum(-1)
+
+  def cumsum(self, axis:int=0) -> Tensor: return self.transpose(axis,-1).pad2d((self.shape[axis]-1,0))._pool((self.shape[axis],)).sum(-1).transpose(axis,-1)
 
   # ***** mlops (unary) *****
 
+  def __neg__(self): return mlops.Neg.apply(self)
   def contiguous(self): return mlops.Contiguous.apply(self)
+  def contiguous_backward(self): return mlops.ContiguousBackward.apply(self)
   def log(self): return mlops.Log.apply(self)
+  def log2(self): return mlops.Log.apply(self)/math.log(2)
   def exp(self): return mlops.Exp.apply(self)
+  def exp2(self): return mlops.Exp.apply(self*math.log(2))
+  def relu(self): return mlops.Relu.apply(self)
+  def sigmoid(self): return mlops.Sigmoid.apply(self)
+  def sin(self): return mlops.Sin.apply(self)
+  def sqrt(self): return mlops.Sqrt.apply(self)
+  def rsqrt(self): return (1/self).sqrt()
+  def cos(self): return ((math.pi/2)-self).sin()
+  def tan(self): return self.sin() / self.cos()
+
+  @staticmethod
+  def _tri(r:int, c:int, k:int=0, **kwargs) -> Tensor: return Tensor.arange(r, **kwargs).unsqueeze(1).expand(r,c) <= Tensor.arange(-k, c-k, **kwargs).unsqueeze(0).expand(r,c)
+  def triu(self, k:int=0) -> Tensor:
+    assert all_int(self.shape), f"does not support symbolic shape {self.shape}"
+    return Tensor._tri(self.shape[-2], self.shape[-1], k=k, dtype=self.dtype, device=self.device).where(self, Tensor.zeros_like(self))
+  def tril(self, k:int=0) -> Tensor:
+    assert all_int(self.shape), f"does not support symbolic shape {self.shape}"
+    return Tensor._tri(self.shape[-2], self.shape[-1], k=k+1, dtype=self.dtype, device=self.device).where(Tensor.zeros_like(self), self)
 
   # ***** math functions (unary) *****
+  def trunc(self: Tensor) -> Tensor: return self.cast(dtypes.int32).contiguous().cast(self.dtype)
+  def ceil(self: Tensor) -> Tensor: return (self > (b := self.trunc())).where(b+1, b)
+  def floor(self: Tensor) -> Tensor: return (self < (b := self.trunc())).where(b-1, b)
 
-  def __neg__(self): return 0.0-self
-  def sqrt(self): return self.pow(0.5)
   def square(self): return self*self
-  def clip(self, min_, max_): return ((self-min_).relu()+min_) - (self-max_).relu()
+  def clip(self, min_, max_): return self.maximum(min_).minimum(max_)
   def abs(self): return self.relu() + (-self).relu()
   def sign(self): return self / (self.abs() + 1e-10)
-  def relu(self): return self.maximum(0)
   def reciprocal(self): return 1.0/self
 
   # ***** activation functions (unary) *****
-
-  def sigmoid(self): return (1.0 + (-self).exp()).reciprocal()
   def elu(self, alpha=1.0): return self.relu() - alpha*(1-self.exp()).relu()
+  def celu(self, alpha=1.0): return self.maximum(0) + (alpha * ((self / alpha).exp() - 1)).minimum(0)
   def swish(self): return self * self.sigmoid()
   def silu(self): return self.swish()   # The SiLU function is also known as the swish function.
   def relu6(self): return self.relu() - (self-6).relu()
   def hardswish(self): return self * (self+3).relu6() * (1/6)
   def tanh(self): return 2.0 * ((2.0 * self).sigmoid()) - 1.0
+  def hardtanh(self, min_val=-1, max_val=1): return self.clip(min_val, max_val)
   def gelu(self): return 0.5 * self * (1 + (self * 0.7978845608 * (1 + 0.044715 * self * self)).tanh())
   def quick_gelu(self): return self * (self * 1.702).sigmoid()
   def leakyrelu(self, neg_slope=0.01): return self.relu() - (-neg_slope*self).relu()
   def mish(self): return self * self.softplus().tanh()
   def softplus(self, beta=1): return (1/beta) * (1 + (self*beta).exp()).log()
+  def softsign(self): return self / (1 + self.abs())
 
   # ***** broadcasted binary mlops *****
 
-  def _broadcasted(self, fxn:Type[Function], other:Union[Tensor, float], reverse:bool=False) -> Tensor:
-    x,y = [Tensor([t], device=self.device, requires_grad=False) if not isinstance(t, Tensor) else t for t in ([other,self] if reverse else [self,other])]
-    x,y = [t.reshape([1]*(max(len(x.shape), len(y.shape))-len(t.shape)) + list(t.shape)) for t in [x,y]]
-    shape_ret = tuple(max(sx, sy) for sx,sy in zip(x.shape, y.shape))
-    return fxn.apply(x.expand(shape_ret), y.expand(shape_ret))
-
-  def add(self, x:Union[Tensor, float], reverse=False) -> Tensor: return self._broadcasted(mlops.Add, x, reverse) if isinstance(x, Tensor) or x != 0.0 else self
-  def sub(self, x:Union[Tensor, float], reverse=False) -> Tensor: return self._broadcasted(mlops.Sub, x, reverse) if isinstance(x, Tensor) or x != 0.0 or reverse else self
-  def mul(self, x:Union[Tensor, float], reverse=False) -> Tensor: return self._broadcasted(mlops.Mul, x, reverse) if isinstance(x, Tensor) or x != 1.0 else self
-  def pow(self, x:Union[Tensor, float], reverse=False) -> Tensor: return self._broadcasted(mlops.Pow, x, reverse) if isinstance(x, Tensor) or x != 1.0 or reverse else self
-  def div(self, x:Union[Tensor, float], reverse=False) -> Tensor: return self._broadcasted(mlops.Div, x, reverse) if isinstance(x, Tensor) or x != 1.0 or reverse else self
+  def _broadcasted(self, y:Union[Tensor, float], reverse:bool=False) -> Tuple[Tensor, Tensor]:
+    x: Tensor = self
+    if not isinstance(y, Tensor):
+      y = Tensor(y, device=self.device, requires_grad=False, dtype=self.dtype if self.dtype != dtypes.bool and self.dtype.__class__ is not ImageDType else dtypes.float32)
+    if reverse: x, y = y, x
+    if (xshape:=x.shape) == (yshape:=y.shape): return (x, y)
+
+    shape_delta = len(xshape) - len(yshape)
+    if shape_delta > 0: y = y.reshape((1,) * shape_delta + yshape)
+    elif shape_delta < 0: x = x.reshape((1,) * -shape_delta + xshape)
+    if (xshape:=x.shape) == (yshape:=y.shape): return (x, y)
+
+    shape_ret = tuple([max(x, y) for x, y in zip(xshape, yshape)])
+    if xshape != shape_ret: x = x.expand(shape_ret)
+    if yshape != shape_ret: y = y.expand(shape_ret)
+    return (x, y)
+
+  def _to_float(self, x:Union[Tensor, float]):
+    return x.lazydata.op.arg if isinstance(x, Tensor) and not x.lazydata.realized and x.lazydata.op.op == LoadOps.CONST and not x.requires_grad \
+      and x.lazydata.st.contiguous and self._broadcasted(x)[0].shape == self.shape else x
+
+  def add(self, x:Union[Tensor, float], reverse=False) -> Tensor:
+    x = self._to_float(x)
+    return mlops.Add.apply(*self._broadcasted(x, reverse)) if x.__class__ is Tensor or x else self
+  def sub(self, x:Union[Tensor, float], reverse=False) -> Tensor:
+    x = self._to_float(x)
+    return mlops.Sub.apply(*self._broadcasted(x, reverse)) if x.__class__ is Tensor or x else (-self if reverse else self)
+  def mul(self, x:Union[Tensor, float], reverse=False) -> Tensor:
+    x = self._to_float(x)
+    if x.__class__ is not Tensor and x == 0.0: return mlops.Zero.apply(self)
+    if x.__class__ is not Tensor and x == -1.0: return -self
+    return mlops.Mul.apply(*self._broadcasted(x, reverse)) if x.__class__ is Tensor or x != 1.0 else self
+  def div(self, x:Union[Tensor, float], reverse=False) -> Tensor:
+    x = self._to_float(x)
+    return mlops.Div.apply(*self._broadcasted(x, reverse)) if x.__class__ is Tensor or reverse or not x or not dtypes.is_float(self.dtype) else self.mul(1/x)
+  def pow(self, x:Union[Tensor, float], reverse=False) -> Tensor:
+    x = self._to_float(x)
+    if x.__class__ is not Tensor and not reverse:
+      # simple pow identities
+      if x < 0: return self.reciprocal().pow(-x)
+      if x == 3.0: return self*self*self
+      if x == 2.0: return self*self
+      if x == 1.0: return self
+      if x == 0.5: return self.sqrt()
+    if not isinstance(x, Tensor) and reverse and x > 0: return self.mul(math.log(x)).exp()
+    ar = self.abs().log().mul(x).exp() if not reverse or isinstance(x, Tensor) else self.mul(math.log(abs(x))).exp()
+    # correct sign of negative numbers raised to a power (cos has a period of 2pi so we use it here to get the oddness of the power)
+    sign = (x * math.pi).cos() if isinstance(x, Tensor) else math.cos(x * math.pi) if not reverse else (self * math.pi).cos()
+    # we only need to correct the sign if the base is negative
+    base_sign = ((self.sign() if not reverse else x.sign() if isinstance(x, Tensor) else math.copysign(1, x)) - 1) / -2
+    # we need 0 to be positive so we need to correct base_sign when the base is 0
+    base_sign = base_sign - (1.5 * (1 - (self.sign().abs() if not reverse else x.sign().abs() if isinstance(x, Tensor) else abs(int(bool(x))))))
+    # inject nan if the base is negative and the power is not an integer
+    to_nan = (((x - x.trunc()) * 1e10).abs().clip(0, 1) if isinstance(x, Tensor) else int(bool(x - int(x))) if not reverse else ((self - self.trunc()) * 1e10).abs().clip(0, 1)) * base_sign
+    inject_nan = ((((-to_nan) * 2) + 1)).log().add(1) if isinstance(to_nan, Tensor) else 1 if not to_nan else float("nan")
+    return ar.mul(sign * base_sign + (1 - base_sign)).mul(inject_nan)
   def matmul(self, x:Tensor, reverse=False) -> Tensor: return x.dot(self) if reverse else self.dot(x)
 
-  def maximum(self, x:Union[Tensor, float]) -> Tensor: return self._broadcasted(mlops.Maximum, x)
+  def maximum(self, x:Union[Tensor, float]) -> Tensor: return (self<x).detach().where(x, (self>x).detach().where(self, (self+x)/2))
   def minimum(self, x:Union[Tensor, float]) -> Tensor: return -((-self).maximum(-x))
-  def eq(self, x) -> Tensor: return self._broadcasted(mlops.Equal, x, False)
+
+  def where(self:Tensor, input_:Union[Tensor, float], other:Union[Tensor, float]):
+    x_,y = self._broadcasted(input_)
+    x,z = x_._broadcasted(other)
+    return mlops.Where.apply(x, *y._broadcasted(z))
 
   # ***** binary op wrappers (18 wasted lines to make the typechecker happy) *****
 
@@ -428,10 +711,12 @@ class Tensor:
   def __itruediv__(self, x) -> Tensor: return self.assign(self.div(x))
   def __imatmul__(self, x) -> Tensor: return self.assign(self.matmul(x))
 
-  def __ge__(self, x) -> Tensor: return self.maximum(x).eq(self)
-  def __le__(self, x) -> Tensor: return self.maximum(x).eq(x)
-  def __lt__(self, x) -> Tensor: return 1.0-(self>=x)
-  def __gt__(self, x) -> Tensor: return 1.0-(self<=x)
+  def __lt__(self, x) -> Tensor: return mlops.Less.apply(*self._broadcasted(x, False))
+  def __gt__(self, x) -> Tensor: return mlops.Less.apply(*self._broadcasted(x, True))
+  def __ge__(self, x) -> Tensor: return 1.0-(self<x)
+  def __le__(self, x) -> Tensor: return 1.0-(self>x)
+  def __ne__(self, x) -> Tensor: return (self<x) + (self>x)   # type: ignore
+  def __eq__(self, x) -> Tensor: return 1.0-(self != x)       # type: ignore
 
   # ***** functional nn ops *****
 
@@ -439,22 +724,67 @@ class Tensor:
     x = self.mul(weight) if len(weight.shape) == 1 else self.dot(weight)
     return x.add(bias) if bias is not None else x
 
-  def sequential(self, ll:List[Callable[[Tensor], Tensor]]): return functools.reduce(lambda x,f: f(x), ll, self)
+  def sequential(self, ll:List[Callable[[Tensor], Tensor]]): return reduce(lambda x,f: f(x), ll, self)
 
   def layernorm(self, axis=-1, eps:float=1e-5) -> Tensor:
-    y = (self - self.mean(axis=axis, keepdim=True))
-    return y.div((y*y).mean(axis=axis, keepdim=True).add(eps).sqrt())
+    y = (self - self.mean(axis, keepdim=True))
+    return y.mul((y*y).mean(axis, keepdim=True).add(eps).rsqrt())
 
-  def batchnorm(self, weight:Tensor, bias:Tensor, mean:Tensor, invstd:Tensor) -> Tensor:
-    x = (self - mean.reshape(shape=[1, -1, 1, 1])) * weight.reshape(shape=[1, -1, 1, 1])
-    return x.mul(invstd.reshape(shape=[1, -1, 1, 1])) + bias.reshape(shape=[1, -1, 1, 1])
+  def batchnorm(self, weight:Optional[Tensor], bias:Optional[Tensor], mean:Tensor, invstd:Tensor) -> Tensor:
+    x = (self - mean.reshape(shape=[1, -1, 1, 1]))
+    if weight: x = x * weight.reshape(shape=[1, -1, 1, 1])
+    ret = x.mul(invstd.reshape(shape=[1, -1, 1, 1]) if len(invstd.shape) == 1 else invstd)
+    return (ret + bias.reshape(shape=[1, -1, 1, 1])) if bias else ret
 
   def dropout(self, p=0.5) -> Tensor:
-    if not Tensor.training: return self
-    _mask : np.ndarray = np.asarray(Tensor._rng.binomial(1, 1.0-p, size=self.shape), dtype=self.dtype)
-    return self * Tensor(_mask, requires_grad=False, device=self.device) * (1/(1.0 - p))
+    if not Tensor.training or p == 0: return self
+    mask = (Tensor.rand(*self.shape, requires_grad=False, device=self.device) >= p).cast(dtypes.bool)
+    return self * mask * (1/(1.0 - p))
+
+  def scaled_dot_product_attention(self, key:Tensor, value:Tensor, attn_mask:Optional[Tensor]=None, dropout_p:float=0.0, is_causal:bool=False) -> Tensor:
+    # NOTE: it works if key, value have symbolic shape
+    assert all_int(self.shape), f"does not support symbolic shape {self.shape}"
+    if is_causal: attn_mask = Tensor.ones(self.shape[-2], key.shape[-2], requires_grad=False, device=self.device).tril(0).cast(dtypes.bool)
+    if attn_mask is not None and attn_mask.dtype == dtypes.bool: attn_mask = (attn_mask == 0).where(-float("inf"), attn_mask)
+    return (self @ key.transpose(-2,-1) / math.sqrt(self.shape[-1]) + attn_mask).softmax(-1).dropout(dropout_p) @ value
+
+  def binary_crossentropy(self, y:Tensor) -> Tensor:
+    return (-y*self.log() - (1-y)*(1-self).log()).mean()
+
+  def binary_crossentropy_logits(self, y:Tensor) -> Tensor:
+    return (self.maximum(0) - y * self + (1 + self.abs().__neg__().exp()).log()).mean()
+
+  def sparse_categorical_crossentropy(self, Y, ignore_index=-1) -> Tensor:
+    loss_mask = Y != ignore_index
+    y_counter = Tensor.arange(self.shape[-1], dtype=dtypes.int32, requires_grad=False, device=self.device).unsqueeze(0).expand(Y.numel(), self.shape[-1])
+    y = ((y_counter == Y.flatten().reshape(-1, 1)).where(-1.0, 0) * loss_mask.reshape(-1, 1)).reshape(*Y.shape, self.shape[-1])
+    return self.log_softmax().mul(y).sum() / loss_mask.sum()
+
+  # ***** cast ops *****
+
+  def cast(self, dtype:DType) -> Tensor: return mlops.Cast.apply(self, dtype=dtype) if self.dtype != dtype else self
+  def bitcast(self, dtype:DType) -> Tensor:
+    assert self.dtype.itemsize == dtype.itemsize, "can't bitcast mismatched dtype itemsizes"
+    return mlops.Cast.apply(self, dtype=dtype, bitcast=True) if self.dtype != dtype else self
+  def float(self) -> Tensor: return self.cast(dtypes.float32)
+  def half(self) -> Tensor: return self.cast(dtypes.float16)
+
+  # ***** convenience stuff *****
+
+  @property
+  def ndim(self) -> int: return len(self.shape)
+  def numel(self) -> sint: return prod(self.shape)
+  def element_size(self) -> int: return self.dtype.itemsize
+  def nbytes(self) -> int: return self.numel() * self.element_size()
+  def is_floating_point(self) -> bool: return dtypes.is_float(self.dtype)
 
 # register functions to move between devices
-for device in [device for device in Device._buffers.keys() if device[0] != "_"]:
-  setattr(Tensor, f"{device.lower()}", functools.partialmethod(Tensor.to, device))
-  setattr(Tensor, f"{device.lower()}_", functools.partialmethod(Tensor.to_, device))
+for device in Device._buffers:
+  setattr(Tensor, f"{device.lower()}", partialmethod(Tensor.to, device))
+  setattr(Tensor, f"{device.lower()}_", partialmethod(Tensor.to_, device))
+
+if IMAGE:
+  # if IMAGE>0 we install these replacement functions in Tensor (hack!)
+  from tinygrad.features.image import image_conv2d, image_dot
+  setattr(Tensor, "conv2d", image_conv2d)
+  setattr(Tensor, "dot", image_dot)
diff --git a/tools/bodyteleop/bodyav.py b/tools/bodyteleop/bodyav.py
new file mode 100644
index 0000000000..3f11f8d4f2
--- /dev/null
+++ b/tools/bodyteleop/bodyav.py
@@ -0,0 +1,159 @@
+import asyncio
+import io
+import numpy as np
+import pyaudio
+import wave
+
+from aiortc.contrib.media import MediaBlackhole
+from aiortc.mediastreams import AudioStreamTrack, MediaStreamError, MediaStreamTrack
+from aiortc.mediastreams import VIDEO_CLOCK_RATE, VIDEO_TIME_BASE
+from aiortc.rtcrtpsender import RTCRtpSender
+from av import CodecContext, Packet
+from pydub import AudioSegment
+import cereal.messaging as messaging
+
+AUDIO_RATE = 16000
+SOUNDS = {
+  'engage': '../../selfdrive/assets/sounds/engage.wav',
+  'disengage': '../../selfdrive/assets/sounds/disengage.wav',
+  'error': '../../selfdrive/assets/sounds/warning_immediate.wav',
+}
+
+
+def force_codec(pc, sender, forced_codec='video/VP9', stream_type="video"):
+  codecs = RTCRtpSender.getCapabilities(stream_type).codecs
+  codec = [codec for codec in codecs if codec.mimeType == forced_codec]
+  transceiver = next(t for t in pc.getTransceivers() if t.sender == sender)
+  transceiver.setCodecPreferences(codec)
+
+
+class EncodedBodyVideo(MediaStreamTrack):
+  kind = "video"
+
+  _start: float
+  _timestamp: int
+
+  def __init__(self):
+    super().__init__()
+    sock_name = 'livestreamDriverEncodeData'
+    messaging.context = messaging.Context()
+    self.sock = messaging.sub_sock(sock_name, None, conflate=True)
+    self.pts = 0
+
+  async def recv(self) -> Packet:
+    while True:
+      msg = messaging.recv_one_or_none(self.sock)
+      if msg is not None:
+        break
+      await asyncio.sleep(0.005)
+
+    evta = getattr(msg, msg.which())
+    self.last_idx = evta.idx.encodeId
+
+    packet = Packet(evta.header + evta.data)
+    packet.time_base = VIDEO_TIME_BASE
+    packet.pts = self.pts
+    self.pts += 0.05 * VIDEO_CLOCK_RATE
+    return packet
+
+
+class WebClientSpeaker(MediaBlackhole):
+  def __init__(self):
+    super().__init__()
+    self.p = pyaudio.PyAudio()
+    self.buffer = io.BytesIO()
+    self.channels = 2
+    self.stream = self.p.open(format=pyaudio.paInt16, channels=self.channels, rate=48000, frames_per_buffer=9600,
+                              output=True, stream_callback=self.pyaudio_callback)
+
+  def pyaudio_callback(self, in_data, frame_count, time_info, status):
+    if self.buffer.getbuffer().nbytes < frame_count * self.channels * 2:
+      buff = np.zeros((frame_count, 2), dtype=np.int16).tobytes()
+    elif self.buffer.getbuffer().nbytes > 115200:  # 3x the usual read size
+      self.buffer.seek(0)
+      buff = self.buffer.read(frame_count * self.channels * 4)
+      buff = buff[:frame_count * self.channels * 2]
+      self.buffer.seek(2)
+    else:
+      self.buffer.seek(0)
+      buff = self.buffer.read(frame_count * self.channels * 2)
+      self.buffer.seek(2)
+    return (buff, pyaudio.paContinue)
+
+  async def consume(self, track):
+    while True:
+      try:
+        frame = await track.recv()
+      except MediaStreamError:
+        return
+      bio = bytes(frame.planes[0])
+      self.buffer.write(bio)
+
+  async def start(self):
+    for track, task in self._MediaBlackhole__tracks.items():
+      if task is None:
+        self._MediaBlackhole__tracks[track] = asyncio.ensure_future(self.consume(track))
+
+  async def stop(self):
+    for task in self._MediaBlackhole__tracks.values():
+      if task is not None:
+        task.cancel()
+    self._MediaBlackhole__tracks = {}
+    self.stream.stop_stream()
+    self.stream.close()
+    self.p.terminate()
+
+
+class BodyMic(AudioStreamTrack):
+  def __init__(self):
+    super().__init__()
+
+    self.sample_rate = AUDIO_RATE
+    self.AUDIO_PTIME = 0.020  # 20ms audio packetization
+    self.samples = int(self.AUDIO_PTIME * self.sample_rate)
+    self.FORMAT = pyaudio.paInt16
+    self.CHANNELS = 2
+    self.RATE = self.sample_rate
+    self.CHUNK = int(AUDIO_RATE * 0.020)
+    self.p = pyaudio.PyAudio()
+    self.mic_stream = self.p.open(format=self.FORMAT, channels=1, rate=self.RATE, input=True, frames_per_buffer=self.CHUNK)
+
+    self.codec = CodecContext.create('pcm_s16le', 'r')
+    self.codec.sample_rate = self.RATE
+    self.codec.channels = 2
+    self.audio_samples = 0
+    self.chunk_number = 0
+
+  async def recv(self):
+    mic_data = self.mic_stream.read(self.CHUNK)
+    mic_sound = AudioSegment(mic_data, sample_width=2, channels=1, frame_rate=self.RATE)
+    mic_sound = AudioSegment.from_mono_audiosegments(mic_sound, mic_sound)
+    mic_sound += 3  # increase volume by 3db
+    packet = Packet(mic_sound.raw_data)
+    frame = self.codec.decode(packet)[0]
+    frame.pts = self.audio_samples
+    self.audio_samples += frame.samples
+    self.chunk_number = self.chunk_number + 1
+    return frame
+
+
+async def play_sound(sound):
+  chunk = 5120
+  with wave.open(SOUNDS[sound], 'rb') as wf:
+    def callback(in_data, frame_count, time_info, status):
+      data = wf.readframes(frame_count)
+      return data, pyaudio.paContinue
+
+    p = pyaudio.PyAudio()
+    stream = p.open(format=p.get_format_from_width(wf.getsampwidth()),
+                    channels=wf.getnchannels(),
+                    rate=wf.getframerate(),
+                    output=True,
+                    frames_per_buffer=chunk,
+                    stream_callback=callback)
+    stream.start_stream()
+    while stream.is_active():
+      await asyncio.sleep(0)
+    stream.stop_stream()
+    stream.close()
+    p.terminate()
diff --git a/tools/bodyteleop/static/index.html b/tools/bodyteleop/static/index.html
new file mode 100644
index 0000000000..3654769756
--- /dev/null
+++ b/tools/bodyteleop/static/index.html
@@ -0,0 +1,103 @@
+<html>
+  <head>
+    <meta charset="UTF-8"/>
+    <meta name="viewport" content="width=device-width, initial-scale=1.0" />
+    <title>commabody</title>
+    <link rel="stylesheet" href="/static/main.css">
+    <link rel="stylesheet" href="https://cdnjs.cloudflare.com/ajax/libs/bootstrap/5.2.3/css/bootstrap.min.css" integrity="sha512-SbiR/eusphKoMVVXysTKG/7VseWii+Y3FdHrt0EpKgpToZeemhqHeZeLWLhJutz/2ut2Vw1uQEj2MbRF+TVBUA==" crossorigin="anonymous" referrerpolicy="no-referrer" /><script src="https://code.jquery.com/jquery-3.6.0.min.js"></script>
+    <link rel="stylesheet" href="https://cdn.jsdelivr.net/npm/bootstrap-icons@1.10.5/font/bootstrap-icons.css">
+    <script src="https://cdnjs.cloudflare.com/ajax/libs/bootstrap/5.2.3/js/bootstrap.min.js" integrity="sha512-1/RvZTcCDEUjY/CypiMz+iqqtaoQfAITmNSJY17Myp4Ms5mdxPS5UV7iOfdZoxcGhzFbOm6sntTKJppjvuhg4g==" crossorigin="anonymous" referrerpolicy="no-referrer"></script>
+    <script src="https://cdnjs.cloudflare.com/ajax/libs/moment.js/2.29.1/moment.min.js"></script>
+    <script src="https://cdn.jsdelivr.net/npm/chart.js@^3"></script>
+    <script src="https://cdn.jsdelivr.net/npm/moment@^2"></script>
+    <script src="https://cdn.jsdelivr.net/npm/chartjs-adapter-moment@^1"></script>
+  </head>
+  <body>
+    <div id="main">
+      <p class="jumbo">comma body</p>
+      <audio id="audio" autoplay="true"></audio>
+      <video id="video" playsinline autoplay muted loop poster="/static/poster.png"></video>
+      <div id="icon-panel" class="row">
+        <div class="col-sm-12 col-md-6 details">
+          <div class="icon-sup-panel col-12">
+            <div class="icon-sub-panel">
+              <div class="icon-sub-sub-panel">
+                <i class="bi bi-speaker-fill pre-blob"></i>
+                <i class="bi bi-mic-fill pre-blob"></i>
+                <i class="bi bi-camera-video-fill pre-blob"></i>
+              </div>
+              <p class="small">body</p>
+            </div>
+            <div class="icon-sub-panel">
+              <div class="icon-sub-sub-panel">
+                <i class="bi bi-speaker-fill pre-blob"></i>
+                <i class="bi bi-mic-fill pre-blob"></i>
+              </div>
+              <p class="small">you</p>
+            </div>
+          </div>
+        </div>
+        <div class="col-sm-12 col-md-6 details">
+          <div class="icon-sup-panel col-12">
+            <div class="icon-sub-panel">
+              <div class="icon-sub-sub-panel">
+                <i id="ping-time" class="pre-blob1">-</i>
+              </div>
+              <p class="bi bi-arrow-repeat small"> ping time</p>
+            </div>
+            <div class="icon-sub-panel">
+              <div class="icon-sub-sub-panel">
+                <i id="battery" class="pre-blob1">-</i>
+              </div>
+              <p class="bi bi-battery-half small"> battery</p>
+            </div>
+          </div>
+        </div>
+        <!-- <div class="icon-sub-panel">
+                <button type="button" id="start" class="btn btn-light btn-lg">Start</button>
+                <button type="button" id="stop" class="btn btn-light btn-lg">Stop</button>
+            </div> -->
+      </div>
+      <div class="row" style="width: 100%; padding: 0px 10px 0px 10px;">
+        <div id="wasd" class="col-md-12 row">
+          <div class="col-md-6 col-sm-12" style="justify-content: center; display: flex; flex-direction: column;">
+            <div class="wasd-row">
+              <div class="keys" id="key-w">W</div>
+              <div id="key-val"><span id="pos-vals">0,0</span><span>x,y</span></div>
+            </div>
+            <div class="wasd-row">
+              <div class="keys" id="key-a">A</div>
+              <div class="keys" id="key-s">S</div>
+              <div class="keys" id="key-d">D</div>
+            </div>
+          </div>
+          <div class="col-md-6 col-sm-12 form-group plan-form">
+            <label for="plan-text">Plan (w, a, s, d, t)</label>
+            <label style="font-size: 15px;" for="plan-text">*Extremely Experimental*</label>
+            <textarea class="form-control" id="plan-text" rows="7" placeholder="1,0,0,0,2"></textarea>
+            <button type="button" id="plan-button" class="btn btn-light btn-lg">Execute</button>
+          </div>
+        </div>
+      </div>
+      <div class="row" style="padding: 0px 10px 0px 10px; width: 100%;">
+        <div class="panel row">
+          <div class="col-sm-3" style="text-align: center;">
+            <p>Play Sounds</p>
+          </div>
+          <div class="btn-group col-sm-8">
+            <button type="button" id="sound-engage" class="btn btn-outline-success btn-lg sound">Engage</button>
+            <button type="button" id="sound-disengage" class="btn btn-outline-warning btn-lg sound">Disengage</button>
+            <button type="button" id="sound-error" class="btn btn-outline-danger btn-lg sound">Error</button>
+          </div>
+        </div>
+      </div>
+      <div class="row" style="padding: 0px 10px 0px 10px; width: 100%;">
+        <div class="panel row">
+          <div class="col-sm-6"><canvas id="chart-ping"></canvas></div>
+          <div class="col-sm-6"><canvas id="chart-battery"></canvas></div>
+        </div>
+      </div>
+    </div>
+    <script src="/static/js/jsmain.js" type="module"></script>
+  </body>
+</html>
diff --git a/tools/bodyteleop/static/js/controls.js b/tools/bodyteleop/static/js/controls.js
new file mode 100644
index 0000000000..b1e0e7ee70
--- /dev/null
+++ b/tools/bodyteleop/static/js/controls.js
@@ -0,0 +1,54 @@
+const keyVals = {w: 0, a: 0, s: 0, d: 0}
+
+export function getXY() {
+  let x = -keyVals.w + keyVals.s
+  let y = -keyVals.d + keyVals.a
+  return {x, y}
+}
+
+export const handleKeyX = (key, setValue) => {
+  if (['w', 'a', 's', 'd'].includes(key)){
+    keyVals[key] = setValue;
+    let color = "#333";
+    if (setValue === 1){
+      color = "#e74c3c";
+    }
+    $("#key-"+key).css('background', color);
+    const {x, y} = getXY();
+    $("#pos-vals").text(x+","+y);
+  }
+};
+
+export async function executePlan() {
+  let plan = $("#plan-text").val();
+  const planList = [];
+  plan.split("\n").forEach(function(e){
+    let line = e.split(",").map(k=>parseInt(k));
+    if (line.length != 5 || line.slice(0, 4).map(e=>[1, 0].includes(e)).includes(false) || line[4] < 0 || line[4] > 10){
+      console.log("invalid plan");
+    }
+    else{
+      planList.push(line)
+    }
+  });
+
+  async function execute() {
+    for (var i = 0; i < planList.length; i++) {
+      let [w, a, s, d, t] = planList[i];
+      while(t > 0){
+        console.log(w, a, s, d, t);
+        if(w==1){$("#key-w").mousedown();}
+        if(a==1){$("#key-a").mousedown();}
+        if(s==1){$("#key-s").mousedown();}
+        if(d==1){$("#key-d").mousedown();}
+        await sleep(50);
+        $("#key-w").mouseup();
+        $("#key-a").mouseup();
+        $("#key-s").mouseup();
+        $("#key-d").mouseup();
+        t = t - 0.05;
+      }
+    }
+  }
+  execute();
+}
\ No newline at end of file
diff --git a/tools/bodyteleop/static/js/jsmain.js b/tools/bodyteleop/static/js/jsmain.js
new file mode 100644
index 0000000000..f521905724
--- /dev/null
+++ b/tools/bodyteleop/static/js/jsmain.js
@@ -0,0 +1,23 @@
+import { handleKeyX, executePlan } from "./controls.js";
+import { start, stop, last_ping } from "./webrtc.js";
+
+export var pc = null;
+export var dc = null;
+
+document.addEventListener('keydown', (e)=>(handleKeyX(e.key.toLowerCase(), 1)));
+document.addEventListener('keyup', (e)=>(handleKeyX(e.key.toLowerCase(), 0)));
+$(".keys").bind("mousedown touchstart", (e)=>handleKeyX($(e.target).attr('id').replace('key-', ''), 1));
+$(".keys").bind("mouseup touchend", (e)=>handleKeyX($(e.target).attr('id').replace('key-', ''), 0));
+$("#plan-button").click(executePlan);
+
+setInterval( () => {
+  const dt = new Date().getTime();
+  if ((dt - last_ping) > 1000) {
+    $(".pre-blob").removeClass('blob');
+    $("#battery").text("-");
+    $("#ping-time").text('-');
+    $("video")[0].load();
+  }
+}, 5000);
+
+start(pc, dc);
\ No newline at end of file
diff --git a/tools/bodyteleop/static/js/plots.js b/tools/bodyteleop/static/js/plots.js
new file mode 100644
index 0000000000..5327bf71be
--- /dev/null
+++ b/tools/bodyteleop/static/js/plots.js
@@ -0,0 +1,53 @@
+export const pingPoints = [];
+export const batteryPoints = [];
+
+function getChartConfig(pts, color, title, ymax=100) {
+  return {
+    type: 'line',
+    data: {
+      datasets: [{
+        label: title,
+        data: pts,
+        borderWidth: 1,
+        borderColor: color,
+        backgroundColor: color,
+        fill: 'origin'
+      }]
+    },
+    options: {
+      scales: {
+        x: {
+          type: 'time',
+          time: {
+            unit: 'minute',
+            displayFormats: {
+              second: 'h:mm a'
+            }
+          },
+          grid: {
+            color: '#222', // Grid lines color
+          },
+          ticks: {
+            source: 'data',
+            fontColor: 'rgba(255, 255, 255, 1.0)', // Y-axis label color
+          }
+        },
+        y: {
+          beginAtZero: true,
+          max: ymax,
+          grid: {
+            color: 'rgba(255, 255, 255, 0.1)', // Grid lines color
+          },
+          ticks: {
+            fontColor: 'rgba(255, 255, 255, 0.7)', // Y-axis label color
+          }
+        }
+      }
+    }
+  }
+}
+
+const ctxPing = document.getElementById('chart-ping');
+const ctxBattery = document.getElementById('chart-battery');
+export const chartPing = new Chart(ctxPing, getChartConfig(pingPoints, 'rgba(192, 57, 43, 0.7)', 'Controls Ping Time (ms)', 250));
+export const chartBattery = new Chart(ctxBattery, getChartConfig(batteryPoints, 'rgba(41, 128, 185, 0.7)', 'Battery %', 100));
diff --git a/tools/bodyteleop/static/js/webrtc.js b/tools/bodyteleop/static/js/webrtc.js
new file mode 100644
index 0000000000..e2f6583c17
--- /dev/null
+++ b/tools/bodyteleop/static/js/webrtc.js
@@ -0,0 +1,227 @@
+import { getXY } from "./controls.js";
+import { pingPoints, batteryPoints, chartPing, chartBattery } from "./plots.js";
+
+export let dcInterval = null;
+export let batteryInterval = null;
+export let last_ping = null;
+
+
+export function createPeerConnection(pc) {
+  var config = {
+    sdpSemantics: 'unified-plan'
+  };
+
+  pc = new RTCPeerConnection(config);
+
+  // connect audio / video
+  pc.addEventListener('track', function(evt) {
+    console.log("Adding Tracks!")
+    if (evt.track.kind == 'video')
+      document.getElementById('video').srcObject = evt.streams[0];
+    else
+      document.getElementById('audio').srcObject = evt.streams[0];
+  });
+  return pc;
+}
+
+
+export function negotiate(pc) {
+  return pc.createOffer({offerToReceiveAudio:true, offerToReceiveVideo:true}).then(function(offer) {
+    return pc.setLocalDescription(offer);
+  }).then(function() {
+    return new Promise(function(resolve) {
+      if (pc.iceGatheringState === 'complete') {
+        resolve();
+      }
+      else {
+        function checkState() {
+          if (pc.iceGatheringState === 'complete') {
+            pc.removeEventListener('icegatheringstatechange', checkState);
+            resolve();
+          }
+        }
+        pc.addEventListener('icegatheringstatechange', checkState);
+      }
+    });
+  }).then(function() {
+    var offer = pc.localDescription;
+    return fetch('/offer', {
+      body: JSON.stringify({
+        sdp: offer.sdp,
+        type: offer.type,
+      }),
+      headers: {
+        'Content-Type': 'application/json'
+      },
+      method: 'POST'
+    });
+  }).then(function(response) {
+    console.log(response);
+    return response.json();
+  }).then(function(answer) {
+    return pc.setRemoteDescription(answer);
+  }).catch(function(e) {
+    alert(e);
+  });
+}
+
+
+function isMobile() {
+    let check = false;
+    (function(a){if(/(android|bb\d+|meego).+mobile|avantgo|bada\/|blackberry|blazer|compal|elaine|fennec|hiptop|iemobile|ip(hone|od)|iris|kindle|lge |maemo|midp|mmp|mobile.+firefox|netfront|opera m(ob|in)i|palm( os)?|phone|p(ixi|re)\/|plucker|pocket|psp|series(4|6)0|symbian|treo|up\.(browser|link)|vodafone|wap|windows ce|xda|xiino/i.test(a)||/1207|6310|6590|3gso|4thp|50[1-6]i|770s|802s|a wa|abac|ac(er|oo|s\-)|ai(ko|rn)|al(av|ca|co)|amoi|an(ex|ny|yw)|aptu|ar(ch|go)|as(te|us)|attw|au(di|\-m|r |s )|avan|be(ck|ll|nq)|bi(lb|rd)|bl(ac|az)|br(e|v)w|bumb|bw\-(n|u)|c55\/|capi|ccwa|cdm\-|cell|chtm|cldc|cmd\-|co(mp|nd)|craw|da(it|ll|ng)|dbte|dc\-s|devi|dica|dmob|do(c|p)o|ds(12|\-d)|el(49|ai)|em(l2|ul)|er(ic|k0)|esl8|ez([4-7]0|os|wa|ze)|fetc|fly(\-|_)|g1 u|g560|gene|gf\-5|g\-mo|go(\.w|od)|gr(ad|un)|haie|hcit|hd\-(m|p|t)|hei\-|hi(pt|ta)|hp( i|ip)|hs\-c|ht(c(\-| |_|a|g|p|s|t)|tp)|hu(aw|tc)|i\-(20|go|ma)|i230|iac( |\-|\/)|ibro|idea|ig01|ikom|im1k|inno|ipaq|iris|ja(t|v)a|jbro|jemu|jigs|kddi|keji|kgt( |\/)|klon|kpt |kwc\-|kyo(c|k)|le(no|xi)|lg( g|\/(k|l|u)|50|54|\-[a-w])|libw|lynx|m1\-w|m3ga|m50\/|ma(te|ui|xo)|mc(01|21|ca)|m\-cr|me(rc|ri)|mi(o8|oa|ts)|mmef|mo(01|02|bi|de|do|t(\-| |o|v)|zz)|mt(50|p1|v )|mwbp|mywa|n10[0-2]|n20[2-3]|n30(0|2)|n50(0|2|5)|n7(0(0|1)|10)|ne((c|m)\-|on|tf|wf|wg|wt)|nok(6|i)|nzph|o2im|op(ti|wv)|oran|owg1|p800|pan(a|d|t)|pdxg|pg(13|\-([1-8]|c))|phil|pire|pl(ay|uc)|pn\-2|po(ck|rt|se)|prox|psio|pt\-g|qa\-a|qc(07|12|21|32|60|\-[2-7]|i\-)|qtek|r380|r600|raks|rim9|ro(ve|zo)|s55\/|sa(ge|ma|mm|ms|ny|va)|sc(01|h\-|oo|p\-)|sdk\/|se(c(\-|0|1)|47|mc|nd|ri)|sgh\-|shar|sie(\-|m)|sk\-0|sl(45|id)|sm(al|ar|b3|it|t5)|so(ft|ny)|sp(01|h\-|v\-|v )|sy(01|mb)|t2(18|50)|t6(00|10|18)|ta(gt|lk)|tcl\-|tdg\-|tel(i|m)|tim\-|t\-mo|to(pl|sh)|ts(70|m\-|m3|m5)|tx\-9|up(\.b|g1|si)|utst|v400|v750|veri|vi(rg|te)|vk(40|5[0-3]|\-v)|vm40|voda|vulc|vx(52|53|60|61|70|80|81|83|85|98)|w3c(\-| )|webc|whit|wi(g |nc|nw)|wmlb|wonu|x700|yas\-|your|zeto|zte\-/i.test(a.substr(0,4))) check = true;})(navigator.userAgent||navigator.vendor||window.opera);
+    return check;
+};
+
+
+export const constraints = {
+  audio: {
+    autoGainControl: false,
+    sampleRate: 48000,
+    sampleSize: 16,
+    echoCancellation: true,
+    noiseSuppression: true,
+    channelCount: 1
+  },
+  video: isMobile()
+};
+
+
+export function createDummyVideoTrack() {
+  const canvas = document.createElement('canvas');
+  const context = canvas.getContext('2d');
+
+  const frameWidth = 5; // Set the width of the frame
+  const frameHeight = 5; // Set the height of the frame
+  canvas.width = frameWidth;
+  canvas.height = frameHeight;
+
+  context.fillStyle = 'black';
+  context.fillRect(0, 0, frameWidth, frameHeight);
+
+  const stream = canvas.captureStream();
+  const videoTrack = stream.getVideoTracks()[0];
+
+  return videoTrack;
+}
+
+
+export function start(pc, dc) {
+  pc = createPeerConnection(pc);
+
+  // add audio track
+  navigator.mediaDevices.enumerateDevices()
+    .then(function(devices) {
+      const hasAudioInput = devices.find((device) => device.kind === "audioinput");
+      var modifiedConstraints = {};
+      modifiedConstraints.video = constraints.video;
+      modifiedConstraints.audio = hasAudioInput ? constraints.audio : false;
+
+      return Promise.resolve(modifiedConstraints);
+    })
+    .then(function(constraints) {
+      if (constraints.audio || constraints.video) {
+        return navigator.mediaDevices.getUserMedia(constraints);
+      } else{
+        return Promise.resolve(null);
+      }
+    })
+    .then(function(stream) {
+      if (stream) {
+        stream.getTracks().forEach(function(track) {
+          pc.addTrack(track, stream);
+        });
+      }
+
+      return negotiate(pc);
+    })
+    .catch(function(err) {
+      alert('Could not acquire media: ' + err);
+    });
+
+  // add a fake video?
+  // const dummyVideoTrack = createDummyVideoTrack();
+  // const dummyMediaStream = new MediaStream();
+  // dummyMediaStream.addTrack(dummyVideoTrack);
+  // pc.addTrack(dummyVideoTrack, dummyMediaStream);
+
+  // setInterval(() => {pc.getStats(null).then((stats) => {stats.forEach((report) => console.log(report))})}, 10000)
+  // var video = document.querySelector('video');
+  // var print = function (e, f){console.log(e, f);  video.requestVideoFrameCallback(print);};
+  // video.requestVideoFrameCallback(print);
+
+  var parameters = {"ordered": true};
+  dc = pc.createDataChannel('data', parameters);
+  dc.onclose = function() {
+    console.log("data channel closed");
+    clearInterval(dcInterval);
+    clearInterval(batteryInterval);
+  };
+  function controlCommand() {
+    const {x, y} = getXY();
+    const dt = new Date().getTime();
+    var message = JSON.stringify({type: 'control_command', x, y, dt});
+    dc.send(message);
+  }
+
+  function batteryLevel() {
+    var message = JSON.stringify({type: 'battery_level'});
+    dc.send(message);
+  }
+
+  dc.onopen = function() {
+    dcInterval = setInterval(controlCommand, 50);
+    batteryInterval = setInterval(batteryLevel, 10000);
+    controlCommand();
+    batteryLevel();
+    $(".sound").click((e)=>{
+      const sound = $(e.target).attr('id').replace('sound-', '')
+      dc.send(JSON.stringify({type: 'play_sound', sound}));
+    });
+  };
+
+  let val_print_idx = 0;
+  dc.onmessage = function(evt) {
+    const data = JSON.parse(evt.data);
+    if(val_print_idx == 0 && data.type === 'ping_time') {
+      const dt = new Date().getTime();
+      const pingtime = dt - data.incoming_time;
+      pingPoints.push({'x': dt, 'y': pingtime});
+        if (pingPoints.length > 1000) {
+        pingPoints.shift();
+      }
+      chartPing.update();
+      $("#ping-time").text((pingtime) + "ms");
+      last_ping = dt;
+      $(".pre-blob").addClass('blob');
+    }
+    val_print_idx = (val_print_idx + 1 ) % 20;
+    if(data.type === 'battery_level') {
+      $("#battery").text(data.value + "%");
+      batteryPoints.push({'x': new Date().getTime(), 'y': data.value});
+        if (batteryPoints.length > 1000) {
+        batteryPoints.shift();
+      }
+      chartBattery.update();
+    }
+  };
+}
+
+
+export function stop(pc, dc) {
+  if (dc) {
+    dc.close();
+  }
+  if (pc.getTransceivers) {
+    pc.getTransceivers().forEach(function(transceiver) {
+      if (transceiver.stop) {
+        transceiver.stop();
+      }
+    });
+  }
+  pc.getSenders().forEach(function(sender) {
+    sender.track.stop();
+  });
+  setTimeout(function() {
+    pc.close();
+  }, 500);
+}
diff --git a/tools/bodyteleop/static/main.css b/tools/bodyteleop/static/main.css
new file mode 100644
index 0000000000..1bfb5982b4
--- /dev/null
+++ b/tools/bodyteleop/static/main.css
@@ -0,0 +1,185 @@
+body {
+  background: #333 !important;
+  color: #fff !important;
+  display: flex;
+  justify-content: center;
+  align-items: start;
+}
+
+p {
+  margin: 0px !important;
+}
+
+i {
+  font-style: normal;
+}
+
+.small {
+  font-size: 1em !important
+}
+
+.jumbo {
+  font-size: 8rem;
+}
+
+
+@media (max-width: 600px) {
+  .small {
+      font-size: 0.5em !important
+  }
+  .jumbo {
+      display: none;
+  }
+  
+}
+
+#main {
+  display: flex;
+  flex-direction: column;
+  align-content: center;
+  justify-content: center;
+  align-items: center;
+  font-size: 30px;
+  width: 100%;
+  max-width: 1200px;
+}
+
+video {
+  width: 95%;
+}
+
+.pre-blob {
+  display: flex;
+  background: #333;
+  border-radius: 50%;
+  margin: 10px;
+  height: 45px;
+  width: 45px;
+  justify-content: center;
+  align-items: center;
+  font-size: 1rem;
+}
+
+.blob {
+  background: rgba(231, 76, 60,1.0);
+  box-shadow: 0 0 0 0 rgba(231, 76, 60,1.0);
+  animation: pulse 2s infinite;
+}
+
+@keyframes pulse {
+  0% {
+    box-shadow: 0 0 0 0px rgba(192, 57, 43, 1);
+  }
+  100% {
+    box-shadow: 0 0 0 20px rgba(192, 57, 43, 0);
+  }
+}
+
+
+.icon-sup-panel {
+  display: flex;
+  flex-direction: row;
+  justify-content: space-around;
+  align-items: center;
+  background: #222;
+  border-radius: 10px;
+  padding: 5px;
+  margin: 5px 0px 5px 0px;
+}
+
+.icon-sub-panel {
+  display: flex;
+  flex-direction: column;
+  justify-content: space-between;
+  align-items: center;
+}
+
+#icon-panel {
+  display: flex;
+  width: 100%;
+  justify-content: space-between;
+  margin-top: 5px;
+}
+
+.icon-sub-sub-panel {
+  display: flex;
+  flex-direction: row;
+}
+
+.keys, #key-val {
+  background: #333;
+  padding: 2rem;
+  margin: 5px;
+  color: #fff;
+  display: flex;
+  justify-content: center;
+  align-items: center;
+  border-radius: 10px;
+  cursor: pointer;
+}
+
+#key-val {
+  pointer-events: none;
+  background: #fff;
+  color: #333;
+  line-height: 1;
+  font-size: 20px;
+  flex-direction: column;
+}
+
+.wasd-row {
+  display: flex;
+  flex-direction: row;
+  justify-content: center;
+  align-items: stretch;
+}
+
+#wasd {
+  margin: 5px 0px 5px 0px;
+  background: #222;
+  border-radius: 10px;
+  width: 100%;
+  padding: 20px;
+  display: flex;
+  flex-direction: row;
+  justify-content: space-around;
+  align-items: stretch;
+
+  user-select: none;
+  -webkit-touch-callout: none;
+  -webkit-user-select: none;
+  -khtml-user-select: none;
+  -moz-user-select: none;
+  -ms-user-select: none;
+  touch-action: manipulation;
+}
+
+.panel {
+  display: flex;
+  justify-content: center;
+  margin: 5px 0px 5px 0px !important;
+  background: #222;
+  border-radius: 10px;
+  width: 100%;
+  padding: 10px;
+}
+
+#ping-time, #battery {
+  font-size: 25px;
+}
+
+#stop {
+  display: none;
+}
+
+.plan-form {
+  display: flex;
+  flex-direction: column;
+  justify-content: space-between;
+  align-items: center;
+}
+
+.details {
+  display: flex;
+  padding: 0px 10px 0px 10px;
+}
diff --git a/tools/bodyteleop/static/poster.png b/tools/bodyteleop/static/poster.png
new file mode 100644
index 0000000000..ba0e846b22
Binary files /dev/null and b/tools/bodyteleop/static/poster.png differ
diff --git a/tools/bodyteleop/web.py b/tools/bodyteleop/web.py
new file mode 100644
index 0000000000..717afdeaf8
--- /dev/null
+++ b/tools/bodyteleop/web.py
@@ -0,0 +1,207 @@
+import asyncio
+import json
+import logging
+import os
+import ssl
+import uuid
+import time
+
+# aiortc and its dependencies have lots of internal warnings :(
+import warnings
+warnings.resetwarnings()
+warnings.simplefilter("always")
+
+from aiohttp import web
+from aiortc import RTCPeerConnection, RTCSessionDescription
+
+import cereal.messaging as messaging
+from openpilot.common.basedir import BASEDIR
+from openpilot.tools.bodyteleop.bodyav import BodyMic, WebClientSpeaker, force_codec, play_sound, MediaBlackhole, EncodedBodyVideo
+
+logger = logging.getLogger("pc")
+logging.basicConfig(level=logging.INFO)
+
+pcs = set()
+pm, sm = None, None
+TELEOPDIR = f"{BASEDIR}/tools/bodyteleop"
+
+
+async def index(request):
+  content = open(TELEOPDIR + "/static/index.html", "r").read()
+  now = time.monotonic()
+  request.app['mutable_vals']['last_send_time'] = now
+  request.app['mutable_vals']['last_override_time'] = now
+  request.app['mutable_vals']['prev_command'] = []
+  request.app['mutable_vals']['find_person'] = False
+
+  return web.Response(content_type="text/html", text=content)
+
+
+async def control_body(data, app):
+  now = time.monotonic()
+  if (data['type'] == 'dummy_controls') and (now < (app['mutable_vals']['last_send_time'] + 0.2)):
+    return
+  if (data['type'] == 'control_command') and (app['mutable_vals']['prev_command'] == [data['x'], data['y']] and data['x'] == 0 and data['y'] == 0):
+    return
+
+  logger.info(str(data))
+  x = max(-1.0, min(1.0, data['x']))
+  y = max(-1.0, min(1.0, data['y']))
+  dat = messaging.new_message('testJoystick')
+  dat.testJoystick.axes = [x, y]
+  dat.testJoystick.buttons = [False]
+  pm.send('testJoystick', dat)
+  app['mutable_vals']['last_send_time'] = now
+  if (data['type'] == 'control_command'):
+    app['mutable_vals']['last_override_time'] = now
+    app['mutable_vals']['prev_command'] = [data['x'], data['y']]
+
+
+async def dummy_controls_msg(app):
+  while True:
+    if 'last_send_time' in app['mutable_vals']:
+      this_time = time.monotonic()
+      if (app['mutable_vals']['last_send_time'] + 0.2) < this_time:
+        await control_body({'type': 'dummy_controls', 'x': 0, 'y': 0}, app)
+    await asyncio.sleep(0.2)
+
+
+async def start_background_tasks(app):
+  app['bgtask_dummy_controls_msg'] = asyncio.create_task(dummy_controls_msg(app))
+
+
+async def stop_background_tasks(app):
+  app['bgtask_dummy_controls_msg'].cancel()
+  await app['bgtask_dummy_controls_msg']
+
+
+async def offer(request):
+  logger.info("\n\n\nnewoffer!\n\n")
+
+  params = await request.json()
+  offer = RTCSessionDescription(sdp=params["sdp"], type=params["type"])
+  speaker = WebClientSpeaker()
+  blackhole = MediaBlackhole()
+
+  pc = RTCPeerConnection()
+  pc_id = "PeerConnection(%s)" % uuid.uuid4()
+  pcs.add(pc)
+
+  def log_info(msg, *args):
+    logger.info(pc_id + " " + msg, *args)
+
+  log_info("Created for %s", request.remote)
+
+  @pc.on("datachannel")
+  def on_datachannel(channel):
+    request.app['mutable_vals']['remote_channel'] = channel
+
+    @channel.on("message")
+    async def on_message(message):
+      data = json.loads(message)
+      if data['type'] == 'control_command':
+        await control_body(data, request.app)
+        times = {
+          'type': 'ping_time',
+          'incoming_time': data['dt'],
+          'outgoing_time': int(time.time() * 1000),
+        }
+        channel.send(json.dumps(times))
+      if data['type'] == 'battery_level':
+        sm.update(timeout=0)
+        if sm.updated['carState']:
+          channel.send(json.dumps({'type': 'battery_level', 'value': int(sm['carState'].fuelGauge * 100)}))
+      if data['type'] == 'play_sound':
+        logger.info(f"Playing sound: {data['sound']}")
+        await play_sound(data['sound'])
+      if data['type'] == 'find_person':
+        request.app['mutable_vals']['find_person'] = data['value']
+
+  @pc.on("connectionstatechange")
+  async def on_connectionstatechange():
+    log_info("Connection state is %s", pc.connectionState)
+    if pc.connectionState == "failed":
+      await pc.close()
+      pcs.discard(pc)
+
+  @pc.on('track')
+  def on_track(track):
+    logger.info(f"Track received: {track.kind}")
+    if track.kind == "audio":
+      speaker.addTrack(track)
+    elif track.kind == "video":
+      blackhole.addTrack(track)
+
+    @track.on("ended")
+    async def on_ended():
+      log_info("Remote %s track ended", track.kind)
+      if track.kind == "audio":
+        await speaker.stop()
+      elif track.kind == "video":
+        await blackhole.stop()
+
+  video_sender = pc.addTrack(EncodedBodyVideo())
+  force_codec(pc, video_sender, forced_codec='video/H264')
+  _ = pc.addTrack(BodyMic())
+
+  await pc.setRemoteDescription(offer)
+  await speaker.start()
+  await blackhole.start()
+  answer = await pc.createAnswer()
+  await pc.setLocalDescription(answer)
+
+  return web.Response(
+    content_type="application/json",
+    text=json.dumps(
+      {"sdp": pc.localDescription.sdp, "type": pc.localDescription.type}
+    ),
+  )
+
+
+async def on_shutdown(app):
+  coros = [pc.close() for pc in pcs]
+  await asyncio.gather(*coros)
+  pcs.clear()
+
+
+async def run(cmd):
+  proc = await asyncio.create_subprocess_shell(
+    cmd,
+    stdout=asyncio.subprocess.PIPE,
+    stderr=asyncio.subprocess.PIPE
+  )
+  stdout, stderr = await proc.communicate()
+  logger.info("Created key and cert!")
+  if stdout:
+    logger.info(f'[stdout]\n{stdout.decode()}')
+  if stderr:
+    logger.info(f'[stderr]\n{stderr.decode()}')
+
+
+def main():
+  global pm, sm
+  pm = messaging.PubMaster(['testJoystick'])
+  sm = messaging.SubMaster(['carState', 'logMessage'])
+  # App needs to be HTTPS for microphone and audio autoplay to work on the browser
+  cert_path = TELEOPDIR + '/cert.pem'
+  key_path = TELEOPDIR + '/key.pem'
+  if (not os.path.exists(cert_path)) or (not os.path.exists(key_path)):
+    asyncio.run(run(f'openssl req -x509 -newkey rsa:4096 -nodes -out {cert_path} -keyout {key_path} \
+                     -days 365 -subj "/C=US/ST=California/O=commaai/OU=comma body"'))
+  else:
+    logger.info("Certificate exists!")
+  ssl_context = ssl.SSLContext()
+  ssl_context.load_cert_chain(cert_path, key_path)
+  app = web.Application()
+  app['mutable_vals'] = {}
+  app.on_shutdown.append(on_shutdown)
+  app.router.add_post("/offer", offer)
+  app.router.add_get("/", index)
+  app.router.add_static('/static', TELEOPDIR + '/static')
+  app.on_startup.append(start_background_tasks)
+  app.on_cleanup.append(stop_background_tasks)
+  web.run_app(app, access_log=None, host="0.0.0.0", port=5000, ssl_context=ssl_context)
+
+
+if __name__ == "__main__":
+  main()
diff --git a/tools/joystick/README.md b/tools/joystick/README.md
index c74e51146c..65422afe80 100644
--- a/tools/joystick/README.md
+++ b/tools/joystick/README.md
@@ -61,4 +61,4 @@ Now start your car and openpilot should go into joystick mode with an alert on s
 
 Make sure the conditions are met in the panda to allow controls (e.g. cruise control engaged). You can also make a modification to the panda code to always allow controls.
 
-![](steer.gif)
+![](https://github.com/commaai/openpilot/assets/8762862/e640cbca-cb7a-4dcb-abce-b23b036ad8e7)
diff --git a/tools/joystick/joystickd.py b/tools/joystick/joystickd.py
index b31dab83fe..82847e3fa1 100755
--- a/tools/joystick/joystickd.py
+++ b/tools/joystick/joystickd.py
@@ -5,10 +5,10 @@ import threading
 from inputs import get_gamepad
 
 import cereal.messaging as messaging
-from common.realtime import Ratekeeper
-from common.numpy_fast import interp, clip
-from common.params import Params
-from tools.lib.kbhit import KBHit
+from openpilot.common.realtime import Ratekeeper
+from openpilot.common.numpy_fast import interp, clip
+from openpilot.common.params import Params
+from openpilot.tools.lib.kbhit import KBHit
 
 
 class Keyboard:
diff --git a/tools/joystick/steer.gif b/tools/joystick/steer.gif
deleted file mode 100644
index 8331b50de9..0000000000
Binary files a/tools/joystick/steer.gif and /dev/null differ
diff --git a/tools/joystick/web.py b/tools/joystick/web.py
deleted file mode 100755
index 5cba4e938d..0000000000
--- a/tools/joystick/web.py
+++ /dev/null
@@ -1,78 +0,0 @@
-#!/usr/bin/env python3
-import time
-import threading
-from flask import Flask
-
-import cereal.messaging as messaging
-
-app = Flask(__name__)
-pm = messaging.PubMaster(['testJoystick'])
-
-index = """
-<html>
-<head>
-<script src="https://github.com/bobboteck/JoyStick/releases/download/v1.1.6/joy.min.js"></script>
-</head>
-<body>
-<div id="joyDiv" style="width:100%;height:100%"></div>
-<script type="text/javascript">
-// Set up gamepad handlers
-let gamepad = null;
-window.addEventListener("gamepadconnected", function(e) {
-  gamepad = e.gamepad;
-});
-window.addEventListener("gamepaddisconnected", function(e) {
-  gamepad = null;
-});
-// Create JoyStick object into the DIV 'joyDiv'
-var joy = new JoyStick('joyDiv');
-setInterval(function(){
-  var x = -joy.GetX()/100;
-  var y = joy.GetY()/100;
-  if (x === 0 && y === 0 && gamepad !== null) {
-    let gamepadstate = navigator.getGamepads()[gamepad.index];
-    x = -gamepadstate.axes[0];
-    y = -gamepadstate.axes[1];
-  }
-  let xhr = new XMLHttpRequest();
-  xhr.open("GET", "/control/"+x+"/"+y);
-  xhr.send();
-}, 50);
-</script>
-"""
-
-@app.route("/")
-def hello_world():
-  return index
-
-last_send_time = time.monotonic()
-@app.route("/control/<x>/<y>")
-def control(x, y):
-  global last_send_time
-  x,y = float(x), float(y)
-  x = max(-1, min(1, x))
-  y = max(-1, min(1, y))
-  dat = messaging.new_message('testJoystick')
-  dat.testJoystick.axes = [y,x]
-  dat.testJoystick.buttons = [False]
-  pm.send('testJoystick', dat)
-  last_send_time = time.monotonic()
-  return ""
-
-def handle_timeout():
-  while 1:
-    this_time = time.monotonic()
-    if (last_send_time+0.5) < this_time:
-      #print("timeout, no web in %.2f s" % (this_time-last_send_time))
-      dat = messaging.new_message('testJoystick')
-      dat.testJoystick.axes = [0,0]
-      dat.testJoystick.buttons = [False]
-      pm.send('testJoystick', dat)
-    time.sleep(0.1)
-
-def main():
-  threading.Thread(target=handle_timeout, daemon=True).start()
-  app.run(host="0.0.0.0")
-
-if __name__ == '__main__':
-  main()
diff --git a/tools/lib/README.md b/tools/lib/README.md
index d77eef5ac5..daf74aaf40 100644
--- a/tools/lib/README.md
+++ b/tools/lib/README.md
@@ -3,10 +3,10 @@
 Route is a class for conveniently accessing all the [logs](/system/loggerd/) from your routes. The LogReader class reads the non-video logs, i.e. rlog.bz2 and qlog.bz2. There's also a matching FrameReader class for reading the videos.
 
 ```python
-from tools.lib.route import Route
-from tools.lib.logreader import LogReader
+from openpilot.tools.lib.route import Route
+from openpilot.tools.lib.logreader import LogReader
 
-r = Route("4cf7a6ad03080c90|2021-09-29--13-46-36")
+r = Route("a2a0ccea32023010|2023-07-27--13-01-19")
 
 # get a list of paths for the route's rlog files
 print(r.log_paths())
@@ -37,11 +37,11 @@ for msg in lr:
 `MultiLogIterator` is similar to `LogReader`, but reads multiple logs. 
 
 ```python
-from tools.lib.route import Route
-from tools.lib.logreader import MultiLogIterator
+from openpilot.tools.lib.route import Route
+from openpilot.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")
+r = Route("a2a0ccea32023010|2023-07-27--13-01-19")
 lr = MultiLogIterator(r.log_paths())
 
 # print all the steering angles values from all the logs in the route
diff --git a/tools/lib/auth.py b/tools/lib/auth.py
index 086027968f..997d1f860d 100755
--- a/tools/lib/auth.py
+++ b/tools/lib/auth.py
@@ -29,8 +29,8 @@ from http.server import BaseHTTPRequestHandler, HTTPServer
 from typing import Any, Dict
 from urllib.parse import parse_qs, urlencode
 
-from tools.lib.api import APIError, CommaApi, UnauthorizedError
-from tools.lib.auth_config import set_token, get_token
+from openpilot.tools.lib.api import APIError, CommaApi, UnauthorizedError
+from openpilot.tools.lib.auth_config import set_token, get_token
 
 PORT = 3000
 
@@ -54,7 +54,7 @@ class ClientRedirectHandler(BaseHTTPRequestHandler):
     self.end_headers()
     self.wfile.write(b'Return to the CLI to continue')
 
-  def log_message(self, format, *args):  # pylint: disable=redefined-builtin
+  def log_message(self, *args):
     pass  # this prevent http server from dumping messages to stdout
 
 
diff --git a/tools/lib/auth_config.py b/tools/lib/auth_config.py
index 7952fee495..bd76761043 100644
--- a/tools/lib/auth_config.py
+++ b/tools/lib/auth_config.py
@@ -1,7 +1,7 @@
 import json
 import os
-from common.file_helpers import mkdirs_exists_ok
-from system.hardware import PC
+from openpilot.common.file_helpers import mkdirs_exists_ok
+from openpilot.system.hardware import PC
 
 
 class MissingAuthConfigError(Exception):
@@ -13,9 +13,6 @@ if PC:
 else:
   CONFIG_DIR = "/tmp/.comma"
 
-mkdirs_exists_ok(CONFIG_DIR)
-
-
 def get_token():
   try:
     with open(os.path.join(CONFIG_DIR, 'auth.json')) as f:
@@ -26,9 +23,13 @@ def get_token():
 
 
 def set_token(token):
+  mkdirs_exists_ok(CONFIG_DIR)
   with open(os.path.join(CONFIG_DIR, 'auth.json'), 'w') as f:
     json.dump({'access_token': token}, f)
 
 
 def clear_token():
-  os.unlink(os.path.join(CONFIG_DIR, 'auth.json'))
+  try:
+    os.unlink(os.path.join(CONFIG_DIR, 'auth.json'))
+  except FileNotFoundError:
+    pass
diff --git a/tools/lib/bootlog.py b/tools/lib/bootlog.py
index 1e474e5dde..01756bb5e9 100644
--- a/tools/lib/bootlog.py
+++ b/tools/lib/bootlog.py
@@ -3,9 +3,9 @@ import functools
 import re
 from typing import List, Optional
 
-from tools.lib.auth_config import get_token
-from tools.lib.api import CommaApi
-from tools.lib.helpers import RE, timestamp_to_datetime
+from openpilot.tools.lib.auth_config import get_token
+from openpilot.tools.lib.api import CommaApi
+from openpilot.tools.lib.helpers import RE, timestamp_to_datetime
 
 
 @functools.total_ordering
diff --git a/tools/lib/cache.py b/tools/lib/cache.py
index 82b2298730..fd214f6bb5 100644
--- a/tools/lib/cache.py
+++ b/tools/lib/cache.py
@@ -1,11 +1,11 @@
 import os
 import urllib.parse
-from common.file_helpers import mkdirs_exists_ok
+from openpilot.common.file_helpers import mkdirs_exists_ok
 
-DEFAULT_CACHE_DIR = os.path.expanduser("~/.commacache")
+DEFAULT_CACHE_DIR = os.getenv("CACHE_ROOT", os.path.expanduser("~/.commacache"))
 
-def cache_path_for_file_path(fn, cache_prefix=None):
-  dir_ = os.path.join(DEFAULT_CACHE_DIR, "local")
+def cache_path_for_file_path(fn, cache_dir=DEFAULT_CACHE_DIR):
+  dir_ = os.path.join(cache_dir, "local")
   mkdirs_exists_ok(dir_)
   fn_parsed = urllib.parse.urlparse(fn)
   if fn_parsed.scheme == '':
diff --git a/tools/lib/filereader.py b/tools/lib/filereader.py
index 215f7b2185..4aec965f1a 100644
--- a/tools/lib/filereader.py
+++ b/tools/lib/filereader.py
@@ -1,11 +1,15 @@
 import os
-from tools.lib.url_file import URLFile
+from openpilot.tools.lib.url_file import URLFile
 
 DATA_ENDPOINT = os.getenv("DATA_ENDPOINT", "http://data-raw.comma.internal/")
 
-def FileReader(fn, debug=False):
+def resolve_name(fn):
   if fn.startswith("cd:/"):
-    fn = fn.replace("cd:/", DATA_ENDPOINT)
-  if fn.startswith("http://") or fn.startswith("https://"):
+    return fn.replace("cd:/", DATA_ENDPOINT)
+  return fn
+
+def FileReader(fn, debug=False):
+  fn = resolve_name(fn)
+  if fn.startswith(("http://", "https://")):
     return URLFile(fn, debug=debug)
   return open(fn, "rb")
diff --git a/tools/lib/framereader.py b/tools/lib/framereader.py
index d9920ab128..275b9b65b8 100644
--- a/tools/lib/framereader.py
+++ b/tools/lib/framereader.py
@@ -1,10 +1,8 @@
-# pylint: skip-file
 import json
 import os
 import pickle
 import struct
 import subprocess
-import tempfile
 import threading
 from enum import IntEnum
 from functools import wraps
@@ -13,11 +11,12 @@ import numpy as np
 from lru import LRU
 
 import _io
-from tools.lib.cache import cache_path_for_file_path
-from tools.lib.exceptions import DataUnreadableError
-from common.file_helpers import atomic_write_in_dir
+from openpilot.tools.lib.cache import cache_path_for_file_path, DEFAULT_CACHE_DIR
+from openpilot.tools.lib.exceptions import DataUnreadableError
+from openpilot.tools.lib.vidindex import hevc_index
+from openpilot.common.file_helpers import atomic_write_in_dir
 
-from tools.lib.filereader import FileReader
+from openpilot.tools.lib.filereader import FileReader, resolve_name
 
 HEVC_SLICE_B = 0
 HEVC_SLICE_P = 1
@@ -60,62 +59,35 @@ def fingerprint_video(fn):
 
 
 def ffprobe(fn, fmt=None):
-  cmd = ["ffprobe",
-         "-v", "quiet",
-         "-print_format", "json",
-         "-show_format", "-show_streams"]
+  fn = resolve_name(fn)
+  cmd = ["ffprobe", "-v", "quiet", "-print_format", "json", "-show_format", "-show_streams"]
   if fmt:
     cmd += ["-f", fmt]
-  cmd += [fn]
+  cmd += ["-i", "-"]
 
   try:
-    ffprobe_output = subprocess.check_output(cmd)
-  except subprocess.CalledProcessError:
-    raise DataUnreadableError(fn)
+    with FileReader(fn) as f:
+      ffprobe_output = subprocess.check_output(cmd, input=f.read(4096))
+  except subprocess.CalledProcessError as e:
+    raise DataUnreadableError(fn) from e
 
   return json.loads(ffprobe_output)
 
 
-def vidindex(fn, typ):
-  vidindex_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), "vidindex")
-  vidindex = os.path.join(vidindex_dir, "vidindex")
-
-  subprocess.check_call(["make"], cwd=vidindex_dir, stdout=open("/dev/null", "w"))
-
-  with tempfile.NamedTemporaryFile() as prefix_f, \
-       tempfile.NamedTemporaryFile() as index_f:
-    try:
-      subprocess.check_call([vidindex, typ, fn, prefix_f.name, index_f.name])
-    except subprocess.CalledProcessError:
-      raise DataUnreadableError(f"vidindex failed on file {fn}")
-    with open(index_f.name, "rb") as f:
-      index = f.read()
-    with open(prefix_f.name, "rb") as f:
-      prefix = f.read()
-
-  index = np.frombuffer(index, np.uint32).reshape(-1, 2)
-
-  assert index[-1, 0] == 0xFFFFFFFF
-  assert index[-1, 1] == os.path.getsize(fn)
-
-  return index, prefix
-
-
 def cache_fn(func):
   @wraps(func)
   def cache_inner(fn, *args, **kwargs):
     if kwargs.pop('no_cache', None):
       cache_path = None
     else:
-      cache_prefix = kwargs.pop('cache_prefix', None)
-      cache_path = cache_path_for_file_path(fn, cache_prefix)
+      cache_dir = kwargs.pop('cache_dir', DEFAULT_CACHE_DIR)
+      cache_path = cache_path_for_file_path(fn, cache_dir)
 
     if cache_path and os.path.exists(cache_path):
       with open(cache_path, "rb") as cache_file:
         cache_value = pickle.load(cache_file)
     else:
       cache_value = func(fn, *args, **kwargs)
-
       if cache_path:
         with atomic_write_in_dir(cache_path, mode="wb", overwrite=True) as cache_file:
           pickle.dump(cache_value, cache_file, -1)
@@ -126,13 +98,13 @@ def cache_fn(func):
 
 
 @cache_fn
-def index_stream(fn, typ):
-  assert typ in ("hevc", )
+def index_stream(fn, ft):
+  if ft != FrameType.h265_stream:
+    raise NotImplementedError("Only h265 supported")
 
-  with FileReader(fn) as f:
-    assert os.path.exists(f.name), fn
-    index, prefix = vidindex(f.name, typ)
-    probe = ffprobe(f.name, typ)
+  frame_types, dat_len, prefix = hevc_index(fn)
+  index = np.array(frame_types + [(0xFFFFFFFF, dat_len)], dtype=np.uint32)
+  probe = ffprobe(fn, "hevc")
 
   return {
     'index': index,
@@ -141,42 +113,8 @@ def index_stream(fn, typ):
   }
 
 
-def index_videos(camera_paths, cache_prefix=None):
-  """Requires that paths in camera_paths are contiguous and of the same type."""
-  if len(camera_paths) < 1:
-    raise ValueError("must provide at least one video to index")
-
-  frame_type = fingerprint_video(camera_paths[0])
-  for fn in camera_paths:
-    index_video(fn, frame_type, cache_prefix)
-
-
-def index_video(fn, frame_type=None, cache_prefix=None):
-  cache_path = cache_path_for_file_path(fn, cache_prefix)
-
-  if os.path.exists(cache_path):
-    return
-
-  if frame_type is None:
-    frame_type = fingerprint_video(fn[0])
-
-  if frame_type == FrameType.h265_stream:
-    index_stream(fn, "hevc", cache_prefix=cache_prefix)
-  else:
-    raise NotImplementedError("Only h265 supported")
-
-
-def get_video_index(fn, frame_type, cache_prefix=None):
-  cache_path = cache_path_for_file_path(fn, cache_prefix)
-
-  if not os.path.exists(cache_path):
-    index_video(fn, frame_type, cache_prefix)
-
-  if not os.path.exists(cache_path):
-    return None
-  with open(cache_path, "rb") as cache_file:
-    return pickle.load(cache_file)
-
+def get_video_index(fn, frame_type, cache_dir=DEFAULT_CACHE_DIR):
+  return index_stream(fn, frame_type, cache_dir=cache_dir)
 
 def read_file_check_size(f, sz, cookie):
   buff = bytearray(sz)
@@ -229,33 +167,21 @@ def rgb24tonv12(rgb):
 
 
 def decompress_video_data(rawdat, vid_fmt, w, h, pix_fmt):
-  # using a tempfile is much faster than proc.communicate for some reason
-
-  with tempfile.TemporaryFile() as tmpf:
-    tmpf.write(rawdat)
-    tmpf.seek(0)
-
-    threads = os.getenv("FFMPEG_THREADS", "0")
-    cuda = os.getenv("FFMPEG_CUDA", "0") == "1"
-    proc = subprocess.Popen(
-      ["ffmpeg",
-       "-threads", threads,
-       "-hwaccel", "none" if not cuda else "cuda",
-       "-c:v", "hevc",
-       "-vsync", "0",
-       "-f", vid_fmt,
-       "-flags2", "showall",
-       "-i", "pipe:0",
-       "-threads", threads,
-       "-f", "rawvideo",
-       "-pix_fmt", pix_fmt,
-       "pipe:1"],
-      stdin=tmpf, stdout=subprocess.PIPE, stderr=open("/dev/null"))
-
-    # dat = proc.communicate()[0]
-    dat = proc.stdout.read()
-    if proc.wait() != 0:
-      raise DataUnreadableError("ffmpeg failed")
+  threads = os.getenv("FFMPEG_THREADS", "0")
+  cuda = os.getenv("FFMPEG_CUDA", "0") == "1"
+  args = ["ffmpeg", "-v", "quiet",
+          "-threads", threads,
+          "-hwaccel", "none" if not cuda else "cuda",
+          "-c:v", "hevc",
+          "-vsync", "0",
+          "-f", vid_fmt,
+          "-flags2", "showall",
+          "-i", "-",
+          "-threads", threads,
+          "-f", "rawvideo",
+          "-pix_fmt", pix_fmt,
+          "-"]
+  dat = subprocess.check_output(args, input=rawdat)
 
   if pix_fmt == "rgb24":
     ret = np.frombuffer(dat, dtype=np.uint8).reshape(-1, h, w, 3)
@@ -287,13 +213,13 @@ class BaseFrameReader:
     raise NotImplementedError
 
 
-def FrameReader(fn, cache_prefix=None, readahead=False, readbehind=False, index_data=None):
+def FrameReader(fn, cache_dir=DEFAULT_CACHE_DIR, readahead=False, readbehind=False, index_data=None):
   frame_type = fingerprint_video(fn)
   if frame_type == FrameType.raw:
     return RawFrameReader(fn)
   elif frame_type in (FrameType.h265_stream,):
     if not index_data:
-      index_data = get_video_index(fn, frame_type, cache_prefix)
+      index_data = get_video_index(fn, frame_type, cache_dir)
     return StreamFrameReader(fn, frame_type, index_data, readahead=readahead, readbehind=readbehind)
   else:
     raise NotImplementedError(frame_type)
@@ -418,7 +344,7 @@ class VideoStreamDecompressor:
         elif self.pix_fmt == "yuv444p":
           ret = np.frombuffer(dat, dtype=np.uint8).reshape((3, self.h, self.w))
         else:
-          assert False
+          raise RuntimeError(f"unknown pix_fmt: {self.pix_fmt}")
         yield ret
 
       result_code = self.proc.wait()
diff --git a/tools/lib/kbhit.py b/tools/lib/kbhit.py
old mode 100644
new mode 100755
diff --git a/tools/lib/logreader.py b/tools/lib/logreader.py
index 46c648ef12..4af922c774 100755
--- a/tools/lib/logreader.py
+++ b/tools/lib/logreader.py
@@ -6,10 +6,13 @@ import urllib.parse
 import capnp
 import warnings
 
+from typing import Iterable, Iterator
 
 from cereal import log as capnp_log
-from tools.lib.filereader import FileReader
-from tools.lib.route import Route, SegmentName
+from openpilot.tools.lib.filereader import FileReader
+from openpilot.tools.lib.route import Route, SegmentName
+
+LogIterable = Iterable[capnp._DynamicStructReader]
 
 # this is an iterator itself, and uses private variables from LogReader
 class MultiLogIterator:
@@ -30,7 +33,7 @@ class MultiLogIterator:
 
     return self._log_readers[i]
 
-  def __iter__(self):
+  def __iter__(self) -> Iterator[capnp._DynamicStructReader]:
     return self
 
   def _inc(self):
@@ -98,7 +101,7 @@ class LogReader:
       for e in ents:
         _ents.append(e)
     except capnp.KjException:
-      warnings.warn("Corrupted events detected", RuntimeWarning)
+      warnings.warn("Corrupted events detected", RuntimeWarning, stacklevel=1)
 
     self._ents = list(sorted(_ents, key=lambda x: x.logMonoTime) if sort_by_time else _ents)
     self._ts = [x.logMonoTime for x in self._ents]
@@ -107,7 +110,7 @@ class LogReader:
   def from_bytes(cls, dat):
     return cls("", dat=dat)
 
-  def __iter__(self):
+  def __iter__(self) -> Iterator[capnp._DynamicStructReader]:
     for ent in self._ents:
       if self._only_union_types:
         try:
diff --git a/tools/lib/route.py b/tools/lib/route.py
index 388cb003e6..e37b7d4434 100644
--- a/tools/lib/route.py
+++ b/tools/lib/route.py
@@ -5,9 +5,9 @@ from collections import defaultdict
 from itertools import chain
 from typing import Optional
 
-from tools.lib.auth_config import get_token
-from tools.lib.api import CommaApi
-from tools.lib.helpers import RE
+from openpilot.tools.lib.auth_config import get_token
+from openpilot.tools.lib.api import CommaApi
+from openpilot.tools.lib.helpers import RE
 
 QLOG_FILENAMES = ['qlog', 'qlog.bz2']
 QCAMERA_FILENAMES = ['qcamera.ts']
diff --git a/tools/lib/tests/test_caching.py b/tools/lib/tests/test_caching.py
old mode 100644
new mode 100755
index d2a2a6872f..73ed843869
--- a/tools/lib/tests/test_caching.py
+++ b/tools/lib/tests/test_caching.py
@@ -1,18 +1,19 @@
 #!/usr/bin/env python3
 import os
-import shutil
 import unittest
 
-os.environ["COMMA_CACHE"] = "/tmp/__test_cache__"
-from tools.lib.url_file import URLFile, CACHE_DIR
+from pathlib import Path
+from parameterized import parameterized
+from unittest import mock
+
+from openpilot.system.hardware.hw import Paths
+from openpilot.tools.lib.url_file import URLFile
 
 
 class TestFileDownload(unittest.TestCase):
 
   def compare_loads(self, url, start=0, length=None):
     """Compares range between cached and non cached version"""
-    shutil.rmtree(CACHE_DIR)
-
     file_cached = URLFile(url, cache=True)
     file_downloaded = URLFile(url, cache=False)
 
@@ -64,6 +65,34 @@ class TestFileDownload(unittest.TestCase):
     self.compare_loads(large_file_url, length - 100, 100)
     self.compare_loads(large_file_url)
 
+  @parameterized.expand([(True, ), (False, )])
+  def test_recover_from_missing_file(self, cache_enabled):
+    os.environ["FILEREADER_CACHE"] = "1" if cache_enabled else "0"
+
+    file_url = "http://localhost:5001/test.png"
+
+    file_exists = False
+
+    def get_length_online_mock(self):
+      if file_exists:
+        return 4
+      return -1
+
+    patch_length = mock.patch.object(URLFile, "get_length_online", get_length_online_mock)
+    patch_length.start()
+    try:
+      length = URLFile(file_url).get_length()
+      self.assertEqual(length, -1)
+
+      file_exists = True
+      length = URLFile(file_url).get_length()
+      self.assertEqual(length, 4)
+    finally:
+      tempfile_length = Path(Paths.download_cache_root()) / "ba2119904385654cb0105a2da174875f8e7648db175f202ecae6d6428b0e838f_length"
+      if tempfile_length.exists():
+        tempfile_length.unlink()
+      patch_length.stop()
+
 
 if __name__ == "__main__":
   unittest.main()
diff --git a/tools/lib/tests/test_readers.py b/tools/lib/tests/test_readers.py
index 6efa0dc351..1f24ae5c8e 100755
--- a/tools/lib/tests/test_readers.py
+++ b/tools/lib/tests/test_readers.py
@@ -5,8 +5,8 @@ import tempfile
 
 from collections import defaultdict
 import numpy as np
-from tools.lib.framereader import FrameReader
-from tools.lib.logreader import LogReader
+from openpilot.tools.lib.framereader import FrameReader
+from openpilot.tools.lib.logreader import LogReader
 
 
 class TestReaders(unittest.TestCase):
diff --git a/tools/lib/tests/test_route_library.py b/tools/lib/tests/test_route_library.py
index fbe7f3e776..7977f17be2 100755
--- a/tools/lib/tests/test_route_library.py
+++ b/tools/lib/tests/test_route_library.py
@@ -2,19 +2,19 @@
 import unittest
 from collections import namedtuple
 
-from tools.lib.route import SegmentName
+from openpilot.tools.lib.route import SegmentName
 
 class TestRouteLibrary(unittest.TestCase):
   def test_segment_name_formats(self):
     Case = namedtuple('Case', ['input', 'expected_route', 'expected_segment_num', 'expected_data_dir'])
 
-    cases = [ Case("4cf7a6ad03080c90|2021-09-29--13-46-36", "4cf7a6ad03080c90|2021-09-29--13-46-36", -1, None),
-              Case("4cf7a6ad03080c90/2021-09-29--13-46-36--1", "4cf7a6ad03080c90|2021-09-29--13-46-36", 1, None),
-              Case("4cf7a6ad03080c90|2021-09-29--13-46-36/2", "4cf7a6ad03080c90|2021-09-29--13-46-36", 2, None),
-              Case("4cf7a6ad03080c90/2021-09-29--13-46-36/3", "4cf7a6ad03080c90|2021-09-29--13-46-36", 3, None),
-              Case("/data/media/0/realdata/4cf7a6ad03080c90|2021-09-29--13-46-36", "4cf7a6ad03080c90|2021-09-29--13-46-36", -1, "/data/media/0/realdata"),
-              Case("/data/media/0/realdata/4cf7a6ad03080c90|2021-09-29--13-46-36--1", "4cf7a6ad03080c90|2021-09-29--13-46-36", 1, "/data/media/0/realdata"),
-              Case("/data/media/0/realdata/4cf7a6ad03080c90|2021-09-29--13-46-36/2", "4cf7a6ad03080c90|2021-09-29--13-46-36", 2, "/data/media/0/realdata") ]
+    cases = [ Case("a2a0ccea32023010|2023-07-27--13-01-19", "a2a0ccea32023010|2023-07-27--13-01-19", -1, None),
+              Case("a2a0ccea32023010/2023-07-27--13-01-19--1", "a2a0ccea32023010|2023-07-27--13-01-19", 1, None),
+              Case("a2a0ccea32023010|2023-07-27--13-01-19/2", "a2a0ccea32023010|2023-07-27--13-01-19", 2, None),
+              Case("a2a0ccea32023010/2023-07-27--13-01-19/3", "a2a0ccea32023010|2023-07-27--13-01-19", 3, None),
+              Case("/data/media/0/realdata/a2a0ccea32023010|2023-07-27--13-01-19", "a2a0ccea32023010|2023-07-27--13-01-19", -1, "/data/media/0/realdata"),
+              Case("/data/media/0/realdata/a2a0ccea32023010|2023-07-27--13-01-19--1", "a2a0ccea32023010|2023-07-27--13-01-19", 1, "/data/media/0/realdata"),
+              Case("/data/media/0/realdata/a2a0ccea32023010|2023-07-27--13-01-19/2", "a2a0ccea32023010|2023-07-27--13-01-19", 2, "/data/media/0/realdata") ]
 
     def _validate(case):
       route_or_segment_name = case.input
diff --git a/tools/lib/url_file.py b/tools/lib/url_file.py
index 1e94a588f1..315ade514b 100644
--- a/tools/lib/url_file.py
+++ b/tools/lib/url_file.py
@@ -1,27 +1,26 @@
-# pylint: skip-file
-
 import os
 import time
-import tempfile
 import threading
-import urllib.parse
 import pycurl
 from hashlib import sha256
 from io import BytesIO
 from tenacity import retry, wait_random_exponential, stop_after_attempt
-from common.file_helpers import mkdirs_exists_ok, atomic_write_in_dir
+from openpilot.common.file_helpers import mkdirs_exists_ok, atomic_write_in_dir
+from openpilot.system.hardware.hw import Paths
 #  Cache chunk size
 K = 1000
 CHUNK_SIZE = 1000 * K
 
-CACHE_DIR = os.environ.get("COMMA_CACHE", "/tmp/comma_download_cache/")
-
 
 def hash_256(link):
   hsh = str(sha256((link.split("?")[0]).encode('utf-8')).hexdigest())
   return hsh
 
 
+class URLFileException(Exception):
+  pass
+
+
 class URLFile:
   _tlocal = threading.local()
 
@@ -40,7 +39,8 @@ class URLFile:
       self._curl = self._tlocal.curl
     except AttributeError:
       self._curl = self._tlocal.curl = pycurl.Curl()
-    mkdirs_exists_ok(CACHE_DIR)
+    if not self._force_download:
+      mkdirs_exists_ok(Paths.download_cache_root())
 
   def __enter__(self):
     return self
@@ -68,15 +68,16 @@ class URLFile:
   def get_length(self):
     if self._length is not None:
       return self._length
-    file_length_path = os.path.join(CACHE_DIR, hash_256(self._url) + "_length")
-    if os.path.exists(file_length_path) and not self._force_download:
+
+    file_length_path = os.path.join(Paths.download_cache_root(), hash_256(self._url) + "_length")
+    if not self._force_download and os.path.exists(file_length_path):
       with open(file_length_path) as file_length:
-          content = file_length.read()
-          self._length = int(content)
-          return self._length
+        content = file_length.read()
+        self._length = int(content)
+        return self._length
 
     self._length = self.get_length_online()
-    if not self._force_download:
+    if not self._force_download and self._length != -1:
       with atomic_write_in_dir(file_length_path, mode="w") as file_length:
         file_length.write(str(self._length))
     return self._length
@@ -95,7 +96,7 @@ class URLFile:
       self._pos = position
       chunk_number = self._pos / CHUNK_SIZE
       file_name = hash_256(self._url) + "_" + str(chunk_number)
-      full_path = os.path.join(CACHE_DIR, str(file_name))
+      full_path = os.path.join(Paths.download_cache_root(), str(file_name))
       data = None
       #  If we don't have a file, download it
       if not os.path.exists(full_path):
@@ -161,11 +162,11 @@ class URLFile:
 
     response_code = c.getinfo(pycurl.RESPONSE_CODE)
     if response_code == 416:  # Requested Range Not Satisfiable
-      raise Exception(f"Error, range out of bounds {response_code} {headers} ({self._url}): {repr(dats.getvalue())[:500]}")
+      raise URLFileException(f"Error, range out of bounds {response_code} {headers} ({self._url}): {repr(dats.getvalue())[:500]}")
     if download_range and response_code != 206:  # Partial Content
-      raise Exception(f"Error, requested range but got unexpected response {response_code} {headers} ({self._url}): {repr(dats.getvalue())[:500]}")
+      raise URLFileException(f"Error, requested range but got unexpected response {response_code} {headers} ({self._url}): {repr(dats.getvalue())[:500]}")
     if (not download_range) and response_code != 200:  # OK
-      raise Exception(f"Error {response_code} {headers} ({self._url}): {repr(dats.getvalue())[:500]}")
+      raise URLFileException(f"Error {response_code} {headers} ({self._url}): {repr(dats.getvalue())[:500]}")
 
     ret = dats.getvalue()
     self._pos += len(ret)
@@ -176,24 +177,4 @@ class URLFile:
 
   @property
   def name(self):
-    """Returns a local path to file with the URLFile's contents.
-
-       This can be used to interface with modules that require local files.
-    """
-    if self._local_file is None:
-      _, ext = os.path.splitext(urllib.parse.urlparse(self._url).path)
-      local_fd, local_path = tempfile.mkstemp(suffix=ext)
-      try:
-        os.write(local_fd, self.read())
-        local_file = open(local_path, "rb")
-      except Exception:
-        os.remove(local_path)
-        raise
-      finally:
-        os.close(local_fd)
-
-      self._local_file = local_file
-      self.read = self._local_file.read
-      self.seek = self._local_file.seek
-
-    return self._local_file.name
+    return self._url
diff --git a/tools/lib/vidindex.py b/tools/lib/vidindex.py
new file mode 100755
index 0000000000..8156faba6b
--- /dev/null
+++ b/tools/lib/vidindex.py
@@ -0,0 +1,312 @@
+#!/usr/bin/env python3
+import argparse
+import os
+import struct
+from enum import IntEnum
+from typing import Tuple
+
+from openpilot.tools.lib.filereader import FileReader
+
+DEBUG = int(os.getenv("DEBUG", "0"))
+
+# compare to ffmpeg parsing
+# ffmpeg -i <input.hevc> -c copy -bsf:v trace_headers -f null - 2>&1 | grep -B4 -A32 '] 0 '
+
+# H.265 specification
+# https://www.itu.int/rec/dologin_pub.asp?lang=e&id=T-REC-H.265-201802-S!!PDF-E&type=items
+
+NAL_UNIT_START_CODE = b"\x00\x00\x01"
+NAL_UNIT_START_CODE_SIZE = len(NAL_UNIT_START_CODE)
+NAL_UNIT_HEADER_SIZE = 2
+
+class HevcNalUnitType(IntEnum):
+  TRAIL_N = 0         # RBSP structure: slice_segment_layer_rbsp( )
+  TRAIL_R = 1         # RBSP structure: slice_segment_layer_rbsp( )
+  TSA_N = 2           # RBSP structure: slice_segment_layer_rbsp( )
+  TSA_R = 3           # RBSP structure: slice_segment_layer_rbsp( )
+  STSA_N = 4          # RBSP structure: slice_segment_layer_rbsp( )
+  STSA_R = 5          # RBSP structure: slice_segment_layer_rbsp( )
+  RADL_N = 6          # RBSP structure: slice_segment_layer_rbsp( )
+  RADL_R = 7          # RBSP structure: slice_segment_layer_rbsp( )
+  RASL_N = 8          # RBSP structure: slice_segment_layer_rbsp( )
+  RASL_R = 9          # RBSP structure: slice_segment_layer_rbsp( )
+  RSV_VCL_N10 = 10
+  RSV_VCL_R11 = 11
+  RSV_VCL_N12 = 12
+  RSV_VCL_R13 = 13
+  RSV_VCL_N14 = 14
+  RSV_VCL_R15 = 15
+  BLA_W_LP = 16       # RBSP structure: slice_segment_layer_rbsp( )
+  BLA_W_RADL = 17     # RBSP structure: slice_segment_layer_rbsp( )
+  BLA_N_LP = 18       # RBSP structure: slice_segment_layer_rbsp( )
+  IDR_W_RADL = 19     # RBSP structure: slice_segment_layer_rbsp( )
+  IDR_N_LP = 20       # RBSP structure: slice_segment_layer_rbsp( )
+  CRA_NUT = 21        # RBSP structure: slice_segment_layer_rbsp( )
+  RSV_IRAP_VCL22 = 22
+  RSV_IRAP_VCL23 = 23
+  RSV_VCL24 = 24
+  RSV_VCL25 = 25
+  RSV_VCL26 = 26
+  RSV_VCL27 = 27
+  RSV_VCL28 = 28
+  RSV_VCL29 = 29
+  RSV_VCL30 = 30
+  RSV_VCL31 = 31
+  VPS_NUT = 32        # RBSP structure: video_parameter_set_rbsp( )
+  SPS_NUT = 33        # RBSP structure: seq_parameter_set_rbsp( )
+  PPS_NUT = 34        # RBSP structure: pic_parameter_set_rbsp( )
+  AUD_NUT = 35
+  EOS_NUT = 36
+  EOB_NUT = 37
+  FD_NUT = 38
+  PREFIX_SEI_NUT = 39
+  SUFFIX_SEI_NUT = 40
+  RSV_NVCL41 = 41
+  RSV_NVCL42 = 42
+  RSV_NVCL43 = 43
+  RSV_NVCL44 = 44
+  RSV_NVCL45 = 45
+  RSV_NVCL46 = 46
+  RSV_NVCL47 = 47
+  UNSPEC48 = 48
+  UNSPEC49 = 49
+  UNSPEC50 = 50
+  UNSPEC51 = 51
+  UNSPEC52 = 52
+  UNSPEC53 = 53
+  UNSPEC54 = 54
+  UNSPEC55 = 55
+  UNSPEC56 = 56
+  UNSPEC57 = 57
+  UNSPEC58 = 58
+  UNSPEC59 = 59
+  UNSPEC60 = 60
+  UNSPEC61 = 61
+  UNSPEC62 = 62
+  UNSPEC63 = 63
+
+# B.2.2 Byte stream NAL unit semantics
+# - The nal_unit_type within the nal_unit( ) syntax structure is equal to VPS_NUT, SPS_NUT or PPS_NUT.
+# - The byte stream NAL unit syntax structure contains the first NAL unit of an access unit in decoding
+#   order, as specified in clause 7.4.2.4.4.
+HEVC_PARAMETER_SET_NAL_UNITS = (
+  HevcNalUnitType.VPS_NUT,
+  HevcNalUnitType.SPS_NUT,
+  HevcNalUnitType.PPS_NUT,
+)
+
+# 3.29 coded slice segment NAL unit: A NAL unit that has nal_unit_type in the range of TRAIL_N to RASL_R,
+# inclusive, or in the range of BLA_W_LP to RSV_IRAP_VCL23, inclusive, which indicates that the NAL unit
+# contains a coded slice segment
+HEVC_CODED_SLICE_SEGMENT_NAL_UNITS = (
+  HevcNalUnitType.TRAIL_N,
+  HevcNalUnitType.TRAIL_R,
+  HevcNalUnitType.TSA_N,
+  HevcNalUnitType.TSA_R,
+  HevcNalUnitType.STSA_N,
+  HevcNalUnitType.STSA_R,
+  HevcNalUnitType.RADL_N,
+  HevcNalUnitType.RADL_R,
+  HevcNalUnitType.RASL_N,
+  HevcNalUnitType.RASL_R,
+  HevcNalUnitType.BLA_W_LP,
+  HevcNalUnitType.BLA_W_RADL,
+  HevcNalUnitType.BLA_N_LP,
+  HevcNalUnitType.IDR_W_RADL,
+  HevcNalUnitType.IDR_N_LP,
+  HevcNalUnitType.CRA_NUT,
+)
+
+class VideoFileInvalid(Exception):
+  pass
+
+def get_ue(dat: bytes, start_idx: int, skip_bits: int) -> Tuple[int, int]:
+  prefix_val = 0
+  prefix_len = 0
+  suffix_val = 0
+  suffix_len = 0
+
+  i = start_idx
+  while i < len(dat):
+    j = 7
+    while j >= 0:
+      if skip_bits > 0:
+        skip_bits -= 1
+      elif prefix_val == 0:
+        prefix_val = (dat[i] >> j) & 1
+        prefix_len += 1
+      else:
+        suffix_val = (suffix_val << 1) | ((dat[i] >> j) & 1)
+        suffix_len += 1
+      j -= 1
+
+      if prefix_val == 1 and prefix_len - 1 == suffix_len:
+        val = 2**(prefix_len-1) - 1 + suffix_val
+        size = prefix_len + suffix_len
+        return val, size
+    i += 1
+
+  raise VideoFileInvalid("invalid exponential-golomb code")
+
+def require_nal_unit_start(dat: bytes, nal_unit_start: int) -> None:
+  if nal_unit_start < 1:
+    raise ValueError("start index must be greater than zero")
+
+  if dat[nal_unit_start:nal_unit_start + NAL_UNIT_START_CODE_SIZE] != NAL_UNIT_START_CODE:
+    raise VideoFileInvalid("data must begin with start code")
+
+def get_hevc_nal_unit_length(dat: bytes, nal_unit_start: int) -> int:
+  try:
+    pos = dat.index(NAL_UNIT_START_CODE, nal_unit_start + NAL_UNIT_START_CODE_SIZE)
+  except ValueError:
+    pos = -1
+
+  # length of NAL unit is byte count up to next NAL unit start index
+  nal_unit_len = (pos if pos != -1 else len(dat)) - nal_unit_start
+  if DEBUG:
+    print("  nal_unit_len:", nal_unit_len)
+  return nal_unit_len
+
+def get_hevc_nal_unit_type(dat: bytes, nal_unit_start: int) -> HevcNalUnitType:
+  # 7.3.1.2 NAL unit header syntax
+  # nal_unit_header( ) {    // descriptor
+  #   forbidden_zero_bit    f(1)
+  #   nal_unit_type         u(6)
+  #   nuh_layer_id          u(6)
+  #   nuh_temporal_id_plus1 u(3)
+  # }
+  header_start = nal_unit_start + NAL_UNIT_START_CODE_SIZE
+  nal_unit_header = dat[header_start:header_start + NAL_UNIT_HEADER_SIZE]
+  if len(nal_unit_header) != 2:
+    raise VideoFileInvalid("data to short to contain nal unit header")
+  nal_unit_type = HevcNalUnitType((nal_unit_header[0] >> 1) & 0x3F)
+  if DEBUG:
+    print("  nal_unit_type:", nal_unit_type.name, f"({nal_unit_type.value})")
+  return nal_unit_type
+
+def get_hevc_slice_type(dat: bytes, nal_unit_start: int, nal_unit_type: HevcNalUnitType) -> Tuple[int, bool]:
+  # 7.3.2.9 Slice segment layer RBSP syntax
+  # slice_segment_layer_rbsp( ) {
+  #   slice_segment_header( )
+  #   slice_segment_data( )
+  #   rbsp_slice_segment_trailing_bits( )
+  # }
+  # ...
+  # 7.3.6.1 General slice segment header syntax
+  # slice_segment_header( ) {                                             // descriptor
+  #   first_slice_segment_in_pic_flag                                     u(1)
+  #   if( nal_unit_type >= BLA_W_LP && nal_unit_type <= RSV_IRAP_VCL23 )
+  #     no_output_of_prior_pics_flag                                      u(1)
+  #   slice_pic_parameter_set_id                                         ue(v)
+  #   if( !first_slice_segment_in_pic_flag ) {
+  #     if( dependent_slice_segments_enabled_flag )
+  #       dependent_slice_segment_flag                                    u(1)
+  #     slice_segment_address                                             u(v)
+  #   }
+  #   if( !dependent_slice_segment_flag ) {
+  #     for( i = 0; i < num_extra_slice_header_bits; i++ )
+  #       slice_reserved_flag[ i ]                                        u(1)
+  #     slice_type                                                       ue(v)
+  # ...
+
+  rbsp_start = nal_unit_start + NAL_UNIT_START_CODE_SIZE + NAL_UNIT_HEADER_SIZE
+  skip_bits = 0
+
+  # 7.4.7.1 General slice segment header semantics
+  # first_slice_segment_in_pic_flag equal to 1 specifies that the slice segment is the first slice segment of the picture in
+  # decoding order. first_slice_segment_in_pic_flag equal to 0 specifies that the slice segment is not the first slice segment
+  # of the picture in decoding order.
+  is_first_slice = dat[rbsp_start] >> 7 & 1 == 1
+  if not is_first_slice:
+    # TODO: parse dependent_slice_segment_flag and slice_segment_address and get real slice_type
+    # for now since we don't use it return -1 for slice_type
+    return (-1, is_first_slice)
+  skip_bits += 1 # skip past first_slice_segment_in_pic_flag
+
+  if nal_unit_type >= HevcNalUnitType.BLA_W_LP and nal_unit_type <= HevcNalUnitType.RSV_IRAP_VCL23:
+    # 7.4.7.1 General slice segment header semantics
+    # no_output_of_prior_pics_flag affects the output of previously-decoded pictures in the decoded picture buffer after the
+    # decoding of an IDR or a BLA picture that is not the first picture in the bitstream as specified in Annex C.
+    skip_bits += 1 # skip past no_output_of_prior_pics_flag
+
+  # 7.4.7.1 General slice segment header semantics
+  # slice_pic_parameter_set_id specifies the value of pps_pic_parameter_set_id for the PPS in use.
+  # The value of slice_pic_parameter_set_id shall be in the range of 0 to 63, inclusive.
+  _, size = get_ue(dat, rbsp_start, skip_bits)
+  skip_bits += size # skip past slice_pic_parameter_set_id
+
+  # 7.4.3.3.1 General picture parameter set RBSP semanal_unit_lenntics
+  # num_extra_slice_header_bits specifies the number of extra slice header bits that are present in the slice header RBSP
+  # for coded pictures referring to the PPS. The value of num_extra_slice_header_bits shall be in the range of 0 to 2, inclusive,
+  # in bitstreams conforming to this version of this Specification. Other values for num_extra_slice_header_bits are reserved
+  # for future use by ITU-T | ISO/IEC. However, decoders shall allow num_extra_slice_header_bits to have any value.
+  # TODO: get from PPS_NUT pic_parameter_set_rbsp( ) for corresponding slice_pic_parameter_set_id
+  num_extra_slice_header_bits = 0
+  skip_bits += num_extra_slice_header_bits
+
+  # 7.4.7.1 General slice segment header semantics
+  # slice_type specifies the coding type of the slice according to Table 7-7.
+  # Table 7-7 - Name association to slice_type
+  # slice_type | Name of slice_type
+  #     0      | B (B slice)
+  #     1      | P (P slice)
+  #     2      | I (I slice)
+  # unsigned integer 0-th order Exp-Golomb-coded syntax element with the left bit first
+  slice_type, _ = get_ue(dat, rbsp_start, skip_bits)
+  if DEBUG:
+    print("  slice_type:", slice_type, f"(first slice: {is_first_slice})")
+  if slice_type > 2:
+    raise VideoFileInvalid("slice_type must be 0, 1, or 2")
+  return slice_type, is_first_slice
+
+def hevc_index(hevc_file_name: str, allow_corrupt: bool=False) -> Tuple[list, int, bytes]:
+  with FileReader(hevc_file_name) as f:
+    dat = f.read()
+
+  if len(dat) < NAL_UNIT_START_CODE_SIZE + 1:
+    raise VideoFileInvalid("data is too short")
+
+  if dat[0] != 0x00:
+    raise VideoFileInvalid("first byte must be 0x00")
+
+  prefix_dat = b""
+  frame_types = list()
+
+  i = 1 # skip past first byte 0x00
+  try:
+    while i < len(dat):
+      require_nal_unit_start(dat, i)
+      nal_unit_len = get_hevc_nal_unit_length(dat, i)
+      nal_unit_type = get_hevc_nal_unit_type(dat, i)
+      if nal_unit_type in HEVC_PARAMETER_SET_NAL_UNITS:
+        prefix_dat += dat[i:i+nal_unit_len]
+      elif nal_unit_type in HEVC_CODED_SLICE_SEGMENT_NAL_UNITS:
+        slice_type, is_first_slice = get_hevc_slice_type(dat, i, nal_unit_type)
+        if is_first_slice:
+          frame_types.append((slice_type, i))
+      i += nal_unit_len
+  except Exception as e:
+    if not allow_corrupt:
+      raise
+    print(f"ERROR: NAL unit skipped @ {i}\n", str(e))
+
+  return frame_types, len(dat), prefix_dat
+
+def main() -> None:
+  parser = argparse.ArgumentParser()
+  parser.add_argument("input_file", type=str)
+  parser.add_argument("output_prefix_file", type=str)
+  parser.add_argument("output_index_file", type=str)
+  args = parser.parse_args()
+
+  frame_types, dat_len, prefix_dat = hevc_index(args.input_file)
+  with open(args.output_prefix_file, "wb") as f:
+    f.write(prefix_dat)
+
+  with open(args.output_index_file, "wb") as f:
+    for ft, fp in frame_types:
+      f.write(struct.pack("<II", ft, fp))
+    f.write(struct.pack("<II", 0xFFFFFFFF, dat_len))
+
+if __name__ == "__main__":
+  main()
diff --git a/tools/lib/vidindex/.gitignore b/tools/lib/vidindex/.gitignore
deleted file mode 100644
index a77a06e97d..0000000000
--- a/tools/lib/vidindex/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-vidindex
diff --git a/tools/lib/vidindex/Makefile b/tools/lib/vidindex/Makefile
deleted file mode 100644
index f6526db212..0000000000
--- a/tools/lib/vidindex/Makefile
+++ /dev/null
@@ -1,6 +0,0 @@
-CC := gcc
-
-vidindex: bitstream.c bitstream.h vidindex.c
-	$(eval $@_TMP := $(shell mktemp))
-	$(CC) -std=c99 bitstream.c vidindex.c -o $($@_TMP)
-	mv $($@_TMP) $@
diff --git a/tools/lib/vidindex/bitstream.c b/tools/lib/vidindex/bitstream.c
deleted file mode 100644
index d174ffa8a7..0000000000
--- a/tools/lib/vidindex/bitstream.c
+++ /dev/null
@@ -1,118 +0,0 @@
-#include <stdbool.h>
-#include <assert.h>
-
-#include "bitstream.h"
-
-static const uint32_t BS_MASKS[33] = {
-    0,           0x1L,        0x3L,       0x7L,       0xFL,       0x1FL,
-    0x3FL,       0x7FL,       0xFFL,      0x1FFL,     0x3FFL,     0x7FFL,
-    0xFFFL,      0x1FFFL,     0x3FFFL,    0x7FFFL,    0xFFFFL,    0x1FFFFL,
-    0x3FFFFL,    0x7FFFFL,    0xFFFFFL,   0x1FFFFFL,  0x3FFFFFL,  0x7FFFFFL,
-    0xFFFFFFL,   0x1FFFFFFL,  0x3FFFFFFL, 0x7FFFFFFL, 0xFFFFFFFL, 0x1FFFFFFFL,
-    0x3FFFFFFFL, 0x7FFFFFFFL, 0xFFFFFFFFL};
-
-void bs_init(struct bitstream* bs, const uint8_t* buffer, size_t input_size) {
-  bs->buffer_ptr = buffer;
-  bs->buffer_end = buffer + input_size;
-  bs->value = 0;
-  bs->pos = 0;
-  bs->shift = 8;
-  bs->size = input_size * 8;
-}
-
-uint32_t bs_get(struct bitstream* bs, int n) {
-  if (n > 32)
-    return 0;
-
-  bs->pos += n;
-  bs->shift += n;
-  while (bs->shift > 8) {
-    if (bs->buffer_ptr < bs->buffer_end) {
-      bs->value <<= 8;
-      bs->value |= *bs->buffer_ptr++;
-      bs->shift -= 8;
-    } else {
-      bs_seek(bs, bs->pos - n);
-      return 0;
-      // bs->value <<= 8;
-      // bs->shift -= 8;
-    }
-  }
-  return (bs->value >> (8 - bs->shift)) & BS_MASKS[n];
-}
-
-void bs_seek(struct bitstream* bs, size_t new_pos) {
-  bs->pos = (new_pos / 32) * 32;
-  bs->shift = 8;
-  bs->value = 0;
-  bs_get(bs, new_pos % 32);
-}
-
-uint32_t bs_peek(struct bitstream* bs, int n) {
-  struct bitstream bak = *bs;
-  return bs_get(&bak, n);
-}
-
-size_t bs_remain(struct bitstream* bs) {
-  return bs->size - bs->pos;
-}
-
-int bs_eof(struct bitstream* bs) {
-  return bs_remain(bs) == 0;
-}
-
-uint32_t bs_ue(struct bitstream* bs) {
-  static const uint8_t exp_golomb_bits[256] = {
-      8, 7, 6, 6, 5, 5, 5, 5, 4, 4, 4, 4, 4, 4, 4, 4, 3, 3, 3, 3, 3, 3, 3, 3,
-      3, 3, 3, 3, 3, 3, 3, 3, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
-      2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, 1,
-      1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
-      1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
-      1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-  };
-  uint32_t bits, read = 0;
-  int bits_left;
-  uint8_t coded;
-  int done = 0;
-  bits = 0;
-  // we want to read 8 bits at a time - if we don't have 8 bits,
-  // read what's left, and shift.  The exp_golomb_bits calc remains the
-  // same.
-  while (!done) {
-    bits_left = bs_remain(bs);
-    if (bits_left < 8) {
-      read = bs_peek(bs, bits_left) << (8 - bits_left);
-      done = 1;
-    } else {
-      read = bs_peek(bs, 8);
-      if (read == 0) {
-        bs_get(bs, 8);
-        bits += 8;
-      } else {
-        done = 1;
-      }
-    }
-  }
-  coded = exp_golomb_bits[read];
-  bs_get(bs, coded);
-  bits += coded;
-
-  //  printf("ue - bits %d\n", bits);
-  return bs_get(bs, bits + 1) - 1;
-}
-
-int32_t bs_se(struct bitstream* bs) {
-  uint32_t ret;
-  ret = bs_ue(bs);
-  if ((ret & 0x1) == 0) {
-    ret >>= 1;
-    int32_t temp = 0 - ret;
-    return temp;
-  }
-  return (ret + 1) >> 1;
-}
diff --git a/tools/lib/vidindex/bitstream.h b/tools/lib/vidindex/bitstream.h
deleted file mode 100644
index 0f538a59ab..0000000000
--- a/tools/lib/vidindex/bitstream.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#ifndef bitstream_H
-#define bitstream_H
-
-
-#include <stddef.h>
-#include <stdint.h>
-
-struct bitstream {
-  const uint8_t *buffer_ptr;
-  const uint8_t *buffer_end;
-  uint64_t value;
-  uint32_t pos;
-  uint32_t shift;
-  size_t size;
-};
-
-void bs_init(struct bitstream *bs, const uint8_t *buffer, size_t input_size);
-void bs_seek(struct bitstream *bs, size_t new_pos);
-uint32_t bs_get(struct bitstream *bs, int n);
-uint32_t bs_peek(struct bitstream *bs, int n);
-size_t bs_remain(struct bitstream *bs);
-int bs_eof(struct bitstream *bs);
-uint32_t bs_ue(struct bitstream *bs);
-int32_t bs_se(struct bitstream *bs);
-
-#endif
diff --git a/tools/lib/vidindex/vidindex.c b/tools/lib/vidindex/vidindex.c
deleted file mode 100644
index 4857c60dd2..0000000000
--- a/tools/lib/vidindex/vidindex.c
+++ /dev/null
@@ -1,307 +0,0 @@
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdbool.h>
-#include <string.h>
-#include <assert.h>
-
-#include <unistd.h>
-#include <fcntl.h>
-#include <sys/stat.h>
-#include <sys/mman.h>
-
-#include "bitstream.h"
-
-#define START_CODE 0x000001
-
-static uint32_t read24be(const uint8_t* ptr) {
-    return (ptr[0] << 16) | (ptr[1] << 8) | ptr[2];
-}
-static void write32le(FILE *of, uint32_t v) {
-  uint8_t va[4] = {
-    v & 0xff, (v >> 8) & 0xff, (v >> 16) & 0xff, (v >> 24) & 0xff
-  };
-  fwrite(va, 1, sizeof(va), of);
-}
-
-// Table 7-1
-enum hevc_nal_type {
-  HEVC_NAL_TYPE_TRAIL_N = 0,
-  HEVC_NAL_TYPE_TRAIL_R = 1,
-  HEVC_NAL_TYPE_TSA_N = 2,
-  HEVC_NAL_TYPE_TSA_R = 3,
-  HEVC_NAL_TYPE_STSA_N = 4,
-  HEVC_NAL_TYPE_STSA_R = 5,
-  HEVC_NAL_TYPE_RADL_N = 6,
-  HEVC_NAL_TYPE_RADL_R = 7,
-  HEVC_NAL_TYPE_RASL_N = 8,
-  HEVC_NAL_TYPE_RASL_R = 9,
-  HEVC_NAL_TYPE_BLA_W_LP = 16,
-  HEVC_NAL_TYPE_BLA_W_RADL = 17,
-  HEVC_NAL_TYPE_BLA_N_LP = 18,
-  HEVC_NAL_TYPE_IDR_W_RADL = 19,
-  HEVC_NAL_TYPE_IDR_N_LP = 20,
-  HEVC_NAL_TYPE_CRA_NUT = 21,
-  HEVC_NAL_TYPE_RSV_IRAP_VCL23 = 23,
-  HEVC_NAL_TYPE_VPS_NUT = 32,
-  HEVC_NAL_TYPE_SPS_NUT = 33,
-  HEVC_NAL_TYPE_PPS_NUT = 34,
-  HEVC_NAL_TYPE_AUD_NUT = 35,
-  HEVC_NAL_TYPE_EOS_NUT = 36,
-  HEVC_NAL_TYPE_EOB_NUT = 37,
-  HEVC_NAL_TYPE_FD_NUT = 38,
-  HEVC_NAL_TYPE_PREFIX_SEI_NUT = 39,
-  HEVC_NAL_TYPE_SUFFIX_SEI_NUT = 40,
-};
-
-// Table 7-7
-enum hevc_slice_type {
-  HEVC_SLICE_B = 0,
-  HEVC_SLICE_P = 1,
-  HEVC_SLICE_I = 2,
-};
-
-static void hevc_index(const uint8_t *data, size_t file_size, FILE *of_prefix, FILE *of_index) {
-  const uint8_t* ptr = data;
-  const uint8_t* ptr_end = data + file_size;
-
-  assert(ptr[0] == 0);
-  ptr++;
-  assert(read24be(ptr) == START_CODE);
-
-  // pps. ignore for now
-  uint32_t num_extra_slice_header_bits = 0;
-  uint32_t dependent_slice_segments_enabled_flag = 0;
-
-  while (ptr < ptr_end) {
-    const uint8_t* next = ptr+1;
-    for (; next < ptr_end-4; next++) {
-      if (read24be(next) == START_CODE) break;
-    }
-    size_t nal_size = next - ptr;
-    if (nal_size < 6) {
-      break;
-    }
-
-    {
-      struct bitstream bs = {0};
-      bs_init(&bs, ptr, nal_size);
-
-      uint32_t start_code = bs_get(&bs, 24);
-      assert(start_code == 0x000001);
-
-      // nal_unit_header
-      uint32_t forbidden_zero_bit = bs_get(&bs, 1);
-      uint32_t nal_unit_type = bs_get(&bs, 6);
-      uint32_t nuh_layer_id = bs_get(&bs, 6);
-      uint32_t nuh_temporal_id_plus1 = bs_get(&bs, 3);
-
-      // if (nal_unit_type != 1) printf("%3d -- %3d %10d %lu\n", nal_unit_type, frame_num, (uint32_t)(ptr-data), nal_size);
-
-      switch (nal_unit_type) {
-      case HEVC_NAL_TYPE_VPS_NUT:
-      case HEVC_NAL_TYPE_SPS_NUT:
-      case HEVC_NAL_TYPE_PPS_NUT:
-        fwrite(ptr, 1, nal_size, of_prefix);
-        break;
-      case HEVC_NAL_TYPE_TRAIL_N:
-      case HEVC_NAL_TYPE_TRAIL_R:
-      case HEVC_NAL_TYPE_TSA_N:
-      case HEVC_NAL_TYPE_TSA_R:
-      case HEVC_NAL_TYPE_STSA_N:
-      case HEVC_NAL_TYPE_STSA_R:
-      case HEVC_NAL_TYPE_RADL_N:
-      case HEVC_NAL_TYPE_RADL_R:
-      case HEVC_NAL_TYPE_RASL_N:
-      case HEVC_NAL_TYPE_RASL_R:
-      case HEVC_NAL_TYPE_BLA_W_LP:
-      case HEVC_NAL_TYPE_BLA_W_RADL:
-      case HEVC_NAL_TYPE_BLA_N_LP:
-      case HEVC_NAL_TYPE_IDR_W_RADL:
-      case HEVC_NAL_TYPE_IDR_N_LP:
-      case HEVC_NAL_TYPE_CRA_NUT: {
-        // slice_segment_header
-        uint32_t first_slice_segment_in_pic_flag = bs_get(&bs, 1);
-        if (nal_unit_type >= HEVC_NAL_TYPE_BLA_W_LP && nal_unit_type <= HEVC_NAL_TYPE_RSV_IRAP_VCL23) {
-          uint32_t no_output_of_prior_pics_flag = bs_get(&bs, 1);
-        }
-        uint32_t slice_pic_parameter_set_id = bs_get(&bs, 1);
-        if (!first_slice_segment_in_pic_flag) {
-          // ...
-          break;
-        }
-
-        if (!dependent_slice_segments_enabled_flag) {
-          for (int i=0; i<num_extra_slice_header_bits; i++) {
-            bs_get(&bs, 1);
-          }
-          uint32_t slice_type = bs_ue(&bs);
-
-          // write the index
-          write32le(of_index, slice_type);
-          write32le(of_index, ptr - data);
-
-          // ...
-        }
-
-        break;
-      }
-      }
-
-      //...
-      // emulation_prevention_three_byte
-    }
-
-    ptr = next;
-  }
-
-  write32le(of_index, -1);
-  write32le(of_index, file_size);
-}
-
-// Table 7-1
-enum h264_nal_type {
-  H264_NAL_SLICE           = 1,
-  H264_NAL_DPA             = 2,
-  H264_NAL_DPB             = 3,
-  H264_NAL_DPC             = 4,
-  H264_NAL_IDR_SLICE       = 5,
-  H264_NAL_SEI             = 6,
-  H264_NAL_SPS             = 7,
-  H264_NAL_PPS             = 8,
-  H264_NAL_AUD             = 9,
-  H264_NAL_END_SEQUENCE    = 10,
-  H264_NAL_END_STREAM      = 11,
-  H264_NAL_FILLER_DATA     = 12,
-  H264_NAL_SPS_EXT         = 13,
-  H264_NAL_AUXILIARY_SLICE = 19,
-};
-
-enum h264_slice_type {
-  H264_SLICE_P = 0,
-  H264_SLICE_B = 1,
-  H264_SLICE_I = 2,
-  // ...
-};
-
-static void h264_index(const uint8_t *data, size_t file_size, FILE *of_prefix, FILE *of_index) {
-  const uint8_t* ptr = data;
-  const uint8_t* ptr_end = data + file_size;
-
-  assert(ptr[0] == 0);
-  ptr++;
-  assert(read24be(ptr) == START_CODE);
-
-
-  uint32_t sps_log2_max_frame_num_minus4;
-
-
-  int last_frame_num = -1;
-
-  while (ptr < ptr_end) {
-    const uint8_t* next = ptr+1;
-    for (; next < ptr_end-4; next++) {
-      if (read24be(next) == START_CODE) break;
-    }
-    size_t nal_size = next - ptr;
-    if (nal_size < 5) {
-      break;
-    }
-
-    {
-      struct bitstream bs = {0};
-      bs_init(&bs, ptr, nal_size);
-
-      uint32_t start_code = bs_get(&bs, 24);
-      assert(start_code == 0x000001);
-
-      // nal_unit_header
-      uint32_t forbidden_zero_bit = bs_get(&bs, 1);
-      uint32_t nal_ref_idx = bs_get(&bs, 2);
-      uint32_t nal_unit_type = bs_get(&bs, 5);
-
-      switch (nal_unit_type) {
-      case H264_NAL_SPS:
-
-        {
-          uint32_t profile_idx = bs_get(&bs, 8);
-          uint32_t constraint_sets = bs_get(&bs, 4);
-          uint32_t reserved = bs_get(&bs, 5);
-          uint32_t level_idc = bs_get(&bs, 5);
-          uint32_t seq_parameter_set_id = bs_ue(&bs);
-          sps_log2_max_frame_num_minus4 = bs_ue(&bs);
-        }
-
-        // fallthrough
-      case H264_NAL_PPS:
-        fwrite(ptr, 1, nal_size, of_prefix);
-        break;
-
-      case H264_NAL_SLICE:
-      case H264_NAL_IDR_SLICE: {
-        // slice header
-        uint32_t first_mb_in_slice = bs_ue(&bs);
-        uint32_t slice_type = bs_ue(&bs);
-        uint32_t pic_parameter_set_id = bs_ue(&bs);
-
-        uint32_t frame_num = bs_get(&bs, sps_log2_max_frame_num_minus4+4);
-
-        if (first_mb_in_slice == 0) {
-          write32le(of_index, slice_type);
-          write32le(of_index, ptr - data);
-        }
-
-        break;
-      }
-
-      }
-
-    }
-
-    ptr = next;
-  }
-
-  write32le(of_index, -1);
-  write32le(of_index, file_size);
-}
-
-int main(int argc, char** argv) {
-  if (argc != 5) {
-    fprintf(stderr, "usage: %s h264|hevc file_path out_prefix out_index\n", argv[0]);
-    exit(1);
-  }
-
-  const char* file_type = argv[1];
-  const char* file_path = argv[2];
-
-  int fd = open(file_path, O_RDONLY, 0);
-  if (fd < 0) {
-    fprintf(stderr, "error: couldn't open %s\n", file_path);
-    exit(1);
-  }
-
-  FILE *of_prefix = fopen(argv[3], "wb");
-  assert(of_prefix);
-  FILE *of_index = fopen(argv[4], "wb");
-  assert(of_index);
-
-  off_t file_size = lseek(fd, 0, SEEK_END);
-  lseek(fd, 0, SEEK_SET);
-
-  assert(file_size > 4);
-
-  const uint8_t* data = (const uint8_t*)mmap(NULL, file_size, PROT_READ, MAP_PRIVATE, fd, 0);
-  assert(data != MAP_FAILED);
-
-  if (strcmp(file_type, "hevc") == 0) {
-    hevc_index(data, file_size, of_prefix, of_index);
-  } else if (strcmp(file_type, "h264") == 0) {
-    h264_index(data, file_size, of_prefix, of_index);
-  } else {
-    assert(false);
-  }
-
-  munmap((void*)data, file_size);
-  close(fd);
-
-  return 0;
-}
diff --git a/tools/replay/camera.cc b/tools/replay/camera.cc
index 66898c9244..49f3010c6c 100644
--- a/tools/replay/camera.cc
+++ b/tools/replay/camera.cc
@@ -1,7 +1,19 @@
 #include "tools/replay/camera.h"
-#include "tools/replay/util.h"
 
 #include <cassert>
+#include <tuple>
+
+#include "third_party/linux/include/msm_media_info.h"
+#include "tools/replay/util.h"
+
+std::tuple<size_t, size_t, size_t> get_nv12_info(int width, int height) {
+  int nv12_width = VENUS_Y_STRIDE(COLOR_FMT_NV12, width);
+  int nv12_height = VENUS_Y_SCANLINES(COLOR_FMT_NV12, height);
+  assert(nv12_width == VENUS_UV_STRIDE(COLOR_FMT_NV12, width));
+  assert(nv12_height / 2 == VENUS_UV_SCANLINES(COLOR_FMT_NV12, height));
+  size_t nv12_buffer_size = 2346 * nv12_width;  // comes from v4l2_format.fmt.pix_mp.plane_fmt[0].sizeimage
+  return {nv12_width, nv12_height, nv12_buffer_size};
+}
 
 CameraServer::CameraServer(std::pair<int, int> camera_size[MAX_CAMERAS]) {
   for (int i = 0; i < MAX_CAMERAS; ++i) {
@@ -25,7 +37,9 @@ void CameraServer::startVipcServer() {
   for (auto &cam : cameras_) {
     if (cam.width > 0 && cam.height > 0) {
       rInfo("camera[%d] frame size %dx%d", cam.type, cam.width, cam.height);
-      vipc_server_->create_buffers(cam.stream_type, YUV_BUFFER_COUNT, false, cam.width, cam.height);
+      auto [nv12_width, nv12_height, nv12_buffer_size] = get_nv12_info(cam.width, cam.height);
+      vipc_server_->create_buffers_with_sizes(cam.stream_type, YUV_BUFFER_COUNT, false, cam.width, cam.height,
+                                              nv12_buffer_size, nv12_width, nv12_width * nv12_height);
       if (!cam.thread.joinable()) {
         cam.thread = std::thread(&CameraServer::cameraThread, this, std::ref(cam));
       }
@@ -38,7 +52,7 @@ void CameraServer::cameraThread(Camera &cam) {
   auto read_frame = [&](FrameReader *fr, int frame_id) {
     VisionBuf *yuv_buf = vipc_server_->get_buffer(cam.stream_type);
     assert(yuv_buf);
-    bool ret = fr->get(frame_id, (uint8_t *)yuv_buf->addr);
+    bool ret = fr->get(frame_id, yuv_buf);
     return ret ? yuv_buf : nullptr;
   };
 
diff --git a/tools/replay/camera.h b/tools/replay/camera.h
index 66d33142fb..9f43c5a362 100644
--- a/tools/replay/camera.h
+++ b/tools/replay/camera.h
@@ -1,11 +1,18 @@
 #pragma once
 
 #include <unistd.h>
+
+#include <memory>
+#include <tuple>
+#include <utility>
+
 #include "cereal/visionipc/visionipc_server.h"
 #include "common/queue.h"
 #include "tools/replay/framereader.h"
 #include "tools/replay/logreader.h"
 
+std::tuple<size_t, size_t, size_t> get_nv12_info(int width, int height);
+
 class CameraServer {
 public:
   CameraServer(std::pair<int, int> camera_size[MAX_CAMERAS] = nullptr);
diff --git a/tools/replay/consoleui.cc b/tools/replay/consoleui.cc
index 5cbd3818a6..719bbd69ca 100644
--- a/tools/replay/consoleui.cc
+++ b/tools/replay/consoleui.cc
@@ -1,7 +1,11 @@
 #include "tools/replay/consoleui.h"
 
-#include <QApplication>
 #include <initializer_list>
+#include <string>
+#include <tuple>
+#include <utility>
+
+#include <QApplication>
 
 #include "common/version.h"
 
diff --git a/tools/replay/filereader.cc b/tools/replay/filereader.cc
index 88879067c9..22af7f5f86 100644
--- a/tools/replay/filereader.cc
+++ b/tools/replay/filereader.cc
@@ -3,11 +3,12 @@
 #include <fstream>
 
 #include "common/util.h"
+#include "system/hardware/hw.h"
 #include "tools/replay/util.h"
 
 std::string cacheFilePath(const std::string &url) {
   static std::string cache_path = [] {
-    const std::string comma_cache = util::getenv("COMMA_CACHE", "/tmp/comma_download_cache/");
+    const std::string comma_cache = Path::download_cache_root();
     util::create_directories(comma_cache, 0755);
     return comma_cache.back() == '/' ? comma_cache : comma_cache + "/";
   }();
diff --git a/tools/replay/framereader.cc b/tools/replay/framereader.cc
index ed276c627c..303f0c60ca 100644
--- a/tools/replay/framereader.cc
+++ b/tools/replay/framereader.cc
@@ -2,9 +2,8 @@
 #include "tools/replay/util.h"
 
 #include <cassert>
-#include "libyuv.h"
-
-#include "cereal/visionipc/visionbuf.h"
+#include <algorithm>
+#include "third_party/libyuv/include/libyuv.h"
 
 #ifdef __APPLE__
 #define HW_DEVICE_TYPE AV_HWDEVICE_TYPE_VIDEOTOOLBOX
@@ -118,7 +117,6 @@ bool FrameReader::load(const std::byte *data, size_t size, bool no_hw_decoder, s
 
   width = (decoder_ctx->width + 3) & ~3;
   height = decoder_ctx->height;
-  visionbuf_compute_aligned_width_and_height(width, height, &aligned_width, &aligned_height);
 
   if (has_hw_decoder && !no_hw_decoder) {
     if (!initHardwareDecoder(HW_DEVICE_TYPE)) {
@@ -177,15 +175,15 @@ bool FrameReader::initHardwareDecoder(AVHWDeviceType hw_device_type) {
   return true;
 }
 
-bool FrameReader::get(int idx, uint8_t *yuv) {
-  assert(yuv != nullptr);
+bool FrameReader::get(int idx, VisionBuf *buf) {
+  assert(buf != nullptr);
   if (!valid_ || idx < 0 || idx >= packets.size()) {
     return false;
   }
-  return decode(idx, yuv);
+  return decode(idx, buf);
 }
 
-bool FrameReader::decode(int idx, uint8_t *yuv) {
+bool FrameReader::decode(int idx, VisionBuf *buf) {
   int from_idx = idx;
   if (idx != prev_idx + 1 && key_frames_count_ > 1) {
     // seeking to the nearest key frame
@@ -201,7 +199,7 @@ bool FrameReader::decode(int idx, uint8_t *yuv) {
   for (int i = from_idx; i <= idx; ++i) {
     AVFrame *f = decodeFrame(packets[i]);
     if (f && i == idx) {
-      return copyBuffers(f, yuv);
+      return copyBuffers(f, buf);
     }
   }
   return false;
@@ -233,22 +231,20 @@ AVFrame *FrameReader::decodeFrame(AVPacket *pkt) {
   }
 }
 
-bool FrameReader::copyBuffers(AVFrame *f, uint8_t *yuv) {
-  assert(f != nullptr && yuv != nullptr);
-  uint8_t *y = yuv;
-  uint8_t *uv = y + width * height;
+bool FrameReader::copyBuffers(AVFrame *f, VisionBuf *buf) {
+  assert(f != nullptr && buf != nullptr);
   if (hw_pix_fmt == HW_PIX_FMT) {
     for (int i = 0; i < height/2; i++) {
-      memcpy(y + (i*2 + 0)*width, f->data[0] + (i*2 + 0)*f->linesize[0], width);
-      memcpy(y + (i*2 + 1)*width, f->data[0] + (i*2 + 1)*f->linesize[0], width);
-      memcpy(uv + i*width, f->data[1] + i*f->linesize[1], width);
+      memcpy(buf->y + (i*2 + 0)*buf->stride, f->data[0] + (i*2 + 0)*f->linesize[0], width);
+      memcpy(buf->y + (i*2 + 1)*buf->stride, f->data[0] + (i*2 + 1)*f->linesize[0], width);
+      memcpy(buf->uv + i*buf->stride, f->data[1] + i*f->linesize[1], width);
     }
   } else {
     libyuv::I420ToNV12(f->data[0], f->linesize[0],
                        f->data[1], f->linesize[1],
                        f->data[2], f->linesize[2],
-                       y, width,
-                       uv, width,
+                       buf->y, buf->stride,
+                       buf->uv, buf->stride,
                        width, height);
   }
   return true;
diff --git a/tools/replay/framereader.h b/tools/replay/framereader.h
index e50b61d7f4..bb72ac8f8d 100644
--- a/tools/replay/framereader.h
+++ b/tools/replay/framereader.h
@@ -4,6 +4,7 @@
 #include <string>
 #include <vector>
 
+#include "cereal/visionipc/visionbuf.h"
 #include "tools/replay/filereader.h"
 
 extern "C" {
@@ -22,19 +23,18 @@ public:
   bool load(const std::string &url, bool no_hw_decoder = false, std::atomic<bool> *abort = nullptr, bool local_cache = false,
             int chunk_size = -1, int retries = 0);
   bool load(const std::byte *data, size_t size, bool no_hw_decoder = false, std::atomic<bool> *abort = nullptr);
-  bool get(int idx, uint8_t *yuv);
+  bool get(int idx, VisionBuf *buf);
   int getYUVSize() const { return width * height * 3 / 2; }
   size_t getFrameCount() const { return packets.size(); }
   bool valid() const { return valid_; }
 
   int width = 0, height = 0;
-  int aligned_width = 0, aligned_height = 0;
 
 private:
   bool initHardwareDecoder(AVHWDeviceType hw_device_type);
-  bool decode(int idx, uint8_t *yuv);
+  bool decode(int idx, VisionBuf *buf);
   AVFrame * decodeFrame(AVPacket *pkt);
-  bool copyBuffers(AVFrame *f, uint8_t *yuv);
+  bool copyBuffers(AVFrame *f, VisionBuf *buf);
 
   std::vector<AVPacket*> packets;
   std::unique_ptr<AVFrame, AVFrameDeleter>av_frame_, hw_frame;
diff --git a/tools/replay/logreader.cc b/tools/replay/logreader.cc
index 9a5df2eeed..c92ff4753f 100644
--- a/tools/replay/logreader.cc
+++ b/tools/replay/logreader.cc
@@ -1,6 +1,7 @@
 #include "tools/replay/logreader.h"
 
 #include <algorithm>
+#include "tools/replay/filereader.h"
 #include "tools/replay/util.h"
 
 Event::Event(const kj::ArrayPtr<const capnp::word> &amsg, bool frame) : reader(amsg), frame(frame) {
@@ -29,8 +30,7 @@ Event::Event(const kj::ArrayPtr<const capnp::word> &amsg, bool frame) : reader(a
 LogReader::LogReader(size_t memory_pool_block_size) {
 #ifdef HAS_MEMORY_RESOURCE
   const size_t buf_size = sizeof(Event) * memory_pool_block_size;
-  pool_buffer_ = ::operator new(buf_size);
-  mbr_ = new std::pmr::monotonic_buffer_resource(pool_buffer_, buf_size);
+  mbr_ = std::make_unique<std::pmr::monotonic_buffer_resource>(buf_size);
 #endif
   events.reserve(memory_pool_block_size);
 }
@@ -39,16 +39,9 @@ LogReader::~LogReader() {
   for (Event *e : events) {
     delete e;
   }
-
-#ifdef HAS_MEMORY_RESOURCE
-  delete mbr_;
-  ::operator delete(pool_buffer_);
-#endif
 }
 
-bool LogReader::load(const std::string &url, std::atomic<bool> *abort,
-                     const std::set<cereal::Event::Which> &allow,
-                     bool local_cache, int chunk_size, int retries) {
+bool LogReader::load(const std::string &url, std::atomic<bool> *abort, bool local_cache, int chunk_size, int retries) {
   raw_ = FileReader(local_cache, chunk_size, retries).read(url, abort);
   if (raw_.empty()) return false;
 
@@ -56,36 +49,30 @@ bool LogReader::load(const std::string &url, std::atomic<bool> *abort,
     raw_ = decompressBZ2(raw_, abort);
     if (raw_.empty()) return false;
   }
-  return parse(allow, abort);
+  return parse(abort);
 }
 
 bool LogReader::load(const std::byte *data, size_t size, std::atomic<bool> *abort) {
   raw_.assign((const char *)data, size);
-  return parse({}, abort);
+  return parse(abort);
 }
 
-bool LogReader::parse(const std::set<cereal::Event::Which> &allow, std::atomic<bool> *abort) {
+bool LogReader::parse(std::atomic<bool> *abort) {
   try {
     kj::ArrayPtr<const capnp::word> words((const capnp::word *)raw_.data(), raw_.size() / sizeof(capnp::word));
     while (words.size() > 0 && !(abort && *abort)) {
 #ifdef HAS_MEMORY_RESOURCE
-      Event *evt = new (mbr_) Event(words);
+      Event *evt = new (mbr_.get()) Event(words);
 #else
       Event *evt = new Event(words);
 #endif
-      if (!allow.empty() && allow.find(evt->which) == allow.end()) {
-        words = kj::arrayPtr(evt->reader.getEnd(), words.end());
-        delete evt;
-        continue;
-      }
-
       // Add encodeIdx packet again as a frame packet for the video stream
       if (evt->which == cereal::Event::ROAD_ENCODE_IDX ||
           evt->which == cereal::Event::DRIVER_ENCODE_IDX ||
           evt->which == cereal::Event::WIDE_ROAD_ENCODE_IDX) {
 
 #ifdef HAS_MEMORY_RESOURCE
-        Event *frame_evt = new (mbr_) Event(words, true);
+        Event *frame_evt = new (mbr_.get()) Event(words, true);
 #else
         Event *frame_evt = new Event(words, true);
 #endif
diff --git a/tools/replay/logreader.h b/tools/replay/logreader.h
index 010839af22..73f822d16c 100644
--- a/tools/replay/logreader.h
+++ b/tools/replay/logreader.h
@@ -5,11 +5,12 @@
 #include <memory_resource>
 #endif
 
-#include <set>
+#include <memory>
+#include <string>
+#include <vector>
 
 #include "cereal/gen/cpp/log.capnp.h"
 #include "system/camerad/cameras/camera_common.h"
-#include "tools/replay/filereader.h"
 
 const CameraType ALL_CAMERAS[] = {RoadCam, DriverCam, WideRoadCam};
 const int MAX_CAMERAS = std::size(ALL_CAMERAS);
@@ -52,16 +53,15 @@ class LogReader {
 public:
   LogReader(size_t memory_pool_block_size = DEFAULT_EVENT_MEMORY_POOL_BLOCK_SIZE);
   ~LogReader();
-  bool load(const std::string &url, std::atomic<bool> *abort = nullptr, const std::set<cereal::Event::Which> &allow = {},
+  bool load(const std::string &url, std::atomic<bool> *abort = nullptr,
             bool local_cache = false, int chunk_size = -1, int retries = 0);
   bool load(const std::byte *data, size_t size, std::atomic<bool> *abort = nullptr);
   std::vector<Event*> events;
 
 private:
-  bool parse(const std::set<cereal::Event::Which> &allow, std::atomic<bool> *abort);
+  bool parse(std::atomic<bool> *abort);
   std::string raw_;
 #ifdef HAS_MEMORY_RESOURCE
-  std::pmr::monotonic_buffer_resource *mbr_ = nullptr;
-  void *pool_buffer_ = nullptr;
+  std::unique_ptr<std::pmr::monotonic_buffer_resource> mbr_;
 #endif
 };
diff --git a/tools/replay/main.cc b/tools/replay/main.cc
index 78be2acd0b..945cb4cd09 100644
--- a/tools/replay/main.cc
+++ b/tools/replay/main.cc
@@ -6,9 +6,13 @@
 #include "tools/replay/replay.h"
 
 int main(int argc, char *argv[]) {
+#ifdef __APPLE__
+  // With all sockets opened, we might hit the default limit of 256 on macOS
+  util::set_file_descriptor_limit(1024);
+#endif
+
   QCoreApplication app(argc, argv);
 
-  const QStringList base_blacklist = {"uiDebug", "userFlag"};
   const std::tuple<QString, REPLAY_FLAGS, QString> flags[] = {
       {"dcam", REPLAY_FLAG_DCAM, "load driver camera"},
       {"ecam", REPLAY_FLAG_ECAM, "load wide road camera"},
@@ -17,7 +21,7 @@ int main(int argc, char *argv[]) {
       {"qcam", REPLAY_FLAG_QCAMERA, "load qcamera"},
       {"no-hw-decoder", REPLAY_FLAG_NO_HW_DECODER, "disable HW video decoding"},
       {"no-vipc", REPLAY_FLAG_NO_VIPC, "do not output video"},
-      {"all", REPLAY_FLAG_ALL_SERVICES, "do output all messages including " + base_blacklist.join(", ") + 
+      {"all", REPLAY_FLAG_ALL_SERVICES, "do output all messages including uiDebug, userFlag"
                                         ". this may causes issues when used along with UI"}
   };
 
@@ -59,7 +63,7 @@ int main(int argc, char *argv[]) {
     op_prefix.reset(new OpenpilotPrefix(prefix.toStdString()));
   }
 
-  Replay *replay = new Replay(route, allow, block, base_blacklist, nullptr, replay_flags, parser.value("data_dir"), &app);
+  Replay *replay = new Replay(route, allow, block, nullptr, replay_flags, parser.value("data_dir"), &app);
   if (!parser.value("c").isEmpty()) {
     replay->setSegmentCacheLimit(parser.value("c").toInt());
   }
diff --git a/tools/replay/replay.cc b/tools/replay/replay.cc
index 74817584cd..1ec484d677 100644
--- a/tools/replay/replay.cc
+++ b/tools/replay/replay.cc
@@ -7,40 +7,25 @@
 #include "cereal/services.h"
 #include "common/params.h"
 #include "common/timing.h"
-#include "system/hardware/hw.h"
 #include "tools/replay/util.h"
 
-Replay::Replay(QString route, QStringList allow, QStringList block, QStringList base_blacklist, SubMaster *sm_, uint32_t flags, QString data_dir, QObject *parent)
-    : sm(sm_), flags_(flags), QObject(parent) {
-  std::vector<const char *> s;
+Replay::Replay(QString route, QStringList allow, QStringList block, SubMaster *sm_,
+               uint32_t flags, QString data_dir, QObject *parent) : sm(sm_), flags_(flags), QObject(parent) {
+  if (!(flags_ & REPLAY_FLAG_ALL_SERVICES)) {
+    block << "uiDebug" << "userFlag";
+  }
   auto event_struct = capnp::Schema::from<cereal::Event>().asStruct();
   sockets_.resize(event_struct.getUnionFields().size());
-  for (const auto &it : services) {
-    uint16_t which = event_struct.getFieldByName(it.name).getProto().getDiscriminantValue();
-    if ((which == cereal::Event::Which::UI_DEBUG || which == cereal::Event::Which::USER_FLAG) &&
-        !(flags & REPLAY_FLAG_ALL_SERVICES) && 
-        !allow.contains(it.name)) {
-      continue;
-    }
-
-    if ((allow.empty() || allow.contains(it.name)) && !block.contains(it.name)) {
-      sockets_[which] = it.name;
-      if (!allow.empty() || !block.empty()) {
-        allow_list.insert((cereal::Event::Which)which);
-      }
-      s.push_back(it.name);
-    }
-  }
-
-  if (!allow_list.empty()) {
-    // the following events are needed for replay to work properly.
-    allow_list.insert(cereal::Event::Which::INIT_DATA);
-    allow_list.insert(cereal::Event::Which::CAR_PARAMS);
-    if (sockets_[cereal::Event::Which::PANDA_STATES] != nullptr) {
-      allow_list.insert(cereal::Event::Which::PANDA_STATE_D_E_P_R_E_C_A_T_E_D);
+  for (const auto &[name, _] : services) {
+    if (!block.contains(name.c_str()) && (allow.empty() || allow.contains(name.c_str()))) {
+      uint16_t which = event_struct.getFieldByName(name).getProto().getDiscriminantValue();
+      sockets_[which] = name.c_str();
     }
   }
 
+  std::vector<const char *> s;
+  std::copy_if(sockets_.begin(), sockets_.end(), std::back_inserter(s),
+               [](const char *name) { return name != nullptr; });
   qDebug() << "services " << s;
   qDebug() << "loading route " << route;
 
@@ -150,13 +135,12 @@ void Replay::buildTimeline() {
     [(int)cereal::ControlsState::AlertStatus::CRITICAL] = TimelineType::AlertCritical,
   };
 
-  for (auto it = segments_.cbegin(); it != segments_.cend() && !exit_; ++it) {
-    LogReader log;
-    if (!log.load(route_->at(it->first).qlog.toStdString(), &exit_,
-                  {cereal::Event::Which::CONTROLS_STATE, cereal::Event::Which::USER_FLAG},
-                  !hasFlag(REPLAY_FLAG_NO_FILE_CACHE), 0, 3)) continue;
+  const auto &route_segments = route_->segments();
+  for (auto it = route_segments.cbegin(); it != route_segments.cend() && !exit_; ++it) {
+    std::shared_ptr<LogReader> log(new LogReader());
+    if (!log->load(it->second.qlog.toStdString(), &exit_, !hasFlag(REPLAY_FLAG_NO_FILE_CACHE), 0, 3)) continue;
 
-    for (const Event *e : log.events) {
+    for (const Event *e : log->events) {
       if (e->which == cereal::Event::Which::CONTROLS_STATE) {
         auto cs = e->event.getControlsState();
 
@@ -184,6 +168,8 @@ void Replay::buildTimeline() {
         timeline.push_back({toSeconds(e->mono_time), toSeconds(e->mono_time), TimelineType::UserFlag});
       }
     }
+    std::sort(timeline.begin(), timeline.end(), [](auto &l, auto &r) { return std::get<2>(l) < std::get<2>(r); });
+    emit qLogLoaded(it->first, log);
   }
 }
 
@@ -226,36 +212,26 @@ void Replay::segmentLoadFinished(bool success) {
   if (!success) {
     Segment *seg = qobject_cast<Segment *>(sender());
     rWarning("failed to load segment %d, removing it from current replay list", seg->seg_num);
-    segments_.erase(seg->seg_num);
+    updateEvents([&]() {
+      segments_.erase(seg->seg_num);
+      return true;
+    });
   }
   queueSegment();
 }
 
 void Replay::queueSegment() {
-  if (segments_.empty()) return;
-
-  SegmentMap::iterator begin, cur;
-  begin = cur = segments_.lower_bound(std::min(current_segment_.load(), segments_.rbegin()->first));
-  int distance = std::max<int>(std::ceil(segment_cache_limit / 2.0) - 1, segment_cache_limit - std::distance(cur, segments_.end()));
-  for (int i = 0; begin != segments_.begin() && i < distance; ++i) {
-    --begin;
-  }
-  auto end = begin;
-  for (int i = 0; end != segments_.end() && i < segment_cache_limit; ++i) {
-    ++end;
-  }
+  auto cur = segments_.lower_bound(current_segment_.load());
+  if (cur == segments_.end()) return;
 
+  auto begin = std::prev(cur, std::min<int>(segment_cache_limit / 2, std::distance(segments_.begin(), cur)));
+  auto end = std::next(begin, std::min<int>(segment_cache_limit, segments_.size()));
   // load one segment at a time
-  for (auto it = cur; it != end; ++it) {
-    auto &[n, seg] = *it;
-    if ((seg && !seg->isLoaded()) || !seg) {
-      if (!seg) {
-        rDebug("loading segment %d...", n);
-        seg = std::make_unique<Segment>(n, route_->at(n), flags_, allow_list);
-        QObject::connect(seg.get(), &Segment::loadFinished, this, &Replay::segmentLoadFinished);
-      }
-      break;
-    }
+  auto it = std::find_if(cur, end, [](auto &it) { return !it.second || !it.second->isLoaded(); });
+  if (it != end && !it->second) {
+    rDebug("loading segment %d...", it->first);
+    it->second = std::make_unique<Segment>(it->first, route_->at(it->first), flags_);
+    QObject::connect(it->second.get(), &Segment::loadFinished, this, &Replay::segmentLoadFinished);
   }
 
   mergeSegments(begin, end);
@@ -292,13 +268,11 @@ void Replay::mergeSegments(const SegmentMap::iterator &begin, const SegmentMap::
     new_events_->clear();
     new_events_->reserve(new_events_size);
     for (int n : segments_need_merge) {
-      const auto &e = segments_[n]->log->events;
-      if (e.size() > 0) {
-        auto insert_from = e.begin();
-        if (new_events_->size() > 0 && (*insert_from)->which == cereal::Event::Which::INIT_DATA) ++insert_from;
-        auto middle = new_events_->insert(new_events_->end(), insert_from, e.end());
-        std::inplace_merge(new_events_->begin(), middle, new_events_->end(), Event::lessThan());
-      }
+      size_t size = new_events_->size();
+      const auto &events = segments_[n]->log->events;
+      std::copy_if(events.begin(), events.end(), std::back_inserter(*new_events_),
+                   [this](auto e) { return e->which < sockets_.size() && sockets_[e->which] != nullptr; });
+      std::inplace_merge(new_events_->begin(), new_events_->begin() + size, new_events_->end(), Event::lessThan());
     }
 
     if (stream_thread_) {
@@ -307,7 +281,8 @@ void Replay::mergeSegments(const SegmentMap::iterator &begin, const SegmentMap::
     updateEvents([&]() {
       events_.swap(new_events_);
       segments_merged_ = segments_need_merge;
-      return true;
+      // Do not wake up the stream thread if the current segment has not been merged.
+      return isSegmentMerged(current_segment_) || (segments_.count(current_segment_) == 0);
     });
   }
 }
@@ -315,13 +290,12 @@ void Replay::mergeSegments(const SegmentMap::iterator &begin, const SegmentMap::
 void Replay::startStream(const Segment *cur_segment) {
   const auto &events = cur_segment->log->events;
 
-  // get route start time from initData
-  auto it = std::find_if(events.begin(), events.end(), [](auto e) { return e->which == cereal::Event::Which::INIT_DATA; });
-  route_start_ts_ = it != events.end() ? (*it)->mono_time : events[0]->mono_time;
-  cur_mono_time_ += route_start_ts_;
+  // each segment has an INIT_DATA
+  route_start_ts_ = events.front()->mono_time;
+  cur_mono_time_ += route_start_ts_ - 1;
 
   // write CarParams
-  it = std::find_if(events.begin(), events.end(), [](auto e) { return e->which == cereal::Event::Which::CAR_PARAMS; });
+  auto it = std::find_if(events.begin(), events.end(), [](auto e) { return e->which == cereal::Event::Which::CAR_PARAMS; });
   if (it != events.end()) {
     car_fingerprint_ = (*it)->event.getCarParams().getCarFingerprint();
     capnp::MallocMessageBuilder builder;
@@ -413,22 +387,12 @@ void Replay::stream() {
       cur_mono_time_ = evt->mono_time;
       setCurrentSegment(toSeconds(cur_mono_time_) / 60);
 
-      // migration for pandaState -> pandaStates to keep UI working for old segments
-      if (cur_which == cereal::Event::Which::PANDA_STATE_D_E_P_R_E_C_A_T_E_D &&
-          sockets_[cereal::Event::Which::PANDA_STATES] != nullptr) {
-        MessageBuilder msg;
-        auto ps = msg.initEvent().initPandaStates(1);
-        ps[0].setIgnitionLine(true);
-        ps[0].setPandaType(cereal::PandaState::PandaType::DOS);
-        pm->send(sockets_[cereal::Event::Which::PANDA_STATES], msg);
-      }
-
-      if (cur_which < sockets_.size() && sockets_[cur_which] != nullptr) {
+      if (sockets_[cur_which] != nullptr) {
         // keep time
         long etime = (cur_mono_time_ - evt_start_ts) / speed_;
         long rtime = nanos_since_boot() - loop_start_ts;
         long behind_ns = etime - rtime;
-        // if behind_ns is greater than 1 second, it means that an invalid segemnt is skipped by seeking/replaying
+        // if behind_ns is greater than 1 second, it means that an invalid segment is skipped by seeking/replaying
         if (behind_ns >= 1 * 1e9 || speed_ != prev_replay_speed) {
           // reset event start times
           evt_start_ts = cur_mono_time_;
@@ -454,7 +418,7 @@ void Replay::stream() {
     }
 
     if (eit == events_->end() && !hasFlag(REPLAY_FLAG_NO_LOOP)) {
-      int last_segment = segments_.rbegin()->first;
+      int last_segment = segments_.empty() ? 0 : segments_.rbegin()->first;
       if (current_segment_ >= last_segment && isSegmentMerged(last_segment)) {
         rInfo("reaches the end of route, restart from beginning");
         QMetaObject::invokeMethod(this, std::bind(&Replay::seekTo, this, 0, false), Qt::QueuedConnection);
diff --git a/tools/replay/replay.h b/tools/replay/replay.h
index 4eb8e3ab30..1144da2601 100644
--- a/tools/replay/replay.h
+++ b/tools/replay/replay.h
@@ -1,13 +1,20 @@
 #pragma once
 
+#include <algorithm>
+#include <map>
+#include <memory>
 #include <optional>
+#include <string>
+#include <tuple>
+#include <vector>
+#include <utility>
 
 #include <QThread>
 
 #include "tools/replay/camera.h"
 #include "tools/replay/route.h"
 
-const QString DEMO_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36";
+const QString DEMO_ROUTE = "a2a0ccea32023010|2023-07-27--13-01-19";
 
 // one segment uses about 100M of memory
 constexpr int MIN_SEGMENTS_CACHE = 5;
@@ -36,13 +43,14 @@ enum class FindFlag {
 
 enum class TimelineType { None, Engaged, AlertInfo, AlertWarning, AlertCritical, UserFlag };
 typedef bool (*replayEventFilter)(const Event *, void *);
+Q_DECLARE_METATYPE(std::shared_ptr<LogReader>);
 
 class Replay : public QObject {
   Q_OBJECT
 
 public:
-  Replay(QString route, QStringList allow, QStringList block, QStringList base_blacklist, SubMaster *sm = nullptr,
-          uint32_t flags = REPLAY_FLAG_NONE, QString data_dir = "", QObject *parent = 0);
+  Replay(QString route, QStringList allow, QStringList block, SubMaster *sm = nullptr,
+         uint32_t flags = REPLAY_FLAG_NONE, QString data_dir = "", QObject *parent = 0);
   ~Replay();
   bool load();
   void start(int seconds = 0);
@@ -72,7 +80,7 @@ public:
   inline void setSpeed(float speed) { speed_ = speed; }
   inline float getSpeed() const { return speed_; }
   inline const std::vector<Event *> *events() const { return events_.get(); }
-  inline const std::map<int, std::unique_ptr<Segment>> &segments() const { return segments_; };
+  inline const std::map<int, std::unique_ptr<Segment>> &segments() const { return segments_; }
   inline const std::string &carFingerprint() const { return car_fingerprint_; }
   inline const std::vector<std::tuple<double, double, TimelineType>> getTimeline() {
     std::lock_guard lk(timeline_lock);
@@ -83,6 +91,7 @@ signals:
   void streamStarted();
   void segmentsMerged();
   void seekedTo(double sec);
+  void qLogLoaded(int segnum, std::shared_ptr<LogReader> qlog);
 
 protected slots:
   void segmentLoadFinished(bool success);
@@ -104,8 +113,6 @@ protected:
   }
 
   QThread *stream_thread_ = nullptr;
-
-  // logs
   std::mutex stream_lock_;
   std::condition_variable stream_cv_;
   std::atomic<bool> updating_events_ = false;
@@ -132,9 +139,8 @@ protected:
   std::mutex timeline_lock;
   QFuture<void> timeline_future;
   std::vector<std::tuple<double, double, TimelineType>> timeline;
-  std::set<cereal::Event::Which> allow_list;
   std::string car_fingerprint_;
-  float speed_ = 1.0;
+  std::atomic<float> speed_ = 1.0;
   replayEventFilter event_filter = nullptr;
   void *filter_opaque = nullptr;
   int segment_cache_limit = MIN_SEGMENTS_CACHE;
diff --git a/tools/replay/route.cc b/tools/replay/route.cc
index 619aeb3f5f..9168d25b35 100644
--- a/tools/replay/route.cc
+++ b/tools/replay/route.cc
@@ -6,11 +6,10 @@
 #include <QJsonDocument>
 #include <QRegExp>
 #include <QtConcurrent>
-
 #include <array>
 
-#include "system/hardware/hw.h"
 #include "selfdrive/ui/qt/api.h"
+#include "system/hardware/hw.h"
 #include "tools/replay/replay.h"
 #include "tools/replay/util.h"
 
@@ -100,9 +99,7 @@ void Route::addFileToSegment(int n, const QString &file) {
 
 // class Segment
 
-Segment::Segment(int n, const SegmentFile &files, uint32_t flags,
-                 const std::set<cereal::Event::Which> &allow)
-    : seg_num(n), flags(flags), allow(allow) {
+Segment::Segment(int n, const SegmentFile &files, uint32_t flags) : seg_num(n), flags(flags) {
   // [RoadCam, DriverCam, WideRoadCam, log]. fallback to qcamera/qlog
   const std::array file_list = {
       (flags & REPLAY_FLAG_QCAMERA) || files.road_cam.isEmpty() ? files.qcamera : files.road_cam,
@@ -133,7 +130,7 @@ void Segment::loadFile(int id, const std::string file) {
     success = frames[id]->load(file, flags & REPLAY_FLAG_NO_HW_DECODER, &abort_, local_cache, 20 * 1024 * 1024, 3);
   } else {
     log = std::make_unique<LogReader>();
-    success = log->load(file, &abort_, allow, local_cache, 0, 3);
+    success = log->load(file, &abort_, local_cache, 0, 3);
   }
 
   if (!success) {
diff --git a/tools/replay/route.h b/tools/replay/route.h
index 86adf6a14d..4dad7a1f37 100644
--- a/tools/replay/route.h
+++ b/tools/replay/route.h
@@ -1,5 +1,9 @@
 #pragma once
 
+#include <map>
+#include <memory>
+#include <string>
+
 #include <QDateTime>
 #include <QFutureSynchronizer>
 
@@ -50,7 +54,7 @@ class Segment : public QObject {
   Q_OBJECT
 
 public:
-  Segment(int n, const SegmentFile &files, uint32_t flags, const std::set<cereal::Event::Which> &allow = {});
+  Segment(int n, const SegmentFile &files, uint32_t flags);
   ~Segment();
   inline bool isLoaded() const { return !loading_ && !abort_; }
 
@@ -68,5 +72,4 @@ protected:
   std::atomic<int> loading_ = 0;
   QFutureSynchronizer<void> synchronizer_;
   uint32_t flags;
-  std::set<cereal::Event::Which> allow;
 };
diff --git a/tools/replay/util.cc b/tools/replay/util.cc
index a6ebbec615..b4f72d0530 100644
--- a/tools/replay/util.cc
+++ b/tools/replay/util.cc
@@ -4,22 +4,22 @@
 #include <curl/curl.h>
 #include <openssl/sha.h>
 
+#include <cstdarg>
 #include <cstring>
 #include <cassert>
 #include <cmath>
 #include <fstream>
 #include <iostream>
+#include <map>
 #include <mutex>
 #include <numeric>
+#include <utility>
 
 #include "common/timing.h"
 #include "common/util.h"
 
 ReplayMessageHandler message_handler = nullptr;
-DownloadProgressHandler download_progress_handler = nullptr;
-
 void installMessageHandler(ReplayMessageHandler handler) { message_handler = handler; }
-void installDownloadProgressHandler(DownloadProgressHandler handler) { download_progress_handler = handler; }
 
 void logMessage(ReplyMsgType type, const char *fmt, ...) {
   static std::mutex lock;
@@ -91,6 +91,11 @@ size_t write_cb(char *data, size_t size, size_t count, void *userp) {
 size_t dumy_write_cb(char *data, size_t size, size_t count, void *userp) { return size * count; }
 
 struct DownloadStats {
+  void installDownloadProgressHandler(DownloadProgressHandler handler) {
+    std::lock_guard lk(lock);
+    download_progress_handler = handler;
+  }
+
   void add(const std::string &url, uint64_t total_bytes) {
     std::lock_guard lk(lock);
     items[url] = {0, total_bytes};
@@ -118,10 +123,17 @@ struct DownloadStats {
   std::mutex lock;
   std::map<std::string, std::pair<uint64_t, uint64_t>> items;
   double prev_tm = 0;
+  DownloadProgressHandler download_progress_handler = nullptr;
 };
 
+static DownloadStats download_stats;
+
 } // namespace
 
+void installDownloadProgressHandler(DownloadProgressHandler handler) {
+  download_stats.installDownloadProgressHandler(handler);
+}
+
 std::string formattedDataSize(size_t size) {
   if (size < 1024) {
     return std::to_string(size) + " B";
@@ -164,7 +176,6 @@ std::string getUrlWithoutQuery(const std::string &url) {
 
 template <class T>
 bool httpDownload(const std::string &url, T &buf, size_t chunk_size, size_t content_length, std::atomic<bool> *abort) {
-  static DownloadStats download_stats;
   download_stats.add(url, content_length);
 
   int parts = 1;