Merge branch 'upstream/openpilot/master' into sync-20250116

# Conflicts:
#    .github/workflows/ui_preview.yaml
#    opendbc_repo
#    panda
#    release/release_files.py
#    selfdrive/test/process_replay/ref_commit
#    selfdrive/ui/tests/test_ui/run.py

.github/workflows/selfdrive_tests.yaml

@@ -67,15 +67,7 @@ jobs:
       timeout-minutes: 1
       run: |
         cd $STRIPPED_DIR
-        ${{ env.RUN }} "release/check-dirty.sh && \
+        ${{ env.RUN }} "release/check-dirty.sh"
-                        MAX_EXAMPLES=5 $PYTEST -m 'not slow' selfdrive/car system/manager"
-    - name: Static analysis
-      timeout-minutes: 1
-      run: |
-        cd $GITHUB_WORKSPACE
-        cp pyproject.toml $STRIPPED_DIR
-        cd $STRIPPED_DIR
-        ${{ env.RUN }} "scripts/lint/lint.sh --skip check_added_large_files"
 
   build:
     runs-on:

.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_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"
+          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 keyboard_uppercase"
           A=($scenes)
 
           DIFF=""

RELEASES.md

@@ -1,16 +1,23 @@
-Version 0.9.8 (2024-XX-XX)
+Version 0.9.9 (2025-03-30)
+========================
+* Coming soon
+  * Rivian support
+  * F-150 & Mach-E support
+  * Tesla Model 3 support
+
+Version 0.9.8 (2025-01-30)
 ========================
-* New driving model
-  * Trained in brand new ML simulator
-  * Model now gates applying positive accel in Chill mode
 * 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 
+  * 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)
 ========================

cereal/log.capnp

@@ -483,6 +483,7 @@ struct GpsLocationData {
   speedAccuracy @12 :Float32;
 
   hasFix @13 :Bool;
+  satelliteCount @14 :Int8;
 
   enum SensorSource {
     android @0;

common/clutil.cc

@@ -79,6 +79,10 @@ cl_context cl_create_context(cl_device_id device_id) {
   return CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err));
 }
 
+void cl_release_context(cl_context context) {
+  clReleaseContext(context);
+}
+
 cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args) {
   return cl_program_from_source(ctx, device_id, util::read_file(path), args);
 }

common/clutil.h

@@ -23,6 +23,7 @@
 
 cl_device_id cl_get_device_id(cl_device_type device_type);
 cl_context cl_create_context(cl_device_id device_id);
+void cl_release_context(cl_context context);
 cl_program cl_program_from_source(cl_context ctx, cl_device_id device_id, const std::string& src, const char* args = nullptr);
 cl_program cl_program_from_binary(cl_context ctx, cl_device_id device_id, const uint8_t* binary, size_t length, const char* args = nullptr);
 cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args);

common/pid.py

@@ -1,9 +1,6 @@
 import numpy as np
 from numbers import Number
 
-from openpilot.common.numpy_fast import clip, interp
-
-
 class PIDController:
   def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
     self._k_p = k_p
@@ -28,15 +25,15 @@ class PIDController:
 
   @property
   def k_p(self):
-    return interp(self.speed, self._k_p[0], self._k_p[1])
+    return np.interp(self.speed, self._k_p[0], self._k_p[1])
 
   @property
   def k_i(self):
-    return interp(self.speed, self._k_i[0], self._k_i[1])
+    return np.interp(self.speed, self._k_i[0], self._k_i[1])
 
   @property
   def k_d(self):
-    return interp(self.speed, self._k_d[0], self._k_d[1])
+    return np.interp(self.speed, self._k_d[0], self._k_d[1])
 
   @property
   def error_integral(self):
@@ -64,10 +61,10 @@ class PIDController:
 
         # Clip i to prevent exceeding control limits
         control_no_i = self.p + self.d + self.f
-        control_no_i = clip(control_no_i, self.neg_limit, self.pos_limit)
+        control_no_i = np.clip(control_no_i, self.neg_limit, self.pos_limit)
-        self.i = clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i)
+        self.i = np.clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i)
 
     control = self.p + self.i + self.d + self.f
 
-    self.control = clip(control, self.neg_limit, self.pos_limit)
+    self.control = np.clip(control, self.neg_limit, self.pos_limit)
     return self.control

common/swaglog.cc

@@ -26,6 +26,9 @@ public:
     zmq_setsockopt(sock, ZMQ_LINGER, &timeout, sizeof(timeout));
     zmq_connect(sock, Path::swaglog_ipc().c_str());
 
+    // workaround for https://github.com/dropbox/json11/issues/38
+    setlocale(LC_NUMERIC, "C");
+
     print_level = CLOUDLOG_WARNING;
     if (const char* print_lvl = getenv("LOGPRINT")) {
       if (strcmp(print_lvl, "debug") == 0) {

common/tests/test_numpy_fast.py

@@ -1,21 +0,0 @@
-import numpy as np
-
-from openpilot.common.numpy_fast import interp
-
-
-class TestInterp:
-  def test_correctness_controls(self):
-    _A_CRUISE_MIN_BP = np.asarray([0., 5., 10., 20., 40.])
-    _A_CRUISE_MIN_V = np.asarray([-1.0, -.8, -.67, -.5, -.30])
-    v_ego_arr = [-1, -1e-12, 0, 4, 5, 6, 7, 10, 11, 15.2, 20, 21, 39,
-                 39.999999, 40, 41]
-
-    expected = np.interp(v_ego_arr, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
-    actual = interp(v_ego_arr, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
-
-    np.testing.assert_equal(actual, expected)
-
-    for v_ego in v_ego_arr:
-      expected = np.interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
-      actual = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
-      np.testing.assert_equal(actual, expected)

docs/CARS.md

@@ -88,7 +88,7 @@ A supported vehicle is one that just works when you install a comma device. All
 |Hyundai|Elantra 2017-18|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai B connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra 2017-18">Buy Here</a></sub></details>||
 |Hyundai|Elantra 2019|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai G connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra 2019">Buy Here</a></sub></details>||
 |Hyundai|Elantra 2021-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra 2021-23">Buy Here</a></sub></details>|<a href="https://youtu.be/_EdYQtV52-c" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
-|Hyundai|Elantra GT 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra GT 2017-19">Buy Here</a></sub></details>||
+|Hyundai|Elantra GT 2017-20|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra GT 2017-20">Buy Here</a></sub></details>||
 |Hyundai|Elantra Hybrid 2021-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra Hybrid 2021-23">Buy Here</a></sub></details>|<a href="https://youtu.be/_EdYQtV52-c" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
 |Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai J connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Genesis 2015-16">Buy Here</a></sub></details>||
 |Hyundai|i30 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=i30 2017-19">Buy Here</a></sub></details>||

release/release_files.py

@@ -6,41 +6,11 @@ from pathlib import Path
 HERE = os.path.abspath(os.path.dirname(__file__))
 ROOT = HERE + "/.."
 
-# blacklisting is for two purposes:
-# - minimizing release download size
-# - keeping the diff readable
 blacklist = [
-  "panda/drivers/",
+  ".git/",
-  "panda/examples/",
-  "panda/tests/safety/",
-
-  "opendbc_repo/dbc/.*.dbc$",
-  "opendbc_repo/dbc/generator/",
-
-  "cereal/.*test.*",
-  "^common/tests/",
-
-  # particularly large text files
-  "uv.lock",
-  "third_party/catch2",
-  "selfdrive/car/tests/test_models.*",
-
-  "^tools/",
-  "^tinygrad_repo/",
 
   "matlab.*.md",
 
-  ".git/",
-  ".github/",
-  ".devcontainer/",
-  "Darwin/",
-  ".vscode",
-
-  # common things
-  "LICENSE",
-  "Dockerfile",
-  ".pre-commit",
-
   # no LFS or submodules in release
   ".lfsconfig",
   ".gitattributes",
@@ -48,153 +18,10 @@ blacklist = [
   ".gitmodules",
 ]
 
-# Sunnypilot blacklist
-sunnypilot_blacklist = [
-  "system/loggerd/sunnylink_uploader.py",  # Temporarily, until we are ready to roll it out widely
-  ".idea/",
-  ".run/",
-  ".*__pycache__/.*",
-  ".*\\.pyc",
-  "teleoprtc/*",
-  "third_party/snpe/x86_64/*",
-  "body/board/canloader.py",
-  "body/board/flash_base.sh",
-  "body/board/flash_knee.sh",
-  "body/board/recover.sh",
-  ".*/test/",
-  ".*/tests/",
-  ".*tinygrad_repo/tinygrad/renderer/",
-  "README.md",
-  ".*internal/",
-  "docs/.*",
-  ".sconsign.dblite",
-  "release/ci/scons_cache/",
-  ".gitlab-ci.yml",
-  ".clang-tidy",
-  ".dockerignore",
-  ".editorconfig",
-  ".python-version",
-  "SECURITY.md",
-  "codecov.yml",
-  "conftest.py",
-  "poetry.lock",
-  ".venv/",
-]
-
-# Merge the blacklists
-blacklist += sunnypilot_blacklist
-
 # gets you through the blacklist
-whitelist = [
+whitelist: list[str] = [
-  "tools/lib/",
-  "tools/bodyteleop/",
-  "tools/joystick/",
-  "tools/longitudinal_maneuvers/",
-
-  "tinygrad_repo/examples/openpilot/compile3.py",
-  "tinygrad_repo/extra/onnx.py",
-  "tinygrad_repo/extra/onnx_ops.py",
-  "tinygrad_repo/extra/thneed.py",
-  "tinygrad_repo/extra/utils.py",
-  "tinygrad_repo/tinygrad/codegen/kernel.py",
-  "tinygrad_repo/tinygrad/codegen/linearizer.py",
-  "tinygrad_repo/tinygrad/features/image.py",
-  "tinygrad_repo/tinygrad/features/search.py",
-  "tinygrad_repo/tinygrad/nn/*",
-  "tinygrad_repo/tinygrad/renderer/cstyle.py",
-  "tinygrad_repo/tinygrad/renderer/opencl.py",
-  "tinygrad_repo/tinygrad/runtime/lib.py",
-  "tinygrad_repo/tinygrad/runtime/ops_cpu.py",
-  "tinygrad_repo/tinygrad/runtime/ops_disk.py",
-  "tinygrad_repo/tinygrad/runtime/ops_gpu.py",
-  "tinygrad_repo/tinygrad/shape/*",
-  "tinygrad_repo/tinygrad/.*.py",
-
-  # TODO: do this automatically
-  "opendbc_repo/dbc/comma_body.dbc",
-  "opendbc_repo/dbc/chrysler_ram_hd_generated.dbc",
-  "opendbc_repo/dbc/chrysler_ram_dt_generated.dbc",
-  "opendbc_repo/dbc/chrysler_pacifica_2017_hybrid_generated.dbc",
-  "opendbc_repo/dbc/chrysler_pacifica_2017_hybrid_private_fusion.dbc",
-  "opendbc_repo/dbc/gm_global_a_powertrain_generated.dbc",
-  "opendbc_repo/dbc/gm_global_a_object.dbc",
-  "opendbc_repo/dbc/gm_global_a_chassis.dbc",
-  "opendbc_repo/dbc/FORD_CADS.dbc",
-  "opendbc_repo/dbc/ford_fusion_2018_adas.dbc",
-  "opendbc_repo/dbc/ford_lincoln_base_pt.dbc",
-  "opendbc_repo/dbc/honda_accord_2018_can_generated.dbc",
-  "opendbc_repo/dbc/acura_ilx_2016_can_generated.dbc",
-  "opendbc_repo/dbc/acura_rdx_2018_can_generated.dbc",
-  "opendbc_repo/dbc/acura_rdx_2020_can_generated.dbc",
-  "opendbc_repo/dbc/honda_civic_touring_2016_can_generated.dbc",
-  "opendbc_repo/dbc/honda_civic_hatchback_ex_2017_can_generated.dbc",
-  "opendbc_repo/dbc/honda_crv_touring_2016_can_generated.dbc",
-  "opendbc_repo/dbc/honda_crv_ex_2017_can_generated.dbc",
-  "opendbc_repo/dbc/honda_crv_ex_2017_body_generated.dbc",
-  "opendbc_repo/dbc/honda_crv_executive_2016_can_generated.dbc",
-  "opendbc_repo/dbc/honda_fit_ex_2018_can_generated.dbc",
-  "opendbc_repo/dbc/honda_odyssey_exl_2018_generated.dbc",
-  "opendbc_repo/dbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc",
-  "opendbc_repo/dbc/honda_insight_ex_2019_can_generated.dbc",
-  "opendbc_repo/dbc/acura_ilx_2016_nidec.dbc",
-  "opendbc_repo/dbc/honda_civic_ex_2022_can_generated.dbc",
-  "opendbc_repo/dbc/hyundai_canfd.dbc",
-  "opendbc_repo/dbc/hyundai_kia_generic.dbc",
-  "opendbc_repo/dbc/hyundai_kia_mando_front_radar_generated.dbc",
-  "opendbc_repo/dbc/mazda_2017.dbc",
-  "opendbc_repo/dbc/nissan_x_trail_2017_generated.dbc",
-  "opendbc_repo/dbc/nissan_leaf_2018_generated.dbc",
-  "opendbc_repo/dbc/subaru_global_2017_generated.dbc",
-  "opendbc_repo/dbc/subaru_global_2020_hybrid_generated.dbc",
-  "opendbc_repo/dbc/subaru_outback_2015_generated.dbc",
-  "opendbc_repo/dbc/subaru_outback_2019_generated.dbc",
-  "opendbc_repo/dbc/subaru_forester_2017_generated.dbc",
-  "opendbc_repo/dbc/toyota_tnga_k_pt_generated.dbc",
-  "opendbc_repo/dbc/toyota_new_mc_pt_generated.dbc",
-  "opendbc_repo/dbc/toyota_nodsu_pt_generated.dbc",
-  "opendbc_repo/dbc/toyota_adas.dbc",
-  "opendbc_repo/dbc/toyota_tss2_adas.dbc",
-  "opendbc_repo/dbc/vw_golf_mk4.dbc",
-  "opendbc_repo/dbc/vw_mqb_2010.dbc",
-  "opendbc_repo/dbc/tesla_can.dbc",
-  "opendbc_repo/dbc/tesla_radar_bosch_generated.dbc",
-  "opendbc_repo/dbc/tesla_radar_continental_generated.dbc",
-  "opendbc_repo/dbc/tesla_powertrain.dbc",
 ]
 
-# Sunnypilot whitelist
-sunnypilot_whitelist = [
-  "^README.md",
-  ".*selfdrive/test/fuzzy_generation.py",
-  ".*selfdrive/test/helpers.py",
-  ".*selfdrive/test/__init__.py",
-  ".*selfdrive/test/setup_device_ci.sh",
-  ".*selfdrive/test/test_time_to_onroad.py",
-  ".*selfdrive/test/test_onroad.py",
-  ".*system/manager/test/test_manager.py",
-  ".*system/manager/test/__init__.py",
-  ".*system/qcomgpsd/tests/test_qcomgpsd.py",
-  ".*system/updated/casync/tests/test_casync.py",
-  ".*system/updated/tests/test_git.py",
-  ".*system/updated/tests/test_base.py",
-  ".*selfdrive/ui/tests/test_translations.py",
-  ".*selfdrive/car/tests/__init__.py",
-  ".*selfdrive/car/tests/test_car_interfaces.py",
-  ".*selfdrive/navd/tests/test_navd.py",
-  ".*selfdrive/navd/tests/test_map_renderer.py",
-  ".*selfdrive/boardd/tests/test_boardd_loopback.py",
-  ".*INTEGRATION.md",
-  ".*HOW-TOS.md",
-  ".*CARS.md",
-  ".*LIMITATIONS.md",
-  ".*CONTRIBUTING.md",
-  ".*sunnyhaibin0850_qrcode_paypal.me.png",
-  "opendbc/.*.dbc",
-]
-
-# Merge the whitelists
-whitelist += sunnypilot_whitelist
-
 
 if __name__ == "__main__":
   for f in Path(ROOT).rglob("**/*"):

selfdrive/car/car_specific.py

@@ -122,7 +122,7 @@ class CarSpecificEvents:
 
     elif self.CP.carName == 'volkswagen':
       events = self.create_common_events(CS, CS_prev, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic],
-                                         pcm_enable=not self.CP.openpilotLongitudinalControl,
+                                         pcm_enable=self.CP.pcmCruise,
                                          enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))
 
       # Low speed steer alert hysteresis logic
@@ -149,7 +149,7 @@ class CarSpecificEvents:
       # Main button also can trigger an engagement on these cars
       self.cruise_buttons.append(any(ev.type in HYUNDAI_ENABLE_BUTTONS for ev in CS.buttonEvents))
       events = self.create_common_events(CS, CS_prev, extra_gears=(GearShifter.sport, GearShifter.manumatic),
-                                         pcm_enable=self.CP.pcmCruise, allow_enable=any(self.cruise_buttons))
+                                         pcm_enable=self.CP.pcmCruise, allow_enable=any(self.cruise_buttons), allow_button_cancel=False)
 
       # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
       if CS.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
@@ -165,7 +165,7 @@ class CarSpecificEvents:
     return events
 
   def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState, extra_gears=None, pcm_enable=True,
-                           allow_enable=True, enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)):
+                           allow_enable=True, allow_button_cancel=True, enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)):
     events = Events()
 
     if CS.doorOpen:
