remove old mapd, sla, ssc

This reverts commit 1eb82fcc

.github/workflows/sunnypilot-master-dev-prep.yaml

@@ -174,6 +174,24 @@ jobs:
           echo '    pushurl = ${{ env.LFS_PUSH_URL }}' >> .lfsconfig
           echo '    locksverify = false' >> .lfsconfig
 
+      - name: Restore workflows from source
+        run: |
+          TARGET_BRANCH="${{ inputs.target_branch || env.DEFAULT_TARGET_BRANCH }}"
+          SOURCE_BRANCH="${{ inputs.source_branch || env.DEFAULT_SOURCE_BRANCH }}"
+
+          # Ensure we are on the target branch
+          git checkout $TARGET_BRANCH
+
+          echo "Restoring .github/workflows from $SOURCE_BRANCH"
+          git checkout origin/$SOURCE_BRANCH -- .github/workflows
+
+          if ! git diff --cached --quiet; then
+            echo "Workflows differ. Committing restoration."
+            git commit -m "chore: restore .github/workflows from $SOURCE_BRANCH"
+          else
+            echo "Workflows match $SOURCE_BRANCH."
+          fi
+
       - uses: actions/create-github-app-token@v2
         id: ci-token
         with:

README.md

@@ -11,66 +11,10 @@ Join the official sunnypilot community forum to stay up to date with all the lat
 https://docs.sunnypilot.ai/ is your one stop shop for everything from features to installation to FAQ about the sunnypilot
 
 ## 🚘 Running on a dedicated device in a car
