maybe it does not need to be complicated ?

Handle missing 'sim_pose' gracefully in model outputs.

Check for the presence of 'sim_pose' in output data before processing. If absent, fallback to using 'plan' data to populate temporal pose fields, ensuring robustness and preventing potential runtime errors.

.github/labeler.yaml

@@ -24,4 +24,4 @@ multilanguage:
 
 autonomy:
   - changed-files:
-    - any-glob-to-all-files: "{selfdrive/modeld/models/**,selfdrive/test/process_replay/model_replay_ref_commit}"
+    - any-glob-to-all-files: "{selfdrive/modeld/models/**,selfdrive/test/process_replay/model_replay_ref_commit,sunnypilot/modeld*/models/**}"

.github/workflows/selfdrive_tests.yaml

@@ -322,7 +322,7 @@ jobs:
       run: |
         ${{ env.RUN }} "source selfdrive/test/setup_xvfb.sh && \
                         source selfdrive/test/setup_vsound.sh && \
-                        CI=1 pytest tools/sim/tests/test_metadrive_bridge.py"
+                        CI=1 pytest -s tools/sim/tests/test_metadrive_bridge.py"
 
   create_ui_report:
     # This job name needs to be the same as UI_JOB_NAME in ui_preview.yaml

.gitignore

@@ -74,9 +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
+sunnypilot/modeld*/thneed/compile
+sunnypilot/modeld*/models/*.thneed
+sunnypilot/modeld*/models/*.pkl
 
 *.bz2
 *.zst

.gitmodules

@@ -1,9 +1,11 @@
 [submodule "panda"]
   path = panda
   url = https://github.com/sunnyhaibin/panda.git
+  branch = tn
 [submodule "opendbc"]
   path = opendbc_repo
   url = https://github.com/sunnypilot/opendbc.git
+  branch = tn
 [submodule "msgq"]
   path = msgq_repo
   url = https://github.com/sunnypilot/msgq.git

Jenkinsfile

@@ -103,7 +103,7 @@ def deviceStage(String stageName, String deviceType, List extra_env, def steps)
             def diffPaths = args.diffPaths ?: []
             def cmdTimeout = args.timeout ?: 9999
 
-            if (branch != "master" && diffPaths && !hasPathChanged(gitDiff, diffPaths)) {
+            if (branch != "master" && !branch.contains("__jenkins_loop_") && diffPaths && !hasPathChanged(gitDiff, diffPaths)) {
               println "Skipping ${name}: no changes in ${diffPaths}."
               return
             } else {
@@ -170,7 +170,7 @@ node {
                          'testing-closet*', 'hotfix-*']
   def excludeRegex = excludeBranches.join('|').replaceAll('\\*', '.*')
 
-  if (env.BRANCH_NAME != 'master') {
+  if (env.BRANCH_NAME != 'master' && !env.BRANCH_NAME.contains('__jenkins_loop_')) {
     properties([
         disableConcurrentBuilds(abortPrevious: true)
     ])

RELEASES.md

@@ -1,23 +1,22 @@
-Version 0.9.9 (2025-03-30)
+Version 0.9.8 (2025-02-27)
 ========================
+* New driving model
+  * Model now gates applying positive acceleration in Chill mode
+* New driving monitoring model
+  * Reduced false positives related to passengers
+* Image processing pipeline moved to the ISP
+  * More GPU time for bigger driving models
+  * Power draw reduced 0.5W, which means your device runs cooler
+* Added toggle to enable driver monitoring even when openpilot is not engaged
+* FIREHOSE mode
+  * Allows you to maximize your training data uploads to improve the models
+* Enable openpilot longitudinal control for Ford Q3 vehicles
+* New Toyota TSS2 longitudinal tune
 * Coming soon
   * Rivian support
   * F-150 & Mach-E support
   * Tesla Model 3 support
 
-Version 0.9.8 (2025-01-30)
-========================
-* New driving monitoring model
-  * Reduced false positives related to passengers
-* Image processing pipeline moved to the ISP
-  * More GPU time for driving models
-  * Power draw reduced 0.5W, which means your device runs cooler
-* Added toggle to enable driver monitoring even when openpilot is not engaged
-* Enable openpilot longitudinal control for Ford Q3 vehicles
-* New Toyota TSS2 longitudinal tune
-* Coming soon
-  * New driving model with gas gating
-  * Training data upload mode
 
 Version 0.9.7 (2024-06-13)
 ========================

SConstruct

@@ -361,7 +361,7 @@ SConscript(['opendbc_repo/SConscript'], exports={'env': env_swaglog})
 SConscript(['cereal/SConscript'])
 
 Import('socketmaster', 'msgq')
-messaging = [socketmaster, msgq, 'zmq', 'capnp', 'kj',]
+messaging = [socketmaster, msgq, 'capnp', 'kj',]
 Export('messaging')
 
 
@@ -373,7 +373,6 @@ SConscript(['rednose/SConscript'])
 
 # Build system services
 SConscript([
-  'system/ui/SConscript',
   'system/proclogd/SConscript',
   'system/ubloxd/SConscript',
   'system/loggerd/SConscript',

cereal/custom.capnp

@@ -70,6 +70,7 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
   enum Runner {
     snpe @0;
     tinygrad @1;
+    stock @2;
   }
 
   struct ModelBundle {
@@ -87,6 +88,7 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
 
 struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   dec @0 :DynamicExperimentalControl;
+  accelPersonality @1 :AccelerationPersonality;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -98,6 +100,13 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
       blended @1;
     }
   }
+
+  enum AccelerationPersonality {
+    sport @0;
+    normal @1;
+    eco @2;
+    stock @3;
+  }
 }
 
 struct OnroadEventSP @0xda96579883444c35 {

cereal/services.py

@@ -29,7 +29,7 @@ _services: dict[str, tuple] = {
   "pandaStates": (True, 10., 1),
   "peripheralState": (True, 2., 1),
   "radarState": (True, 20., 5),
-  "roadEncodeIdx": (False, 20., 1),
+  "roadEncodeIdx": (False, 20., 20),
   "liveTracks": (True, 20.),
   "sendcan": (True, 100., 139),
   "logMessage": (True, 0.),
@@ -53,15 +53,15 @@ _services: dict[str, tuple] = {
   "livePose": (True, 20., 4),
   "liveParameters": (True, 20., 5),
   "cameraOdometry": (True, 20., 10),
-  "thumbnail": (True, 0.2, 1),
+  "thumbnail": (True, 1 / 60., 1),
   "onroadEvents": (True, 1., 1),
   "carParams": (True, 0.02, 1),
   "roadCameraState": (True, 20., 20),
   "driverCameraState": (True, 20., 20),
-  "driverEncodeIdx": (False, 20., 1),
+  "driverEncodeIdx": (False, 20., 20),
   "driverStateV2": (True, 20., 10),
   "driverMonitoringState": (True, 20., 10),
-  "wideRoadEncodeIdx": (False, 20., 1),
+  "wideRoadEncodeIdx": (False, 20., 20),
   "wideRoadCameraState": (True, 20., 20),
   "drivingModelData": (True, 20., 10),
   "modelV2": (True, 20.),

common/file_helpers.py

@@ -1,6 +1,10 @@
+import io
 import os
 import tempfile
 import contextlib
+import zstandard as zstd
+
+LOG_COMPRESSION_LEVEL = 10 # little benefit up to level 15. level ~17 is a small step change
 
 
 class CallbackReader:
@@ -35,3 +39,20 @@ def atomic_write_in_dir(path: str, mode: str = 'w', buffering: int = -1, encodin
     yield tmp_file
     tmp_file_name = tmp_file.name
   os.replace(tmp_file_name, path)
+
+
+def get_upload_stream(filepath: str, should_compress: bool) -> tuple[io.BufferedIOBase, int]:
+  if not should_compress:
+    file_size = os.path.getsize(filepath)
+    file_stream = open(filepath, "rb")
+    return file_stream, file_size
+
+  # Compress the file on the fly
+  compressed_stream = io.BytesIO()
+  compressor = zstd.ZstdCompressor(level=LOG_COMPRESSION_LEVEL)
+
+  with open(filepath, "rb") as f:
+    compressor.copy_stream(f, compressed_stream)
+    compressed_size = compressed_stream.tell()
+    compressed_stream.seek(0)
+    return compressed_stream, compressed_size

common/model.h

@@ -1 +1 @@
-#define DEFAULT_MODEL "Notre Dame (Default)"
+#define DEFAULT_MODEL "Not Too Shabby (Default)"

common/params.cc

@@ -120,6 +120,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"ExperimentalLongitudinalEnabled", PERSISTENT | DEVELOPMENT_ONLY | BACKUP},
     {"ExperimentalMode", PERSISTENT | BACKUP},
     {"ExperimentalModeConfirmed", PERSISTENT | BACKUP},
+    {"FirehoseMode", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"FirmwareQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"ForcePowerDown", PERSISTENT},
     {"GitBranch", PERSISTENT},
@@ -238,6 +239,13 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"HyundaiRadarTracksToggle", PERSISTENT},
 
     {"DynamicExperimentalControl", PERSISTENT},
+    {"ToyotaAutoHold", PERSISTENT},
+    {"ToyotaEnhancedBsm", PERSISTENT},
+    {"ToyotaTSS2Long", PERSISTENT},
+    {"FastTakeOff", PERSISTENT},
+    {"AccelPersonality", PERSISTENT},
+    {"ToyotaDriveMode", PERSISTENT},
+    {"RainbowMode", PERSISTENT},
 };
 
 } // namespace

common/realtime.py

@@ -27,11 +27,6 @@ class Priority:
   CTRL_HIGH = 53
 
 
-def set_realtime_priority(level: int) -> None:
-  if not PC:
-    os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level))
-
-
 def set_core_affinity(cores: list[int]) -> None:
   if not PC:
     os.sched_setaffinity(0, cores)
@@ -39,7 +34,8 @@ def set_core_affinity(cores: list[int]) -> None:
 
 def config_realtime_process(cores: int | list[int], priority: int) -> None:
   gc.disable()
-  set_realtime_priority(priority)
+  if not PC:
+    os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(priority))
   c = cores if isinstance(cores, list) else [cores, ]
   set_core_affinity(c)
 

conftest.py

@@ -19,6 +19,7 @@ collect_ignore = [
 collect_ignore_glob = [
   "selfdrive/debug/*.py",
   "selfdrive/modeld/*.py",
+  "sunnypilot/modeld*/*.py",
 ]
 
 

