Merge branch 'dec-new' into dec-new-refactored

The `messaging` module was added to resolve potential issues with undefined references. This change ensures all required imports are present, improving the reliability and maintainability of the code.

.github/workflows/selfdrive_tests.yaml

@@ -352,7 +352,7 @@ jobs:
       - name: Build openpilot
         run: ${{ env.RUN }} "scons -j$(nproc)"
       - name: Create Test Report
-        timeout-minutes: ${{ ((steps.frames-cache.outputs.cache-hit == 'true') && 1 || 3) }}
+        timeout-minutes: ${{ ((steps.frames-cache.outputs.cache-hit == 'true') && 2 || 4) }}
         run: >
             ${{ env.RUN }} "PYTHONWARNINGS=ignore &&
                             source selfdrive/test/setup_xvfb.sh &&

.github/workflows/sunnypilot-build-model.yaml

@@ -18,6 +18,18 @@ on:
         description: 'Custom name for the model'
         required: false
         type: string
+      file_name:
+        description: 'File name prefix for the model files'
+        required: false
+        type: string
+      is_20hz:
+        description: 'Is this a 20Hz model'
+        required: false
+        type: boolean
+        default: false
+
+
+run-name: Build model [${{ inputs.custom_name || inputs.upstream_branch }}] from ref [${{ inputs.upstream_branch }}]
 
 jobs:
   build_model:
@@ -37,12 +49,15 @@ jobs:
         with:
           path: ${{env.SCONS_CACHE_DIR}}
           key: scons-${{ runner.os }}-${{ runner.arch }}-${{ github.head_ref || github.ref_name }}-model-${{ github.sha }}
+          # Note: GitHub Actions enforces cache isolation between different build sources (PR builds, workflow dispatches, etc.) 
+          #   for security. Only caches from the default branch are shared across all builds. This is by design and cannot be overridden.
           restore-keys: |
-            scons-${{ runner.os }}-${{ runner.arch }}-${{ github.head_ref || github.ref_name }}-model-${{ github.sha }}
             scons-${{ runner.os }}-${{ runner.arch }}-${{ github.head_ref || github.ref_name }}-model
             scons-${{ runner.os }}-${{ runner.arch }}-${{ github.head_ref || github.ref_name }}
-            scons-${{ runner.os }}-${{ runner.arch }}-master-new
-            scons-${{ runner.os }}-${{ runner.arch }}-master
+            scons-${{ runner.os }}-${{ runner.arch }}-${{ env.MASTER_NEW_BRANCH }}-model
+            scons-${{ runner.os }}-${{ runner.arch }}-${{ env.MASTER_BRANCH }}-model
+            scons-${{ runner.os }}-${{ runner.arch }}-${{ env.MASTER_NEW_BRANCH }}
+            scons-${{ runner.os }}-${{ runner.arch }}-${{ env.MASTER_BRANCH }}
             scons-${{ runner.os }}-${{ runner.arch }}
 
       - name: Setup build environment
@@ -69,6 +84,8 @@ jobs:
         run: |
           sudo rm -rf ${OUTPUT_DIR}
           mkdir -p ${OUTPUT_DIR}
+          
+          # Copy the model files
           rsync -avm \
             --include='*.dlc' \
             --include='*.thneed' \
@@ -78,6 +95,36 @@ jobs:
             --delete-excluded \
             --chown=comma:comma \
             ./selfdrive/modeld/models/ ${OUTPUT_DIR}/
+          
+          # Rename files if file_name is provided
+          if [ ! -z "${{ inputs.file_name }}" ]; then
+            mv ${OUTPUT_DIR}/supercombo.thneed ${OUTPUT_DIR}/supercombo-${{ inputs.file_name }}.thneed
+            mv ${OUTPUT_DIR}/supercombo_metadata.pkl ${OUTPUT_DIR}/supercombo-${{ inputs.file_name }}_metadata.pkl
+          fi
+          
+          # Calculate SHA256 hashes
+          HASH=$(sha256sum ${OUTPUT_DIR}/supercombo*.thneed | cut -d' ' -f1)
+          METADATA_HASH=$(sha256sum ${OUTPUT_DIR}/supercombo*_metadata.pkl | cut -d' ' -f1)
+          
+          # Create metadata.json
+          cat > ${OUTPUT_DIR}/metadata.json << EOF
+          {
+            "display_name": "${{ inputs.custom_name || inputs.upstream_branch }}",
+            "full_name": "${{ inputs.file_name || 'default' }}",
+            "is_20hz": ${{ inputs.is_20hz || false }},
+            "files": {
+              "drive_model": {
+                "file_name": "$(basename ${OUTPUT_DIR}/supercombo*.thneed)",
+                "sha256": "${HASH}"
+              },
+              "metadata": {
+                "file_name": "$(basename ${OUTPUT_DIR}/supercombo*_metadata.pkl)",
+                "sha256": "${METADATA_HASH}"
+              }
+            },
+            "ref": "${{ inputs.upstream_branch }}",
+            "build_time": "$(date -u +"%Y-%m-%dT%H:%M:%SZ")"
+          }     
 
       - name: Upload Build Artifacts
         uses: actions/upload-artifact@v4

.github/workflows/sunnypilot-build-prebuilt.yaml

@@ -51,9 +51,10 @@ jobs:
         with:
           path: ${{env.SCONS_CACHE_DIR}}
           key: scons-${{ runner.os }}-${{ runner.arch }}-${{ github.head_ref || github.ref_name }}-${{ github.sha }}
+          # Note: GitHub Actions enforces cache isolation between different build sources (PR builds, workflow dispatches, etc.) 
+          #   for security. Only caches from the default branch are shared across all builds. This is by design and cannot be overridden.
           restore-keys: |
-            scons-${{ runner.os }}-${{ runner.arch }}-${{ github.head_ref }}
-            scons-${{ runner.os }}-${{ runner.arch }}-${{ github.ref_name }}
+            scons-${{ runner.os }}-${{ runner.arch }}-${{ github.head_ref || github.ref_name }}
             scons-${{ runner.os }}-${{ runner.arch }}-${{ env.MASTER_NEW_BRANCH }}
             scons-${{ runner.os }}-${{ runner.arch }}-${{ env.MASTER_BRANCH }}
             scons-${{ runner.os }}-${{ runner.arch }}
@@ -119,6 +120,7 @@ jobs:
           if [[ "${{ runner.debug }}" == "1" ]]; then
             printenv
           fi
+          PYTHONPATH=$PYTHONPATH:${{ github.workspace }}/ ${{ github.workspace }}/scripts/manage-powersave.py --disable
 
       - name: Build Panda
         run: |
@@ -187,6 +189,11 @@ jobs:
           name: prebuilt
           path: prebuilt.tar.gz
 
+      - name: Re-enable powersave
+        if: always()
+        run: |
+          PYTHONPATH=$PYTHONPATH:${{ github.workspace }}/ ${{ github.workspace }}/scripts/manage-powersave.py --enable
+
   publish:
     concurrency:
       group: publish-${{ github.head_ref || github.ref_name }}

.github/workflows/ui_preview.yaml

@@ -86,7 +86,7 @@ jobs:
         run: >-
           sudo apt-get install -y imagemagick
 
-          scenes="homescreen settings_device settings_software settings_sunnylink settings_toggles settings_sunnypilot settings_sunnypilot_mads settings_developer offroad_alert update_available prime onroad onroad_disengaged onroad_override onroad_sidebar onroad_wide onroad_wide_sidebar onroad_alert_small onroad_alert_mid onroad_alert_full driver_camera body keyboard"
+          scenes="homescreen settings_device settings_software settings_sunnylink settings_toggles settings_sunnypilot settings_sunnypilot_mads settings_trips settings_developer offroad_alert update_available prime onroad onroad_disengaged onroad_override onroad_sidebar onroad_wide onroad_wide_sidebar onroad_alert_small onroad_alert_mid onroad_alert_full driver_camera body keyboard"
           A=($scenes)
 
           DIFF=""

.gitignore