-* A supported device to run this software
-    * a [comma three](https://comma.ai/shop/products/three) or a [C3X](https://comma.ai/shop/comma-3x)
-* This software
-* One of [the 325+ supported cars](https://github.com/sunnypilot/sunnypilot/blob/master/docs/CARS.md). We support Honda, Toyota, Hyundai, Nissan, Kia, Chrysler, Lexus, Acura, Audi, VW, Ford, and more. If your car is not supported but has adaptive cruise control and lane-keeping assist, it's likely able to run sunnypilot.
-* A [car harness](https://comma.ai/shop/products/car-harness) to connect to your car
-
-Detailed instructions for [how to mount the device in a car](https://comma.ai/setup).
+First, check out this list of items you'll need to [get started](https://community.sunnypilot.ai/t/getting-started-using-sunnypilot-in-your-supported-car/251).
 
 ## Installation
-Please refer to [Recommended Branches](#recommended-branches) to find your preferred/supported branch. This guide will assume you want to install the latest `staging` branch.
-
-### If you want to use our newest branches (our rewrite)
-> [!TIP]
->You can see the rewrite state on our [rewrite project board](https://github.com/orgs/sunnypilot/projects/2), and to install the new branches, you can use the following links
-
-* sunnypilot not installed or you installed a version before 0.8.17?
-  1. [Factory reset/uninstall](https://github.com/commaai/openpilot/wiki/FAQ#how-can-i-reset-the-device) the previous software if you have another software/fork installed.
-  2. After factory reset/uninstall and upon reboot, select `Custom Software` when given the option.
-  3. Input the installation URL per [Recommended Branches](#recommended-branches). Example: ```https://staging.sunnypilot.ai```.
-  4. Complete the rest of the installation following the onscreen instructions.
-
-* sunnypilot already installed and you installed a version after 0.8.17?
-  1. On the comma three/3X, go to `Settings` ▶️ `Software`.
-  2. At the `Download` option, press `CHECK`. This will fetch the list of latest branches from sunnypilot.
-  3. At the `Target Branch` option, press `SELECT` to open the Target Branch selector.
-  4. Scroll to select the desired branch per  Recommended Branches (see below). Example: `staging`
-
-### Recommended Branches
-|      Branch     |                 Installation URL              |
-|:---------------:|:---------------------------------------------:|
-| `release`       | `https://release.sunnypilot.ai`               |
-| `staging`       | `https://staging.sunnypilot.ai`               |
-|   `dev`         | `https://dev.sunnypilot.ai`                   |
-| `custom-branch` | `https://install.sunnypilot.ai/{branch_name}` |
-
-> [!TIP]
-> You can use sunnypilot/targetbranch as an install URL. Example: 'sunnypilot/staging'.
-
-> [!NOTE]
-> Do you require further assistance with software installation? Join the [sunnypilot community forum](https://community.sunnypilot.ai/new-topic?category=general/qa) and create a topic in the General/Q&A Category channel.
-
-
-<details>
-
-<summary>Older legacy branches</summary>
-
-### If you want to use our older legacy branches (*not recommended*)
-
-> [**IMPORTANT**]
-> It is recommended to [re-flash AGNOS](https://flash.comma.ai/) if you intend to downgrade from the new branches.
-> You can still restore the latest sunnylink backup made on the old branches.
-
-|    Branch    |         Installation URL         |
-|:------------:|:--------------------------------:|
-| `release-c3` | https://release-c3.sunnypilot.ai |
-| `staging-c3` | https://staging-c3.sunnypilot.ai |
-|   `dev-c3`   | https://dev-c3.sunnypilot.ai     |
-
-</details>
-
+Next, refer to the sunnypilot community forum for [installation instructions](https://community.sunnypilot.ai/t/read-before-installing-sunnypilot/254), as well as a complete list of [Recommended Branch Installations](https://community.sunnypilot.ai/t/recommended-branch-installations/235).
 
 ## 🎆 Pull Requests
 We welcome both pull requests and issues on GitHub. Bug fixes are encouraged.

common/params_keys.h

@@ -145,6 +145,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"CarParamsSPPersistent", {PERSISTENT, BYTES}},
     {"CarPlatformBundle", {PERSISTENT | BACKUP, JSON}},
     {"ChevronInfo", {PERSISTENT | BACKUP, INT, "4"}},
+    {"CompletedSunnylinkConsentVersion", {PERSISTENT, STRING, "0"}},
     {"CustomAccIncrementsEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"CustomAccLongPressIncrement", {PERSISTENT | BACKUP, INT, "5"}},
     {"CustomAccShortPressIncrement", {PERSISTENT | BACKUP, INT, "1"}},
@@ -154,6 +155,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
     {"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}},
+    {"HasAcceptedTermsSP", {PERSISTENT, STRING, "0"}},
     {"HideVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"IntelligentCruiseButtonManagement", {PERSISTENT | BACKUP , BOOL}},
     {"InteractivityTimeout", {PERSISTENT | BACKUP, INT, "0"}},
@@ -219,11 +221,13 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
 
     // sunnypilot model params
+    {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
     {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},
     {"LagdToggleDelay", {PERSISTENT | BACKUP, FLOAT, "0.2"}},
     {"LagdValueCache", {PERSISTENT, FLOAT, "0.2"}},
     {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}},
+    {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
 
     // mapd
     {"MapAdvisorySpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT}},

selfdrive/car/card.py

@@ -217,7 +217,6 @@ class Car:
     if can_rcv_valid and REPLAY:
       self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
 
-    self.v_cruise_helper.update_speed_limit_assist(self.is_metric, self.sm['longitudinalPlanSP'])
     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

selfdrive/car/cruise.py

@@ -53,7 +53,6 @@ class VCruiseHelper(VCruiseHelperSP):
       if not self.CP.pcmCruise or (not self.CP_SP.pcmCruiseSpeed and _enabled):
         # if stock cruise is completely disabled, then we can use our own set speed logic
         self._update_v_cruise_non_pcm(CS, _enabled, is_metric)
-        self.update_speed_limit_assist_v_cruise_non_pcm()
         self.v_cruise_cluster_kph = self.v_cruise_kph
         self.update_button_timers(CS, enabled)
       else:
@@ -105,12 +104,6 @@ class VCruiseHelper(VCruiseHelperSP):
     if not self.button_change_states[button_type]["enabled"]:
       return
 
-    # Speed Limit Assist for Non PCM long cars.
-    # True: Disallow set speed changes when user confirmed the target set speed during preActive state
-    # False: Allow set speed changes as SLA is not requesting user confirmation
-    if self.update_speed_limit_assist_pre_active_confirmed(button_type):
-      return
-
     long_press, v_cruise_delta = VCruiseHelperSP.update_v_cruise_delta(self, long_press, v_cruise_delta)
     if long_press and self.v_cruise_kph % v_cruise_delta != 0:  # partial interval
       self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta

selfdrive/monitoring/helpers.py

@@ -449,7 +449,6 @@ class DriverMonitoring:
       rpyCalib = [0., 0., 0.]
     else:
       highway_speed = sm['carState'].vEgo
-      # TODO-SP: unit test to assert both control checks are always present
       enabled = sm['selfdriveState'].enabled or sm['carControl'].latActive
       wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low)
       standstill = sm['carState'].standstill

selfdrive/monitoring/test_monitoring.py

@@ -1,6 +1,7 @@
 import numpy as np
+import pytest
 
-from cereal import log
+from cereal import log, car
 from openpilot.common.realtime import DT_DMON
 from openpilot.selfdrive.monitoring.helpers import DriverMonitoring, DRIVER_MONITOR_SETTINGS
 from openpilot.system.hardware import HARDWARE
@@ -204,3 +205,66 @@ class TestMonitoring:
     assert EventName.driverUnresponsive in \
                               events[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names
 
+
+@pytest.mark.parametrize("enabled_state, lat_active_state, expected", [
+  (False, False, False), # Both Disabled
+  (True, False, True),   # OP Enabled, Lat Inactive
+  (False, True, True),   # OP Disabled, Lat Active (e.g. MADS)
+  (True, True, True)     # Both Active
+])
+def test_enabled_states(enabled_state, lat_active_state, expected):
+  """
+  Test DriverMonitoring.run_step with all 4 combinations of:
+  - selfdriveState.enabled (True/False)
+  - carControl.latActive (True/False)
+  """
+  cs = car.CarState.new_message()
+  cs.vEgo = 30.0
+  cs.gearShifter = car.CarState.GearShifter.drive
+  cs.standstill = False
+  cs.steeringPressed = False
+  cs.gasPressed = False
+
+  ss = log.SelfdriveState.new_message()
+  ss.enabled = enabled_state
+
+  cc = car.CarControl.new_message()
+  cc.latActive = lat_active_state
+
+  mv2 = log.ModelDataV2.new_message()
+  mv2.meta.disengagePredictions.brakeDisengageProbs = [0.0]
+
+  lc = log.LiveCalibrationData.new_message()
+  lc.rpyCalib = [0.0, 0.0, 0.0]
+
+  ds = make_msg(False)
+
+  sm = {
+      'carState': cs,
+      'selfdriveState': ss,
+      'carControl': cc,
+      'modelV2': mv2,
+      'liveCalibration': lc,
+      'driverStateV2': ds
+  }
+
+  driver_monitoring = DriverMonitoring()
+
+  # run_test doesn't assign enabled to a variable, so we need to spy on _update_events to see its value
+  captured_args = []
+  original_update_events = driver_monitoring._update_events
+
+  def spy_update_events(driver_engaged, op_engaged, standstill, wrong_gear, car_speed):
+      captured_args.append(op_engaged)
+      return original_update_events(driver_engaged, op_engaged, standstill, wrong_gear, car_speed)
+
+  driver_monitoring._update_events = spy_update_events
+
+  driver_monitoring.run_step(sm, demo=False)
+
+  # Assertion
+  assert len(captured_args) == 1, "Expected _update_events to be called exactly once"
+  actual_enabled = captured_args[0]
+
+  assert actual_enabled == expected, f"Expected op_engaged={expected}, but got {actual_enabled}"
+

selfdrive/ui/layouts/onboarding.py

@@ -11,7 +11,9 @@ from openpilot.system.ui.widgets import Widget
 from openpilot.system.ui.widgets.button import Button, ButtonStyle
 from openpilot.system.ui.widgets.label import Label
 from openpilot.selfdrive.ui.ui_state import ui_state
-from openpilot.system.version import terms_version, training_version
+from openpilot.system.version import terms_version, training_version, terms_version_sp
+
+from openpilot.selfdrive.ui.sunnypilot.layouts.onboarding import SunnylinkOnboarding
 
 DEBUG = False
 
@@ -33,6 +35,7 @@ class OnboardingState(IntEnum):
   TERMS = 0
   ONBOARDING = 1
   DECLINE = 2
+  SUNNYLINK_CONSENT = 3
 
 
 class TrainingGuide(Widget):
@@ -110,14 +113,14 @@ class TermsPage(Widget):
     self._on_decline = on_decline
 
     self._title = Label(tr("Welcome to sunnypilot"), font_size=90, font_weight=FontWeight.BOLD, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT)
-    self._desc = Label(tr("You must accept the Terms and Conditions to use sunnypilot. Read the latest terms at https://comma.ai/terms before continuing."),
+    self._desc = Label(tr("You must accept the Terms of Service to use sunnypilot. Read the latest terms at https://sunnypilot.ai/terms before continuing."),
                        font_size=90, font_weight=FontWeight.MEDIUM, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT)
 
     self._decline_btn = Button(tr("Decline"), click_callback=on_decline)
     self._accept_btn = Button(tr("Agree"), button_style=ButtonStyle.PRIMARY, click_callback=on_accept)
 
   def _render(self, _):
-    welcome_x = self._rect.x + 165
+    welcome_x = self._rect.x + 95
     welcome_y = self._rect.y + 165
     welcome_rect = rl.Rectangle(welcome_x, welcome_y, self._rect.width - welcome_x, 90)
     self._title.render(welcome_rect)
@@ -143,7 +146,7 @@ class TermsPage(Widget):
 class DeclinePage(Widget):
   def __init__(self, back_callback=None):
     super().__init__()
-    self._text = Label(tr("You must accept the Terms and Conditions in order to use sunnypilot."),
+    self._text = Label(tr("You must accept the Terms of Service in order to use sunnypilot."),
                        font_size=90, font_weight=FontWeight.MEDIUM, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT)
     self._back_btn = Button(tr("Back"), click_callback=back_callback)
     self._uninstall_btn = Button(tr("Decline, uninstall sunnypilot"), button_style=ButtonStyle.DANGER,
@@ -180,9 +183,21 @@ class OnboardingWindow(Widget):
     self._training_guide: TrainingGuide | None = None
     self._decline_page = DeclinePage(back_callback=self._on_decline_back)
 
+    # sunnylink consent pages
+    self._accepted_terms = self._accepted_terms and ui_state.params.get("HasAcceptedTermsSP") == terms_version_sp
+    self._sunnylink = SunnylinkOnboarding()
+    if not self._accepted_terms:
+      self._state = OnboardingState.TERMS
+    elif not self._sunnylink.completed:
+      self._state = OnboardingState.SUNNYLINK_CONSENT
+    elif not self._training_done:
+      self._state = OnboardingState.ONBOARDING
+    else:
+      self._state = OnboardingState.ONBOARDING
+
   @property
   def completed(self) -> bool:
-    return self._accepted_terms and self._training_done
+    return self._accepted_terms and self._sunnylink.completed and self._training_done
 
   def _on_terms_declined(self):
     self._state = OnboardingState.DECLINE
@@ -192,8 +207,12 @@ class OnboardingWindow(Widget):
 
   def _on_terms_accepted(self):
     ui_state.params.put("HasAcceptedTerms", terms_version)
-    self._state = OnboardingState.ONBOARDING
-    if self._training_done:
+    ui_state.params.put("HasAcceptedTermsSP", terms_version_sp)
+    if not self._sunnylink.completed:
+      self._state = OnboardingState.SUNNYLINK_CONSENT
+    elif not self._training_done:
+      self._state = OnboardingState.ONBOARDING
+    else:
       gui_app.set_modal_overlay(None)
 
   def _on_completed_training(self):
@@ -206,8 +225,18 @@ class OnboardingWindow(Widget):
 
     if self._state == OnboardingState.TERMS:
       self._terms.render(self._rect)
-    if self._state == OnboardingState.ONBOARDING:
-      self._training_guide.render(self._rect)
+    elif self._state == OnboardingState.SUNNYLINK_CONSENT:
+      self._sunnylink.render(self._rect)
+      if self._sunnylink.completed:
+        if not self._training_done:
+          self._state = OnboardingState.ONBOARDING
+        else:
+          gui_app.set_modal_overlay(None)
+    elif self._state == OnboardingState.ONBOARDING:
+      if not self._training_done:
+        self._training_guide.render(self._rect)
+      else:
+        gui_app.set_modal_overlay(None)
     elif self._state == OnboardingState.DECLINE:
       self._decline_page.render(self._rect)
     return -1

selfdrive/ui/layouts/sidebar.py

@@ -9,6 +9,8 @@ from openpilot.system.ui.lib.multilang import tr, tr_noop
 from openpilot.system.ui.lib.text_measure import measure_text_cached
 from openpilot.system.ui.widgets import Widget
 
+from openpilot.selfdrive.ui.sunnypilot.layouts.sidebar import SidebarSP
+
 SIDEBAR_WIDTH = 300
 METRIC_HEIGHT = 126
 METRIC_WIDTH = 240
@@ -62,9 +64,10 @@ class MetricData:
     self.color = color
 
 
-class Sidebar(Widget):
+class Sidebar(Widget, SidebarSP):
   def __init__(self):
-    super().__init__()
+    Widget.__init__(self)
+    SidebarSP.__init__(self)
     self._net_type = NETWORK_TYPES.get(NetworkType.none)
     self._net_strength = 0
 
@@ -112,6 +115,7 @@ class Sidebar(Widget):
     self._update_temperature_status(device_state)
     self._update_connection_status(device_state)
     self._update_panda_status()
+    SidebarSP._update_sunnylink_status(self)
 
   def _update_network_status(self, device_state):
     self._net_type = NETWORK_TYPES.get(device_state.networkType.raw, tr_noop("Unknown"))
@@ -200,6 +204,13 @@ class Sidebar(Widget):
     rl.draw_text_ex(self._font_regular, tr(self._net_type), text_pos, FONT_SIZE, 0, Colors.WHITE)
 
   def _draw_metrics(self, rect: rl.Rectangle):
+    if gui_app.sunnypilot_ui():
+      metrics, start_y, spacing = SidebarSP._draw_metrics_w_sunnylink(self, rect, self._temp_status, self._panda_status, self._connect_status)
+      for idx, metric in enumerate(metrics):
+        self._draw_metric(rect, metric, start_y + idx * spacing)
+
+      return
+
     metrics = [(self._temp_status, 338), (self._panda_status, 496), (self._connect_status, 654)]
 
     for metric, y_offset in metrics:

selfdrive/ui/mici/layouts/home.py

@@ -109,7 +109,7 @@ class MiciHomeLayout(Widget):
     self._cell_high_txt = gui_app.texture("icons_mici/settings/network/cell_strength_high.png", 55, 35)
     self._cell_full_txt = gui_app.texture("icons_mici/settings/network/cell_strength_full.png", 55, 35)
 
-    self._openpilot_label = MiciLabel("sunnypilot", font_size=96, color=rl.Color(255, 255, 255, int(255 * 0.9)), font_weight=FontWeight.DISPLAY)
+    self._openpilot_label = MiciLabel("sunnypilot", font_size=90, color=rl.Color(255, 255, 255, int(255 * 0.9)), font_weight=FontWeight.AUDIOWIDE)
     self._version_label = MiciLabel("", font_size=36, font_weight=FontWeight.ROMAN)
     self._large_version_label = MiciLabel("", font_size=64, color=rl.GRAY, font_weight=FontWeight.ROMAN)
     self._date_label = MiciLabel("", font_size=36, color=rl.GRAY, font_weight=FontWeight.ROMAN)

selfdrive/ui/mici/layouts/onboarding.py

@@ -17,13 +17,16 @@ from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer
 from openpilot.selfdrive.ui.mici.onroad.driver_camera_dialog import DriverCameraDialog
 from openpilot.system.ui.widgets.label import gui_label
 from openpilot.system.ui.lib.multilang import tr
-from openpilot.system.version import terms_version, training_version
+from openpilot.system.version import terms_version, training_version, terms_version_sp
+
+from openpilot.selfdrive.ui.sunnypilot.mici.layouts.onboarding import SunnylinkOnboarding
 
 
 class OnboardingState(IntEnum):
   TERMS = 0
   ONBOARDING = 1
   DECLINE = 2
+  SUNNYLINK_CONSENT = 3
 
 
 class DriverCameraSetupDialog(DriverCameraDialog):
@@ -412,10 +415,10 @@ class TermsPage(SetupTermsPage):
     super().__init__(on_accept, on_decline, "decline")
 
     info_txt = gui_app.texture("icons_mici/setup/green_info.png", 60, 60)
-    self._title_header = TermsHeader("terms & conditions", info_txt)
+    self._title_header = TermsHeader("terms of service", info_txt)
 
-    self._terms_label = UnifiedLabel("You must accept the Terms and Conditions to use sunnypilot. " +
-                                     "Read the latest terms at https://comma.ai/terms before continuing.", 36,
+    self._terms_label = UnifiedLabel("You must accept the Terms of Service to use sunnypilot. " +
+                                     "Read the latest terms at https://sunnypilot.ai/terms before continuing.", 36,
                                      FontWeight.ROMAN)
 
   @property
@@ -449,6 +452,18 @@ class OnboardingWindow(Widget):
     self._training_guide = TrainingGuide(completed_callback=self._on_completed_training)
     self._decline_page = DeclinePage(back_callback=self._on_decline_back)
 
+    # sunnylink consent pages
+    self._accepted_terms = self._accepted_terms and ui_state.params.get("HasAcceptedTermsSP") == terms_version_sp
+    self._sunnylink = SunnylinkOnboarding()
+    if not self._accepted_terms:
+      self._state = OnboardingState.TERMS
+    elif not self._sunnylink.completed:
+      self._state = OnboardingState.SUNNYLINK_CONSENT
+    elif not self._training_done:
+      self._state = OnboardingState.ONBOARDING
+    else:
+      self._state = OnboardingState.ONBOARDING
+
   def show_event(self):
     super().show_event()
     device.set_override_interactive_timeout(300)
@@ -459,7 +474,7 @@ class OnboardingWindow(Widget):
 
   @property
   def completed(self) -> bool:
-    return self._accepted_terms and self._training_done
+    return self._accepted_terms and self._sunnylink.completed and self._training_done
 
   def _on_terms_declined(self):
     self._state = OnboardingState.DECLINE
@@ -473,7 +488,13 @@ class OnboardingWindow(Widget):
 
   def _on_terms_accepted(self):
     ui_state.params.put("HasAcceptedTerms", terms_version)
-    self._state = OnboardingState.ONBOARDING
+    ui_state.params.put("HasAcceptedTermsSP", terms_version_sp)
+    if not self._sunnylink.completed:
+      self._state = OnboardingState.SUNNYLINK_CONSENT
+    elif not self._training_done:
+      self._state = OnboardingState.ONBOARDING
+    else:
+      self.close()
 
   def _on_completed_training(self):
     ui_state.params.put("CompletedTrainingVersion", training_version)
@@ -482,8 +503,18 @@ class OnboardingWindow(Widget):
   def _render(self, _):
     if self._state == OnboardingState.TERMS:
       self._terms.render(self._rect)
+    elif self._state == OnboardingState.SUNNYLINK_CONSENT:
+      self._sunnylink.render(self._rect)
+      if self._sunnylink.completed:
+        if not self._training_done:
+          self._state = OnboardingState.ONBOARDING
+        else:
+          self.close()
     elif self._state == OnboardingState.ONBOARDING:
-      self._training_guide.render(self._rect)
+      if not self._training_done:
+        self._training_guide.render(self._rect)
+      else:
+        self.close()
     elif self._state == OnboardingState.DECLINE:
       self._decline_page.render(self._rect)
     return -1

selfdrive/ui/mici/onroad/model_renderer.py

@@ -80,6 +80,9 @@ class ModelRenderer(Widget):
     self._transform_dirty = True
     self._clip_region = None
 
+    self._counter = -1
+    self._camera_offset = ui_state.params.get("CameraOffset", return_default=True) if ui_state.active_bundle else 0.0
+
     self._exp_gradient = Gradient(
       start=(0.0, 1.0),  # Bottom of path
       end=(0.0, 0.0),  # Top of path
@@ -99,6 +102,10 @@ class ModelRenderer(Widget):
   def _render(self, rect: rl.Rectangle):
     sm = ui_state.sm
 
+    if self._counter % 180 == 0:  # This runs at 60fps, so we query every 3 seconds
+      self._camera_offset = ui_state.params.get("CameraOffset", return_default=True) if ui_state.active_bundle else 0.0
+    self._counter += 1
+
     self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque)
 
     # Check if data is up-to-date
@@ -150,13 +157,13 @@ class ModelRenderer(Widget):
 
   def _update_raw_points(self, model):
     """Update raw 3D points from model data"""
-    self._path.raw_points = np.array([model.position.x, model.position.y, model.position.z], dtype=np.float32).T
+    self._path.raw_points = np.array([model.position.x, np.array(model.position.y) + self._camera_offset, model.position.z], dtype=np.float32).T
 
     for i, lane_line in enumerate(model.laneLines):
-      self._lane_lines[i].raw_points = np.array([lane_line.x, lane_line.y, lane_line.z], dtype=np.float32).T
+      self._lane_lines[i].raw_points = np.array([lane_line.x, np.array(lane_line.y) + self._camera_offset, lane_line.z], dtype=np.float32).T
 
     for i, road_edge in enumerate(model.roadEdges):
-      self._road_edges[i].raw_points = np.array([road_edge.x, road_edge.y, road_edge.z], dtype=np.float32).T
+      self._road_edges[i].raw_points = np.array([road_edge.x, np.array(road_edge.y) + self._camera_offset, road_edge.z], dtype=np.float32).T
 
     self._lane_line_probs = np.array(model.laneLineProbs, dtype=np.float32)
     self._road_edge_stds = np.array(model.roadEdgeStds, dtype=np.float32)
@@ -174,7 +181,7 @@ class ModelRenderer(Widget):
 
         # Get z-coordinate from path at the lead vehicle position
         z = self._path.raw_points[idx, 2] if idx < len(self._path.raw_points) else 0.0
-        point = self._map_to_screen(d_rel, -y_rel, z + self._path_offset_z)
+        point = self._map_to_screen(d_rel, -y_rel + self._camera_offset, z + self._path_offset_z)
         if point:
           self._lead_vehicles[i] = self._update_lead_vehicle(d_rel, v_rel, point, self._rect)
 

selfdrive/ui/onroad/augmented_road_view.py

@@ -16,6 +16,7 @@ from openpilot.common.transformations.orientation import rot_from_euler
 
 if gui_app.sunnypilot_ui():
   from openpilot.selfdrive.ui.sunnypilot.onroad.hud_renderer import HudRendererSP as HudRenderer
+  from openpilot.selfdrive.ui.sunnypilot.onroad.driver_state import DriverStateRendererSP as DriverStateRenderer
 
 from openpilot.selfdrive.ui.sunnypilot.onroad.augmented_road_view import BORDER_COLORS_SP
 

selfdrive/ui/onroad/model_renderer.py

@@ -56,7 +56,8 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
     self._road_edge_stds = np.zeros(2, dtype=np.float32)
     self._lead_vehicles = [LeadVehicle(), LeadVehicle()]
     self._path_offset_z = HEIGHT_INIT[0]
-
+    self._counter = -1
+    self._camera_offset = ui_state.params.get("CameraOffset", return_default=True) if ui_state.active_bundle else 0.0
     # Initialize ModelPoints objects
     self._path = ModelPoints()
     self._lane_lines = [ModelPoints() for _ in range(4)]
@@ -103,6 +104,10 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
     live_calib = sm['liveCalibration']
     self._path_offset_z = live_calib.height[0] if live_calib.height else HEIGHT_INIT[0]
 
+    if self._counter % 60 == 0:
+      self._camera_offset = ui_state.params.get("CameraOffset", return_default=True) if ui_state.active_bundle else 0.0
+    self._counter += 1
+
     if sm.updated['carParams']:
       self._longitudinal_control = sm['carParams'].openpilotLongitudinalControl
 
@@ -136,13 +141,13 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
 
   def _update_raw_points(self, model):
     """Update raw 3D points from model data"""
-    self._path.raw_points = np.array([model.position.x, model.position.y, model.position.z], dtype=np.float32).T
+    self._path.raw_points = np.array([model.position.x, np.array(model.position.y) + self._camera_offset, model.position.z], dtype=np.float32).T
 
     for i, lane_line in enumerate(model.laneLines):
-      self._lane_lines[i].raw_points = np.array([lane_line.x, lane_line.y, lane_line.z], dtype=np.float32).T
+      self._lane_lines[i].raw_points = np.array([lane_line.x, np.array(lane_line.y) + self._camera_offset, lane_line.z], dtype=np.float32).T
 
     for i, road_edge in enumerate(model.roadEdges):
-      self._road_edges[i].raw_points = np.array([road_edge.x, road_edge.y, road_edge.z], dtype=np.float32).T
+      self._road_edges[i].raw_points = np.array([road_edge.x, np.array(road_edge.y) + self._camera_offset, road_edge.z], dtype=np.float32).T
 
     self._lane_line_probs = np.array(model.laneLineProbs, dtype=np.float32)
     self._road_edge_stds = np.array(model.roadEdgeStds, dtype=np.float32)
@@ -160,7 +165,7 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
 
         # Get z-coordinate from path at the lead vehicle position
         z = self._path.raw_points[idx, 2] if idx < len(self._path.raw_points) else 0.0
-        point = self._map_to_screen(d_rel, -y_rel, z + self._path_offset_z)
+        point = self._map_to_screen(d_rel, -y_rel + self._camera_offset, z + self._path_offset_z)
         if point:
           self._lead_vehicles[i] = self._update_lead_vehicle(d_rel, v_rel, point, self._rect)
 

selfdrive/ui/sunnypilot/layouts/onboarding.py

@@ -0,0 +1,116 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import pyray as rl
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.system.ui.lib.application import FontWeight
+from openpilot.system.ui.lib.multilang import tr
+from openpilot.system.ui.widgets import Widget
+from openpilot.system.ui.widgets.button import Button, ButtonStyle
+from openpilot.system.ui.widgets.label import Label
+from openpilot.system.version import sunnylink_consent_version, sunnylink_consent_declined
+
+
+class SunnylinkConsentPage(Widget):
+  def __init__(self, done_callback=None):
+    super().__init__()
+    self._done_callback = done_callback
+    self._step = 0
+
+    self._title = Label(tr("sunnylink"), font_size=90, font_weight=FontWeight.BOLD, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT)
+
+    self._content = [
+      {
+        "text": tr("sunnylink enables secured remote access to your comma device from anywhere, " +
+                   "including settings management, remote monitoring, real-time dashboard, etc."),
+        "primary_btn": tr("Enable"),
+        "secondary_btn": tr("Disable"),
+        "highlight_primary": True
+      },
+      {
+        "text": tr("sunnylink is designed to be enabled as part of sunnypilot's core functionality. " +
+                   "If sunnylink is disabled, features such as settings management, remote monitoring, " +
+                   "real-time dashboards will be unavailable."),
+        "secondary_btn": tr("Back"),
+        "danger_btn": tr("Disable"),
+        "highlight_primary": True
+      }
+    ]
+
+    self._primary_btn = Button("", button_style=ButtonStyle.PRIMARY, click_callback=lambda: self._handle_choice("enable"))
+    self._secondary_btn = Button("", button_style=ButtonStyle.NORMAL, click_callback=lambda: self._handle_choice("secondary"))
+    self._danger_btn = Button("", button_style=ButtonStyle.DANGER, click_callback=lambda: self._handle_choice("disable"))
+
+  def _handle_choice(self, choice):
+    if choice == "enable":
+      ui_state.params.put_bool("SunnylinkEnabled", True)
+      ui_state.params.put("CompletedSunnylinkConsentVersion", sunnylink_consent_version)
+      if self._done_callback:
+        self._done_callback()
+    elif choice == "secondary":
+      if self._step == 0:
+        self._step = 1
+      elif self._step == 1:
+        self._step = 0
+    elif choice == "disable":
+      ui_state.params.put_bool("SunnylinkEnabled", False)
+      ui_state.params.put("CompletedSunnylinkConsentVersion", sunnylink_consent_declined)
+      if self._done_callback:
+        self._done_callback()
+
+  def _render(self, _):
+    step_data = self._content[self._step]
+
+    welcome_x = self._rect.x + 95
+    welcome_y = self._rect.y + 165
+    welcome_rect = rl.Rectangle(welcome_x, welcome_y, self._rect.width - welcome_x, 90)
+    self._title.render(welcome_rect)
+
+    desc_x = welcome_x
+    desc_y = welcome_y + 120
+    desc_rect = rl.Rectangle(desc_x, desc_y, self._rect.width - desc_x, self._rect.height - desc_y - 250)
+
+    desc_label = Label(step_data["text"], font_size=90, font_weight=FontWeight.MEDIUM, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT)
+    desc_label.render(desc_rect)
+
+    btn_y = self._rect.y + self._rect.height - 160 - 45
+
+    if "danger_btn" in step_data:
+      btn_width = (self._rect.width - 45 * 3) / 2
+
+      self._secondary_btn.set_text(step_data["secondary_btn"])
+      self._secondary_btn.render(rl.Rectangle(self._rect.x + 45, btn_y, btn_width, 160))
+
+      self._danger_btn.set_text(step_data["danger_btn"])
+      self._danger_btn.render(rl.Rectangle(self._rect.x + 45 * 2 + btn_width, btn_y, btn_width, 160))
+
+    else:
+      btn_width = (self._rect.width - 45 * 3) / 2
+
+      self._secondary_btn.set_text(step_data["secondary_btn"])
+      self._secondary_btn.render(rl.Rectangle(self._rect.x + 45, btn_y, btn_width, 160))
+
+      self._primary_btn.set_text(step_data["primary_btn"])
+      self._primary_btn.render(rl.Rectangle(self._rect.x + 45 * 2 + btn_width, btn_y, btn_width, 160))
+
+    return -1
+
+
+class SunnylinkOnboarding:
+  def __init__(self):
+    self.consent_page = SunnylinkConsentPage(done_callback=self._on_done)
+    self.consent_done: bool = ui_state.params.get("CompletedSunnylinkConsentVersion") in {sunnylink_consent_version, sunnylink_consent_declined}
+
+  @property
+  def completed(self) -> bool:
+    return self.consent_done
+
+  def _on_done(self):
+    self.consent_done = True
+
+  def render(self, rect):
+    if not self.consent_done:
+      self.consent_page.render(rect)

selfdrive/ui/sunnypilot/layouts/settings/device.py

@@ -5,8 +5,216 @@ This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
 from openpilot.selfdrive.ui.layouts.settings.device import DeviceLayout
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.system.hardware import HARDWARE
+from openpilot.system.ui.lib.application import gui_app
+from openpilot.system.ui.lib.multilang import tr
+from openpilot.system.ui.sunnypilot.widgets.list_view import option_item_sp, multiple_button_item_sp, button_item_sp, \
+  dual_button_item_sp, Spacer
+from openpilot.system.ui.widgets import DialogResult
+from openpilot.system.ui.widgets.button import ButtonStyle
+from openpilot.system.ui.widgets.confirm_dialog import alert_dialog, ConfirmDialog
+from openpilot.system.ui.widgets.list_view import text_item
+from openpilot.system.ui.widgets.scroller_tici import LineSeparator
+
+offroad_time_options = {
+  0: 0,
+  1: 5,
+  2: 10,
+  3: 15,
+  4: 30,
+  5: 60,
+  6: 120,
+  7: 180,
+  8: 300,
+  9: 600,
+  10: 1440,
+  11: 1800,
+}
 
 
 class DeviceLayoutSP(DeviceLayout):
   def __init__(self):
     DeviceLayout.__init__(self)
+    self._scroller._line_separator = None
+
+  def _initialize_items(self):
+    DeviceLayout._initialize_items(self)
+
+    # Using dual button with no right button for better alignment
+    self._always_offroad_btn = dual_button_item_sp(
+      left_text=lambda: tr("Enable Always Offroad"),
+      left_callback=self._handle_always_offroad,
+      right_text="",
+      right_callback=None,
+    )
+    self._always_offroad_btn.action_item.right_button.set_visible(False)
+
+    self._max_time_offroad = option_item_sp(
+      title=lambda: tr("Max Time Offroad"),
+      description=lambda: tr("Device will automatically shutdown after set time once the engine is turned off.\n(30h is the default)"),
+      param="MaxTimeOffroad",
+      min_value=0,
+      max_value=11,
+      value_change_step=1,
+      on_value_changed=None,
+      enabled=True,
+      icon="",
+      value_map=offroad_time_options,
+      label_width=360,
+      use_float_scaling=False,
+      inline=True,
+      label_callback=self._update_max_time_offroad_label
+    )
+
+    self._device_wake_mode = multiple_button_item_sp(
+      title=lambda: tr("Wake Up Behavior"),
+      description=self.wake_mode_description,
+      param="DeviceBootMode",
+      buttons=[lambda: tr("Default"), lambda: tr("Offroad")],
+      button_width=364,
+      callback=None,
+      inline=True,
+    )
+
+    self._quiet_mode_and_dcam = dual_button_item_sp(
+      left_text=lambda: tr("Quiet Mode"),
+      right_text=lambda: tr("Driver Camera Preview"),
+      left_callback=lambda: ui_state.params.put_bool("QuietMode", not ui_state.params.get_bool("QuietMode")),
+      right_callback=self._show_driver_camera
+    )
+    self._quiet_mode_and_dcam.action_item.right_button.set_button_style(ButtonStyle.NORMAL)
+
+    self._reg_and_training = dual_button_item_sp(
+      left_text=lambda: tr("Regulatory"),
+      left_callback=self._on_regulatory,
+      right_text=lambda: tr("Training Guide"),
+      right_callback=self._on_review_training_guide
+    )
+    self._reg_and_training.action_item.right_button.set_button_style(ButtonStyle.NORMAL)
+
+    self._onroad_uploads_and_reset_settings = dual_button_item_sp(
+      left_text=lambda: tr("Onroad Uploads"),
+      left_callback=lambda: ui_state.params.put_bool("OnroadUploads", not ui_state.params.get_bool("OnroadUploads")),
+      right_text=lambda: tr("Reset Settings"),
+      right_callback=self._reset_settings
+    )
+
+    self._power_buttons = dual_button_item_sp(
+      left_text=lambda: tr("Reboot"),
+      right_text=lambda: tr("Power Off"),
+      left_callback=self._reboot_prompt,
+      right_callback=self._power_off_prompt
+    )
+
+    items = [
+      text_item(lambda: tr("Dongle ID"), self._params.get("DongleId") or (lambda: tr("N/A"))),
+      LineSeparator(),
+      text_item(lambda: tr("Serial"), self._params.get("HardwareSerial") or (lambda: tr("N/A"))),
+      LineSeparator(),
+      self._pair_device_btn,
+      LineSeparator(),
+      self._reset_calib_btn,
+      LineSeparator(),
+      button_item_sp(lambda: tr("Change Language"), lambda: tr("CHANGE"), callback=self._show_language_dialog),
+      LineSeparator(),
+      self._device_wake_mode,
+      LineSeparator(),
+      self._max_time_offroad,
+      LineSeparator(height=10),
+      self._quiet_mode_and_dcam,
+      self._reg_and_training,
+      self._onroad_uploads_and_reset_settings,
+      Spacer(10),
+      LineSeparator(height=10),
+      self._power_buttons,
+    ]
+
+    return items
+
+  def _offroad_transition(self):
+    self._power_buttons.action_item.right_button.set_visible(ui_state.is_offroad())
+
+  @staticmethod
+  def wake_mode_description() -> str:
+    def_str = tr("Default: Device will boot/wake-up normally & will be ready to engage.")
+    offrd_str = tr("Offroad: Device will be in Always Offroad mode after boot/wake-up.")
+    header = tr("Controls state of the device after boot/sleep.")
+
+    return f"{header}\n\n{def_str}\n{offrd_str}"
+
+  @staticmethod
+  def _reset_settings():
+    def _do_reset(result: int):
+      if result == DialogResult.CONFIRM:
+        for _key in ui_state.params.all_keys():
+          ui_state.params.remove(_key)
+        HARDWARE.reboot()
+
+    def _second_confirm(result: int):
+      if result == DialogResult.CONFIRM:
+        gui_app.set_modal_overlay(ConfirmDialog(
+          text=tr("The reset cannot be undone. You have been warned."),
+          confirm_text=tr("Confirm")
+        ), callback=_do_reset)
+
+    gui_app.set_modal_overlay(ConfirmDialog(
+      text=tr("Are you sure you want to reset all sunnypilot settings to default? Once the settings are reset, there is no going back."),
+      confirm_text=tr("Reset")
+    ), callback=_second_confirm)
+
+  @staticmethod
+  def _handle_always_offroad():
+    if ui_state.engaged:
+      gui_app.set_modal_overlay(alert_dialog(tr("Disengage to Enter Always Offroad Mode")))
+      return
+
+    _offroad_mode_state = ui_state.params.get_bool("OffroadMode")
+    _offroad_mode_str = tr("Are you sure you want to exit Always Offroad mode?") if _offroad_mode_state else \
+                        tr("Are you sure you want to enter Always Offroad mode?")
+
+    def _set_always_offroad(result: int):
+      if result == DialogResult.CONFIRM and not ui_state.engaged:
+        ui_state.params.put_bool("OffroadMode", not _offroad_mode_state)
+
+    gui_app.set_modal_overlay(ConfirmDialog(_offroad_mode_str, tr("Confirm")), callback=lambda result: _set_always_offroad(result))
+
+  @staticmethod
+  def _update_max_time_offroad_label(value: int) -> str:
+    label = tr("Always On") if value == 0 else f"{value}" + tr("m") if value < 60 else f"{value // 60}" + tr("h")
+    label += tr(" (Default)") if value == 1800 else ""
+    return label
+
+  def _update_state(self):
+    super()._update_state()
+
+    # Handle Always Offroad button
+    always_offroad = ui_state.params.get_bool("OffroadMode")
+
+    # Text & Color
+    offroad_mode_btn_text = tr("Exit Always Offroad") if always_offroad else tr("Enable Always Offroad")
+    offroad_mode_btn_style = ButtonStyle.NORMAL if always_offroad else ButtonStyle.DANGER
+    self._always_offroad_btn.action_item.left_button.set_text(offroad_mode_btn_text)
+    self._always_offroad_btn.action_item.left_button.set_button_style(offroad_mode_btn_style)
+
+    # Position
+    if self._scroller._items.__contains__(self._always_offroad_btn):
+      self._scroller._items.remove(self._always_offroad_btn)
+    if ui_state.is_offroad() and not always_offroad:
+      self._scroller._items.insert(len(self._scroller._items) - 1, self._always_offroad_btn)
+    else:
+      self._scroller._items.insert(0, self._always_offroad_btn)
+
+    # Quiet Mode button
+    self._quiet_mode_and_dcam.action_item.left_button.set_button_style(ButtonStyle.PRIMARY if ui_state.params.get_bool("QuietMode") else ButtonStyle.NORMAL)
+
+    # Onroad Uploads
+    self._onroad_uploads_and_reset_settings.action_item.left_button.set_button_style(
+      ButtonStyle.PRIMARY if ui_state.params.get_bool("OnroadUploads") else ButtonStyle.NORMAL
+    )
+
+    # Offroad only buttons
+    self._quiet_mode_and_dcam.action_item.right_button.set_enabled(ui_state.is_offroad())
+    self._reg_and_training.action_item.left_button.set_enabled(ui_state.is_offroad())
+    self._reg_and_training.action_item.right_button.set_enabled(ui_state.is_offroad())
+    self._onroad_uploads_and_reset_settings.action_item.right_button.set_enabled(ui_state.is_offroad())

selfdrive/ui/sunnypilot/layouts/settings/settings.py

@@ -9,28 +9,28 @@ from enum import IntEnum
 
 import pyray as rl
 from openpilot.selfdrive.ui.layouts.settings import settings as OP
-from openpilot.selfdrive.ui.sunnypilot.layouts.settings.device import DeviceLayoutSP
 from openpilot.selfdrive.ui.layouts.settings.firehose import FirehoseLayout
-from openpilot.selfdrive.ui.sunnypilot.layouts.settings.software import SoftwareLayoutSP
 from openpilot.selfdrive.ui.layouts.settings.toggles import TogglesLayout
-from openpilot.system.ui.lib.application import gui_app, MousePos
-from openpilot.system.ui.lib.multilang import tr_noop
-from openpilot.system.ui.sunnypilot.lib.styles import style
-from openpilot.system.ui.widgets.scroller_tici import Scroller
-from openpilot.system.ui.lib.text_measure import measure_text_cached
-from openpilot.system.ui.lib.wifi_manager import WifiManager
-from openpilot.system.ui.widgets import Widget
+from openpilot.selfdrive.ui.sunnypilot.layouts.settings.cruise import CruiseLayout
+from openpilot.selfdrive.ui.sunnypilot.layouts.settings.developer import DeveloperLayoutSP
+from openpilot.selfdrive.ui.sunnypilot.layouts.settings.device import DeviceLayoutSP
+from openpilot.selfdrive.ui.sunnypilot.layouts.settings.display import DisplayLayout
 from openpilot.selfdrive.ui.sunnypilot.layouts.settings.models import ModelsLayout
 from openpilot.selfdrive.ui.sunnypilot.layouts.settings.network import NetworkUISP
-from openpilot.selfdrive.ui.sunnypilot.layouts.settings.sunnylink import SunnylinkLayout
 from openpilot.selfdrive.ui.sunnypilot.layouts.settings.osm import OSMLayout
+from openpilot.selfdrive.ui.sunnypilot.layouts.settings.software import SoftwareLayoutSP
+from openpilot.selfdrive.ui.sunnypilot.layouts.settings.steering import SteeringLayout
+from openpilot.selfdrive.ui.sunnypilot.layouts.settings.sunnylink import SunnylinkLayout
 from openpilot.selfdrive.ui.sunnypilot.layouts.settings.trips import TripsLayout
 from openpilot.selfdrive.ui.sunnypilot.layouts.settings.vehicle import VehicleLayout
-from openpilot.selfdrive.ui.sunnypilot.layouts.settings.steering import SteeringLayout
-from openpilot.selfdrive.ui.sunnypilot.layouts.settings.cruise import CruiseLayout
 from openpilot.selfdrive.ui.sunnypilot.layouts.settings.visuals import VisualsLayout
-from openpilot.selfdrive.ui.sunnypilot.layouts.settings.display import DisplayLayout
-from openpilot.selfdrive.ui.sunnypilot.layouts.settings.developer import DeveloperLayoutSP
+from openpilot.system.ui.lib.application import gui_app, MousePos
+from openpilot.system.ui.lib.multilang import tr_noop
+from openpilot.system.ui.lib.text_measure import measure_text_cached
+from openpilot.system.ui.lib.wifi_manager import WifiManager
+from openpilot.system.ui.sunnypilot.lib.styles import style
+from openpilot.system.ui.widgets import Widget
+from openpilot.system.ui.widgets.scroller_tici import Scroller
 
 # from openpilot.selfdrive.ui.sunnypilot.layouts.settings.navigation import NavigationLayout
 

selfdrive/ui/sunnypilot/layouts/settings/sunnylink.py

@@ -4,23 +4,23 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
 This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
+import pyray as rl
 from cereal import custom
+from openpilot.selfdrive.ui.sunnypilot.layouts.onboarding import SunnylinkConsentPage
 from openpilot.selfdrive.ui.ui_state import ui_state
 from openpilot.sunnypilot.sunnylink.api import UNREGISTERED_SUNNYLINK_DONGLE_ID
 from openpilot.system.ui.lib.application import gui_app, FontWeight
 from openpilot.system.ui.lib.multilang import tr
+from openpilot.system.ui.sunnypilot.widgets.list_view import button_item_sp
+from openpilot.system.ui.sunnypilot.widgets.list_view import toggle_item_sp
 from openpilot.system.ui.sunnypilot.widgets.sunnylink_pairing_dialog import SunnylinkPairingDialog
+from openpilot.system.ui.widgets import Widget, DialogResult
 from openpilot.system.ui.widgets.button import ButtonStyle, Button
 from openpilot.system.ui.widgets.confirm_dialog import alert_dialog, ConfirmDialog
 from openpilot.system.ui.widgets.label import UnifiedLabel
-from openpilot.system.ui.widgets.list_view import button_item, dual_button_item
+from openpilot.system.ui.widgets.list_view import dual_button_item
 from openpilot.system.ui.widgets.scroller_tici import Scroller, LineSeparator
-from openpilot.system.ui.widgets import Widget, DialogResult
-from openpilot.system.ui.sunnypilot.widgets.list_view import toggle_item_sp
-import pyray as rl
-
-if gui_app.sunnypilot_ui():
-  from openpilot.system.ui.sunnypilot.widgets.list_view import button_item_sp as button_item
+from openpilot.system.version import sunnylink_consent_version
 
 
 class SunnylinkHeader(Widget):
@@ -160,14 +160,14 @@ class SunnylinkLayout(Widget):
     self._sunnylink_description = SunnylinkDescriptionItem()
     self._sunnylink_description.set_visible(False)
 
-    self._sponsor_btn = button_item(
+    self._sponsor_btn = button_item_sp(
       title=tr("Sponsor Status"),
       button_text=tr("SPONSOR"),
       description=tr(
         "Become a sponsor of sunnypilot to get early access to sunnylink features when they become available."),
       callback=lambda: self._handle_pair_btn(False)
     )
-    self._pair_btn = button_item(
+    self._pair_btn = button_item_sp(
       title=tr("Pair GitHub Account"),
       button_text=tr("Not Paired"),
       description=tr(
@@ -209,8 +209,8 @@ class SunnylinkLayout(Widget):
     return items
 
   @staticmethod
-  def _get_sunnylink_dongle_id() -> str | None:
-    return str(ui_state.params.get("SunnylinkDongleId") or (lambda: tr("N/A")))
+  def _get_sunnylink_dongle_id() -> str:
+    return ui_state.params.get("SunnylinkDongleId") or tr("N/A")
 
   def _handle_pair_btn(self, sponsor_pairing: bool = False):
     sunnylink_dongle_id = self._get_sunnylink_dongle_id()
@@ -302,6 +302,22 @@ class SunnylinkLayout(Widget):
       self._restore_btn.set_text(tr("Restore Settings"))
 
   def _sunnylink_toggle_callback(self, state: bool):
+    sl_consent: bool = ui_state.params.get("CompletedSunnylinkConsentVersion") == sunnylink_consent_version
+    sl_enabled: bool = ui_state.params.get_bool("SunnylinkEnabled")
+
+    if state and not sl_consent and not sl_enabled:
+      def on_consent_done():
+        enabled = ui_state.params.get_bool("SunnylinkEnabled")
+        self._update_description(enabled)
+        gui_app.set_modal_overlay(None)
+
+      sl_terms_dlg = SunnylinkConsentPage(done_callback=on_consent_done)
+      gui_app.set_modal_overlay(sl_terms_dlg)
+    else:
+      ui_state.params.put_bool("SunnylinkEnabled", state)
+      self._update_description(state)
+
+  def _update_description(self, state: bool):
     if state:
       description = tr(
         "Welcome back!! We're excited to see you've enabled sunnylink again!")

selfdrive/ui/sunnypilot/layouts/sidebar.py

@@ -0,0 +1,87 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import pyray as rl
+import time
+from dataclasses import dataclass
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.sunnypilot.sunnylink.api import UNREGISTERED_SUNNYLINK_DONGLE_ID
+from openpilot.system.ui.lib.multilang import tr_noop
+
+
+PING_TIMEOUT_NS = 80_000_000_000  # 80 seconds in nanoseconds
+METRIC_HEIGHT = 126
+METRIC_MARGIN = 30
+METRIC_START_Y = 300
+HOME_BTN = rl.Rectangle(60, 860, 180, 180)
+
+
+# Color scheme
+class Colors:
+  WHITE = rl.WHITE
+  WHITE_DIM = rl.Color(255, 255, 255, 85)
+  GRAY = rl.Color(84, 84, 84, 255)
+
+  # Status colors
+  GOOD = rl.WHITE
+  WARNING = rl.Color(218, 202, 37, 255)
+  DANGER = rl.Color(201, 34, 49, 255)
+  PROGRESS = rl.Color(0, 134, 233, 255)
+  DISABLED = rl.Color(128, 128, 128, 255)
+
+  # UI elements
+  METRIC_BORDER = rl.Color(255, 255, 255, 85)
+  BUTTON_NORMAL = rl.WHITE
+  BUTTON_PRESSED = rl.Color(255, 255, 255, 166)
+
+
+@dataclass(slots=True)
+class MetricData:
+  label: str
+  value: str
+  color: rl.Color
+
+  def update(self, label: str, value: str, color: rl.Color):
+    self.label = label
+    self.value = value
+    self.color = color
+
+
+class SidebarSP:
+  def __init__(self):
+    self._sunnylink_status = MetricData(tr_noop("SUNNYLINK"), tr_noop("OFFLINE"), Colors.WARNING)
+
+  def _update_sunnylink_status(self):
+    if not ui_state.params.get_bool("SunnylinkEnabled"):
+      self._sunnylink_status.update(tr_noop("SUNNYLINK"), tr_noop("DISABLED"), Colors.DISABLED)
+      return
+
+    last_ping = ui_state.params.get("LastSunnylinkPingTime") or 0
+    dongle_id = ui_state.params.get("SunnylinkDongleId")
+
+    is_online = last_ping and (time.monotonic_ns() - last_ping) < PING_TIMEOUT_NS
+    is_temp_fault = ui_state.params.get_bool("SunnylinkTempFault")
+    is_registering = not is_temp_fault and dongle_id in (None, "", UNREGISTERED_SUNNYLINK_DONGLE_ID)
+
+    # Determine status/color pair based on priority
+    if last_ping:
+      status, color = (tr_noop("ONLINE"), Colors.GOOD) if is_online else (tr_noop("ERROR"), Colors.DANGER)
+    elif is_temp_fault:
+      status, color = (tr_noop("FAULT"), Colors.WARNING)
+    elif is_registering:
+      status, color = (tr_noop("REGIST..."), Colors.PROGRESS)
+    else:
+      status, color = (tr_noop("OFFLINE"), Colors.DANGER)
+
+    self._sunnylink_status.update(tr_noop("SUNNYLINK"), status, color)
+
+  def _draw_metrics_w_sunnylink(self, rect: rl.Rectangle, _temp, _panda, _connect):
+    metrics = [_temp, _panda, _connect, self._sunnylink_status]
+    start_y = int(rect.y) + METRIC_START_Y
+    available_height = max(0, int(HOME_BTN.y) - METRIC_MARGIN - METRIC_HEIGHT - start_y)
+    spacing = available_height / max(1, len(metrics) - 1)
+
+    return metrics, start_y, spacing

selfdrive/ui/sunnypilot/mici/layouts/onboarding.py

@@ -0,0 +1,97 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import pyray as rl
+from openpilot.system.ui.lib.application import FontWeight, gui_app
+from openpilot.system.ui.widgets.label import UnifiedLabel
+from openpilot.system.ui.widgets.slider import SmallSlider
+from openpilot.system.ui.mici_setup import TermsHeader, TermsPage as SetupTermsPage
+from openpilot.system.version import sunnylink_consent_version, sunnylink_consent_declined
+from openpilot.selfdrive.ui.ui_state import ui_state
+
+
+class SunnylinkConsentPage(SetupTermsPage):
+  def __init__(self, on_accept=None, on_decline=None, left_text: str = "disable", right_text: str = "enable"):
+    super().__init__(on_accept, on_decline, left_text, continue_text=right_text)
+
+    self._title_header = TermsHeader("sunnylink",
+                                     gui_app.texture("../../sunnypilot/selfdrive/assets/logo.png", 66, 60))
+
+    self._terms_label = UnifiedLabel("sunnylink enables secured remote access to your comma device from anywhere, " +
+                                     "including settings management, remote monitoring, real-time dashboard, etc.",
+                                     36, FontWeight.ROMAN)
+
+  @property
+  def _content_height(self):
+    return self._terms_label.rect.y + self._terms_label.rect.height - self._scroll_panel.get_offset()
+
+  def _render(self, _):
+    super()._render(_)
+    return -1
+
+  def _render_content(self, scroll_offset):
+    self._title_header.set_position(self._rect.x + 16, self._rect.y + 12 + scroll_offset)
+    self._title_header.render()
+
+    self._terms_label.render(rl.Rectangle(
+      self._rect.x + 16,
+      self._title_header.rect.y + self._title_header.rect.height + self.ITEM_SPACING,
+      self._rect.width - 100,
+      self._terms_label.get_content_height(int(self._rect.width - 100)),
+    ))
+
+
+class SunnylinkConsentDisableConfirmPage(SunnylinkConsentPage):
+  def __init__(self, on_accept=None, on_decline=None):
+    super().__init__(on_accept=on_decline, on_decline=on_accept, left_text="enable", right_text="disable")
+
+    # we flip the continue & disable buttons to use slider for disable
+    self._continue_slider = True
+    self._continue_button = SmallSlider("disable", confirm_callback=on_decline)
+    self._scroll_panel.set_enabled(lambda: not self._continue_button.is_pressed)
+
+    self._title_header = TermsHeader("disable sunnylink?",
+                                     gui_app.texture("icons_mici/setup/red_warning.png", 66, 60))
+
+    self._terms_label = UnifiedLabel("sunnylink is designed to be enabled as part of sunnypilot's core functionality. " +
+                                     "If sunnylink is disabled, features such as settings management, " +
+                                     "remote monitoring, real-time dashboards will be unavailable.",
+                                     36, FontWeight.ROMAN)
+
+
+class SunnylinkOnboarding:
+  def __init__(self):
+    self.consent_done: bool = ui_state.params.get("CompletedSunnylinkConsentVersion") in {sunnylink_consent_version, sunnylink_consent_declined}
+    self.disable_confirm = False
+
+    self.consent_page = SunnylinkConsentPage(on_decline=self._on_decline, on_accept=self._on_accept)
+    self.confirm_page = SunnylinkConsentDisableConfirmPage(on_decline=self._on_confirm_decline, on_accept=self._on_accept)
+
+  @property
+  def completed(self) -> bool:
+    return self.consent_done
+
+  def _on_accept(self):
+    ui_state.params.put("CompletedSunnylinkConsentVersion", sunnylink_consent_version)
+    ui_state.params.put_bool("SunnylinkEnabled", True)
+    self.consent_done = True
+
+  def _on_decline(self):
+    self.disable_confirm = True
+
+  def _on_confirm_decline(self):
+    ui_state.params.put_bool("SunnylinkEnabled", False)
+    ui_state.params.put("CompletedSunnylinkConsentVersion", sunnylink_consent_declined)
+    self.consent_done = True
+
+  def render(self, rect):
+    if self.consent_done:
+      return
+
+    if self.disable_confirm:
+      self.confirm_page.render(rect)
+    else:
+      self.consent_page.render(rect)

selfdrive/ui/sunnypilot/mici/layouts/sunnylink.py

@@ -8,16 +8,17 @@ from collections.abc import Callable
 
 import pyray as rl
 from cereal import custom
-from openpilot.selfdrive.ui.mici.widgets.dialog import BigDialog, BigConfirmationDialogV2
-from openpilot.selfdrive.ui.sunnypilot.mici.widgets.sunnylink_pairing_dialog import SunnylinkPairingDialog
-from openpilot.sunnypilot.sunnylink.api import UNREGISTERED_SUNNYLINK_DONGLE_ID
-from openpilot.system.ui.lib.multilang import tr
-
-from openpilot.system.ui.widgets.scroller import Scroller
 from openpilot.selfdrive.ui.mici.widgets.button import BigButton, BigToggle
-from openpilot.system.ui.lib.application import gui_app, MousePos
-from openpilot.system.ui.widgets import NavWidget
+from openpilot.selfdrive.ui.mici.widgets.dialog import BigDialog, BigConfirmationDialogV2
+from openpilot.selfdrive.ui.sunnypilot.mici.layouts.onboarding import SunnylinkConsentPage
+from openpilot.selfdrive.ui.sunnypilot.mici.widgets.sunnylink_pairing_dialog import SunnylinkPairingDialog
 from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.sunnypilot.sunnylink.api import UNREGISTERED_SUNNYLINK_DONGLE_ID
+from openpilot.system.ui.lib.application import gui_app, MousePos
+from openpilot.system.ui.lib.multilang import tr
+from openpilot.system.ui.widgets import NavWidget
+from openpilot.system.ui.widgets.scroller import Scroller
+from openpilot.system.version import sunnylink_consent_version, sunnylink_consent_declined
 
 
 class SunnylinkLayoutMici(NavWidget):
@@ -28,9 +29,9 @@ class SunnylinkLayoutMici(NavWidget):
     self._backup_in_progress = False
     self._sunnylink_enabled = ui_state.params.get("SunnylinkEnabled")
 
-    self._sunnylink_toggle = BigToggle(text="",
+    self._sunnylink_toggle = BigToggle(text=tr("enable sunnylink"),
                                        initial_state=self._sunnylink_enabled,
-                                       toggle_callback=SunnylinkLayoutMici._sunnylink_toggle_callback)
+                                       toggle_callback=self._sunnylink_toggle_callback)
     self._sunnylink_sponsor_button = SunnylinkPairBigButton(sponsor_pairing=False)
     self._sunnylink_pair_button = SunnylinkPairBigButton(sponsor_pairing=True)
     self._backup_btn = BigButton(tr("backup settings"), "", "")
@@ -38,7 +39,7 @@ class SunnylinkLayoutMici(NavWidget):
     self._restore_btn = BigButton(tr("restore settings"), "", "")
     self._restore_btn.set_click_callback(lambda: self._handle_backup_restore_btn(restore=True))
     self._sunnylink_uploader_toggle = BigToggle(text=tr("sunnylink uploader"), initial_state=False,
-                                                toggle_callback=SunnylinkLayoutMici._sunnylink_uploader_callback)
+                                                toggle_callback=self._sunnylink_uploader_callback)
 
     self._scroller = Scroller([
       self._sunnylink_toggle,
@@ -51,8 +52,8 @@ class SunnylinkLayoutMici(NavWidget):
 
   def _update_state(self):
     super()._update_state()
-    self._sunnylink_enabled = ui_state.sunnylink_enabled
-    self._sunnylink_toggle.set_text(tr("enable sunnylink"))
+    self._sunnylink_enabled = ui_state.params.get("SunnylinkEnabled")
+    self._sunnylink_toggle.set_checked(self._sunnylink_enabled)
     self._sunnylink_pair_button.set_visible(self._sunnylink_enabled)
     self._sunnylink_sponsor_button.set_visible(self._sunnylink_enabled)
     self._backup_btn.set_visible(self._sunnylink_enabled)
@@ -83,7 +84,25 @@ class SunnylinkLayoutMici(NavWidget):
 
   @staticmethod
   def _sunnylink_toggle_callback(state: bool):
-    ui_state.params.put_bool("SunnylinkEnabled", state)
+    sl_consent: bool = ui_state.params.get("CompletedSunnylinkConsentVersion") == sunnylink_consent_version
+    sl_enabled: bool = ui_state.params.get("SunnylinkEnabled")
+
+    def sl_terms_accepted():
+      ui_state.params.put("CompletedSunnylinkConsentVersion", sunnylink_consent_version)
+      ui_state.params.put_bool("SunnylinkEnabled", True)
+      gui_app.set_modal_overlay(None)
+
+    def sl_terms_declined():
+      ui_state.params.put("CompletedSunnylinkConsentVersion", sunnylink_consent_declined)
+      ui_state.params.put_bool("SunnylinkEnabled", False)
+      gui_app.set_modal_overlay(None)
+
+    if state and not sl_consent and not sl_enabled:
+      sl_terms_dlg = SunnylinkConsentPage(on_accept=sl_terms_accepted, on_decline=sl_terms_declined)
+      gui_app.set_modal_overlay(sl_terms_dlg)
+    else:
+      ui_state.params.put_bool("SunnylinkEnabled", state)
+
     ui_state.update_params()
 
   @staticmethod

selfdrive/ui/sunnypilot/onroad/developer_ui/__init__.py

@@ -22,6 +22,7 @@ class DeveloperUiRenderer(Widget):
   DEV_UI_RIGHT = 1
   DEV_UI_BOTTOM = 2
   DEV_UI_BOTH = 3
+  BOTTOM_BAR_HEIGHT = 61
 
   def __init__(self):
     super().__init__()
@@ -43,6 +44,12 @@ class DeveloperUiRenderer(Widget):
     self.bearing_elem = BearingDegElement()
     self.altitude_elem = AltitudeElement()
 
+  @staticmethod
+  def get_bottom_dev_ui_offset():
+    if ui_state.developer_ui in (DeveloperUiRenderer.DEV_UI_BOTTOM, DeveloperUiRenderer.DEV_UI_BOTH):
+      return DeveloperUiRenderer.BOTTOM_BAR_HEIGHT
+    return 0
+
   def _update_state(self) -> None:
     self.dev_ui_mode = ui_state.developer_ui
 
@@ -78,10 +85,11 @@ class DeveloperUiRenderer(Widget):
     ]
     if controls_state.lateralControlState.which() == 'torqueState':
       elements.append(self.desired_lat_accel_elem.update(sm, ui_state.is_metric))
-      elements.append(self.actual_lat_accel_elem.update(sm, ui_state.is_metric))
     else:
       elements.append(self.desired_steer_elem.update(sm, ui_state.is_metric))
 
+    elements.append(self.actual_lat_accel_elem.update(sm, ui_state.is_metric))
+
     current_y = y
     for element in elements:
       current_y += self._draw_right_dev_ui_element(x, current_y, element)
@@ -105,7 +113,7 @@ class DeveloperUiRenderer(Widget):
     if element.unit:
       units_height = measure_text_cached(self._font_bold, element.unit, unit_size, 0).x
 
-      units_x = x + container_width - 10
+      units_x = x + container_width
       units_y = y + (value_size / 2) + (units_height / 2)
 
       rl.draw_text_pro(self._font_bold, element.unit, rl.Vector2(units_x, units_y), rl.Vector2(0, 0), -90.0, unit_size, 0, rl.WHITE)
@@ -143,22 +151,35 @@ class DeveloperUiRenderer(Widget):
     if sm.valid['gpsLocationExternal'] or sm.valid['gpsLocation']:
       elements.append(self.altitude_elem.update(sm, ui_state.is_metric))
 
-    current_x = int(rect.x + 90)
-    center_y = y + bar_height // 2
-    for element in elements:
-      current_x += self._draw_bottom_dev_ui_element(current_x, center_y, element)
+    if not elements:
+      return
 
-  def _draw_bottom_dev_ui_element(self, x: int, y: int, element: UiElement) -> int:
     font_size = 38
+    element_widths = []
+    for element in elements:
+      element.measure(self._font_bold, font_size)
+      element_widths.append(element.total_width)
 
-    label_text = f"{element.label} "
-    label_width = measure_text_cached(self._font_bold, label_text, font_size, 0).x
-    rl.draw_text_ex(self._font_bold, label_text, rl.Vector2(x, y - font_size // 2), font_size, 0, rl.WHITE)
+    total_element_width = sum(element_widths)
+    num_gaps = len(elements) + 1
+    available_width = rect.width
+    gap_width = (available_width - total_element_width) / num_gaps
 
-    value_width = measure_text_cached(self._font_bold, element.value, font_size, 0).x
-    rl.draw_text_ex(self._font_bold, element.value, rl.Vector2(x + label_width + 10, y - font_size // 2), font_size, 0, element.color)
+    center_y = y + bar_height // 2
+    current_x = rect.x + gap_width
+
+    for i, element in enumerate(elements):
+      element_center_x = int(current_x + element_widths[i] / 2)
+      self._draw_bottom_dev_ui_element(element_center_x, center_y, element)
+      current_x += element_widths[i] + gap_width
+
+  def _draw_bottom_dev_ui_element(self, center_x: int, y: int, element: UiElement) -> None:
+    font_size = 38
+    start_x = center_x - element.total_width / 2
+
+    rl.draw_text_ex(self._font_bold, element.label_text, rl.Vector2(start_x, y - font_size // 2), font_size, 0, rl.WHITE)
+    rl.draw_text_ex(self._font_bold, element.val_text, rl.Vector2(start_x + element.label_width, y - font_size // 2), font_size, 0, element.color)
 
     if element.unit:
-      rl.draw_text_ex(self._font_bold, element.unit, rl.Vector2(x + label_width + value_width + 20, y - font_size // 2), font_size, 0, rl.WHITE)
-
-    return 400
+      rl.draw_text_ex(self._font_bold, element.unit_text, rl.Vector2(start_x + element.label_width + element.val_width, y - font_size // 2),
+                      font_size, 0, rl.WHITE)

selfdrive/ui/sunnypilot/onroad/developer_ui/elements.py

@@ -10,12 +10,33 @@ from dataclasses import dataclass
 from openpilot.common.constants import CV
 
 
+from openpilot.system.ui.lib.text_measure import measure_text_cached
+
+
 @dataclass
 class UiElement:
   value: str
   label: str
   unit: str
   color: rl.Color
+  val_text: str = ""
+  label_text: str = ""
+  unit_text: str = ""
+  val_width: float = 0.0
+  label_width: float = 0.0
+  unit_width: float = 0.0
+  total_width: float = 0.0
+
+  def measure(self, font, font_size: int):
+    self.label_text = f"{self.label} "
+    self.val_text = self.value
+    self.unit_text = f" {self.unit}" if self.unit else ""
+
+    self.label_width = measure_text_cached(font, self.label_text, font_size, 0).x
+    self.val_width = measure_text_cached(font, self.val_text, font_size, 0).x
+    self.unit_width = measure_text_cached(font, self.unit_text, font_size, 0).x if self.unit else 0
+
+    self.total_width = self.label_width + self.val_width + self.unit_width
 
 
 class LeadInfoElement:

selfdrive/ui/sunnypilot/onroad/driver_state.py

@@ -0,0 +1,49 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import numpy as np
+
+from openpilot.selfdrive.ui import UI_BORDER_SIZE
+from openpilot.selfdrive.ui.onroad.driver_state import DriverStateRenderer, BTN_SIZE, ARC_LENGTH
+from openpilot.selfdrive.ui.sunnypilot.onroad.developer_ui import DeveloperUiRenderer
+
+
+class DriverStateRendererSP(DriverStateRenderer):
+  def __init__(self):
+    super().__init__()
+    self.dev_ui_offset = DeveloperUiRenderer.get_bottom_dev_ui_offset()
+
+  def _pre_calculate_drawing_elements(self):
+    """Pre-calculate all drawing elements based on the current rectangle"""
+    # Calculate icon position (bottom-left or bottom-right)
+    width, height = self._rect.width, self._rect.height
+    offset = UI_BORDER_SIZE + BTN_SIZE // 2
+    self.position_x = self._rect.x + (width - offset if self.is_rhd else offset)
+    self.position_y = self._rect.y + height - offset - self.dev_ui_offset
+
+    # Pre-calculate the face lines positions
+    positioned_keypoints = self.face_keypoints_transformed + np.array([self.position_x, self.position_y])
+    for i in range(len(positioned_keypoints)):
+      self.face_lines[i].x = positioned_keypoints[i][0]
+      self.face_lines[i].y = positioned_keypoints[i][1]
+
+    # Calculate arc dimensions based on head rotation
+    delta_x = -self.driver_pose_sins[1] * ARC_LENGTH / 2.0  # Horizontal movement
+    delta_y = -self.driver_pose_sins[0] * ARC_LENGTH / 2.0  # Vertical movement
+
+    # Horizontal arc
+    h_width = abs(delta_x)
+    self.h_arc_data = self._calculate_arc_data(
+      delta_x, h_width, self.position_x, self.position_y - ARC_LENGTH / 2,
+      self.driver_pose_sins[1], self.driver_pose_diff[1], is_horizontal=True
+    )
+
+    # Vertical arc
+    v_height = abs(delta_y)
+    self.v_arc_data = self._calculate_arc_data(
+      delta_y, v_height, self.position_x - ARC_LENGTH / 2, self.position_y,
+      self.driver_pose_sins[0], self.driver_pose_diff[0], is_horizontal=False
+    )

selfdrive/ui/sunnypilot/ui_state.py

@@ -17,13 +17,18 @@ class UIStateSP:
     self.params = Params()
     self.sm_services_ext = [
       "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
-      "gpsLocation", "liveTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "liveDelay"
+      "gpsLocation", "liveTorqueParameters", "carStateSP", "carParamsSP", "liveDelay"
     ]
 
     self.sunnylink_state = SunnylinkState()
 
+    self.custom_interactive_timeout: int = self.params.get("InteractivityTimeout", return_default=True)
+
   def update(self) -> None:
-    self.sunnylink_state.start()
+    if self.sunnylink_enabled:
+      self.sunnylink_state.start()
+    else:
+      self.sunnylink_state.stop()
 
   @staticmethod
   def update_status(ss, ss_sp, onroad_evt) -> str:
@@ -70,3 +75,14 @@ class UIStateSP:
     self.developer_ui = self.params.get("DevUIInfo")
     self.rainbow_path = self.params.get_bool("RainbowMode")
     self.chevron_metrics = self.params.get("ChevronInfo")
+    self.active_bundle = self.params.get("ModelManager_ActiveBundle")
+    self.custom_interactive_timeout = self.params.get("InteractivityTimeout", return_default=True)
+
+
+class DeviceSP:
+  def __init__(self):
+    self._params = Params()
+
+  def _set_awake(self, on: bool):
+    if on and self._params.get("DeviceBootMode", return_default=True) == 1:
+      self._params.put_bool("OffroadMode", True)

selfdrive/ui/tests/diff/replay.py

@@ -13,7 +13,7 @@ if "RECORD_OUTPUT" not in os.environ:
 os.environ["RECORD_OUTPUT"] = os.path.join(DIFF_OUT_DIR, os.environ["RECORD_OUTPUT"])
 
 from openpilot.common.params import Params
-from openpilot.system.version import terms_version, training_version
+from openpilot.system.version import terms_version, training_version, terms_version_sp, sunnylink_consent_version
 from openpilot.system.ui.lib.application import gui_app, MousePos, MouseEvent
 from openpilot.selfdrive.ui.ui_state import ui_state
 from openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout
@@ -45,6 +45,8 @@ def setup_state():
   params.put("CompletedTrainingVersion", training_version)
   params.put("DongleId", "test123456789")
   params.put("UpdaterCurrentDescription", "0.10.1 / test-branch / abc1234 / Nov 30")
+  params.put("HasAcceptedTermsSP", terms_version_sp)
+  params.put("CompletedSunnylinkConsentVersion", sunnylink_consent_version)
   return None
 
 

selfdrive/ui/tests/test_ui/raylib_screenshots.py

@@ -18,7 +18,7 @@ from openpilot.common.prefix import OpenpilotPrefix
 from openpilot.selfdrive.test.helpers import with_processes
 from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
 from openpilot.system.updated.updated import parse_release_notes
-from openpilot.system.version import terms_version, training_version
+from openpilot.system.version import terms_version, training_version, terms_version_sp, sunnylink_consent_version
 
 AlertSize = log.SelfdriveState.AlertSize
 AlertStatus = log.SelfdriveState.AlertStatus
@@ -378,6 +378,8 @@ def create_screenshots():
       # Set terms and training version (to skip onboarding)
       params.put("HasAcceptedTerms", terms_version)
       params.put("CompletedTrainingVersion", training_version)
+      params.put("HasAcceptedTermsSP", terms_version_sp)
+      params.put("CompletedSunnylinkConsentVersion", sunnylink_consent_version)
 
       if name == "homescreen_paired":
         params.put("PrimeType", 0)  # NONE

selfdrive/ui/ui_state.py

@@ -12,7 +12,7 @@ from openpilot.selfdrive.ui.lib.prime_state import PrimeState
 from openpilot.system.ui.lib.application import gui_app
 from openpilot.system.hardware import HARDWARE, PC
 
-from openpilot.selfdrive.ui.sunnypilot.ui_state import UIStateSP
+from openpilot.selfdrive.ui.sunnypilot.ui_state import UIStateSP, DeviceSP
 
 BACKLIGHT_OFFROAD = 65 if HARDWARE.get_device_type() == "mici" else 50
 
@@ -192,8 +192,9 @@ class UIState(UIStateSP):
     self._param_update_time = time.monotonic()
 
 
-class Device:
+class Device(DeviceSP):
   def __init__(self):
+    DeviceSP.__init__(self)
     self._ignition = False
     self._interaction_time: float = -1
     self._override_interactive_timeout: int | None = None
@@ -220,6 +221,9 @@ class Device:
     if self._override_interactive_timeout is not None:
       return self._override_interactive_timeout
 
+    if gui_app.sunnypilot_ui() and ui_state.custom_interactive_timeout != 0:
+      return ui_state.custom_interactive_timeout
+
     ignition_timeout = 10 if gui_app.big_ui() else 5
     return ignition_timeout if ui_state.ignition else 30
 
@@ -284,6 +288,7 @@ class Device:
 
   def _set_awake(self, on: bool):
     if on != self._awake:
+      DeviceSP._set_awake(self, on)
       self._awake = on
       cloudlog.debug(f"setting display power {int(on)}")
       HARDWARE.set_display_power(on)

sunnypilot/mapd/__init__.py

@@ -1,5 +0,0 @@
-import os
-from openpilot.common.basedir import BASEDIR
-
-MAPD_BIN_DIR = os.path.join(BASEDIR, 'third_party/mapd_pfeiferj')
-MAPD_PATH = os.path.join(MAPD_BIN_DIR, 'mapd')

sunnypilot/mapd/live_map_data/__init__.py

@@ -1,22 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-from openpilot.common.swaglog import cloudlog
-
-LOOK_AHEAD_HORIZON_TIME = 15.  # s. Time horizon for look ahead of turn speed sections to provide on liveMapDataSP msg.
-_DEBUG = False
-_CLOUDLOG_DEBUG = False
-ROAD_NAME_TIMEOUT = 30  # secs
-R = 6373000.0  # approximate radius of earth in mts
-QUERY_RADIUS = 3000  # mts. Radius to use on OSM data queries.
-QUERY_RADIUS_OFFLINE = 2250  # mts. Radius to use on offline OSM data queries.
-
-
-def get_debug(msg, log_to_cloud=True):
-  if _CLOUDLOG_DEBUG and log_to_cloud:
-    cloudlog.debug(msg)
-  if _DEBUG:
-    print(msg)

sunnypilot/mapd/live_map_data/base_map_data.py

@@ -1,65 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-from abc import abstractmethod, ABC
-
-import cereal.messaging as messaging
-from openpilot.common.params import Params
-from openpilot.common.constants import CV
-from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
-from openpilot.sunnypilot.navd.helpers import coordinate_from_param
-
-MAX_SPEED_LIMIT = V_CRUISE_UNSET * CV.KPH_TO_MS
-
-
-class BaseMapData(ABC):
-  def __init__(self):
-    self.params = Params()
-
-    self.sm = messaging.SubMaster(['liveLocationKalman'])
-    self.pm = messaging.PubMaster(['liveMapDataSP'])
-
-    self.localizer_valid = False
-    self.last_bearing = None
-    self.last_position = coordinate_from_param("LastGPSPositionLLK", self.params)
-
-  @abstractmethod
-  def update_location(self) -> None:
-    pass
-
-  @abstractmethod
-  def get_current_speed_limit(self) -> float:
-    pass
-
-  @abstractmethod
-  def get_next_speed_limit_and_distance(self) -> tuple[float, float]:
-    pass
-
-  @abstractmethod
-  def get_current_road_name(self) -> str:
-    pass
-
-  def publish(self) -> None:
-    speed_limit = self.get_current_speed_limit()
-    next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance()
-
-    mapd_sp_send = messaging.new_message('liveMapDataSP')
-    mapd_sp_send.valid = self.sm['liveLocationKalman'].gpsOK
-    live_map_data = mapd_sp_send.liveMapDataSP
-
-    live_map_data.speedLimitValid = bool(MAX_SPEED_LIMIT > speed_limit > 0)
-    live_map_data.speedLimit = speed_limit
-    live_map_data.speedLimitAheadValid = bool(MAX_SPEED_LIMIT > next_speed_limit > 0)
-    live_map_data.speedLimitAhead = next_speed_limit
-    live_map_data.speedLimitAheadDistance = next_speed_limit_distance
-    live_map_data.roadName = self.get_current_road_name()
-
-    self.pm.send('liveMapDataSP', mapd_sp_send)
-
-  def tick(self) -> None:
-    self.sm.update(0)
-    self.update_location()
-    self.publish()

sunnypilot/mapd/live_map_data/debug.py

@@ -1,56 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-# DISCLAIMER: This code is intended principally for development and debugging purposes.
-# Although it provides a standalone entry point to the program, users should refer
-# to the actual implementations for consumption. Usage outside of development scenarios
-# is not advised and could lead to unpredictable results.
-
-import threading
-import traceback
-
-from cereal import messaging
-from openpilot.common.gps import get_gps_location_service
-from openpilot.common.params import Params
-from openpilot.common.realtime import config_realtime_process
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Policy
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver
-from openpilot.sunnypilot.mapd.live_map_data import get_debug
-
-
-def excepthook(args):
-  get_debug(f'MapD: Threading exception:\n{args}')
-  traceback.print_exception(args.exc_type, args.exc_value, args.exc_traceback)
-
-
-def live_map_data_sp_thread():
-  config_realtime_process([0, 1, 2, 3], 5)
-
-  params = Params()
-  gps_location_service = get_gps_location_service(params)
-
-  while True:
-    live_map_data_sp_thread_debug(gps_location_service)
-
-
-def live_map_data_sp_thread_debug(gps_location_service):
-  _sub_master = messaging.SubMaster(['carState', 'livePose', 'liveMapDataSP', 'longitudinalPlanSP', 'carStateSP', gps_location_service])
-  _sub_master.update()
-
-  v_ego = _sub_master['carState'].vEgo
-  _resolver = SpeedLimitResolver()
-  _resolver.policy = Policy.car_state_priority
-  _resolver.update(v_ego, _sub_master)
-  print(_resolver.speed_limit, _resolver.distance, _resolver.source)
-
-
-def main():
-  threading.excepthook = excepthook
-  live_map_data_sp_thread()
-
-
-if __name__ == "__main__":
-  main()

sunnypilot/mapd/live_map_data/osm_map_data.py

@@ -1,61 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import json
-import math
-import platform
-
-from cereal import log
-from openpilot.common.params import Params
-from openpilot.sunnypilot.mapd.live_map_data.base_map_data import BaseMapData
-from openpilot.sunnypilot.navd.helpers import Coordinate
-
-
-class OsmMapData(BaseMapData):
-  def __init__(self):
-    super().__init__()
-    self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
-
-  def update_location(self) -> None:
-    location = self.sm['liveLocationKalman']
-    self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
-
-    if self.localizer_valid:
-      self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
-      self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
-
-    if self.last_position is None:
-      return
-
-    params = {
-      "latitude": self.last_position.latitude,
-      "longitude": self.last_position.longitude,
-    }
-
-    if self.last_bearing is not None:
-      params['bearing'] = self.last_bearing
-
-    self.mem_params.put("LastGPSPosition", json.dumps(params))
-
-  def get_current_speed_limit(self) -> float:
-    return float(self.mem_params.get("MapSpeedLimit") or 0.0)
-
-  def get_current_road_name(self) -> str:
-    return str(self.mem_params.get("RoadName") or "")
-
-  def get_next_speed_limit_and_distance(self) -> tuple[float, float]:
-    next_speed_limit_section_str = self.mem_params.get("NextMapSpeedLimit")
-    next_speed_limit_section = next_speed_limit_section_str if next_speed_limit_section_str else {}
-    next_speed_limit = next_speed_limit_section.get('speedlimit', 0.0)
-    next_speed_limit_latitude = next_speed_limit_section.get('latitude')
-    next_speed_limit_longitude = next_speed_limit_section.get('longitude')
-    next_speed_limit_distance = 0.0
-
-    if next_speed_limit_latitude and next_speed_limit_longitude:
-      next_speed_limit_coordinates = Coordinate(next_speed_limit_latitude, next_speed_limit_longitude)
-      next_speed_limit_distance = (self.last_position or Coordinate(0, 0)).distance_to(next_speed_limit_coordinates)
-
-    return next_speed_limit, next_speed_limit_distance

sunnypilot/mapd/live_map_data/standalone.py

@@ -1,42 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-# DISCLAIMER: This code is intended principally for development and debugging purposes.
-# Although it provides a standalone entry point to the program, users should refer
-# to the actual implementations for consumption. Usage outside of development scenarios
-# is not advised and could lead to unpredictable results.
-
-import threading
-import traceback
-
-from openpilot.common.realtime import Ratekeeper, config_realtime_process
-from openpilot.sunnypilot.mapd.live_map_data import get_debug
-from openpilot.sunnypilot.mapd.live_map_data.osm_map_data import OsmMapData
-
-
-def excepthook(args):
-  get_debug(f'MapD: Threading exception:\n{args}')
-  traceback.print_exception(args.exc_type, args.exc_value, args.exc_traceback)
-
-
-def live_map_data_sp_thread():
-  config_realtime_process([0, 1, 2, 3], 5)
-
-  live_map_sp = OsmMapData()
-  rk = Ratekeeper(1, print_delay_threshold=None)
-
-  while True:
-    live_map_sp.tick()
-    rk.keep_time()
-
-
-def main():
-  threading.excepthook = excepthook
-  live_map_data_sp_thread()
-
-
-if __name__ == "__main__":
-  main()

sunnypilot/mapd/mapd_installer.py

@@ -1,154 +0,0 @@
-#!/usr/bin/env python3
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import logging
-import os
-import stat
-import time
-import traceback
-import requests
-from pathlib import Path
-from urllib.request import urlopen
-
-from cereal import messaging
-from openpilot.common.params import Params
-from openpilot.system.hardware.hw import Paths
-from openpilot.common.spinner import Spinner
-from openpilot.system.version import is_prebuilt
-from openpilot.sunnypilot.mapd import MAPD_PATH, MAPD_BIN_DIR
-import openpilot.system.sentry as sentry
-
-VERSION = "v1.12.0"
-URL = f"https://github.com/pfeiferj/openpilot-mapd/releases/download/{VERSION}/mapd"
-
-
-def update_installed_version(version: str, params: Params = None) -> None:
-  if params is None:
-    params = Params()
-
-  params.put("MapdVersion", version)
-
-
-class MapdInstallManager:
-  def __init__(self, spinner_ref: Spinner):
-    self._spinner = spinner_ref
-    self._params = Params()
-
-  def download(self) -> None:
-    self.ensure_directories_exist()
-    self._download_file()
-    update_installed_version(VERSION, self._params)
-
-  def check_and_download(self) -> None:
-    if self.download_needed():
-      self.download()
-
-  def download_needed(self) -> bool:
-    return not os.path.exists(MAPD_PATH) or self.get_installed_version() != VERSION
-
-  @staticmethod
-  def ensure_directories_exist() -> None:
-    if not os.path.exists(Paths.mapd_root()):
-      os.makedirs(Paths.mapd_root())
-    if not os.path.exists(MAPD_BIN_DIR):
-      os.makedirs(MAPD_BIN_DIR)
-
-  @staticmethod
-  def _safe_write_and_set_executable(file_path: Path, content: bytes) -> None:
-    with open(file_path, 'wb') as output:
-      output.write(content)
-      output.flush()
-      os.fsync(output.fileno())
-    current_permissions = stat.S_IMODE(os.lstat(file_path).st_mode)
-    os.chmod(file_path, current_permissions | stat.S_IEXEC)
-
-  def _download_file(self, num_retries=5) -> None:
-    temp_file = Path(MAPD_PATH + ".tmp")
-    download_timeout = 60
-    for cnt in range(num_retries):
-      try:
-        response = requests.get(URL, stream=True, timeout=download_timeout)
-        response.raise_for_status()
-        self._safe_write_and_set_executable(temp_file, response.content)
-        # No exceptions encountered. Safe to replace original file.
-        temp_file.replace(MAPD_PATH)
-        return
-      except requests.exceptions.ReadTimeout:
-        self._spinner.update(f"ReadTimeout caught. Timeout is [{download_timeout}]. Retrying download... [{cnt}]")
-        time.sleep(0.5)
-      except requests.exceptions.RequestException as e:
-        self._spinner.update(f"RequestException caught: {e}. Retrying download... [{cnt}]")
-        time.sleep(0.5)
-
-    # Delete temp file if the process was not successful.
-    if temp_file.exists():
-      temp_file.unlink()
-    logging.error("Failed to download file after all retries")
-
-  def get_installed_version(self) -> str:
-    return str(self._params.get("MapdVersion") or "")
-
-  def wait_for_internet_connection(self, return_on_failure: bool = False) -> bool:
-    max_retries = 10
-    for retries in range(max_retries + 1):
-      self._spinner.update(f"Waiting for internet connection... [{retries}/{max_retries}]")
-      time.sleep(2)
-      try:
-        _ = urlopen('https://sentry.io', timeout=10)
-        return True
-      except Exception as e:
-        print(f'Wait for internet failed: {e}')
-        if return_on_failure and retries == max_retries:
-          return False
-
-    return False
-
-  def non_prebuilt_install(self) -> None:
-    sm = messaging.SubMaster(['deviceState'])
-    metered = sm['deviceState'].networkMetered
-
-    if metered:
-      self._spinner.update("Can't proceed with mapd install since network is metered!")
-      time.sleep(5)
-      return
-
-    try:
-      self.ensure_directories_exist()
-      if not self.download_needed():
-        self._spinner.update("Mapd is good!")
-        time.sleep(0.1)
-        return
-
-      if self.wait_for_internet_connection(return_on_failure=True):
-        self._spinner.update(f"Downloading pfeiferj's mapd [{self.get_installed_version()}] => [{VERSION}].")
-        time.sleep(0.1)
-        self.check_and_download()
-      self._spinner.close()
-
-    except Exception:
-      for i in range(6):
-        self._spinner.update("Failed to download OSM maps won't work until properly downloaded!" +
-                             "Try again manually rebooting. " +
-                             f"Boot will continue in {5 - i}s...")
-        time.sleep(1)
-
-      sentry.init(sentry.SentryProject.SELFDRIVE)
-      traceback.print_exc()
-      sentry.capture_exception()
-
-
-if __name__ == "__main__":
-  spinner = Spinner()
-  install_manager = MapdInstallManager(spinner)
-  install_manager.ensure_directories_exist()
-  if is_prebuilt():
-    debug_msg = f"[DEBUG] This is prebuilt, no mapd install required. VERSION: [{VERSION}], Param [{install_manager.get_installed_version()}]"
-    spinner.update(debug_msg)
-    update_installed_version(VERSION)
-  else:
-    spinner.update(f"Checking if mapd is installed and valid. Prebuilt [{is_prebuilt()}]")
-    install_manager.non_prebuilt_install()

sunnypilot/mapd/mapd_manager.py

@@ -1,144 +0,0 @@
-#!/usr/bin/env python3
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import json
-import platform
-import os
-import glob
-import shutil
-from datetime import datetime
-
-from openpilot.common.params import Params
-from openpilot.common.realtime import Ratekeeper, config_realtime_process
-from openpilot.common.swaglog import cloudlog
-from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
-from openpilot.sunnypilot.mapd.live_map_data.osm_map_data import OsmMapData
-from openpilot.system.hardware.hw import Paths
-from openpilot.sunnypilot.mapd import MAPD_PATH
-from openpilot.sunnypilot.mapd.mapd_installer import VERSION, update_installed_version
-
-# PFEIFER - MAPD {{
-params = Params()
-mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else params
-# }} PFEIFER - MAPD
-
-
-def get_files_for_cleanup() -> list[str]:
-  paths = [
-    f"{Paths.mapd_root()}/db",
-    f"{Paths.mapd_root()}/v*"
-  ]
-  files_to_remove = []
-  for path in paths:
-    if os.path.exists(path):
-      files = glob.glob(path + '/**', recursive=True)
-      files_to_remove.extend(files)
-  # check for version and mapd files
-  if not os.path.isfile(MAPD_PATH):
-    files_to_remove.append(MAPD_PATH)
-  return files_to_remove
-
-
-def cleanup_old_osm_data(files_to_remove: list[str]) -> None:
-  for file in files_to_remove:
-    # Remove trailing slash if path is file
-    if file.endswith('/') and os.path.isfile(file[:-1]):
-      file = file[:-1]
-    # Try to remove as file or symbolic link first
-    if os.path.islink(file) or os.path.isfile(file):
-      os.remove(file)
-    elif os.path.isdir(file):  # If it's a directory
-      shutil.rmtree(file, ignore_errors=False)
-
-
-def request_refresh_osm_location_data(nations: list[str], states: list[str] = None) -> None:
-  params.put("OsmDownloadedDate", str(datetime.now().timestamp()))
-  params.put_bool("OsmDbUpdatesCheck", False)
-
-  osm_download_locations = {
-    "nations": nations,
-    "states": states or []
-  }
-
-  print(f"Downloading maps for {json.dumps(osm_download_locations)}")
-  mem_params.put("OSMDownloadLocations", osm_download_locations)
-
-
-def filter_nations_and_states(nations: list[str], states: list[str] = None) -> tuple[list[str], list[str]]:
-  """Filters and prepares nation and state data for OSM map download.
-
-  If the nation is 'US' and a specific state is provided, the nation 'US' is removed from the list.
-  If the nation is 'US' and the state is 'All', the 'All' is removed from the list.
-  The idea behind these filters is that if a specific state in the US is provided,
-  there's no need to download map data for the entire US. Conversely,
-  if the state is unspecified (i.e., 'All'), we intend to download map data for the whole US,
-  and 'All' isn't a valid state name, so it's removed.
-
-  Parameters:
-  nations (list): A list of nations for which the map data is to be downloaded.
-  states (list, optional): A list of states for which the map data is to be downloaded. Defaults to None.
-
-  Returns:
-  tuple: Two lists. The first list is filtered nations and the second list is filtered states.
-  """
-
-  if "US" in nations and states and not any(x.lower() == "all" for x in states):
-    # If a specific state in the US is provided, remove 'US' from nations
-    nations.remove("US")
-  elif "US" in nations and states and any(x.lower() == "all" for x in states):
-    # If 'All' is provided as a state (case invariant), remove those instances from states
-    states = [x for x in states if x.lower() != "all"]
-  elif "US" not in nations and states and any(x.lower() == "all" for x in states):
-    states.remove("All")
-  return nations, states or []
-
-
-def update_osm_db() -> None:
-  if params.get_bool("OsmDbUpdatesCheck"):
-    cleanup_old_osm_data(get_files_for_cleanup())
-    country = params.get("OsmLocationName", return_default=True)
-    state = params.get("OsmStateName", return_default=True)
-    filtered_nations, filtered_states = filter_nations_and_states([country], [state])
-    request_refresh_osm_location_data(filtered_nations, filtered_states)
-
-  if not mem_params.get("OSMDownloadBounds"):
-    mem_params.put("OSMDownloadBounds", "")
-
-  if not mem_params.get("LastGPSPosition"):
-    mem_params.put("LastGPSPosition", "{}")
-
-
-def main_thread():
-  update_installed_version(VERSION, params)
-  config_realtime_process([0, 1, 2, 3], 5)
-
-  rk = Ratekeeper(1, print_delay_threshold=None)
-  live_map_sp = OsmMapData()
-
-  # Create folder needed for OSM
-  try:
-    os.mkdir(Paths.mapd_root())
-  except FileExistsError:
-    pass
-  except PermissionError:
-    cloudlog.exception(f"mapd: failed to make {Paths.mapd_root()}")
-
-  while True:
-    show_alert = get_files_for_cleanup() and params.get_bool("OsmLocal")
-    set_offroad_alert("Offroad_OSMUpdateRequired", show_alert, "This alert will be cleared when new maps are downloaded.")
-
-    update_osm_db()
-    live_map_sp.tick()
-    rk.keep_time()
-
-
-def main():
-  main_thread()
-
-
-if __name__ == "__main__":
-  main()

sunnypilot/mapd/tests/mapd_hash

@@ -1 +0,0 @@
-fdb3b49ee19956e6ce09fdc3373cbba557f1263b2180e9f344c1d4053852284b

sunnypilot/mapd/tests/test_mapd_version.py

@@ -1,19 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-from openpilot.sunnypilot import get_file_hash
-from openpilot.sunnypilot.mapd import MAPD_PATH
-from openpilot.sunnypilot.mapd.update_version import MAPD_HASH_PATH
-
-
-class TestMapdVersion:
-  def test_compare_versions(self):
-    mapd_hash = get_file_hash(MAPD_PATH)
-
-    with open(MAPD_HASH_PATH) as f:
-      current_hash = f.read().strip()
-
-    assert current_hash == mapd_hash, "Run sunnypilot/mapd/update_version.py to update the current mapd version and hash"

sunnypilot/mapd/update_version.py

@@ -1,97 +0,0 @@
-#!/usr/bin/env python3
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import argparse
-import os
-import re
-
-from openpilot.sunnypilot import get_file_hash
-from openpilot.common.basedir import BASEDIR
-from openpilot.sunnypilot.mapd import MAPD_PATH
-
-MAPD_HASH_PATH = os.path.join(BASEDIR, "sunnypilot", "mapd", "tests", "mapd_hash")
-MAPD_VERSION_PATH = os.path.join(BASEDIR, "sunnypilot", "mapd", "mapd_installer.py")
-
-
-def update_mapd_hash():
-  mapd_hash = get_file_hash(MAPD_PATH)
-
-  with open(MAPD_HASH_PATH, "w") as f:
-    f.write(mapd_hash)
-
-  print(f"Generated and updated new mapd hash to {MAPD_HASH_PATH}")
-
-
-def get_current_mapd_version(path: str) -> str:
-  print("[GET CURRENT MAPD VERSION]")
-  with open(path) as f:
-    for line in f:
-      if line.strip().startswith("VERSION"):
-        # Match VERSION = 'v1.11.0' or VERSION="v1.11.0" (with optional spaces)
-        match = re.search(r'VERSION\s*=\s*[\'"]([^\'"]+)[\'"]', line)
-        if match:
-          ver = match.group(1)
-          print(f'Current mapd version: "{ver}"')
-          return ver
-        else:
-          print("[ERROR] VERSION line found but no quoted value detected.")
-          return ""
-  print("[ERROR] VERSION not found in file!")
-  return ""
-
-
-def update_mapd_version(ver: str, path: str):
-  print("[CHANGE CURRENT MAPD VERSION]")
-
-  with open(path) as f:
-    lines = f.readlines()
-
-  found = False
-  new_lines = []
-  for line in lines:
-    if not found and line.startswith("VERSION ="):
-      new_lines.append(f'VERSION = "{ver}"\n')
-      found = True
-      new_lines.extend(lines[lines.index(line) + 1:])
-      break
-    else:
-      new_lines.append(line)
-
-  if not found:
-    print("[ERROR] VERSION line not found! Aborting without writing.")
-    return
-
-  with open(path, "w") as f:
-    f.writelines(new_lines)
-
-  print(f'New mapd version: "{ver}"')
-  print("[DONE]")
-
-
-if __name__ == "__main__":
-  parser = argparse.ArgumentParser(description="Update mapd version and hash")
-  parser.add_argument("--new_ver", type=str, help="New mapd version")
-  args = parser.parse_args()
-
-  if not args.new_ver:
-    print("Warning: No new mapd version provided. Use --new_ver to specify")
-    print("Example:")
-    print("  python sunnypilot/mapd/update_version.py --new_ver \"v1.12.0\"")
-    print("Current mapd version and hash will not be updated! (aborted)")
-    exit(0)
-
-  current_ver = get_current_mapd_version(MAPD_VERSION_PATH)
-  new_ver = f"{args.new_ver}"
-  if current_ver == new_ver:
-    print(f'Proposed mapd version: "{new_ver}"')
-    confirm = input("Proposed mapd version is the same as the current mapd version. Confirm? (y/n): ").upper().strip()
-    if confirm != "Y":
-      print("Current mapd version and hash will not be updated! (aborted)")
-      exit(0)
-
-  update_mapd_version(new_ver, MAPD_VERSION_PATH)
-  update_mapd_hash()

sunnypilot/modeld/modeld.py

@@ -24,6 +24,7 @@ from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld.runners import ModelRunner, Runtime
 from openpilot.sunnypilot.modeld.parse_model_outputs import Parser
 from openpilot.sunnypilot.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
+from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
 from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan
 from openpilot.sunnypilot.models.helpers import get_active_bundle, get_model_path, load_metadata, prepare_inputs, load_meta_constants
 from openpilot.sunnypilot.modeld.models.commonmodel_pyx import ModelFrame, CLContext
@@ -195,6 +196,7 @@ def main(demo=False):
   buf_main, buf_extra = None, None
   meta_main = FrameMeta()
   meta_extra = FrameMeta()
+  camera_offset_helper = CameraOffsetHelper()
 
 
   if demo:
@@ -250,12 +252,14 @@ def main(demo=False):
     frame_id = sm["roadCameraState"].frameId
     if sm.frame % 60 == 0:
       model.lat_delay = get_lat_delay(params, sm["liveDelay"].lateralDelay)
+      camera_offset_helper.set_offset(params.get("CameraOffset", return_default=True))
     lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS
     if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
       dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
       model_transform_main = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics, False).astype(np.float32)
       model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics, True).astype(np.float32)
+      model_transform_main, model_transform_extra = camera_offset_helper.update(model_transform_main, model_transform_extra, sm, main_wide_camera)
       live_calib_seen = True
 
     traffic_convention = np.zeros(2)

sunnypilot/modeld_v2/SConscript

@@ -1,3 +1,4 @@
+import os
 import glob
 
 Import('env', 'envCython', 'arch', 'cereal', 'messaging', 'common', 'visionipc', 'transformations')
@@ -28,3 +29,38 @@ for pathdef, fn in {'TRANSFORM': 'transforms/transform.cl', 'LOADYUV': 'transfor
 cython_libs = envCython["LIBS"] + libs
 commonmodel_lib = lenv.Library('commonmodel', common_src)
 lenvCython.Program('models/commonmodel_pyx.so', 'models/commonmodel_pyx.pyx', LIBS=[commonmodel_lib, *cython_libs], FRAMEWORKS=frameworks)
+tinygrad_files = ["#"+x for x in glob.glob(env.Dir("#tinygrad_repo").relpath + "/**", recursive=True, root_dir=env.Dir("#").abspath) if 'pycache' not in x]
+
+# Get model metadata
+PC = not os.path.isfile('/TICI')
+if PC:
+  inputs = tinygrad_files + [File(Dir("#sunnypilot/modeld_v2").File("install_models_pc.py").abspath)]
+  outputs = []
+  model_dir = Dir("models").abspath
+  cmd = f'python3 {Dir("#sunnypilot/modeld_v2").abspath}/install_models_pc.py {model_dir}'
+
+  for model_name in ['supercombo', 'driving_vision', 'driving_policy']:
+    if File(f"models/{model_name}.onnx").exists():
+        inputs.append(File(f"models/{model_name}.onnx"))
+        inputs.append(File(f"models/{model_name}_tinygrad.pkl"))
+        outputs.append(File(f"models/{model_name}_metadata.pkl"))
+  if outputs:
+    lenv.Command(outputs, inputs, cmd)
+
+def tg_compile(flags, model_name):
+  pythonpath_string = 'PYTHONPATH="${PYTHONPATH}:' + env.Dir("#tinygrad_repo").abspath + '"'
+  fn = File(f"models/{model_name}").abspath
+  return lenv.Command(
+    fn + "_tinygrad.pkl",
+    [fn + ".onnx"] + tinygrad_files,
+    f'{pythonpath_string} {flags} python3 {Dir("#tinygrad_repo").abspath}/examples/openpilot/compile3.py {fn}.onnx {fn}_tinygrad.pkl'
+  )
+
+# Compile small models
+for model_name in ['supercombo', 'driving_vision', 'driving_policy']:
+  if File(f"models/{model_name}.onnx").exists():
+    flags = {
+      'larch64': 'DEV=QCOM',
+      'Darwin': f'DEV=CPU HOME={os.path.expanduser("~")} IMAGE=0', # tinygrad calls brew which needs a $HOME in the env
+    }.get(arch, 'DEV=CPU CPU_LLVM=1 IMAGE=0')
+    tg_compile(flags, model_name)

sunnypilot/modeld_v2/camera_offset_helper.py

@@ -0,0 +1,39 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import numpy as np
+
+from openpilot.common.transformations.camera import DEVICE_CAMERAS
+
+
+class CameraOffsetHelper:
+  def __init__(self):
+    self.camera_offset = 0.0
+    self.actual_camera_offset = 0.0
+
+  @staticmethod
+  def apply_camera_offset(model_transform, intrinsics, height, offset_param):
+    cy = intrinsics[1, 2]
+    shear = np.eye(3, dtype=np.float32)
+    shear[0, 1] = offset_param / height
+    shear[0, 2] = -offset_param / height * cy
+    model_transform = (shear @ model_transform).astype(np.float32)
+    return model_transform
+
+  def set_offset(self, offset):
+    self.camera_offset = offset
+
+  def update(self, model_transform_main, model_transform_extra, sm, main_wide_camera):
+    self.actual_camera_offset = (0.9 * self.actual_camera_offset) + (0.1 * self.camera_offset)
+    dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
+    height = sm["liveCalibration"].height[0] if sm['liveCalibration'].height else 1.22
+
+    intrinsics_main = dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics
+    model_transform_main = self.apply_camera_offset(model_transform_main, intrinsics_main, height, self.actual_camera_offset)
+
+    intrinsics_extra = dc.ecam.intrinsics
+    model_transform_extra = self.apply_camera_offset(model_transform_extra, intrinsics_extra, height, self.actual_camera_offset)
+    return model_transform_main, model_transform_extra

sunnypilot/modeld_v2/install_models_pc.py

@@ -0,0 +1,89 @@
+#!/usr/bin/env python3
+import sys
+import shutil
+import pickle
+import codecs
+import onnx
+from pathlib import Path
+
+from openpilot.system.hardware.hw import Paths
+
+
+def get_name_and_shape(value_info):
+  shape = tuple([int(dim.dim_value) for dim in value_info.type.tensor_type.shape.dim])
+  return value_info.name, shape
+
+
+def get_metadata_value_by_name(model, name):
+  for prop in model.metadata_props:
+    if prop.key == name:
+      return prop.value
+  return None
+
+
+def generate_metadata_pkl(model_path, output_path):
+  try:
+    model = onnx.load(str(model_path))
+    output_slices = get_metadata_value_by_name(model, 'output_slices')
+
+    if output_slices:
+      metadata = {
+        'model_checkpoint': get_metadata_value_by_name(model, 'model_checkpoint'),
+        'output_slices': pickle.loads(codecs.decode(output_slices.encode(), "base64")),
+        'input_shapes': dict([get_name_and_shape(x) for x in model.graph.input]),
+        'output_shapes': dict([get_name_and_shape(x) for x in model.graph.output])
+      }
+      with open(output_path, 'wb') as f:
+        pickle.dump(metadata, f)
+      return True
+    else:
+      return False
+  except Exception:
+    return False
+
+
+def install_models(model_dir):
+  model_dir = Path(model_dir)
+  models = ["driving_policy", "driving_vision"]
+  found_models = []
+
+  for model in models:
+    if (model_dir / f"{model}.onnx").exists():
+      found_models.append(model)
+
+  if not found_models:
+    return
+
+  try:
+    custom_name = input(f"Found models ({', '.join(found_models)}). Enter model short name (e.g. wmiv4): ").strip()
+  except EOFError:
+    return
+
+  if not custom_name:
+    print("No name provided, skipping installation.")
+    return
+
+  dest_dir = Path(Paths.model_root())
+  dest_dir.mkdir(parents=True, exist_ok=True)
+
+  for model in found_models:
+    onnx_path = model_dir / f"{model}.onnx"
+    tinygrad_pkl = model_dir / f"{model}_tinygrad.pkl"
+    metadata_pkl = model_dir / f"{model}_metadata.pkl"
+
+    if not metadata_pkl.exists():
+      generate_metadata_pkl(onnx_path, metadata_pkl)
+
+    dest_tinygrad = dest_dir / f"{model}_{custom_name}_tinygrad.pkl"
+    dest_metadata = dest_dir / f"{model}_{custom_name}_metadata.pkl"
+
+    if tinygrad_pkl.exists():
+      shutil.move(str(tinygrad_pkl), str(dest_tinygrad))
+    if metadata_pkl.exists():
+      shutil.move(str(metadata_pkl), str(dest_metadata))
+
+if __name__ == "__main__":
+  if len(sys.argv) < 2:
+    print("Usage: install_models_pc.py <model_dir>")
+    sys.exit(1)
+  install_models(sys.argv[1])

sunnypilot/modeld_v2/modeld.py

@@ -21,6 +21,7 @@ from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_p
 from openpilot.sunnypilot.modeld_v2.constants import Plan
 from openpilot.sunnypilot.modeld_v2.models.commonmodel_pyx import DrivingModelFrame, CLContext
 from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants
+from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
 
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase
@@ -28,7 +29,6 @@ from openpilot.sunnypilot.models.helpers import get_active_bundle
 from openpilot.sunnypilot.models.runners.helpers import get_model_runner
 
 PROCESS_NAME = "selfdrive.modeld.modeld_tinygrad"
-RECOVERY_POWER = 1.0 # The higher this number the more aggressively the model will recover to lanecenter, too high and it will ping-pong
 
 
 class FrameMeta:
@@ -63,6 +63,7 @@ class ModelState(ModelStateBase):
     self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0"))
     self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0"))
     self.MIN_LAT_CONTROL_SPEED = 0.3
+    self.PLANPLUS_CONTROL: float = 1.0
 
     buffer_length = 5 if self.model_runner.is_20hz else 2
     self.frames = {name: DrivingModelFrame(context, buffer_length) for name in self.model_runner.vision_input_names}
@@ -158,7 +159,8 @@ class ModelState(ModelStateBase):
                             lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action:
     plan = model_output['plan'][0]
     if 'planplus' in model_output:
-      plan = plan + RECOVERY_POWER*model_output['planplus'][0]
+      recovery_power = self.PLANPLUS_CONTROL * (0.75 if v_ego > 20.0 else 1.0)
+      plan = plan + recovery_power * model_output['planplus'][0]
     desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS,
                                                      action_t=long_action_t)
     desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
@@ -229,6 +231,7 @@ def main(demo=False):
   buf_main, buf_extra = None, None
   meta_main = FrameMeta()
   meta_extra = FrameMeta()
+  camera_offset_helper = CameraOffsetHelper()
 
 
   if demo:
@@ -283,13 +286,15 @@ def main(demo=False):
     v_ego = max(sm["carState"].vEgo, 0.)
     if sm.frame % 60 == 0:
       model.lat_delay = get_lat_delay(params, sm["liveDelay"].lateralDelay)
+      model.PLANPLUS_CONTROL = params.get("PlanplusControl", return_default=True)
+      camera_offset_helper.set_offset(params.get("CameraOffset", return_default=True))
     lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS
     if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
       dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
-      model_transform_main = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics,
-                                             False).astype(np.float32)
+      model_transform_main = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics, False).astype(np.float32)
       model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics, True).astype(np.float32)
+      model_transform_main, model_transform_extra = camera_offset_helper.update(model_transform_main, model_transform_extra, sm, main_wide_camera)
       live_calib_seen = True
 
     traffic_convention = np.zeros(2)

sunnypilot/modeld_v2/tests/test_camera_offset_helper.py

@@ -0,0 +1,84 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import numpy as np
+
+from openpilot.common.transformations.camera import DEVICE_CAMERAS
+from openpilot.common.transformations.model import get_warp_matrix
+from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
+
+
+class MockStruct:
+  def __init__(self, **kwargs):
+    for k, v in kwargs.items():
+      setattr(self, k, v)
+
+  def __getitem__(self, item):
+    return getattr(self, item)
+
+
+class TestCameraOffset:
+  def setup_method(self):
+    self.camera_offset = CameraOffsetHelper()
+    self.dc = DEVICE_CAMERAS[('mici', 'os04c10')]
+
+  def test_smoothing(self):
+    self.camera_offset.set_offset(0.2)
+
+    sm = MockStruct(
+      deviceState=MockStruct(deviceType='mici'),
+      roadCameraState=MockStruct(sensor='os04c10'),
+      liveCalibration=MockStruct(rpyCalib=[0.0, 0.0, 0.0], height=[1.22])
+    )
+
+    intrinsics_main = self.dc.fcam.intrinsics
+    intrinsics_extra = self.dc.ecam.intrinsics
+    device_from_calib_euler = np.array([0.0, 0.0, 0.0], dtype=np.float32)
+    main_transform = get_warp_matrix(device_from_calib_euler, intrinsics_main, False).astype(np.float32)
+    extra_transform = get_warp_matrix(device_from_calib_euler, intrinsics_extra, True).astype(np.float32)
+
+    self.camera_offset.update(main_transform, extra_transform, sm, False)
+    np.testing.assert_almost_equal(self.camera_offset.actual_camera_offset, 0.02)
+    self.camera_offset.update(main_transform, extra_transform, sm, False)
+    np.testing.assert_almost_equal(self.camera_offset.actual_camera_offset, 0.038)
+
+  def test_camera_offset_(self):
+    intrinsics = self.dc.fcam.intrinsics
+    transform = np.eye(3, dtype=np.float32)
+    height = 1.22
+    offset = 0.1
+
+    cy = intrinsics[1, 2]
+    expected_shear = np.eye(3, dtype=np.float32)
+    expected_shear[0, 1] = offset / height
+    expected_shear[0, 2] = -offset / height * cy
+
+    result = CameraOffsetHelper.apply_camera_offset(transform, intrinsics, height, offset)
+    np.testing.assert_array_almost_equal(result, expected_shear)
+
+  def test_update(self):
+    sm = MockStruct(
+      deviceState=MockStruct(deviceType='mici'),
+      roadCameraState=MockStruct(sensor='os04c10'),
+      liveCalibration=MockStruct(rpyCalib=[0.0, 0.0, 0.0], height=[1.22])
+    )
+    intrinsics_main = self.dc.fcam.intrinsics
+    intrinsics_extra = self.dc.ecam.intrinsics
+    device_from_calib_euler = np.array([0.0, 0.0, 0.0], dtype=np.float32)
+    main_transform = get_warp_matrix(device_from_calib_euler, intrinsics_main, False).astype(np.float32)
+    extra_transform = get_warp_matrix(device_from_calib_euler, intrinsics_extra, True).astype(np.float32)
+
+    self.camera_offset.set_offset(0.0)  # test default offset doesn't change transformation
+    main_out, extra_out = self.camera_offset.update(main_transform, extra_transform, sm, False)
+    np.testing.assert_array_equal(main_out, main_transform)
+    np.testing.assert_array_equal(extra_out, extra_transform)
+
+    self.camera_offset.set_offset(0.2)  # test valid offset changes transformation
+    main_out, extra_out = self.camera_offset.update(main_transform, extra_transform, sm, False)
+    assert not np.array_equal(main_out, main_transform)
+    assert not np.array_equal(extra_out, extra_transform)
+    assert main_out[0, 1] != 0.0
+    assert main_out[0, 2] != 0.0

sunnypilot/modeld_v2/tests/test_modeld.py

@@ -1,102 +0,0 @@
-import numpy as np
-import random
-
-import cereal.messaging as messaging
-from msgq.visionipc import VisionIpcServer, VisionStreamType
-from opendbc.car.car_helpers import get_demo_car_params
-from openpilot.common.params import Params
-from openpilot.common.transformations.camera import DEVICE_CAMERAS
-from openpilot.common.realtime import DT_MDL
-from openpilot.system.manager.process_config import managed_processes
-from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state
-
-CAM = DEVICE_CAMERAS[("tici", "ar0231")].fcam
-IMG = np.zeros(int(CAM.width*CAM.height*(3/2)), dtype=np.uint8)
-IMG_BYTES = IMG.flatten().tobytes()
-
-
-class TestModeld:
-
-  def setup_method(self):
-    self.vipc_server = VisionIpcServer("camerad")
-    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, CAM.width, CAM.height)
-    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, CAM.width, CAM.height)
-    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, CAM.width, CAM.height)
-    self.vipc_server.start_listener()
-    Params().put("CarParams", get_demo_car_params().to_bytes())
-
-    self.sm = messaging.SubMaster(['modelV2', 'cameraOdometry'])
-    self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'liveCalibration'])
-
-    managed_processes['modeld'].start()
-    self.pm.wait_for_readers_to_update("roadCameraState", 10)
-
-  def teardown_method(self):
-    managed_processes['modeld'].stop()
-    del self.vipc_server
-
-  def _send_frames(self, frame_id, cams=None):
-    if cams is None:
-      cams = ('roadCameraState', 'wideRoadCameraState')
-
-    cs = None
-    for cam in cams:
-      msg = messaging.new_message(cam)
-      cs = getattr(msg, cam)
-      cs.frameId = frame_id
-      cs.timestampSof = int((frame_id * DT_MDL) * 1e9)
-      cs.timestampEof = int(cs.timestampSof + (DT_MDL * 1e9))
-      cam_meta = meta_from_camera_state(cam)
-
-      self.pm.send(msg.which(), msg)
-      self.vipc_server.send(cam_meta.stream, IMG_BYTES, cs.frameId,
-                            cs.timestampSof, cs.timestampEof)
-    return cs
-
-  def _wait(self):
-    self.sm.update(5000)
-    if self.sm['modelV2'].frameId != self.sm['cameraOdometry'].frameId:
-      self.sm.update(1000)
-
-  def test_modeld(self):
-    for n in range(1, 500):
-      cs = self._send_frames(n)
-      self._wait()
-
-      mdl = self.sm['modelV2']
-      assert mdl.frameId == n
-      assert mdl.frameIdExtra == n
-      assert mdl.timestampEof == cs.timestampEof
-      assert mdl.frameAge == 0
-      assert mdl.frameDropPerc == 0
-
-      odo = self.sm['cameraOdometry']
-      assert odo.frameId == n
-      assert odo.timestampEof == cs.timestampEof
-
-  def test_dropped_frames(self):
-    """
-      modeld should only run on consecutive road frames
-    """
-    frame_id = -1
-    road_frames = list()
-    for n in range(1, 50):
-      if (random.random() < 0.1) and n > 3:
-        cams = random.choice([(), ('wideRoadCameraState', )])
-        self._send_frames(n, cams)
-      else:
-        self._send_frames(n)
-        road_frames.append(n)
-      self._wait()
-
-      if len(road_frames) < 3 or road_frames[-1] - road_frames[-2] == 1:
-        frame_id = road_frames[-1]
-
-      mdl = self.sm['modelV2']
-      odo = self.sm['cameraOdometry']
-      assert mdl.frameId == frame_id
-      assert mdl.frameIdExtra == frame_id
-      assert odo.frameId == frame_id
-      if n != frame_id:
-        assert not self.sm.updated['modelV2']
-        assert not self.sm.updated['cameraOdometry']

sunnypilot/modeld_v2/tests/test_recovery_power.py

@@ -0,0 +1,61 @@
+import numpy as np
+
+from cereal import log
+
+from openpilot.sunnypilot.modeld_v2.constants import Plan
+from openpilot.sunnypilot.modeld_v2.modeld import ModelState
+import openpilot.sunnypilot.modeld_v2.modeld as modeld
+
+
+class MockStruct:
+  def __init__(self, **kwargs):
+    for k, v in kwargs.items():
+      setattr(self, k, v)
+
+
+def test_recovery_power_scaling():
+  state = MockStruct(
+    PLANPLUS_CONTROL=1.0,
+    LONG_SMOOTH_SECONDS=0.3,
+    LAT_SMOOTH_SECONDS=0.1,
+    MIN_LAT_CONTROL_SPEED=0.3,
+    mlsim=True,
+    generation=12,
+    constants=MockStruct(T_IDXS=np.arange(100), DESIRE_LEN=8)
+  )
+  prev_action = log.ModelDataV2.Action()
+  recorded_vel: list = []
+
+  def mock_accel(plan_vel, plan_accel, t_idxs, action_t=0.0):
+    recorded_vel.append(plan_vel.copy())
+    return 0.0, False
+
+  modeld.get_accel_from_plan = mock_accel
+  modeld.get_curvature_from_output = lambda *args: 0.0
+  plan = np.random.rand(1, 100, 15).astype(np.float32)
+  planplus = np.random.rand(1, 100, 15).astype(np.float32)
+
+  model_output: dict = {
+    'plan': plan.copy(),
+    'planplus': planplus.copy()
+  }
+
+  test_cases: list = [
+    # (control, v_ego, expected_factor)
+    (0.55, 20.0, 1.0),
+    (1.0, 25.0, .75),
+    (1.5, 25.1, 0.75),
+    (2.0, 20.0, 1.0),
+    (0.75, 19.0, 1.0),
+    (0.8, 25.1, 0.75),
+  ]
+
+  for control, v_ego, factor in test_cases:
+    state.PLANPLUS_CONTROL = control
+    recorded_vel.clear()
+    ModelState.get_action_from_model(state, model_output, prev_action, 0.0, 0.0, v_ego)
+
+    expected_recovery_power = control * factor
+    expected_plan_vel = plan[0, :, Plan.VELOCITY][:, 0] + expected_recovery_power * planplus[0, :, Plan.VELOCITY][:, 0]
+
+    np.testing.assert_allclose(recorded_vel[0], expected_plan_vel, rtol=1e-5, atol=1e-6)

sunnypilot/models/runners/helpers.py

@@ -2,25 +2,17 @@ from openpilot.sunnypilot.models.helpers import get_active_bundle
 from openpilot.sunnypilot.models.runners.model_runner import ModelRunner
 from openpilot.sunnypilot.models.runners.tinygrad.tinygrad_runner import TinygradRunner, TinygradSplitRunner
 from openpilot.sunnypilot.models.runners.constants import ModelType
-from openpilot.system.hardware import TICI
 
-if not TICI:
-  from openpilot.sunnypilot.models.runners.onnx.onnx_runner import ONNXRunner
 
 def get_model_runner() -> ModelRunner:
   """
   Factory function to create and return the appropriate ModelRunner instance.
 
-  Selects between ONNXRunner (for non-TICI platforms) and TinygradRunner
-  (for TICI platforms), choosing TinygradSplitRunner if separate vision/policy
+  Selects TinygradRunner, choosing TinygradSplitRunner if separate vision/policy
   models are detected in the active bundle.
 
   :return: An instance of a ModelRunner subclass (ONNXRunner, TinygradRunner, or TinygradSplitRunner).
   """
-  if not TICI:
-    return ONNXRunner()
-
-  # On TICI platforms, use Tinygrad runners
   bundle = get_active_bundle()
   if bundle and bundle.models:
     model_types = {m.type.raw for m in bundle.models}

sunnypilot/selfdrive/car/cruise_ext.py

@@ -6,16 +6,12 @@ See the LICENSE.md file in the root directory for more details.
 """
 import numpy as np
 
-from cereal import car, custom
+from cereal import car
 from opendbc.car import structs
-from openpilot.common.constants import CV
 from openpilot.common.params import Params
 from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import ACTIVE_STATES as SLA_ACTIVE_STATES
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target
 
 ButtonType = car.CarState.ButtonEvent.Type
-SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
 
 CRUISE_BUTTON_TIMER = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0,
                        ButtonType.setCruise: 0, ButtonType.resumeCruise: 0,
@@ -54,16 +50,6 @@ class VCruiseHelperSP:
 
     self.enable_button_timers = CRUISE_BUTTON_TIMER
 
-    # Speed Limit Assist
-    self.sla_state = SpeedLimitAssistState.disabled
-    self.prev_sla_state = SpeedLimitAssistState.disabled
-    self.has_speed_limit = False
-    self.speed_limit_final_last = 0.
-    self.speed_limit_final_last_kph = 0.
-    self.prev_speed_limit_final_last_kph = 0.
-    self.req_plus = False
-    self.req_minus = False
-
   def read_custom_set_speed_params(self) -> None:
     self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled")
     self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True)
@@ -106,33 +92,3 @@ class VCruiseHelperSP:
       return enabled and self.enabled_prev
 
     return enabled
-
-  def update_speed_limit_assist(self, is_metric, LP_SP: custom.LongitudinalPlanSP) -> None:
-    resolver = LP_SP.speedLimit.resolver
-    self.has_speed_limit = resolver.speedLimitValid or resolver.speedLimitLastValid
-    self.speed_limit_final_last = LP_SP.speedLimit.resolver.speedLimitFinalLast
-    self.speed_limit_final_last_kph = self.speed_limit_final_last * CV.MS_TO_KPH
-    self.sla_state = LP_SP.speedLimit.assist.state
-    self.req_plus, self.req_minus = compare_cluster_target(self.v_cruise_cluster_kph * CV.KPH_TO_MS,
-                                                           self.speed_limit_final_last, is_metric)
-
-  @property
-  def update_speed_limit_final_last_changed(self) -> bool:
-    return self.has_speed_limit and bool(self.speed_limit_final_last_kph != self.prev_speed_limit_final_last_kph)
-
-  def update_speed_limit_assist_pre_active_confirmed(self, button_type: car.CarState.ButtonEvent.Type) -> bool:
-    if self.sla_state == SpeedLimitAssistState.preActive or self.prev_sla_state == SpeedLimitAssistState.preActive:
-      if button_type == ButtonType.decelCruise and self.req_minus:
-        return True
-      if button_type == ButtonType.accelCruise and self.req_plus:
-        return True
-
-    return False
-
-  def update_speed_limit_assist_v_cruise_non_pcm(self) -> None:
-    if self.sla_state in SLA_ACTIVE_STATES and (self.prev_sla_state not in SLA_ACTIVE_STATES or
-                                                self.update_speed_limit_final_last_changed):
-      self.v_cruise_kph = np.clip(round(self.speed_limit_final_last_kph, 1), self.v_cruise_min, V_CRUISE_MAX)
-
-    self.prev_sla_state = self.sla_state
-    self.prev_speed_limit_final_last_kph = self.speed_limit_final_last_kph

sunnypilot/selfdrive/car/interfaces.py

@@ -11,7 +11,6 @@ from opendbc.car.interfaces import CarInterfaceBase
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
 from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import get_nn_model_path
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import set_speed_limit_assist_availability
 
 import openpilot.system.sentry as sentry
 
@@ -86,10 +85,6 @@ def _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsS
     cloudlog.warning("openpilot Longitudinal Control and ICBM not available, cleaning up params")
     params.remove("DynamicExperimentalControl")
     params.remove("CustomAccIncrementsEnabled")
-    params.remove("SmartCruiseControlVision")
-    params.remove("SmartCruiseControlMap")
-
-  set_speed_limit_assist_availability(CP, CP_SP, params)
 
 
 def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None:

sunnypilot/selfdrive/controls/lib/smart_cruise_control/__init__.py

@@ -1,9 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-from openpilot.common.constants import CV
-
-MIN_V = 20 * CV.KPH_TO_MS  # Do not operate under 20 km/h

sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py

@@ -1,261 +0,0 @@
-import json
-import math
-import platform
-
-from cereal import custom
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
-from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
-from openpilot.sunnypilot.navd.helpers import coordinate_from_param, Coordinate
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
-
-MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
-
-ACTIVE_STATES = (MapState.turning, )
-ENABLED_STATES = (MapState.enabled, MapState.overriding, *ACTIVE_STATES)
-
-R = 6373000.0  # approximate radius of earth in meters
-TO_RADIANS = math.pi / 180
-TO_DEGREES = 180 / math.pi
-TARGET_JERK = -0.6  # m/s^3 There's some jounce limits that are not consistent so we're fudging this some
-TARGET_ACCEL = -1.2  # m/s^2 should match up with the long planner limit
-TARGET_OFFSET = 1.0  # seconds - This controls how soon before the curve you reach the target velocity. It also helps
-                     # reach the target velocity when inaccuracies in the distance modeling logic would cause overshoot.
-                     # The value is multiplied against the target velocity to determine the additional distance. This is
-                     # done to keep the distance calculations consistent but results in the offset actually being less
-                     # time than specified depending on how much of a speed differential there is between v_ego and the
-                     # target velocity.
-
-
-def velocities_from_param(param: str, params: Params):
-  if params is None:
-    params = Params()
-
-  json_str = params.get(param)
-  if json_str is None:
-    return None
-
-  velocities = json.loads(json_str)
-
-  return velocities
-
-
-def calculate_accel(t, target_jerk, a_ego):
-  return a_ego + target_jerk * t
-
-
-def calculate_velocity(t, target_jerk, a_ego, v_ego):
-  return v_ego + a_ego * t + target_jerk/2 * (t ** 2)
-
-
-def calculate_distance(t, target_jerk, a_ego, v_ego):
-  return t * v_ego + a_ego/2 * (t ** 2) + target_jerk/6 * (t ** 3)
-
-
-# points should be in radians
-# output is meters
-def distance_to_point(ax, ay, bx, by):
-  a = math.sin((bx-ax)/2)*math.sin((bx-ax)/2) + math.cos(ax) * math.cos(bx)*math.sin((by-ay)/2)*math.sin((by-ay)/2)
-  c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
-
-  return R * c  # in meters
-
-
-class SmartCruiseControlMap:
-  v_target: float = 0
-  a_target: float = 0.
-  v_ego: float = 0.
-  a_ego: float = 0.
-  output_v_target: float = V_CRUISE_UNSET
-  output_a_target: float = 0.
-
-  def __init__(self):
-    self.params = Params()
-    self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
-    self.enabled = self.params.get_bool("SmartCruiseControlMap")
-    self.long_enabled = False
-    self.long_override = False
-    self.is_enabled = False
-    self.is_active = False
-    self.state = MapState.disabled
-    self.v_cruise = 0
-    self.target_lat = 0.0
-    self.target_lon = 0.0
-    self.frame = -1
-
-    self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0)
-    self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or []
-
-  def get_v_target_from_control(self) -> float:
-    if self.is_active:
-      return max(self.v_target, MIN_V)
-
-    return V_CRUISE_UNSET
-
-  def get_a_target_from_control(self) -> float:
-    return self.a_ego
-
-  def update_params(self):
-    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
-      self.enabled = self.params.get_bool("SmartCruiseControlMap")
-
-  def update_calculations(self) -> None:
-    self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0)
-    lat = self.last_position.latitude
-    lon = self.last_position.longitude
-
-    self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or []
-
-    if self.last_position is None or self.target_velocities is None:
-      return
-
-    min_dist = 1000
-    min_idx = 0
-    distances = []
-
-    # find our location in the path
-    for i in range(len(self.target_velocities)):
-      target_velocity = self.target_velocities[i]
-      tlat = target_velocity["latitude"]
-      tlon = target_velocity["longitude"]
-      d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, tlat * TO_RADIANS, tlon * TO_RADIANS)
-      distances.append(d)
-      if d < min_dist:
-        min_dist = d
-        min_idx = i
-
-    # only look at values from our current position forward
-    forward_points = self.target_velocities[min_idx:]
-    forward_distances = distances[min_idx:]
-
-    # find velocities that we are within the distance we need to adjust for
-    valid_velocities = []
-    for i in range(len(forward_points)):
-      target_velocity = forward_points[i]
-      tlat = target_velocity["latitude"]
-      tlon = target_velocity["longitude"]
-      tv = target_velocity["velocity"]
-      if tv > self.v_ego:
-        continue
-
-      d = forward_distances[i]
-
-      a_diff = (self.a_ego - TARGET_ACCEL)
-      accel_t = abs(a_diff / TARGET_JERK)
-      min_accel_v = calculate_velocity(accel_t, TARGET_JERK, self.a_ego, self.v_ego)
-
-      max_d = 0
-      if tv > min_accel_v:
-        # calculate time needed based on target jerk
-        a = 0.5 * TARGET_JERK
-        b = self.a_ego
-        c = self.v_ego - tv
-        t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a
-        t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a
-        if not isinstance(t_a, complex) and t_a > 0:
-          t = t_a
-        else:
-          t = t_b
-        if isinstance(t, complex):
-          continue
-
-        max_d = max_d + calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego)
-      else:
-        t = accel_t
-        max_d = calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego)
-
-        # calculate additional time needed based on target accel
-        t = abs((min_accel_v - tv) / TARGET_ACCEL)
-        max_d += calculate_distance(t, 0, TARGET_ACCEL, min_accel_v)
-
-      if d < max_d + tv * TARGET_OFFSET:
-        valid_velocities.append((float(tv), tlat, tlon))
-
-    # Find the smallest velocity we need to adjust for
-    min_v = 100.0
-    target_lat = 0.0
-    target_lon = 0.0
-    for tv, lat, lon in valid_velocities:
-      if tv < min_v:
-        min_v = tv
-        target_lat = lat
-        target_lon = lon
-
-    if self.v_target < min_v and not (self.target_lat == 0 and self.target_lon == 0):
-      for i in range(len(forward_points)):
-        target_velocity = forward_points[i]
-        tlat = target_velocity["latitude"]
-        tlon = target_velocity["longitude"]
-        tv = target_velocity["velocity"]
-        if tv > self.v_ego:
-          continue
-
-        if tlat == self.target_lat and tlon == self.target_lon and tv == self.v_target:
-          return
-
-      # not found so let's reset
-      self.v_target = 0.0
-      self.target_lat = 0.0
-      self.target_lon = 0.0
-
-    self.v_target = min_v
-    self.target_lat = target_lat
-    self.target_lon = target_lon
-
-  def _update_state_machine(self) -> tuple[bool, bool]:
-    # ENABLED, TURNING
-    if self.state != MapState.disabled:
-      if not self.long_enabled or not self.enabled:
-        self.state = MapState.disabled
-      elif self.long_override:
-        self.state = MapState.overriding
-
-      else:
-        # ENABLED
-        if self.state == MapState.enabled:
-          if self.v_cruise > self.v_target != 0:
-            self.state = MapState.turning
-
-        # TURNING
-        elif self.state == MapState.turning:
-          if self.v_cruise <= self.v_target or self.v_target == 0:
-            self.state = MapState.enabled
-
-        # OVERRIDING
-        elif self.state == MapState.overriding:
-          if not self.long_override:
-            if self.v_cruise > self.v_target != 0:
-              self.state = MapState.turning
-            else:
-              self.state = MapState.enabled
-
-    # DISABLED
-    elif self.state == MapState.disabled:
-      if self.long_enabled and self.enabled:
-        if self.long_override:
-          self.state = MapState.overriding
-        else:
-          self.state = MapState.enabled
-
-    enabled = self.state in ENABLED_STATES
-    active = self.state in ACTIVE_STATES
-
-    return enabled, active
-
-  def update(self, long_enabled: bool, long_override: bool, v_ego, a_ego, v_cruise) -> None:
-    self.long_enabled = long_enabled
-    self.long_override = long_override
-    self.v_ego = v_ego
-    self.a_ego = a_ego
-    self.v_cruise = v_cruise
-
-    self.update_params()
-    self.update_calculations()
-
-    self.is_enabled, self.is_active = self._update_state_machine()
-
-    self.output_v_target = self.get_v_target_from_control()
-    self.output_a_target = self.get_a_target_from_control()
-
-    self.frame += 1

sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py

@@ -1,19 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import cereal.messaging as messaging
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
-
-
-class SmartCruiseControl:
-  def __init__(self):
-    self.vision = SmartCruiseControlVision()
-    self.map = SmartCruiseControlMap()
-
-  def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise: float) -> None:
-    self.map.update(long_enabled, long_override, v_ego, a_ego, v_cruise)
-    self.vision.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)

sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_map_controller.py

@@ -1,58 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import platform
-
-from cereal import custom
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
-
-MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
-
-
-class TestSmartCruiseControlMap:
-
-  def setup_method(self):
-    self.params = Params()
-    self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
-    self.reset_params()
-    self.scc_m = SmartCruiseControlMap()
-
-  def reset_params(self):
-    self.params.put_bool("SmartCruiseControlMap", True)
-
-    # TODO-SP: mock data from gpsLocation
-    self.params.put("LastGPSPosition", "{}")
-    self.params.put("MapTargetVelocities", "{}")
-
-  def test_initial_state(self):
-    assert self.scc_m.state == VisionState.disabled
-    assert not self.scc_m.is_active
-    assert self.scc_m.output_v_target == V_CRUISE_UNSET
-    assert self.scc_m.output_a_target == 0.
-
-  def test_system_disabled(self):
-    self.params.put_bool("SmartCruiseControlMap", False)
-    self.scc_m.enabled = self.params.get_bool("SmartCruiseControlMap")
-
-    for _ in range(int(10. / DT_MDL)):
-      self.scc_m.update(True, False, 0., 0., 0.)
-    assert self.scc_m.state == VisionState.disabled
-    assert not self.scc_m.is_active
-
-  def test_disabled(self):
-    for _ in range(int(10. / DT_MDL)):
-      self.scc_m.update(False, False, 0., 0., 0.)
-    assert self.scc_m.state == VisionState.disabled
-
-  def test_transition_disabled_to_enabled(self):
-    for _ in range(int(10. / DT_MDL)):
-      self.scc_m.update(True, False, 0., 0., 0.)
-    assert self.scc_m.state == VisionState.enabled
-
-  # TODO-SP: mock data from modelV2 to test other states

sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py

@@ -1,104 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import numpy as np
-
-import cereal.messaging as messaging
-from cereal import custom, log
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
-from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision
-
-VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
-
-
-def generate_modelV2():
-  model = messaging.new_message('modelV2')
-  position = log.XYZTData.new_message()
-  speed = 30
-  position.x = [float(x) for x in (speed + 0.5) * np.array(ModelConstants.T_IDXS)]
-  model.modelV2.position = position
-  orientation = log.XYZTData.new_message()
-  curvature = 0.05
-  orientation.x = [float(curvature) for _ in ModelConstants.T_IDXS]
-  orientation.y = [0.0 for _ in ModelConstants.T_IDXS]
-  model.modelV2.orientation = orientation
-  orientationRate = log.XYZTData.new_message()
-  orientationRate.z = [float(z) for z in ModelConstants.T_IDXS]
-  model.modelV2.orientationRate = orientationRate
-  velocity = log.XYZTData.new_message()
-  velocity.x = [float(x) for x in (speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
-  velocity.x[0] = float(speed)  # always start at current speed
-  model.modelV2.velocity = velocity
-  acceleration = log.XYZTData.new_message()
-  acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
-  acceleration.y = [float(y) for y in np.zeros_like(ModelConstants.T_IDXS)]
-  model.modelV2.acceleration = acceleration
-
-  return model
-
-
-def generate_carState():
-  car_state = messaging.new_message('carState')
-  speed = 30
-  v_cruise = 50
-  car_state.carState.vEgo = float(speed)
-  car_state.carState.standstill = False
-  car_state.carState.vCruise = float(v_cruise * 3.6)
-
-  return car_state
-
-
-def generate_controlsState():
-  controls_state = messaging.new_message('controlsState')
-  controls_state.controlsState.curvature = 0.05
-
-  return controls_state
-
-
-class TestSmartCruiseControlVision:
-
-  def setup_method(self):
-    self.params = Params()
-    self.reset_params()
-    self.scc_v = SmartCruiseControlVision()
-
-    mdl = generate_modelV2()
-    cs = generate_carState()
-    controls_state = generate_controlsState()
-    self.sm = {'modelV2': mdl.modelV2, 'carState': cs.carState, 'controlsState': controls_state.controlsState}
-
-  def reset_params(self):
-    self.params.put_bool("SmartCruiseControlVision", True)
-
-  def test_initial_state(self):
-    assert self.scc_v.state == VisionState.disabled
-    assert not self.scc_v.is_active
-    assert self.scc_v.output_v_target == V_CRUISE_UNSET
-    assert self.scc_v.output_a_target == 0.
-
-  def test_system_disabled(self):
-    self.params.put_bool("SmartCruiseControlVision", False)
-    self.scc_v.enabled = self.params.get_bool("SmartCruiseControlVision")
-
-    for _ in range(int(10. / DT_MDL)):
-      self.scc_v.update(self.sm, True, False, 0., 0., 0.)
-    assert self.scc_v.state == VisionState.disabled
-    assert not self.scc_v.is_active
-
-  def test_disabled(self):
-    for _ in range(int(10. / DT_MDL)):
-      self.scc_v.update(self.sm, False, False, 0., 0., 0.)
-    assert self.scc_v.state == VisionState.disabled
-
-  def test_transition_disabled_to_enabled(self):
-    for _ in range(int(10. / DT_MDL)):
-      self.scc_v.update(self.sm, True, False, 0., 0., 0.)
-    assert self.scc_v.state == VisionState.enabled
-
-  # TODO-SP: mock modelV2 data to test other states

sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py

@@ -1,203 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import numpy as np
-
-import cereal.messaging as messaging
-from cereal import custom
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
-from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
-
-VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
-
-ACTIVE_STATES = (VisionState.entering, VisionState.turning, VisionState.leaving)
-ENABLED_STATES = (VisionState.enabled, VisionState.overriding, *ACTIVE_STATES)
-
-_ENTERING_PRED_LAT_ACC_TH = 1.3  # Predicted Lat Acc threshold to trigger entering turn state.
-_ABORT_ENTERING_PRED_LAT_ACC_TH = 1.1  # Predicted Lat Acc threshold to abort entering state if speed drops.
-
-_TURNING_LAT_ACC_TH = 1.6  # Lat Acc threshold to trigger turning state.
-
-_LEAVING_LAT_ACC_TH = 1.3  # Lat Acc threshold to trigger leaving turn state.
-_FINISH_LAT_ACC_TH = 1.1  # Lat Acc threshold to trigger the end of the turn cycle.
-
-_A_LAT_REG_MAX = 2.  # Maximum lateral acceleration
-
-_NO_OVERSHOOT_TIME_HORIZON = 4.  # s. Time to use for velocity desired based on a_target when not overshooting.
-
-# Lookup table for the minimum smooth deceleration during the ENTERING state
-# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead.
-_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.]  # min decel value allowed on ENTERING state
-_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.]  # absolute value of lat acc ahead
-
-# Lookup table for the acceleration for the TURNING state
-# depending on the current lateral acceleration of the vehicle.
-_TURNING_ACC_V = [0.5, 0., -0.4]  # acc value
-_TURNING_ACC_BP = [1.5, 2.3, 3.]  # absolute value of current lat acc
-
-_LEAVING_ACC = 0.5  # Conformable acceleration to regain speed while leaving a turn.
-
-
-class SmartCruiseControlVision:
-  v_target: float = 0
-  a_target: float = 0.
-  v_ego: float = 0.
-  a_ego: float = 0.
-  output_v_target: float = V_CRUISE_UNSET
-  output_a_target: float = 0.
-
-  def __init__(self):
-    self.params = Params()
-    self.frame = -1
-    self.long_enabled = False
-    self.long_override = False
-    self.is_enabled = False
-    self.is_active = False
-    self.enabled = self.params.get_bool("SmartCruiseControlVision")
-    self.v_cruise_setpoint = 0.
-
-    self.state = VisionState.disabled
-    self.current_lat_acc = 0.
-    self.max_pred_lat_acc = 0.
-
-  def get_a_target_from_control(self) -> float:
-    return self.a_target
-
-  def get_v_target_from_control(self) -> float:
-    if self.is_active:
-      return max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON
-
-    return V_CRUISE_UNSET
-
-  def _update_params(self) -> None:
-    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
-      self.enabled = self.params.get_bool("SmartCruiseControlVision")
-
-  def _update_calculations(self, sm: messaging.SubMaster) -> None:
-    if not self.long_enabled:
-      return
-    else:
-      rate_plan = np.array(np.abs(sm['modelV2'].orientationRate.z))
-      vel_plan = np.array(sm['modelV2'].velocity.x)
-
-      self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature)
-
-      # get the maximum lat accel from the model
-      predicted_lat_accels = rate_plan * vel_plan
-      self.max_pred_lat_acc = np.amax(predicted_lat_accels)
-
-      # get the maximum curve based on the current velocity
-      v_ego = max(self.v_ego, 0.1)  # ensure a value greater than 0 for calculations
-      max_curve = self.max_pred_lat_acc / (v_ego**2)
-
-      # Get the target velocity for the maximum curve
-      self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5
-
-  def _update_state_machine(self) -> tuple[bool, bool]:
-    # ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING
-    if self.state != VisionState.disabled:
-      # longitudinal and feature disable always have priority in a non-disabled state
-      if not self.long_enabled or not self.enabled:
-        self.state = VisionState.disabled
-      elif self.long_override:
-        self.state = VisionState.overriding
-
-      else:
-        # ENABLED
-        if self.state == VisionState.enabled:
-          # Do not enter a turn control cycle if the speed is low.
-          if self.v_ego <= MIN_V:
-            pass
-          # If significant lateral acceleration is predicted ahead, then move to Entering turn state.
-          elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
-            self.state = VisionState.entering
-
-        # OVERRIDING
-        elif self.state == VisionState.overriding:
-          if not self.long_override:
-            self.state = VisionState.enabled
-
-        # ENTERING
-        elif self.state == VisionState.entering:
-          # Transition to Turning if current lateral acceleration is over the threshold.
-          if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
-            self.state = VisionState.turning
-          # Abort if the predicted lateral acceleration drops
-          elif self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH:
-            self.state = VisionState.enabled
-
-        # TURNING
-        elif self.state == VisionState.turning:
-          # Transition to Leaving if current lateral acceleration drops below a threshold.
-          if self.current_lat_acc <= _LEAVING_LAT_ACC_TH:
-            self.state = VisionState.leaving
-
-        # LEAVING
-        elif self.state == VisionState.leaving:
-          # Transition back to Turning if current lateral acceleration goes back over the threshold.
-          if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
-            self.state = VisionState.turning
-          # Finish if current lateral acceleration goes below a threshold.
-          elif self.current_lat_acc < _FINISH_LAT_ACC_TH:
-            self.state = VisionState.enabled
-
-    # DISABLED
-    elif self.state == VisionState.disabled:
-      if self.long_enabled and self.enabled:
-        if self.long_override:
-          self.state = VisionState.overriding
-        else:
-          self.state = VisionState.enabled
-
-    enabled = self.state in ENABLED_STATES
-    active = self.state in ACTIVE_STATES
-
-    return enabled, active
-
-  def _update_solution(self) -> float:
-    # DISABLED, ENABLED, OVERRIDING
-    if self.state not in ACTIVE_STATES:
-      # when not overshooting, calculate v_turn as the speed at the prediction horizon when following
-      # the smooth deceleration.
-      a_target = self.a_ego
-    # ENTERING
-    elif self.state == VisionState.entering:
-      # when not overshooting, target a smooth deceleration in preparation for a sharp turn to come.
-      a_target = np.interp(self.max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V)
-    # TURNING
-    elif self.state == VisionState.turning:
-      # When turning, we provide a target acceleration that is comfortable for the lateral acceleration felt.
-      a_target = np.interp(self.current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V)
-    # LEAVING
-    elif self.state == VisionState.leaving:
-      # When leaving, we provide a comfortable acceleration to regain speed.
-      a_target = _LEAVING_ACC
-    else:
-      raise NotImplementedError(f"SCC-V state not supported: {self.state}")
-
-    return a_target
-
-  def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float,
-             v_cruise_setpoint: float) -> None:
-    self.long_enabled = long_enabled
-    self.long_override = long_override
-    self.v_ego = v_ego
-    self.a_ego = a_ego
-    self.v_cruise_setpoint = v_cruise_setpoint
-
-    self._update_params()
-    self._update_calculations(sm)
-
-    self.is_enabled, self.is_active = self._update_state_machine()
-    self.a_target = self._update_solution()
-
-    self.output_v_target = self.get_v_target_from_control()
-    self.output_a_target = self.get_a_target_from_control()
-
-    self.frame += 1

sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py

@@ -1,19 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-LIMIT_ADAPT_ACC = -1.  # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits.
-LIMIT_MAX_MAP_DATA_AGE = 10.  # s Maximum time to hold to map data, then consider it invalid inside limits controllers.
-
-# Speed Limit Assist constants
-PCM_LONG_REQUIRED_MAX_SET_SPEED = {
-  True: (33.3333, 36.1111),  # km/h, (120, 130)
-  False: (31.2928, 35.7632),  # mph, (70, 80)
-}
-
-CONFIRM_SPEED_THRESHOLD = {
-  True: 80,   # km/h
-  False: 50,  # mph
-}

sunnypilot/selfdrive/controls/lib/speed_limit/common.py

@@ -1,29 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-
-from openpilot.sunnypilot import IntEnumBase
-
-
-class Policy(IntEnumBase):
-  car_state_only = 0
-  map_data_only = 1
-  car_state_priority = 2
-  map_data_priority = 3
-  combined = 4
-
-
-class OffsetType(IntEnumBase):
-  off = 0
-  fixed = 1
-  percentage = 2
-
-
-class Mode(IntEnumBase):
-  off = 0
-  information = 1
-  warning = 2
-  assist = 3

sunnypilot/selfdrive/controls/lib/speed_limit/helpers.py

@@ -1,44 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-
-from cereal import custom, car
-from openpilot.common.constants import CV
-from openpilot.common.params import Params
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode as SpeedLimitMode
-
-
-def compare_cluster_target(v_cruise_cluster: float, target_set_speed: float, is_metric: bool) -> tuple[bool, bool]:
-  speed_conv = CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH
-  v_cruise_cluster_conv = round(v_cruise_cluster * speed_conv)
-  target_set_speed_conv = round(target_set_speed * speed_conv)
-
-  req_plus = v_cruise_cluster_conv < target_set_speed_conv
-  req_minus = v_cruise_cluster_conv > target_set_speed_conv
-
-  return req_plus, req_minus
-
-
-def set_speed_limit_assist_availability(CP: car.CarParams, CP_SP: custom.CarParamsSP, params: Params = None) -> bool:
-  if params is None:
-    params = Params()
-
-  is_release = params.get_bool("IsReleaseSpBranch")
-  disallow_in_release = CP.brand == "tesla" and is_release
-  always_disallow = CP.brand == "rivian"
-  allowed = True
-
-  if disallow_in_release or always_disallow:
-    allowed = False
-
-  if not CP.openpilotLongitudinalControl and CP_SP.pcmCruiseSpeed:
-    allowed = False
-
-  if not allowed:
-    if params.get("SpeedLimitMode", return_default=True) == SpeedLimitMode.assist:
-      params.put("SpeedLimitMode", int(SpeedLimitMode.warning))
-
-  return allowed

sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py

@@ -1,414 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import time
-
-from cereal import custom, car
-from openpilot.common.params import Params
-from openpilot.common.constants import CV
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
-from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
-from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target, set_speed_limit_assist_availability
-
-ButtonType = car.CarState.ButtonEvent.Type
-EventNameSP = custom.OnroadEventSP.EventName
-SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
-SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
-
-ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting)
-ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, *ACTIVE_STATES)
-
-DISABLED_GUARD_PERIOD = 0.5  # secs.
-# secs. Time to wait after activation before considering temp deactivation signal.
-PRE_ACTIVE_GUARD_PERIOD = {
-  True: 15,
-  False: 5,
-}
-SPEED_LIMIT_CHANGED_HOLD_PERIOD = 1  # secs. Time to wait after speed limit change before switching to preActive.
-
-LIMIT_MIN_ACC = -1.5  # m/s^2 Maximum deceleration allowed for limit controllers to provide.
-LIMIT_MAX_ACC = 1.0   # m/s^2 Maximum acceleration allowed for limit controllers to provide while active.
-LIMIT_MIN_SPEED = 8.33  # m/s, Minimum speed limit to provide as solution on limit controllers.
-LIMIT_SPEED_OFFSET_TH = -1.  # m/s Maximum offset between speed limit and current speed for adapting state.
-V_CRUISE_UNSET = 255.
-
-CRUISE_BUTTONS_PLUS = (ButtonType.accelCruise, ButtonType.resumeCruise)
-CRUISE_BUTTONS_MINUS = (ButtonType.decelCruise, ButtonType.setCruise)
-CRUISE_BUTTON_CONFIRM_HOLD = 0.5  # secs.
-
-
-class SpeedLimitAssist:
-  _speed_limit_final_last: float
-  _distance: float
-  v_ego: float
-  a_ego: float
-  v_offset: float
-
-  def __init__(self, CP: car.CarParams, CP_SP: custom.CarParamsSP):
-    self.params = Params()
-    self.CP = CP
-    self.CP_SP = CP_SP
-    self.frame = -1
-    self.long_engaged_timer = 0
-    self.pre_active_timer = 0
-    self.is_metric = self.params.get_bool("IsMetric")
-    set_speed_limit_assist_availability(self.CP, self.CP_SP, self.params)
-    self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
-    self.long_enabled = False
-    self.long_enabled_prev = False
-    self.is_enabled = False
-    self.is_active = False
-    self.output_v_target = V_CRUISE_UNSET
-    self.output_a_target = 0.
-    self.v_ego = 0.
-    self.a_ego = 0.
-    self.v_offset = 0.
-    self.target_set_speed_conv = 0
-    self.prev_target_set_speed_conv = 0
-    self.v_cruise_cluster = 0.
-    self.v_cruise_cluster_prev = 0.
-    self.v_cruise_cluster_conv = 0
-    self.prev_v_cruise_cluster_conv = 0
-    self._has_speed_limit = False
-    self._speed_limit = 0.
-    self._speed_limit_final_last = 0.
-    self.speed_limit_prev = 0.
-    self.speed_limit_final_last_conv = 0
-    self.prev_speed_limit_final_last_conv = 0
-    self._distance = 0.
-    self.state = SpeedLimitAssistState.disabled
-    self._state_prev = SpeedLimitAssistState.disabled
-    self.pcm_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise
-
-    self._plus_hold = 0.
-    self._minus_hold = 0.
-    self._last_carstate_ts = 0.
-
-    # TODO-SP: SLA's own output_a_target for planner
-    # Solution functions mapped to respective states
-    self.acceleration_solutions = {
-      SpeedLimitAssistState.disabled: self.get_current_acceleration_as_target,
-      SpeedLimitAssistState.inactive: self.get_current_acceleration_as_target,
-      SpeedLimitAssistState.preActive: self.get_current_acceleration_as_target,
-      SpeedLimitAssistState.pending: self.get_current_acceleration_as_target,
-      SpeedLimitAssistState.adapting: self.get_adapting_state_target_acceleration,
-      SpeedLimitAssistState.active: self.get_active_state_target_acceleration,
-    }
-
-  @property
-  def speed_limit_changed(self) -> bool:
-    return self._has_speed_limit and bool(self._speed_limit != self.speed_limit_prev)
-
-  @property
-  def v_cruise_cluster_changed(self) -> bool:
-    return bool(self.v_cruise_cluster_conv != self.prev_v_cruise_cluster_conv)
-
-  @property
-  def target_set_speed_confirmed(self) -> bool:
-    return bool(self.v_cruise_cluster_conv == self.target_set_speed_conv)
-
-  @property
-  def v_cruise_cluster_below_confirm_speed_threshold(self) -> bool:
-    return bool(self.v_cruise_cluster_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric])
-
-  def update_active_event(self, events_sp: EventsSP) -> None:
-    if self.v_cruise_cluster_below_confirm_speed_threshold:
-      events_sp.add(EventNameSP.speedLimitChanged)
-    else:
-      events_sp.add(EventNameSP.speedLimitActive)
-
-  def get_v_target_from_control(self) -> float:
-    if self._has_speed_limit:
-      if self.pcm_op_long and self.is_enabled:
-        return self._speed_limit_final_last
-      if not self.pcm_op_long and self.is_active:
-        return self._speed_limit_final_last
-
-    # Fallback
-    return V_CRUISE_UNSET
-
-  # TODO-SP: SLA's own output_a_target for planner
-  def get_a_target_from_control(self) -> float:
-    return self.a_ego
-
-  def update_params(self) -> None:
-    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
-      self.is_metric = self.params.get_bool("IsMetric")
-      set_speed_limit_assist_availability(self.CP, self.CP_SP, self.params)
-      self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
-
-  def update_car_state(self, CS: car.CarState) -> None:
-    now = time.monotonic()
-    self._last_carstate_ts = now
-
-    for b in CS.buttonEvents:
-      if not b.pressed:
-        if b.type in CRUISE_BUTTONS_PLUS:
-          self._plus_hold = max(self._plus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
-        elif b.type in CRUISE_BUTTONS_MINUS:
-          self._minus_hold = max(self._minus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
-
-  def _get_button_release(self, req_plus: bool, req_minus: bool) -> bool:
-    now = time.monotonic()
-    if req_plus and now <= self._plus_hold:
-      self._plus_hold = 0.
-      return True
-    elif req_minus and now <= self._minus_hold:
-      self._minus_hold = 0.
-      return True
-
-    # expired
-    if now > self._plus_hold:
-      self._plus_hold = 0.
-    if now > self._minus_hold:
-      self._minus_hold = 0.
-    return False
-
-  def update_calculations(self, v_cruise_cluster: float) -> None:
-    speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH
-    self.v_cruise_cluster = v_cruise_cluster
-
-    # Update current velocity offset (error)
-    self.v_offset = self._speed_limit_final_last - self.v_ego
-
-    self.speed_limit_final_last_conv = round(self._speed_limit_final_last * speed_conv)
-    self.v_cruise_cluster_conv = round(self.v_cruise_cluster * speed_conv)
-
-    cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.is_metric]
-    pcm_long_required_max = cst_low if self._has_speed_limit and self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric] else \
-                            cst_high
-    pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv)
-
-    self.target_set_speed_conv = pcm_long_required_max_set_speed_conv if self.pcm_op_long else self.speed_limit_final_last_conv
-
-  @property
-  def apply_confirm_speed_threshold(self) -> bool:
-    # below CST: always require user confirmation
-    if self.v_cruise_cluster_below_confirm_speed_threshold:
-      return True
-
-    # at/above CST:
-    # - new speed limit >= CST: auto change
-    # - new speed limit < CST: user confirmation required
-    return bool(self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric])
-
-  def get_current_acceleration_as_target(self) -> float:
-    return self.a_ego
-
-  def get_adapting_state_target_acceleration(self) -> float:
-    if self._distance > 0:
-      return (self._speed_limit_final_last ** 2 - self.v_ego ** 2) / (2. * self._distance)
-
-    return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
-
-  def get_active_state_target_acceleration(self) -> float:
-    return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
-
-  def _update_confirmed_state(self):
-    if self._has_speed_limit:
-      if self.v_offset < LIMIT_SPEED_OFFSET_TH:
-        self.state = SpeedLimitAssistState.adapting
-      else:
-        self.state = SpeedLimitAssistState.active
-    else:
-      self.state = SpeedLimitAssistState.pending
-
-  def _update_non_pcm_long_confirmed_state(self) -> bool:
-    if self.target_set_speed_confirmed:
-      return True
-
-    if self.state != SpeedLimitAssistState.preActive:
-      return False
-
-    req_plus, req_minus = compare_cluster_target(self.v_cruise_cluster, self._speed_limit_final_last, self.is_metric)
-
-    return self._get_button_release(req_plus, req_minus)
-
-  def update_state_machine_pcm_op_long(self):
-    self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
-    self.pre_active_timer = max(0, self.pre_active_timer - 1)
-
-    # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE
-    if self.state != SpeedLimitAssistState.disabled:
-      if not self.long_enabled or not self.enabled:
-        self.state = SpeedLimitAssistState.disabled
-
-      else:
-        # ACTIVE
-        if self.state == SpeedLimitAssistState.active:
-          if self.v_cruise_cluster_changed:
-            self.state = SpeedLimitAssistState.inactive
-          elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
-          elif self._has_speed_limit and self.v_offset < LIMIT_SPEED_OFFSET_TH:
-            self.state = SpeedLimitAssistState.adapting
-
-        # ADAPTING
-        elif self.state == SpeedLimitAssistState.adapting:
-          if self.v_cruise_cluster_changed:
-            self.state = SpeedLimitAssistState.inactive
-          elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
-          elif self.v_offset >= LIMIT_SPEED_OFFSET_TH:
-            self.state = SpeedLimitAssistState.active
-
-        # PENDING
-        elif self.state == SpeedLimitAssistState.pending:
-          if self.target_set_speed_confirmed:
-            self._update_confirmed_state()
-          elif self.speed_limit_changed:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
-
-        # PRE_ACTIVE
-        elif self.state == SpeedLimitAssistState.preActive:
-          if self.target_set_speed_confirmed:
-            self._update_confirmed_state()
-          elif self.pre_active_timer <= 0:
-            # Timeout - session ended
-            self.state = SpeedLimitAssistState.inactive
-
-        # INACTIVE
-        elif self.state == SpeedLimitAssistState.inactive:
-          pass
-
-    # DISABLED
-    elif self.state == SpeedLimitAssistState.disabled:
-      if self.long_enabled and self.enabled:
-        # start or reset preActive timer if initially enabled or manual set speed change detected
-        if not self.long_enabled_prev or self.v_cruise_cluster_changed:
-          self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
-
-        elif self.long_engaged_timer <= 0:
-          if self.target_set_speed_confirmed:
-            self._update_confirmed_state()
-          elif self._has_speed_limit:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
-          else:
-            self.state = SpeedLimitAssistState.pending
-
-    enabled = self.state in ENABLED_STATES
-    active = self.state in ACTIVE_STATES
-
-    return enabled, active
-
-  def update_state_machine_non_pcm_long(self):
-    self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
-    self.pre_active_timer = max(0, self.pre_active_timer - 1)
-
-    # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE
-    if self.state != SpeedLimitAssistState.disabled:
-      if not self.long_enabled or not self.enabled:
-        self.state = SpeedLimitAssistState.disabled
-
-      else:
-        # ACTIVE
-        if self.state == SpeedLimitAssistState.active:
-          if self.v_cruise_cluster_changed:
-            self.state = SpeedLimitAssistState.inactive
-
-          elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
-
-        # PRE_ACTIVE
-        elif self.state == SpeedLimitAssistState.preActive:
-          if self._update_non_pcm_long_confirmed_state():
-            self.state = SpeedLimitAssistState.active
-          elif self.pre_active_timer <= 0:
-            # Timeout - session ended
-            self.state = SpeedLimitAssistState.inactive
-
-        # INACTIVE
-        elif self.state == SpeedLimitAssistState.inactive:
-          if self.speed_limit_changed:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
-          elif self._update_non_pcm_long_confirmed_state():
-            self.state = SpeedLimitAssistState.active
-
-    # DISABLED
-    elif self.state == SpeedLimitAssistState.disabled:
-      if self.long_enabled and self.enabled:
-        # start or reset preActive timer if initially enabled or manual set speed change detected
-        if not self.long_enabled_prev or self.v_cruise_cluster_changed:
-          self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
-
-        elif self.long_engaged_timer <= 0:
-          if self._update_non_pcm_long_confirmed_state():
-            self.state = SpeedLimitAssistState.active
-          elif self._has_speed_limit:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
-          else:
-            self.state = SpeedLimitAssistState.inactive
-
-    enabled = self.state in ENABLED_STATES
-    active = self.state in ACTIVE_STATES
-
-    return enabled, active
-
-  def update_events(self, events_sp: EventsSP) -> None:
-    if self.state == SpeedLimitAssistState.preActive:
-      events_sp.add(EventNameSP.speedLimitPreActive)
-
-    if self.state == SpeedLimitAssistState.pending and self._state_prev != SpeedLimitAssistState.pending:
-      events_sp.add(EventNameSP.speedLimitPending)
-
-    if self.is_active:
-      if self._state_prev not in ACTIVE_STATES:
-        self.update_active_event(events_sp)
-
-      # only notify if we acquire a valid speed limit
-      # do not check has_speed_limit here
-      elif self._speed_limit != self.speed_limit_prev:
-        if self.speed_limit_prev <= 0:
-          self.update_active_event(events_sp)
-        elif self.speed_limit_prev > 0 and self._speed_limit > 0:
-          self.update_active_event(events_sp)
-
-  def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_cluster: float, speed_limit: float,
-             speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP) -> None:
-    self.long_enabled = long_enabled
-    self.v_ego = v_ego
-    self.a_ego = a_ego
-
-    self._has_speed_limit = has_speed_limit
-    self._speed_limit = speed_limit
-    self._speed_limit_final_last = speed_limit_final_last
-    self._distance = distance
-
-    self.update_params()
-    self.update_calculations(v_cruise_cluster)
-
-    self._state_prev = self.state
-    if self.pcm_op_long:
-      self.is_enabled, self.is_active = self.update_state_machine_pcm_op_long()
-    else:
-      self.is_enabled, self.is_active = self.update_state_machine_non_pcm_long()
-
-    self.update_events(events_sp)
-
-    # Update change tracking variables
-    self.speed_limit_prev = self._speed_limit
-    self.v_cruise_cluster_prev = self.v_cruise_cluster
-    self.long_enabled_prev = self.long_enabled
-    self.prev_target_set_speed_conv = self.target_set_speed_conv
-    self.prev_v_cruise_cluster_conv = self.v_cruise_cluster_conv
-    self.prev_speed_limit_final_last_conv = self.speed_limit_final_last_conv
-
-    self.output_v_target = self.get_v_target_from_control()
-    self.output_a_target = self.get_a_target_from_control()
-
-    self.frame += 1

sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py

@@ -1,190 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import time
-
-import cereal.messaging as messaging
-from cereal import custom
-from openpilot.common.constants import CV
-from openpilot.common.gps import get_gps_location_service
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD, get_sanitize_int_param
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy, OffsetType
-
-SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
-
-ALL_SOURCES = tuple(SpeedLimitSource.schema.enumerants.values())
-
-
-class SpeedLimitResolver:
-  limit_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float]
-  distance_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float]
-  v_ego: float
-  speed_limit: float
-  speed_limit_last: float
-  speed_limit_final: float
-  speed_limit_final_last: float
-  distance: float
-  source: custom.LongitudinalPlanSP.SpeedLimit.Source
-  speed_limit_offset: float
-
-  def __init__(self):
-    self.params = Params()
-    self.frame = -1
-
-    self._gps_location_service = get_gps_location_service(self.params)
-    self.limit_solutions = {}  # Store for speed limit solutions from different sources
-    self.distance_solutions = {}  # Store for distance to current speed limit start for different sources
-
-    self.policy = self.params.get("SpeedLimitPolicy", return_default=True)
-    self.policy = get_sanitize_int_param(
-      "SpeedLimitPolicy",
-      Policy.min().value,
-      Policy.max().value,
-      self.params
-    )
-    self._policy_to_sources_map = {
-      Policy.car_state_only: [SpeedLimitSource.car],
-      Policy.map_data_only: [SpeedLimitSource.map],
-      Policy.car_state_priority: [SpeedLimitSource.car, SpeedLimitSource.map],
-      Policy.map_data_priority: [SpeedLimitSource.map, SpeedLimitSource.car],
-      Policy.combined: [SpeedLimitSource.car, SpeedLimitSource.map],
-    }
-    self.source = SpeedLimitSource.none
-    for source in ALL_SOURCES:
-      self._reset_limit_sources(source)
-
-    self.is_metric = self.params.get_bool("IsMetric")
-    self.offset_type = get_sanitize_int_param(
-      "SpeedLimitOffsetType",
-      OffsetType.min().value,
-      OffsetType.max().value,
-      self.params
-    )
-    self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
-
-    self.speed_limit = 0.
-    self.speed_limit_last = 0.
-    self.speed_limit_final = 0.
-    self.speed_limit_final_last = 0.
-    self.speed_limit_offset = 0.
-
-  def update_speed_limit_states(self) -> None:
-    self.speed_limit_final = self.speed_limit + self.speed_limit_offset
-
-    if self.speed_limit > 0.:
-      self.speed_limit_last = self.speed_limit
-      self.speed_limit_final_last = self.speed_limit_final
-
-  @property
-  def speed_limit_valid(self) -> bool:
-    return self.speed_limit > 0.
-
-  @property
-  def speed_limit_last_valid(self) -> bool:
-    return self.speed_limit_last > 0.
-
-  def update_params(self):
-    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
-      self.policy = self.params.get("SpeedLimitPolicy", return_default=True)
-      self.is_metric = self.params.get_bool("IsMetric")
-      self.offset_type = self.params.get("SpeedLimitOffsetType", return_default=True)
-      self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
-
-  def _get_speed_limit_offset(self) -> float:
-    if self.offset_type == OffsetType.off:
-      return 0
-    elif self.offset_type == OffsetType.fixed:
-      return float(self.offset_value * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS))
-    elif self.offset_type == OffsetType.percentage:
-      return float(self.offset_value * 0.01 * self.speed_limit)
-    else:
-      raise NotImplementedError("Offset not supported")
-
-  def _reset_limit_sources(self, source: custom.LongitudinalPlanSP.SpeedLimit.Source) -> None:
-    self.limit_solutions[source] = 0.
-    self.distance_solutions[source] = 0.
-
-  def _get_from_car_state(self, sm: messaging.SubMaster) -> None:
-    self._reset_limit_sources(SpeedLimitSource.car)
-    self.limit_solutions[SpeedLimitSource.car] = sm['carStateSP'].speedLimit
-    self.distance_solutions[SpeedLimitSource.car] = 0.
-
-  def _get_from_map_data(self, sm: messaging.SubMaster) -> None:
-    self._reset_limit_sources(SpeedLimitSource.map)
-    self._process_map_data(sm)
-
-  def _process_map_data(self, sm: messaging.SubMaster) -> None:
-    gps_data = sm[self._gps_location_service]
-    map_data = sm['liveMapDataSP']
-
-    gps_fix_age = time.monotonic() - gps_data.unixTimestampMillis * 1e-3
-    if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE:
-      return
-
-    speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0.
-    next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0.
-
-    self._calculate_map_data_limits(sm, speed_limit, next_speed_limit)
-
-  def _calculate_map_data_limits(self, sm: messaging.SubMaster, speed_limit: float, next_speed_limit: float) -> None:
-    gps_data = sm[self._gps_location_service]
-    map_data = sm['liveMapDataSP']
-
-    distance_since_fix = self.v_ego * (time.monotonic() - gps_data.unixTimestampMillis * 1e-3)
-    distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix)
-
-    self.limit_solutions[SpeedLimitSource.map] = speed_limit
-    self.distance_solutions[SpeedLimitSource.map] = 0.
-
-    # FIXME-SP: this is not working as expected
-    if 0. < next_speed_limit < self.v_ego:
-      adapt_time = (next_speed_limit - self.v_ego) / LIMIT_ADAPT_ACC
-      adapt_distance = self.v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time ** 2
-
-      if distance_to_speed_limit_ahead <= adapt_distance:
-        self.limit_solutions[SpeedLimitSource.map] = next_speed_limit
-        self.distance_solutions[SpeedLimitSource.map] = distance_to_speed_limit_ahead
-
-  def _get_source_solution_according_to_policy(self) -> custom.LongitudinalPlanSP.SpeedLimit.Source:
-    sources_for_policy = self._policy_to_sources_map[self.policy]
-
-    if self.policy != Policy.combined:
-      # They are ordered in the order of preference, so we pick the first that's non-zero
-      for source in sources_for_policy:
-        if self.limit_solutions[source] > 0.:
-          return source
-      return SpeedLimitSource.none
-
-    sources_with_limits = [(s, limit) for s, limit in [(s, self.limit_solutions[s]) for s in sources_for_policy] if limit > 0.]
-    if sources_with_limits:
-      return min(sources_with_limits, key=lambda x: x[1])[0]
-
-    return SpeedLimitSource.none
-
-  def _resolve_limit_sources(self, sm: messaging.SubMaster) -> tuple[float, float, custom.LongitudinalPlanSP.SpeedLimit.Source]:
-    """Get limit solutions from each data source"""
-    self._get_from_car_state(sm)
-    self._get_from_map_data(sm)
-
-    source = self._get_source_solution_according_to_policy()
-    speed_limit = self.limit_solutions[source] if source else 0.
-    distance = self.distance_solutions[source] if source else 0.
-
-    return speed_limit, distance, source
-
-  def update(self, v_ego: float, sm: messaging.SubMaster) -> None:
-    self.v_ego = v_ego
-    self.update_params()
-
-    self.speed_limit, self.distance, self.source = self._resolve_limit_sources(sm)
-    self.speed_limit_offset = self._get_speed_limit_offset()
-
-    self.update_speed_limit_states()
-
-    self.frame += 1

sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py

@@ -1,278 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-
-import pytest
-
-from cereal import custom
-from opendbc.car.car_helpers import interfaces
-from opendbc.car.rivian.values import CAR as RIVIAN
-from opendbc.car.tesla.values import CAR as TESLA
-from opendbc.car.toyota.values import CAR as TOYOTA
-from openpilot.common.constants import CV
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
-from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
-from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist, \
-  PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES
-from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
-
-SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
-
-ALL_STATES = tuple(SpeedLimitAssistState.schema.enumerants.values())
-
-SPEED_LIMITS = {
-  'residential': 25 * CV.MPH_TO_MS,  # 25 mph
-  'city': 35 * CV.MPH_TO_MS,         # 35 mph
-  'highway': 65 * CV.MPH_TO_MS,      # 65 mph
-  'freeway': 80 * CV.MPH_TO_MS,      # 80 mph
-}
-
-DEFAULT_CAR = TOYOTA.TOYOTA_RAV4_TSS2
-
-
-@pytest.fixture
-def car_name(request):
-  return getattr(request, "param", DEFAULT_CAR)
-
-
-@pytest.fixture(autouse=True)
-def set_car_name_on_instance(request, car_name):
-  instance = getattr(request, "instance", None)
-  if instance:
-    instance.car_name = car_name
-
-
-class TestSpeedLimitAssist:
-
-  def setup_method(self, method):
-    self.params = Params()
-    self.reset_custom_params()
-    self.events_sp = EventsSP()
-    CI = self._setup_platform(self.car_name)
-    self.sla = SpeedLimitAssist(CI.CP, CI.CP_SP)
-    self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.sla.pcm_op_long] / DT_MDL)
-    self.pcm_long_max_set_speed = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric][1]  # use 80 MPH for now
-    self.speed_conv = CV.MS_TO_KPH if self.sla.is_metric else CV.MS_TO_MPH
-
-  def teardown_method(self, method):
-    self.reset_state()
-
-  def _setup_platform(self, car_name):
-    CarInterface = interfaces[car_name]
-    CP = CarInterface.get_non_essential_params(car_name)
-    CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
-    CI = CarInterface(CP, CP_SP)
-    CI.CP.openpilotLongitudinalControl = True  # always assume it's openpilot longitudinal
-    sunnypilot_interfaces.setup_interfaces(CI, self.params)
-    return CI
-
-  def reset_custom_params(self):
-    self.params.put("IsReleaseSpBranch", True)
-    self.params.put("SpeedLimitMode", int(Mode.assist))
-    self.params.put_bool("IsMetric", False)
-    self.params.put("SpeedLimitOffsetType", 0)
-    self.params.put("SpeedLimitValueOffset", 0)
-
-  def reset_state(self):
-    self.sla.state = SpeedLimitAssistState.disabled
-    self.sla.frame = -1
-    self.sla.last_op_engaged_frame = 0
-    self.sla.op_engaged = False
-    self.sla.op_engaged_prev = False
-    self.sla._speed_limit = 0.
-    self.sla.speed_limit_prev = 0.
-    self.sla.last_valid_speed_limit_offsetted = 0.
-    self.sla._distance = 0.
-    self.events_sp.clear()
-
-  def initialize_active_state(self, initialize_v_cruise):
-    self.sla.state = SpeedLimitAssistState.active
-    self.sla.v_cruise_cluster = initialize_v_cruise
-    self.sla.v_cruise_cluster_prev = initialize_v_cruise
-    self.sla.prev_v_cruise_cluster_conv = round(initialize_v_cruise * self.speed_conv)
-
-  def test_initial_state(self):
-    assert self.sla.state == SpeedLimitAssistState.disabled
-    assert not self.sla.is_enabled
-    assert not self.sla.is_active
-    assert V_CRUISE_UNSET == self.sla.get_v_target_from_control()
-
-  @pytest.mark.parametrize("car_name", [RIVIAN.RIVIAN_R1_GEN1, TESLA.TESLA_MODEL_Y], indirect=True)
-  def test_disallowed_brands(self, car_name):
-    """
-      Speed Limit Assist is disabled for the following brands and conditions:
-      - All Tesla and is a release branch;
-      - All Rivian
-    """
-    assert not self.sla.enabled
-
-    # stay disallowed even when the param may have changed from somewhere else
-    self.params.put("SpeedLimitMode", int(Mode.assist))
-    for _ in range(int(PARAMS_UPDATE_PERIOD / DT_MDL)):
-      self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'],
-                      SPEED_LIMITS['city'], True, 0, self.events_sp)
-    assert not self.sla.enabled
-
-  def test_disabled(self):
-    self.params.put("SpeedLimitMode", int(Mode.off))
-    for _ in range(int(10. / DT_MDL)):
-      self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.disabled
-
-  def test_transition_disabled_to_preactive(self):
-    for _ in range(int(3. / DT_MDL)):
-      self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.preActive
-    assert self.sla.is_enabled and not self.sla.is_active
-
-  def test_transition_disabled_to_pending_no_speed_limit_not_max_initial_set_speed(self):
-    for _ in range(int(3. / DT_MDL)):
-      self.sla.update(True, False, SPEED_LIMITS['highway'], 0, SPEED_LIMITS['city'], 0, 0, False, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.pending
-    assert self.sla.is_enabled and not self.sla.is_active
-
-  def test_preactive_to_active_with_max_speed_confirmation(self):
-    self.sla.state = SpeedLimitAssistState.preActive
-    self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
-                    SPEED_LIMITS['highway'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.active
-    assert self.sla.is_enabled and self.sla.is_active
-    assert self.sla.output_v_target == SPEED_LIMITS['highway']
-
-  def test_preactive_timeout_to_inactive(self):
-    self.sla.state = SpeedLimitAssistState.preActive
-    self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
-
-    for _ in range(int(PRE_ACTIVE_GUARD_PERIOD[self.sla.pcm_op_long] / DT_MDL)):
-      self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.inactive
-
-  def test_preactive_to_pending_no_speed_limit(self):
-    self.sla.state = SpeedLimitAssistState.preActive
-    self.sla.update(True, False, SPEED_LIMITS['highway'], 0, self.pcm_long_max_set_speed, 0, 0, False, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.pending
-    assert self.sla.is_enabled and not self.sla.is_active
-
-  def test_pending_to_active_when_speed_limit_available(self):
-    self.sla.state = SpeedLimitAssistState.pending
-    self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
-    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
-
-    self.sla.update(True, False, SPEED_LIMITS['highway'], 0, self.pcm_long_max_set_speed,
-                    SPEED_LIMITS['highway'], SPEED_LIMITS['highway'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.active
-
-  def test_pending_to_adapting_when_below_speed_limit(self):
-    self.sla.state = SpeedLimitAssistState.pending
-    self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
-    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
-
-    self.sla.update(True, False, SPEED_LIMITS['highway'] + 5, 0, self.pcm_long_max_set_speed,
-                    SPEED_LIMITS['highway'], SPEED_LIMITS['highway'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.adapting
-    assert self.sla.is_enabled and self.sla.is_active
-
-  def test_active_to_adapting_transition(self):
-    self.initialize_active_state(self.pcm_long_max_set_speed)
-
-    self.sla.update(True, False, SPEED_LIMITS['highway'] + 2, 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
-                    SPEED_LIMITS['highway'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.adapting
-
-  def test_adapting_to_active_transition(self):
-    self.sla.state = SpeedLimitAssistState.adapting
-    self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
-    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
-
-    self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
-                    SPEED_LIMITS['highway'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.active
-
-  def test_manual_cruise_change_detection(self):
-    self.sla.state = SpeedLimitAssistState.active
-    expected_cruise = SPEED_LIMITS['highway']
-    self.sla.v_cruise_cluster_prev = expected_cruise
-
-    different_cruise = SPEED_LIMITS['highway'] + 5
-    self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.inactive
-
-  # TODO-SP: test lower CST cases
-  def test_rapid_speed_limit_changes(self):
-    self.initialize_active_state(self.pcm_long_max_set_speed)
-    speed_limits = [SPEED_LIMITS['highway'], SPEED_LIMITS['freeway']]
-
-    for _, speed_limit in enumerate(speed_limits):
-      self.sla.update(True, False, speed_limit, 0, self.pcm_long_max_set_speed, speed_limit, speed_limit, True, 0, self.events_sp)
-    assert self.sla.state in ACTIVE_STATES
-
-  def test_invalid_speed_limits_handling(self):
-    self.initialize_active_state(self.pcm_long_max_set_speed)
-
-    invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS]
-
-    for invalid_limit in invalid_limits:
-      self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, invalid_limit, SPEED_LIMITS['city'], True, 0, self.events_sp)
-      assert isinstance(self.sla.output_v_target, (int, float))
-      assert self.sla.output_v_target == V_CRUISE_UNSET or self.sla.output_v_target > 0
-
-  def test_stale_data_handling(self):
-    self.initialize_active_state(self.pcm_long_max_set_speed)
-    old_speed_limit = SPEED_LIMITS['city']
-
-    self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, 0, old_speed_limit, True, 0, self.events_sp)
-    assert self.sla.state in ACTIVE_STATES
-    assert self.sla.output_v_target == old_speed_limit
-
-  def test_distance_based_adapting(self):
-    self.sla.state = SpeedLimitAssistState.adapting
-    self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
-    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
-
-    distance = 100.0
-    current_speed = SPEED_LIMITS['freeway']
-    target_speed = SPEED_LIMITS['highway']
-
-    self.sla.update(True, False, current_speed, 0, self.pcm_long_max_set_speed, target_speed, target_speed, True, distance, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.adapting
-    assert self.sla.output_v_target == target_speed  # TODO-SP: assert expected accel, need to enable self.acceleration_solutions
-
-  def test_long_disengaged_to_disabled(self):
-    self.initialize_active_state(self.pcm_long_max_set_speed)
-
-    self.sla.update(False, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'],
-                    SPEED_LIMITS['city'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.disabled
-    assert self.sla.output_v_target == V_CRUISE_UNSET
-
-  def test_maintain_states_with_no_changes(self):
-    """Test that states are maintained when no significant changes occur"""
-    test_states = [
-      SpeedLimitAssistState.preActive,
-      SpeedLimitAssistState.pending,
-      SpeedLimitAssistState.active,
-      SpeedLimitAssistState.adapting
-    ]
-
-    for state in test_states:
-      self.sla.state = state
-      self.sla.op_engaged = True
-
-      initial_state = state
-
-      self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
-
-      assert self.sla.state in ALL_STATES  # Sanity check
-
-      if initial_state == SpeedLimitAssistState.preActive:
-        assert self.sla.state in [SpeedLimitAssistState.preActive, SpeedLimitAssistState.active]
-      elif initial_state in ACTIVE_STATES:
-        assert self.sla.state in ACTIVE_STATES

sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_resolver.py

@@ -1,144 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import random
-import time
-
-import pytest
-from pytest_mock import MockerFixture
-
-from cereal import custom
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE
-
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver, ALL_SOURCES
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy
-
-SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
-
-
-def create_mock(properties, mocker: MockerFixture):
-  mock = mocker.MagicMock()
-  for _property, value in properties.items():
-    setattr(mock, _property, value)
-  return mock
-
-
-def setup_sm_mock(mocker: MockerFixture):
-  cruise_speed_limit = random.uniform(0, 120)
-  live_map_data_limit = random.uniform(0, 120)
-
-  car_state = create_mock({
-    'gasPressed': False,
-    'brakePressed': False,
-    'standstill': False,
-  }, mocker)
-  car_state_sp = create_mock({
-    'speedLimit': cruise_speed_limit,
-  }, mocker)
-  live_map_data = create_mock({
-    'speedLimit': live_map_data_limit,
-    'speedLimitValid': True,
-    'speedLimitAhead': 0.,
-    'speedLimitAheadValid': 0.,
-    'speedLimitAheadDistance': 0.,
-  }, mocker)
-  gps_data = create_mock({
-    'unixTimestampMillis': time.monotonic() * 1e3,
-  }, mocker)
-  sm_mock = mocker.MagicMock()
-  sm_mock.__getitem__.side_effect = lambda key: {
-    'carState': car_state,
-    'liveMapDataSP': live_map_data,
-    'carStateSP': car_state_sp,
-    'gpsLocation': gps_data,
-  }[key]
-  return sm_mock
-
-
-parametrized_policies = pytest.mark.parametrize(
-  "policy, sm_key, function_key", [
-    (Policy.car_state_only, 'carStateSP', SpeedLimitSource.car),
-    (Policy.car_state_priority, 'carStateSP', SpeedLimitSource.car),
-    (Policy.map_data_only, 'liveMapDataSP', SpeedLimitSource.map),
-    (Policy.map_data_priority, 'liveMapDataSP', SpeedLimitSource.map),
-  ],
-  ids=lambda val: val.name if hasattr(val, 'name') else str(val)
-)
-
-
-@pytest.mark.parametrize("resolver_class", [SpeedLimitResolver])
-class TestSpeedLimitResolverValidation:
-
-  @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
-  def test_initial_state(self, resolver_class, policy):
-    resolver = resolver_class()
-    resolver.policy = policy
-    for source in ALL_SOURCES:
-      if source in resolver.limit_solutions:
-        assert resolver.limit_solutions[source] == 0.
-        assert resolver.distance_solutions[source] == 0.
-
-  @parametrized_policies
-  def test_resolver(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
-    resolver = resolver_class()
-    resolver.policy = policy
-    sm_mock = setup_sm_mock(mocker)
-    source_speed_limit = sm_mock[sm_key].speedLimit
-
-    # Assert the resolver
-    resolver.update(source_speed_limit, sm_mock)
-    assert resolver.speed_limit == source_speed_limit
-    assert resolver.source == ALL_SOURCES[function_key]
-
-  def test_resolver_combined(self, resolver_class, mocker: MockerFixture):
-    resolver = resolver_class()
-    resolver.policy = Policy.combined
-    sm_mock = setup_sm_mock(mocker)
-    socket_to_source = {'carStateSP': SpeedLimitSource.car, 'liveMapDataSP': SpeedLimitSource.map}
-    minimum_key, minimum_speed_limit = min(
-      ((key, sm_mock[key].speedLimit) for key in
-       socket_to_source.keys()), key=lambda x: x[1])
-
-    # Assert the resolver
-    resolver.update(minimum_speed_limit, sm_mock)
-    assert resolver.speed_limit == minimum_speed_limit
-    assert resolver.source == socket_to_source[minimum_key]
-
-  @parametrized_policies
-  def test_parser(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
-    resolver = resolver_class()
-    resolver.policy = policy
-    sm_mock = setup_sm_mock(mocker)
-    source_speed_limit = sm_mock[sm_key].speedLimit
-
-    # Assert the parsing
-    resolver.update(source_speed_limit, sm_mock)
-    assert resolver.limit_solutions[ALL_SOURCES[function_key]] == source_speed_limit
-    assert resolver.distance_solutions[ALL_SOURCES[function_key]] == 0.
-
-  @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
-  def test_resolve_interaction_in_update(self, resolver_class, policy, mocker: MockerFixture):
-    v_ego = 50
-    resolver = resolver_class()
-    resolver.policy = policy
-
-    sm_mock = setup_sm_mock(mocker)
-    resolver.update(v_ego, sm_mock)
-
-    # After resolution
-    assert resolver.speed_limit is not None
-    assert resolver.distance is not None
-    assert resolver.source is not None
-
-  @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
-  def test_old_map_data_ignored(self, resolver_class, policy, mocker: MockerFixture):
-    resolver = resolver_class()
-    resolver.policy = policy
-    sm_mock = mocker.MagicMock()
-    sm_mock['gpsLocation'].unixTimestampMillis = (time.monotonic() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3
-    resolver._get_from_map_data(sm_mock)
-    assert resolver.limit_solutions[SpeedLimitSource.map] == 0.
-    assert resolver.distance_solutions[SpeedLimitSource.map] == 0.

sunnypilot/selfdrive/selfdrived/events.py

@@ -1,9 +1,7 @@
-import cereal.messaging as messaging
 from cereal import log, car, custom
-from openpilot.common.constants import CV
+
 from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \
   NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD
 
 
 AlertSize = log.SelfdriveState.AlertSize
@@ -18,43 +16,6 @@ EventNameSP = custom.OnroadEventSP.EventName
 EVENT_NAME_SP = {v: k for k, v in EventNameSP.schema.enumerants.items()}
 
 
-def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
-  speedLimit = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimit
-  speed = round(speedLimit * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))
-  message = f'Adjusting to {speed} {"km/h" if metric else "mph"} speed limit'
-  return Alert(
-    message,
-    "",
-    AlertStatus.normal, AlertSize.small,
-    Priority.LOW, VisualAlert.none, AudibleAlert.none, 4.)
-
-
-def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
-  speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH
-  speed_limit_final_last = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimitFinalLast
-  speed_limit_final_last_conv = round(speed_limit_final_last * speed_conv)
-  alert_1_str = ""
-  alert_2_str = ""
-  alert_size = AlertSize.none
-
-  if CP.openpilotLongitudinalControl and CP.pcmCruise:
-    # PCM long
-    cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[metric]
-    pcm_long_required_max = cst_low if speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[metric] else cst_high
-    pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv)
-    speed_unit = "km/h" if metric else "mph"
-
-    alert_1_str = "Speed Limit Assist: Activation Required"
-    alert_2_str = f"Manually change set speed to {pcm_long_required_max_set_speed_conv} {speed_unit} to activate"
-    alert_size = AlertSize.mid
-
-  return Alert(
-    alert_1_str,
-    alert_2_str,
-    AlertStatus.normal, alert_size,
-    Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleLow, .1)
-
-
 class EventsSP(EventsBase):
   def __init__(self):
     super().__init__()
@@ -191,34 +152,6 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
       Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
   },
 
-  EventNameSP.speedLimitActive: {
-    ET.WARNING: Alert(
-      "Automatically adjusting to the posted speed limit",
-      "",
-      AlertStatus.normal, AlertSize.small,
-      Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleHigh, 5.),
-  },
-
-  EventNameSP.speedLimitChanged: {
-    ET.WARNING: Alert(
-      "Set speed changed",
-      "",
-      AlertStatus.normal, AlertSize.small,
-      Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleHigh, 5.),
-  },
-
-  EventNameSP.speedLimitPreActive: {
-    ET.WARNING: speed_limit_pre_active_alert,
-  },
-
-  EventNameSP.speedLimitPending: {
-    ET.WARNING: Alert(
-      "Automatically adjusting to the last speed limit",
-      "",
-      AlertStatus.normal, AlertSize.small,
-      Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleHigh, 5.),
-  },
-
   EventNameSP.e2eChime: {
     ET.PERMANENT: Alert(
       "",

sunnypilot/sunnylink/athena/sunnylinkd.py

@@ -42,10 +42,14 @@ METADATA_PATH = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__f
 
 params = Params()
 
-# Parameters that should never be remotely modified for security reasons
+# Parameters that should never be remotely modified
 BLOCKED_PARAMS = {
+  "CompletedSunnylinkConsentVersion",
+  "CompletedTrainingVersion",
   "GithubUsername",  # Could grant SSH access
   "GithubSshKeys",   # Direct SSH key injection
+  "HasAcceptedTerms",
+  "HasAcceptedTermsSP",
 }
 
 
@@ -62,7 +66,7 @@ def handle_long_poll(ws: WebSocket, exit_event: threading.Event | None) -> None:
               threading.Thread(target=ws_ping, args=(ws, end_event), name='ws_ping'),
               threading.Thread(target=ws_queue, args=(end_event,), name='ws_queue'),
               threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler'),
-              # threading.Thread(target=sunny_log_handler, args=(end_event, comma_prime_cellular_end_event), name='log_handler'),
+              threading.Thread(target=sunny_log_handler, args=(end_event, comma_prime_cellular_end_event), name='log_handler'),
               threading.Thread(target=stat_handler, args=(end_event, Paths.stats_sp_root(), True), name='stat_handler'),
             ] + [
               threading.Thread(target=jsonrpc_handler, args=(end_event, partial(startLocalProxy, end_event),), name=f'worker_{x}')

sunnypilot/sunnylink/params_metadata.json

@@ -121,9 +121,18 @@
     "title": "Camera Debug Exp Time",
     "description": ""
   },
+  "CameraOffset": {
+    "title": "Adjust Camera Offset",
+    "description": "Virtually shift camera's perspective to move model's center to Left(+ values) or Right (- values)",
+    "min": -0.35,
+    "max": 0.35,
+    "step": 0.01,
+    "unit": "meters"
+  },
   "CarBatteryCapacity": {
     "title": "Car Battery Capacity",
-    "description": ""
+    "description": "Battery Size",
+    "unit": "kWh"
   },
   "CarList": {
     "title": "Supported Car List",
@@ -165,6 +174,10 @@
     "title": "Chevron Info",
     "description": ""
   },
+  "CompletedSunnylinkConsentVersion": {
+    "title": "Completed sunnylink Consent Version",
+    "description": ""
+  },
   "CompletedTrainingVersion": {
     "title": "Completed Training Version",
     "description": ""
@@ -182,7 +195,7 @@
     "description": ""
   },
   "CustomAccIncrementsEnabled": {
-    "title": "Custom ACC Increments Enabled",
+    "title": "Custom ACC Increments",
     "description": ""
   },
   "CustomAccLongPressIncrement": {
@@ -200,8 +213,8 @@
     "step": 1
   },
   "CustomTorqueParams": {
-    "title": "Custom Torque Params",
-    "description": ""
+    "title": "Enable Custom Torque Tuning",
+    "description": "Enables custom tuning for Torque lateral control"
   },
   "DevUIInfo": {
     "title": "Developer UI Info",
@@ -275,7 +288,7 @@
   },
   "EnforceTorqueControl": {
     "title": "Enforce Torque Control",
-    "description": ""
+    "description": "Enable this to enforce sunnypilot to steer with Torque lateral control."
   },
   "ExperimentalMode": {
     "title": "Experimental Mode",
@@ -349,6 +362,10 @@
     "title": "Has Accepted Terms",
     "description": ""
   },
+  "HasAcceptedTermsSP": {
+    "title": "Has Accepted sunnypilot Terms",
+    "description": ""
+  },
   "HideVEgoUI": {
     "title": "Hide vEgo UI",
     "description": ""
@@ -381,7 +398,8 @@
   },
   "InteractivityTimeout": {
     "title": "Interactivity Timeout",
-    "description": ""
+    "description": "",
+    "unit": "seconds"
   },
   "IsDevelopmentBranch": {
     "title": "Is Development Branch",
@@ -436,12 +454,15 @@
     "description": ""
   },
   "LagdToggle": {
-    "title": "LaGD Toggle",
-    "description": ""
+    "title": "Live Learning Steer Delay",
+    "description": "Allow device to learn and adapt car's steering response time"
   },
   "LagdToggleDelay": {
-    "title": "LaGD Toggle Delay",
-    "description": ""
+    "title": "Manual Software Delay",
+    "description": "Software delay to use when Live Learning Steer Delay is toggled off",
+    "min": 0.05,
+    "max": 0.5,
+    "step": 0.01
   },
   "LagdValueCache": {
     "title": "LaGD Value Cache",
@@ -449,11 +470,11 @@
   },
   "LaneTurnDesire": {
     "title": "Lane Turn Desire",
-    "description": ""
+    "description": "Force model to plan an intent to turn based on blinker"
   },
   "LaneTurnValue": {
-    "title": "Lane Turn Value",
-    "description": "",
+    "title": "Lane Turn Speed",
+    "description": "Maximum speed for lane turn desire",
     "min": 0,
     "max": 20,
     "step": 1
@@ -531,12 +552,12 @@
     "description": ""
   },
   "LiveTorqueParamsRelaxedToggle": {
-    "title": "Live Torque Params Relaxed Toggle",
-    "description": ""
+    "title": "Less Restrict Settings for Self-Tune (Beta)",
+    "description": "Less strict settings when using Self-Tune. This allows torqued to be more forgiving when learning values."
   },
   "LiveTorqueParamsToggle": {
-    "title": "Live Torque Params Toggle",
-    "description": ""
+    "title": "Self-Tune",
+    "description": "Enables self-tune for Torque lateral control"
   },
   "LocationFilterInitialState": {
     "title": "Location Filter Initial State",
@@ -612,7 +633,8 @@
   },
   "MaxTimeOffroad": {
     "title": "Max Time Offroad",
-    "description": ""
+    "description": "",
+    "unit": "seconds"
   },
   "ModelManager_ActiveBundle": {
     "title": "Model Manager Active Bundle",
@@ -689,7 +711,7 @@
     "description": ""
   },
   "OffroadMode": {
-    "title": "Offroad Mode",
+    "title": "Force Offroad Mode",
     "description": ""
   },
   "Offroad_CarUnrecognized": {
@@ -753,22 +775,69 @@
     "description": ""
   },
   "OnroadScreenOffBrightness": {
-    "title": "Onroad Screen Off Brightness",
+    "title": "Onroad Brightness",
     "description": "",
     "min": 0,
     "max": 100,
     "step": 5
   },
   "OnroadScreenOffControl": {
-    "title": "Onroad Screen Off Control",
-    "description": ""
+    "title": "Onroad Screen: Reduced Brightness",
+    "description": "Turn off device screen or reduce brightness after driving starts"
   },
   "OnroadScreenOffTimer": {
-    "title": "Onroad Screen Off Timer",
+    "title": "Onroad Brightness Delay",
     "description": "",
-    "min": 0,
-    "max": 60,
-    "step": 1
+    "options": [
+      {
+        "value": 15,
+        "label": "15s"
+      },
+      {
+        "value": 30,
+        "label": "30s"
+      },
+      {
+        "value": 60,
+        "label": "1m"
+      },
+      {
+        "value": 120,
+        "label": "2m"
+      },
+      {
+        "value": 180,
+        "label": "3m"
+      },
+      {
+        "value": 240,
+        "label": "4m"
+      },
+      {
+        "value": 300,
+        "label": "5m"
+      },
+      {
+        "value": 360,
+        "label": "6m"
+      },
+      {
+        "value": 420,
+        "label": "7m"
+      },
+      {
+        "value": 480,
+        "label": "8m"
+      },
+      {
+        "value": 540,
+        "label": "9m"
+      },
+      {
+        "value": 600,
+        "label": "10m"
+      }
+    ]
   },
   "OnroadUploads": {
     "title": "Onroad Uploads",
@@ -826,6 +895,13 @@
     "title": "Panda Som Reset Triggered",
     "description": ""
   },
+  "PlanplusControl": {
+    "title": "Plan Plus Controls",
+    "description": "Adjust planplus model recentering strength. The higher this number the more aggressively the model will recover to lanecenter, too high and it will ping-pong",
+    "min": 0.0,
+    "max": 2.0,
+    "step": 0.1
+  },
   "PrimeType": {
     "title": "Prime Type",
     "description": ""
@@ -863,7 +939,7 @@
     "description": ""
   },
   "RoadNameToggle": {
-    "title": "Road Name Toggle",
+    "title": "Display Road Name",
     "description": ""
   },
   "RouteCount": {
@@ -876,7 +952,7 @@
   },
   "ShowAdvancedControls": {
     "title": "Show Advanced Controls",
-    "description": ""
+    "description": "Enable to show advanced controls on device"
   },
   "ShowDebugInfo": {
     "title": "UI Debug Mode",
@@ -887,11 +963,11 @@
     "description": ""
   },
   "SmartCruiseControlMap": {
-    "title": "Smart Cruise Control Map",
+    "title": "Smart Cruise Control - Map",
     "description": ""
   },
   "SmartCruiseControlVision": {
-    "title": "Smart Cruise Control Vision",
+    "title": "Smart Cruise Control - Vision",
     "description": ""
   },
   "SnoozeUpdate": {
@@ -899,7 +975,7 @@
     "description": ""
   },
   "SpeedLimitMode": {
-    "title": "Speed Limit Mode",
+    "title": "Speed Limit Assist Mode",
     "description": "",
     "options": [
       {
@@ -939,7 +1015,7 @@
     ]
   },
   "SpeedLimitPolicy": {
-    "title": "Speed Limit Policy",
+    "title": "Speed Limit Source",
     "description": "",
     "options": [
       {
@@ -965,7 +1041,7 @@
     ]
   },
   "SpeedLimitValueOffset": {
-    "title": "Speed Limit Value Offset",
+    "title": "Speed Limit Offset Value",
     "description": "",
     "min": -30,
     "max": 30,
@@ -1020,22 +1096,23 @@
     "description": ""
   },
   "TorqueParamsOverrideEnabled": {
-    "title": "Torque Params Override Enabled",
+    "title": "Manual Real-Time Tuning",
     "description": ""
   },
   "TorqueParamsOverrideFriction": {
-    "title": "Torque Params Override Friction",
+    "title": "Manual Tune - Friction",
     "description": "",
     "min": 0.0,
     "max": 1.0,
     "step": 0.01
   },
   "TorqueParamsOverrideLatAccelFactor": {
-    "title": "Torque Params Override Lat Accel Factor",
+    "title": "Manual Tune - Lateral Acceleration Factor",
     "description": "",
     "min": 0.1,
     "max": 5.0,
-    "step": 0.1
+    "step": 0.1,
+    "unit": "m/s²"
   },
   "ToyotaEnforceStockLongitudinal": {
     "title": "Toyota: Enforce Factory Longitudinal Control",

sunnypilot/sunnylink/sunnylink_state.py

@@ -136,10 +136,11 @@ class SunnylinkState:
       token = self._api.get_token()
       response = self._api.api_get(f"device/{self.sunnylink_dongle_id}/roles", method='GET', access_token=token)
       if response.status_code == 200:
-        self._roles = _parse_roles(response.text)
-        self._params.put("SunnylinkCache_Roles", response.text)
-        sponsor_tier = self._get_highest_tier()
+        roles = response.text
+        self._params.put("SunnylinkCache_Roles", roles)
         with self._lock:
+          self._roles = _parse_roles(roles)
+          sponsor_tier = self._get_highest_tier()
           if sponsor_tier != self.sponsor_tier:
             self.sponsor_tier = sponsor_tier
             cloudlog.info(f"Sunnylink sponsor tier updated to {sponsor_tier.name}")
@@ -157,7 +158,7 @@ class SunnylinkState:
         users = response.text
         self._params.put("SunnylinkCache_Users", users)
         with self._lock:
-          _parse_users(users)
+          self._users = _parse_users(users)
     except Exception as e:
       cloudlog.exception(f"Failed to fetch sunnylink users: {e} for dongle id {self.sunnylink_dongle_id}")
 

system/hardware/hardwared.py

@@ -23,7 +23,7 @@ from openpilot.system.statsd import statlog
 from openpilot.common.swaglog import cloudlog
 from openpilot.system.hardware.power_monitoring import PowerMonitoring
 from openpilot.system.hardware.fan_controller import TiciFanController
-from openpilot.system.version import terms_version, training_version, get_build_metadata
+from openpilot.system.version import terms_version, training_version, get_build_metadata, terms_version_sp
 
 ThermalStatus = log.DeviceState.ThermalStatus
 NetworkType = log.DeviceState.NetworkType
@@ -310,6 +310,7 @@ def hardware_thread(end_event, hw_queue) -> None:
     startup_conditions["no_excessive_actuation"] = params.get("Offroad_ExcessiveActuation") is None
     startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall")
     startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version
+    startup_conditions["accepted_terms_sp"] = params.get("HasAcceptedTermsSP") == terms_version_sp
 
     # with 2% left, we killall, otherwise the phone will take a long time to boot
     startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2

system/ui/sunnypilot/widgets/list_view.py

@@ -11,15 +11,29 @@ from openpilot.common.params import Params
 from openpilot.system.ui.lib.application import gui_app, MousePos, FontWeight
 from openpilot.system.ui.lib.text_measure import measure_text_cached
 from openpilot.system.ui.sunnypilot.widgets.toggle import ToggleSP
+from openpilot.system.ui.widgets import Widget
 from openpilot.system.ui.widgets.button import Button, ButtonStyle
 from openpilot.system.ui.widgets.label import gui_label
 from openpilot.system.ui.widgets.list_view import ListItem, ToggleAction, ItemAction, MultipleButtonAction, ButtonAction, \
-                                                  _resolve_value, BUTTON_WIDTH, BUTTON_HEIGHT, TEXT_PADDING
+                                                  _resolve_value, BUTTON_WIDTH, BUTTON_HEIGHT, TEXT_PADDING, DualButtonAction
 from openpilot.system.ui.widgets.scroller_tici import LineSeparator, LINE_COLOR, LINE_PADDING
 from openpilot.system.ui.sunnypilot.lib.styles import style
 from openpilot.system.ui.sunnypilot.widgets.option_control import OptionControlSP, LABEL_WIDTH
 
 
+class Spacer(Widget):
+  def __init__(self, height: int = 1):
+    super().__init__()
+    self._rect = rl.Rectangle(0, 0, 0, height)
+
+  def set_parent_rect(self, parent_rect: rl.Rectangle) -> None:
+    super().set_parent_rect(parent_rect)
+    self._rect.width = parent_rect.width
+
+  def _render(self, _):
+    rl.draw_rectangle(int(self._rect.x), int(self._rect.y), int(self._rect.x + self._rect.width), int(self._rect.y), rl.Color(0,0,0,0))
+
+
 class ToggleActionSP(ToggleAction):
   def __init__(self, initial_state: bool = False, width: int = style.TOGGLE_WIDTH, enabled: bool | Callable[[], bool] = True,
                callback: Callable[[bool], None] | None = None, param: str | None = None):
@@ -84,6 +98,33 @@ class ButtonActionSP(ButtonAction):
     return pressed
 
 
+class DualButtonActionSP(DualButtonAction):
+  def __init__(self, left_text: str | Callable[[], str], right_text: str | Callable[[], str], left_callback: Callable = None,
+               right_callback: Callable = None, enabled: bool | Callable[[], bool] = True, border_radius: int = 15):
+    DualButtonAction.__init__(self, left_text, right_text, left_callback, right_callback, enabled)
+    self.left_button._border_radius = self.right_button._border_radius = border_radius
+
+  def _render(self, rect: rl.Rectangle):
+    button_spacing = 20
+    button_height = 150
+    button_width = (rect.width - button_spacing) / 2
+    button_y = rect.y + (rect.height - button_height) / 2
+
+    left_rect = rl.Rectangle(rect.x, button_y, button_width, button_height)
+    right_rect = rl.Rectangle(rect.x + button_width + button_spacing, button_y, button_width, button_height)
+
+    # expand one to full width if other is not visible
+    if not self.left_button.is_visible:
+      right_rect.x = rect.x
+      right_rect.width = rect.width
+    elif not self.right_button.is_visible:
+      left_rect.width = rect.width
+
+    # Render buttons
+    self.left_button.render(left_rect)
+    self.right_button.render(right_rect)
+
+
 class MultipleButtonActionSP(MultipleButtonAction):
   def __init__(self, buttons: list[str | Callable[[], str]], button_width: int, selected_index: int = 0, callback: Callable = None,
                param: str | None = None):
@@ -251,13 +292,13 @@ class ListItemSP(ListItem):
         item_y = self._rect.y + (style.ITEM_BASE_HEIGHT - self._text_size.y) // 2 if self.inline else self._rect.y + style.ITEM_PADDING * 1.5
         rl.draw_text_ex(self._font, self.title, rl.Vector2(text_x, item_y), style.ITEM_TEXT_FONT_SIZE, 0, self.title_color)
 
-        # Draw right item if present
-        if self.action_item:
-          right_rect = self.get_right_item_rect(self._rect)
-          if self.action_item.render(right_rect) and self.action_item.enabled:
-            # Right item was clicked/activated
-            if self.callback:
-              self.callback()
+      # Draw right item if present
+      if self.action_item:
+        right_rect = self.get_right_item_rect(self._rect)
+        if self.action_item.render(right_rect) and self.action_item.enabled:
+          # Right item was clicked/activated
+          if self.callback:
+            self.callback()
 
     # Draw description if visible
     if self.description_visible:
@@ -296,12 +337,12 @@ def option_item_sp(title: str | Callable[[], str], param: str,
                    value_change_step: int = 1, on_value_changed: Callable[[int], None] | None = None,
                    enabled: bool | Callable[[], bool] = True,
                    icon: str = "", label_width: int = LABEL_WIDTH, value_map: dict[int, int] | None = None,
-                   use_float_scaling: bool = False, label_callback: Callable[[int], str] | None = None) -> ListItemSP:
+                   use_float_scaling: bool = False, label_callback: Callable[[int], str] | None = None, inline: bool = False) -> ListItemSP:
   action = OptionControlSP(
     param, min_value, max_value, value_change_step,
     enabled, on_value_changed, value_map, label_width, use_float_scaling, label_callback
   )
-  return ListItemSP(title=title, description=description, action_item=action, icon=icon)
+  return ListItemSP(title=title, description=description, action_item=action, icon=icon, inline=inline)
 
 
 def button_item_sp(title: str | Callable[[], str], button_text: str | Callable[[], str], description: str | Callable[[], str] | None = None,
@@ -310,6 +351,13 @@ def button_item_sp(title: str | Callable[[], str], button_text: str | Callable[[
   return ListItemSP(title=title, description=description, action_item=action, callback=callback)
 
 
+def dual_button_item_sp(left_text: str | Callable[[], str], right_text: str | Callable[[], str], left_callback: Callable = None,
+                        right_callback: Callable = None, description: str | Callable[[], str] | None = None,
+                        enabled: bool | Callable[[], bool] = True, border_radius: int = 15) -> ListItemSP:
+  action = DualButtonActionSP(left_text, right_text, left_callback, right_callback, enabled, border_radius)
+  return ListItemSP(title="", description=description, action_item=action)
+
+
 class LineSeparatorSP(LineSeparator):
   def __init__(self, height: int = 1):
     super().__init__()

system/version.py

@@ -30,6 +30,9 @@ BUILD_METADATA_FILENAME = "build.json"
 
 training_version: str = "0.2.0"
 terms_version: str = "2"
+terms_version_sp: str = "1.0"
+sunnylink_consent_version: str = "1.0"
+sunnylink_consent_declined: str = "-1"
 
 
 def get_version(path: str = BASEDIR) -> str:

third_party/mapd_pfeiferj/README.md

@@ -1,2 +0,0 @@
-# MAPD implementation by pfeiferj
-https://github.com/pfeiferj/openpilot-mapd/releases/