docs/CARS.md

@@ -184,7 +184,7 @@ A supported vehicle is one that just works when you install a comma device. All
 |Lexus|RX Hybrid 2016|Lexus Safety System+|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=RX Hybrid 2016">Buy Here</a></sub></details>||
 |Lexus|RX Hybrid 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=RX Hybrid 2017-19">Buy Here</a></sub></details>||
 |Lexus|RX Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=RX Hybrid 2020-22">Buy Here</a></sub></details>||
-|Lexus|UX Hybrid 2019-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=UX Hybrid 2019-23">Buy Here</a></sub></details>||
+|Lexus|UX Hybrid 2019-24|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 RJ45 cable (7 ft)<br>- 1 Toyota A connector<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lexus&model=UX Hybrid 2019-24">Buy Here</a></sub></details>||
 |Lincoln|Aviator 2020-24|Co-Pilot360 Plus|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lincoln&model=Aviator 2020-24">Buy Here</a></sub></details>||
 |Lincoln|Aviator Plug-in Hybrid 2020-24|Co-Pilot360 Plus|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Lincoln&model=Aviator Plug-in Hybrid 2020-24">Buy Here</a></sub></details>||
 |MAN|eTGE 2020-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,12</sup>](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 angled mount (8 degrees)<br>- 1 comma 3X<br>- 1 harness box<br>- 1 long OBD-C cable<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=MAN&model=eTGE 2020-24">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|

launch_env.sh

@@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1
 export VECLIB_MAXIMUM_THREADS=1
 
 if [ -z "$AGNOS_VERSION" ]; then
-  export AGNOS_VERSION="11.6"
+  export AGNOS_VERSION="11.8"
 fi
 
 export STAGING_ROOT="/data/safe_staging"

pyproject.toml

@@ -111,6 +111,7 @@ dev = [
   "tabulate",
   "types-requests",
   "types-tabulate",
+  "raylib",
 ]
 
 tools = [

release/build_release.sh

@@ -75,6 +75,7 @@ find . -name 'moc_*' -delete
 find . -name '__pycache__' -delete
 rm -rf .sconsign.dblite Jenkinsfile release/
 rm selfdrive/modeld/models/supercombo.onnx
+rm sunnypilot/modeld*/models/supercombo.onnx
 
 find third_party/ -name '*x86*' -exec rm -r {} +
 find third_party/ -name '*Darwin*' -exec rm -r {} +

release/ci/install_github_runner.sh

@@ -67,8 +67,6 @@ 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
-    sudo -u ${RUNNER_USER} bash -c "truncate -s 0 '${BASE_DIR}/.bash_logout'"
 }
 
 create_sudoers_entry() {
@@ -77,8 +75,8 @@ create_sudoers_entry() {
 
 set_directory_permissions() {
     sudo chown -R ${RUNNER_USER}:comma "$BASE_DIR"
-    sudo chmod g+rwx "$BASE_DIR"
-    sudo chmod g+s "$BASE_DIR"
+    sudo chmod -R g+rwx "$BASE_DIR"
+    sudo find "$BASE_DIR" -type d -exec chmod g+s {} +
 }
 
 setup_directories() {
@@ -86,30 +84,42 @@ setup_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"
+    sync
+}
+
+wipe_bash_logout() {
+  export BASE_DIR
+  sudo -u ${RUNNER_USER} bash -c "touch ${BASE_DIR}/.bash_logout"
+  sudo -u ${RUNNER_USER} bash -c "truncate -s 0 '${BASE_DIR}/.bash_logout'"
 }
 
 # System configuration functions (depends on basic utility functions)
 setup_system_configs() {
     echo "Setting up system configurations..."
+    remount_rw
     setup_runner_user
     create_sudoers_entry
+    remount_ro
     set_directory_permissions
+    wipe_bash_logout
 }
 
 # 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
+    curl -o actions-runner-linux-arm64-2.322.0.tar.gz -L https://github.com/actions/runner/releases/download/v2.322.0/actions-runner-linux-arm64-2.322.0.tar.gz
+    sudo -u ${RUNNER_USER} tar -xzf ./actions-runner-linux-arm64-2.322.0.tar.gz
+    sudo rm ./actions-runner-linux-arm64-2.322.0.tar.gz
+    sudo chmod +x ./config.sh
 }
 
 configure_runner() {
+    remount_rw
     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
+    remount_ro
 }
 
 create_service_template() {
@@ -139,6 +149,7 @@ EOL
 }
 
 install_service() {
+    remount_rw
     echo "Installing systemd service..."
     cd "$RUNNER_DIR"
     sudo ./svc.sh install $RUNNER_USER
@@ -152,6 +163,7 @@ install_service() {
         fi
         sudo systemctl disable "${service_name}"
     fi
+    remount_ro
 }
 
 check_restore_prerequisites() {
@@ -203,23 +215,20 @@ check_restore_prerequisites() {
 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
+    install_runner
+    set_directory_permissions
+    create_service_template
     configure_runner
     install_service
-    remount_ro
     echo "Installation completed successfully"
 }
 

scripts/jenkins_loop_test.sh

@@ -0,0 +1,145 @@
+#!/usr/bin/env bash
+set -e
+
+YELLOW='\033[0;33m'
+GREEN='\033[0;32m'
+UNDERLINE='\033[4m'
+BOLD='\033[1m'
+NC='\033[0m'
+
+BRANCH="master"
+RUNS="20"
+
+COOKIE_JAR=/tmp/cookies
+CRUMB=$(curl -s --cookie-jar $COOKIE_JAR 'https://jenkins.comma.life/crumbIssuer/api/xml?xpath=concat(//crumbRequestField,":",//crumb)')
+
+function loop() {
+  JENKINS_BRANCH="__jenkins_loop_${BRANCH}"
+  API_ROUTE="https://jenkins.comma.life/job/openpilot/job/$JENKINS_BRANCH"
+
+  for run in $(seq 1 $((RUNS / 2))); do
+
+    N=2
+    TEST_BUILDS=()
+
+    # Try to find previous builds
+    ALL_BUILDS=( $(curl -s $API_ROUTE/api/json | jq .builds.[].number 2> /dev/null || :) )
+
+    # No builds. Create branch
+    if [[ ${#ALL_BUILDS[@]} -eq 0 ]]; then
+      TEMP_DIR=$(mktemp -d)
+      GIT_LFS_SKIP_SMUDGE=1 git clone --quiet -b $BRANCH --depth=1 --no-tags git@github.com:commaai/openpilot $TEMP_DIR
+      git -C $TEMP_DIR checkout --quiet -b $JENKINS_BRANCH
+      echo "TESTING" >> $TEMP_DIR/testing_jenkins
+      git -C $TEMP_DIR add testing_jenkins
+      git -C $TEMP_DIR commit --quiet -m "testing"
+      git -C $TEMP_DIR push --quiet -f origin $JENKINS_BRANCH
+      rm -rf $TEMP_DIR
+      FIRST_BUILD=1
+      echo ''
+      echo 'waiting on Jenkins...'
+      echo ''
+      sleep 90
+    else
+      # Found some builds. Wait for them to end if they are still running
+      for i in ${ALL_BUILDS[@]}; do
+        running=$(curl -s $API_ROUTE/$i/api/json/ | jq .inProgress)
+        if [[ $running == "false" ]]; then
+          continue
+        fi
+        TEST_BUILDS=( ${ALL_BUILDS[@]} )
+        N=${#TEST_BUILDS[@]}
+        break
+      done
+    fi
+
+    # No running builds found
+    if [[ ${#TEST_BUILDS[@]} -eq 0 ]]; then
+      FIRST_BUILD=$(curl -s $API_ROUTE/api/json | jq .nextBuildNumber)
+      LAST_BUILD=$((FIRST_BUILD+N-1))
+      TEST_BUILDS=( $(seq $FIRST_BUILD $LAST_BUILD) )
+
+      # Start N new builds
+      for i in ${TEST_BUILDS[@]};
+      do
+        echo "Starting build $i"
+        curl -s --output /dev/null --cookie $COOKIE_JAR -H "$CRUMB" -X POST $API_ROUTE/build?delay=0sec
+        sleep 5
+      done
+      echo ""
+    fi
+
+    # Wait for all builds to end
+    while true; do
+      sleep 30
+
+      count=0
+      for i in ${TEST_BUILDS[@]};
+      do
+        RES=$(curl -s -w "\n%{http_code}" --cookie $COOKIE_JAR -H "$CRUMB" $API_ROUTE/$i/api/json)
+        HTTP_CODE=$(tail -n1 <<< "$RES")
+        JSON=$(sed '$ d' <<< "$RES")
+
+        if [[ $HTTP_CODE == "200" ]]; then
+          STILL_RUNNING=$(echo $JSON | jq .inProgress)
+          if [[ $STILL_RUNNING == "true" ]]; then
+            echo -e "Build $i: ${YELLOW}still running${NC}"
+            continue
+          else
+            count=$((count+1))
+            echo -e "Build $i: ${GREEN}done${NC}"
+          fi
+        else
+          echo "No status for build $i"
+        fi
+      done
+      echo "See live results: ${API_ROUTE}/buildTimeTrend"
+      echo ""
+
+      if [[ $count -ge $N ]]; then
+        break
+      fi
+    done
+
+  done
+}
+
+function usage() {
+  echo ""
+  echo "Run the Jenkins tests multiple times on a specific branch"
+  echo ""
+  echo -e "${BOLD}${UNDERLINE}Options:${NC}"
+  echo -e "  ${BOLD}-n, --n${NC}"
+  echo -e "          Specify how many runs to do (default to ${BOLD}20${NC})"
+  echo -e "  ${BOLD}-b, --branch${NC}"
+  echo -e "          Specify which branch to run the tests against (default to ${BOLD}master${NC})"
+  echo ""
+}
+
+function _looper() {
+  if [[ $# -eq 0 ]]; then
+    usage
+    exit 0
+  fi
+
+  # parse Options
+  while [[ $# -gt 0 ]]; do
+    case $1 in
+      -n | --n ) shift 1; RUNS="$1"; shift 1 ;;
+      -b | --b | --branch | -branch ) shift 1; BRANCH="$1"; shift 1 ;;
+      * ) usage; exit 0 ;;
+    esac
+  done
+
+  echo ""
+  echo -e "You are about to start $RUNS Jenkins builds against the $BRANCH branch."
+  echo -e "If you expect this to run overnight, ${UNDERLINE}${BOLD}unplug the cold reboot power switch${NC} from the testing closet before."
+  echo ""
+  read -p "Press (y/Y) to confirm: " choice
+  if [[ "$choice" == "y" || "$choice" == "Y" ]]; then
+    loop
+  fi
+
+}
+
+_looper $@

scripts/lint/lint.sh

@@ -53,7 +53,6 @@ function run_tests() {
   run "check_shebang_scripts_are_executable" python3 -m pre_commit_hooks.check_shebang_scripts_are_executable $ALL_FILES
   run "check_shebang_format" $DIR/check_shebang_format.sh $ALL_FILES
   run "check_nomerge_comments" $DIR/check_nomerge_comments.sh $ALL_FILES
-  run "check_raylib_includes" $DIR/check_raylib_includes.sh $ALL_FILES
 
   if [[ -z "$FAST" ]]; then
     run "mypy" mypy $PYTHON_FILES

selfdrive/car/car_specific.py

@@ -45,9 +45,6 @@ class CarSpecificEvents:
     if self.CP.brand in ('body', 'mock'):
       events = Events()
 
-    elif self.CP.brand in ('subaru', 'mazda'):
-      events = self.create_common_events(CS, CS_prev)
-
     elif self.CP.brand == 'ford':
       events = self.create_common_events(CS, CS_prev, extra_gears=[GearShifter.manumatic])
 
@@ -102,13 +99,9 @@ class CarSpecificEvents:
             events.add(EventName.manualRestart)
 
     elif self.CP.brand == 'gm':
-      # The ECM allows enabling on falling edge of set, but only rising edge of resume
       events = self.create_common_events(CS, CS_prev, extra_gears=[GearShifter.sport, GearShifter.low,
                                                                    GearShifter.eco, GearShifter.manumatic],
-                                         pcm_enable=self.CP.pcmCruise, enable_buttons=(ButtonType.decelCruise,))
-      if not self.CP.pcmCruise:
-        if any(b.type == ButtonType.accelCruise and b.pressed for b in CS.buttonEvents):
-          events.add(EventName.buttonEnable)
+                                         pcm_enable=self.CP.pcmCruise)
 
       # Enabling at a standstill with brake is allowed
       # TODO: verify 17 Volt can enable for the first time at a stop and allow for all GMs
@@ -122,8 +115,7 @@ class CarSpecificEvents:
 
     elif self.CP.brand == 'volkswagen':
       events = self.create_common_events(CS, CS_prev, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic],
-                                         pcm_enable=self.CP.pcmCruise,
-                                         enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))
+                                         pcm_enable=self.CP.pcmCruise)
 
       # Low speed steer alert hysteresis logic
       if (self.CP.minSteerSpeed - 1e-3) > VWCarControllerParams.DEFAULT_MIN_STEER_SPEED and CS.vEgo < (self.CP.minSteerSpeed + 1.):
@@ -160,12 +152,12 @@ class CarSpecificEvents:
         events.add(EventName.belowSteerSpeed)
 
     else:
-      raise ValueError(f"Unsupported car: {self.CP.brand}")
+      events = self.create_common_events(CS, CS_prev)
 
     return events
 
   def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState, extra_gears=None, pcm_enable=True,
-                           allow_enable=True, allow_button_cancel=True, enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)):
+                           allow_enable=True, allow_button_cancel=True):
     events = Events()
 
     if CS.doorOpen:
@@ -209,12 +201,11 @@ class CarSpecificEvents:
       events.add(EventName.invalidLkasSetting)
     if CS.lowSpeedAlert:
       events.add(EventName.belowSteerSpeed)
+    if CS.buttonEnable:
+      events.add(EventName.buttonEnable)
 
-    # Handle button presses
+    # Handle cancel button presses
     for b in CS.buttonEvents:
-      # Enable OP long on falling edge of enable buttons (defaults to accelCruise and decelCruise, overridable per-port)
-      if not self.CP.pcmCruise and (b.type in enable_buttons and not b.pressed):
-        events.add(EventName.buttonEnable)
       # Disable on rising and falling edge of cancel for both stock and OP long
       # TODO: only check the cancel button with openpilot longitudinal on all brands to match panda safety
       if b.type == ButtonType.cancel and (allow_button_cancel or not self.CP.pcmCruise):

selfdrive/car/card.py

@@ -8,17 +8,17 @@ import cereal.messaging as messaging
 
 from cereal import car, log, custom
 
-from panda import ALTERNATIVE_EXPERIENCE
-
 from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper
 from openpilot.common.swaglog import cloudlog, ForwardingHandler
 
-from opendbc.car import DT_CTRL, carlog, structs
+from opendbc.car import DT_CTRL, structs
 from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable
+from opendbc.car.carlog import carlog
 from opendbc.car.fw_versions import ObdCallback
 from opendbc.car.car_helpers import get_car, get_radar_interface
 from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase
+from opendbc.safety import ALTERNATIVE_EXPERIENCE
 from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
 from openpilot.selfdrive.car.cruise import VCruiseHelper
 from openpilot.selfdrive.car.car_specific import MockCarState
@@ -121,9 +121,13 @@ class Car:
 
     # set alternative experiences from parameters
     disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
+    sp_toyota_auto_brake_hold = self.params.get_bool("ToyotaAutoHold")
     self.CP.alternativeExperience = 0
     if not disengage_on_accelerator:
       self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
+    if sp_toyota_auto_brake_hold:
+      self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB
+
 
     # mads
     MadsParams().set_alternative_experience(self.CP)

selfdrive/car/tests/test_models.py

@@ -1,3 +1,4 @@
+import time
 import capnp
 import copy
 import os
@@ -10,17 +11,15 @@ import hypothesis.strategies as st
 from hypothesis import Phase, given, settings
 from parameterized import parameterized_class
 
-from cereal import messaging, log, car
-from openpilot.common.basedir import BASEDIR
-from openpilot.common.params import Params
 from opendbc.car import DT_CTRL, gen_empty_fingerprint, structs
-from opendbc.car.fingerprints import all_known_cars, MIGRATION
+from opendbc.car.can_definitions import CanData
 from opendbc.car.car_helpers import FRAME_FINGERPRINT, interfaces
+from opendbc.car.fingerprints import all_known_cars, MIGRATION
 from opendbc.car.honda.values import CAR as HONDA, HondaFlags
-from opendbc.car.values import Platform
+from opendbc.car.structs import car
 from opendbc.car.tests.routes import non_tested_cars, routes, CarTestRoute
-from openpilot.selfdrive.selfdrived.events import ET
-from openpilot.selfdrive.selfdrived.selfdrived import SelfdriveD
+from opendbc.car.values import Platform
+from openpilot.common.basedir import BASEDIR
 from openpilot.selfdrive.pandad import can_capnp_to_list
 from openpilot.selfdrive.test.helpers import read_segment_list
 from openpilot.system.hardware.hw import DEFAULT_DOWNLOAD_CACHE_ROOT
@@ -30,8 +29,6 @@ from openpilot.tools.lib.route import SegmentName
 
 from panda.tests.libpanda import libpanda_py
 
-EventName = log.OnroadEvent.EventName
-PandaType = log.PandaState.PandaType
 SafetyModel = car.CarParams.SafetyModel
 
 NUM_JOBS = int(os.environ.get("NUM_JOBS", "1"))
@@ -174,8 +171,6 @@ class TestCarModelBase(unittest.TestCase):
     self.CI = self.CarInterface(self.CP.copy(), copy.deepcopy(self.CP_SP), self.CarController, self.CarState)
     assert self.CI
 
-    Params().put_bool("OpenpilotEnabledToggle", self.openpilot_enabled)
-
     # TODO: check safetyModel is in release panda build
     self.safety = libpanda_py.libpanda
 
@@ -344,10 +339,8 @@ class TestCarModelBase(unittest.TestCase):
       to_send = libpanda_py.make_CANPacket(address, bus, dat)
       self.safety.safety_rx_hook(to_send)
 
-      can = messaging.new_message('can', 1)
-      can.can = [log.CanData(address=address, dat=dat, src=bus)]
-
-      CS = self.CI.update(can_capnp_to_list((can.to_bytes(),)))
+      can = [(int(time.monotonic() * 1e9), [CanData(address=address, dat=dat, src=bus)])]
+      CS = self.CI.update(can)
 
       if self.safety.get_gas_pressed_prev() != prev_panda_gas:
         self.assertEqual(CS.gasPressed, self.safety.get_gas_pressed_prev())
@@ -392,8 +385,6 @@ class TestCarModelBase(unittest.TestCase):
     controls_allowed_prev = False
     CS_prev = car.CarState.new_message()
     checks = defaultdict(int)
-    selfdrived = SelfdriveD(CP=self.CP, CP_SP=self.CP_SP)
-    selfdrived.initialized = True
     for idx, can in enumerate(self.can_msgs):
       CS = self.CI.update(can_capnp_to_list((can.as_builder().to_bytes(), ))).as_reader()
       for msg in filter(lambda m: m.src in range(64), can.can):
@@ -437,11 +428,8 @@ class TestCarModelBase(unittest.TestCase):
         if not self.CP.notCar:
           checks['cruiseState'] += CS.cruiseState.enabled != self.safety.get_cruise_engaged_prev()
       else:
-        # Check for enable events on rising edge of controls allowed
-        selfdrived.update_events(CS)
-        selfdrived.CS_prev = CS
-        button_enable = (selfdrived.events.contains(ET.ENABLE) and
-                         EventName.pedalPressed not in selfdrived.events.names)
+        # Check for user button enable on rising edge of controls allowed
+        button_enable = CS.buttonEnable and (not CS.brakePressed or CS.standstill)
         mismatch = button_enable != (self.safety.get_controls_allowed() and not controls_allowed_prev)
         checks['controlsAllowed'] += mismatch
         controls_allowed_prev = self.safety.get_controls_allowed()

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -3,12 +3,14 @@ import os
 import time
 import numpy as np
 from cereal import log
-from opendbc.car.interfaces import ACCEL_MIN
+from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.common.realtime import DT_MDL
 from openpilot.common.swaglog import cloudlog
 # WARNING: imports outside of constants will not trigger a rebuild
 from openpilot.selfdrive.modeld.constants import index_function
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
+from openpilot.common.conversions import Conversions as CV
+
 
 if __name__ == '__main__':  # generating code
   from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
@@ -55,6 +57,8 @@ FCW_IDXS = T_IDXS < 5.0
 T_DIFFS = np.diff(T_IDXS, prepend=[0.])
 COMFORT_BRAKE = 2.5
 STOP_DISTANCE = 6.0
+CRUISE_MIN_ACCEL = -1.2
+CRUISE_MAX_ACCEL = 1.6
 
 def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
   if personality==log.LongitudinalPersonality.relaxed:
@@ -62,24 +66,48 @@ def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
   elif personality==log.LongitudinalPersonality.standard:
     return 1.0
   elif personality==log.LongitudinalPersonality.aggressive:
-    return 0.5
+    return 0.3
   else:
     raise NotImplementedError("Longitudinal personality not supported")
 
 
 def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard):
   if personality==log.LongitudinalPersonality.relaxed:
-    return 1.75
+    return 1.80
   elif personality==log.LongitudinalPersonality.standard:
-    return 1.45
-  elif personality==log.LongitudinalPersonality.aggressive:
     return 1.25
+  elif personality==log.LongitudinalPersonality.aggressive:
+    return 1.10
   else:
     raise NotImplementedError("Longitudinal personality not supported")
 
 def get_stopped_equivalence_factor(v_lead):
   return (v_lead**2) / (2 * COMFORT_BRAKE)
 
+def get_stopped_equivalence_factor_krkeegen(v_lead, v_ego):
+  v_diff_offset = 0
+  v_diff_offset_max = 12
+  speed_to_reach_max_v_diff_offset = 26 * CV.KPH_TO_MS  # in m/s
+  delta_speed = v_lead - v_ego
+
+  if np.any(delta_speed > 0):
+    # Scale v_diff_offset with a hybrid approach: linear with a smooth transition
+    v_diff_offset = np.clip(delta_speed * 1.5, 0, v_diff_offset_max)
+    scaling_factor = np.clip((speed_to_reach_max_v_diff_offset - v_ego) / speed_to_reach_max_v_diff_offset, 0, 1)
+    # Apply a stronger decay at higher speeds to avoid pulling too close
+    smooth_scaling = scaling_factor ** 3 * (10 - 9 * scaling_factor)
+    v_diff_offset *= smooth_scaling
+
+  stopping_distance = (v_lead ** 2) / (2 * COMFORT_BRAKE) + v_diff_offset
+  return stopping_distance
+
+
+
+
+
+
+
+
 def get_safe_obstacle_distance(v_ego, t_follow):
   return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE
 
@@ -281,7 +309,7 @@ class LongitudinalMpc:
     elif self.mode == 'blended':
       a_change_cost = 40.0 if prev_accel_constraint else 0
       cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0]
-      constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0]
+      constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
     else:
       raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set')
     self.set_cost_weights(cost_weights, constraint_cost_weights)
@@ -325,13 +353,7 @@ class LongitudinalMpc:
     lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
     return lead_xv
 
-  def set_accel_limits(self, min_a, max_a):
-    # TODO this sets a max accel limit, but the minimum limit is only for cruise decel
-    # needs refactor
-    self.cruise_min_a = min_a
-    self.max_a = max_a
-
-  def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard):
+  def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard, fast_take_off = False):
     t_follow = get_T_FOLLOW(personality)
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
@@ -342,12 +364,16 @@ class LongitudinalMpc:
     # To estimate a safe distance from a moving lead, we calculate how much stopping
     # distance that lead needs as a minimum. We can add that to the current distance
     # and then treat that as a stopped car/obstacle at this new distance.
-    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
-    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
+    if fast_take_off:
+      lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor_krkeegen(lead_xv_0[:,1], v_ego)
+      lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor_krkeegen(lead_xv_1[:,1], v_ego)
+    else:
+      lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
+      lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
+
 
     self.params[:,0] = ACCEL_MIN
-    # negative accel constraint causes problems because negative speed is not allowed
-    self.params[:,1] = max(0.0, self.max_a)
+    self.params[:,1] = ACCEL_MAX
 
     # Update in ACC mode or ACC/e2e blend
     if self.mode == 'acc':
@@ -355,9 +381,9 @@ class LongitudinalMpc:
 
       # Fake an obstacle for cruise, this ensures smooth acceleration to set speed
       # when the leads are no factor.
-      v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05)
+      v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05)
       # TODO does this make sense when max_a is negative?