@@ -74,6 +74,9 @@ comma*.sh
 selfdrive/modeld/thneed/compile
 selfdrive/modeld/models/*.thneed
 selfdrive/modeld/models/*.pkl
+sunnypilot/modeld/thneed/compile
+sunnypilot/modeld/models/*.thneed
+sunnypilot/modeld/models/*.pkl
 
 *.bz2
 *.zst

.gitmodules

@@ -15,4 +15,4 @@
   url = https://github.com/commaai/teleoprtc
 [submodule "tinygrad"]
   path = tinygrad_repo
-  url = https://github.com/tinygrad/tinygrad.git
+  url = https://github.com/commaai/tinygrad.git

SConstruct

@@ -392,6 +392,8 @@ SConscript(['third_party/SConscript'])
 
 SConscript(['selfdrive/SConscript'])
 
+SConscript(['sunnypilot/SConscript'])
+
 if Dir('#tools/cabana/').exists() and GetOption('extras'):
   SConscript(['tools/replay/SConscript'])
   if arch != "larch64":

cereal/custom.capnp

@@ -8,6 +8,11 @@ $Cxx.namespace("cereal");
 # cereal, so use these if you want custom events in your fork.
 
 # you can rename the struct, but don't change the identifier
+enum MpcSource {
+  acc @0;
+  blended @1;
+}
+
 struct SelfdriveStateSP @0x81c2f05a394cf4af {
   mads @0 :ModularAssistiveDrivingSystem;
 
@@ -65,6 +70,11 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
     eta @2 :UInt32;
   }
 
+  enum Runner {
+    snpe @0;
+    tinygrad @1;
+  }
+
   struct ModelBundle {
     index @0 :UInt32;
     internalName @1 :Text;
@@ -73,10 +83,15 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
     status @4 :DownloadStatus;
     generation @5 :UInt32;
     environment @6 :Text;
+    runner @7 :Runner;
   }
 }
 
-struct CustomReserved2 @0xf35cc4560bbf6ec2 {
+struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
+  e2eBlended @0 :Text;
+  e2eStatus @1 :Bool;
+  mpcSource @2 :MpcSource;
+  dynamicExperimentalControl @3 :Bool;
 }
 
 struct CustomReserved3 @0xda96579883444c35 {

cereal/log.capnp

@@ -2633,7 +2633,7 @@ struct Event {
     # *********** Custom: reserved for forks ***********
     selfdriveStateSP @107 :Custom.SelfdriveStateSP;
     modelManagerSP @108 :Custom.ModelManagerSP;
-    customReserved2 @109 :Custom.CustomReserved2;
+    longitudinalPlanSP @109 :Custom.LongitudinalPlanSP;
     customReserved3 @110 :Custom.CustomReserved3;
     customReserved4 @111 :Custom.CustomReserved4;
     customReserved5 @112 :Custom.CustomReserved5;

cereal/services.py

@@ -77,6 +77,7 @@ _services: dict[str, tuple] = {
   # sunnypilot
   "modelManagerSP": (False, 1., 1),
   "selfdriveStateSP": (True, 100., 10),
+  "longitudinalPlanSP": (True, 20., 5),
 
   # debug
   "uiDebug": (True, 0., 1),

common/params.cc

@@ -201,7 +201,9 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"Version", PERSISTENT},
 
     // --- sunnypilot params --- //
+    {"ApiCache_DriveStats", PERSISTENT},
     {"EnableGithubRunner", PERSISTENT | BACKUP},
+    {"ModelRunnerTypeCache", CLEAR_ON_ONROAD_TRANSITION},
 
     // MADS params
     {"Mads", PERSISTENT | BACKUP},
@@ -221,6 +223,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"SunnylinkDongleId", PERSISTENT},
     {"SunnylinkdPid", PERSISTENT},
     {"SunnylinkEnabled", PERSISTENT},
+    {"DynamicExperimentalControl", PERSISTENT},
 };
 
 } // namespace

release/ci/install_github_runner.sh

@@ -4,6 +4,7 @@ set -e
 # Default values
 DEFAULT_REPO_URL="https://github.com/sunnypilot"
 START_AT_BOOT=false
+RESTORE_MODE=false
 
 # Parse command line arguments
 while [[ $# -gt 0 ]]; do
@@ -20,6 +21,10 @@ while [[ $# -gt 0 ]]; do
             REPO_URL="$2"
             shift 2
             ;;
+        --restore)
+            RESTORE_MODE=true
+            shift
+            ;;
         *)
             if [ -z "$GITHUB_TOKEN" ]; then
                 GITHUB_TOKEN="$1"
@@ -31,19 +36,6 @@ while [[ $# -gt 0 ]]; do
     esac
 done
 
-# Check required arguments
-if [ -z "$GITHUB_TOKEN" ]; then
-    echo "Usage: $0 [--start-at-boot] [--token <github_token>] [--repo <repository_url>]"
-    echo "Required argument: github_token"
-    echo "Optional arguments:"
-    echo "  --start-at-boot    Enable auto-start at boot (default: false)"
-    echo "  --repo            Repository URL (default: ${DEFAULT_REPO_URL})"
-    exit 1
-fi
-
-# Set repository URL if not provided
-REPO_URL="${REPO_URL:-$DEFAULT_REPO_URL}"
-
 # Determine BASE_DIR based on mount point
 if mountpoint -q /data/media; then
     BASE_DIR="/data/media/0/github"
@@ -60,20 +52,19 @@ LOGS_DIR="${BASE_DIR}/logs"
 CACHE_DIR="${BASE_DIR}/cache"
 OPENPILOT_DIR="${BASE_DIR}/openpilot"
 
-create_directories() {
-    sudo mkdir -p "$RUNNER_DIR" "$BUILDS_DIR" "$LOGS_DIR" "$CACHE_DIR" "$OPENPILOT_DIR"
-    mkdir -p "/data/openpilot"
-    sudo chown -R comma:comma "/data/openpilot"
+# Basic utility functions (no dependencies)
+remount_rw() {
+    sudo mount -o remount,rw /
 }
 
-download_and_setup_runner() {
-    cd "$RUNNER_DIR"
-    curl -o actions-runner-linux-arm64-2.321.0.tar.gz -L https://github.com/actions/runner/releases/download/v2.321.0/actions-runner-linux-arm64-2.321.0.tar.gz
-    tar xzf ./actions-runner-linux-arm64-2.321.0.tar.gz
-    rm ./actions-runner-linux-arm64-2.321.0.tar.gz
-    chmod +x ./config.sh
+remount_ro() {
+    sync || true  # Try to sync but continue even if it fails
+    sudo mount -o remount,ro /  # Always try to remount as read-only
 }
 
+# Always ensure we try to remount as read-only on exit
+trap remount_ro EXIT
+
 setup_runner_user() {
     sudo useradd --comment 'GitHub Runner' --create-home --home-dir ${BASE_DIR} ${RUNNER_USER} --shell /bin/bash -G ${USER_GROUPS} || sudo usermod -aG ${USER_GROUPS} ${RUNNER_USER}
     export BASE_DIR
@@ -84,18 +75,45 @@ create_sudoers_entry() {
     sudo grep -qxF "${RUNNER_USER} ALL=(ALL) NOPASSWD: ALL" /etc/sudoers || echo "${RUNNER_USER} ALL=(ALL) NOPASSWD: ALL" | sudo tee -a /etc/sudoers
 }
 
-configure_runner() {
-    cd "$RUNNER_DIR"
-    sudo -u ${RUNNER_USER} ./config.sh --url "$REPO_URL" --token "$GITHUB_TOKEN" --name $(hostname) --runnergroup "tici-tizi" --labels "tici" --work "$BUILDS_DIR" --unattended
-}
-
 set_directory_permissions() {
     sudo chown -R ${RUNNER_USER}:comma "$BASE_DIR"
     sudo chmod g+rwx "$BASE_DIR"
     sudo chmod g+s "$BASE_DIR"
 }
 
-modify_service_template() {
+setup_directories() {
+    echo "Creating necessary directories..."
+    sudo mkdir -p "$RUNNER_DIR" "$BUILDS_DIR" "$LOGS_DIR" "$CACHE_DIR" "$OPENPILOT_DIR"
+    mkdir -p "/data/openpilot"
+    sudo chown -R comma:comma "/data/openpilot"
+}
+
+# System configuration functions (depends on basic utility functions)
+setup_system_configs() {
+    echo "Setting up system configurations..."
+    setup_runner_user
+    create_sudoers_entry
+    set_directory_permissions
+}
+
+# Runner setup functions
+install_runner() {
+    echo "Downloading and setting up runner..."
+    cd "$RUNNER_DIR"
+    curl -o actions-runner-linux-arm64-2.321.0.tar.gz -L https://github.com/actions/runner/releases/download/v2.321.0/actions-runner-linux-arm64-2.321.0.tar.gz
+    tar xzf ./actions-runner-linux-arm64-2.321.0.tar.gz
+    rm ./actions-runner-linux-arm64-2.321.0.tar.gz
+    chmod +x ./config.sh
+}
+
+configure_runner() {
+    echo "Configuring runner..."
+    cd "$RUNNER_DIR"
+    sudo -u ${RUNNER_USER} ./config.sh --url "$REPO_URL" --token "$GITHUB_TOKEN" --name $(hostname) --runnergroup "tici-tizi" --labels "tici" --work "$BUILDS_DIR" --unattended
+}
+
+create_service_template() {
+    echo "Creating service template..."
     cat <<EOL > "$RUNNER_DIR/bin/actions.runner.service.template"
 [Unit]
 Description={{Description}}
@@ -120,28 +138,116 @@ WantedBy=multi-user.target
 EOL
 }
 
-# Make filesystem writable
-sudo mount -o remount,rw /
+install_service() {
+    echo "Installing systemd service..."
+    cd "$RUNNER_DIR"
+    sudo ./svc.sh install $RUNNER_USER
 
-# Ensure filesystem is remounted as read-only on script exit
-trap "sudo mount -o remount,ro /" EXIT
+    if [ "$START_AT_BOOT" = false ]; then
+        local service_name
+        if [ -f "${RUNNER_DIR}/.service" ]; then
+            service_name=$(cat "${RUNNER_DIR}/.service")
+        else
+            service_name="actions.runner.sunnypilot.$(uname -n)"
+        fi
+        sudo systemctl disable "${service_name}"
+    fi
+}
 
-# Execute installation steps
-setup_runner_user
-create_sudoers_entry
-create_directories
-download_and_setup_runner
-modify_service_template
-configure_runner
-set_directory_permissions
+check_restore_prerequisites() {
+    local needs_restore=false
+    local can_restore=false
+    local service_name=""
 
-# Install and start service using built-in installer
-cd "$RUNNER_DIR"
-sudo ./svc.sh install $RUNNER_USER
+    # Check if base runner directory exists
+    if [ ! -d "${RUNNER_DIR}" ]; then
+        echo "ERROR: Runner directory ${RUNNER_DIR} does not exist"
+        echo "This directory is required for restore operations"
+        exit 1
+    fi
 
-# Handle auto-start configuration
-if [ "$START_AT_BOOT" = false ]; then
-    sudo systemctl disable actions.runner.sunnypilot.$(uname -n)
-fi
+    # First check if we have the required files for restoration
+    if [ -f "${RUNNER_DIR}/.credentials" ] && [ -f "${RUNNER_DIR}/.service" ]; then
+        can_restore=true
+        service_name=$(cat "${RUNNER_DIR}/.service")
+        echo "Found required runner configuration files"
+    else
+        echo "Missing required runner configuration files"
+        echo "Required: .credentials and .service files in ${RUNNER_DIR}"
+        exit 1
+    fi
 
-sudo ./svc.sh start
+    # Then check if restoration is needed (if either service or user is missing)
+    if ! systemctl list-unit-files "${service_name}" &>/dev/null; then
+        echo "Service ${service_name} not found in systemd"
+        needs_restore=true
+    fi
+
+    if ! id "${RUNNER_USER}" &>/dev/null; then
+        echo "User ${RUNNER_USER} does not exist"
+        needs_restore=true
+    fi
+
+    # Only proceed if we can restore AND need to restore
+    if [ "$can_restore" = true ] && [ "$needs_restore" = true ]; then
+        echo "Restoration is needed and possible"
+        return 0
+    else
+        if [ "$needs_restore" = false ]; then
+            echo "System is already properly configured (user and service exist)"
+        fi
+        exit 0
+    fi
+}
+
+perform_restore() {
+    echo "Starting runner restoration..."
+    setup_directories
+    remount_rw
+    setup_system_configs
+    install_service
+    remount_ro
+    echo "Runner restoration completed successfully"
+}
+
+perform_install() {
+    echo "Starting fresh installation..."
+    setup_directories
+    install_runner
+    create_service_template
+    remount_rw
+    setup_system_configs
+    configure_runner
+    install_service
+    remount_ro
+    echo "Installation completed successfully"
+}
+
+main() {
+    if [ "$RESTORE_MODE" = true ]; then
+        echo "Running in restore mode - will only restore system configurations..."
+        check_restore_prerequisites
+        perform_restore
+    else
+        # Check required arguments for normal installation
+        if [ -z "$GITHUB_TOKEN" ]; then
+            echo "Usage: $0 [--start-at-boot] [--token <github_token>] [--repo <repository_url>] [--restore]"
+            echo "Required argument (except for --restore): github_token"
+            echo "Optional arguments:"
+            echo "  --start-at-boot    Enable auto-start at boot (default: false)"
+            echo "  --repo            Repository URL (default: ${DEFAULT_REPO_URL})"
+            echo "  --restore         Restore existing runner configuration"
+            exit 1
+        fi
+
+        # Set repository URL if not provided
+        REPO_URL="${REPO_URL:-$DEFAULT_REPO_URL}"
+        perform_install
+    fi
+
+    echo "Starting runner service..."
+    cd "$RUNNER_DIR"
+    sudo ./svc.sh start
+}
+
+main

scripts/manage-powersave.py

@@ -0,0 +1,27 @@
+#!/usr/bin/env python3
+import argparse
+import multiprocessing
+from openpilot.system.hardware import HARDWARE
+
+
+def main():
+  parser = argparse.ArgumentParser(description='Control power saving mode')
+  parser.add_argument('--enable', action='store_true', help='Enable power saving mode')
+  parser.add_argument('--disable', action='store_true', help='Disable power saving mode')
+  args = parser.parse_args()
+
+  if args.enable and args.disable:
+    parser.error("Cannot specify both --enable and --disable")
+  elif not (args.enable or args.disable):
+    parser.error("Must specify either --enable or --disable")
+
+  print(f"Number of CPU cores available before: [{multiprocessing.cpu_count()}]")
+  HARDWARE.set_power_save(args.enable)
+
+  state = "enabled" if args.enable else "disabled"
+  print(f"Power save mode set to: [{state}]")
+  print(f"Number of CPU cores available now: [{multiprocessing.cpu_count()}]")
+
+
+if __name__ == "__main__":
+  main()

selfdrive/car/card.py

@@ -76,6 +76,8 @@ class Car:
     self.CC_prev = car.CarControl.new_message()
     self.initialized_prev = False
 
+    self.dynamic_experimental_control = False
+
     self.last_actuators_output = structs.CarControl.Actuators()
 
     self.params = Params()
@@ -157,6 +159,7 @@ class Car:
 
     self.is_metric = self.params.get_bool("IsMetric")
     self.experimental_mode = self.params.get_bool("ExperimentalMode")
+    self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl")
 
     # card is driven by can recv, expected at 100Hz
     self.rk = Ratekeeper(100, print_delay_threshold=None)

selfdrive/controls/controlsd.py

@@ -38,7 +38,7 @@ class Controls:
 
     self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
                                    'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
-                                   'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState')
+                                   'driverMonitoringState', 'onroadEvents', 'driverAssistance', 'longitudinalPlanSP'], poll='selfdriveState')
     self.pm = messaging.PubMaster(['carControl', 'controlsState'])
 
     self.steer_limited = False

selfdrive/controls/lib/longitudinal_planner.py

@@ -15,6 +15,8 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDX
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_error
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
 from openpilot.common.swaglog import cloudlog
+from sunnypilot.selfdrive.controls.dec.dynamic_experimental_controller import DynamicExperimentalController
+from sunnypilot.selfdrive.controls.dec.helpers import get_mpc_mode, publish_longitudinal_plan_sp
 
 LON_MPC_STEP = 0.2  # first step is 0.2s
 A_CRUISE_MIN = -1.2
@@ -83,6 +85,7 @@ class LongitudinalPlanner:
     self.a_desired_trajectory = np.zeros(CONTROL_N)
     self.j_desired_trajectory = np.zeros(CONTROL_N)
     self.solverExecutionTime = 0.0
+    self.dynamic_experimental_controller = DynamicExperimentalController()
 
   @staticmethod
   def parse_model(model_msg, model_error):
@@ -105,7 +108,7 @@ class LongitudinalPlanner:
     return x, v, a, j, throttle_prob
 
   def update(self, sm):
-    self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
+    self.mpc.mode = get_mpc_mode(sm, self.dynamic_experimental_controller, self.mpc, self.CP)
 
     if len(sm['carControl'].orientationNED) == 3:
       accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
@@ -206,3 +209,4 @@ class LongitudinalPlanner:
     longitudinalPlan.allowThrottle = self.allow_throttle
 
     pm.send('longitudinalPlan', plan_send)
+    publish_longitudinal_plan_sp(sm, pm, self.mpc, self.dynamic_experimental_controller)

selfdrive/controls/plannerd.py

@@ -18,7 +18,7 @@ def main():
 
   ldw = LaneDepartureWarning()
   longitudinal_planner = LongitudinalPlanner(CP)
-  pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance'])
+  pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
   sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'],
                            poll='modelV2', ignore_avg_freq=['radarState'])
 

selfdrive/debug/uiview.py

@@ -3,14 +3,19 @@ import time
 
 from cereal import car, log, messaging
 from openpilot.common.params import Params
-from openpilot.system.manager.process_config import managed_processes
+from openpilot.system.manager.process_config import managed_processes, is_snpe_model
 from openpilot.system.hardware import HARDWARE
 
 if __name__ == "__main__":
   CP = car.CarParams(notCar=True, wheelbase=1, steerRatio=10)
-  Params().put("CarParams", CP.to_bytes())
+  params = Params()
+  params.put("CarParams", CP.to_bytes())
+  if use_snpe_modeld := is_snpe_model(False, params, CP):
+    print("Using SNPE modeld")
+  HARDWARE.set_power_save(False)
 
-  procs = ['camerad', 'ui', 'modeld', 'calibrationd', 'plannerd', 'dmonitoringmodeld', 'dmonitoringd']
+  procs = ['camerad', 'ui', 'calibrationd', 'plannerd', 'dmonitoringmodeld', 'dmonitoringd']
+  procs += ["modeld_snpe" if use_snpe_modeld else "modeld"]
   for p in procs:
     managed_processes[p].start()
 

selfdrive/test/test_onroad.py

@@ -103,7 +103,7 @@ TIMINGS = {
 
 LOGS_SIZE_RATE = {
   "qlog": 0.0083,
-  "rlog": 0.1528,
+  "rlog": 0.135,
   "qcamera.ts": 0.03828,
 }
 LOGS_SIZE_RATE.update(dict.fromkeys(['ecamera.hevc', 'fcamera.hevc'], 1.2740))

selfdrive/ui/SConscript

@@ -34,7 +34,7 @@ Export('widgets')
 qt_libs = [widgets, qt_util] + base_libs
 
 qt_src = ["main.cc", "ui.cc", "qt/sidebar.cc", "qt/body.cc",
-          "qt/window.cc", "qt/home.cc", "qt/offroad/settings.cc",
+          "qt/window.cc", "qt/home.cc", "qt/offroad/settings.cc", "qt/offroad/offroad_home.cc",
           "qt/offroad/software_settings.cc", "qt/offroad/developer_panel.cc", "qt/offroad/onboarding.cc",
           "qt/offroad/driverview.cc", "qt/offroad/experimental_mode.cc",
           "qt/onroad/onroad_home.cc", "qt/onroad/annotated_camera.cc", "qt/onroad/model.cc",

selfdrive/ui/qt/home.cc

@@ -2,12 +2,8 @@
 
 #include <QHBoxLayout>
 #include <QMouseEvent>
-#include <QStackedWidget>
-#include <QVBoxLayout>
 
-#include "selfdrive/ui/qt/offroad/experimental_mode.h"
 #include "selfdrive/ui/qt/util.h"
-#include "selfdrive/ui/qt/widgets/prime.h"
 
 // HomeWindow: the container for the offroad and onroad UIs
 
@@ -82,147 +78,3 @@ void HomeWindow::mouseDoubleClickEvent(QMouseEvent* e) {
     showSidebar(false);
   }
 }
-
-// OffroadHome: the offroad home page
-
-OffroadHome::OffroadHome(QWidget* parent) : QFrame(parent) {
-  QVBoxLayout* main_layout = new QVBoxLayout(this);
-  main_layout->setContentsMargins(40, 40, 40, 40);
-
-  // top header
-  QHBoxLayout* header_layout = new QHBoxLayout();
-  header_layout->setContentsMargins(0, 0, 0, 0);
-  header_layout->setSpacing(16);
-
-  update_notif = new QPushButton(tr("UPDATE"));
-  update_notif->setVisible(false);
-  update_notif->setStyleSheet("background-color: #364DEF;");
-  QObject::connect(update_notif, &QPushButton::clicked, [=]() { center_layout->setCurrentIndex(1); });
-  header_layout->addWidget(update_notif, 0, Qt::AlignHCenter | Qt::AlignLeft);
-
-  alert_notif = new QPushButton();
-  alert_notif->setVisible(false);
-  alert_notif->setStyleSheet("background-color: #E22C2C;");
-  QObject::connect(alert_notif, &QPushButton::clicked, [=] { center_layout->setCurrentIndex(2); });
-  header_layout->addWidget(alert_notif, 0, Qt::AlignHCenter | Qt::AlignLeft);
-
-  version = new ElidedLabel();
-  header_layout->addWidget(version, 0, Qt::AlignHCenter | Qt::AlignRight);
-
-  main_layout->addLayout(header_layout);
-
-  // main content
-  main_layout->addSpacing(25);
-  center_layout = new QStackedLayout();
-
-  QWidget *home_widget = new QWidget(this);
-  {
-    QHBoxLayout *home_layout = new QHBoxLayout(home_widget);
-    home_layout->setContentsMargins(0, 0, 0, 0);
-    home_layout->setSpacing(30);
-
-    // left: PrimeAdWidget
-    QStackedWidget *left_widget = new QStackedWidget(this);
-    QVBoxLayout *left_prime_layout = new QVBoxLayout();
-    left_prime_layout->setContentsMargins(0, 0, 0, 0);
-    QWidget *prime_user = new PrimeUserWidget();
-    prime_user->setStyleSheet(R"(
-    border-radius: 10px;
-    background-color: #333333;
-    )");
-    left_prime_layout->addWidget(prime_user);
-    left_prime_layout->addStretch();
-    left_widget->addWidget(new LayoutWidget(left_prime_layout));
-    left_widget->addWidget(new PrimeAdWidget);
-    left_widget->setStyleSheet("border-radius: 10px;");
-
-    connect(uiState()->prime_state, &PrimeState::changed, [left_widget]() {
-      left_widget->setCurrentIndex(uiState()->prime_state->isSubscribed() ? 0 : 1);
-    });
-
-    home_layout->addWidget(left_widget, 1);
-
-    // right: ExperimentalModeButton, SetupWidget
-    QWidget* right_widget = new QWidget(this);
-    QVBoxLayout* right_column = new QVBoxLayout(right_widget);
-    right_column->setContentsMargins(0, 0, 0, 0);
-    right_widget->setFixedWidth(750);
-    right_column->setSpacing(30);
-
-    ExperimentalModeButton *experimental_mode = new ExperimentalModeButton(this);
-    QObject::connect(experimental_mode, &ExperimentalModeButton::openSettings, this, &OffroadHome::openSettings);
-    right_column->addWidget(experimental_mode, 1);
-
-    SetupWidget *setup_widget = new SetupWidget;
-    QObject::connect(setup_widget, &SetupWidget::openSettings, this, &OffroadHome::openSettings);
-    right_column->addWidget(setup_widget, 1);
-
-    home_layout->addWidget(right_widget, 1);
-  }
-  center_layout->addWidget(home_widget);
-
-  // add update & alerts widgets
-  update_widget = new UpdateAlert();
-  QObject::connect(update_widget, &UpdateAlert::dismiss, [=]() { center_layout->setCurrentIndex(0); });
-  center_layout->addWidget(update_widget);
-  alerts_widget = new OffroadAlert();
-  QObject::connect(alerts_widget, &OffroadAlert::dismiss, [=]() { center_layout->setCurrentIndex(0); });
-  center_layout->addWidget(alerts_widget);
-
-  main_layout->addLayout(center_layout, 1);
-
-  // set up refresh timer
-  timer = new QTimer(this);
-  timer->callOnTimeout(this, &OffroadHome::refresh);
-
-  setStyleSheet(R"(
-    * {
-      color: white;
-    }
-    OffroadHome {
-      background-color: black;
-    }
-    OffroadHome > QPushButton {
-      padding: 15px 30px;
-      border-radius: 5px;
-      font-size: 40px;
-      font-weight: 500;
-    }
-    OffroadHome > QLabel {
-      font-size: 55px;
-    }
-  )");
-}
-
-void OffroadHome::showEvent(QShowEvent *event) {
-  refresh();
-  timer->start(10 * 1000);
-}
-
-void OffroadHome::hideEvent(QHideEvent *event) {
-  timer->stop();
-}
-
-void OffroadHome::refresh() {
-  version->setText(getBrand() + " " +  QString::fromStdString(params.get("UpdaterCurrentDescription")));
-
-  bool updateAvailable = update_widget->refresh();
-  int alerts = alerts_widget->refresh();
-
-  // pop-up new notification
-  int idx = center_layout->currentIndex();
-  if (!updateAvailable && !alerts) {
-    idx = 0;
-  } else if (updateAvailable && (!update_notif->isVisible() || (!alerts && idx == 2))) {
-    idx = 1;
-  } else if (alerts && (!alert_notif->isVisible() || (!updateAvailable && idx == 1))) {
-    idx = 2;
-  }
-  center_layout->setCurrentIndex(idx);
-
-  update_notif->setVisible(updateAvailable);
-  alert_notif->setVisible(alerts);
-  if (alerts) {
-    alert_notif->setText(QString::number(alerts) + (alerts > 1 ? tr(" ALERTS") : tr(" ALERT")));
-  }
-}

selfdrive/ui/qt/home.h

@@ -7,49 +7,26 @@
 #include <QTimer>
 #include <QWidget>
 
-#include "common/params.h"
-#include "selfdrive/ui/qt/body.h"
-#include "selfdrive/ui/qt/widgets/offroad_alerts.h"
 #include "selfdrive/ui/ui.h"
 
 #ifdef SUNNYPILOT
 #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
 #include "selfdrive/ui/sunnypilot/qt/onroad/onroad_home.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/offroad_home.h"
 #include "selfdrive/ui/sunnypilot/qt/sidebar.h"
 #define OnroadWindow OnroadWindowSP
+#define OffroadHome OffroadHomeSP
 #define LayoutWidget LayoutWidgetSP
 #define Sidebar SidebarSP
 #define ElidedLabel ElidedLabelSP
+#define SetupWidget SetupWidgetSP
 #else
 #include "selfdrive/ui/qt/widgets/controls.h"
 #include "selfdrive/ui/qt/onroad/onroad_home.h"
 #include "selfdrive/ui/qt/sidebar.h"
 #endif
 
-class OffroadHome : public QFrame {
-  Q_OBJECT
-
-public:
-  explicit OffroadHome(QWidget* parent = 0);
-
-signals:
-  void openSettings(int index = 0, const QString &param = "");
-
-private:
-  void showEvent(QShowEvent *event) override;
-  void hideEvent(QHideEvent *event) override;
-  void refresh();
-
-  Params params;
-
-  QTimer* timer;
-  ElidedLabel* version;
-  QStackedLayout* center_layout;
-  UpdateAlert *update_widget;
-  OffroadAlert* alerts_widget;
-  QPushButton* alert_notif;
-  QPushButton* update_notif;
-};
+#include "selfdrive/ui/qt/offroad/offroad_home.h"
 
 class HomeWindow : public QWidget {
   Q_OBJECT

selfdrive/ui/qt/offroad/experimental_mode.cc

@@ -8,12 +8,18 @@
 
 #include "selfdrive/ui/ui.h"
 
+#ifdef SUNNYPILOT
+constexpr int toggles_settings_index = 3;
+#else
+constexpr int toggles_settings_index = 2;
+#endif
+
 ExperimentalModeButton::ExperimentalModeButton(QWidget *parent) : QPushButton(parent) {
   chill_pixmap = QPixmap("../assets/img_couch.svg").scaledToWidth(img_width, Qt::SmoothTransformation);
   experimental_pixmap = QPixmap("../assets/img_experimental_grey.svg").scaledToWidth(img_width, Qt::SmoothTransformation);
 
   // go to toggles and expand experimental mode description
-  connect(this, &QPushButton::clicked, [=]() { emit openSettings(2, "ExperimentalMode"); });
+  connect(this, &QPushButton::clicked, [=]() { emit openSettings(toggles_settings_index, "ExperimentalMode"); });
 
   setFixedHeight(125);
   QHBoxLayout *main_layout = new QHBoxLayout;

selfdrive/ui/qt/offroad/offroad_home.cc

@@ -0,0 +1,158 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#include "selfdrive/ui/qt/offroad/offroad_home.h"
+
+#include "selfdrive/ui/qt/offroad/experimental_mode.h"
+#include "selfdrive/ui/qt/util.h"
+#include "selfdrive/ui/qt/widgets/prime.h"
+
+// OffroadHome: the offroad home page
+
+OffroadHome::OffroadHome(QWidget* parent) : QFrame(parent) {
+  QVBoxLayout* main_layout = new QVBoxLayout(this);
+  main_layout->setContentsMargins(40, 40, 40, 40);
+
+  // top header
+  QHBoxLayout* header_layout = new QHBoxLayout();
+  header_layout->setContentsMargins(0, 0, 0, 0);
+  header_layout->setSpacing(16);
+
+  update_notif = new QPushButton(tr("UPDATE"));
+  update_notif->setVisible(false);
+  update_notif->setStyleSheet("background-color: #364DEF;");
+  QObject::connect(update_notif, &QPushButton::clicked, [=]() { center_layout->setCurrentIndex(1); });
+  header_layout->addWidget(update_notif, 0, Qt::AlignHCenter | Qt::AlignLeft);
+
+  alert_notif = new QPushButton();
+  alert_notif->setVisible(false);
+  alert_notif->setStyleSheet("background-color: #E22C2C;");
+  QObject::connect(alert_notif, &QPushButton::clicked, [=] { center_layout->setCurrentIndex(2); });
+  header_layout->addWidget(alert_notif, 0, Qt::AlignHCenter | Qt::AlignLeft);
+
+  version = new ElidedLabel();
+  header_layout->addWidget(version, 0, Qt::AlignHCenter | Qt::AlignRight);
+
+  main_layout->addLayout(header_layout);
+
+  // main content
+  main_layout->addSpacing(25);
+  center_layout = new QStackedLayout();
+
+  QWidget *home_widget = new QWidget(this);
+  {
+    home_layout = new QHBoxLayout(home_widget);
+    home_layout->setContentsMargins(0, 0, 0, 0);
+    home_layout->setSpacing(30);
+
+#ifndef SUNNYPILOT
+    // left: PrimeAdWidget
+    QStackedWidget *left_widget = new QStackedWidget(this);
+    QVBoxLayout *left_prime_layout = new QVBoxLayout();
+    left_prime_layout->setContentsMargins(0, 0, 0, 0);
+    QWidget *prime_user = new PrimeUserWidget();
+    prime_user->setStyleSheet(R"(
+    border-radius: 10px;
+    background-color: #333333;
+    )");
+    left_prime_layout->addWidget(prime_user);
+    left_prime_layout->addStretch();
+    left_widget->addWidget(new LayoutWidget(left_prime_layout));
+    left_widget->addWidget(new PrimeAdWidget);
+    left_widget->setStyleSheet("border-radius: 10px;");
+
+    connect(uiState()->prime_state, &PrimeState::changed, [left_widget]() {
+      left_widget->setCurrentIndex(uiState()->prime_state->isSubscribed() ? 0 : 1);
+    });
+
+    home_layout->addWidget(left_widget, 1);
+#endif
+
+    // right: ExperimentalModeButton, SetupWidget
+    QWidget* right_widget = new QWidget(this);
+    QVBoxLayout* right_column = new QVBoxLayout(right_widget);
+    right_column->setContentsMargins(0, 0, 0, 0);
+    right_widget->setFixedWidth(750);
+    right_column->setSpacing(30);
+
+    ExperimentalModeButton *experimental_mode = new ExperimentalModeButton(this);
+    QObject::connect(experimental_mode, &ExperimentalModeButton::openSettings, this, &OffroadHome::openSettings);
+    right_column->addWidget(experimental_mode, 1);
+
+    SetupWidget *setup_widget = new SetupWidget;
+    QObject::connect(setup_widget, &SetupWidget::openSettings, this, &OffroadHome::openSettings);
+    right_column->addWidget(setup_widget, 1);
+
+    home_layout->addWidget(right_widget, 1);
+  }
+  center_layout->addWidget(home_widget);
+
+  // add update & alerts widgets
+  update_widget = new UpdateAlert();
+  QObject::connect(update_widget, &UpdateAlert::dismiss, [=]() { center_layout->setCurrentIndex(0); });
+  center_layout->addWidget(update_widget);
+  alerts_widget = new OffroadAlert();
+  QObject::connect(alerts_widget, &OffroadAlert::dismiss, [=]() { center_layout->setCurrentIndex(0); });
+  center_layout->addWidget(alerts_widget);
+
+  main_layout->addLayout(center_layout, 1);
+
+  // set up refresh timer
+  timer = new QTimer(this);
+  timer->callOnTimeout(this, &OffroadHome::refresh);
+
+  setStyleSheet(R"(
+    * {
+      color: white;
+    }
+    OffroadHome {
+      background-color: black;
+    }
+    OffroadHome > QPushButton {
+      padding: 15px 30px;
+      border-radius: 5px;
+      font-size: 40px;
+      font-weight: 500;
+    }
+    OffroadHome > QLabel {
+      font-size: 55px;
+    }
+  )");
+}
+
+void OffroadHome::showEvent(QShowEvent *event) {
+  refresh();
+  timer->start(10 * 1000);
+}
+
+void OffroadHome::hideEvent(QHideEvent *event) {
+  timer->stop();
+}
+
+void OffroadHome::refresh() {
+  version->setText(getBrand() + " " +  QString::fromStdString(params.get("UpdaterCurrentDescription")));
+
+  bool updateAvailable = update_widget->refresh();
+  int alerts = alerts_widget->refresh();
+
+  // pop-up new notification
+  int idx = center_layout->currentIndex();
+  if (!updateAvailable && !alerts) {
+    idx = 0;
+  } else if (updateAvailable && (!update_notif->isVisible() || (!alerts && idx == 2))) {
+    idx = 1;
+  } else if (alerts && (!alert_notif->isVisible() || (!updateAvailable && idx == 1))) {
+    idx = 2;
+  }
+  center_layout->setCurrentIndex(idx);
+
+  update_notif->setVisible(updateAvailable);
+  alert_notif->setVisible(alerts);
+  if (alerts) {
+    alert_notif->setText(QString::number(alerts) + (alerts > 1 ? tr(" ALERTS") : tr(" ALERT")));
+  }
+}

selfdrive/ui/qt/offroad/offroad_home.h

@@ -0,0 +1,57 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#pragma once
+
+#include "common/params.h"
+#include "selfdrive/ui/qt/body.h"
+#include "selfdrive/ui/qt/widgets/offroad_alerts.h"
+
+#ifdef SUNNYPILOT
+#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
+#include "selfdrive/ui/sunnypilot/qt/onroad/onroad_home.h"
+#include "selfdrive/ui/sunnypilot/qt/sidebar.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/prime.h"
+#define OnroadWindow OnroadWindowSP
+#define LayoutWidget LayoutWidgetSP
+#define Sidebar SidebarSP
+#define ElidedLabel ElidedLabelSP
+#define SetupWidget SetupWidgetSP
+#else
+#include "selfdrive/ui/qt/widgets/controls.h"
+#include "selfdrive/ui/qt/onroad/onroad_home.h"
+#include "selfdrive/ui/qt/sidebar.h"
+#include "selfdrive/ui/qt/widgets/prime.h"
+#endif
+
+class OffroadHome : public QFrame {
+  Q_OBJECT
+
+public:
+  explicit OffroadHome(QWidget* parent = 0);
+
+  signals:
+    void openSettings(int index = 0, const QString &param = "");
+
+protected:
+  QHBoxLayout *home_layout;
+
+private:
+  void showEvent(QShowEvent *event) override;
+  void hideEvent(QHideEvent *event) override;
+  void refresh();
+
+  Params params;
+
+  QTimer* timer;
+  ElidedLabel* version;
+  QStackedLayout* center_layout;
+  UpdateAlert *update_widget;
+  OffroadAlert* alerts_widget;
+  QPushButton* alert_notif;
+  QPushButton* update_notif;
+};

selfdrive/ui/qt/offroad/settings.cc

@@ -40,6 +40,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
       "",
       "../assets/img_experimental_white.svg",
     },
+    {
+      "DynamicExperimentalControl",
+      tr("Enable Dynamic Experimental Control"),
+      tr("Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal."),
+      "../assets/offroad/icon_blank.png",
+    },
     {
       "DisengageOnAccelerator",
       tr("Disengage on Accelerator Pedal"),
@@ -356,8 +362,7 @@ void SettingsWindow::setCurrentPanel(int index, const QString &param) {
 }
 
 SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) {
-  RETURN_IF_SUNNYPILOT
-
+#ifndef SUNNYPILOT
   // setup two main layouts
   sidebar_widget = new QWidget;
   QVBoxLayout *sidebar_layout = new QVBoxLayout(sidebar_widget);
@@ -457,4 +462,5 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) {
       border-radius: 30px;
     }
   )");
+#endif
 }

selfdrive/ui/qt/onroad/model.cc

@@ -106,8 +106,14 @@ void ModelRenderer::drawLaneLines(QPainter &painter) {
 }
 
 void ModelRenderer::drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, int height) {
+
+  auto *s = uiState();
+  auto &sm = *(s->sm);
+  const auto long_plan_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP();
+  bool exp_mode_path = (long_plan_sp.getDynamicExperimentalControl() && long_plan_sp.getMpcSource() == cereal::MpcSource::BLENDED) ||
+                       (!long_plan_sp.getDynamicExperimentalControl() && sm["selfdriveState"].getSelfdriveState().getExperimentalMode());
   QLinearGradient bg(0, height, 0, 0);
-  if (experimental_mode) {
+  if (exp_mode_path) {
     // The first half of track_vertices are the points for the right side of the path
     const auto &acceleration = model.getAcceleration().getX();
     const int max_len = std::min<int>(track_vertices.length() / 2, acceleration.size());

selfdrive/ui/qt/util.h

@@ -13,12 +13,6 @@
 #include "cereal/gen/cpp/car.capnp.h"
 #include "common/params.h"
 
-#ifdef SUNNYPILOT
-#define RETURN_IF_SUNNYPILOT return;
-#else
-#define RETURN_IF_SUNNYPILOT  // Do nothing
-#endif
-
 QString getVersion();
 QString getBrand();
 QString getUserAgent();

selfdrive/ui/qt/widgets/controls.cc

@@ -6,7 +6,7 @@
 #include "selfdrive/ui/qt/util.h"
 
 AbstractControl::AbstractControl(const QString &title, const QString &desc, const QString &icon, QWidget *parent) : QFrame(parent) {
-  RETURN_IF_SUNNYPILOT
+#ifndef SUNNYPILOT
   QVBoxLayout *main_layout = new QVBoxLayout(this);
   main_layout->setMargin(0);
 
@@ -57,6 +57,7 @@ AbstractControl::AbstractControl(const QString &title, const QString &desc, cons
   });
 
   main_layout->addStretch();
+#endif
 }
 
 void AbstractControl::hideEvent(QHideEvent *e) {

selfdrive/ui/qt/widgets/prime.cc

@@ -221,7 +221,7 @@ SetupWidget::SetupWidget(QWidget* parent) : QFrame(parent) {
   outer_layout->addWidget(mainLayout);
 
   QWidget *content = new QWidget;
-  QVBoxLayout *content_layout = new QVBoxLayout(content);
+  content_layout = new QVBoxLayout(content);
   content_layout->setContentsMargins(0, 0, 0, 0);
   content_layout->setSpacing(30);
 

selfdrive/ui/qt/widgets/prime.h

@@ -63,6 +63,9 @@ public:
 signals:
   void openSettings(int index = 0, const QString &param = "");
 
+protected:
+  QVBoxLayout *content_layout;
+
 private:
   PairingPopup *popup;
   QStackedWidget *mainLayout;

selfdrive/ui/sunnypilot/SConscript

@@ -1,6 +1,8 @@
 widgets_src = [
   "sunnypilot/qt/widgets/toggle.cc",
   "sunnypilot/qt/widgets/controls.cc",
+  "sunnypilot/qt/widgets/drive_stats.cc",
+  "sunnypilot/qt/widgets/prime.cc",
   "sunnypilot/qt/widgets/scrollview.cc",
 ]
 
@@ -13,10 +15,12 @@ qt_src = [
   "sunnypilot/qt/sidebar.cc",
   "sunnypilot/qt/window.cc",
   "sunnypilot/qt/home.cc",
+  "sunnypilot/qt/offroad/offroad_home.cc",
   "sunnypilot/qt/offroad/settings/settings.cc",
   "sunnypilot/qt/offroad/settings/software_panel.cc",
   "sunnypilot/qt/offroad/settings/sunnylink_panel.cc",
   "sunnypilot/qt/offroad/settings/sunnypilot_panel.cc",
+  "sunnypilot/qt/offroad/settings/trips_panel.cc",
   "sunnypilot/qt/onroad/annotated_camera.cc",
   "sunnypilot/qt/onroad/hud.cc",
   "sunnypilot/qt/onroad/model.cc",

selfdrive/ui/sunnypilot/qt/home.cc

@@ -7,6 +7,8 @@
 
 #include "selfdrive/ui/sunnypilot/qt/home.h"
 
+#include "selfdrive/ui/sunnypilot/qt/widgets/drive_stats.h"
+
 HomeWindowSP::HomeWindowSP(QWidget *parent) : HomeWindow(parent) {
 }
 

selfdrive/ui/sunnypilot/qt/offroad/offroad_home.cc

@@ -0,0 +1,20 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#include "selfdrive/ui/sunnypilot/qt/offroad/offroad_home.h"
+
+#include <QStackedWidget>
+
+#include "selfdrive/ui/sunnypilot/qt/widgets/drive_stats.h"
+
+OffroadHomeSP::OffroadHomeSP(QWidget *parent) : OffroadHome(parent) {
+  QStackedWidget *left_widget = new QStackedWidget(this);
+  left_widget->addWidget(new DriveStats(this));
+  left_widget->setStyleSheet("border-radius: 10px;");
+
+  home_layout->insertWidget(0, left_widget);
+}

selfdrive/ui/sunnypilot/qt/offroad/offroad_home.h

@@ -0,0 +1,17 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#pragma once
+
+#include "selfdrive/ui/qt/offroad/offroad_home.h"
+
+class OffroadHomeSP : public OffroadHome {
+  Q_OBJECT
+
+public:
+  explicit OffroadHomeSP(QWidget *parent = 0);
+};

selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc

@@ -14,6 +14,7 @@
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink_panel.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_panel.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/trips_panel.h"
 
 TogglesPanelSP::TogglesPanelSP(SettingsWindow *parent) : TogglesPanel(parent) {
   QObject::connect(uiStateSP(), &UIStateSP::uiUpdate, this, &TogglesPanelSP::updateState);
@@ -74,6 +75,7 @@ SettingsWindowSP::SettingsWindowSP(QWidget *parent) : SettingsWindow(parent) {
     PanelInfo("   " + tr("Toggles"), toggles, "../../sunnypilot/selfdrive/assets/offroad/icon_toggle.png"),
     PanelInfo("   " + tr("Software"), new SoftwarePanelSP(this), "../../sunnypilot/selfdrive/assets/offroad/icon_software.png"),
     PanelInfo("   " + tr("sunnypilot"), new SunnypilotPanel(this), "../assets/images/button_home.png"),
+    PanelInfo("   " + tr("Trips"), new TripsPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_trips.png"),
     PanelInfo("   " + tr("Developer"), new DeveloperPanel(this), "../assets/offroad/icon_shell.png"),
   };
 

selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc

@@ -29,16 +29,17 @@ SoftwarePanelSP::SoftwarePanelSP(QWidget *parent) : SoftwarePanel(parent) {
  * Reads status from modelManagerSP cereal message and displays status for all models
  */
 void SoftwarePanelSP::handleBundleDownloadProgress() {
-  const SubMaster &sm = *(uiStateSP()->sm);
-  const auto model_manager = sm["modelManagerSP"].getModelManagerSP();
-
-  if (!model_manager.hasSelectedBundle()) {
-    currentModelLblBtn->setDescription("");
+  using DS = cereal::ModelManagerSP::DownloadStatus;
+  if (!model_manager.hasSelectedBundle() && !model_manager.hasActiveBundle()) {
+    currentModelLblBtn->setDescription(tr("No custom model selected!"));
     return;
   }
 
-  const auto &bundle = model_manager.getSelectedBundle();
+  const bool showSelectedBundle = model_manager.hasSelectedBundle() && (isDownloading() || model_manager.getSelectedBundle().getStatus() == DS::FAILED);
+  const auto &bundle = showSelectedBundle ? model_manager.getSelectedBundle() : model_manager.getActiveBundle();
   const auto &models = bundle.getModels();
+  download_status = bundle.getStatus();
+  const auto download_status_changed = prev_download_status != download_status;
   QStringList status;
 
   // Get status for each model type in order
@@ -67,9 +68,9 @@ void SoftwarePanelSP::handleBundleDownloadProgress() {
     if (progress.getStatus() == cereal::ModelManagerSP::DownloadStatus::DOWNLOADING) {
       line = tr("Downloading %1 model [%2]... (%3%)").arg(typeName, modelName).arg(progress.getProgress(), 0, 'f', 2);
     } else if (progress.getStatus() == cereal::ModelManagerSP::DownloadStatus::DOWNLOADED) {
-      line = tr("%1 model [%2] downloaded").arg(typeName, modelName);
+      line = tr("%1 model [%2] %3").arg(typeName, modelName, download_status_changed ? tr("downloaded") : tr("ready"));
     } else if (progress.getStatus() == cereal::ModelManagerSP::DownloadStatus::CACHED) {
-      line = tr("%1 model [%2] from cache").arg(typeName, modelName);
+      line = tr("%1 model [%2] %3").arg(typeName, modelName, download_status_changed ? tr("from cache") : tr("ready"));
     } else if (progress.getStatus() == cereal::ModelManagerSP::DownloadStatus::FAILED) {
       line = tr("%1 model [%2] download failed").arg(typeName, modelName);
     } else {
@@ -80,9 +81,19 @@ void SoftwarePanelSP::handleBundleDownloadProgress() {
 
   currentModelLblBtn->setDescription(status.join("\n"));
 
-  if (bundle.getStatus() == cereal::ModelManagerSP::DownloadStatus::DOWNLOADING) {
-    currentModelLblBtn->showDescription();
+  if (prev_download_status != download_status) {
+    switch (bundle.getStatus()) {
+      case cereal::ModelManagerSP::DownloadStatus::DOWNLOADING:
+      case cereal::ModelManagerSP::DownloadStatus::CACHED:
+      case cereal::ModelManagerSP::DownloadStatus::DOWNLOADED:
+        currentModelLblBtn->showDescription();
+        break;
+      case cereal::ModelManagerSP::DownloadStatus::FAILED:
+      default:
+        break;
+    }
   }
+  prev_download_status = download_status;
 }
 
 /**
@@ -90,9 +101,6 @@ void SoftwarePanelSP::handleBundleDownloadProgress() {
  * @return Display name of the selected bundle or default model name
  */
 QString SoftwarePanelSP::GetActiveModelName() {
-  const SubMaster &sm = *(uiStateSP()->sm);
-  const auto model_manager = sm["modelManagerSP"].getModelManagerSP();
-
   if (model_manager.hasActiveBundle()) {
     return QString::fromStdString(model_manager.getActiveBundle().getDisplayName());
   }
@@ -100,6 +108,11 @@ QString SoftwarePanelSP::GetActiveModelName() {
   return "";
 }
 
+void SoftwarePanelSP::updateModelManagerState() {
+  const SubMaster &sm = *(uiStateSP()->sm);
+  model_manager = sm["modelManagerSP"].getModelManagerSP();
+}
+
 /**
  * @brief Handles the model bundle selection button click
  * Displays available bundles, allows selection, and initiates download
@@ -108,9 +121,6 @@ void SoftwarePanelSP::handleCurrentModelLblBtnClicked() {
   currentModelLblBtn->setEnabled(false);
   currentModelLblBtn->setValue(tr("Fetching models..."));
 
-  const SubMaster &sm = *(uiStateSP()->sm);
-  const auto model_manager = sm["modelManagerSP"].getModelManagerSP();
-
   // Create mapping of bundle indices to display names
   QMap<uint32_t, QString> index_to_bundle;
   const auto bundles = model_manager.getAvailableBundles();
@@ -120,6 +130,9 @@ void SoftwarePanelSP::handleCurrentModelLblBtnClicked() {
 
   // Sort bundles by index in descending order
   QStringList bundleNames;
+  // Add "Default" as the first option
+  bundleNames.append(tr("Use Default"));
+
   auto indices = index_to_bundle.keys();
   std::sort(indices.begin(), indices.end(), std::greater<uint32_t>());
   for (const auto &index: indices) {
@@ -135,14 +148,21 @@ void SoftwarePanelSP::handleCurrentModelLblBtnClicked() {
     return;
   }
 
-  // Find selected bundle and initiate download
-  for (const auto &bundle: bundles) {
-    if (QString::fromStdString(bundle.getDisplayName()) == selectedBundleName) {
-      params.put("ModelManager_DownloadIndex", std::to_string(bundle.getIndex()));
-      if (bundle.getGeneration() != model_manager.getActiveBundle().getGeneration()) {
-        showResetParamsDialog();
+  // Handle "Stock" selection differently
+  if (selectedBundleName == tr("Use Default")) {
+    params.remove("ModelManager_ActiveBundle");
+    currentModelLblBtn->setValue(tr("Default"));
+    showResetParamsDialog();
+  } else {
+    // Find selected bundle and initiate download
+    for (const auto &bundle: bundles) {
+      if (QString::fromStdString(bundle.getDisplayName()) == selectedBundleName) {
+        params.put("ModelManager_DownloadIndex", std::to_string(bundle.getIndex()));
+        if (bundle.getGeneration() != model_manager.getActiveBundle().getGeneration()) {
+          showResetParamsDialog();
+        }
+        break;
       }
-      break;
     }
   }
 
@@ -157,9 +177,11 @@ void SoftwarePanelSP::updateLabels() {
     return;
   }
 
+  updateModelManagerState();
   handleBundleDownloadProgress();
   currentModelLblBtn->setEnabled(!is_onroad && !isDownloading());
   currentModelLblBtn->setValue(GetActiveModelName());
+
   SoftwarePanel::updateLabels();
 }
 

selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h

@@ -19,12 +19,10 @@ public:
 
 private:
   QString GetActiveModelName();
+  void updateModelManagerState();
 
   bool isDownloading() const {
-    const SubMaster &sm = *(uiStateSP()->sm);
-    const auto model_manager = sm["modelManagerSP"].getModelManagerSP();
-
-    if (!model_manager.hasSelectedBundle() || !sm.updated("modelManagerSP")) {
+    if (!model_manager.hasSelectedBundle()) {
       return false;
     }
 
@@ -37,6 +35,9 @@ private:
   void handleCurrentModelLblBtnClicked();
   void handleBundleDownloadProgress();
   void showResetParamsDialog();
+  cereal::ModelManagerSP::Reader model_manager;
+  cereal::ModelManagerSP::DownloadStatus download_status{};
+  cereal::ModelManagerSP::DownloadStatus prev_download_status{};
 
   bool canContinueOnMeteredDialog() {
     if (!is_metered) return true;

selfdrive/ui/sunnypilot/qt/offroad/settings/trips_panel.cc

@@ -0,0 +1,36 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/trips_panel.h"
+
+TripsPanel::TripsPanel(QWidget* parent) : QFrame(parent) {
+  QVBoxLayout* main_layout = new QVBoxLayout(this);
+  main_layout->setMargin(0);
+
+  // main content
+  main_layout->addSpacing(25);
+  center_layout = new QStackedLayout();
+
+  driveStatsWidget = new DriveStats;
+  driveStatsWidget->setStyleSheet(R"(
+    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; }
+  )");
+  center_layout->addWidget(driveStatsWidget);
+
+  main_layout->addLayout(center_layout, 1);
+
+  setStyleSheet(R"(
+    * {
+      color: white;
+    }
+    TripsPanel > QLabel {
+      font-size: 55px;
+    }
+  )");
+}

selfdrive/ui/sunnypilot/qt/offroad/settings/trips_panel.h

@@ -0,0 +1,25 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#pragma once
+
+#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
+
+#include "selfdrive/ui/sunnypilot/qt/widgets/drive_stats.h"
+
+class TripsPanel : public QFrame {
+  Q_OBJECT
+
+public:
+  explicit TripsPanel(QWidget* parent = 0);
+
+private:
+  Params params;
+
+  QStackedLayout* center_layout;
+  DriveStats *driveStatsWidget;
+};

selfdrive/ui/sunnypilot/qt/widgets/drive_stats.cc

@@ -0,0 +1,103 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#include "selfdrive/ui/sunnypilot/qt/widgets/drive_stats.h"
+
+#include <QDebug>
+#include <QGridLayout>
+#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();
+  }
+}

selfdrive/ui/sunnypilot/qt/widgets/drive_stats.h

@@ -0,0 +1,32 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#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);
+};

selfdrive/ui/sunnypilot/qt/widgets/prime.cc

@@ -0,0 +1,19 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#include "selfdrive/ui/sunnypilot/qt/widgets/prime.h"
+
+SetupWidgetSP::SetupWidgetSP(QWidget *parent) : SetupWidget(parent) {
+  PrimeUserWidget *primeUser = new PrimeUserWidget;
+  content_layout->insertWidget(0, primeUser);
+
+  primeUser->setVisible(uiState()->prime_state->isSubscribed());
+
+  QObject::connect(uiState()->prime_state, &PrimeState::changed, [=]() {
+    primeUser->setVisible(uiState()->prime_state->isSubscribed());
+  });
+}

selfdrive/ui/sunnypilot/qt/widgets/prime.h

@@ -0,0 +1,19 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#pragma once
+
+#include "selfdrive/ui/qt/widgets/prime.h"
+
+#include "selfdrive/ui/sunnypilot/ui.h"
+
+class SetupWidgetSP : public SetupWidget {
+  Q_OBJECT
+
+public:
+  explicit SetupWidgetSP(QWidget *parent = nullptr);
+};

selfdrive/ui/sunnypilot/ui.cc

@@ -18,7 +18,7 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) {
     "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
     "pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2",
     "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
-    "modelManagerSP", "selfdriveStateSP",
+    "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP",
   });
 
   // update timer

selfdrive/ui/tests/test_ui/run.py

@@ -34,28 +34,29 @@ DATA: dict[str, capnp.lib.capnp._DynamicStructBuilder] = dict.fromkeys(
   "liveCalibration", "modelV2", "radarState", "driverMonitoringState", "carState",
   "driverStateV2", "roadCameraState", "wideRoadCameraState", "driverCameraState"], None)
 
-def setup_homescreen(click, pm: PubMaster):
+def setup_homescreen(click, pm: PubMaster, scroll=None):
   pass
 
-def setup_settings_device(click, pm: PubMaster):
+def setup_settings_device(click, pm: PubMaster, scroll=None):
   click(100, 100)
 
-def setup_settings_toggles(click, pm: PubMaster):
+def setup_settings_toggles(click, pm: PubMaster, scroll=None):
   setup_settings_device(click, pm)
-  click(278, 640)
+  click(278, 632)
   time.sleep(UI_DELAY)
 
-def setup_settings_software(click, pm: PubMaster):
+def setup_settings_software(click, pm: PubMaster, scroll=None):
   setup_settings_device(click, pm)
-  click(278, 750)
+  click(278, 742)
   time.sleep(UI_DELAY)
 
-def setup_settings_developer(click, pm: PubMaster):
+def setup_settings_developer(click, pm: PubMaster, scroll=None):
   setup_settings_device(click, pm)
+  scroll(-100, 278, 962)
   click(278, 970)
   time.sleep(UI_DELAY)
 
-def setup_onroad(click, pm: PubMaster):
+def setup_onroad(click, pm: PubMaster, scroll=None):
   vipc_server = VisionIpcServer("camerad")
   for stream_type, cam, _ in STREAMS:
     vipc_server.create_buffers(stream_type, 5, cam.width, cam.height)
@@ -87,45 +88,46 @@ def setup_onroad(click, pm: PubMaster):
     packet_id += 1
     time.sleep(0.05)
 
-def setup_onroad_disengaged(click, pm: PubMaster):
+def setup_onroad_disengaged(click, pm: PubMaster, scroll=None):
   DATA['selfdriveState'].selfdriveState.enabled = False
   setup_onroad(click, pm)
   DATA['selfdriveState'].selfdriveState.enabled = True
 
-def setup_onroad_override(click, pm: PubMaster):
+def setup_onroad_override(click, pm: PubMaster, scroll=None):
   DATA['selfdriveState'].selfdriveState.state = log.SelfdriveState.OpenpilotState.overriding
   setup_onroad(click, pm)
   DATA['selfdriveState'].selfdriveState.state = log.SelfdriveState.OpenpilotState.enabled
 
 
-def setup_onroad_wide(click, pm: PubMaster):
+def setup_onroad_wide(click, pm: PubMaster, scroll=None):
   DATA['selfdriveState'].selfdriveState.experimentalMode = True
   DATA["carState"].carState.vEgo = 1
   setup_onroad(click, pm)
 
-def setup_onroad_sidebar(click, pm: PubMaster):
+def setup_onroad_sidebar(click, pm: PubMaster, scroll=None):
   setup_onroad(click, pm)
   click(500, 500)
   setup_onroad(click, pm)
 
-def setup_onroad_wide_sidebar(click, pm: PubMaster):
+def setup_onroad_wide_sidebar(click, pm: PubMaster, scroll=None):
   setup_onroad_wide(click, pm)
   click(500, 500)
   setup_onroad_wide(click, pm)
 
-def setup_body(click, pm: PubMaster):
+def setup_body(click, pm: PubMaster, scroll=None):
   DATA['carParams'].carParams.carName = "BODY"
   DATA['carParams'].carParams.notCar = True
   DATA['carState'].carState.charging = True
   DATA['carState'].carState.fuelGauge = 50.0
   setup_onroad(click, pm)
 
-def setup_keyboard(click, pm: PubMaster):
+def setup_keyboard(click, pm: PubMaster, scroll=None):
   setup_settings_device(click, pm)
-  click(250, 965)
+  scroll(-100, 278, 962)
+  click(278, 970)
   click(1930, 228)
 
-def setup_driver_camera(click, pm: PubMaster):
+def setup_driver_camera(click, pm: PubMaster, scroll=None):
   setup_settings_device(click, pm)
   click(1950, 435)
   DATA['deviceState'].deviceState.started = False
@@ -145,16 +147,16 @@ def setup_onroad_alert(click, pm: PubMaster, text1, text2, size, status=log.Self
   setup_onroad(click, pm)
   DATA['selfdriveState'] = log_from_bytes(origin_state_bytes).as_builder()
 
-def setup_onroad_alert_small(click, pm: PubMaster):
+def setup_onroad_alert_small(click, pm: PubMaster, scroll=None):
   setup_onroad_alert(click, pm, 'This is a small alert message', '', log.SelfdriveState.AlertSize.small)
 
-def setup_onroad_alert_mid(click, pm: PubMaster):
+def setup_onroad_alert_mid(click, pm: PubMaster, scroll=None):
   setup_onroad_alert(click, pm, 'Medium Alert', 'This is a medium alert message', log.SelfdriveState.AlertSize.mid)
 
-def setup_onroad_alert_full(click, pm: PubMaster):
+def setup_onroad_alert_full(click, pm: PubMaster, scroll=None):
   setup_onroad_alert(click, pm, 'Full Alert', 'This is a full alert message', log.SelfdriveState.AlertSize.full)
 
-def setup_offroad_alert(click, pm: PubMaster):
+def setup_offroad_alert(click, pm: PubMaster, scroll=None):
   for alert in OFFROAD_ALERTS:
     set_offroad_alert(alert, True)
 
@@ -162,7 +164,7 @@ def setup_offroad_alert(click, pm: PubMaster):
   setup_settings_device(click, pm)
   click(100, 100)
 
-def setup_update_available(click, pm: PubMaster):
+def setup_update_available(click, pm: PubMaster, scroll=None):
   Params().put_bool("UpdateAvailable", True)
   release_notes_path = os.path.join(BASEDIR, "RELEASES.md")
   with open(release_notes_path) as file:
@@ -172,30 +174,35 @@ def setup_update_available(click, pm: PubMaster):
   setup_settings_device(click, pm)
   click(100, 100)
 
-def setup_pair_device(click, pm: PubMaster):
+def setup_pair_device(click, pm: PubMaster, scroll=None):
   click(1950, 435)
   click(1800, 900)
 
-def setup_settings_sunnylink(click, pm: PubMaster):
+def setup_settings_sunnylink(click, pm: PubMaster, scroll=None):
   Params().put_bool("SunnylinkEnabled", True)
 
   setup_settings_device(click, pm)
-  click(278, 530)
+  click(278, 522)
   time.sleep(UI_DELAY)
 
-def setup_settings_sunnypilot(click, pm: PubMaster):
+def setup_settings_sunnypilot(click, pm: PubMaster, scroll=None):
   setup_settings_device(click, pm)
-  click(278, 860)
+  click(278, 852)
   time.sleep(UI_DELAY)
 
-def setup_settings_sunnypilot_mads(click, pm: PubMaster):
+def setup_settings_sunnypilot_mads(click, pm: PubMaster, scroll=None):
   Params().put_bool("Mads", True)
 
   setup_settings_device(click, pm)
-  click(278, 860)
+  click(278, 852)
   click(970, 455)
   time.sleep(UI_DELAY)
 
+def setup_settings_trips(click, pm: PubMaster, scroll=None):
+  setup_settings_device(click, pm)
+  click(278, 962)
+  time.sleep(UI_DELAY)
+
 CASES = {
   "homescreen": setup_homescreen,
   "prime": setup_homescreen,
@@ -224,6 +231,7 @@ CASES.update({
   "settings_sunnylink": setup_settings_sunnylink,
   "settings_sunnypilot": setup_settings_sunnypilot,
   "settings_sunnypilot_mads": setup_settings_sunnypilot_mads,
+  "settings_trips": setup_settings_trips,
 })
 
 TEST_DIR = pathlib.Path(__file__).parent
@@ -260,10 +268,14 @@ class TestUI:
     pyautogui.click(self.ui.left + x, self.ui.top + y, *args, **kwargs)
     time.sleep(UI_DELAY) # give enough time for the UI to react
 
+  def scroll(self, clicks, x, y, *args, **kwargs):
+    pyautogui.scroll(clicks, self.ui.left + x, self.ui.top + y, *args, **kwargs)
+    time.sleep(UI_DELAY)
+
   @with_processes(["ui"])
   def test_ui(self, name, setup_case):
     self.setup()
-    setup_case(self.click, self.pm)
+    setup_case(self.click, self.pm, self.scroll)
     self.screenshot(name)
 
 def create_screenshots():

selfdrive/ui/translations/main_ar.ts

@@ -275,6 +275,33 @@
         <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>DriverViewWindow</name>
     <message>
@@ -729,6 +756,10 @@ This may take up to a minute.</source>
         <source>sunnylink</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Trips</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Setup</name>
@@ -1067,18 +1098,10 @@ This may take up to a minute.</source>
         <source>Downloading %1 model [%2]... (%3%)</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] downloaded</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>%1 model [%2] download failed</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] from cache</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Select a Model</source>
         <translation type="unfinished"></translation>
@@ -1091,6 +1114,34 @@ This may take up to a minute.</source>
         <source>Model download has started in the background.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>%1 model [%2] %3</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>downloaded</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>ready</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>from cache</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Use Default</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>No custom model selected!</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Default</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1321,6 +1372,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation>تمكين مراقبة السائق حتى عندما لا يكون نظام OpenPilot مُفعّلاً.</translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_de.ts

@@ -275,6 +275,33 @@
         <translation type="unfinished"></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>DriverViewWindow</name>
     <message>
@@ -711,6 +738,10 @@ This may take up to a minute.</source>
         <source>sunnylink</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Trips</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Setup</name>
@@ -1051,18 +1082,10 @@ This may take up to a minute.</source>
         <source>Downloading %1 model [%2]... (%3%)</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] downloaded</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>%1 model [%2] download failed</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] from cache</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Select a Model</source>
         <translation type="unfinished"></translation>
@@ -1075,6 +1098,34 @@ This may take up to a minute.</source>
         <source>Model download has started in the background.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>%1 model [%2] %3</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>downloaded</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>ready</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>from cache</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Use Default</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>No custom model selected!</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Default</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1305,6 +1356,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_es.ts

@@ -275,6 +275,33 @@
         <translation>Desactivar para apagar</translation>
     </message>
 </context>
+<context>
+    <name>DriveStats</name>
+    <message>
+        <source>Drives</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Hours</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>ALL TIME</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>PAST WEEK</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>KM</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Miles</source>
+        <translation type="unfinished"></translation>
+    </message>
+</context>
 <context>
     <name>DriverViewWindow</name>
     <message>
@@ -713,6 +740,10 @@ Esto puede tardar un minuto.</translation>
         <source>sunnylink</source>
         <translation>sunnylink</translation>
     </message>
+    <message>
+        <source>Trips</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Setup</name>
@@ -1059,22 +1090,42 @@ Esto puede tardar un minuto.</translation>
         <source>Downloading %1 model [%2]... (%3%)</source>
         <translation>Descargando modelo de %1 [%2]... (%3%)</translation>
     </message>
-    <message>
-        <source>%1 model [%2] downloaded</source>
-        <translation>Modelo de %1 [%2] descargado</translation>
-    </message>
     <message>
         <source>%1 model [%2] download failed</source>
         <translation>Falló descarga modelo de %1 [%2]</translation>
     </message>
-    <message>
-        <source>%1 model [%2] from cache</source>
-        <translation>Modelo de %1 [%2] desde caché</translation>
-    </message>
     <message>
         <source>Select a Model</source>
         <translation>Selecciona un Modelo</translation>
     </message>
+    <message>
+        <source>%1 model [%2] %3</source>
+        <translation>Modelo de %1 [%2] %3</translation>
+    </message>
+    <message>
+        <source>downloaded</source>
+        <translation>descargado</translation>
+    </message>
+    <message>
+        <source>ready</source>
+        <translation>listo</translation>
+    </message>
+    <message>
+        <source>from cache</source>
+        <translation>desde caché</translation>
+    </message>
+    <message>
+        <source>Use Default</source>
+        <translation>Por Defecto</translation>
+    </message>
+    <message>
+        <source>No custom model selected!</source>
+        <translation>No hay ningún modelo personalizado elegido!.</translation>
+    </message>
+    <message>
+        <source>Default</source>
+        <translation>Por Defecto</translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1305,6 +1356,14 @@ Esto puede tardar un minuto.</translation>
         <source>Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode.</source>
         <translation>Activar el control longitudinal (fase experimental) para permitir el modo Experimental.</translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_fr.ts

@@ -275,6 +275,33 @@
         <translation type="unfinished"></translation>
     </message>
 </context>
+<context>
+    <name>DriveStats</name>
+    <message>
+        <source>Drives</source>
+        <translation>Trajets</translation>
+    </message>
+    <message>
+        <source>Hours</source>
+        <translation>Heures</translation>
+    </message>
+    <message>
+        <source>ALL TIME</source>
+        <translation>DEPUIS TOUJOURS</translation>
+    </message>
+    <message>
+        <source>PAST WEEK</source>
+        <translation>CETTE SEMAINE</translation>
+    </message>
+    <message>
+        <source>KM</source>
+        <translation>KM</translation>
+    </message>
+    <message>
+        <source>Miles</source>
+        <translation>Miles</translation>
+    </message>
+</context>
 <context>
     <name>DriverViewWindow</name>
     <message>
@@ -713,6 +740,10 @@ Cela peut prendre jusqu&apos;à une minute.</translation>
         <source>sunnylink</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Trips</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Setup</name>
@@ -1051,18 +1082,10 @@ Cela peut prendre jusqu&apos;à une minute.</translation>
         <source>Downloading %1 model [%2]... (%3%)</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] downloaded</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>%1 model [%2] download failed</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] from cache</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Select a Model</source>
         <translation type="unfinished"></translation>
@@ -1075,6 +1098,34 @@ Cela peut prendre jusqu&apos;à une minute.</translation>
         <source>Model download has started in the background.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>%1 model [%2] %3</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>downloaded</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>ready</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>from cache</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Use Default</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>No custom model selected!</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Default</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1305,6 +1356,14 @@ Cela peut prendre jusqu&apos;à une minute.</translation>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_ja.ts

@@ -275,6 +275,33 @@
         <translation type="unfinished"></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>DriverViewWindow</name>
     <message>
@@ -707,6 +734,10 @@ This may take up to a minute.</source>
         <source>sunnylink</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Trips</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Setup</name>
@@ -1045,18 +1076,10 @@ This may take up to a minute.</source>
         <source>Downloading %1 model [%2]... (%3%)</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] downloaded</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>%1 model [%2] download failed</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] from cache</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Select a Model</source>
         <translation type="unfinished"></translation>
@@ -1069,6 +1092,34 @@ This may take up to a minute.</source>
         <source>Model download has started in the background.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>%1 model [%2] %3</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>downloaded</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>ready</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>from cache</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Use Default</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>No custom model selected!</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Default</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1299,6 +1350,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_ko.ts

@@ -275,6 +275,33 @@
         <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>DriverViewWindow</name>
     <message>
@@ -709,6 +736,10 @@ This may take up to a minute.</source>
         <source>sunnylink</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Trips</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Setup</name>
@@ -1047,18 +1078,10 @@ This may take up to a minute.</source>
         <source>Downloading %1 model [%2]... (%3%)</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] downloaded</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>%1 model [%2] download failed</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] from cache</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Select a Model</source>
         <translation type="unfinished"></translation>
@@ -1071,6 +1094,34 @@ This may take up to a minute.</source>
         <source>Model download has started in the background.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>%1 model [%2] %3</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>downloaded</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>ready</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>from cache</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Use Default</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>No custom model selected!</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Default</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1301,6 +1352,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation>Openpilot이 활성화되지 않은 경우에도 드라이버 모니터링을 활성화합니다.</translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_pt-BR.ts

@@ -275,6 +275,33 @@
         <translation>PAREAR</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>DriverViewWindow</name>
     <message>
@@ -713,6 +740,10 @@ Isso pode levar até um minuto.</translation>
         <source>sunnylink</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Trips</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Setup</name>
@@ -1051,18 +1082,10 @@ Isso pode levar até um minuto.</translation>
         <source>Downloading %1 model [%2]... (%3%)</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] downloaded</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>%1 model [%2] download failed</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] from cache</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Select a Model</source>
         <translation type="unfinished"></translation>
@@ -1075,6 +1098,34 @@ Isso pode levar até um minuto.</translation>
         <source>Model download has started in the background.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>%1 model [%2] %3</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>downloaded</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>ready</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>from cache</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Use Default</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>No custom model selected!</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Default</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1305,6 +1356,14 @@ Isso pode levar até um minuto.</translation>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation>Habilite o monitoramento do motorista mesmo quando o openpilot não estiver acionado.</translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_th.ts

@@ -275,6 +275,33 @@
         <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>DriverViewWindow</name>
     <message>
@@ -709,6 +736,10 @@ This may take up to a minute.</source>
         <source>sunnylink</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Trips</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Setup</name>
@@ -1047,18 +1078,10 @@ This may take up to a minute.</source>
         <source>Downloading %1 model [%2]... (%3%)</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] downloaded</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>%1 model [%2] download failed</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] from cache</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Select a Model</source>
         <translation type="unfinished"></translation>
@@ -1071,6 +1094,34 @@ This may take up to a minute.</source>
         <source>Model download has started in the background.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>%1 model [%2] %3</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>downloaded</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>ready</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>from cache</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Use Default</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>No custom model selected!</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Default</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1301,6 +1352,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_tr.ts

@@ -275,6 +275,33 @@
         <translation type="unfinished"></translation>
     </message>
 </context>
+<context>
+    <name>DriveStats</name>
+    <message>
+        <source>Drives</source>
+        <translation>Sürücüler</translation>
+    </message>
+    <message>
+        <source>Hours</source>
+        <translation>Saat</translation>
+    </message>
+    <message>
+        <source>ALL TIME</source>
+        <translation>TÜM ZAMANLAR</translation>
+    </message>
+    <message>
+        <source>PAST WEEK</source>
+        <translation>GEÇEN HAFTA</translation>
+    </message>
+    <message>
+        <source>KM</source>
+        <translation>KM</translation>
+    </message>
+    <message>
+        <source>Miles</source>
+        <translation>Mil</translation>
+    </message>
+</context>
 <context>
     <name>DriverViewWindow</name>
     <message>
@@ -707,6 +734,10 @@ This may take up to a minute.</source>
         <source>sunnylink</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Trips</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Setup</name>
@@ -1045,18 +1076,10 @@ This may take up to a minute.</source>
         <source>Downloading %1 model [%2]... (%3%)</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] downloaded</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>%1 model [%2] download failed</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] from cache</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Select a Model</source>
         <translation type="unfinished"></translation>
@@ -1069,6 +1092,34 @@ This may take up to a minute.</source>
         <source>Model download has started in the background.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>%1 model [%2] %3</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>downloaded</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>ready</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>from cache</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Use Default</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>No custom model selected!</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Default</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1299,6 +1350,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_zh-CHS.ts

@@ -275,6 +275,33 @@
         <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>DriverViewWindow</name>
     <message>
@@ -709,6 +736,10 @@ This may take up to a minute.</source>
         <source>sunnylink</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Trips</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Setup</name>
@@ -1047,18 +1078,10 @@ This may take up to a minute.</source>
         <source>Downloading %1 model [%2]... (%3%)</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] downloaded</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>%1 model [%2] download failed</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] from cache</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Select a Model</source>
         <translation type="unfinished"></translation>
@@ -1071,6 +1094,34 @@ This may take up to a minute.</source>
         <source>Model download has started in the background.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>%1 model [%2] %3</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>downloaded</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>ready</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>from cache</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Use Default</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>No custom model selected!</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Default</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1301,6 +1352,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation>即使在openpilot未激活时也启用驾驶员监控。</translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_zh-CHT.ts

@@ -275,6 +275,33 @@
         <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>DriverViewWindow</name>
     <message>
@@ -709,6 +736,10 @@ This may take up to a minute.</source>
         <source>sunnylink</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Trips</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Setup</name>
@@ -1047,18 +1078,10 @@ This may take up to a minute.</source>
         <source>Downloading %1 model [%2]... (%3%)</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] downloaded</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>%1 model [%2] download failed</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>%1 model [%2] from cache</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Select a Model</source>
         <translation type="unfinished"></translation>
@@ -1071,6 +1094,34 @@ This may take up to a minute.</source>
         <source>Model download has started in the background.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>%1 model [%2] %3</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>downloaded</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>ready</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>from cache</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Use Default</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>No custom model selected!</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Default</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1301,6 +1352,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation>即使在openpilot未激活時也啟用駕駛監控。</translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/ui.cc

@@ -111,15 +111,16 @@ UIState::UIState(QObject *parent) : QObject(parent) {
   prime_state = new PrimeState(this);
   language = QString::fromStdString(Params().get("LanguageSetting"));
 
-  RETURN_IF_SUNNYPILOT
+#ifndef SUNNYPILOT
   // update timer
   timer = new QTimer(this);
   QObject::connect(timer, &QTimer::timeout, this, &UIState::update);
   timer->start(1000 / UI_FREQ);
+#endif
 }
 
 void UIState::update() {
-  RETURN_IF_SUNNYPILOT
+#ifndef SUNNYPILOT
   update_sockets(this);
   update_state(this);
   updateStatus();
@@ -128,6 +129,7 @@ void UIState::update() {
     watchdog_kick(nanos_since_boot());
   }
   emit uiUpdate(*this);
+#endif
 }
 
 Device::Device(QObject *parent) : brightness_filter(BACKLIGHT_OFFROAD, BACKLIGHT_TS, BACKLIGHT_DT), QObject(parent) {

sunnypilot/SConscript

@@ -0,0 +1 @@
+SConscript(['modeld/SConscript'])

sunnypilot/mads/mads.py

@@ -124,6 +124,9 @@ class ModularAssistiveDrivingSystem:
           self.events.add(EventName.silentLkasEnable)
 
       self.events.remove(EventName.preEnableStandstill)
+      self.events.remove(EventName.belowEngageSpeed)
+      self.events.remove(EventName.speedTooLow)
+      self.events.remove(EventName.cruiseDisabled)
 
     if self.events.has(EventName.pcmEnable) or self.events.has(EventName.buttonEnable):
       update_unified_engagement_mode()

sunnypilot/modeld/.gitignore

@@ -0,0 +1 @@
+*_pyx.cpp

sunnypilot/modeld/SConscript

@@ -0,0 +1,65 @@
+import glob
+
+Import('env', 'envCython', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'transformations')
+lenv = env.Clone()
+lenvCython = envCython.Clone()
+
+libs = [cereal, messaging, visionipc, gpucommon, common, 'capnp', 'kj', 'pthread']
+frameworks = []
+
+common_src = [
+  "models/commonmodel.cc",
+  "transforms/loadyuv.cc",
+  "transforms/transform.cc",
+]
+
+thneed_src_common = [
+  "thneed/thneed_common.cc",
+  "thneed/serialize.cc",
+]
+
+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
+
+# SNPE except on Mac and ARM Linux
+snpe_lib = []
+if arch != "Darwin" and arch != "aarch64":
+  common_src += ['runners/snpemodel.cc']
+  snpe_lib += ['SNPE']
+
+# OpenCL is a framework on Mac
+if arch == "Darwin":
+  frameworks += ['OpenCL']
+else:
+  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)
+
+tinygrad_files = ["#"+x for x in glob.glob(env.Dir("#tinygrad_repo").relpath + "/**", recursive=True, root_dir=env.Dir("#").abspath)]
+
+# Get model metadata
+fn = File("models/supercombo").abspath
+cmd = f'python3 {Dir("#selfdrive/modeld").abspath}/get_model_metadata.py {fn}.onnx'
+lenv.Command(fn + "_metadata.pkl", [fn + ".onnx"] + tinygrad_files, cmd)
+
+if arch == "larch64":
+  thneed_lib = env.SharedLibrary('thneed', thneed_src, LIBS=[gpucommon, common, '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', 'OpenCL'])

sunnypilot/modeld/constants.py

@@ -0,0 +1,118 @@
+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.]
+
+  # 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
+  LAT_PLANNER_STATE_LEN = 4
+  LATERAL_CONTROL_PARAMS_LEN = 2
+  PREV_DESIRED_CURV_LEN = 1
+
+  # 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
+  DESIRED_CURV_WIDTH = 1
+
+  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
+
+  POLY_PATH_DEGREE = 4
+
+# 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, 31, 6)
+  BRAKE_DISENGAGE = slice(2, 31, 6)
+  STEER_OVERRIDE = slice(3, 31, 6)
+  HARD_BRAKE_3 = slice(4, 31, 6)
+  HARD_BRAKE_4 = slice(5, 31, 6)
+  HARD_BRAKE_5 = slice(6, 31, 6)
+  # next 0, 2, 4, 6, 8, 10 seconds
+  GAS_PRESS = slice(31, 55, 4)
+  BRAKE_PRESS = slice(32, 55, 4)
+  LEFT_BLINKER = slice(33, 55, 4)
+  RIGHT_BLINKER = slice(34, 55, 4)
+
+
+class MetaTombRaider:
+  ENGAGED = slice(0, 1)
+  # next 2, 4, 6, 8, 10 seconds
+  GAS_DISENGAGE = slice(1, 41, 8)
+  BRAKE_DISENGAGE = slice(2, 41, 8)
+  STEER_OVERRIDE = slice(3, 41, 8)
+  HARD_BRAKE_3 = slice(4, 41, 8)
+  HARD_BRAKE_4 = slice(5, 41, 8)
+  HARD_BRAKE_5 = slice(6, 41, 8)
+  GAS_PRESS = slice(7, 41, 8)
+  BRAKE_PRESS = slice(8, 41, 8)
+  # next 0, 2, 4, 6, 8, 10 seconds
+  LEFT_BLINKER = slice(41, 53, 2)
+  RIGHT_BLINKER = slice(42, 53, 2)
+
+
+class MetaSimPose:
+  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)

sunnypilot/modeld/fill_model_msg.py

@@ -0,0 +1,233 @@
+import os
+import capnp
+import numpy as np
+from cereal import log
+from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan
+from openpilot.sunnypilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_lag_adjusted_curvature, MIN_SPEED
+
+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_xyz_poly(builder, degree, x, y, z):
+  xyz = np.stack([x, y, z], axis=1)
+  coeffs = np.polynomial.polynomial.polyfit(ModelConstants.T_IDXS, xyz, deg=degree)
+  builder.xCoefficients = coeffs[:, 0].tolist()
+  builder.yCoefficients = coeffs[:, 1].tolist()
+  builder.zCoefficients = coeffs[:, 2].tolist()
+
+def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_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, model_execution_time: float, valid: bool,
+                   v_ego: float, steer_delay: float, meta_const) -> None:
+  frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
+  frame_drop_perc = frame_drop * 100
+  extended_msg.valid = valid
+  base_msg.valid = valid
+
+  if 'lat_planner_solution' in net_output_data:
+    x, y, yaw, yawRate = [net_output_data['lat_planner_solution'][0, :, i].tolist() for i in range(4)]
+    x_sol = np.column_stack([x, y, yaw, yawRate])
+    v_ego = max(MIN_SPEED, v_ego)
+    psis = x_sol[0:CONTROL_N, 2].tolist()
+    curvatures = (x_sol[0:CONTROL_N, 3] / v_ego).tolist()
+    desired_curvature = get_lag_adjusted_curvature(steer_delay, v_ego, psis, curvatures)
+  else:
+    desired_curvature = float(net_output_data['desired_curvature'][0, 0])
+
+  driving_model_data = base_msg.drivingModelData
+
+  driving_model_data.frameId = vipc_frame_id
+  driving_model_data.frameIdExtra = vipc_frame_id_extra
+  driving_model_data.frameDropPerc = frame_drop_perc
+  driving_model_data.modelExecutionTime = model_execution_time
+
+  action = driving_model_data.action
+  action.desiredCurvature = desired_curvature
+
+  modelV2 = extended_msg.modelV2
+  modelV2.frameId = vipc_frame_id
+  modelV2.frameIdExtra = vipc_frame_id_extra
+  modelV2.frameAge = frame_age
+  modelV2.frameDropPerc = frame_drop_perc
+  modelV2.timestampEof = timestamp_eof
+  modelV2.modelExecutionTime = model_execution_time
+
+  # 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)
+
+  # temporal pose
+  temporal_pose = modelV2.temporalPose
+  temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist()
+  temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist()
+  temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist()
+  temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist()
+
+  # poly path
+  poly_path = driving_model_data.path
+  fill_xyz_poly(poly_path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)
+
+  # lateral planning
+  action = modelV2.action
+  action.desiredCurvature = desired_curvature
+
+  # 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()
+
+  lane_line_meta = driving_model_data.laneLineMeta
+  lane_line_meta.leftY = modelV2.laneLines[1].y[0]
+  lane_line_meta.leftProb = modelV2.laneLineProbs[1]
+  lane_line_meta.rightY = modelV2.laneLines[2].y[0]
+  lane_line_meta.rightProb = modelV2.laneLineProbs[2]
+
+  # 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_const.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_const.BRAKE_DISENGAGE].tolist()
+  disengage_predictions.gasDisengageProbs = net_output_data['meta'][0,meta_const.GAS_DISENGAGE].tolist()
+  disengage_predictions.steerOverrideProbs = net_output_data['meta'][0,meta_const.STEER_OVERRIDE].tolist()
+  disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,meta_const.HARD_BRAKE_3].tolist()
+  disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,meta_const.HARD_BRAKE_4].tolist()
+  disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,meta_const.HARD_BRAKE_5].tolist()
+  if 'sim_pose' not in net_output_data:
+    disengage_predictions.gasPressProbs = net_output_data['meta'][0,meta_const.GAS_PRESS].tolist()
+    disengage_predictions.brakePressProbs = net_output_data['meta'][0,meta_const.BRAKE_PRESS].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_const.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_const.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()
+
+  # confidence
+  if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0:
+    # any disengage prob
+    brake_disengage_probs = net_output_data['meta'][0,meta_const.BRAKE_DISENGAGE]
+    gas_disengage_probs = net_output_data['meta'][0,meta_const.GAS_DISENGAGE]
+    steer_override_probs = net_output_data['meta'][0,meta_const.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()

sunnypilot/modeld/get_model_metadata.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python3
+import sys
+import pathlib
+import onnx
+import codecs
+import pickle
+
+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}')

sunnypilot/modeld/modeld

@@ -0,0 +1,10 @@
+#!/usr/bin/env bash
+
+DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
+cd "$DIR/../../"
+
+if [ -f "$DIR/libthneed.so" ]; then
+  export LD_PRELOAD="$DIR/libthneed.so"
+fi
+
+exec "$DIR/modeld.py" "$@"

sunnypilot/modeld/modeld.py

@@ -0,0 +1,316 @@
+#!/usr/bin/env python3
+import os
+import time
+import numpy as np
+import cereal.messaging as messaging
+from cereal import car, log
+from pathlib import Path
+from setproctitle import setproctitle
+from cereal.messaging import PubMaster, SubMaster
+from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
+from opendbc.car.car_helpers import get_demo_car_params
+from openpilot.common.swaglog import cloudlog
+from openpilot.common.params import Params
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.realtime import DT_MDL, config_realtime_process
+from openpilot.common.numpy_fast import interp
+from openpilot.common.transformations.camera import DEVICE_CAMERAS
+from openpilot.common.transformations.model import get_warp_matrix
+from openpilot.system import sentry
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+from openpilot.sunnypilot.modeld.runners import ModelRunner, Runtime
+from openpilot.sunnypilot.modeld.parse_model_outputs import Parser
+from openpilot.sunnypilot.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
+from openpilot.sunnypilot.modeld.constants import ModelConstants
+from openpilot.sunnypilot.modeld.models.commonmodel_pyx import ModelFrame, CLContext
+from openpilot.sunnypilot.modeld.runners.run_helpers import get_model_path, load_metadata, prepare_inputs, load_meta_constants
+
+PROCESS_NAME = "selfdrive.modeld.modeld_snpe"
+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)
+
+    model_paths = get_model_path()
+    self.model_metadata = load_metadata()
+    self.inputs = prepare_inputs(self.model_metadata)
+    self.meta = load_meta_constants(self.model_metadata)
+
+    self.output_slices = self.model_metadata['output_slices']
+    net_output_size = self.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) -> dict[str, np.ndarray] | None:
+    # 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']
+
+    for k in self.inputs:
+      if k in inputs and k != 'desire':
+        self.inputs[k][:] = inputs[k]
+
+    # 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, :]
+
+    if "lat_planner_solution" in outputs and "lat_planner_state" in self.inputs.keys():
+      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])
+
+    if "desired_curvature" in outputs:
+      if "prev_desired_curvs" in self.inputs.keys():
+        self.inputs['prev_desired_curvs'][:-1] = self.inputs['prev_desired_curvs'][1:]
+        self.inputs['prev_desired_curvs'][-1] = outputs['desired_curvature'][0, 0]
+
+      if "prev_desired_curv" in self.inputs.keys():
+        self.inputs['prev_desired_curv'][:-1] = self.inputs['prev_desired_curv'][1:]
+        self.inputs['prev_desired_curv'][-1:] = outputs['desired_curvature'][0, :]
+    return outputs
+
+
+def main(demo=False):
+  cloudlog.warning("modeld init")
+
+  sentry.set_tag("daemon", PROCESS_NAME)
+  cloudlog.bind(daemon=PROCESS_NAME)
+  setproctitle(PROCESS_NAME)
+  config_realtime_process(7, 54)
+
+  cloudlog.warning("setting up CL context")
+  cl_context = CLContext()
+  cloudlog.warning("CL context ready; loading model")
+  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", "drivingModelData", "cameraOdometry"])
+  sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl"])
+
+  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
+  buf_main, buf_extra = None, None
+  meta_main = FrameMeta()
+  meta_extra = FrameMeta()
+
+
+  if demo:
+    CP = get_demo_car_params()
+  else:
+    CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
+
+  cloudlog.info("modeld got CarParams: %s", CP.carName)
+
+  # TODO this needs more thought, use .2s extra for now to estimate other delays
+  steer_delay = CP.steerActuatorDelay + .2
+
+  DH = DesireHelper()
+
+  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.debug("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.debug("vipc_client_extra no frame")
+        continue
+
+      if abs(meta_main.timestamp_sof - meta_extra.timestamp_sof) > 10000000:
+        cloudlog.error(f"frames out of sync! main: {meta_main.frame_id} ({meta_main.timestamp_sof / 1e9:.5f}),\
+                         extra: {meta_extra.frame_id} ({meta_extra.timestamp_sof / 1e9:.5f})")
+
+    else:
+      # Use single camera
+      buf_extra = buf_main
+      meta_extra = meta_main
+
+    sm.update(0)
+    desire = DH.desire
+    v_ego = sm["carState"].vEgo
+    is_rhd = sm["driverMonitoringState"].isRHD
+    frame_id = sm["roadCameraState"].frameId
+    if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
+      device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
+      dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
+      model_transform_main = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics, False).astype(np.float32)
+      model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics, 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
+
+    # 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,
+      }
+
+    if "lateral_control_params" in model.inputs.keys():
+      inputs['lateral_control_params'] = np.array([max(v_ego, 0.), steer_delay], dtype=np.float32)
+
+    if "driving_style" in model.inputs.keys():
+      inputs['driving_style'] = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], dtype=np.float32)
+
+    if "nav_features" in model.inputs.keys():
+      inputs['nav_features'] = np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32)
+
+    if "nav_instructions" in model.inputs.keys():
+      inputs['nav_instructions'] = np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32)
+
+    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')
+      drivingdata_send = messaging.new_message('drivingModelData')
+      posenet_send = messaging.new_message('cameraOdometry')
+      fill_model_msg(drivingdata_send, modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
+                     frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen,
+                     v_ego, steer_delay, model.meta)
+
+      desire_state = modelv2_send.modelV2.meta.desireState
+      l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
+      r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
+      lane_change_prob = l_lane_change_prob + r_lane_change_prob
+      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
+      modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
+      modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
+      drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
+      drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
+
+      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('drivingModelData', drivingdata_send)
+      pm.send('cameraOdometry', posenet_send)
+
+    last_vipc_frame_id = meta_main.frame_id
+
+
+if __name__ == "__main__":
+  try:
+    import argparse
+    parser = argparse.ArgumentParser()
+    parser.add_argument('--demo', action='store_true', help='A boolean for demo mode.')
+    args = parser.parse_args()
+    main(demo=args.demo)
+  except KeyboardInterrupt:
+    cloudlog.warning(f"child {PROCESS_NAME} got SIGINT")
+  except Exception:
+    sentry.capture_exception()
+    raise

sunnypilot/modeld/models/commonmodel.cc

@@ -0,0 +1,50 @@
+#include "sunnypilot/modeld/models/commonmodel.h"
+
+#include <cassert>
+#include <cmath>
+#include <cstring>
+
+#include "common/clutil.h"
+
+ModelFrame::ModelFrame(cl_device_id device_id, cl_context context) {
+  input_frames = std::make_unique<float[]>(buf_size);
+
+  q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
+  y_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, MODEL_WIDTH * MODEL_HEIGHT, NULL, &err));
+  u_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (MODEL_WIDTH / 2) * (MODEL_HEIGHT / 2), NULL, &err));
+  v_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (MODEL_WIDTH / 2) * (MODEL_HEIGHT / 2), NULL, &err));
+  net_input_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, MODEL_FRAME_SIZE * sizeof(float), NULL, &err));
+
+  transform_init(&transform, context, device_id);
+  loadyuv_init(&loadyuv, context, device_id, MODEL_WIDTH, MODEL_HEIGHT);
+}
+
+float* ModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3 &projection, cl_mem *output) {
+  transform_queue(&this->transform, q,
+                  yuv_cl, frame_width, frame_height, frame_stride, frame_uv_offset,
+                  y_cl, u_cl, v_cl, MODEL_WIDTH, MODEL_HEIGHT, projection);
+
+  if (output == NULL) {
+    loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, net_input_cl);
+
+    std::memmove(&input_frames[0], &input_frames[MODEL_FRAME_SIZE], sizeof(float) * MODEL_FRAME_SIZE);
+    CL_CHECK(clEnqueueReadBuffer(q, net_input_cl, CL_TRUE, 0, MODEL_FRAME_SIZE * sizeof(float), &input_frames[MODEL_FRAME_SIZE], 0, nullptr, nullptr));
+    clFinish(q);
+    return &input_frames[0];
+  } else {
+    loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, *output, true);
+    // NOTE: Since thneed is using a different command queue, this clFinish is needed to ensure the image is ready.
+    clFinish(q);
+    return NULL;
+  }
+}
+
+ModelFrame::~ModelFrame() {
+  transform_destroy(&transform);
+  loadyuv_destroy(&loadyuv);
+  CL_CHECK(clReleaseMemObject(net_input_cl));
+  CL_CHECK(clReleaseMemObject(v_cl));
+  CL_CHECK(clReleaseMemObject(u_cl));
+  CL_CHECK(clReleaseMemObject(y_cl));
+  CL_CHECK(clReleaseCommandQueue(q));
+}

sunnypilot/modeld/models/commonmodel.h

@@ -0,0 +1,36 @@
+#pragma once
+
+#include <cfloat>
+#include <cstdlib>
+
+#include <memory>
+
+#define CL_USE_DEPRECATED_OPENCL_1_2_APIS
+#ifdef __APPLE__
+#include <OpenCL/cl.h>
+#else
+#include <CL/cl.h>
+#endif
+
+#include "common/mat.h"
+#include "sunnypilot/modeld/transforms/loadyuv.h"
+#include "sunnypilot/modeld/transforms/transform.h"
+
+class ModelFrame {
+public:
+  ModelFrame(cl_device_id device_id, cl_context context);
+  ~ModelFrame();
+  float* prepare(cl_mem yuv_cl, int width, int height, int frame_stride, int frame_uv_offset, const mat3& transform, cl_mem *output);
+
+  const int MODEL_WIDTH = 512;
+  const int MODEL_HEIGHT = 256;
+  const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2;
+  const int buf_size = MODEL_FRAME_SIZE * 2;
+
+private:
+  Transform transform;
+  LoadYUVState loadyuv;
+  cl_command_queue q;
+  cl_mem y_cl, u_cl, v_cl, net_input_cl;
+  std::unique_ptr<float[]> input_frames;
+};

sunnypilot/modeld/models/commonmodel.pxd

@@ -0,0 +1,18 @@
+# distutils: language = c++
+
+from msgq.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 "sunnypilot/modeld/models/commonmodel.h":
+  cppclass ModelFrame:
+    int buf_size
+    ModelFrame(cl_device_id, cl_context)
+    float * prepare(cl_mem, int, int, int, int, mat3, cl_mem*)

sunnypilot/modeld/models/commonmodel_pyx.pxd

@@ -0,0 +1,13 @@
+# distutils: language = c++
+
+from msgq.visionipc.visionipc cimport cl_mem
+from msgq.visionipc.visionipc_pyx cimport CLContext as BaseCLContext
+
+cdef class CLContext(BaseCLContext):
+  pass
+
+cdef class CLMem:
+  cdef cl_mem * mem
+
+  @staticmethod
+  cdef create(void*)

sunnypilot/modeld/models/commonmodel_pyx.pyx

@@ -0,0 +1,45 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii, language_level=3
+
+import numpy as np
+cimport numpy as cnp
+from libc.string cimport memcpy
+
+from msgq.visionipc.visionipc cimport cl_mem
+from msgq.visionipc.visionipc_pyx cimport VisionBuf, CLContext as BaseCLContext
+from openpilot.sunnypilot.modeld.models.commonmodel cimport CL_DEVICE_TYPE_DEFAULT, cl_get_device_id, cl_create_context
+from openpilot.sunnypilot.modeld.models.commonmodel cimport mat3, ModelFrame as cppModelFrame
+
+
+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
+    if output is None:
+      data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, NULL)
+    else:
+      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)

sunnypilot/modeld/models/supercombo.onnx

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fb2018c74cdd9e5cb070ec7bed7f8581fabd55e39057d0a03aaffd2e42408154
+size 62486347

sunnypilot/modeld/parse_model_outputs.py

@@ -0,0 +1,107 @@
+import numpy as np
+from openpilot.sunnypilot.modeld.constants import ModelConstants
+
+def safe_exp(x, out=None):
+  # -11 is around 10**14, more causes float16 overflow
+  return np.exp(np.clip(x, -np.inf, 11), out=out)
+
+def sigmoid(x):
+  return 1. / (1. + safe_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:
+    safe_exp(x, out=x)
+  else:
+    x = safe_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))
+
+    n_values = (raw.shape[2] - out_N)//2
+    pred_mu = raw[:,:,:n_values]
+    pred_std = safe_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,))
+    if 'sim_pose' in outs:
+      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))
+    if 'lat_planner_solution' in outs:
+      self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N,ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
+    if 'desired_curvature' in outs:
+      self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_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

sunnypilot/modeld/runners/__init__.py

@@ -0,0 +1,27 @@
+import os
+from openpilot.system.hardware import TICI
+from openpilot.sunnypilot.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.sunnypilot.modeld.runners.thneedmodel_pyx import ThneedModel as Runner
+      runner_type = ModelRunner.THNEED
+    elif ModelRunner.SNPE in paths and USE_SNPE:
+      from openpilot.sunnypilot.modeld.runners.snpemodel_pyx import SNPEModel as Runner
+      runner_type = ModelRunner.SNPE
+    elif ModelRunner.ONNX in paths:
+      from openpilot.sunnypilot.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)

sunnypilot/modeld/runners/onnxmodel.py

@@ -0,0 +1,98 @@
+import onnx
+import itertools
+import os
+import sys
+import numpy as np
+from typing import Any
+
+from openpilot.sunnypilot.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(onnx_path_or_bytes):
+  if isinstance(onnx_path_or_bytes, bytes):
+    model = onnx.load_from_string(onnx_path_or_bytes)
+  elif isinstance(onnx_path_or_bytes, str):
+    model = onnx.load(onnx_path_or_bytes)
+
+  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:
+    if i.op_type == 'Cast' and i.attribute[0].i == 10:
+      i.attribute[0].i = 1
+    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: 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.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(self.input_dtypes[k]) 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

sunnypilot/modeld/runners/run.h

@@ -0,0 +1,4 @@
+#pragma once
+
+#include "sunnypilot/modeld/runners/runmodel.h"
+#include "sunnypilot/modeld/runners/snpemodel.h"

sunnypilot/modeld/runners/run_helpers.py

@@ -0,0 +1,90 @@
+# Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+#
+# This file is part of sunnypilot and is licensed under the MIT License.
+# See the LICENSE.md file in the root directory for more details.
+
+import os
+import pickle
+import numpy as np
+
+from cereal import custom
+from openpilot.sunnypilot.modeld.constants import Meta, MetaTombRaider, MetaSimPose
+from openpilot.sunnypilot.modeld.runners import ModelRunner
+from openpilot.sunnypilot.models.helpers import get_active_bundle
+from openpilot.system.hardware import PC
+from openpilot.system.hardware.hw import Paths
+from pathlib import Path
+
+USE_ONNX = os.getenv('USE_ONNX', PC)
+
+CUSTOM_MODEL_PATH = Paths.model_root()
+METADATA_PATH = Path(__file__).parent / '../models/supercombo_metadata.pkl'
+
+ModelManager = custom.ModelManagerSP
+
+
+def get_model_path():
+  if USE_ONNX:
+    return {ModelRunner.ONNX: Path(__file__).parent / '../models/supercombo.onnx'}
+
+  if bundle := get_active_bundle():
+    drive_model = next(model for model in bundle.models if model.type == ModelManager.Type.drive)
+    return {ModelRunner.THNEED: f"{CUSTOM_MODEL_PATH}/{drive_model.fileName}"}
+
+  return {ModelRunner.THNEED: Path(__file__).parent / '../models/supercombo.thneed'}
+
+
+def load_metadata():
+  metadata_path = METADATA_PATH
+
+  if bundle := get_active_bundle():
+    metadata_model = next(model for model in bundle.models if model.type == ModelManager.Type.metadata)
+    metadata_path = f"{CUSTOM_MODEL_PATH}/{metadata_model.fileName}"
+
+  with open(metadata_path, 'rb') as f:
+    return pickle.load(f)
+
+
+def prepare_inputs(model_metadata) -> dict[str, np.ndarray]:
+  # img buffers are managed in openCL transform code so we don't pass them as inputs
+  inputs = {
+    k: np.zeros(v, dtype=np.float32).flatten()
+    for k, v in model_metadata['input_shapes'].items()
+    if 'img' not in k
+  }
+
+  return inputs
+
+
+def load_meta_constants(model_metadata):
+  """
+  Determines and loads the appropriate meta model class based on the metadata provided. The function checks
+  specific keys and conditions within the provided metadata dictionary to identify the corresponding meta
+  model class to return.
+
+  :param model_metadata: Dictionary containing metadata about the model. It includes
+      details such as input shapes, output slices, and other configurations for identifying
+      metadata-dependent meta model classes.
+  :type model_metadata: dict
+  :return: The appropriate meta model class (Meta, MetaSimPose, or MetaTombRaider)
+      based on the conditions and metadata provided.
+  :rtype: type
+  """
+  meta = Meta  # Default Meta
+
+  if 'sim_pose' in model_metadata['input_shapes'].keys():
+    # Meta for models with sim_pose input
+    meta = MetaSimPose
+  else:
+    # Meta for Tomb Raider, it does not include sim_pose input but has the same meta slice as previous models
+    meta_slice = model_metadata['output_slices']['meta']
+    meta_tf_slice = slice(5868, 5921, None)
+
+    if (
+          meta_slice.start == meta_tf_slice.start and
+          meta_slice.stop == meta_tf_slice.stop and
+          meta_slice.step == meta_tf_slice.step
+    ):
+      meta = MetaTombRaider
+
+  return meta

sunnypilot/modeld/runners/runmodel.h

@@ -0,0 +1,49 @@
+#pragma once
+
+#include <string>
+#include <vector>
+#include <memory>
+#include <cassert>
+
+#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;
+  int size;
+
+  ModelInput(const std::string _name, float *_buffer, int _size) : name(_name), buffer(_buffer), size(_size) {}
+  virtual void setBuffer(float *_buffer, int _size) {
+    assert(size == _size || size == 0);
+    buffer = _buffer;
+    size = _size;
+  }
+};
+
+class RunModel {
+public:
+  std::vector<std::unique_ptr<ModelInput>> inputs;
+
+  virtual ~RunModel() {}
+  virtual void execute() {}
+  virtual void* getCLBuffer(const std::string name) { return nullptr; }
+
+  virtual void addInput(const std::string name, float *buffer, int size) {
+    inputs.push_back(std::unique_ptr<ModelInput>(new ModelInput(name, buffer, size)));
+  }
+  virtual void setInputBuffer(const std::string name, float *buffer, int size) {
+    for (auto &input : inputs) {
+      if (name == input->name) {
+        input->setBuffer(buffer, size);
+        return;
+      }
+    }
+    LOGE("Tried to update input `%s` but no input with this name exists", name.c_str());
+    assert(false);
+  }
+};

sunnypilot/modeld/runners/runmodel.pxd

@@ -0,0 +1,14 @@
+# distutils: language = c++
+
+from libcpp.string cimport string
+
+cdef extern from "sunnypilot/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()

sunnypilot/modeld/runners/runmodel_pyx.pxd

@@ -0,0 +1,6 @@
+# distutils: language = c++
+
+from .runmodel cimport RunModel as cppRunModel
+
+cdef class RunModel:
+  cdef cppRunModel * model

sunnypilot/modeld/runners/runmodel_pyx.pyx

@@ -0,0 +1,37 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii, language_level=3
+
+from libcpp.string cimport string
+
+from .runmodel cimport USE_CPU_RUNTIME, USE_GPU_RUNTIME, USE_DSP_RUNTIME
+from sunnypilot.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()

sunnypilot/modeld/runners/snpemodel.cc

@@ -0,0 +1,116 @@
+#pragma clang diagnostic ignored "-Wexceptions"
+
+#include "sunnypilot/modeld/runners/snpemodel.h"
+
+#include <cstring>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "common/util.h"
+#include "common/timing.h"
+
+void PrintErrorStringAndExit() {
+  std::cerr << zdl::DlSystem::getLastErrorString() << std::endl;
+  std::exit(EXIT_FAILURE);
+}
+
+SNPEModel::SNPEModel(const std::string path, float *_output, size_t _output_size, int runtime, bool _use_tf8, cl_context context) {
+  output = _output;
+  output_size = _output_size;
+  use_tf8 = _use_tf8;
+
+#ifdef QCOM2
+  if (runtime == USE_GPU_RUNTIME) {
+    snpe_runtime = zdl::DlSystem::Runtime_t::GPU;
+  } else if (runtime == USE_DSP_RUNTIME) {
+    snpe_runtime = zdl::DlSystem::Runtime_t::DSP;
+  } else {
+    snpe_runtime = zdl::DlSystem::Runtime_t::CPU;
+  }
+  assert(zdl::SNPE::SNPEFactory::isRuntimeAvailable(snpe_runtime));
+#endif
+  model_data = util::read_file(path);
+  assert(model_data.size() > 0);
+
+  // load model
+  std::unique_ptr<zdl::DlContainer::IDlContainer> container = zdl::DlContainer::IDlContainer::open((uint8_t*)model_data.data(), model_data.size());
+  if (!container) { PrintErrorStringAndExit(); }
+  LOGW("loaded model with size: %lu", model_data.size());
+
+  // create model runner
+  zdl::SNPE::SNPEBuilder snpe_builder(container.get());
+  while (!snpe) {
+#ifdef QCOM2
+    snpe = snpe_builder.setOutputLayers({})
+                       .setRuntimeProcessor(snpe_runtime)
+                       .setUseUserSuppliedBuffers(true)
+                       .setPerformanceProfile(zdl::DlSystem::PerformanceProfile_t::HIGH_PERFORMANCE)
+                       .build();
+#else
+    snpe = snpe_builder.setOutputLayers({})
+                       .setUseUserSuppliedBuffers(true)
+                       .setPerformanceProfile(zdl::DlSystem::PerformanceProfile_t::HIGH_PERFORMANCE)
+                       .build();
+#endif
+    if (!snpe) std::cerr << zdl::DlSystem::getLastErrorString() << std::endl;
+  }
+
+  // create output buffer
+  zdl::DlSystem::UserBufferEncodingFloat ub_encoding_float;
+  zdl::DlSystem::IUserBufferFactory &ub_factory = zdl::SNPE::SNPEFactory::getUserBufferFactory();
+
+  const auto &output_tensor_names_opt = snpe->getOutputTensorNames();
+  if (!output_tensor_names_opt) throw std::runtime_error("Error obtaining output tensor names");
+  const auto &output_tensor_names = *output_tensor_names_opt;
+  assert(output_tensor_names.size() == 1);
+  const char *output_tensor_name = output_tensor_names.at(0);
+  const zdl::DlSystem::TensorShape &buffer_shape = snpe->getInputOutputBufferAttributes(output_tensor_name)->getDims();
+  if (output_size != 0) {
+    assert(output_size == buffer_shape[1]);
+  } else {
+    output_size = buffer_shape[1];
+  }
+  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());
+}
+
+void SNPEModel::addInput(const std::string name, float *buffer, int size) {
+  const int idx = inputs.size();
+  const auto &input_tensor_names_opt = snpe->getInputTensorNames();
+  if (!input_tensor_names_opt) throw std::runtime_error("Error obtaining input tensor names");
+  const auto &input_tensor_names = *input_tensor_names_opt;
+  const char *input_tensor_name = input_tensor_names.at(idx);
+  const bool input_tf8 = use_tf8 && strcmp(input_tensor_name, "input_img") == 0;  // TODO: This is a terrible hack, get rid of this name check both here and in onnx_runner.py
+  LOGW("adding index %d: %s", idx, input_tensor_name);
+
+  zdl::DlSystem::UserBufferEncodingFloat ub_encoding_float;
+  zdl::DlSystem::UserBufferEncodingTf8 ub_encoding_tf8(0, 1./255); // network takes 0-1
+  zdl::DlSystem::IUserBufferFactory &ub_factory = zdl::SNPE::SNPEFactory::getUserBufferFactory();
+  zdl::DlSystem::UserBufferEncoding *input_encoding = input_tf8 ? (zdl::DlSystem::UserBufferEncoding*)&ub_encoding_tf8 : (zdl::DlSystem::UserBufferEncoding*)&ub_encoding_float;
+
+  const auto &buffer_shape_opt = snpe->getInputDimensions(input_tensor_name);
+  const zdl::DlSystem::TensorShape &buffer_shape = *buffer_shape_opt;
+  size_t size_of_input = input_tf8 ? sizeof(uint8_t) : sizeof(float);
+  std::vector<size_t> strides(buffer_shape.rank());
+  strides[strides.size() - 1] = size_of_input;
+  size_t product = 1;
+  for (size_t i = 0; i < buffer_shape.rank(); i++) product *= buffer_shape[i];
+  size_t stride = strides[strides.size() - 1];
+  for (size_t i = buffer_shape.rank() - 1; i > 0; i--) {
+    stride *= buffer_shape[i];
+    strides[i-1] = stride;
+  }
+
+  auto input_buffer = ub_factory.createUserBuffer(buffer, product*size_of_input, strides, input_encoding);
+  input_map.add(input_tensor_name, input_buffer.get());
+  inputs.push_back(std::unique_ptr<SNPEModelInput>(new SNPEModelInput(name, buffer, size, std::move(input_buffer))));
+}
+
+void SNPEModel::execute() {
+  if (!snpe->execute(input_map, output_map)) {
+    PrintErrorStringAndExit();
+  }
+}

sunnypilot/modeld/runners/snpemodel.h

@@ -0,0 +1,52 @@
+#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>
+#include <DlSystem/ITensorFactory.hpp>
+#include <DlSystem/IUserBuffer.hpp>
+#include <DlSystem/IUserBufferFactory.hpp>
+#include <SNPE/SNPE.hpp>
+#include <SNPE/SNPEBuilder.hpp>
+#include <SNPE/SNPEFactory.hpp>
+
+#include "sunnypilot/modeld/runners/runmodel.h"
+
+struct SNPEModelInput : public ModelInput {
+  std::unique_ptr<zdl::DlSystem::IUserBuffer> snpe_buffer;
+
+  SNPEModelInput(const std::string _name, float *_buffer, int _size, std::unique_ptr<zdl::DlSystem::IUserBuffer> _snpe_buffer) : ModelInput(_name, _buffer, _size), snpe_buffer(std::move(_snpe_buffer)) {}
+  void setBuffer(float *_buffer, int _size) {
+    ModelInput::setBuffer(_buffer, _size);
+    assert(snpe_buffer->setBufferAddress(_buffer) == true);
+  }
+};
+
+class SNPEModel : public RunModel {
+public:
+  SNPEModel(const std::string path, float *_output, size_t _output_size, int runtime, bool use_tf8 = false, cl_context context = NULL);
+  void addInput(const std::string name, float *buffer, int size);
+  void execute();
+
+private:
+  std::string model_data;
+
+#ifdef QCOM2
+  zdl::DlSystem::Runtime_t snpe_runtime;
+#endif
+
+  // snpe model stuff
+  std::unique_ptr<zdl::SNPE::SNPE> snpe;
+  zdl::DlSystem::UserBufferMap input_map;
+  zdl::DlSystem::UserBufferMap output_map;
+  std::unique_ptr<zdl::DlSystem::IUserBuffer> output_buffer;
+
+  bool use_tf8;
+  float *output;
+  size_t output_size;
+};

sunnypilot/modeld/runners/snpemodel.pxd

@@ -0,0 +1,9 @@
+# distutils: language = c++
+
+from libcpp.string cimport string
+
+from msgq.visionipc.visionipc cimport cl_context
+
+cdef extern from "sunnypilot/modeld/runners/snpemodel.h":
+  cdef cppclass SNPEModel:
+    SNPEModel(string, float*, size_t, int, bool, cl_context)

sunnypilot/modeld/runners/snpemodel_pyx.pyx

@@ -0,0 +1,17 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii, language_level=3
+
+import os
+from libcpp cimport bool
+from libcpp.string cimport string
+
+from .snpemodel cimport SNPEModel as cppSNPEModel
+from sunnypilot.modeld.models.commonmodel_pyx cimport CLContext
+from sunnypilot.modeld.runners.runmodel_pyx cimport RunModel
+from sunnypilot.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)

sunnypilot/modeld/runners/thneedmodel.cc

@@ -0,0 +1,58 @@
+#include "sunnypilot/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) {
+  thneed = new Thneed(true, context);
+  thneed->load(path.c_str());
+  thneed->clexec();
+
+  recorded = false;
+  output = _output;
+}
+
+void* ThneedModel::getCLBuffer(const std::string name) {
+  int index = -1;
+  for (int i = 0; i < inputs.size(); i++) {
+    if (name == inputs[i]->name) {
+      index = i;
+      break;
+    }
+  }
+
+  if (index == -1) {
+    LOGE("Tried to get CL buffer for input `%s` but no input with this name exists", name.c_str());
+    assert(false);
+  }
+
+  if (thneed->input_clmem.size() >= inputs.size()) {
+    return &thneed->input_clmem[inputs.size() - index - 1];
+  } else {
+    return nullptr;
+  }
+}
+
+void ThneedModel::execute() {
+  if (!recorded) {
+    thneed->record = true;
+    float *input_buffers[inputs.size()];
+    for (int i = 0; i < inputs.size(); i++) {
+      input_buffers[inputs.size() - i - 1] = inputs[i]->buffer;
+    }
+
+    thneed->copy_inputs(input_buffers);
+    thneed->clexec();
+    thneed->copy_output(output);
+    thneed->stop();
+
+    recorded = true;
+  } else {
+    float *input_buffers[inputs.size()];
+    for (int i = 0; i < inputs.size(); i++) {
+      input_buffers[inputs.size() - i - 1] = inputs[i]->buffer;
+    }
+    thneed->execute(input_buffers, output);
+  }
+}

sunnypilot/modeld/runners/thneedmodel.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include <string>
+
+#include "sunnypilot/modeld/runners/runmodel.h"
+#include "sunnypilot/modeld/thneed/thneed.h"
+
+class ThneedModel : public RunModel {
+public:
+  ThneedModel(const std::string path, float *_output, size_t _output_size, int runtime, bool use_tf8 = false, cl_context context = NULL);
+  void *getCLBuffer(const std::string name);
+  void execute();
+private:
+  Thneed *thneed = NULL;
+  bool recorded;
+  float *output;
+};

sunnypilot/modeld/runners/thneedmodel.pxd

@@ -0,0 +1,9 @@
+# distutils: language = c++
+
+from libcpp.string cimport string
+
+from msgq.visionipc.visionipc cimport cl_context
+
+cdef extern from "sunnypilot/modeld/runners/thneedmodel.h":
+  cdef cppclass ThneedModel:
+    ThneedModel(string, float*, size_t, int, bool, cl_context)

sunnypilot/modeld/runners/thneedmodel_pyx.pyx

@@ -0,0 +1,14 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii, language_level=3
+
+from libcpp cimport bool
+from libcpp.string cimport string
+
+from .thneedmodel cimport ThneedModel as cppThneedModel
+from sunnypilot.modeld.models.commonmodel_pyx cimport CLContext
+from sunnypilot.modeld.runners.runmodel_pyx cimport RunModel
+from sunnypilot.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)

sunnypilot/modeld/thneed/README

@@ -0,0 +1,8 @@
+thneed is an SNPE accelerator. I know SNPE is already an accelerator, but sometimes things need to go even faster..
+
+It runs on the local device, and caches a single model run. Then it replays it, but fast.
+
+thneed slices through abstraction layers like a fish.
+
+You need a thneed.
+

sunnypilot/modeld/thneed/serialize.cc

@@ -0,0 +1,154 @@
+#include <cassert>
+#include <set>
+
+#include "third_party/json11/json11.hpp"
+#include "common/util.h"
+#include "common/clutil.h"
+#include "common/swaglog.h"
+#include "sunnypilot/modeld/thneed/thneed.h"
+using namespace json11;
+
+extern map<cl_program, string> g_program_source;
+
+void Thneed::load(const char *filename) {
+  LOGD("Thneed::load: loading from %s\n", filename);
+
+  string buf = util::read_file(filename);
+  int jsz = *(int *)buf.data();
+  string jsonerr;
+  string jj(buf.data() + sizeof(int), jsz);
+  Json jdat = Json::parse(jj, jsonerr);
+
+  map<cl_mem, cl_mem> real_mem;
+  real_mem[NULL] = NULL;
+
+  int ptr = sizeof(int)+jsz;
+  for (auto &obj : jdat["objects"].array_items()) {
+    auto mobj = obj.object_items();
+    int sz = mobj["size"].int_value();
+    cl_mem clbuf = NULL;
+
+    if (mobj["buffer_id"].string_value().size() > 0) {
+      // image buffer must already be allocated
+      clbuf = real_mem[*(cl_mem*)(mobj["buffer_id"].string_value().data())];
+      assert(mobj["needs_load"].bool_value() == false);
+    } else {
+      if (mobj["needs_load"].bool_value()) {
+        clbuf = clCreateBuffer(context, CL_MEM_COPY_HOST_PTR | CL_MEM_READ_WRITE, sz, &buf[ptr], NULL);
+        if (debug >= 1) printf("loading %p %d @ 0x%X\n", clbuf, sz, ptr);
+        ptr += sz;
+      } else {
+        // TODO: is there a faster way to init zeroed out buffers?
+        void *host_zeros = calloc(sz, 1);
+        clbuf = clCreateBuffer(context, CL_MEM_COPY_HOST_PTR | CL_MEM_READ_WRITE, sz, host_zeros, NULL);
+        free(host_zeros);
+      }
+    }
+    assert(clbuf != NULL);
+
+    if (mobj["arg_type"] == "image2d_t" || mobj["arg_type"] == "image1d_t") {
+      cl_image_desc desc = {0};
+      desc.image_type = (mobj["arg_type"] == "image2d_t") ? CL_MEM_OBJECT_IMAGE2D : CL_MEM_OBJECT_IMAGE1D_BUFFER;
+      desc.image_width = mobj["width"].int_value();
+      desc.image_height = mobj["height"].int_value();
+      desc.image_row_pitch = mobj["row_pitch"].int_value();
+      assert(sz == desc.image_height*desc.image_row_pitch);
+#ifdef QCOM2
+      desc.buffer = clbuf;
+#else
+      // TODO: we are creating unused buffers on PC
+      clReleaseMemObject(clbuf);
+#endif
+      cl_image_format format = {0};
+      format.image_channel_order = CL_RGBA;
+      format.image_channel_data_type = mobj["float32"].bool_value() ? CL_FLOAT : CL_HALF_FLOAT;
+
+      cl_int errcode;
+
+#ifndef QCOM2
+      if (mobj["needs_load"].bool_value()) {
+        clbuf = clCreateImage(context, CL_MEM_COPY_HOST_PTR | CL_MEM_READ_WRITE, &format, &desc, &buf[ptr-sz], &errcode);
+      } else {
+        clbuf = clCreateImage(context, CL_MEM_READ_WRITE, &format, &desc, NULL, &errcode);
+      }
+#else
+      clbuf = clCreateImage(context, CL_MEM_READ_WRITE, &format, &desc, NULL, &errcode);
+#endif
+      if (clbuf == NULL) {
+        LOGE("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);
+      }
+      assert(clbuf != NULL);
+    }
+
+    real_mem[*(cl_mem*)(mobj["id"].string_value().data())] = clbuf;
+  }
+
+  map<string, cl_program> g_programs;
+  for (const auto &[name, source] : jdat["programs"].object_items()) {
+    if (debug >= 1) printf("building %s with size %zu\n", name.c_str(), source.string_value().size());
+    g_programs[name] = cl_program_from_source(context, device_id, source.string_value());
+  }
+
+  for (auto &obj : jdat["inputs"].array_items()) {
+    auto mobj = obj.object_items();
+    int sz = mobj["size"].int_value();
+    cl_mem aa = real_mem[*(cl_mem*)(mobj["buffer_id"].string_value().data())];
+    input_clmem.push_back(aa);
+    input_sizes.push_back(sz);
+    LOGD("Thneed::load: adding input %s with size %d\n", mobj["name"].string_value().data(), sz);
+
+    cl_int cl_err;
+    void *ret = clEnqueueMapBuffer(command_queue, aa, CL_TRUE, CL_MAP_WRITE, 0, sz, 0, NULL, NULL, &cl_err);
+    if (cl_err != CL_SUCCESS) LOGE("clError: %s map %p %d\n", cl_get_error_string(cl_err), aa, sz);
+    assert(cl_err == CL_SUCCESS);
+    inputs.push_back(ret);
+  }
+
+  for (auto &obj : jdat["outputs"].array_items()) {
+    auto mobj = obj.object_items();
+    int sz = mobj["size"].int_value();
+    LOGD("Thneed::save: adding output with size %d\n", sz);
+    // TODO: support multiple outputs
+    output = real_mem[*(cl_mem*)(mobj["buffer_id"].string_value().data())];
+    assert(output != NULL);
+  }
+
+  for (auto &obj : jdat["binaries"].array_items()) {
+    string name = obj["name"].string_value();
+    size_t length = obj["length"].int_value();
+    if (debug >= 1) printf("binary %s with size %zu\n", name.c_str(), length);
+    g_programs[name] = cl_program_from_binary(context, device_id, (const uint8_t*)&buf[ptr], length);
+    ptr += length;
+  }
+
+  for (auto &obj : jdat["kernels"].array_items()) {
+    auto gws = obj["global_work_size"];
+    auto lws = obj["local_work_size"];
+    auto kk = shared_ptr<CLQueuedKernel>(new CLQueuedKernel(this));
+
+    kk->name = obj["name"].string_value();
+    kk->program = g_programs[kk->name];
+    kk->work_dim = obj["work_dim"].int_value();
+    for (int i = 0; i < kk->work_dim; i++) {
+      kk->global_work_size[i] = gws[i].int_value();
+      kk->local_work_size[i] = lws[i].int_value();
+    }
+    kk->num_args = obj["num_args"].int_value();
+    for (int i = 0; i < kk->num_args; i++) {
+      string arg = obj["args"].array_items()[i].string_value();
+      int arg_size = obj["args_size"].array_items()[i].int_value();
+      kk->args_size.push_back(arg_size);
+      if (arg_size == 8) {
+        cl_mem val = *(cl_mem*)(arg.data());
+        val = real_mem[val];
+        kk->args.push_back(string((char*)&val, sizeof(val)));
+      } else {
+        kk->args.push_back(arg);
+      }
+    }
+    kq.push_back(kk);
+  }
+
+  clFinish(command_queue);
+}

sunnypilot/modeld/thneed/thneed.h

@@ -0,0 +1,133 @@
+#pragma once
+
+#ifndef __user
+#define __user __attribute__(())
+#endif
+
+#include <cstdint>
+#include <cstdlib>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <CL/cl.h>
+
+#include "third_party/linux/include/msm_kgsl.h"
+
+using namespace std;
+
+cl_int thneed_clSetKernelArg(cl_kernel kernel, cl_uint arg_index, size_t arg_size, const void *arg_value);
+
+namespace json11 {
+  class Json;
+}
+class Thneed;
+
+class GPUMalloc {
+  public:
+    GPUMalloc(int size, int fd);
+    ~GPUMalloc();
+    void *alloc(int size);
+  private:
+    uint64_t base;
+    int remaining;
+};
+
+class CLQueuedKernel {
+  public:
+    CLQueuedKernel(Thneed *lthneed) { thneed = lthneed; }
+    CLQueuedKernel(Thneed *lthneed,
+                   cl_kernel _kernel,
+                   cl_uint _work_dim,
+                   const size_t *_global_work_size,
+                   const size_t *_local_work_size);
+    cl_int exec();
+    void debug_print(bool verbose);
+    int get_arg_num(const char *search_arg_name);
+    cl_program program;
+    string name;
+    cl_uint num_args;
+    vector<string> arg_names;
+    vector<string> arg_types;
+    vector<string> args;
+    vector<int> args_size;
+    cl_kernel kernel = NULL;
+    json11::Json to_json() const;
+
+    cl_uint work_dim;
+    size_t global_work_size[3] = {0};
+    size_t local_work_size[3] = {0};
+  private:
+    Thneed *thneed;
+};
+
+class CachedIoctl {
+  public:
+    virtual void exec() {}
+};
+
+class CachedSync: public CachedIoctl {
+  public:
+    CachedSync(Thneed *lthneed, string ldata) { thneed = lthneed; data = ldata; }
+    void exec();
+  private:
+    Thneed *thneed;
+    string data;
+};
+
+class CachedCommand: public CachedIoctl {
+  public:
+    CachedCommand(Thneed *lthneed, struct kgsl_gpu_command *cmd);
+    void exec();
+  private:
+    void disassemble(int cmd_index);
+    struct kgsl_gpu_command cache;
+    unique_ptr<kgsl_command_object[]> cmds;
+    unique_ptr<kgsl_command_object[]> objs;
+    Thneed *thneed;
+    vector<shared_ptr<CLQueuedKernel> > kq;
+};
+
+class Thneed {
+  public:
+    Thneed(bool do_clinit=false, cl_context _context = NULL);
+    void stop();
+    void execute(float **finputs, float *foutput, bool slow=false);
+    void wait();
+
+    vector<cl_mem> input_clmem;
+    vector<void *> inputs;
+    vector<size_t> input_sizes;
+    cl_mem output = NULL;
+
+    cl_context context = NULL;
+    cl_command_queue command_queue;
+    cl_device_id device_id;
+    int context_id;
+
+    // protected?
+    bool record = false;
+    int debug;
+    int timestamp;
+
+#ifdef QCOM2
+    unique_ptr<GPUMalloc> ram;
+    vector<unique_ptr<CachedIoctl> > cmds;
+    int fd;
+#endif
+
+    // all CL kernels
+    void copy_inputs(float **finputs, bool internal=false);
+    void copy_output(float *foutput);
+    cl_int clexec();
+    vector<shared_ptr<CLQueuedKernel> > kq;
+
+    // pending CL kernels
+    vector<shared_ptr<CLQueuedKernel> > ckq;
+
+    // loading
+    void load(const char *filename);
+  private:
+    void clinit();
+};
+

sunnypilot/modeld/thneed/thneed_common.cc

@@ -0,0 +1,216 @@
+#include "sunnypilot/modeld/thneed/thneed.h"
+
+#include <cassert>
+#include <cstring>
+#include <map>
+
+#include "common/clutil.h"
+#include "common/timing.h"
+
+map<pair<cl_kernel, int>, string> g_args;
+map<pair<cl_kernel, int>, int> g_args_size;
+map<cl_program, string> g_program_source;
+
+void Thneed::stop() {
+  //printf("Thneed::stop: recorded %lu commands\n", cmds.size());
+  record = false;
+}
+
+void Thneed::clinit() {
+  device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
+  if (context == NULL) context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err));
+  //cl_command_queue_properties props[3] = {CL_QUEUE_PROPERTIES, CL_QUEUE_PROFILING_ENABLE, 0};
+  cl_command_queue_properties props[3] = {CL_QUEUE_PROPERTIES, 0, 0};
+  command_queue = CL_CHECK_ERR(clCreateCommandQueueWithProperties(context, device_id, props, &err));
+  printf("Thneed::clinit done\n");
+}
+
+cl_int Thneed::clexec() {
+  if (debug >= 1) printf("Thneed::clexec: running %lu queued kernels\n", kq.size());
+  for (auto &k : kq) {
+    if (record) ckq.push_back(k);
+    cl_int ret = k->exec();
+    assert(ret == CL_SUCCESS);
+  }
+  return clFinish(command_queue);
+}
+
+void Thneed::copy_inputs(float **finputs, bool internal) {
+  for (int idx = 0; idx < inputs.size(); ++idx) {
+    if (debug >= 1) printf("copying %lu -- %p -> %p (cl %p)\n", input_sizes[idx], finputs[idx], inputs[idx], input_clmem[idx]);
+
+    if (internal) {
+      // if it's internal, using memcpy is fine since the buffer sync is cached in the ioctl layer
+      if (finputs[idx] != NULL) memcpy(inputs[idx], finputs[idx], input_sizes[idx]);
+    } else {
+      if (finputs[idx] != NULL) CL_CHECK(clEnqueueWriteBuffer(command_queue, input_clmem[idx], CL_TRUE, 0, input_sizes[idx], finputs[idx], 0, NULL, NULL));
+    }
+  }
+}
+
+void Thneed::copy_output(float *foutput) {
+  if (output != NULL) {
+    size_t sz;
+    clGetMemObjectInfo(output, CL_MEM_SIZE, sizeof(sz), &sz, NULL);
+    if (debug >= 1) printf("copying %lu for output %p -> %p\n", sz, output, foutput);
+    CL_CHECK(clEnqueueReadBuffer(command_queue, output, CL_TRUE, 0, sz, foutput, 0, NULL, NULL));
+  } else {
+    printf("CAUTION: model output is NULL, does it have no outputs?\n");
+  }
+}
+
+// *********** CLQueuedKernel ***********
+
+CLQueuedKernel::CLQueuedKernel(Thneed *lthneed,
+                               cl_kernel _kernel,
+                               cl_uint _work_dim,
+                               const size_t *_global_work_size,
+                               const size_t *_local_work_size) {
+  thneed = lthneed;
+  kernel = _kernel;
+  work_dim = _work_dim;
+  assert(work_dim <= 3);
+  for (int i = 0; i < work_dim; i++) {
+    global_work_size[i] = _global_work_size[i];
+    local_work_size[i] = _local_work_size[i];
+  }
+
+  char _name[0x100];
+  clGetKernelInfo(kernel, CL_KERNEL_FUNCTION_NAME, sizeof(_name), _name, NULL);
+  name = string(_name);
+  clGetKernelInfo(kernel, CL_KERNEL_NUM_ARGS, sizeof(num_args), &num_args, NULL);
+
+  // get args
+  for (int i = 0; i < num_args; i++) {
+    char arg_name[0x100] = {0};
+    clGetKernelArgInfo(kernel, i, CL_KERNEL_ARG_NAME, sizeof(arg_name), arg_name, NULL);
+    arg_names.push_back(string(arg_name));
+    clGetKernelArgInfo(kernel, i, CL_KERNEL_ARG_TYPE_NAME, sizeof(arg_name), arg_name, NULL);
+    arg_types.push_back(string(arg_name));
+
+    args.push_back(g_args[make_pair(kernel, i)]);
+    args_size.push_back(g_args_size[make_pair(kernel, i)]);
+  }
+
+  // get program
+  clGetKernelInfo(kernel, CL_KERNEL_PROGRAM, sizeof(program), &program, NULL);
+}
+
+int CLQueuedKernel::get_arg_num(const char *search_arg_name) {
+  for (int i = 0; i < num_args; i++) {
+    if (arg_names[i] == search_arg_name) return i;
+  }
+  printf("failed to find %s in %s\n", search_arg_name, name.c_str());
+  assert(false);
+}
+
+cl_int CLQueuedKernel::exec() {
+  if (kernel == NULL) {
+    kernel = clCreateKernel(program, name.c_str(), NULL);
+    arg_names.clear();
+    arg_types.clear();
+
+    for (int j = 0; j < num_args; j++) {
+      char arg_name[0x100] = {0};
+      clGetKernelArgInfo(kernel, j, CL_KERNEL_ARG_NAME, sizeof(arg_name), arg_name, NULL);
+      arg_names.push_back(string(arg_name));
+      clGetKernelArgInfo(kernel, j, CL_KERNEL_ARG_TYPE_NAME, sizeof(arg_name), arg_name, NULL);
+      arg_types.push_back(string(arg_name));
+
+      cl_int ret;
+      if (args[j].size() != 0) {
+        assert(args[j].size() == args_size[j]);
+        ret = thneed_clSetKernelArg(kernel, j, args[j].size(), args[j].data());
+      } else {
+        ret = thneed_clSetKernelArg(kernel, j, args_size[j], NULL);
+      }
+      assert(ret == CL_SUCCESS);
+    }
+  }
+
+  if (thneed->debug >= 1) {
+    debug_print(thneed->debug >= 2);
+  }
+
+  return clEnqueueNDRangeKernel(thneed->command_queue,
+    kernel, work_dim, NULL, global_work_size, local_work_size, 0, NULL, NULL);
+}
+
+void CLQueuedKernel::debug_print(bool verbose) {
+  printf("%p %56s -- ", kernel, name.c_str());
+  for (int i = 0; i < work_dim; i++) {
+    printf("%4zu ", global_work_size[i]);
+  }
+  printf(" -- ");
+  for (int i = 0; i < work_dim; i++) {
+    printf("%4zu ", local_work_size[i]);
+  }
+  printf("\n");
+
+  if (verbose) {
+    for (int i = 0; i < num_args; i++) {
+      string arg = args[i];
+      printf("  %s %s", arg_types[i].c_str(), arg_names[i].c_str());
+      void *arg_value = (void*)arg.data();
+      int arg_size = arg.size();
+      if (arg_size == 0) {
+        printf(" (size) %d", args_size[i]);
+      } else if (arg_size == 1) {
+        printf(" = %d", *((char*)arg_value));
+      } else if (arg_size == 2) {
+        printf(" = %d", *((short*)arg_value));
+      } else if (arg_size == 4) {
+        if (arg_types[i] == "float") {
+          printf(" = %f", *((float*)arg_value));
+        } else {
+          printf(" = %d", *((int*)arg_value));
+        }
+      } else if (arg_size == 8) {
+        cl_mem val = (cl_mem)(*((uintptr_t*)arg_value));
+        printf(" = %p", val);
+        if (val != NULL) {
+          cl_mem_object_type obj_type;
+          clGetMemObjectInfo(val, CL_MEM_TYPE, sizeof(obj_type), &obj_type, NULL);
+          if (arg_types[i] == "image2d_t" || arg_types[i] == "image1d_t" || obj_type == CL_MEM_OBJECT_IMAGE2D) {
+            cl_image_format format;
+            size_t width, height, depth, array_size, row_pitch, slice_pitch;
+            cl_mem buf;
+            clGetImageInfo(val, CL_IMAGE_FORMAT, sizeof(format), &format, NULL);
+            assert(format.image_channel_order == CL_RGBA);
+            assert(format.image_channel_data_type == CL_HALF_FLOAT || format.image_channel_data_type == CL_FLOAT);
+            clGetImageInfo(val, CL_IMAGE_WIDTH, sizeof(width), &width, NULL);
+            clGetImageInfo(val, CL_IMAGE_HEIGHT, sizeof(height), &height, NULL);
+            clGetImageInfo(val, CL_IMAGE_ROW_PITCH, sizeof(row_pitch), &row_pitch, NULL);
+            clGetImageInfo(val, CL_IMAGE_DEPTH, sizeof(depth), &depth, NULL);
+            clGetImageInfo(val, CL_IMAGE_ARRAY_SIZE, sizeof(array_size), &array_size, NULL);
+            clGetImageInfo(val, CL_IMAGE_SLICE_PITCH, sizeof(slice_pitch), &slice_pitch, NULL);
+            assert(depth == 0);
+            assert(array_size == 0);
+            assert(slice_pitch == 0);
+
+            clGetImageInfo(val, CL_IMAGE_BUFFER, sizeof(buf), &buf, NULL);
+            size_t sz = 0;
+            if (buf != NULL) clGetMemObjectInfo(buf, CL_MEM_SIZE, sizeof(sz), &sz, NULL);
+            printf(" image %zu x %zu rp %zu @ %p buffer %zu", width, height, row_pitch, buf, sz);
+          } else {
+            size_t sz;
+            clGetMemObjectInfo(val, CL_MEM_SIZE, sizeof(sz), &sz, NULL);
+            printf(" buffer %zu", sz);
+          }
+        }
+      }
+      printf("\n");
+    }
+  }
+}
+
+cl_int thneed_clSetKernelArg(cl_kernel kernel, cl_uint arg_index, size_t arg_size, const void *arg_value) {
+  g_args_size[make_pair(kernel, arg_index)] = arg_size;
+  if (arg_value != NULL) {
+    g_args[make_pair(kernel, arg_index)] = string((char*)arg_value, arg_size);
+  } else {
+    g_args[make_pair(kernel, arg_index)] = string("");
+  }
+  cl_int ret = clSetKernelArg(kernel, arg_index, arg_size, arg_value);
+  return ret;
+}

sunnypilot/modeld/thneed/thneed_pc.cc

@@ -0,0 +1,32 @@
+#include "sunnypilot/modeld/thneed/thneed.h"
+
+#include <cassert>
+
+#include "common/clutil.h"
+#include "common/timing.h"
+
+Thneed::Thneed(bool do_clinit, cl_context _context) {
+  context = _context;
+  if (do_clinit) clinit();
+  char *thneed_debug_env = getenv("THNEED_DEBUG");
+  debug = (thneed_debug_env != NULL) ? atoi(thneed_debug_env) : 0;
+}
+
+void Thneed::execute(float **finputs, float *foutput, bool slow) {
+  uint64_t tb, te;
+  if (debug >= 1) tb = nanos_since_boot();
+
+  // ****** copy inputs
+  copy_inputs(finputs);
+
+  // ****** run commands
+  clexec();
+
+  // ****** copy outputs
+  copy_output(foutput);
+
+  if (debug >= 1) {
+    te = nanos_since_boot();
+    printf("model exec in %lu us\n", (te-tb)/1000);
+  }
+}

sunnypilot/modeld/thneed/thneed_qcom2.cc

@@ -0,0 +1,258 @@
+#include "sunnypilot/modeld/thneed/thneed.h"
+
+#include <dlfcn.h>
+#include <sys/mman.h>
+
+#include <cassert>
+#include <cerrno>
+#include <cstring>
+#include <map>
+#include <string>
+
+#include "common/clutil.h"
+#include "common/timing.h"
+
+Thneed *g_thneed = NULL;
+int g_fd = -1;
+
+void hexdump(uint8_t *d, int len) {
+  assert((len%4) == 0);
+  printf("  dumping %p len 0x%x\n", d, len);
+  for (int i = 0; i < len/4; i++) {
+    if (i != 0 && (i%0x10) == 0) printf("\n");
+    printf("%8x ", d[i]);
+  }
+  printf("\n");
+}
+
+// *********** ioctl interceptor ***********
+
+extern "C" {
+
+int (*my_ioctl)(int filedes, unsigned long request, void *argp) = NULL;
+#undef ioctl
+int ioctl(int filedes, unsigned long request, void *argp) {
+  request &= 0xFFFFFFFF;  // needed on QCOM2
+  if (my_ioctl == NULL) my_ioctl = reinterpret_cast<decltype(my_ioctl)>(dlsym(RTLD_NEXT, "ioctl"));
+  Thneed *thneed = g_thneed;
+
+  // save the fd
+  if (request == IOCTL_KGSL_GPUOBJ_ALLOC) g_fd = filedes;
+
+  // note that this runs always, even without a thneed object
+  if (request == IOCTL_KGSL_DRAWCTXT_CREATE) {
+    struct kgsl_drawctxt_create *create = (struct kgsl_drawctxt_create *)argp;
+    create->flags &= ~KGSL_CONTEXT_PRIORITY_MASK;
+    create->flags |= 6 << KGSL_CONTEXT_PRIORITY_SHIFT;   // priority from 1-15, 1 is max priority
+    printf("IOCTL_KGSL_DRAWCTXT_CREATE: creating context with flags 0x%x\n", create->flags);
+  }
+
+  if (thneed != NULL) {
+    if (request == IOCTL_KGSL_GPU_COMMAND) {
+      struct kgsl_gpu_command *cmd = (struct kgsl_gpu_command *)argp;
+      if (thneed->record) {
+        thneed->timestamp = cmd->timestamp;
+        thneed->context_id = cmd->context_id;
+        thneed->cmds.push_back(unique_ptr<CachedCommand>(new CachedCommand(thneed, cmd)));
+      }
+      if (thneed->debug >= 1) {
+        printf("IOCTL_KGSL_GPU_COMMAND(%2zu): flags: 0x%lx    context_id: %u  timestamp: %u  numcmds: %d  numobjs: %d\n",
+            thneed->cmds.size(),
+            cmd->flags,
+            cmd->context_id, cmd->timestamp, cmd->numcmds, cmd->numobjs);
+      }
+    } else if (request == IOCTL_KGSL_GPUOBJ_SYNC) {
+      struct kgsl_gpuobj_sync *cmd = (struct kgsl_gpuobj_sync *)argp;
+      struct kgsl_gpuobj_sync_obj *objs = (struct kgsl_gpuobj_sync_obj *)(cmd->objs);
+
+      if (thneed->debug >= 2) {
+        printf("IOCTL_KGSL_GPUOBJ_SYNC count:%d ", cmd->count);
+        for (int i = 0; i < cmd->count; i++) {
+          printf(" -- offset:0x%lx len:0x%lx id:%d op:%d  ", objs[i].offset, objs[i].length, objs[i].id, objs[i].op);
+        }
+        printf("\n");
+      }
+
+      if (thneed->record) {
+        thneed->cmds.push_back(unique_ptr<CachedSync>(new
+              CachedSync(thneed, string((char *)objs, sizeof(struct kgsl_gpuobj_sync_obj)*cmd->count))));
+      }
+    } else if (request == IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID) {
+      struct kgsl_device_waittimestamp_ctxtid *cmd = (struct kgsl_device_waittimestamp_ctxtid *)argp;
+      if (thneed->debug >= 1) {
+        printf("IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID: context_id: %d  timestamp: %d  timeout: %d\n",
+            cmd->context_id, cmd->timestamp, cmd->timeout);
+      }
+    } else if (request == IOCTL_KGSL_SETPROPERTY) {
+      if (thneed->debug >= 1) {
+        struct kgsl_device_getproperty *prop = (struct kgsl_device_getproperty *)argp;
+        printf("IOCTL_KGSL_SETPROPERTY: 0x%x sizebytes:%zu\n", prop->type, prop->sizebytes);
+        if (thneed->debug >= 2) {
+          hexdump((uint8_t *)prop->value, prop->sizebytes);
+          if (prop->type == KGSL_PROP_PWR_CONSTRAINT) {
+            struct kgsl_device_constraint *constraint = (struct kgsl_device_constraint *)prop->value;
+            hexdump((uint8_t *)constraint->data, constraint->size);
+          }
+        }
+      }
+    } else if (request == IOCTL_KGSL_DRAWCTXT_CREATE || request == IOCTL_KGSL_DRAWCTXT_DESTROY) {
+      // this happens
+    } else if (request == IOCTL_KGSL_GPUOBJ_ALLOC || request == IOCTL_KGSL_GPUOBJ_FREE) {
+      // this happens
+    } else {
+      if (thneed->debug >= 1) {
+        printf("other ioctl %lx\n", request);
+      }
+    }
+  }
+
+  int ret = my_ioctl(filedes, request, argp);
+  // 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;
+}
+
+}
+
+// *********** GPUMalloc ***********
+
+GPUMalloc::GPUMalloc(int size, int fd) {
+  struct kgsl_gpuobj_alloc alloc;
+  memset(&alloc, 0, sizeof(alloc));
+  alloc.size = size;
+  alloc.flags = 0x10000a00;
+  ioctl(fd, IOCTL_KGSL_GPUOBJ_ALLOC, &alloc);
+  void *addr = mmap64(NULL, alloc.mmapsize, 0x3, 0x1, fd, alloc.id*0x1000);
+  assert(addr != MAP_FAILED);
+
+  base = (uint64_t)addr;
+  remaining = size;
+}
+
+GPUMalloc::~GPUMalloc() {
+  // TODO: free the GPU malloced area
+}
+
+void *GPUMalloc::alloc(int size) {
+  void *ret = (void*)base;
+  size = (size+0xff) & (~0xFF);
+  assert(size <= remaining);
+  remaining -= size;
+  base += size;
+  return ret;
+}
+
+// *********** CachedSync, at the ioctl layer ***********
+
+void CachedSync::exec() {
+  struct kgsl_gpuobj_sync cmd;
+
+  cmd.objs = (uint64_t)data.data();
+  cmd.obj_len = data.length();
+  cmd.count = data.length() / sizeof(struct kgsl_gpuobj_sync_obj);
+
+  int ret = ioctl(thneed->fd, IOCTL_KGSL_GPUOBJ_SYNC, &cmd);
+  assert(ret == 0);
+}
+
+// *********** CachedCommand, at the ioctl layer ***********
+
+CachedCommand::CachedCommand(Thneed *lthneed, struct kgsl_gpu_command *cmd) {
+  thneed = lthneed;
+  assert(cmd->numsyncs == 0);
+
+  memcpy(&cache, cmd, sizeof(cache));
+
+  if (cmd->numcmds > 0) {
+    cmds = make_unique<struct kgsl_command_object[]>(cmd->numcmds);
+    memcpy(cmds.get(), (void *)cmd->cmdlist, sizeof(struct kgsl_command_object)*cmd->numcmds);
+    cache.cmdlist = (uint64_t)cmds.get();
+    for (int i = 0; i < cmd->numcmds; i++) {
+      void *nn = thneed->ram->alloc(cmds[i].size);
+      memcpy(nn, (void*)cmds[i].gpuaddr, cmds[i].size);
+      cmds[i].gpuaddr = (uint64_t)nn;
+    }
+  }
+
+  if (cmd->numobjs > 0) {
+    objs = make_unique<struct kgsl_command_object[]>(cmd->numobjs);
+    memcpy(objs.get(), (void *)cmd->objlist, sizeof(struct kgsl_command_object)*cmd->numobjs);
+    cache.objlist = (uint64_t)objs.get();
+    for (int i = 0; i < cmd->numobjs; i++) {
+      void *nn = thneed->ram->alloc(objs[i].size);
+      memset(nn, 0, objs[i].size);
+      objs[i].gpuaddr = (uint64_t)nn;
+    }
+  }
+
+  kq = thneed->ckq;
+  thneed->ckq.clear();
+}
+
+void CachedCommand::exec() {
+  cache.timestamp = ++thneed->timestamp;
+  int ret = ioctl(thneed->fd, IOCTL_KGSL_GPU_COMMAND, &cache);
+
+  if (thneed->debug >= 1) printf("CachedCommand::exec got %d\n", ret);
+
+  if (thneed->debug >= 2) {
+    for (auto &it : kq) {
+      it->debug_print(false);
+    }
+  }
+
+  assert(ret == 0);
+}
+
+// *********** Thneed ***********
+
+Thneed::Thneed(bool do_clinit, cl_context _context) {
+  // TODO: QCOM2 actually requires a different context
+  //context = _context;
+  if (do_clinit) clinit();
+  assert(g_fd != -1);
+  fd = g_fd;
+  ram = make_unique<GPUMalloc>(0x80000, fd);
+  timestamp = -1;
+  g_thneed = this;
+  char *thneed_debug_env = getenv("THNEED_DEBUG");
+  debug = (thneed_debug_env != NULL) ? atoi(thneed_debug_env) : 0;
+}
+
+void Thneed::wait() {
+  struct kgsl_device_waittimestamp_ctxtid wait;
+  wait.context_id = context_id;
+  wait.timestamp = timestamp;
+  wait.timeout = -1;
+
+  uint64_t tb = nanos_since_boot();
+  int wret = ioctl(fd, IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID, &wait);
+  uint64_t te = nanos_since_boot();
+
+  if (debug >= 1) printf("wait %d after %lu us\n", wret, (te-tb)/1000);
+}
+
+void Thneed::execute(float **finputs, float *foutput, bool slow) {
+  uint64_t tb, te;
+  if (debug >= 1) tb = nanos_since_boot();
+
+  // ****** copy inputs
+  copy_inputs(finputs, true);
+
+  // ****** run commands
+  int i = 0;
+  for (auto &it : cmds) {
+    ++i;
+    if (debug >= 1) printf("run %2d @ %7lu us: ", i, (nanos_since_boot()-tb)/1000);
+    it->exec();
+    if ((i == cmds.size()) || slow) wait();
+  }
+
+  // ****** copy outputs
+  copy_output(foutput);
+
+  if (debug >= 1) {
+    te = nanos_since_boot();
+    printf("model exec in %lu us\n", (te-tb)/1000);
+  }
+}

sunnypilot/modeld/transforms/loadyuv.cc

@@ -0,0 +1,74 @@
+#include "sunnypilot/modeld/transforms/loadyuv.h"
+
+#include <cassert>
+#include <cstdio>
+#include <cstring>
+
+void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height) {
+  memset(s, 0, sizeof(*s));
+
+  s->width = width;
+  s->height = height;
+
+  char args[1024];
+  snprintf(args, sizeof(args),
+           "-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, LOADYUV_PATH, args);
+
+  s->loadys_krnl = CL_CHECK_ERR(clCreateKernel(prg, "loadys", &err));
+  s->loaduv_krnl = CL_CHECK_ERR(clCreateKernel(prg, "loaduv", &err));
+  s->copy_krnl = CL_CHECK_ERR(clCreateKernel(prg, "copy", &err));
+
+  // done with this
+  CL_CHECK(clReleaseProgram(prg));
+}
+
+void loadyuv_destroy(LoadYUVState* s) {
+  CL_CHECK(clReleaseKernel(s->loadys_krnl));
+  CL_CHECK(clReleaseKernel(s->loaduv_krnl));
+  CL_CHECK(clReleaseKernel(s->copy_krnl));
+}
+
+void loadyuv_queue(LoadYUVState* s, cl_command_queue q,
+                   cl_mem y_cl, cl_mem u_cl, cl_mem v_cl,
+                   cl_mem out_cl, bool do_shift) {
+  cl_int global_out_off = 0;
+  if (do_shift) {
+    // shift the image in slot 1 to slot 0, then place the new image in slot 1
+    global_out_off += (s->width*s->height) + (s->width/2)*(s->height/2)*2;
+    CL_CHECK(clSetKernelArg(s->copy_krnl, 0, sizeof(cl_mem), &out_cl));
+    CL_CHECK(clSetKernelArg(s->copy_krnl, 1, sizeof(cl_int), &global_out_off));
+    const size_t copy_work_size = global_out_off/8;
+    CL_CHECK(clEnqueueNDRangeKernel(q, s->copy_krnl, 1, NULL,
+                                &copy_work_size, NULL, 0, 0, NULL));
+  }
+
+  CL_CHECK(clSetKernelArg(s->loadys_krnl, 0, sizeof(cl_mem), &y_cl));
+  CL_CHECK(clSetKernelArg(s->loadys_krnl, 1, sizeof(cl_mem), &out_cl));
+  CL_CHECK(clSetKernelArg(s->loadys_krnl, 2, sizeof(cl_int), &global_out_off));
+
+  const size_t loadys_work_size = (s->width*s->height)/8;
+  CL_CHECK(clEnqueueNDRangeKernel(q, s->loadys_krnl, 1, NULL,
+                               &loadys_work_size, NULL, 0, 0, NULL));
+
+  const size_t loaduv_work_size = ((s->width/2)*(s->height/2))/8;
+  global_out_off += (s->width*s->height);
+
+  CL_CHECK(clSetKernelArg(s->loaduv_krnl, 0, sizeof(cl_mem), &u_cl));
+  CL_CHECK(clSetKernelArg(s->loaduv_krnl, 1, sizeof(cl_mem), &out_cl));
+  CL_CHECK(clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &global_out_off));
+
+  CL_CHECK(clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL,
+                               &loaduv_work_size, NULL, 0, 0, NULL));
+
+  global_out_off += (s->width/2)*(s->height/2);
+
+  CL_CHECK(clSetKernelArg(s->loaduv_krnl, 0, sizeof(cl_mem), &v_cl));
+  CL_CHECK(clSetKernelArg(s->loaduv_krnl, 1, sizeof(cl_mem), &out_cl));
+  CL_CHECK(clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &global_out_off));
+
+  CL_CHECK(clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL,
+                               &loaduv_work_size, NULL, 0, 0, NULL));
+}