@@ -191,7 +191,7 @@ class CarSpecificEvents:
       events.add(EventName.speedTooHigh)
     if CS.cruiseState.nonAdaptive:
       events.add(EventName.wrongCruiseMode)
-    if CS.brakeHoldActive and self.CP.openpilotLongitudinalControl:
+    if CS.brakeHoldActive and self.CP.openpilotLongitudinalControl and self.CP.carName != 'toyota':
       events.add(EventName.brakeHold)
     if CS.parkingBrake:
       events.add(EventName.parkBrake)
@@ -216,7 +216,8 @@ class CarSpecificEvents:
       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
-      if b.type == ButtonType.cancel:
+      # 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):
         events.add(EventName.buttonCancel)
 
     # Handle permanent and temporary steering faults

selfdrive/car/card.py

@@ -75,6 +75,7 @@ class Car:
     self.can_rcv_cum_timeout_counter = 0
 
     self.CC_prev = car.CarControl.new_message()
+    self.CS_prev = car.CarState.new_message()
     self.initialized_prev = False
 
     self.last_actuators_output = structs.CarControl.Actuators()
@@ -188,8 +189,12 @@ class Car:
     if can_rcv_valid and REPLAY:
       self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
 
-    # TODO: mirror the carState.cruiseState struct?
     self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric)
+    if self.sm['carControl'].enabled and not self.CC_prev.enabled:
+      # Use CarState w/ buttons from the step selfdrived enables on
+      self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode)
+
+    # TODO: mirror the carState.cruiseState struct?
     CS.vCruise = float(self.v_cruise_helper.v_cruise_kph)
     CS.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph)
 
@@ -247,9 +252,6 @@ class Car:
   def step(self):
     CS, RD = self.state_update()
 
-    if self.sm['carControl'].enabled and not self.CC_prev.enabled:
-      self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode)
-
     self.state_publish(CS, RD)
 
     initialized = (not any(e.name == EventName.selfdriveInitializing for e in self.sm['onroadEvents']) and
@@ -258,6 +260,7 @@ class Car:
       self.controls_update(CS, self.sm['carControl'])
 
     self.initialized_prev = initialized
+    self.CS_prev = CS
 
   def params_thread(self, evt):
     while not evt.is_set():

selfdrive/car/cruise.py

@@ -1,8 +1,8 @@
 import math
+import numpy as np
 
 from cereal import car
 from openpilot.common.conversions import Conversions as CV
-from openpilot.common.numpy_fast import clip
 
 
 # WARNING: this value was determined based on the model's training distribution,
@@ -106,7 +106,7 @@ class VCruiseHelper:
     if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
       self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH)
 
-    self.v_cruise_kph = clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)
+    self.v_cruise_kph = np.clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)
 
   def update_button_timers(self, CS, enabled):
     # increment timer for buttons still pressed
@@ -130,6 +130,6 @@ class VCruiseHelper:
     if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_initialized:
       self.v_cruise_kph = self.v_cruise_kph_last
     else:
-      self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
+      self.v_cruise_kph = int(round(np.clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
 
     self.v_cruise_cluster_kph = self.v_cruise_kph

selfdrive/controls/controlsd.py

@@ -119,15 +119,16 @@ class Controls:
 
     # accel PID loop
     pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
-    actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)
+    actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits))
 
     # Steering PID loop and lateral MPC
     self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature)
-    actuators.curvature = self.desired_curvature
+    actuators.curvature = float(self.desired_curvature)
-    actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
+    steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
                                                                             self.steer_limited, self.desired_curvature,
                                                                             self.calibrated_pose) # TODO what if not available
-
+    actuators.steer = float(steer)
+    actuators.steeringAngleDeg = float(steeringAngleDeg)
     # Ensure no NaNs/Infs
     for p in ACTUATOR_FIELDS:
       attr = getattr(actuators, p)
@@ -192,7 +193,7 @@ class Controls:
 
     cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
     cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
-    cs.desiredCurvature = self.desired_curvature
+    cs.desiredCurvature = float(self.desired_curvature)
     cs.longControlState = self.LoC.long_control_state
     cs.upAccelCmd = float(self.LoC.pid.p)
     cs.uiAccelCmd = float(self.LoC.pid.i)

selfdrive/controls/lib/drive_helpers.py

@@ -1,5 +1,5 @@
+import numpy as np
 from cereal import log
-from openpilot.common.numpy_fast import clip
 from openpilot.common.realtime import DT_CTRL
 
 MIN_SPEED = 1.0
@@ -13,10 +13,10 @@ MAX_LATERAL_JERK = 5.0
 MAX_VEL_ERR = 5.0
 
 def clip_curvature(v_ego, prev_curvature, new_curvature):