-      v_upper = v_ego + (T_IDXS * self.max_a * 1.05)
+      v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05)
       v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
                                  v_lower,
                                  v_upper)

selfdrive/controls/lib/longitudinal_planner.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 import math
 import numpy as np
-
+from openpilot.common.params import Params
 import cereal.messaging as messaging
 from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.common.conversions import Conversions as CV
@@ -18,7 +18,6 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP
 
 LON_MPC_STEP = 0.2  # first step is 0.2s
-A_CRUISE_MIN = -1.2
 A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
 A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
@@ -79,13 +78,29 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
     self.a_desired = init_a
     self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
+    self.prev_accel_clip = [ACCEL_MIN, ACCEL_MAX]
     self.v_model_error = 0.0
+    self.output_a_target = 0.0
+    self.output_should_stop = False
 
     self.v_desired_trajectory = np.zeros(CONTROL_N)
     self.a_desired_trajectory = np.zeros(CONTROL_N)
     self.j_desired_trajectory = np.zeros(CONTROL_N)
     self.solverExecutionTime = 0.0
 
+    self.params = Params()
+    self.param_read_counter = 0
+    self.read_param()
+
+    self.fast_take_off = False
+
+
+  def read_param(self):
+    try:
+      self.fast_take_off = self.params.get_bool("FastTakeOff")
+    except AttributeError:
+      pass
+
   @staticmethod
   def parse_model(model_msg, model_error):
     if (len(model_msg.position.x) == ModelConstants.IDX_N and
@@ -108,6 +123,9 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
   def update(self, sm):
     LongitudinalPlannerSP.update(self, sm)
+    if self.param_read_counter % 50 == 0:
+      self.read_param()
+    self.param_read_counter += 1
     self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
     if dec_mpc_mode := self.get_mpc_mode():
       self.mpc.mode = dec_mpc_mode
@@ -134,17 +152,45 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     prev_accel_constraint = not (reset_state or sm['carState'].standstill)
 
     if self.mpc.mode == 'acc':
-      accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
+      accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
       steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
-      accel_limits_turns = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_limits, self.CP)
+      accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
     else:
-      accel_limits = [ACCEL_MIN, ACCEL_MAX]
-      accel_limits_turns = [ACCEL_MIN, ACCEL_MAX]
+      accel_clip = [ACCEL_MIN, ACCEL_MAX]
+
+    # Override accel using Accel Controller if enabled
+    if self.accel_controller.is_enabled:
+      max_limit = self.accel_controller.get_accel_limits(v_ego, accel_clip)
+
+      # Ensure max_limit is a single float value
+      if isinstance(max_limit, list):
+        max_limit = max_limit[1]
+
+      # If needed, ensure braking is allowed
+      # if not self.allow_throttle:
+      #     max_limit = min(max_limit, -3.5)  # Ensure braking is allowed if needed
+      #     print(f"allow_throttle={self.allow_throttle}, max_limit before={max_limit:.2f}")
+
+      print(f"Accel Controller: max_limit={max_limit:.2f}")
+
+      if self.mpc.mode == 'acc':
+        # Use the accel controller limits directly
+        accel_clip = [ACCEL_MIN, max_limit]
+        # Recalculate limit turn according to the new max limit
+        steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
+        accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
+        print(f"ACC Mode Final: v_ego={v_ego:.2f}, accel_clip={accel_clip}")
+      else:
+        print(f"Blended Mode (Accel Controller Enabled): accel_clip={accel_clip}")
+    else:
+      print(f"Accel Controller Disabled: accel_clip={accel_clip}")
+
+
 
     if reset_state:
       self.v_desired_filter.x = v_ego
       # Clip aEgo to cruise limits to prevent large accelerations when becoming active
-      self.a_desired = np.clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
+      self.a_desired = np.clip(sm['carState'].aEgo, accel_clip[0], accel_clip[1])
 
     # Prevent divergence, smooth in current v_ego
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
@@ -155,20 +201,19 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
 
     if not self.allow_throttle:
-      clipped_accel_coast = max(accel_coast, accel_limits_turns[0])
-      clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast])
-      accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast_interp)
+      clipped_accel_coast = max(accel_coast, accel_clip[0])
+      clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast])
+      accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp)
 
     if force_slow_decel:
       v_cruise = 0.0
-    # clip limits, cannot init MPC outside of bounds
-    accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
-    accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
 
     self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
-    self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
-    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality)
+    #print("Fast take off status:", self.fast_take_off)
+    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality, fast_take_off=self.fast_take_off)
+
+
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
@@ -184,6 +229,15 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
     self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
 
+    action_t =  self.CP.longitudinalActuatorDelay + DT_MDL
+    output_a_target, self.output_should_stop = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory,
+                                                                        action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
+
+    for idx in range(2):
+      accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)
+    self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1])
+    self.prev_accel_clip = accel_clip
+
   def publish(self, sm, pm):
     plan_send = messaging.new_message('longitudinalPlan')
 
@@ -202,11 +256,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     longitudinalPlan.longitudinalPlanSource = self.mpc.source
     longitudinalPlan.fcw = self.fcw
 
-    action_t =  self.CP.longitudinalActuatorDelay + DT_MDL
-    a_target, should_stop = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels,
-                                                action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
-    longitudinalPlan.aTarget = float(a_target)
-    longitudinalPlan.shouldStop = bool(should_stop)
+    longitudinalPlan.aTarget = float(self.output_a_target)
+    longitudinalPlan.shouldStop = bool(self.output_should_stop)
     longitudinalPlan.allowBrake = True
     longitudinalPlan.allowThrottle = bool(self.allow_throttle)
 