-  new_curvature = clip(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
+  new_curvature = np.clip(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
   v_ego = max(MIN_SPEED, v_ego)
   max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
-  safe_desired_curvature = clip(new_curvature,
+  safe_desired_curvature = np.clip(new_curvature,
                                 prev_curvature - max_curvature_rate * DT_CTRL,
                                 prev_curvature + max_curvature_rate * DT_CTRL)
 
@@ -26,6 +26,6 @@ def clip_curvature(v_ego, prev_curvature, new_curvature):
 def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float:
   # ToDo: Try relative error, and absolute speed
   if len(modelV2.temporalPose.trans):
-    vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
+    vel_err = np.clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
     return float(vel_err)
   return 0.0

selfdrive/controls/lib/latcontrol.py

@@ -1,6 +1,6 @@
+import numpy as np
 from abc import abstractmethod, ABC
 
-from openpilot.common.numpy_fast import clip
 from openpilot.common.realtime import DT_CTRL
 
 MIN_LATERAL_CONTROL_SPEED = 0.3  # m/s
@@ -28,5 +28,5 @@ class LatControl(ABC):
       self.sat_count += self.sat_count_rate
     else:
       self.sat_count -= self.sat_count_rate
-    self.sat_count = clip(self.sat_count, 0.0, self.sat_limit)
+    self.sat_count = np.clip(self.sat_count, 0.0, self.sat_limit)
     return self.sat_count > (self.sat_limit - 1e-3)

selfdrive/controls/lib/latcontrol_angle.py

@@ -23,7 +23,7 @@ class LatControlAngle(LatControl):
       angle_steers_des += params.angleOffsetDeg
 
     angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
-    angle_log.saturated = self._check_saturation(angle_control_saturated, CS, False)
+    angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False))
     angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
     angle_log.steeringAngleDesiredDeg = angle_steers_des
     return 0, float(angle_steers_des), angle_log

selfdrive/controls/lib/latcontrol_pid.py

@@ -39,10 +39,10 @@ class LatControlPID(LatControl):
       output_steer = self.pid.update(error, override=CS.steeringPressed,
                                      feedforward=steer_feedforward, speed=CS.vEgo)
       pid_log.active = True
-      pid_log.p = self.pid.p
+      pid_log.p = float(self.pid.p)
-      pid_log.i = self.pid.i
+      pid_log.i = float(self.pid.i)
-      pid_log.f = self.pid.f
+      pid_log.f = float(self.pid.f)
-      pid_log.output = output_steer
+      pid_log.output = float(output_steer)
-      pid_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited)
+      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited))
 
     return output_steer, angle_steers_des, pid_log

selfdrive/controls/lib/latcontrol_torque.py

@@ -1,8 +1,8 @@
 import math
+import numpy as np
 
 from cereal import log
 from opendbc.car.interfaces import LatControlInputs
-from openpilot.common.numpy_fast import interp
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 from openpilot.common.pid import PIDController
 from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
@@ -51,7 +51,7 @@ class LatControlTorque(LatControl):
       else:
         assert calibrated_pose is not None
         actual_curvature_pose = calibrated_pose.angular_velocity.yaw / CS.vEgo
-        actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose])
+        actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose])
         curvature_deadzone = 0.0
       desired_lateral_accel = desired_curvature * CS.vEgo ** 2
 
@@ -60,7 +60,7 @@ class LatControlTorque(LatControl):
       actual_lateral_accel = actual_curvature * CS.vEgo ** 2
       lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
 
-      low_speed_factor = interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2
+      low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2
       setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
       measurement = actual_lateral_accel + low_speed_factor * actual_curvature
       gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation
@@ -68,7 +68,7 @@ class LatControlTorque(LatControl):
                                                             setpoint, lateral_accel_deadzone, friction_compensation=False, gravity_adjusted=False)
       torque_from_measurement = self.torque_from_lateral_accel(LatControlInputs(measurement, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
                                                                measurement, lateral_accel_deadzone, friction_compensation=False, gravity_adjusted=False)
-      pid_log.error = torque_from_setpoint - torque_from_measurement
+      pid_log.error = float(torque_from_setpoint - torque_from_measurement)
       ff = self.torque_from_lateral_accel(LatControlInputs(gravity_adjusted_lateral_accel, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
                                           desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, friction_compensation=True,
                                           gravity_adjusted=True)
@@ -80,14 +80,14 @@ class LatControlTorque(LatControl):
                                       freeze_integrator=freeze_integrator)
 
       pid_log.active = True
-      pid_log.p = self.pid.p
+      pid_log.p = float(self.pid.p)
-      pid_log.i = self.pid.i
+      pid_log.i = float(self.pid.i)
-      pid_log.d = self.pid.d
+      pid_log.d = float(self.pid.d)
-      pid_log.f = self.pid.f
+      pid_log.f = float(self.pid.f)
-      pid_log.output = -output_torque
+      pid_log.output = float(-output_torque)
-      pid_log.actualLateralAccel = actual_lateral_accel
+      pid_log.actualLateralAccel = float(actual_lateral_accel)
-      pid_log.desiredLateralAccel = desired_lateral_accel
+      pid_log.desiredLateralAccel = float(desired_lateral_accel)
-      pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited)
+      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited))
 
     # TODO left is positive in this convention
     return -output_torque, 0.0, pid_log

selfdrive/controls/lib/longcontrol.py

@@ -1,5 +1,5 @@
+import numpy as np
 from cereal import car
-from openpilot.common.numpy_fast import clip
 from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.common.pid import PIDController
@@ -84,5 +84,5 @@ class LongControl:
       output_accel = self.pid.update(error, speed=CS.vEgo,
                                      feedforward=a_target)
 
-    self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
+    self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
     return self.last_output_accel

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -4,7 +4,6 @@ import time
 import numpy as np
 from cereal import log
 from opendbc.car.interfaces import ACCEL_MIN
-from openpilot.common.numpy_fast import clip
 from openpilot.common.realtime import DT_MDL
 from openpilot.common.swaglog import cloudlog
 # WARNING: imports outside of constants will not trigger a rebuild
@@ -320,9 +319,9 @@ class LongitudinalMpc:
     # MPC will not converge if immediate crash is expected
     # Clip lead distance to what is still possible to brake for
     min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-ACCEL_MIN * 2)
-    x_lead = clip(x_lead, min_x_lead, 1e8)
+    x_lead = np.clip(x_lead, min_x_lead, 1e8)
-    v_lead = clip(v_lead, 0.0, 1e8)
+    v_lead = np.clip(v_lead, 0.0, 1e8)
-    a_lead = clip(a_lead, -10., 5.)
+    a_lead = np.clip(a_lead, -10., 5.)
     lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
     return lead_xv
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -1,7 +1,6 @@
 #!/usr/bin/env python3
 import math
 import numpy as np
-from openpilot.common.numpy_fast import clip, interp
 
 import cereal.messaging as messaging
 from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
@@ -30,7 +29,7 @@ _A_TOTAL_MAX_BP = [20., 40.]
 
 
 def get_max_accel(v_ego):
-  return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
+  return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
 
 def get_coast_accel(pitch):
   return np.sin(pitch) * -5.65 - 0.3  # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py
@@ -43,7 +42,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
   """
   # FIXME: This function to calculate lateral accel is incorrect and should use the VehicleModel
   # The lookup table for turns should also be updated if we do this
-  a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
+  a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
   a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
   a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
 
@@ -55,9 +54,9 @@ def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05):
     v_now = speeds[0]
     a_now = accels[0]
 
-    v_target = interp(action_t, CONTROL_N_T_IDX, speeds)
+    v_target = np.interp(action_t, CONTROL_N_T_IDX, speeds)
     a_target = 2 * (v_target - v_now) / (action_t) - a_now
-    v_target_1sec = interp(action_t + 1.0, CONTROL_N_T_IDX, speeds)
+    v_target_1sec = np.interp(action_t + 1.0, CONTROL_N_T_IDX, speeds)
   else:
     v_target = 0.0
     v_target_1sec = 0.0
@@ -139,7 +138,7 @@ class LongitudinalPlanner:
     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 = clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
+      self.a_desired = np.clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
 
     # Prevent divergence, smooth in current v_ego
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
@@ -151,7 +150,7 @@ class LongitudinalPlanner:
 
     if not self.allow_throttle:
       clipped_accel_coast = max(accel_coast, accel_limits_turns[0])
-      clipped_accel_coast_interp = interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast])
+      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)
 
     if force_slow_decel:
@@ -176,7 +175,7 @@ class LongitudinalPlanner:
 
     # Interpolate 0.05 seconds and save as starting point for next iteration
     a_prev = self.a_desired
-    self.a_desired = float(interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
+    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
 
   def publish(self, sm, pm):
@@ -200,9 +199,9 @@ class LongitudinalPlanner:
     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 = a_target
+    longitudinalPlan.aTarget = float(a_target)
-    longitudinalPlan.shouldStop = should_stop
+    longitudinalPlan.shouldStop = bool(should_stop)
     longitudinalPlan.allowBrake = True
-    longitudinalPlan.allowThrottle = self.allow_throttle
+    longitudinalPlan.allowThrottle = bool(self.allow_throttle)
 
     pm.send('longitudinalPlan', plan_send)

selfdrive/controls/radard.py

@@ -1,11 +1,11 @@
 #!/usr/bin/env python3
 import math
+import numpy as np
 from collections import deque
 from typing import Any
 
 import capnp
 from cereal import messaging, log, car
-from openpilot.common.numpy_fast import interp
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process
 from openpilot.common.swaglog import cloudlog
@@ -44,7 +44,7 @@ class KalmanParams:
           0.28144091, 0.27958406, 0.27783249, 0.27617149, 0.27458948, 0.27307714,
           0.27162685, 0.27023228, 0.26888809, 0.26758976, 0.26633338, 0.26511557,
           0.26393339, 0.26278425]
-    self.K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]]
+    self.K = [[np.interp(dt, dts, K0)], [np.interp(dt, dts, K1)]]
 
 
 class Track:

selfdrive/debug/clear_dtc.py

@@ -2,8 +2,8 @@
 import sys
 import argparse
 from subprocess import check_output, CalledProcessError
+from opendbc.car.uds import UdsClient, MessageTimeoutError, SESSION_TYPE, DTC_GROUP_TYPE
 from panda import Panda
-from panda.python.uds import UdsClient, MessageTimeoutError, SESSION_TYPE, DTC_GROUP_TYPE
 
 parser = argparse.ArgumentParser(description="clear DTC status")
 parser.add_argument("addr", type=lambda x: int(x,0), nargs="?", default=0x7DF) # default is functional (broadcast) address

selfdrive/debug/debug_fw_fingerprinting_offline.py

@@ -1,9 +1,9 @@
 #!/usr/bin/env python3
 import argparse
 
+from opendbc.car import uds
 from openpilot.tools.lib.live_logreader import live_logreader
 from openpilot.tools.lib.logreader import LogReader, ReadMode
-from panda.python import uds
 
 
 def main(route: str | None, addrs: list[int]):

selfdrive/debug/hyundai_enable_radar_points.py

@@ -16,8 +16,8 @@ import argparse
 from typing import NamedTuple
 from subprocess import check_output, CalledProcessError
 
+from opendbc.car.uds import UdsClient, SESSION_TYPE, DATA_IDENTIFIER_TYPE
 from panda.python import Panda
-from panda.python.uds import UdsClient, SESSION_TYPE, DATA_IDENTIFIER_TYPE
 
 class ConfigValues(NamedTuple):
   default_config: bytes

selfdrive/debug/live_cpu_and_temp.py

@@ -1,10 +1,10 @@
 #!/usr/bin/env python3
 import argparse
+import numpy as np
 import capnp
 from collections import defaultdict
 
 from cereal.messaging import SubMaster
-from openpilot.common.numpy_fast import mean
 
 def cputime_total(ct):
   return ct.user + ct.nice + ct.system + ct.idle + ct.iowait + ct.irq + ct.softirq
@@ -49,7 +49,7 @@ if __name__ == "__main__":
 
     if sm.updated['deviceState']:
       t = sm['deviceState']
-      last_temp = mean(t.cpuTempC)
+      last_temp = np.mean(t.cpuTempC)
       last_mem = t.memoryUsagePercent
 
     if sm.updated['procLog']:
@@ -72,7 +72,7 @@ if __name__ == "__main__":
       total_times = total_times_new[:]
       busy_times = busy_times_new[:]
 
-      print(f"CPU {100.0 * mean(cores):.2f}% - RAM: {last_mem:.2f}% - Temp {last_temp:.2f}C")
+      print(f"CPU {100.0 * np.mean(cores):.2f}% - RAM: {last_mem:.2f}% - Temp {last_temp:.2f}C")
 
       if args.cpu and prev_proclog is not None and prev_proclog_t is not None:
         procs: dict[str, float] = defaultdict(float)

selfdrive/debug/read_dtc_status.py

@@ -2,9 +2,8 @@
 import sys
 import argparse
 from subprocess import check_output, CalledProcessError
+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 panda import Panda
-from panda.python.uds import UdsClient, SESSION_TYPE, DTC_REPORT_TYPE, DTC_STATUS_MASK_TYPE
-from panda.python.uds import get_dtc_num_as_str, get_dtc_status_names
 
 parser = argparse.ArgumentParser(description="read DTC status")
 parser.add_argument("addr", type=lambda x: int(x,0))

selfdrive/debug/vw_mqb_config.py

@@ -3,9 +3,9 @@
 import argparse
 import struct
 from enum import IntEnum
-from panda import Panda
+from opendbc.car.uds import UdsClient, MessageTimeoutError, NegativeResponseError, SESSION_TYPE,\
-from panda.python.uds import UdsClient, MessageTimeoutError, NegativeResponseError, SESSION_TYPE,\
   DATA_IDENTIFIER_TYPE, ACCESS_TYPE
+from panda import Panda
 from datetime import date
 
 # TODO: extend UDS library to allow custom/vendor-defined data identifiers without ignoring type checks

selfdrive/locationd/paramsd.py

@@ -8,7 +8,6 @@ import cereal.messaging as messaging
 from cereal import car, log
 from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, DT_MDL
-from openpilot.common.numpy_fast import clip
 from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
 from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
@@ -66,7 +65,7 @@ class ParamsLearner:
         # This is done to bound the road roll estimate when localizer values are invalid
         roll = 0.0
         roll_std = np.radians(10.0)
-      self.roll = clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
+      self.roll = np.clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
 
       yaw_rate_valid = msg.angularVelocityDevice.valid and self.calibrator.calib_valid
       yaw_rate_valid = yaw_rate_valid and 0 < self.yaw_rate_std < 10  # rad/s
@@ -203,11 +202,11 @@ def main():
         learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
         x = learner.kf.x
 
-      angle_offset_average = clip(math.degrees(x[States.ANGLE_OFFSET].item()),
+      angle_offset_average = np.clip(math.degrees(x[States.ANGLE_OFFSET].item()),
                                   angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA)
-      angle_offset = clip(math.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
+      angle_offset = np.clip(math.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
                           angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA)
-      roll = clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA)
+      roll = np.clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA)
       roll_std = float(P[States.ROAD_ROLL].item())
       if learner.active and learner.speed > LOW_ACTIVE_SPEED:
         # Account for the opposite signs of the yaw rates
@@ -226,9 +225,9 @@ def main():
       liveParameters.sensorValid = sensors_valid
       liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
       liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
-      liveParameters.roll = roll
+      liveParameters.roll = float(roll)
-      liveParameters.angleOffsetAverageDeg = angle_offset_average
+      liveParameters.angleOffsetAverageDeg = float(angle_offset_average)
-      liveParameters.angleOffsetDeg = angle_offset
+      liveParameters.angleOffsetDeg = float(angle_offset)
       liveParameters.valid = all((
         avg_offset_valid,
         total_offset_valid,

selfdrive/modeld/models/commonmodel.pxd

@@ -10,6 +10,7 @@ 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)
+  void cl_release_context(cl_context)
 
 cdef extern from "selfdrive/modeld/models/commonmodel.h":
   cppclass ModelFrame:

selfdrive/modeld/models/commonmodel_pyx.pyx

@@ -8,7 +8,7 @@ from libc.stdint cimport uintptr_t
 
 from msgq.visionipc.visionipc cimport cl_mem
 from msgq.visionipc.visionipc_pyx cimport VisionBuf, CLContext as BaseCLContext
-from .commonmodel cimport CL_DEVICE_TYPE_DEFAULT, cl_get_device_id, cl_create_context
+from .commonmodel cimport CL_DEVICE_TYPE_DEFAULT, cl_get_device_id, cl_create_context, cl_release_context
 from .commonmodel cimport mat3, ModelFrame as cppModelFrame, DrivingModelFrame as cppDrivingModelFrame, MonitoringModelFrame as cppMonitoringModelFrame
 
 
@@ -17,6 +17,10 @@ cdef class CLContext(BaseCLContext):
     self.device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT)
     self.context = cl_create_context(self.device_id)
 
+  def __dealloc__(self):
+    if self.context:
+      cl_release_context(self.context)
+
 cdef class CLMem:
   @staticmethod
   cdef create(void * cmem):

selfdrive/monitoring/helpers.py

@@ -1,9 +1,9 @@
 from math import atan2
+import numpy as np
 
 from cereal import car, log
 import cereal.messaging as messaging
 from openpilot.selfdrive.selfdrived.events import Events
-from openpilot.common.numpy_fast import interp
 from openpilot.common.realtime import DT_DMON
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.stat_live import RunningStatFilter
@@ -205,10 +205,10 @@ class DriverMonitoring:
     bp = model_data.meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
     k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2)
     bp_normal = max(min(bp / k1, 0.5),0)
-    self.pose.cfactor_pitch = interp(bp_normal, [0, 0.5],
+    self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5],
                                            [self.settings._POSE_PITCH_THRESHOLD_SLACK,
                                             self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD
-    self.pose.cfactor_yaw = interp(bp_normal, [0, 0.5],
+    self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5],
                                            [self.settings._POSE_YAW_THRESHOLD_SLACK,
                                             self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD
 

selfdrive/selfdrived/selfdrived.py

@@ -452,6 +452,7 @@ class SelfdriveD:
     ss.alertStatus = self.AM.current_alert.alert_status
     ss.alertType = self.AM.current_alert.alert_type
     ss.alertSound = self.AM.current_alert.audible_alert
+    ss.alertHudVisual = self.AM.current_alert.visual_alert
 
     self.pm.send('selfdriveState', ss_msg)
 

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -121,7 +121,7 @@ class Plant:
     ss.selfdriveState.personality = self.personality
     control.controlsState.forceDecel = self.force_decel
     car_state.carState.vEgo = float(self.speed)
-    car_state.carState.standstill = self.speed < 0.01
+    car_state.carState.standstill = bool(self.speed < 0.01)
     car_state.carState.vCruise = float(v_cruise * 3.6)
     car_control.carControl.orientationNED = [0., float(pitch), 0.]
 

selfdrive/test/process_replay/migration.py

@@ -1,7 +1,8 @@
 from collections import defaultdict
 from collections.abc import Callable
-import functools
 import capnp
+import functools
+import traceback
 
 from cereal import messaging, car, log
 from opendbc.car.fingerprints import MIGRATION
@@ -107,7 +108,8 @@ def migrate_longitudinalPlan(msgs):
     if msg.which() != 'longitudinalPlan':
       continue
     new_msg = msg.as_builder()
-    new_msg.longitudinalPlan.aTarget, new_msg.longitudinalPlan.shouldStop = get_accel_from_plan(msg.longitudinalPlan.speeds, msg.longitudinalPlan.accels)
+    a_target, should_stop = get_accel_from_plan(msg.longitudinalPlan.speeds, msg.longitudinalPlan.accels)
+    new_msg.longitudinalPlan.aTarget, new_msg.longitudinalPlan.shouldStop = float(a_target), bool(should_stop)
     ops.append((index, new_msg.as_reader()))
   return ops, [], []
 
@@ -424,13 +426,19 @@ def migrate_sensorEvents(msgs):
 def migrate_onroadEvents(msgs):
   ops = []
   for index, msg in msgs:
+    onroadEvents = []
+    for event in msg.onroadEventsDEPRECATED:
+      try:
+        if not str(event.name).endswith('DEPRECATED'):
+          # dict converts name enum into string representation
+          onroadEvents.append(log.OnroadEvent(**event.to_dict()))
+      except RuntimeError:  # Member was null
+        traceback.print_exc()
+
     new_msg = messaging.new_message('onroadEvents', len(msg.onroadEventsDEPRECATED))
     new_msg.valid = msg.valid
     new_msg.logMonoTime = msg.logMonoTime
-
+    new_msg.onroadEvents = onroadEvents
-    # dict converts name enum into string representation
-    new_msg.onroadEvents = [log.OnroadEvent(**event.to_dict()) for event in msg.onroadEventsDEPRECATED if
-                            not str(event.name).endswith('DEPRECATED')]
     ops.append((index, new_msg.as_reader()))
 
   return ops, [], []
@@ -441,10 +449,16 @@ def migrate_driverMonitoringState(msgs):
   ops = []
   for index, msg in msgs:
     msg = msg.as_builder()
-    # dict converts name enum into string representation
+    events = []
-    msg.driverMonitoringState.events = [log.OnroadEvent(**event.to_dict()) for event in
+    for event in msg.driverMonitoringState.eventsDEPRECATED:
-                                        msg.driverMonitoringState.eventsDEPRECATED if
+      try:
-                                        not str(event.name).endswith('DEPRECATED')]
+        if not str(event.name).endswith('DEPRECATED'):
+          # dict converts name enum into string representation
+          events.append(log.OnroadEvent(**event.to_dict()))
+      except RuntimeError:  # Member was null
+        traceback.print_exc()
+
+    msg.driverMonitoringState.events = events
     ops.append((index, msg.as_reader()))
 
   return ops, [], []

selfdrive/ui/qt/widgets/keyboard.cc

@@ -116,7 +116,7 @@ Keyboard::Keyboard(QWidget *parent) : QFrame(parent) {
     {"Q", "W", "E", "R", "T", "Y", "U", "I", "O", "P"},
     {"A", "S", "D", "F", "G", "H", "J", "K", "L"},
     {"↓", "Z", "X", "C", "V", "B", "N", "M", BACKSPACE_KEY},
-    {"123", "  ", ".", ENTER_KEY},
+    {"123", "/", "-", "  ", ".", ENTER_KEY},
   };
   main_layout->addWidget(new KeyboardLayout(this, uppercase));
 

selfdrive/ui/tests/test_ui/run.py

@@ -127,6 +127,10 @@ def setup_keyboard(click, pm: PubMaster, scroll=None):
   click(278, 970)
   click(1930, 228)
 
+def setup_keyboard_uppercase(click, pm: PubMaster, scroll=None):
+  setup_keyboard(click, pm, scroll)
+  click(200, 800)
+
 def setup_driver_camera(click, pm: PubMaster, scroll=None):
   setup_settings_device(click, pm)
   click(950, 620)
@@ -224,7 +228,8 @@ CASES = {
   "body": setup_body,
   "offroad_alert": setup_offroad_alert,
   "update_available": setup_update_available,
-  "keyboard": setup_keyboard
+  "keyboard": setup_keyboard,
+  "keyboard_uppercase": setup_keyboard_uppercase
 }
 
 CASES.update({

system/hardware/fan_controller.py

@@ -1,8 +1,8 @@
 #!/usr/bin/env python3
+import numpy as np
 from abc import ABC, abstractmethod
 
 from openpilot.common.realtime import DT_HW
-from openpilot.common.numpy_fast import interp
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.pid import PIDController
 
@@ -30,7 +30,7 @@ class TiciFanController(BaseFanController):
     error = 70 - cur_temp
     fan_pwr_out = -int(self.controller.update(
                       error=error,
-                      feedforward=interp(cur_temp, [60.0, 100.0], [0, -100])
+                      feedforward=np.interp(cur_temp, [60.0, 100.0], [0, -100])
                     ))
 
     self.last_ignition = ignition

system/loggerd/config.py

@@ -9,7 +9,7 @@ STATS_DIR_FILE_LIMIT = 10000
 STATS_SOCKET = "ipc:///tmp/stats"
 STATS_FLUSH_TIME_S = 60
 
-def get_available_percent(default=None):
+def get_available_percent(default: float) -> float:
   try:
     statvfs = os.statvfs(Paths.log_root())
     available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks
@@ -19,7 +19,7 @@ def get_available_percent(default=None):
   return available_percent
 
 
-def get_available_bytes(default=None):
+def get_available_bytes(default: int) -> int:
   try:
     statvfs = os.statvfs(Paths.log_root())
     available_bytes = statvfs.f_bavail * statvfs.f_frsize

system/loggerd/deleter.py

@@ -23,6 +23,7 @@ def has_preserve_xattr(d: str) -> bool:
 
 
 def get_preserved_segments(dirs_by_creation: list[str]) -> list[str]:
+  # skip deleting most recent N preserved segments (and their prior segment)
   preserved = []
   for n, d in enumerate(filter(has_preserve_xattr, reversed(dirs_by_creation))):
     if n == PRESERVE_COUNT:
@@ -44,15 +45,13 @@ def get_preserved_segments(dirs_by_creation: list[str]) -> list[str]:
   return preserved
 
 
-def deleter_thread(exit_event):
+def deleter_thread(exit_event: threading.Event):
   while not exit_event.is_set():
     out_of_bytes = get_available_bytes(default=MIN_BYTES + 1) < MIN_BYTES
     out_of_percent = get_available_percent(default=MIN_PERCENT + 1) < MIN_PERCENT
 
     if out_of_percent or out_of_bytes:
       dirs = listdir_by_creation(Paths.log_root())
-
-      # skip deleting most recent N preserved segments (and their prior segment)
       preserved_dirs = get_preserved_segments(dirs)
 
       # remove the earliest directory we can
@@ -64,10 +63,7 @@ def deleter_thread(exit_event):
 
         try:
           cloudlog.info(f"deleting {delete_path}")
-          if os.path.isfile(delete_path):
+          shutil.rmtree(delete_path)
-            os.remove(delete_path)
-          else:
-            shutil.rmtree(delete_path)
           break
         except OSError:
           cloudlog.exception(f"issue deleting {delete_path}")

system/loggerd/encoder/encoder.cc

@@ -14,12 +14,12 @@ VideoEncoder::VideoEncoder(const EncoderInfo &encoder_info, int in_width, int in
   pm.reset(new PubMaster(pubs));
 }
 
-void VideoEncoder::publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra,
+void VideoEncoder::publisher_publish(int segment_num, uint32_t idx, VisionIpcBufExtra &extra,
                                      unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat) {
   // broadcast packet
   MessageBuilder msg;
   auto event = msg.initEvent(true);
-  auto edat = (event.*(e->encoder_info.init_encode_data_func))();
+  auto edat = (event.*(encoder_info.init_encode_data_func))();
   auto edata = edat.initIdx();
   struct timespec ts;
   timespec_get(&ts, TIME_UTC);
@@ -27,8 +27,8 @@ void VideoEncoder::publisher_publish(VideoEncoder *e, int segment_num, uint32_t
   edata.setFrameId(extra.frame_id);
   edata.setTimestampSof(extra.timestamp_sof);
   edata.setTimestampEof(extra.timestamp_eof);
-  edata.setType(e->encoder_info.encode_type);
+  edata.setType(encoder_info.encode_type);
-  edata.setEncodeId(e->cnt++);
+  edata.setEncodeId(cnt++);
   edata.setSegmentNum(segment_num);
   edata.setSegmentId(idx);
   edata.setFlags(flags);
@@ -39,21 +39,21 @@ void VideoEncoder::publisher_publish(VideoEncoder *e, int segment_num, uint32_t
   if (flags & V4L2_BUF_FLAG_KEYFRAME) edat.setHeader(header);
 
   uint32_t bytes_size = capnp::computeSerializedSizeInWords(msg) * sizeof(capnp::word);
-  if (e->msg_cache.size() < bytes_size) {
+  if (msg_cache.size() < bytes_size) {
-    e->msg_cache.resize(bytes_size);
+    msg_cache.resize(bytes_size);
   }
-  kj::ArrayOutputStream output_stream(kj::ArrayPtr<capnp::byte>(e->msg_cache.data(), bytes_size));
+  kj::ArrayOutputStream output_stream(kj::ArrayPtr<capnp::byte>(msg_cache.data(), bytes_size));
   capnp::writeMessage(output_stream, msg);
-  e->pm->send(e->encoder_info.publish_name, e->msg_cache.data(), bytes_size);
+  pm->send(encoder_info.publish_name, msg_cache.data(), bytes_size);
 
   // Publish keyframe thumbnail
-  if ((flags & V4L2_BUF_FLAG_KEYFRAME) && e->encoder_info.thumbnail_name != NULL) {
+  if ((flags & V4L2_BUF_FLAG_KEYFRAME) && encoder_info.thumbnail_name != NULL) {
     MessageBuilder tm;
     auto thumbnail = tm.initEvent().initThumbnail();
     thumbnail.setFrameId(extra.frame_id);
     thumbnail.setTimestampEof(extra.timestamp_eof);
     thumbnail.setThumbnail(dat);
     thumbnail.setEncoding(cereal::Thumbnail::Encoding::KEYFRAME);
-    pm->send(e->encoder_info.thumbnail_name, tm);
+    pm->send(encoder_info.thumbnail_name, tm);
   }
-}
+}

system/loggerd/encoder/encoder.h

@@ -27,7 +27,7 @@ public:
   virtual void encoder_open(const char* path) = 0;
   virtual void encoder_close() = 0;
 
-  void publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra, unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat);
+  void publisher_publish(int segment_num, uint32_t idx, VisionIpcBufExtra &extra, unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat);
 
 protected:
   void publish_thumbnail(uint32_t frame_id, uint64_t timestamp_eof, kj::ArrayPtr<capnp::byte> dat);

system/loggerd/encoder/ffmpeg_encoder.cc

@@ -138,7 +138,7 @@ int FfmpegEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) {
       printf("%20s got %8d bytes flags %8x idx %4d id %8d\n", encoder_info.publish_name, pkt.size, pkt.flags, counter, extra->frame_id);
     }
 
-    publisher_publish(this, segment_num, counter, *extra,
+    publisher_publish(segment_num, counter, *extra,
       (pkt.flags & AV_PKT_FLAG_KEY) ? V4L2_BUF_FLAG_KEYFRAME : 0,
       kj::arrayPtr<capnp::byte>(pkt.data, (size_t)0), // TODO: get the header
       kj::arrayPtr<capnp::byte>(pkt.data, pkt.size));

system/loggerd/encoder/v4l_encoder.cc

@@ -133,7 +133,7 @@ void V4LEncoder::dequeue_handler(V4LEncoder *e) {
         assert(extra.timestamp_eof/1000 == ts); // stay in sync
         frame_id = extra.frame_id;
         ++idx;
-        e->publisher_publish(e, e->segment_num, idx, extra, flags, header, kj::arrayPtr<capnp::byte>(buf, bytesused));
+        e->publisher_publish(e->segment_num, idx, extra, flags, header, kj::arrayPtr<capnp::byte>(buf, bytesused));
       }
 
       if (env_debug_encoder) {

system/qcomgpsd/qcomgpsd.py

@@ -17,7 +17,7 @@ from cereal import log
 import cereal.messaging as messaging
 from openpilot.common.gpio import gpio_init, gpio_set
 from openpilot.common.retry import retry
-from openpilot.common.time import system_time_valid
+from openpilot.common.time_helpers import system_time_valid
 from openpilot.system.hardware.tici.pins import GPIO
 from openpilot.common.swaglog import cloudlog
 from openpilot.system.qcomgpsd.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv

system/timed.py

@@ -5,7 +5,7 @@ import time
 from typing import NoReturn
 
 import cereal.messaging as messaging
-from openpilot.common.time import min_date, system_time_valid
+from openpilot.common.time_helpers import min_date, system_time_valid
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.params import Params
 from openpilot.common.gps import get_gps_location_service

system/ubloxd/pigeond.py

@@ -9,7 +9,7 @@ import urllib.parse
 from datetime import datetime, UTC
 
 from cereal import messaging
-from openpilot.common.time import system_time_valid
+from openpilot.common.time_helpers import system_time_valid
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
 from openpilot.system.hardware import TICI

system/ubloxd/ublox_msg.cc

@@ -134,6 +134,7 @@ kj::Array<capnp::word> UbloxMsgParser::gen_nav_pvt(ubx_t::nav_pvt_t *msg) {
   gpsLoc.setSpeed(msg->g_speed() * 1e-03);
   gpsLoc.setBearingDeg(msg->head_mot() * 1e-5);
   gpsLoc.setHorizontalAccuracy(msg->h_acc() * 1e-03);
+  gpsLoc.setSatelliteCount(msg->num_sv());
   std::tm timeinfo = std::tm();
   timeinfo.tm_year = msg->year() - 1900;
   timeinfo.tm_mon = msg->month() - 1;

system/ui/raylib/spinner.cc

@@ -12,7 +12,7 @@ constexpr int kTextureSize = 360;
 constexpr int kFontSize = 80;
 
 int main(int argc, char *argv[]) {
-  initApp("spinner", 30);
+  App app("spinner", 30);
 
   // Turn off input buffering for std::cin
   std::cin.sync_with_stdio(false);
@@ -55,14 +55,12 @@ int main(int argc, char *argv[]) {
         bar.width *= progress / 100.0f;
         DrawRectangleRounded(bar, 0.5f, 10, RAYLIB_RAYWHITE);
       } else {
-        Vector2 textSize = MeasureTextEx(getFont(), userInput.c_str(), kFontSize, 1.0);
+        Vector2 textSize = MeasureTextEx(app.getFont(), userInput.c_str(), kFontSize, 1.0);
-        DrawTextEx(getFont(), userInput.c_str(), {center.x - textSize.x / 2, yPos}, kFontSize, 1.0, RAYLIB_WHITE);
+        DrawTextEx(app.getFont(), userInput.c_str(), {center.x - textSize.x / 2, yPos}, kFontSize, 1.0, RAYLIB_WHITE);
       }
     }
 
     EndDrawing();
   }
-
-  CloseWindow();
   return 0;
 }

system/ui/raylib/util.cc

@@ -1,6 +1,7 @@
 #include "system/ui/raylib/util.h"
 
 #include <array>
+#include <filesystem>
 
 #undef GREEN
 #undef RED
@@ -9,35 +10,16 @@
 #include "system/hardware/hw.h"
 
 constexpr std::array<const char *, static_cast<int>(FontWeight::Count)> FONT_FILE_PATHS = {
-    "../../assets/fonts/Inter-Black.ttf",
+    "../../selfdrive/assets/fonts/Inter-Black.ttf",
-    "../../assets/fonts/Inter-Bold.ttf",
+    "../../selfdrive/assets/fonts/Inter-Bold.ttf",
-    "../../assets/fonts/Inter-ExtraBold.ttf",
+    "../../selfdrive/assets/fonts/Inter-ExtraBold.ttf",
-    "../../assets/fonts/Inter-ExtraLight.ttf",
+    "../../selfdrive/assets/fonts/Inter-ExtraLight.ttf",
-    "../../assets/fonts/Inter-Medium.ttf",
+    "../../selfdrive/assets/fonts/Inter-Medium.ttf",
-    "../../assets/fonts/Inter-Regular.ttf",
+    "../../selfdrive/assets/fonts/Inter-Regular.ttf",
-    "../../assets/fonts/Inter-SemiBold.ttf",
+    "../../selfdrive/assets/fonts/Inter-SemiBold.ttf",
-    "../../assets/fonts/Inter-Thin.ttf",
+    "../../selfdrive/assets/fonts/Inter-Thin.ttf",
 };
 
-struct FontManager {
-  FontManager() {
-    for (int i = 0; i < fonts.size(); ++i) {
-      fonts[i] = LoadFontEx(FONT_FILE_PATHS[i], 120, nullptr, 250);
-    }
-  }
-
-  ~FontManager() {
-    for (auto &f : fonts) UnloadFont(f);
-  }
-
-  std::array<Font, static_cast<int>(FontWeight::Count)> fonts;
-};
-
-const Font& getFont(FontWeight weight) {
-  static FontManager font_manager;
-  return font_manager.fonts[(int)weight];
-}
-
 Texture2D LoadTextureResized(const char *fileName, int size) {
   Image img = LoadImage(fileName);
   ImageResize(&img, size, size);
@@ -45,10 +27,39 @@ Texture2D LoadTextureResized(const char *fileName, int size) {
   return texture;
 }
 
-void initApp(const char *title, int fps) {
+App *pApp = nullptr;
+
+App::App(const char *title, int fps) {
+  // Ensure the current dir matches the exectuable's directory
+  auto self_path = util::readlink("/proc/self/exe");
+  auto exe_dir = std::filesystem::path(self_path).parent_path();
+  chdir(exe_dir.c_str());
+
   Hardware::set_display_power(true);
   Hardware::set_brightness(65);
+
   // SetTraceLogLevel(LOG_NONE);
   InitWindow(2160, 1080, title);
   SetTargetFPS(fps);
+
+  // Load fonts
+  fonts_.reserve(FONT_FILE_PATHS.size());
+  for (int i = 0; i < FONT_FILE_PATHS.size(); ++i) {
+    fonts_.push_back(LoadFontEx(FONT_FILE_PATHS[i], 120, nullptr, 250));
+  }
+
+  pApp = this;
+}
+
+App::~App() {
+  for (auto &font : fonts_) {
+    UnloadFont(font);
+  }
+
+  CloseWindow();
+  pApp = nullptr;
+}
+
+const Font &App::getFont(FontWeight weight) const {
+  return fonts_[static_cast<int>(weight)];
 }

system/ui/raylib/util.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <string>
+#include <vector>
 
 #include "system/ui/raylib/raylib.h"
 
@@ -16,6 +17,17 @@ enum class FontWeight {
   Count // To represent the total number of fonts
 };
 
-void initApp(const char *title, int fps);
-const Font& getFont(FontWeight weight = FontWeight::Normal);
 Texture2D LoadTextureResized(const char *fileName, int size);
+
+class App {
+public:
+  App(const char *title, int fps);
+  ~App();
+  const Font &getFont(FontWeight weight = FontWeight::Normal) const;
+
+protected:
+  std::vector<Font> fonts_;
+};
+
+// Global pointer to the App instance
+extern App *pApp;

system/updated/updated.py

@@ -14,7 +14,7 @@ from pathlib import Path
 
 from openpilot.common.basedir import BASEDIR
 from openpilot.common.params import Params
-from openpilot.common.time import system_time_valid
+from openpilot.common.time_helpers import system_time_valid
 from openpilot.common.markdown import parse_markdown
 from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert

third_party/raylib/setup.sh

@@ -39,3 +39,8 @@ cd src
 
 make -j$(nproc) PLATFORM=$RAYLIB_PLATFORM
 sudo make install RAYLIB_INSTALL_PATH=$INSTALL_DIR RAYLIB_H_INSTALL_PATH=$INSTALL_H_DIR
+
+# this commit needs to be in line with raylib
+set -x
+RAYGUI_COMMIT="76b36b597edb70ffaf96f046076adc20d67e7827"
+wget -O $INSTALL_H_DIR/raygui.h https://raw.githubusercontent.com/raysan5/raygui/$RAYGUI_COMMIT/src/raygui.h

tools/joystick/joystick_control.py

@@ -2,10 +2,10 @@
 import os
 import argparse
 import threading
+import numpy as np
 from inputs import UnpluggedError, get_gamepad
 
 from cereal import messaging
-from openpilot.common.numpy_fast import interp, clip
 from openpilot.common.params import Params
 from openpilot.common.realtime import Ratekeeper
 from openpilot.system.hardware import HARDWARE
@@ -34,7 +34,7 @@ class Keyboard:
     elif key in self.axes_map:
       axis = self.axes_map[key]
       incr = self.axis_increment if key in ['w', 'a'] else -self.axis_increment
-      self.axes_values[axis] = clip(self.axes_values[axis] + incr, -1, 1)
+      self.axes_values[axis] = np.clip(self.axes_values[axis] + incr, -1, 1)
     else:
       return False
     return True
@@ -83,7 +83,7 @@ class Joystick:
       self.max_axis_value[event[0]] = max(event[1], self.max_axis_value[event[0]])
       self.min_axis_value[event[0]] = min(event[1], self.min_axis_value[event[0]])
 
-      norm = -interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.])
+      norm = -np.interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.])
       norm = norm if abs(norm) > 0.03 else 0.  # center can be noisy, deadzone of 3%
       self.axes_values[event[0]] = EXPO * norm ** 3 + (1 - EXPO) * norm  # less action near center for fine control
     else:

tools/joystick/joystickd.py

@@ -1,9 +1,9 @@
 #!/usr/bin/env python3
 
 import math
+import numpy as np
 
 from cereal import messaging, car
-from openpilot.common.numpy_fast import clip
 from openpilot.common.realtime import DT_CTRL, Ratekeeper
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
@@ -45,13 +45,13 @@ def joystickd_thread():
       joystick_axes = [0.0, 0.0]
 
     if CC.longActive:
-      actuators.accel = 4.0 * clip(joystick_axes[0], -1, 1)
+      actuators.accel = 4.0 * np.clip(joystick_axes[0], -1, 1)
 
     if CC.latActive:
       max_curvature = MAX_LAT_ACCEL / max(sm['carState'].vEgo ** 2, 5)
       max_angle = math.degrees(VM.get_steer_from_curvature(max_curvature, sm['carState'].vEgo, sm['liveParameters'].roll))
 
-      actuators.steer = clip(joystick_axes[1], -1, 1)
+      actuators.steer = np.clip(joystick_axes[1], -1, 1)
       actuators.steeringAngleDeg, actuators.curvature = actuators.steer * max_angle, actuators.steer * -max_curvature
 
     pm.send('carControl', cc_msg)

tools/plotjuggler/layouts/locationd_debug.xml

@@ -0,0 +1,100 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget name="Main Window" parent="main_window">
+  <Tab tab_name="tab1" containers="1">
+   <Container>
+    <DockSplitter sizes="0.166588;0.167062;0.166113;0.166588;0.167062;0.166588" orientation="-" count="6">
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range right="2280.128382" left="0.000000" top="1.025000" bottom="-0.025000"/>
+       <limitY/>
+       <curve color="#ff7f0e" name="/livePose/inputsOK"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range right="2280.128382" left="0.000000" top="14.542814" bottom="-5.586039"/>
+       <limitY/>
+       <curve color="#f14cc1" name="/accelerometer/acceleration/v/0"/>
+       <curve color="#9467bd" name="/accelerometer/acceleration/v/1"/>
+       <curve color="#17becf" name="/accelerometer/acceleration/v/2"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range right="2280.128382" left="0.000000" top="0.988911" bottom="-0.745939"/>
+       <limitY/>
+       <curve color="#d62728" name="/gyroscope/gyroUncalibrated/v/0"/>
+       <curve color="#1ac938" name="/gyroscope/gyroUncalibrated/v/1"/>
+       <curve color="#ff7f0e" name="/gyroscope/gyroUncalibrated/v/2"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range right="2280.128382" left="0.000000" top="1.025000" bottom="-0.025000"/>
+       <limitY/>
+       <curve color="#17becf" name="/accelerometer/__valid"/>
+       <curve color="#bcbd22" name="/gyroscope/__valid"/>
+       <curve color="#f14cc1" name="/carState/__valid"/>
+       <curve color="#1ac938" name="/liveCalibration/__valid"/>
+       <curve color="#9467bd" name="/cameraOdometry/__valid"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range right="2280.128382" left="0.000000" top="1000000000.292252" bottom="999999999.735447"/>
+       <limitY/>
+       <curve color="#1f77b4" name="/gyroscope/__logMonoTime">
+        <transform alias="/gyroscope/__logMonoTime[Derivative]" name="Derivative">
+         <options lineEdit="1.0" radioChecked="radioActual"/>
+        </transform>
+       </curve>
+       <curve color="#d62728" name="/accelerometer/__logMonoTime">
+        <transform alias="/accelerometer/__logMonoTime[Derivative]" name="Derivative">
+         <options lineEdit="1.0" radioChecked="radioActual"/>
+        </transform>
+       </curve>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range right="2280.128382" left="0.000000" top="20790107743.932232" bottom="-529653831.495853"/>
+       <limitY/>
+       <curve color="#bcbd22" name="/accelerometer/timestamp">
+        <transform alias="/accelerometer/timestamp[Derivative]" name="Derivative">
+         <options lineEdit="1.0" radioChecked="radioActual"/>
+        </transform>
+       </curve>
+       <curve color="#1f77b4" name="/gyroscope/timestamp">
+        <transform alias="/gyroscope/timestamp[Derivative]" name="Derivative">
+         <options lineEdit="1.0" radioChecked="radioActual"/>
+        </transform>
+       </curve>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <currentTabIndex index="0"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="1"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <Plugins>
+  <plugin ID="DataLoad CSV">
+   <default delimiter="0" time_axis=""/>
+  </plugin>
+  <plugin ID="DataLoad Rlog"/>
+  <plugin ID="DataLoad ULog"/>
+  <plugin ID="Cereal Subscriber"/>
+  <plugin ID="UDP Server"/>
+  <plugin ID="ZMQ Subscriber"/>
+  <plugin ID="Fast Fourier Transform"/>
+  <plugin ID="Quaternion to RPY"/>
+  <plugin ID="CSV Exporter"/>
+ </Plugins>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+</root>
+

tools/replay/lib/ui_helpers.py

@@ -0,0 +1,226 @@
+import itertools
+from typing import Any
+
+import matplotlib.pyplot as plt
+import numpy as np
+import pygame
+
+from matplotlib.backends.backend_agg import FigureCanvasAgg
+
+from openpilot.common.transformations.camera import get_view_frame_from_calib_frame
+from openpilot.selfdrive.controls.radard import RADAR_TO_CAMERA
+
+
+RED = (255, 0, 0)
+GREEN = (0, 255, 0)
+BLUE = (0, 0, 255)
+YELLOW = (255, 255, 0)
+BLACK = (0, 0, 0)
+WHITE = (255, 255, 255)
+
+class UIParams:
+  lidar_x, lidar_y, lidar_zoom = 384, 960, 6
+  lidar_car_x, lidar_car_y = lidar_x / 2., lidar_y / 1.1
+  car_hwidth = 1.7272 / 2 * lidar_zoom
+  car_front = 2.6924 * lidar_zoom
+  car_back = 1.8796 * lidar_zoom
+  car_color = 110
+UP = UIParams
+
+METER_WIDTH = 20
+
+class Calibration:
+  def __init__(self, num_px, rpy, intrinsic, calib_scale):
+    self.intrinsic = intrinsic
+    self.extrinsics_matrix = get_view_frame_from_calib_frame(rpy[0], rpy[1], rpy[2], 0.0)[:,:3]
+    self.zoom = calib_scale
+
+  def car_space_to_ff(self, x, y, z):
+    car_space_projective = np.column_stack((x, y, z)).T
+
+    ep = self.extrinsics_matrix.dot(car_space_projective)
+    kep = self.intrinsic.dot(ep)
+    return (kep[:-1, :] / kep[-1, :]).T
+
+  def car_space_to_bb(self, x, y, z):
+    pts = self.car_space_to_ff(x, y, z)
+    return pts / self.zoom
+
+
+_COLOR_CACHE : dict[tuple[int, int, int], Any] = {}
+def find_color(lidar_surface, color):
+  if color in _COLOR_CACHE:
+    return _COLOR_CACHE[color]
+  tcolor = 0
+  ret = 255
+  for x in lidar_surface.get_palette():
+    if x[0:3] == color:
+      ret = tcolor
+      break
+    tcolor += 1
+  _COLOR_CACHE[color] = ret
+  return ret
+
+
+def to_topdown_pt(y, x):
+  px, py = x * UP.lidar_zoom + UP.lidar_car_x, -y * UP.lidar_zoom + UP.lidar_car_y
+  if px > 0 and py > 0 and px < UP.lidar_x and py < UP.lidar_y:
+    return int(px), int(py)
+  return -1, -1
+
+
+def draw_path(path, color, img, calibration, top_down, lid_color=None, z_off=0):
+  x, y, z = np.asarray(path.x), np.asarray(path.y), np.asarray(path.z) + z_off
+  pts = calibration.car_space_to_bb(x, y, z)
+  pts = np.round(pts).astype(int)
+
+  # draw lidar path point on lidar
+  # find color in 8 bit
+  if lid_color is not None and top_down is not None:
+    tcolor = find_color(top_down[0], lid_color)
+    for i in range(len(x)):
+      px, py = to_topdown_pt(x[i], y[i])
+      if px != -1:
+        top_down[1][px, py] = tcolor
+
+  height, width = img.shape[:2]
+  for x, y in pts:
+    if 1 < x < width - 1 and 1 < y < height - 1:
+      for a, b in itertools.permutations([-1, 0, -1], 2):
+        img[y + a, x + b] = color
+
+
+def init_plots(arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles):
+  color_palette = { "r": (1, 0, 0),
+                    "g": (0, 1, 0),
+                    "b": (0, 0, 1),
+                    "k": (0, 0, 0),
+                    "y": (1, 1, 0),
+                    "p": (0, 1, 1),
+                    "m": (1, 0, 1)}
+
+  dpi = 90
+  fig = plt.figure(figsize=(575 / dpi, 600 / dpi), dpi=dpi)
+  canvas = FigureCanvasAgg(fig)
+
+  fig.set_facecolor((0.2, 0.2, 0.2))
+
+  axs = []
+  for pn in range(len(plot_ylims)):
+    ax = fig.add_subplot(len(plot_ylims), 1, len(axs)+1)
+    ax.set_xlim(plot_xlims[pn][0], plot_xlims[pn][1])
+    ax.set_ylim(plot_ylims[pn][0], plot_ylims[pn][1])
+    ax.patch.set_facecolor((0.4, 0.4, 0.4))
+    axs.append(ax)
+
+  plots, idxs, plot_select = [], [], []
+  for i, pl_list in enumerate(plot_names):
+    for j, item in enumerate(pl_list):
+      plot, = axs[i].plot(arr[:, name_to_arr_idx[item]],
+                          label=item,
+                          color=color_palette[plot_colors[i][j]],
+                          linestyle=plot_styles[i][j])
+      plots.append(plot)
+      idxs.append(name_to_arr_idx[item])
+      plot_select.append(i)
+    axs[i].set_title(", ".join(f"{nm} ({cl})"
+                               for (nm, cl) in zip(pl_list, plot_colors[i], strict=False)), fontsize=10)
+    axs[i].tick_params(axis="x", colors="white")
+    axs[i].tick_params(axis="y", colors="white")
+    axs[i].title.set_color("white")
+
+    if i < len(plot_ylims) - 1:
+      axs[i].set_xticks([])
+
+  canvas.draw()
+
+  def draw_plots(arr):
+    for ax in axs:
+      ax.draw_artist(ax.patch)
+    for i in range(len(plots)):
+      plots[i].set_ydata(arr[:, idxs[i]])
+      axs[plot_select[i]].draw_artist(plots[i])
+
+    raw_data = canvas.buffer_rgba()
+    plot_surface = pygame.image.frombuffer(raw_data, canvas.get_width_height(), "RGBA").convert()
+    return plot_surface
+
+  return draw_plots
+
+
+def pygame_modules_have_loaded():
+  return pygame.display.get_init() and pygame.font.get_init()
+
+
+def plot_model(m, img, calibration, top_down):
+  if calibration is None or top_down is None:
+    return
+
+  for lead in m.leadsV3:
+    if lead.prob < 0.5:
+      continue
+
+    x, y = lead.x[0], lead.y[0]
+    x_std = lead.xStd[0]
+    x -= RADAR_TO_CAMERA
+
+    _, py_top = to_topdown_pt(x + x_std, y)
+    px, py_bottom = to_topdown_pt(x - x_std, y)
+    top_down[1][int(round(px - 4)):int(round(px + 4)), py_top:py_bottom] = find_color(top_down[0], YELLOW)
+
+  for path, prob, _ in zip(m.laneLines, m.laneLineProbs, m.laneLineStds, strict=True):
+    color = (0, int(255 * prob), 0)
+    draw_path(path, color, img, calibration, top_down, YELLOW)
+
+  for edge, std in zip(m.roadEdges, m.roadEdgeStds, strict=True):
+    prob = max(1 - std, 0)
+    color = (int(255 * prob), 0, 0)
+    draw_path(edge, color, img, calibration, top_down, RED)
+
+  color = (255, 0, 0)
+  draw_path(m.position, color, img, calibration, top_down, RED, 1.22)
+
+
+def plot_lead(rs, top_down):
+  for lead in [rs.leadOne, rs.leadTwo]:
+    if not lead.status:
+      continue
+
+    x = lead.dRel
+    px_left, py = to_topdown_pt(x, -10)
+    px_right, _ = to_topdown_pt(x, 10)
+    top_down[1][px_left:px_right, py] = find_color(top_down[0], RED)
+
+
+def maybe_update_radar_points(lt, lid_overlay):
+  ar_pts = []
+  if lt is not None:
+    ar_pts = {}
+    for track in lt:
+      ar_pts[track.trackId] = [track.dRel, track.yRel, track.vRel, track.aRel]
+  for ids, pt in ar_pts.items():
+    # negative here since radar is left positive
+    px, py = to_topdown_pt(pt[0], -pt[1])
+    if px != -1:
+      color = 255
+      if int(ids) == 1:
+        lid_overlay[px - 2:px + 2, py - 10:py + 10] = 100
+      else:
+        lid_overlay[px - 2:px + 2, py - 2:py + 2] = color
+
+def get_blank_lid_overlay(UP):
+  lid_overlay = np.zeros((UP.lidar_x, UP.lidar_y), 'uint8')
+  # Draw the car.
+  lid_overlay[int(round(UP.lidar_car_x - UP.car_hwidth)):int(
+    round(UP.lidar_car_x + UP.car_hwidth)), int(round(UP.lidar_car_y -
+                                                      UP.car_front))] = UP.car_color
+  lid_overlay[int(round(UP.lidar_car_x - UP.car_hwidth)):int(
+    round(UP.lidar_car_x + UP.car_hwidth)), int(round(UP.lidar_car_y +
+                                                      UP.car_back))] = UP.car_color
+  lid_overlay[int(round(UP.lidar_car_x - UP.car_hwidth)), int(
+    round(UP.lidar_car_y - UP.car_front)):int(round(
+      UP.lidar_car_y + UP.car_back))] = UP.car_color
+  lid_overlay[int(round(UP.lidar_car_x + UP.car_hwidth)), int(
+    round(UP.lidar_car_y - UP.car_front)):int(round(
+      UP.lidar_car_y + UP.car_back))] = UP.car_color
+  return lid_overlay

tools/replay/ui.py

@@ -0,0 +1,238 @@
+#!/usr/bin/env python3
+import argparse
+import os
+import sys
+
+import cv2
+import numpy as np
+import pygame
+
+import cereal.messaging as messaging
+from openpilot.common.numpy_fast import clip
+from openpilot.common.basedir import BASEDIR
+from openpilot.common.transformations.camera import DEVICE_CAMERAS
+from openpilot.tools.replay.lib.ui_helpers import (UP,
+                                         BLACK, GREEN,
+                                         YELLOW, Calibration,
+                                         get_blank_lid_overlay, init_plots,
+                                         maybe_update_radar_points, plot_lead,
+                                         plot_model,
+                                         pygame_modules_have_loaded)
+from msgq.visionipc import VisionIpcClient, VisionStreamType
+
+os.environ['BASEDIR'] = BASEDIR
+
+ANGLE_SCALE = 5.0
+
+def ui_thread(addr):
+  cv2.setNumThreads(1)
+  pygame.init()
+  pygame.font.init()
+  assert pygame_modules_have_loaded()
+
+  disp_info = pygame.display.Info()
+  max_height = disp_info.current_h
+
+  hor_mode = os.getenv("HORIZONTAL") is not None
+  hor_mode = True if max_height < 960+300 else hor_mode
+
+  if hor_mode:
+    size = (640+384+640, 960)
+    write_x = 5
+    write_y = 680
+  else:
+    size = (640+384, 960+300)
+    write_x = 645
+    write_y = 970
+
+  pygame.display.set_caption("openpilot debug UI")
+  screen = pygame.display.set_mode(size, pygame.DOUBLEBUF)
+
+  alert1_font = pygame.font.SysFont("arial", 30)
+  alert2_font = pygame.font.SysFont("arial", 20)
+  info_font = pygame.font.SysFont("arial", 15)
+
+  camera_surface = pygame.surface.Surface((640, 480), 0, 24).convert()
+  top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y), 0, 8)
+
+  sm = messaging.SubMaster(['carState', 'longitudinalPlan', 'carControl', 'radarState', 'liveCalibration', 'controlsState',
+                            'selfdriveState', 'liveTracks', 'modelV2', 'liveParameters', 'roadCameraState'], addr=addr)
+
+  img = np.zeros((480, 640, 3), dtype='uint8')
+  imgff = None
+  num_px = 0
+  calibration = None
+
+  lid_overlay_blank = get_blank_lid_overlay(UP)
+
+  # plots
+  name_to_arr_idx = { "gas": 0,
+                      "computer_gas": 1,
+                      "user_brake": 2,
+                      "computer_brake": 3,
+                      "v_ego": 4,
+                      "v_pid": 5,
+                      "angle_steers_des": 6,
+                      "angle_steers": 7,
+                      "angle_steers_k": 8,
+                      "steer_torque": 9,
+                      "v_override": 10,
+                      "v_cruise": 11,
+                      "a_ego": 12,
+                      "a_target": 13}
+
+  plot_arr = np.zeros((100, len(name_to_arr_idx.values())))
+
+  plot_xlims = [(0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0])]
+  plot_ylims = [(-0.1, 1.1), (-ANGLE_SCALE, ANGLE_SCALE), (0., 75.), (-3.0, 2.0)]
+  plot_names = [["gas", "computer_gas", "user_brake", "computer_brake"],
+                ["angle_steers", "angle_steers_des", "angle_steers_k", "steer_torque"],
+                ["v_ego", "v_override", "v_pid", "v_cruise"],
+                ["a_ego", "a_target"]]
+  plot_colors = [["b", "b", "g", "r", "y"],
+                 ["b", "g", "y", "r"],
+                 ["b", "g", "r", "y"],
+                 ["b", "r"]]
+  plot_styles = [["-", "-", "-", "-", "-"],
+                 ["-", "-", "-", "-"],
+                 ["-", "-", "-", "-"],
+                 ["-", "-"]]
+
+  draw_plots = init_plots(plot_arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles)
+
+  vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
+  while True:
+    for event in pygame.event.get():
+      if event.type == pygame.QUIT:
+        pygame.quit()
+        sys.exit()
+
+    screen.fill((64, 64, 64))
+    lid_overlay = lid_overlay_blank.copy()
+    top_down = top_down_surface, lid_overlay
+
+    # ***** frame *****
+    if not vipc_client.is_connected():
+      vipc_client.connect(True)
+
+    yuv_img_raw = vipc_client.recv()
+    if yuv_img_raw is None or not yuv_img_raw.data.any():
+      continue
+
+    sm.update(0)
+
+    camera = DEVICE_CAMERAS[("tici", str(sm['roadCameraState'].sensor))]
+
+    imgff = np.frombuffer(yuv_img_raw.data, dtype=np.uint8).reshape((len(yuv_img_raw.data) // vipc_client.stride, vipc_client.stride))
+    num_px = vipc_client.width * vipc_client.height
+    rgb = cv2.cvtColor(imgff[:vipc_client.height * 3 // 2, :vipc_client.width], cv2.COLOR_YUV2RGB_NV12)
+
+    qcam = "QCAM" in os.environ
+    bb_scale = (528 if qcam else camera.fcam.width) / 640.
+    calib_scale = camera.fcam.width / 640.
+    zoom_matrix = np.asarray([
+        [bb_scale, 0., 0.],
+        [0., bb_scale, 0.],
+        [0., 0., 1.]])
+    cv2.warpAffine(rgb, zoom_matrix[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP)
+
+    intrinsic_matrix = camera.fcam.intrinsics
+
+    w = sm['controlsState'].lateralControlState.which()
+    if w == 'lqrStateDEPRECATED':
+      angle_steers_k = sm['controlsState'].lateralControlState.lqrStateDEPRECATED.steeringAngleDeg
+    elif w == 'indiState':
+      angle_steers_k = sm['controlsState'].lateralControlState.indiState.steeringAngleDeg
+    else:
+      angle_steers_k = np.inf
+
+    plot_arr[:-1] = plot_arr[1:]
+    plot_arr[-1, name_to_arr_idx['angle_steers']] = sm['carState'].steeringAngleDeg
+    plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm['carControl'].actuators.steeringAngleDeg
+    plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k
+    plot_arr[-1, name_to_arr_idx['gas']] = sm['carState'].gas
+    # TODO gas is deprecated
+    plot_arr[-1, name_to_arr_idx['computer_gas']] = clip(sm['carControl'].actuators.accel/4.0, 0.0, 1.0)
+    plot_arr[-1, name_to_arr_idx['user_brake']] = sm['carState'].brake
+    plot_arr[-1, name_to_arr_idx['steer_torque']] = sm['carControl'].actuators.steer * ANGLE_SCALE
+    # TODO brake is deprecated
+    plot_arr[-1, name_to_arr_idx['computer_brake']] = clip(-sm['carControl'].actuators.accel/4.0, 0.0, 1.0)
+    plot_arr[-1, name_to_arr_idx['v_ego']] = sm['carState'].vEgo
+    plot_arr[-1, name_to_arr_idx['v_cruise']] = sm['carState'].cruiseState.speed
+    plot_arr[-1, name_to_arr_idx['a_ego']] = sm['carState'].aEgo
+
+    if len(sm['longitudinalPlan'].accels):
+      plot_arr[-1, name_to_arr_idx['a_target']] = sm['longitudinalPlan'].accels[0]
+
+    if sm.recv_frame['modelV2']:
+      plot_model(sm['modelV2'], img, calibration, top_down)
+
+    if sm.recv_frame['radarState']:
+      plot_lead(sm['radarState'], top_down)
+
+    # draw all radar points
+    maybe_update_radar_points(sm['liveTracks'].points, top_down[1])
+
+    if sm.updated['liveCalibration'] and num_px:
+      rpyCalib = np.asarray(sm['liveCalibration'].rpyCalib)
+      calibration = Calibration(num_px, rpyCalib, intrinsic_matrix, calib_scale)
+
+    # *** blits ***
+    pygame.surfarray.blit_array(camera_surface, img.swapaxes(0, 1))
+    screen.blit(camera_surface, (0, 0))
+
+    # display alerts
+    alert_line1 = alert1_font.render(sm['selfdriveState'].alertText1, True, (255, 0, 0))
+    alert_line2 = alert2_font.render(sm['selfdriveState'].alertText2, True, (255, 0, 0))
+    screen.blit(alert_line1, (180, 150))
+    screen.blit(alert_line2, (180, 190))
+
+    if hor_mode:
+      screen.blit(draw_plots(plot_arr), (640+384, 0))
+    else:
+      screen.blit(draw_plots(plot_arr), (0, 600))
+
+    pygame.surfarray.blit_array(*top_down)
+    screen.blit(top_down[0], (640, 0))
+
+    SPACING = 25
+
+    lines = [
+      info_font.render("ENABLED", True, GREEN if sm['selfdriveState'].enabled else BLACK),
+      info_font.render("SPEED: " + str(round(sm['carState'].vEgo, 1)) + " m/s", True, YELLOW),
+      info_font.render("LONG CONTROL STATE: " + str(sm['controlsState'].longControlState), True, YELLOW),
+      info_font.render("LONG MPC SOURCE: " + str(sm['longitudinalPlan'].longitudinalPlanSource), True, YELLOW),
+      None,
+      info_font.render("ANGLE OFFSET (AVG): " + str(round(sm['liveParameters'].angleOffsetAverageDeg, 2)) + " deg", True, YELLOW),
+      info_font.render("ANGLE OFFSET (INSTANT): " + str(round(sm['liveParameters'].angleOffsetDeg, 2)) + " deg", True, YELLOW),
+      info_font.render("STIFFNESS: " + str(round(sm['liveParameters'].stiffnessFactor * 100., 2)) + " %", True, YELLOW),
+      info_font.render("STEER RATIO: " + str(round(sm['liveParameters'].steerRatio, 2)), True, YELLOW)
+    ]
+
+    for i, line in enumerate(lines):
+      if line is not None:
+        screen.blit(line, (write_x, write_y + i * SPACING))
+
+    # this takes time...vsync or something
+    pygame.display.flip()
+
+def get_arg_parser():
+  parser = argparse.ArgumentParser(
+    description="Show replay data in a UI.",
+    formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+
+  parser.add_argument("ip_address", nargs="?", default="127.0.0.1",
+                      help="The ip address on which to receive zmq messages.")
+
+  parser.add_argument("--frame-address", default=None,
+                      help="The frame address (fully qualified ZMQ endpoint for frames) on which to receive zmq messages.")
+  return parser
+
+if __name__ == "__main__":
+  args = get_arg_parser().parse_args(sys.argv[1:])
+
+  if args.ip_address != "127.0.0.1":
+    os.environ["ZMQ"] = "1"
+    messaging.reset_context()
+
+  ui_thread(args.ip_address)

tools/sim/bridge/common.py

@@ -1,6 +1,7 @@
 import signal
 import threading
 import functools
+import numpy as np
 
 from collections import namedtuple
 from enum import Enum
@@ -9,7 +10,6 @@ from abc import ABC, abstractmethod
 
 from opendbc.car.honda.values import CruiseButtons
 from openpilot.common.params import Params
-from openpilot.common.numpy_fast import clip
 from openpilot.common.realtime import Ratekeeper
 from openpilot.selfdrive.test.helpers import set_params_enabled
 from openpilot.tools.sim.lib.common import SimulatorState, World
@@ -173,8 +173,8 @@ Ignition: {self.simulator_state.ignition} Engaged: {self.simulator_state.is_enga
       self.simulator_state.is_engaged = self.simulated_car.sm['selfdriveState'].active
 
       if self.simulator_state.is_engaged:
-        throttle_op = clip(self.simulated_car.sm['carControl'].actuators.accel / 1.6, 0.0, 1.0)
+        throttle_op = np.clip(self.simulated_car.sm['carControl'].actuators.accel / 1.6, 0.0, 1.0)
-        brake_op = clip(-self.simulated_car.sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
+        brake_op = np.clip(-self.simulated_car.sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
         steer_op = self.simulated_car.sm['carControl'].actuators.steeringAngleDeg
 
         self.past_startup_engaged = True