selfdrive/controls/radard.py

@@ -150,7 +150,7 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa
     "vRel": float(lead_v_rel_pred),
     "vLead": float(v_ego + lead_v_rel_pred),
     "vLeadK": float(v_ego + lead_v_rel_pred),
-    "aLeadK": 0.0,
+    "aLeadK": float(lead_msg.a[0]),
     "aLeadTau": 0.3,
     "fcw": False,
     "modelProb": float(lead_msg.prob),

selfdrive/debug/car/disable_ecu.py

@@ -11,5 +11,5 @@ if __name__ == "__main__":
   time.sleep(1)
 
   # honda bosch radar disable
-  disabled = disable_ecu(*can_callbacks, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03', timeout=0.5, debug=False)
+  disabled = disable_ecu(*can_callbacks, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03', timeout=0.5)
   print(f"disabled: {disabled}")

selfdrive/debug/car/ecu_addrs.py

@@ -2,6 +2,7 @@
 import argparse
 import time
 import cereal.messaging as messaging
+from opendbc.car.carlog import carlog
 from opendbc.car.ecu_addrs import get_all_ecu_addrs
 from openpilot.common.params import Params
 from openpilot.selfdrive.car.card import can_comm_callbacks, obd_callback
@@ -15,6 +16,9 @@ if __name__ == "__main__":
   parser.add_argument('--timeout', type=float, default=1.0)
   args = parser.parse_args()
 
+  if args.debug:
+    carlog.setLevel('DEBUG')
+
   logcan = messaging.sub_sock('can')
   sendcan = messaging.pub_sock('sendcan')
   can_callbacks = can_comm_callbacks(logcan, sendcan)
@@ -29,7 +33,7 @@ if __name__ == "__main__":
   obd_callback(params)(not args.no_obd)
 
   print("Getting ECU addresses ...")
-  ecu_addrs = get_all_ecu_addrs(*can_callbacks, args.bus, args.timeout, debug=args.debug)
+  ecu_addrs = get_all_ecu_addrs(*can_callbacks, args.bus, args.timeout)
 
   print()
   print("Found ECUs on rx addresses:")

selfdrive/debug/car/fw_versions.py

@@ -3,6 +3,7 @@ import time
 import argparse
 import cereal.messaging as messaging
 from cereal import car
+from opendbc.car.carlog import carlog
 from opendbc.car.fw_versions import get_fw_versions, match_fw_to_car
 from opendbc.car.vin import get_vin
 from openpilot.common.params import Params
@@ -18,6 +19,9 @@ if __name__ == "__main__":
   parser.add_argument('--brand', help='Only query addresses/with requests for this brand')
   args = parser.parse_args()
 
+  if args.debug:
+    carlog.setLevel('DEBUG')
+
   logcan = messaging.sub_sock('can')
   pandaStates_sock = messaging.sub_sock('pandaStates')
   sendcan = messaging.pub_sock('sendcan')
@@ -46,13 +50,13 @@ if __name__ == "__main__":
   t = time.time()
   print("Getting vin...")
   set_obd_multiplexing(True)
-  vin_rx_addr, vin_rx_bus, vin = get_vin(*can_callbacks, (0, 1), debug=args.debug)
+  vin_rx_addr, vin_rx_bus, vin = get_vin(*can_callbacks, (0, 1))
   print(f'RX: {hex(vin_rx_addr)}, BUS: {vin_rx_bus}, VIN: {vin}')
   print(f"Getting VIN took {time.time() - t:.3f} s")
   print()
 
   t = time.time()
-  fw_vers = get_fw_versions(*can_callbacks, set_obd_multiplexing, query_brand=args.brand, extra=extra, num_pandas=num_pandas, debug=args.debug, progress=True)
+  fw_vers = get_fw_versions(*can_callbacks, set_obd_multiplexing, query_brand=args.brand, extra=extra, num_pandas=num_pandas, progress=True)
   _, candidates = match_fw_to_car(fw_vers, vin)
 
   print()

selfdrive/debug/car/vin.py

@@ -2,6 +2,7 @@
 import argparse
 import time
 import cereal.messaging as messaging
+from opendbc.car.carlog import carlog
 from opendbc.car.vin import get_vin
 from openpilot.selfdrive.car.card import can_comm_callbacks
 
@@ -13,10 +14,13 @@ if __name__ == "__main__":
   parser.add_argument('--retry', type=int, default=5)
   args = parser.parse_args()
 
+  if args.debug:
+    carlog.setLevel('DEBUG')
+
   sendcan = messaging.pub_sock('sendcan')
   logcan = messaging.sub_sock('can')
   can_callbacks = can_comm_callbacks(logcan, sendcan)
   time.sleep(1)
 
-  vin_rx_addr, vin_rx_bus, vin = get_vin(*can_callbacks, (args.bus,), args.timeout, args.retry, debug=args.debug)
+  vin_rx_addr, vin_rx_bus, vin = get_vin(*can_callbacks, (args.bus,), args.timeout, args.retry)
   print(f'RX: {hex(vin_rx_addr)}, BUS: {vin_rx_bus}, VIN: {vin}')

selfdrive/debug/clear_dtc.py

@@ -2,7 +2,9 @@
 import sys
 import argparse
 from subprocess import check_output, CalledProcessError
+from opendbc.car.carlog import carlog
 from opendbc.car.uds import UdsClient, MessageTimeoutError, SESSION_TYPE, DTC_GROUP_TYPE
+from opendbc.safety import Safety
 from panda import Panda
 
 parser = argparse.ArgumentParser(description="clear DTC status")
@@ -11,6 +13,9 @@ parser.add_argument("--bus", type=int, default=0)
 parser.add_argument('--debug', action='store_true')
 args = parser.parse_args()
 
+if args.debug:
+  carlog.setLevel('DEBUG')
+
 try:
   check_output(["pidof", "pandad"])
   print("pandad is running, please kill openpilot before running this script! (aborted)")
@@ -20,8 +25,8 @@ except CalledProcessError as e:
     raise e
 
 panda = Panda()
-panda.set_safety_mode(Panda.SAFETY_ELM327)
-uds_client = UdsClient(panda, args.addr, bus=args.bus, debug=args.debug)
+panda.set_safety_mode(Safety.SAFETY_ELM327)
+uds_client = UdsClient(panda, args.addr, bus=args.bus)
 print("extended diagnostic session ...")
 try:
   uds_client.diagnostic_session_control(SESSION_TYPE.EXTENDED_DIAGNOSTIC)

selfdrive/debug/hyundai_enable_radar_points.py

@@ -16,7 +16,9 @@ import argparse
 from typing import NamedTuple
 from subprocess import check_output, CalledProcessError
 
+from opendbc.car.carlog import carlog
 from opendbc.car.uds import UdsClient, SESSION_TYPE, DATA_IDENTIFIER_TYPE
+from opendbc.safety import Safety
 from panda.python import Panda
 
 class ConfigValues(NamedTuple):
@@ -78,6 +80,9 @@ if __name__ == "__main__":
   parser.add_argument('--bus', type=int, default=0, help='can bus to use (default: 0)')
   args = parser.parse_args()
 
+  if args.debug:
+    carlog.setLevel('DEBUG')
+
   try:
     check_output(["pidof", "pandad"])
     print("pandad is running, please kill openpilot before running this script! (aborted)")
@@ -92,8 +97,8 @@ if __name__ == "__main__":
     sys.exit(0)
 
   panda = Panda()
-  panda.set_safety_mode(Panda.SAFETY_ELM327)
-  uds_client = UdsClient(panda, 0x7D0, bus=args.bus, debug=args.debug)
+  panda.set_safety_mode(Safety.SAFETY_ELM327)
+  uds_client = UdsClient(panda, 0x7D0, bus=args.bus)
 
   print("\n[START DIAGNOSTIC SESSION]")
   session_type : SESSION_TYPE = 0x07 # type: ignore

selfdrive/debug/qlog_size.py

@@ -6,7 +6,7 @@ from collections import defaultdict
 import matplotlib.pyplot as plt
 
 from cereal.services import SERVICE_LIST
-from openpilot.system.loggerd.uploader import LOG_COMPRESSION_LEVEL
+from openpilot.common.file_helpers import LOG_COMPRESSION_LEVEL
 from openpilot.tools.lib.logreader import LogReader
 from tqdm import tqdm
 

selfdrive/debug/read_dtc_status.py

@@ -2,7 +2,9 @@
 import sys
 import argparse
 from subprocess import check_output, CalledProcessError
+from opendbc.car.carlog import carlog
 from opendbc.car.uds import UdsClient, SESSION_TYPE, DTC_REPORT_TYPE, DTC_STATUS_MASK_TYPE, get_dtc_num_as_str, get_dtc_status_names
+from opendbc.safety import Safety
 from panda import Panda
 
 parser = argparse.ArgumentParser(description="read DTC status")
@@ -11,6 +13,9 @@ parser.add_argument("--bus", type=int, default=0)
 parser.add_argument('--debug', action='store_true')
 args = parser.parse_args()
 
+if args.debug:
+  carlog.setLevel('DEBUG')
+
 try:
   check_output(["pidof", "pandad"])
   print("pandad is running, please kill openpilot before running this script! (aborted)")
@@ -20,8 +25,8 @@ except CalledProcessError as e:
     raise e
 
 panda = Panda()
-panda.set_safety_mode(Panda.SAFETY_ELM327)
-uds_client = UdsClient(panda, args.addr, bus=args.bus, debug=args.debug)
+panda.set_safety_mode(Safety.SAFETY_ELM327)
+uds_client = UdsClient(panda, args.addr, bus=args.bus)
 print("extended diagnostic session ...")
 uds_client.diagnostic_session_control(SESSION_TYPE.EXTENDED_DIAGNOSTIC)
 print("read diagnostic codes ...")

selfdrive/debug/uiview.py

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

selfdrive/debug/vw_mqb_config.py

@@ -3,8 +3,10 @@
 import argparse
 import struct
 from enum import IntEnum
+from opendbc.car.carlog import carlog
 from opendbc.car.uds import UdsClient, MessageTimeoutError, NegativeResponseError, SESSION_TYPE,\
   DATA_IDENTIFIER_TYPE, ACCESS_TYPE
+from opendbc.safety import Safety
 from panda import Panda
 from datetime import date
 
@@ -33,10 +35,13 @@ if __name__ == "__main__":
   parser.add_argument("action", choices={"show", "enable", "disable"}, help="show or modify current EPS HCA config")
   args = parser.parse_args()
 
+  if args.debug:
+    carlog.setLevel('DEBUG')
+
   panda = Panda()
-  panda.set_safety_mode(Panda.SAFETY_ELM327)
+  panda.set_safety_mode(Safety.SAFETY_ELM327)
   bus = 1 if panda.has_obd() else 0
-  uds_client = UdsClient(panda, MQB_EPS_CAN_ADDR, MQB_EPS_CAN_ADDR + RX_OFFSET, bus, timeout=0.2, debug=args.debug)
+  uds_client = UdsClient(panda, MQB_EPS_CAN_ADDR, MQB_EPS_CAN_ADDR + RX_OFFSET, bus, timeout=0.2)
 
   try:
     uds_client.diagnostic_session_control(SESSION_TYPE.EXTENDED_DIAGNOSTIC)

selfdrive/locationd/calibrationd.py

@@ -6,7 +6,6 @@ While the roll calibration is a real value that can be estimated, here we assume
 and the image input into the neural network is not corrected for roll.
 '''
 
-import gc
 import os
 import capnp
 import numpy as np
@@ -16,7 +15,7 @@ from cereal import log
 import cereal.messaging as messaging
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.params import Params
-from openpilot.common.realtime import set_realtime_priority
+from openpilot.common.realtime import config_realtime_process
 from openpilot.common.transformations.orientation import rot_from_euler, euler_from_rot
 from openpilot.common.swaglog import cloudlog
 
@@ -256,8 +255,7 @@ class Calibrator:
 
 
 def main() -> NoReturn:
-  gc.disable()
-  set_realtime_priority(1)
+  config_realtime_process([0, 1, 2, 3], 5)
 
   pm = messaging.PubMaster(['liveCalibration'])
   sm = messaging.SubMaster(['cameraOdometry', 'carState', 'carParams'], poll='cameraOdometry')

selfdrive/locationd/locationd.py

@@ -12,6 +12,7 @@ from cereal.services import SERVICE_LIST
 from openpilot.common.transformations.orientation import rot_from_euler
 from openpilot.common.realtime import config_realtime_process
 from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.locationd.helpers import rotate_std
 from openpilot.selfdrive.locationd.models.pose_kf import PoseKalman, States
 from openpilot.selfdrive.locationd.models.constants import ObservationKind, GENERATED_DIR
@@ -78,20 +79,20 @@ class LocationEstimator:
     # sensor time and log time should be close
     sensor_time_invalid = abs(sensor_time - t) > MAX_SENSOR_TIME_DIFF
     if sensor_time_invalid:
-      print("Sensor reading ignored, sensor timestamp more than 100ms off from log time")
+      cloudlog.warning("Sensor reading ignored, sensor timestamp more than 100ms off from log time")
     return not sensor_time_invalid
 
   def _validate_timestamp(self, t: float):
     kf_t = self.kf.t
     invalid = not np.isnan(kf_t) and (kf_t - t) > MAX_FILTER_REWIND_TIME
     if invalid:
-      print("Observation timestamp is older than the max rewind threshold of the filter")
+      cloudlog.warning("Observation timestamp is older than the max rewind threshold of the filter")
     return not invalid
 
   def _finite_check(self, t: float, new_x: np.ndarray, new_P: np.ndarray):
     all_finite = np.isfinite(new_x).all() and np.isfinite(new_P).all()
     if not all_finite:
-      print("Non-finite values detected, kalman reset")
+      cloudlog.error("Non-finite values detected, kalman reset")
       self.reset(t)
 
   def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader) -> HandleLogResult:
@@ -308,13 +309,12 @@ def main():
             continue
 
           if res == HandleLogResult.TIMING_INVALID:
-            print(f"Observation {which} ignored due to failed timing check")
+            cloudlog.warning(f"Observation {which} ignored due to failed timing check")
             observation_input_invalid[which] += 1
-            print(observation_input_invalid[which])
           elif res == HandleLogResult.INPUT_INVALID:
-            print(f"Observation {which} ignored due to failed sanity check")
+            cloudlog.warning(f"Observation {which} ignored due to failed sanity check")
             observation_input_invalid[which] += 1
-          else:
+          elif res == HandleLogResult.SUCCESS:
             observation_input_invalid[which] *= input_invalid_decay[which]
     else:
       filter_initialized = sm.all_checks() and sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)

selfdrive/modeld/constants.py

@@ -31,7 +31,6 @@ class ModelConstants:
   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
@@ -72,13 +71,14 @@ class Plan:
 class Meta:
   ENGAGED = slice(0, 1)
   # next 2, 4, 6, 8, 10 seconds
-  GAS_DISENGAGE = slice(1, 36, 7)
-  BRAKE_DISENGAGE = slice(2, 36, 7)
-  STEER_OVERRIDE = slice(3, 36, 7)
-  HARD_BRAKE_3 = slice(4, 36, 7)
-  HARD_BRAKE_4 = slice(5, 36, 7)
-  HARD_BRAKE_5 = slice(6, 36, 7)
-  GAS_PRESS = slice(7, 36, 7)
+  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
-  LEFT_BLINKER = slice(36, 48, 2)
-  RIGHT_BLINKER = slice(37, 48, 2)
+  GAS_PRESS = slice(31, 55, 4)
+  BRAKE_PRESS = slice(32, 55, 4)
+  LEFT_BLINKER = slice(33, 55, 4)
+  RIGHT_BLINKER = slice(34, 55, 4)

selfdrive/modeld/dmonitoringmodeld.py

@@ -8,7 +8,6 @@ if TICI:
   os.environ['QCOM'] = '1'
 else:
   from openpilot.selfdrive.modeld.runners.ort_helpers import make_onnx_cpu_runner
-import gc
 import math
 import time
 import pickle
@@ -21,7 +20,7 @@ from cereal import messaging
 from cereal.messaging import PubMaster, SubMaster
 from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
 from openpilot.common.swaglog import cloudlog
-from openpilot.common.realtime import set_realtime_priority
+from openpilot.common.realtime import config_realtime_process
 from openpilot.common.transformations.model import dmonitoringmodel_intrinsics, DM_INPUT_SIZE
 from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
 from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame
@@ -140,9 +139,8 @@ def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts:
 
 
 def main():
-  gc.disable()
   setproctitle(PROCESS_NAME)
-  set_realtime_priority(1)
+  config_realtime_process([0, 1, 2, 3], 5)
 
   sentry.set_tag("daemon", PROCESS_NAME)
   cloudlog.bind(daemon=PROCESS_NAME)

selfdrive/modeld/fill_model_msg.py

@@ -112,10 +112,17 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
 
   # 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()
+  if 'sim_pose' in net_output_data:
+    temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist()
+    temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist()
+    temporal_pose.rot = net_output_data['sim_pose'][0,ModelConstants.POSE_WIDTH//2:].tolist()
+    temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,ModelConstants.POSE_WIDTH//2:].tolist()
+  else:
+    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
   fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)

selfdrive/modeld/parse_model_outputs.py

@@ -91,6 +91,8 @@ class Parser:
     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))

selfdrive/monitoring/dmonitoringd.py

@@ -1,15 +1,12 @@
 #!/usr/bin/env python3
-import gc
-
 import cereal.messaging as messaging
 from openpilot.common.params import Params
-from openpilot.common.realtime import set_realtime_priority
+from openpilot.common.realtime import config_realtime_process
 from openpilot.selfdrive.monitoring.helpers import DriverMonitoring
 
 
 def dmonitoringd_thread():
-  gc.disable()
-  set_realtime_priority(2)
+  config_realtime_process([0, 1, 2, 3], 5)
 
   params = Params()
   pm = messaging.PubMaster(['driverMonitoringState'])

selfdrive/pandad/__init__.py

@@ -2,10 +2,3 @@
 from openpilot.selfdrive.pandad.pandad_api_impl import can_list_to_can_capnp, can_capnp_to_list
 assert can_list_to_can_capnp
 assert can_capnp_to_list
-
-def can_capnp_to_can_list(can, src_filter=None):
-  ret = []
-  for msg in can:
-    if src_filter is None or msg.src in src_filter:
-      ret.append((msg.address, msg.dat, msg.src))
-  return ret

selfdrive/pandad/can_list_to_can_capnp.cc

@@ -1,8 +1,7 @@
 #include "cereal/messaging/messaging.h"
-#include "selfdrive/pandad/panda.h"
 #include "opendbc/can/common.h"
 
-void can_list_to_can_capnp_cpp(const std::vector<can_frame> &can_list, std::string &out, bool sendcan, bool valid) {
+void can_list_to_can_capnp_cpp(const std::vector<CanFrame> &can_list, std::string &out, bool sendcan, bool valid) {
   MessageBuilder msg;
   auto event = msg.initEvent(valid);
 

selfdrive/pandad/pandad_api_impl.pyx

@@ -6,12 +6,6 @@ from libcpp.string cimport string
 from libcpp cimport bool
 from libc.stdint cimport uint8_t, uint32_t, uint64_t
 
-cdef extern from "panda.h":
-  cdef struct can_frame:
-    long address
-    string dat
-    long src
-
 cdef extern from "opendbc/can/common.h":
   cdef struct CanFrame:
     long src
@@ -23,12 +17,12 @@ cdef extern from "opendbc/can/common.h":
     vector[CanFrame] frames
 
 cdef extern from "can_list_to_can_capnp.cc":
-  void can_list_to_can_capnp_cpp(const vector[can_frame] &can_list, string &out, bool sendcan, bool valid)
+  void can_list_to_can_capnp_cpp(const vector[CanFrame] &can_list, string &out, bool sendcan, bool valid)
   void can_capnp_to_can_list_cpp(const vector[string] &strings, vector[CanData] &can_data, bool sendcan)
 
 def can_list_to_can_capnp(can_msgs, msgtype='can', valid=True):
-  cdef can_frame *f
-  cdef vector[can_frame] can_list
+  cdef CanFrame *f
+  cdef vector[CanFrame] can_list
 
   can_list.reserve(len(can_msgs))
   for can_msg in can_msgs:

selfdrive/selfdrived/selfdrived.py

@@ -7,7 +7,7 @@ import cereal.messaging as messaging
 
 from cereal import car, log, custom
 from msgq.visionipc import VisionIpcClient, VisionStreamType
-from panda import ALTERNATIVE_EXPERIENCE
+from opendbc.safety import ALTERNATIVE_EXPERIENCE
 
 
 from openpilot.common.params import Params
@@ -31,7 +31,6 @@ from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 REPLAY = "REPLAY" in os.environ
 SIMULATION = "SIMULATION" in os.environ
 TESTING_CLOSET = "TESTING_CLOSET" in os.environ
-IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"}
 LONGITUDINAL_PERSONALITY_MAP = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()}
 
 ThermalStatus = log.DeviceState.ThermalStatus
@@ -284,7 +283,7 @@ class SelfdriveD(CruiseHelper):
       if not_running != self.not_running_prev:
         cloudlog.event("process_not_running", not_running=not_running, error=True)
       self.not_running_prev = not_running
-    if self.sm.recv_frame['managerState'] and (not_running - IGNORE_PROCESSES):
+    if self.sm.recv_frame['managerState'] and not_running:
       self.events.add(EventName.processNotRunning)
     else:
       if not SIMULATION and not self.rk.lagging:
@@ -380,6 +379,10 @@ class SelfdriveD(CruiseHelper):
       if self.sm['modelV2'].frameDropPerc > 20:
         self.events.add(EventName.modeldLagging)
 
+    # mute canBusMissing event if in Park, as it sometimes may trigger a false alarm with MADS in Paused state
+    if CS.gearShifter == car.CarState.GearShifter.park and self.mads.enabled:
+      self.events.remove(EventName.canBusMissing)
+
     CruiseHelper.update(self, CS, self.events_sp, self.experimental_mode)
 
     # decrement personality on distance button press

selfdrive/test/longitudinal_maneuvers/maneuver.py

@@ -66,7 +66,7 @@ class Maneuver:
         print("Crashed!!!!")
         valid = False
 
-      if self.ensure_start and log['v_rel'] > 0 and log['speeds'][-1] <= 0.1:
+      if self.ensure_start and log['v_rel'] > 0 and log['acceleration'] < 1e-3:
         print('LongitudinalPlanner not starting!')
         valid = False
 

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -30,8 +30,8 @@ class Plant:
 
     self.distance = 0.
     self.speed = speed
+    self.should_stop = False
     self.acceleration = 0.0
-    self.speeds = []
 
     # lead car
     self.lead_relevancy = lead_relevancy
@@ -134,9 +134,9 @@ class Plant:
           'liveParameters': lp.liveParameters,
           'modelV2': model.modelV2}
     self.planner.update(sm)
-    self.speed = self.planner.v_desired_filter.x
-    self.acceleration = self.planner.a_desired
-    self.speeds = self.planner.v_desired_trajectory.tolist()
+    self.acceleration = self.planner.output_a_target
+    self.speed = self.speed + self.acceleration * self.ts
+    self.should_stop = self.planner.output_should_stop
     fcw = self.planner.fcw
     self.distance_lead = self.distance_lead + v_lead * self.ts
 
@@ -168,7 +168,7 @@ class Plant:
       "distance": self.distance,
       "speed": self.speed,
       "acceleration": self.acceleration,
-      "speeds": self.speeds,
+      "should_stop": self.should_stop,
       "distance_lead": self.distance_lead,
       "fcw": fcw,
     }

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -150,10 +150,7 @@ def create_maneuvers(kwargs):
       enabled=False,
       **kwargs,
     ),
-  ]
-  if not kwargs['e2e']:
-    # allow_throttle won't trigger with e2e
-    maneuvers.append(Maneuver(
+    Maneuver(
       "slow to 5m/s with allow_throttle = False and pitch = +0.1",
       duration=30.,
       initial_speed=20.,
@@ -164,7 +161,7 @@ def create_maneuvers(kwargs):
       breakpoints=[0.0, 2., 2.01],
       ensure_slowdown=True,
       **kwargs,
-    ))
+    )]
   if not kwargs['force_decel']:
     # controls relies on planner commanding to move for stock-ACC resume spamming
     maneuvers.append(Maneuver(