toyota: stop controlsd crashing with LTA angle steering on

Wires the ToyotaAngleSteering and ToyotaAngleSteeringHighOutput params 
into the car interface, adds the raylib and sunnylink toggles (Prius 
TSS2 only), registers the new params key, and bumps opendbc_repo to the 
sunnypilot LTA implementation.

.github/workflows/test_models.yml

@@ -3,9 +3,62 @@ on:
   pull_request:
     paths:
       - 'tinygrad_repo'
+      - 'openpilot/sunnypilot/modeld_v2/**'
   workflow_dispatch:
 
+env:
+  CI: 1
+  GIT_CONFIG_COUNT: 1
+  GIT_CONFIG_KEY_0: lfs.fetchexclude
+  GIT_CONFIG_VALUE_0: openpilot/selfdrive/modeld/models/big_driving_tinygrad.pkl
+
 jobs:
+  plain-artifact:
+    name: Plain artifact round trip and pickle converter
+    runs-on: ubuntu-24.04
+    env:
+      PYTHONPATH: ${{ github.workspace }}/tinygrad_repo:${{ github.workspace }}
+      TINYGRAD_TESTS: 1
+    steps:
+      - uses: actions/checkout@v7
+      - run: ./tools/op.sh setup
+      - name: Build openpilot
+        run: scons -j$(nproc)
+      - name: Compile, load, cache and fuse a model with the pinned tinygrad
+        env:
+          DEV: CPU
+        run: tools/op.sh test -v openpilot/sunnypilot/modeld_v2/tests/test_plain_artifact.py
+      - name: Install LLVM 20 and ROCm comgr for the amd mock, as tinygrad_repo/.github/actions/setup-tinygrad does
+        run: |
+          . /etc/os-release
+          curl -fsSL https://apt.llvm.org/llvm-snapshot.gpg.key | sudo tee /etc/apt/trusted.gpg.d/apt.llvm.org.asc > /dev/null
+          echo "deb http://apt.llvm.org/$VERSION_CODENAME/ llvm-toolchain-$VERSION_CODENAME-20 main" | sudo tee /etc/apt/sources.list.d/llvm.list
+          sudo mkdir -p /etc/apt/keyrings
+          curl -fsSL https://repo.radeon.com/rocm/rocm.gpg.key | gpg --dearmor | sudo tee /etc/apt/keyrings/rocm.gpg > /dev/null
+          echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/rocm.gpg] https://repo.radeon.com/rocm/apt/7.1 $VERSION_CODENAME main" | sudo tee /etc/apt/sources.list.d/rocm.list
+          printf 'Package: *\nPin: release o=repo.radeon.com\nPin-Priority: 600\n' | sudo tee /etc/apt/preferences.d/rocm-pin-600
+          sudo apt-get -qq update && sudo apt-get -qq install -y --no-install-recommends libllvm20 clang-20 lld-20 comgr
+          printf '/opt/rocm/lib\n/opt/rocm/lib64\n' | sudo tee /etc/ld.so.conf.d/rocm.conf && sudo ldconfig
+          echo "/usr/lib/llvm-20/bin" >> "$GITHUB_PATH"
+      - name: Convert a pickled USB+AMD jit and replay it under tinygrad's amd mock
+        env:
+          DEV: MOCKUSB+AMD
+          CHECK_OOB: 0
+          GMMU: 0
+        run: tools/op.sh test -v openpilot/sunnypilot/modeld_v2/tests/test_convert_upstream_pkl.py
+      - name: Where tinygrad moved, for the helper or failure point named above
+        if: failure() && github.event_name == 'pull_request'
+        run: |
+          git fetch -q --depth=1 origin "${{ github.base_ref }}"
+          old=$(git rev-parse "origin/${{ github.base_ref }}:tinygrad_repo") new=$(git rev-parse HEAD:tinygrad_repo)
+          git -C tinygrad_repo fetch -q origin "$old" || true
+          echo "tinygrad_repo $old -> $new, in what helpers.py and plain_artifact_compiler.py read:"
+          git -C tinygrad_repo diff "$old" "$new" -- tinygrad/uop/ops.py tinygrad/engine/jit.py tinygrad/engine/realize.py tinygrad/device.py \
+            tinygrad/helpers.py tinygrad/dtype.py tinygrad/renderer/__init__.py
+          echo "and in what convert_upstream_pkl.py decodes:"
+          git -C tinygrad_repo diff --stat "$old" "$new" -- tinygrad/runtime/autogen/am/pm4_nv.py tinygrad/runtime/autogen/amd_gpu.py
+          git -C tinygrad_repo diff "$old" "$new" -- tinygrad/runtime/support/hcq2.py tinygrad/runtime/support/usb.py tinygrad/runtime/ops_amd.py
+
   generate-matrix:
     runs-on: ubuntu-latest
     outputs:

.github/workflows/tests.yaml

@@ -50,6 +50,12 @@ jobs:
       timeout-minutes: 3
       run: TARGET_DIR=$STRIPPED_DIR tools/release/build_stripped.sh
     - run: ./tools/op.sh setup
+    - name: Restore scons cache
+      uses: actions/cache@v6
+      with:
+        path: /tmp/scons_cache
+        key: scons-${{ runner.os }}-${{ github.job }}-${{ github.sha }}
+        restore-keys: scons-${{ runner.os }}-${{ github.job }}-
     - name: Build openpilot and run checks
       timeout-minutes: 30
       working-directory: ${{ env.STRIPPED_DIR }}
@@ -89,6 +95,12 @@ jobs:
         FILTERED=$(echo "$PATH" | tr ':' '\n' | grep -v '/opt/homebrew' | tr '\n' ':')
         echo "PATH=${FILTERED}/usr/local/bin:/usr/bin:/bin:/usr/sbin:/sbin" >> $GITHUB_ENV
     - run: ./tools/op.sh setup
+    - name: Restore scons cache
+      uses: actions/cache@v6
+      with:
+        path: /tmp/scons_cache
+        key: scons-${{ runner.os }}-${{ github.job }}-${{ github.sha }}
+        restore-keys: scons-${{ runner.os }}-${{ github.job }}-
     - name: Building openpilot
       run: scons
 
@@ -121,6 +133,12 @@ jobs:
     - name: Install raylib headless dependencies
       if: ${{ !contains(runner.name, 'nsc') }}
       run: sudo apt-get install -y --no-install-recommends libgles2 libegl1
+    - name: Restore scons cache
+      uses: actions/cache@v6
+      with:
+        path: /tmp/scons_cache
+        key: scons-${{ runner.os }}-${{ github.job }}-${{ github.sha }}
+        restore-keys: scons-${{ runner.os }}-${{ github.job }}-
     - name: Build openpilot
       run: scons
     - name: Run unit tests
@@ -140,6 +158,12 @@ jobs:
     steps:
     - uses: actions/checkout@v7
     - run: ./tools/op.sh setup
+    - name: Restore scons cache
+      uses: actions/cache@v6
+      with:
+        path: /tmp/scons_cache
+        key: scons-${{ runner.os }}-${{ github.job }}-${{ github.sha }}
+        restore-keys: scons-${{ runner.os }}-${{ github.job }}-
     - name: Build openpilot
       run: scons
     - name: Run replay
@@ -228,6 +252,12 @@ jobs:
       - name: Install raylib headless dependencies
         if: ${{ !contains(runner.name, 'nsc') }}
         run: sudo apt-get install -y --no-install-recommends libgles2 libegl1
+      - name: Restore scons cache
+        uses: actions/cache@v6
+        with:
+          path: /tmp/scons_cache
+          key: scons-${{ runner.os }}-${{ github.job }}-${{ github.sha }}
+          restore-keys: scons-${{ runner.os }}-${{ github.job }}-
       - name: Build openpilot
         run: scons
       - name: Create UI Report

.gitmodules

@@ -4,6 +4,7 @@
 [submodule "opendbc"]
   path = opendbc_repo
   url = https://github.com/sunnypilot/opendbc.git
+  branch = toyotabsmhack
 [submodule "msgq"]
   path = msgq_repo
   url = https://github.com/sunnypilot/msgq.git

openpilot/cereal/custom.capnp

@@ -468,6 +468,13 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
   laneTurnDirection @0 :TurnDirection;
   leftLaneChangeEdgeBlock @1 :Bool;
   rightLaneChangeEdgeBlock @2 :Bool;
+  blendfulCurvature @3 :Float32;
+  blendfulTrust @4 :Float32;
+  modelCurvature @5 :Float32;
+  blendfulGoingAround @6 :Bool;
+  blendfulLeftProb @7 :Float32;
+  blendfulRightProb @8 :Float32;
+  blendfulLaneWidth @9 :Float32;
 
   enum TurnDirection {
     none @0;

openpilot/cereal/services.py

@@ -94,7 +94,7 @@ _services: dict[str, tuple] = {
   "carControlSP": (True, 100., 10),
   "carStateSP": (True, 100., 10),
   "liveMapDataSP": (True, 1., 1),
-  "modelDataV2SP": (True, 20., None, QueueSize.BIG),
+  "modelDataV2SP": (True, 20., 10, QueueSize.BIG),
   "liveLocationKalman": (True, 20.),
 
   # debug

openpilot/common/params_keys.h

@@ -170,6 +170,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}},
     {"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
     {"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
+    {"FollowDistanceAccelBoost", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}},
     {"HasAcceptedTermsSP", {PERSISTENT, STRING, "0"}},
@@ -244,6 +245,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"SubaruStopAndGoManualParkingBrake", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"TeslaCoopSteering", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"TeslaMadsScreenButton", {PERSISTENT | BACKUP, INT, "0"}},
+    {"ToyotaAngleSteering", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"ToyotaAngleSteeringHighOutput", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"ToyotaEnhancedBsm", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ToyotaEnforceStockLongitudinal", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ToyotaStopAndGoHack", {PERSISTENT | BACKUP, BOOL, "0"}},
 
@@ -256,6 +260,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"LagdValueCache", {PERSISTENT, FLOAT, "0.2"}},
     {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}},
+    {"Blendful", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
 
     // mapd

openpilot/selfdrive/controls/lib/desire_helper.py

@@ -64,16 +64,19 @@ class DesireHelper:
                          ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
                           (carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
 
-        blindspot_detected = (((carstate.leftBlindspot or left_edge_detected) and self.lane_change_direction == LaneChangeDirection.left) or
-                              ((carstate.rightBlindspot or right_edge_detected) and self.lane_change_direction == LaneChangeDirection.right))
+        blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
+                              (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
 
-        self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
+        edge_detected = ((left_edge_detected and self.lane_change_direction == LaneChangeDirection.left) or
+                         (right_edge_detected and self.lane_change_direction == LaneChangeDirection.right))
+
+        self.alc.update_lane_change(blindspot_detected, edge_detected, carstate.brakePressed, torque_applied)
 
         if not one_blinker or below_lane_change_speed:
           self.lane_change_state = LaneChangeState.off
           self.lane_change_direction = LaneChangeDirection.none
           self.lane_change_timer = 0.0
-        elif (torque_applied or self.alc.auto_lane_change_allowed) and not blindspot_detected:
+        elif (torque_applied or self.alc.auto_lane_change_allowed) and not self.alc.lane_change_blocked:
           self.lane_change_state = LaneChangeState.laneChangeStarting
           self.lane_change_timer = 0.0
 

openpilot/selfdrive/controls/tests/test_accel_boost.py

@@ -0,0 +1,101 @@
+from types import SimpleNamespace
+
+from openpilot.cereal import log
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.controls.lib.accel_boost import (
+  AccelBoost, ACCEL_BOOST_MAX, ACCEL_BOOST_PER_OVERRIDE, ACCEL_BOOST_RATE
+)
+
+E2E_ACCEL = 0.5
+PLANNER_ACCEL = 2.0
+RATE_PER_STEP = ACCEL_BOOST_RATE * DT_MDL
+
+
+class FakeSubMaster:
+  def __init__(self, enabled=True, gas_pressed=False, v_ego=15.0,
+               personality=log.LongitudinalPersonality.standard):
+    self._messages = {
+      'selfdriveState': SimpleNamespace(enabled=enabled, personality=personality),
+      'carState': SimpleNamespace(gasPressed=gas_pressed, vEgo=v_ego),
+    }
+
+  def __getitem__(self, key):
+    return self._messages[key]
+
+
+def run_boost(boost, sm, steps=1):
+  out = E2E_ACCEL
+  for _ in range(steps):
+    out = boost.update(sm, E2E_ACCEL, PLANNER_ACCEL, PLANNER_ACCEL)
+  return out
+
+
+class TestAccelBoost(OpenpilotTestCase):
+  def _boost_to_max(self):
+    boost = AccelBoost()
+    sm = FakeSubMaster()
+    steps = round(ACCEL_BOOST_PER_OVERRIDE / RATE_PER_STEP)
+    # Eligibility is latched on a frame without gas, then applied on the next override.
+    run_boost(boost, sm)
+    for _ in range(10):
+      sm['carState'].gasPressed = True
+      run_boost(boost, sm, steps)
+      sm['carState'].gasPressed = False
+      run_boost(boost, sm)
+    return boost
+
+  def test_no_boost_without_gas(self):
+    boost = AccelBoost()
+    sm = FakeSubMaster(gas_pressed=False)
+    out = run_boost(boost, sm)
+    self.assertEqual(boost.total_boost, 0.0)
+    self.assertAlmostEqual(out, E2E_ACCEL)
+
+  def test_boost_requires_e2e_to_be_limiting(self):
+    boost = AccelBoost()
+    sm = FakeSubMaster(gas_pressed=False)
+    # Eligibility is latched before the override, so keep the e2e plan at the planner limit here.
+    boost.update(sm, E2E_ACCEL, E2E_ACCEL, E2E_ACCEL)
+    sm['carState'].gasPressed = True
+    for _ in range(10):
+      boost.update(sm, E2E_ACCEL, E2E_ACCEL, E2E_ACCEL)
+    self.assertEqual(boost.total_boost, 0.0)
+
+  def test_per_override_cap(self):
+    boost = AccelBoost()
+    sm = FakeSubMaster(gas_pressed=False)
+    run_boost(boost, sm)
+    sm['carState'].gasPressed = True
+    run_boost(boost, sm, 1000)
+    self.assertAlmostEqual(boost.total_boost, ACCEL_BOOST_PER_OVERRIDE)
+
+  def test_max_boost(self):
+    boost = self._boost_to_max()
+    self.assertAlmostEqual(boost.total_boost, ACCEL_BOOST_MAX)
+
+  def test_boost_is_applied_to_e2e_accel(self):
+    boost = self._boost_to_max()
+    sm = FakeSubMaster(gas_pressed=True)
+    out = run_boost(boost, sm)
+    self.assertAlmostEqual(out, E2E_ACCEL + boost.total_boost)
+
+  def test_boost_does_not_depend_on_following_distance(self):
+    totals = []
+    for personality in (log.LongitudinalPersonality.relaxed,
+                        log.LongitudinalPersonality.standard,
+                        log.LongitudinalPersonality.aggressive):
+      boost = AccelBoost()
+      sm = FakeSubMaster(personality=personality)
+      steps = round(ACCEL_BOOST_PER_OVERRIDE / RATE_PER_STEP)
+      run_boost(boost, sm)
+      for _ in range(10):
+        sm['carState'].gasPressed = True
+        run_boost(boost, sm, steps)
+        sm['carState'].gasPressed = False
+        run_boost(boost, sm)
+      totals.append(boost.total_boost)
+
+    self.assertEqual(len(totals), 3)
+    for total in totals:
+      self.assertAlmostEqual(total, ACCEL_BOOST_MAX)

openpilot/selfdrive/ui/mici/layouts/settings/toggles.py

@@ -21,6 +21,9 @@ class TogglesLayoutMici(NavScroller):
                                                                "Use the steering wheel distance button on supported cars.")
     self._initial_cruise_speed = InitialCruiseSpeedButton()
     self._initial_cruise_speed.set_visible(lambda: ui_state.CP is not None and ui_state.has_longitudinal_control and not ui_state.CP.pcmCruise)
+    self._accel_boost_toggle = BigParamControl("extra acceleration boost", "FollowDistanceAccelBoost",
+                                               description="Adds extra acceleration on top of the stock gas-override boost.\n" +
+                                                           "The extra boost follows Following Distance: strongest when Aggressive, weakest when Relaxed.")
     is_metric_toggle = BigToggle("use metric units", initial_state=ui_state.params.get_bool("IsMetric"), toggle_callback=self._on_metric_toggle)
     ldw_toggle = BigParamControl("lane departure warnings", "IsLdwEnabled",
                                  description="Warn when you drift across a detected lane line.\n" +
@@ -40,6 +43,7 @@ class TogglesLayoutMici(NavScroller):
 
     self._scroller.add_widgets([
       self._personality_toggle,
+      self._accel_boost_toggle,
       self._initial_cruise_speed,
       is_metric_toggle,
       ldw_toggle,
@@ -51,6 +55,7 @@ class TogglesLayoutMici(NavScroller):
 
     # Toggle lists
     self._refresh_toggles = (
+      ("FollowDistanceAccelBoost", self._accel_boost_toggle),
       ("IsMetric", is_metric_toggle),
       ("IsLdwEnabled", ldw_toggle),
       ("AlwaysOnDM", always_on_dm_toggle),
@@ -92,6 +97,7 @@ class TogglesLayoutMici(NavScroller):
 
     if ui_state.CP is not None:
       self._personality_toggle.set_visible(ui_state.has_longitudinal_control)
+      self._accel_boost_toggle.set_visible(ui_state.has_longitudinal_control)
 
     # Refresh toggles from params to mirror external changes
     for key, item in self._refresh_toggles:

openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py

@@ -22,6 +22,7 @@ from openpilot.common.transformations.orientation import rot_from_euler
 from enum import IntEnum
 
 if gui_app.sunnypilot_ui():
+  from openpilot.selfdrive.ui.sunnypilot.mici.onroad.accel_boost_indicator import AccelBoostIndicator
   from openpilot.selfdrive.ui.sunnypilot.mici.onroad.hud_renderer import HudRendererSP as HudRenderer
   from openpilot.selfdrive.ui.sunnypilot.ui_state import OnroadTimerStatus
 
@@ -181,6 +182,7 @@ class AugmentedRoadView(CameraView):
     self._alert_renderer = AlertRenderer()
     self._driver_state_renderer = DriverStateRenderer()
     self._confidence_ball = ConfidenceBall()
+    self._accel_boost_indicator = AccelBoostIndicator() if gui_app.sunnypilot_ui() else None
     self._offroad_label = UnifiedLabel("start the car to\nuse sunnypilot", 54, FontWeight.DISPLAY,
                                        text_color=rl.Color(255, 255, 255, int(255 * 0.9)),
                                        alignment=TextAlignment.CENTER,
@@ -275,7 +277,13 @@ class AugmentedRoadView(CameraView):
     if show_navigation:
       self._navigation.render(rl.Rectangle(self._content_rect.x + self._content_rect.width, self.rect.y, PANEL_WIDTH, self.rect.height))
     else:
-      self._confidence_ball.render(self.rect)
+      # The boost bar owns the side panel while the extra boost is on; the ball only shows with the feature off
+      if gui_app.sunnypilot_ui():
+        self._accel_boost_indicator.render(self.rect)
+        if not self._accel_boost_indicator.panel_in_use:
+          self._confidence_ball.render(self.rect)
+      else:
+        self._confidence_ball.render(self.rect)
 
     self._bookmark_icon.render(self.rect)
 

openpilot/selfdrive/ui/mici/onroad/model_renderer.py

@@ -141,6 +141,7 @@ class ModelRenderer(Widget, ModelRendererSP):
     if ui_state.status != UIStatus.DISENGAGED:
       self._draw_lane_lines()
       self._draw_path()
+      self.blendful.draw(self._rect)
 
     if render_lead_indicator:
       self._update_leads(sm)
@@ -219,6 +220,7 @@ class ModelRenderer(Widget, ModelRendererSP):
     # Update road edges using raw points
     for road_edge in self._road_edges:
       road_edge.projected_points = self._map_line_to_polygon(road_edge.raw_points, line_width_factor, 0.0, max_idx)
+    self.blendful.update(self, max_idx)
 
     # Update path using raw points
     if lead and lead.present:

openpilot/selfdrive/ui/onroad/model_renderer.py

@@ -123,6 +123,8 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
     # Draw elements
     self._draw_lane_lines()
     self._draw_path()
+    if self._lateral_active:
+      self.blendful.draw(self._rect)
 
     if render_lead_indicator and radar_state:
       self._draw_lead_indicator()
@@ -172,6 +174,7 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
     # Update road edges using raw points
     for road_edge in self._road_edges:
       road_edge.projected_points = self._map_line_to_polygon(road_edge.raw_points, 0.025, 0.0, max_idx, max_distance)
+    self.blendful.update(self, max_idx, max_distance)
 
     # Update path using raw points
     if lead and lead.present:

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

@@ -30,6 +30,11 @@ ACC_NOLONG_DESCRIPTION = tr_noop("This feature can only be used with sunnypilot
 ACC_PCMCRUISE_DISABLED_DESCRIPTION = tr_noop("This feature is not supported on this platform due to vehicle limitations.")
 ONROAD_ONLY_DESCRIPTION = tr_noop("Start the vehicle to check vehicle compatibility.")
 
+ACCEL_BOOST_EXT_DESCRIPTION = tr_noop("Adds extra acceleration on top of the stock gas-override boost when you hold the gas pedal " +
+                                       "while sunnypilot longitudinal control is active. The extra boost follows the Following Distance setting: " +
+                                       "strongest when Aggressive, weakest when Relaxed. The stock boost remains active when this is off.")
+ACCEL_BOOST_EXT_NOLONG_DESCRIPTION = tr_noop("This feature requires sunnypilot longitudinal control to be available.")
+
 
 class CruiseLayout(Widget):
   def __init__(self):
@@ -76,6 +81,11 @@ class CruiseLayout(Widget):
       min_value=1, max_value=3, value_change_step=1,
       inline=True)
 
+    self.accel_boost_toggle = toggle_item_sp(
+      title=tr("Extra Acceleration Boost by Following Distance"),
+      description="",
+      param="FollowDistanceAccelBoost")
+
     self.sla_settings_button = simple_button_item_sp(
       button_text=lambda: tr("Speed Limit"),
       button_width=800,
@@ -86,6 +96,7 @@ class CruiseLayout(Widget):
       self.icbm_toggle,
       self.scc_v_toggle,
       self.scc_m_toggle,
+      self.accel_boost_toggle,
       self.custom_acc_toggle,
       self.custom_acc_short_increment,
       self.custom_acc_long_increment,
@@ -104,6 +115,7 @@ class CruiseLayout(Widget):
     self._scroller.show_event()
     self.icbm_toggle.show_description(True)
     self.custom_acc_toggle.show_description(True)
+    self.accel_boost_toggle.show_description(True)
 
   def _set_current_panel(self, panel: PanelType):
     self._current_panel = panel
@@ -148,10 +160,17 @@ class CruiseLayout(Widget):
         self.scc_v_toggle.action_item.set_enabled(False)
         self.scc_m_toggle.action_item.set_enabled(False)
 
+      self.accel_boost_toggle.action_item.set_enabled(has_long)
+      boost_desc = tr(ACCEL_BOOST_EXT_DESCRIPTION) if has_long else tr(ACCEL_BOOST_EXT_NOLONG_DESCRIPTION)
+      if self.accel_boost_toggle.description != boost_desc:
+        self.accel_boost_toggle.set_description(boost_desc)
+        self.accel_boost_toggle.show_description(True)
+
     else:
       has_icbm = has_long = False
       self.icbm_toggle.action_item.set_enabled(False)
       self.icbm_toggle.set_description(tr(ONROAD_ONLY_DESCRIPTION))
+      self.accel_boost_toggle.action_item.set_enabled(False)
 
     show_custom_acc_desc = False
 

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

@@ -102,13 +102,19 @@ class ModelsLayout(Widget):
 
     self.lagd_toggle = toggle_item_sp(tr("Live Learning Steer Delay"), "", param="LagdToggle")
 
+    self.blendful_toggle = toggle_item_sp(tr("Blendful (Alpha)"),
+                                          tr("Gently pulls the model toward the lane center, as much as it trusts the lane lines," +
+                                             " and yields around obstacles."),
+                                          param="Blendful")
+
     self.camera_offset = option_item_sp(tr("Adjust Camera Offset"), "CameraOffset", -35, 35,
                                         tr("Virtually shift camera's perspective to move model's center to Left(+ values) or Right (- values)"),
                                         1, None, True, "", style.BUTTON_ACTION_WIDTH, None, True,
                                         lambda v: f"{v / 100:.2f} m")
 
     self.items = [self.small_model_item, self.big_model_item, self.cancel_download_item, self.download_item, self.refresh_item, self.clear_cache_item,
-                  self.lane_turn_desire_toggle, self.lane_turn_value_control, self.lagd_toggle, self.delay_control, self.camera_offset]
+                  self.blendful_toggle, self.lane_turn_desire_toggle, self.lane_turn_value_control, self.lagd_toggle, self.delay_control,
+                  self.camera_offset]
 
   def _update_lagd_description(self, lagd_toggle: bool):
     desc = tr("Enable this for the car to learn and adapt its steering response time. Disable to use a fixed steering response time. " +
@@ -243,7 +249,9 @@ class ModelsLayout(Widget):
     if ds.verifying in statuses:
       return {"name": name, "downloading": True, "progress": progress, "status_text": tr("verifying")}
     if ds.downloading in statuses:
-      return {"name": name, "downloading": True, "progress": progress}
+      speed = max((p.speed for p in progresses), default=0.0)
+      status_text = f"{speed / 1024 ** 2:.1f} MB/s" if speed > 0 else "--.- MB/s"
+      return {"name": name, "downloading": True, "progress": progress, "status_text": status_text}
     if statuses <= {ds.downloaded, ds.cached}:
       return {"name": name, "text_color": ON_COLOR, "icon": "icons/checkmark.png"}
     # circled_slash is authored grey; tinting it again only darkens it
@@ -320,6 +328,15 @@ class ModelsLayout(Widget):
     self.lane_turn_desire_toggle.action_item.set_state(turn_desire)
     self.lane_turn_value_control.set_visible(turn_desire and advanced_controls)
     self.lagd_toggle.action_item.set_state(live_delay)
+
+    blendful_allowed = ui_state.active_bundle is not None
+    if blendful_allowed:
+      self.blendful_toggle.action_item.set_state(ui_state.params.get_bool("Blendful"))
+      self.blendful_toggle.action_item.set_enabled(True)
+    else:
+      self.blendful_toggle.action_item.set_state(False)
+      self.blendful_toggle.action_item.set_enabled(False)
+
     self.delay_control.set_visible(not live_delay and advanced_controls)
     new_step = int(round(100 / CV.MPH_TO_KPH)) if ui_state.is_metric else 100
     if self.lane_turn_value_control.action_item is not None and self.lane_turn_value_control.action_item.value_change_step != new_step:

openpilot/selfdrive/ui/sunnypilot/layouts/settings/vehicle/brands/toyota.py

@@ -11,10 +11,18 @@ from openpilot.system.ui.lib.multilang import tr, tr_noop
 from openpilot.system.ui.widgets import DialogResult
 from openpilot.system.ui.widgets.confirm_dialog import ConfirmDialog
 from openpilot.system.ui.sunnypilot.widgets.list_view import toggle_item_sp
+from opendbc.car.toyota.values import CAR, ToyotaFlags
+from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
 
 
 ONROAD_ONLY_DESCRIPTION = tr_noop("Start the vehicle to check vehicle compatibility.")
 SNG_HACK_UNAVAILABLE = tr_noop("sunnypilot Longitudinal Control must be available and enabled for your vehicle to use this feature.")
+BSM_UNAVAILABLE = tr_noop("Enhanced Blind Spot Monitoring requires a TSS2 Toyota/Lexus without SecOC.")
+BSM_STOCK_LONG_UNAVAILABLE = tr_noop("Enhanced Blind Spot Monitoring is not available while Enforce Factory Longitudinal Control is enabled.")
+BSM_REQUIRED = tr_noop("This vehicle does not report blind spot status over the standard CAN signals. " +
+                        "Enhanced Blind Spot Monitoring is required for blind spot detection.")
+ANGLE_STEERING_UNAVAILABLE = tr_noop("LTA Angle Steering is only available on the Toyota Prius/Prime 2021-22 (TSS2).")
+ANGLE_STEERING_HIGH_OUTPUT_UNAVAILABLE = tr_noop("LTA Angle Steering must be enabled to use high-output mode.")
 
 DESCRIPTIONS = {
   'enforce_stock_longitudinal': tr_noop(
@@ -23,6 +31,19 @@ DESCRIPTIONS = {
   'stop_and_go_hack': tr_noop(
     'sunnypilot will allow some Toyota/Lexus cars to auto resume during stop and go traffic. ' +
     'This feature is only applicable to certain models that are able to use longitudinal control. This is an alpha feature. Use at your own risk.'
+  ),
+  'enhanced_bsm': tr_noop(
+    'sunnypilot will poll the blind spot monitor diagnostics directly from the radar. Use this on vehicles whose blind spot monitors ' +
+    'do not report blind spot status over the standard CAN signals. This is an alpha feature. Use at your own risk.'
+  ),
+  'angle_steering': tr_noop(
+    'sunnypilot will command steering angle through Toyota Lane Tracing Assist (LTA) instead of steering torque. ' +
+    'This is an alpha feature. Use at your own risk.'
+  ),
+  'angle_steering_high_output': tr_noop(
+    'sunnypilot will use the wider angle envelope of an EPS patched for high-output mode, with up to 180 deg of target ' +
+    'angle, a 250 deg/s angle rate and a 3200 torque monitor. Requires patched EPS firmware, the stock EPS only accepts ' +
+    '94.9461 deg. This is an alpha feature. Use at your own risk.'
   )
 }
 
@@ -47,9 +68,36 @@ class ToyotaSettings(BrandSettings):
       enabled=lambda: not ui_state.engaged,
     )
 
+    self.enhanced_bsm = toggle_item_sp(
+      lambda: tr("Enhanced Blind Spot Monitoring (Alpha)"),
+      description=lambda: tr(DESCRIPTIONS["enhanced_bsm"]),
+      initial_state=ui_state.params.get_bool("ToyotaEnhancedBsm"),
+      callback=self._on_enable_enhanced_bsm,
+      enabled=lambda: not ui_state.engaged,
+    )
+
+    self.angle_steering = toggle_item_sp(
+      lambda: tr("LTA Angle Steering (Alpha)"),
+      description=lambda: tr(DESCRIPTIONS["angle_steering"]),
+      initial_state=ui_state.params.get_bool("ToyotaAngleSteering"),
+      callback=self._on_enable_angle_steering,
+      enabled=lambda: not ui_state.engaged,
+    )
+
+    self.angle_steering_high_output = toggle_item_sp(
+      lambda: tr("LTA Angle Steering High-Output (Alpha)"),
+      description=lambda: tr(DESCRIPTIONS["angle_steering_high_output"]),
+      initial_state=ui_state.params.get_bool("ToyotaAngleSteeringHighOutput"),
+      callback=self._on_enable_angle_steering_high_output,
+      enabled=lambda: not ui_state.engaged,
+    )
+
     self.items = [
       self.enforce_stock_longitudinal,
       self.stop_and_go_hack,
+      self.enhanced_bsm,
+      self.angle_steering,
+      self.angle_steering_high_output,
     ]
 
   def _on_enable_enforce_stock_longitudinal(self, state: bool):
@@ -94,6 +142,65 @@ class ToyotaSettings(BrandSettings):
       ui_state.params.put_bool("ToyotaStopAndGoHack", False)
       ui_state.params.put_bool("OnroadCycleRequested", True)
 
+  def _on_enable_enhanced_bsm(self, state: bool):
+    if state:
+      def confirm_callback(result: int):
+        if result == DialogResult.CONFIRM:
+          ui_state.params.put_bool("ToyotaEnhancedBsm", True)
+          ui_state.params.put_bool("OnroadCycleRequested", True)
+        else:
+          self.enhanced_bsm.action_item.set_state(False)
+
+      content = (f"<h1>{self.enhanced_bsm.title}</h1><br>" +
+                 f"<p>{self.enhanced_bsm.description}</p>")
+
+      dlg = ConfirmDialog(content, tr("Enable"), rich=True, callback=confirm_callback)
+      gui_app.push_widget(dlg)
+
+    else:
+      ui_state.params.put_bool("ToyotaEnhancedBsm", False)
+      ui_state.params.put_bool("OnroadCycleRequested", True)
+
+  def _on_enable_angle_steering(self, state: bool):
+    if state:
+      def confirm_callback(result: int):
+        if result == DialogResult.CONFIRM:
+          ui_state.params.put_bool("ToyotaAngleSteering", True)
+          ui_state.params.put_bool("OnroadCycleRequested", True)
+        else:
+          self.angle_steering.action_item.set_state(False)
+
+      content = (f"<h1>{self.angle_steering.title}</h1><br>" +
+                 f"<p>{self.angle_steering.description}</p>")
+
+      dlg = ConfirmDialog(content, tr("Enable"), rich=True, callback=confirm_callback)
+      gui_app.push_widget(dlg)
+
+    else:
+      ui_state.params.put_bool("ToyotaAngleSteering", False)
+      ui_state.params.put_bool("ToyotaAngleSteeringHighOutput", False)
+      self.angle_steering_high_output.action_item.set_state(False)
+      ui_state.params.put_bool("OnroadCycleRequested", True)
+
+  def _on_enable_angle_steering_high_output(self, state: bool):
+    if state:
+      def confirm_callback(result: int):
+        if result == DialogResult.CONFIRM:
+          ui_state.params.put_bool("ToyotaAngleSteeringHighOutput", True)
+          ui_state.params.put_bool("OnroadCycleRequested", True)
+        else:
+          self.angle_steering_high_output.action_item.set_state(False)
+
+      content = (f"<h1>{self.angle_steering_high_output.title}</h1><br>" +
+                 f"<p>{self.angle_steering_high_output.description}</p>")
+
+      dlg = ConfirmDialog(content, tr("Enable"), rich=True, callback=confirm_callback)
+      gui_app.push_widget(dlg)
+
+    else:
+      ui_state.params.put_bool("ToyotaAngleSteeringHighOutput", False)
+      ui_state.params.put_bool("OnroadCycleRequested", True)
+
   def update_settings(self):
     if ui_state.CP is not None:
       longitudinal = ui_state.CP.openpilotLongitudinalControl
@@ -113,9 +220,70 @@ class ToyotaSettings(BrandSettings):
         self.stop_and_go_hack.set_description(new_desc)
         if show_desc:
           self.stop_and_go_hack.show_description(True)
+
+      # Enhanced BSM needs the radar diagnostic address, which is only in the TX allowlist for
+      # TSS2 cars without SecOC that are not enforcing factory longitudinal control
+      bsm_supported = bool(ui_state.CP.flags & ToyotaFlags.TSS2) and not bool(ui_state.CP.flags & ToyotaFlags.SECOC)
+      bsm_available = bsm_supported and not enforce_stock
+      self.enhanced_bsm.action_item.set_enabled(bsm_available and not ui_state.engaged)
+
+      bsm_desc = tr(DESCRIPTIONS["enhanced_bsm"])
+      if not bsm_supported:
+        self.enhanced_bsm.action_item.set_state(False)
+        bsm_desc = "<b>" + tr(BSM_UNAVAILABLE) + "</b>\n\n" + bsm_desc
+      elif enforce_stock:
+        self.enhanced_bsm.action_item.set_state(False)
+        bsm_desc = "<b>" + tr(BSM_STOCK_LONG_UNAVAILABLE) + "</b>\n\n" + bsm_desc
+      elif ui_state.CP_SP is not None and ui_state.CP_SP.flags & ToyotaFlagsSP.SP_NEED_DEBUG_BSM:
+        bsm_desc = "<b>" + tr(BSM_REQUIRED) + "</b>\n\n" + bsm_desc
+
+      if self.enhanced_bsm.description != bsm_desc:
+        self.enhanced_bsm.set_description(bsm_desc)
+        if not bsm_available:
+          self.enhanced_bsm.show_description(True)
+
+      # LTA angle steering is only wired up for the Prius TSS2 platform
+      angle_steering_supported = ui_state.CP.carFingerprint == CAR.TOYOTA_PRIUS_TSS2
+      self.angle_steering.action_item.set_enabled(angle_steering_supported and not ui_state.engaged)
+
+      angle_desc = tr(DESCRIPTIONS["angle_steering"])
+      if not angle_steering_supported:
+        self.angle_steering.action_item.set_state(False)
+        angle_desc = "<b>" + tr(ANGLE_STEERING_UNAVAILABLE) + "</b>\n\n" + angle_desc
+
+      if self.angle_steering.description != angle_desc:
+        self.angle_steering.set_description(angle_desc)
+        if not angle_steering_supported:
+          self.angle_steering.show_description(True)
+
+      # high-output mode is only meaningful on top of LTA angle steering
+      high_output_available = angle_steering_supported and self.angle_steering.action_item.get_state()
+      self.angle_steering_high_output.action_item.set_enabled(high_output_available and not ui_state.engaged)
+
+      high_output_desc = tr(DESCRIPTIONS["angle_steering_high_output"])
+      if not high_output_available:
+        self.angle_steering_high_output.action_item.set_state(False)
+        high_output_desc = "<b>" + tr(ANGLE_STEERING_HIGH_OUTPUT_UNAVAILABLE) + "</b>\n\n" + high_output_desc
+
+      if self.angle_steering_high_output.description != high_output_desc:
+        self.angle_steering_high_output.set_description(high_output_desc)
+        if not high_output_available:
+          self.angle_steering_high_output.show_description(True)
     else:
       self.stop_and_go_hack.action_item.set_enabled(False)
       new_desc = "<b>" + tr(ONROAD_ONLY_DESCRIPTION) + "</b>\n\n" + tr(DESCRIPTIONS["stop_and_go_hack"])
       if self.stop_and_go_hack.description != new_desc:
         self.stop_and_go_hack.set_description(new_desc)
         self.stop_and_go_hack.show_description(True)
+
+      self.enhanced_bsm.action_item.set_enabled(False)
+      bsm_desc = "<b>" + tr(ONROAD_ONLY_DESCRIPTION) + "</b>\n\n" + tr(DESCRIPTIONS["enhanced_bsm"])
+      if self.enhanced_bsm.description != bsm_desc:
+        self.enhanced_bsm.set_description(bsm_desc)
+        self.enhanced_bsm.show_description(True)
+
+      self.angle_steering_high_output.action_item.set_enabled(False)
+      high_output_desc = "<b>" + tr(ONROAD_ONLY_DESCRIPTION) + "</b>\n\n" + tr(DESCRIPTIONS["angle_steering_high_output"])
+      if self.angle_steering_high_output.description != high_output_desc:
+        self.angle_steering_high_output.set_description(high_output_desc)
+        self.angle_steering_high_output.show_description(True)

openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py

@@ -11,7 +11,7 @@ import pyray as rl
 from openpilot.cereal import custom
 from openpilot.selfdrive.ui.mici.widgets.dialog import BigConfirmationDialog, BigDialog
 from openpilot.sunnypilot.models.helpers import ACTIVE_BUNDLE_KEYS, get_selected_bundle
-from openpilot.selfdrive.ui.mici.widgets.button import BigButton
+from openpilot.selfdrive.ui.mici.widgets.button import BigButton, BigParamControl
 from openpilot.selfdrive.ui.ui_state import ui_state, device
 from openpilot.selfdrive.ui.sunnypilot.model_info import (active_source, big_model_state, bundles_for_source, carrying_model,
                                                            default_model_name, model_cache_size_mb, model_info, queued_name,
@@ -95,7 +95,12 @@ class ModelsLayoutMici(NavScroller):
     self.clear_cache_btn.set_click_callback(self._confirm_clear_cache)
     self._cache_size_time = 0.0
 
-    self.main_items = [self.current_model_info, self.select_model_btn, self.cancel_download_btn, self.refresh_btn, self.clear_cache_btn]
+    self.blendful_toggle = BigParamControl("blendful (alpha)", "Blendful",
+                                           description="Gently pulls the model toward the lane center, as much as it trusts the lane lines, " +
+                                                       "and yields around obstacles.")
+
+    self.main_items = [self.current_model_info, self.select_model_btn, self.cancel_download_btn, self.refresh_btn, self.clear_cache_btn,
+                       self.blendful_toggle]
     self._scroller.add_widgets(self.main_items)
 
   @property
@@ -205,6 +210,10 @@ class ModelsLayoutMici(NavScroller):
       device.set_override_interactive_timeout(None)
       self._was_downloading = False
 
+  def show_event(self):
+    super().show_event()
+    self.blendful_toggle.refresh()
+
   def _update_state(self):
     super()._update_state()
 
@@ -213,6 +222,14 @@ class ModelsLayoutMici(NavScroller):
     self.current_model_info.current_model_header._shimmer = False
     self.current_model_info.info_header._shimmer = False
 
+    blendful_allowed = ui_state.active_bundle is not None
+    if blendful_allowed:
+      self.blendful_toggle.set_checked(ui_state.params.get_bool("Blendful"))
+      self.blendful_toggle.set_enabled(True)
+    else:
+      self.blendful_toggle.set_checked(False)
+      self.blendful_toggle.set_enabled(False)
+
     manager = self.model_manager
     self._download_frame += 1
     should_update = self._download_frame % (gui_app.target_fps / 2) == 0
@@ -257,6 +274,7 @@ class ModelsLayoutMici(NavScroller):
       count = 0
       verifying = False
       canceling = False
+      speed = 0.0
       for model in manager.selectedBundle.models:
         count += 1
         p = model.artifact.downloadProgress
@@ -265,6 +283,7 @@ class ModelsLayoutMici(NavScroller):
           progress += p.progress
           verifying = verifying or p.status == custom.ModelManagerSP.DownloadStatus.verifying
           canceling = canceling or p.canceling
+          speed = max(speed, p.speed)  # only one file downloads at a time, so the non-zero speed is the live one
         elif p.status in (custom.ModelManagerSP.DownloadStatus.downloaded,
                           custom.ModelManagerSP.DownloadStatus.cached):
           progress += 100.0
@@ -279,7 +298,10 @@ class ModelsLayoutMici(NavScroller):
       self.current_model_info.current_model_text.set_text(name_text)
       self.current_model_info.info_header.set_text(tr("progress") + self._download_progress)
       self.current_model_info.info_header._shimmer = True
-      self.current_model_info.info_text.set_text(f"{progress/count:.2f}%")
+      progress_text = f"{progress/count:.2f}%"
+      if not verifying and not canceling:
+        progress_text += f"  |  {speed / 1024 ** 2:.1f} MB/s" if speed > 0 else "  |  --.- MB/s"
+      self.current_model_info.info_text.set_text(progress_text)
 
     elif manager.selectedBundle and manager.selectedBundle.status == custom.ModelManagerSP.DownloadStatus.downloaded:
       self.current_model_info.info_header.set_text(tr("downloaded"))

openpilot/selfdrive/ui/sunnypilot/mici/onroad/accel_boost_indicator.py

@@ -0,0 +1,122 @@
+"""
+Copyright (c) 2021-, 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.cereal import log
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.selfdrive.ui.mici.onroad import SIDE_PANEL_WIDTH
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.system.ui.lib.application import gui_app
+from openpilot.system.ui.widgets import Widget
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost_ext import get_accel_boost_max
+
+# The readout is wider than the side panel, so the value is right-aligned and bleeds over the camera view
+BAR_WIDTH = 24
+# Fades to white only near the profile cap, like the stock torque bar fading toward orange at high torque
+CAP_BLEND_START = 0.75
+MAX_BOOST_BLEND = 0.45
+
+COLOR_TRACK = rl.Color(255, 255, 255, 45)
+COLOR_TRACK_EDGE = rl.Color(255, 255, 255, 70)
+
+AGGRESSIVE_COLOR = rl.Color(255, 80, 60, 255)
+STANDARD_COLOR = rl.Color(0, 230, 120, 255)
+RELAXED_COLOR = rl.Color(70, 150, 255, 255)
+
+FADE_THRESHOLD = 0.01
+
+
+def lighten(color: rl.Color, amount: float) -> rl.Color:
+  """Lerp towards white in RGB, so the hue stays put the way HSV blending would not."""
+  amount = min(max(amount, 0.0), 1.0)
+  return rl.Color(int(color.r + (255 - color.r) * amount),
+                  int(color.g + (255 - color.g) * amount),
+                  int(color.b + (255 - color.b) * amount), color.a)
+
+
+def get_boost_fraction(boost: float, max_boost: float) -> float:
+  """How much of the active profile's boost budget is in use, clamped to a full bar."""
+  if max_boost <= 0.0:
+    return 0.0
+  return min(max(boost / max_boost, 0.0), 1.0)
+
+
+def get_boost_style(personality) -> tuple[rl.Color, float]:
+  if personality == log.LongitudinalPersonality.aggressive:
+    color = AGGRESSIVE_COLOR
+  elif personality == log.LongitudinalPersonality.relaxed:
+    color = RELAXED_COLOR
+  else:
+    color = STANDARD_COLOR
+
+  return color, get_accel_boost_max(personality)
+
+
+class AccelBoostIndicator(Widget):
+  def __init__(self):
+    super().__init__()
+    self._boost_filter = FirstOrderFilter(0.0, 0.15, 1 / gui_app.target_fps)
+    self._alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
+    # Smoothly rescale the bar when the follow distance profile changes mid-drive
+    self._max_boost_filter = FirstOrderFilter(0.0, 0.25, 1 / gui_app.target_fps)
+    self._profile_color = STANDARD_COLOR
+    self._max_boost = 0.0
+
+    self.panel_in_use = False
+
+  def _update_state(self):
+    sm = ui_state.sm
+    extra_enabled = ui_state.follow_distance_accel_boost
+    visible = (ui_state.has_longitudinal_control and sm.alive['selfdriveState'] and sm.alive['longitudinalPlan'] and
+               sm['selfdriveState'].enabled)
+    self._alpha_filter.update(1.0 if visible and extra_enabled else 0.0)
+    self._boost_filter.update(float(sm['longitudinalPlan'].accelBoost) if visible else 0.0)
+
+    if sm.alive['selfdriveState']:
+      self._profile_color, self._max_boost = get_boost_style(sm['selfdriveState'].personality)
+    self._max_boost_filter.update(self._max_boost)
+
+    # The bar owns the panel only while it is actually drawn; the ball returns when the bar fades out
+    self.panel_in_use = visible and extra_enabled
+
+  def _render(self, _):
+    alpha = self._alpha_filter.x
+    if alpha < FADE_THRESHOLD or self._max_boost <= 0.0:
+      return
+
+    cx = self.rect.x + self.rect.width - SIDE_PANEL_WIDTH / 2
+    bar_x = cx - BAR_WIDTH / 2
+    track_top = self.rect.y + self.rect.height * 0.08
+    track_bottom = self.rect.y + self.rect.height * 0.92
+    track_h = max(1.0, track_bottom - track_top)
+
+    # track, light enough to read on the black side panel
+    rl.draw_rectangle_rounded(rl.Rectangle(bar_x, track_top, BAR_WIDTH, track_h), 0.5, 12,
+                              rl.Color(COLOR_TRACK.r, COLOR_TRACK.g, COLOR_TRACK.b, int(COLOR_TRACK.a * alpha)))
+    rl.draw_rectangle_rounded_lines_ex(rl.Rectangle(bar_x, track_top, BAR_WIDTH, track_h), 0.5, 12, 1,
+                                       rl.Color(COLOR_TRACK_EDGE.r, COLOR_TRACK_EDGE.g, COLOR_TRACK_EDGE.b,
+                                                int(COLOR_TRACK_EDGE.a * alpha)))
+
+    # Scale to the current profile's cap, so a maxed boost fills the bar and a profile change rescales it
+    boost = max(self._boost_filter.x, 0.0)
+    ratio = get_boost_fraction(boost, self._max_boost_filter.x)
+    fill_h = ratio * track_h
+    cap_blend = max(0.0, (ratio - CAP_BLEND_START) / (1.0 - CAP_BLEND_START))
+    fill_color = lighten(self._profile_color, MAX_BOOST_BLEND * cap_blend)
+    if fill_h > 0.0:
+      # soft glow around the tip, like the confidence ball's gradient
+      tip_y = track_bottom - fill_h
+      glow = rl.Color(fill_color.r, fill_color.g, fill_color.b, int(70 * alpha))
+      rl.draw_circle_gradient(rl.Vector2(cx, tip_y), int(BAR_WIDTH * 0.9), glow, rl.BLANK)
+
+      fill = rl.Color(fill_color.r, fill_color.g, fill_color.b, int(255 * alpha))
+      if fill_h < BAR_WIDTH:
+        # Keep the cap inside the track: a fixed radius would paint below the track near zero boost
+        radius = max(1, int(fill_h / 2))
+        rl.draw_circle(int(cx), int(track_bottom - radius), radius, fill)
+      else:
+        rl.draw_rectangle_rounded(rl.Rectangle(bar_x, track_bottom - fill_h, BAR_WIDTH, fill_h), 0.5, 12, fill)

openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py

@@ -6,6 +6,7 @@ See the LICENSE.md file in the root directory for more details.
 """
 import pyray as rl
 from openpilot.selfdrive.ui.ui_state import UIStatus
+from openpilot.selfdrive.ui.sunnypilot.onroad.blendful import BlendfulRenderer
 from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath
 
 LANE_LINE_COLORS_SP = {
@@ -17,3 +18,4 @@ LANE_LINE_COLORS_SP = {
 class ModelRendererSP:
   def __init__(self):
     self.rainbow_path = RainbowPath()
+    self.blendful = BlendfulRenderer()

openpilot/selfdrive/ui/sunnypilot/mici/tests/test_accel_boost_indicator.py

@@ -0,0 +1,59 @@
+"""
+Copyright (c) 2021-, 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 unittest
+
+from openpilot.cereal import log
+from openpilot.selfdrive.controls.lib.accel_boost import ACCEL_BOOST_MAX
+from openpilot.selfdrive.ui.sunnypilot.mici.onroad.accel_boost_indicator import (get_boost_style, get_boost_fraction, lighten,
+                                                                                AGGRESSIVE_COLOR, RELAXED_COLOR, STANDARD_COLOR)
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost_ext import (SCALE_AGGRESSIVE, SCALE_RELAXED, SCALE_STANDARD,
+                                                                        get_accel_boost_max)
+
+
+class TestAccelBoostIndicator(unittest.TestCase):
+  def test_bar_scales_with_following_distance(self):
+    for personality, color, scale in (
+        (log.LongitudinalPersonality.relaxed, RELAXED_COLOR, SCALE_RELAXED),
+        (log.LongitudinalPersonality.standard, STANDARD_COLOR, SCALE_STANDARD),
+        (log.LongitudinalPersonality.aggressive, AGGRESSIVE_COLOR, SCALE_AGGRESSIVE)):
+      bar_color, full_scale = get_boost_style(personality)
+      self.assertEqual(bar_color, color)
+      self.assertAlmostEqual(full_scale, ACCEL_BOOST_MAX * (1.0 + scale))
+
+  def test_bar_fills_at_the_active_profile_cap(self):
+    caps = [get_accel_boost_max(p) for p in (log.LongitudinalPersonality.relaxed,
+                                             log.LongitudinalPersonality.standard,
+                                             log.LongitudinalPersonality.aggressive)]
+    self.assertEqual(caps, sorted(caps))
+
+    for cap in caps:
+      self.assertAlmostEqual(get_boost_fraction(cap, cap), 1.0)
+      self.assertAlmostEqual(get_boost_fraction(0.0, cap), 0.0)
+      self.assertAlmostEqual(get_boost_fraction(cap * 0.5, cap), 0.5)
+
+    # A boost beyond the current cap (e.g. after switching to a shorter profile) still fills the bar
+    self.assertAlmostEqual(get_boost_fraction(caps[2], caps[0]), 1.0)
+    self.assertAlmostEqual(get_boost_fraction(0.0, 0.0), 0.0)
+
+  def test_cap_lighten_keeps_the_profile_hue(self):
+    # An HSV blend towards white sweeps relaxed blue through pink, so the fill lightens in RGB instead
+    relaxed = lighten(RELAXED_COLOR, 0.45)
+    aggressive = lighten(AGGRESSIVE_COLOR, 0.45)
+    self.assertGreater(relaxed.b, relaxed.r)
+    self.assertGreaterEqual(relaxed.b, relaxed.g)
+    self.assertGreater(aggressive.r, aggressive.b)
+    # lighter in RGB means the dimmest channel came up towards white
+    self.assertGreater(relaxed.r, RELAXED_COLOR.r)
+    self.assertGreater(aggressive.g, AGGRESSIVE_COLOR.g)
+
+  def test_wire_personality_picks_the_right_profile(self):
+    # Over the wire personality is a capnp DynamicEnum, which compares equal to the cereal enum but hashes differently
+    msg = log.SelfdriveState.new_message()
+    msg.personality = log.LongitudinalPersonality.aggressive
+    color, full_scale = get_boost_style(msg.personality)
+    self.assertEqual(color, AGGRESSIVE_COLOR)
+    self.assertAlmostEqual(full_scale, get_accel_boost_max(log.LongitudinalPersonality.aggressive))

openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py

@@ -0,0 +1,90 @@
+"""
+Copyright (c) 2021-, 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 pyray as rl
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.sunnypilot.modeld_v2.blendful import LANE_WIDTH_CLIP, LOOKAHEAD_TIME, MIN_LOOKAHEAD
+from openpilot.system.ui.lib.application import gui_app, FontWeight
+from openpilot.system.ui.lib.shader_polygon import draw_polygon
+from openpilot.system.ui.lib.text_measure import measure_text_cached
+
+BLENDFUL_COLOR = (0, 200, 255)
+FULL_BAR = 1.0
+
+
+class BlendfulRenderer:
+  def __init__(self, readout: bool = True):
+    self._readout = readout
+    self._center_points = np.empty((0, 2), dtype=np.float32)
+    self._trust = 0.0
+    self._font = gui_app.font(FontWeight.BOLD)
+    self._pull_display = FirstOrderFilter(0.0, 0.5, DT_MDL)
+    self._trust_display = FirstOrderFilter(0.0, 0.5, DT_MDL)
+    self._alpha = FirstOrderFilter(0.0, 0.3, DT_MDL)
+
+  def update(self, renderer, max_idx: int, *map_args) -> None:
+    blendful = ui_state.sm['modelDataV2SP']
+    if ui_state.sm.updated['modelDataV2SP']:
+      self._alpha.update(float(blendful.blendfulTrust > 0.0))
+      if blendful.blendfulTrust > 0.0:
+        self._trust = blendful.blendfulTrust
+      if self._readout:
+        lookahead = max(ui_state.sm['carState'].vEgo * LOOKAHEAD_TIME, MIN_LOOKAHEAD)
+        self._pull_display.update(blendful.blendfulCurvature * lookahead ** 2 / 2)
+        self._trust_display.update(self._trust)
+
+    left, right = renderer._lane_lines[1].raw_points, renderer._lane_lines[2].raw_points
+    stds = list(ui_state.sm['modelV2'].laneLineStds)
+    if self._alpha.x < 0.01 or len(left) == 0 or len(right) != len(left) or len(stds) < 3:
+      self._center_points = np.empty((0, 2), dtype=np.float32)
+      return
+
+    l_prob, r_prob = blendful.blendfulLeftProb, blendful.blendfulRightProb
+    if blendful.blendfulTrust > 0.0 and l_prob + r_prob > 0.0:
+      half_width = np.clip(right[:, 1] - left[:, 1], *LANE_WIDTH_CLIP) / 2 if l_prob * r_prob > 0.5 else blendful.blendfulLaneWidth / 2
+      l_var = stds[1] ** 2 / max(l_prob, 1e-4)
+      r_var = stds[2] ** 2 / max(r_prob, 1e-4)
+      center = left.copy()
+      center[:, 1] = (r_var * (left[:, 1] + half_width) + l_var * (right[:, 1] - half_width)) / (l_var + r_var)
+      self._center_points = renderer._map_line_to_polygon(center, 0.15, 0.0, max_idx, *map_args)
+
+  def _color(self, rgb, alpha: float) -> rl.Color:
+    return rl.Color(*rgb, int(alpha * self._alpha.x))
+
+  def draw(self, rect: rl.Rectangle) -> None:
+    if self._center_points.size and self._trust > 0.05:
+      draw_polygon(rect, self._center_points, self._color(BLENDFUL_COLOR, np.clip(self._trust, 0.2, 1.0) * 255))
+    if self._center_points.size and self._readout:
+      self._draw_readout(rect)
+
+  def _draw_readout(self, rect: rl.Rectangle) -> None:
+    pull, trust = self._pull_display.x, self._trust_display.x
+    w, h = 190, 76
+    x, y = rect.x + (rect.width - w) / 2, rect.y + 10
+    cx, cy = x + w / 2, y + 30
+    rl.draw_rectangle_rounded(rl.Rectangle(x, y, w, h), 0.5, 12, self._color((0, 0, 0), 170))
+
+    text = f"{abs(pull) * 100:.0f}"
+    text_w = measure_text_cached(self._font, text, 44, 0).x
+    rl.draw_text_ex(self._font, text, rl.Vector2(cx - text_w / 2, cy - 24), 44, 0, self._color((255, 255, 255), 255))
+
+    active = self._color(BLENDFUL_COLOR, np.clip(abs(pull) / 0.1, 0.0, 1.0) * 255)
+    idle = self._color((255, 255, 255), 40)
+    lx, rx = x + 18, x + w - 18
+    rl.draw_triangle(rl.Vector2(lx, cy), rl.Vector2(lx + 16, cy + 12), rl.Vector2(lx + 16, cy - 12), active if pull < 0 else idle)
+    rl.draw_triangle(rl.Vector2(rx - 16, cy - 12), rl.Vector2(rx - 16, cy + 12), rl.Vector2(rx, cy), active if pull > 0 else idle)
+
+    bar = rl.Rectangle(x + 24, y + h - 16, w - 48, 6)
+    rl.draw_rectangle_rounded(bar, 1.0, 6, self._color((255, 255, 255), 40))
+    rl.draw_rectangle(int(cx - 1), int(bar.y - 2), 2, int(bar.height + 4), self._color((255, 255, 255), 120))
+    fill = bar.width / 2 * float(np.sqrt(np.clip(abs(pull) / FULL_BAR, 0.0, 1.0)))
+    if fill > 1:
+      fill_x = cx if pull > 0 else cx - fill
+      rl.draw_rectangle_rounded(rl.Rectangle(fill_x, bar.y, fill, bar.height), 1.0, 6,
+                                self._color(BLENDFUL_COLOR, np.clip(0.3 + 0.7 * trust, 0.0, 1.0) * 255))

openpilot/selfdrive/ui/sunnypilot/onroad/model_renderer.py

@@ -6,6 +6,7 @@ See the LICENSE.md file in the root directory for more details.
 """
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
+from openpilot.selfdrive.ui.sunnypilot.onroad.blendful import BlendfulRenderer
 from openpilot.selfdrive.ui.sunnypilot.onroad.chevron_metrics import ChevronMetrics
 from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath
 from openpilot.selfdrive.ui.sunnypilot.ui_state import MADSState
@@ -16,6 +17,7 @@ class ModelRendererSP:
   def __init__(self):
     self.rainbow_path = RainbowPath()
     self.chevron_metrics = ChevronMetrics()
+    self.blendful = BlendfulRenderer(readout=False)
     self._width_filter = FirstOrderFilter(0.9, 0.1, 1 / gui_app.target_fps)
 
   @property

openpilot/selfdrive/ui/sunnypilot/ui_state.py

@@ -36,7 +36,7 @@ class UIStateSP:
     self.is_sp_release: bool = self.params.get_bool("IsReleaseSpBranch")
     self.sm_services_ext = [
       "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
-      "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay"
+      "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay", "modelDataV2SP"
     ]
 
     self.sunnylink_state = SunnylinkState()
@@ -64,6 +64,7 @@ class UIStateSP:
     self.enforce_torque_control: bool = False
     self.custom_torque_params: bool = False
     self.torque_override_enabled: bool = False
+    self.follow_distance_accel_boost: bool = False
     self._sp_initialized: bool = False
 
   def update(self) -> None:
@@ -177,6 +178,7 @@ class UIStateSP:
     self.enforce_torque_control = self.params.get_bool("EnforceTorqueControl")
     self.custom_torque_params = self.params.get_bool("CustomTorqueParams")
     self.torque_override_enabled = self.params.get_bool("TorqueParamsOverrideEnabled")
+    self.follow_distance_accel_boost = self.params.get_bool("FollowDistanceAccelBoost")
     self.torque_override_lat_accel_factor = float(self.params.get("TorqueParamsOverrideLatAccelFactor", return_default=True))
     self.torque_override_friction = float(self.params.get("TorqueParamsOverrideFriction", return_default=True))
     self.true_v_ego_ui = self.params.get_bool("TrueVEgoUI")
@@ -231,6 +233,10 @@ class UIStateSP:
       self.params.remove("IntelligentCruiseButtonManagement")
       self.has_icbm = False
 
+    # Extra boost needs sunnypilot longitudinal control; ICBM alone is not enough
+    if not has_long:
+      self.params.remove("FollowDistanceAccelBoost")
+
     # Cruise features requiring longitudinal or ICBM
     if not (has_long or self.has_icbm):
       self.params.remove("CustomAccIncrementsEnabled")

openpilot/sunnypilot/modeld_v2/blendful.py

@@ -0,0 +1,145 @@
+"""
+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.filter_simple import FirstOrderFilter
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
+
+LANE_WIDTH_DEFAULT = 3.6  # m
+LANE_WIDTH_CLIP = (2.8, 4.5)  # m
+LOOKAHEAD_TIME = 1.5  # s
+MIN_LOOKAHEAD = 10.0  # m
+MIN_SPEED = 5.0  # m/s
+LINE_STD = (0.2, 0.6)
+POINT_STD = 0.1
+YIELD = (0.2, 0.45)
+X_IDXS = np.array(ModelConstants.X_IDXS)
+
+
+class Blendful:
+  def __init__(self):
+    self.lane_width = FirstOrderFilter(LANE_WIDTH_DEFAULT, 10.0, DT_MDL, initialized=False)
+    self.line_gone = FirstOrderFilter(np.zeros(2), 2.0, DT_MDL)
+    self.line_probs_slow = FirstOrderFilter(np.zeros(2), 3.0, DT_MDL)
+    self.line_probs = FirstOrderFilter(np.zeros(2), 0.5, DT_MDL)
+    self.usual_offset = FirstOrderFilter(0.0, 10.0, DT_MDL, initialized=False)
+    self.going_around = False
+    self.since_turning = 0.0
+    self.trust = 0.0
+    self.line_trust = 0.0
+    self.dk = 0.0 # delta kappa; difference between lane center and model plan
+    self.l_prob = 0.0
+    self.r_prob = 0.0
+    self.model_curvature = 0.0
+    self.smooth = 0.0 # smoothens lanelines below .53 lat accel
+    self.curvature = 0.0
+
+  def update(self, model_output: dict[str, np.ndarray], v_ego: float, active: bool, model_curvature: float, enabled: bool = True) -> float:
+    self.l_prob = self.r_prob = 0.0
+    self.model_curvature = model_curvature
+    if not enabled:
+      if self.curvature or self.trust:
+        self.__init__()
+      return 0.0
+    fading = not active or v_ego < MIN_SPEED
+    if fading:
+      self.trust = self.line_trust = 0.0
+      self.usual_offset.initialized = False
+      self.lane_width.initialized = False
+      self.going_around = False
+    target = 0.0 if fading else self._target(model_output, v_ego, model_curvature)
+    target = self.smooth = self.smooth + (target - self.smooth) * DT_MDL / (DT_MDL + np.interp(abs(model_curvature) * v_ego ** 2, [0.18, 0.53], [0.5, 0.0]))
+    build = 0.5 / max(v_ego, 1.0) ** 2 * DT_MDL  # jerk over 1 second
+    release = (0.5 if fading else 2.0) / max(v_ego, 1.0) ** 2 * DT_MDL  # lets go fast, but past zero it builds like any other pull
+    k = self.curvature
+    self.curvature = float(np.clip(target, max(k - release, -build) if k > 0 else k - build, min(k + release, build) if k < 0 else k + build))
+    return self.curvature
+
+  def _target(self, model_output: dict[str, np.ndarray], v_ego: float, model_curvature: float) -> float:
+    lookahead = max(v_ego * LOOKAHEAD_TIME, MIN_LOOKAHEAD)
+    lines = model_output['lane_lines'][0, :, :, 0]
+    stds = model_output['lane_lines_stds'][0, :, :, 0]
+
+    lone_weight = np.interp(abs(model_curvature) * v_ego ** 2, [0.18, 0.53], [0.5, 1.0])  # 0.5 on straights, 1.0 in curves
+    probs = self.line_probs.update(model_output['lane_lines_prob'][0, 1::2][1:3])
+    absent = (probs < 0.3) & (lone_weight > 0.5)
+    gone = self.line_gone.update((model_output['lane_lines_prob'][0, 1::2][1:3] < 0.3).astype(float))
+    missing = np.where(absent, 1.0, gone)
+    dropping = bool(np.any((probs < 0.6 * self.line_probs_slow.update(model_output['lane_lines_prob'][0, 1::2][1:3])) & ~absent))
+    l_prob = float(probs[0] * np.interp(np.interp(0.5 * lookahead, X_IDXS, stds[1]), LINE_STD, [1.0, 0.0]))
+    r_prob = float(probs[1] * np.interp(np.interp(0.5 * lookahead, X_IDXS, stds[2]), LINE_STD, [1.0, 0.0]))
+    self.l_prob, self.r_prob = l_prob, r_prob
+    # one line steadily on its own still centers at lone_weight. a line that is disappearing gives control back to laneless
+    if missing[1] > 0.8 and l_prob > 0.5:
+      lane_trust = l_prob * lone_weight
+    elif missing[0] > 0.8 and r_prob > 0.5:
+      lane_trust = r_prob * lone_weight
+    elif dropping:
+      lane_trust = min(l_prob, r_prob)
+    else:
+      lane_trust = l_prob + r_prob - l_prob * r_prob
+    if lone_weight > 0.5 and lane_trust < self.line_trust:
+      lane_trust = self.line_trust + (lane_trust - self.line_trust) * DT_MDL / (lone_weight - 0.5 + DT_MDL)
+    self.line_trust = lane_trust
+    if lane_trust <= 0.0:
+      self.trust = 0.0
+      return 0.0
+    if l_prob + r_prob <= 0.0:  # keep the last fit while trust lets go
+      self.trust = min(self.trust, lane_trust)
+      return float(self.trust * self.dk)
+
+    win = (X_IDXS >= 0.5 * lookahead) & (X_IDXS <= 1.5 * lookahead)
+    x = X_IDXS[win]
+    half_width = self.lane_width.x / 2
+    if l_prob * r_prob > 0.5:
+      width = np.interp(lookahead, X_IDXS, lines[2]) - np.interp(lookahead, X_IDXS, lines[1])
+      self.lane_width.update(float(np.clip(width, *LANE_WIDTH_CLIP)))
+      half_width = np.clip(lines[2, win] - lines[1, win], *LANE_WIDTH_CLIP) / 2
+    l_var = stds[1, win] ** 2 / max(l_prob, 1e-4)
+    r_var = stds[2, win] ** 2 / max(r_prob, 1e-4)
+    center = (r_var * (lines[1, win] + half_width) + l_var * (lines[2, win] - half_width)) / (l_var + r_var)
+    weight = 1.0 / (l_var * r_var / (l_var + r_var) + POINT_STD ** 2)
+
+    # Fit curvature to place the plant in center: e ~ dk x^2 / 2
+    plan_x = np.interp(x, model_output['plan'][0, :, 0], model_output['plan'][0, :, 1])
+    plan_share = np.interp(abs(model_curvature) * lookahead ** 2 / 2, [0.5, 2.0], [0.3, 0.5]) # blending up to 50% plan with action during sharp curvature
+    error = np.clip(center - ((1 - plan_share) * model_curvature * x ** 2 / 2 + plan_share * plan_x), -1.0, 1.0)
+    dk = 2.0 * np.sum(weight * error * x ** 2) / np.sum(weight * x ** 4)
+
+    # A model planning to end up further from its usual spot in the lane than the plant is now is likely going around something
+    hw_now = np.clip(lines[2, 0] - lines[1, 0], *LANE_WIDTH_CLIP) / 2 if l_prob * r_prob > 0.5 else self.lane_width.x / 2
+    center_now = (l_prob * (lines[1, 0] + hw_now) + r_prob * (lines[2, 0] - hw_now)) / (l_prob + r_prob)
+    plan_y = np.interp(lookahead, model_output['plan'][0, :, 0], model_output['plan'][0, :, 1])
+    offset = plan_y - np.interp(lookahead, x, center)
+    if min(gone) > 0.5:  # both lines gone: turns, intersections, relearn ideal model position
+      self.usual_offset.initialized = False
+    usual = self.usual_offset.x if self.usual_offset.initialized else offset
+    swerve = abs(offset - usual) - 0.25 * abs(model_curvature) * lookahead ** 2 / 2
+    away = swerve - abs(-center_now - usual)
+    lane_curvature = 2.0 * np.polyfit(x, center, 2)[0]
+    inside_curve = abs(lane_curvature) * lookahead ** 2 / 2 > 0.1 and (offset - usual) * lane_curvature > 0
+    self.since_turning = 0.0 if abs(model_curvature) * lookahead ** 2 / 2 > 0.5 else self.since_turning + DT_MDL
+    if inside_curve:
+      self.going_around = False
+    elif away > YIELD[0] and self.since_turning > 2.0:
+      self.going_around = True
+    elif swerve < YIELD[0] / 2:
+      self.going_around = False
+    self.usual_offset.update_alpha(20.0 if self.going_around else 10.0)
+    self.usual_offset.update(offset)
+
+    if inside_curve:
+      lane_trust *= np.interp(abs(offset) - abs(center_now) - 0.25 * abs(model_curvature) * lookahead ** 2 / 2, (0.5, 1.0), [1.0, 0.0])
+    else:
+      lane_trust *= np.interp(swerve, (YIELD[0] / 2, YIELD[0]), [1.0, 0.0]) if self.going_around else np.interp(away, YIELD, [1.0, 0.0])
+
+    if lane_trust < self.trust:
+      dk = float(np.clip(dk, min(self.dk, 0.0), max(self.dk, 0.0)))
+    self.trust = lane_trust if lane_trust < self.trust else self.trust + (lane_trust - self.trust) * DT_MDL / (1.5 + DT_MDL)
+    self.dk = dk
+    return float(self.trust * dk)

openpilot/sunnypilot/modeld_v2/convert_upstream_pkl.py

@@ -7,6 +7,7 @@ See the LICENSE.md file in the root directory for more details.
 """
 import argparse
 import dataclasses
+import inspect
 import itertools
 import mmap
 import re
@@ -45,6 +46,11 @@ def _address(v):
   return _root(v.src[0]), add + start, size
 
 
+def _buffer(u):
+  from tinygrad.uop.ops import Ops
+  return u.buffer if u.op is Ops.BUFFER else getattr(u.arg, "buffer", None)
+
+
 def _patched(buf, topo) -> tuple[bytearray, dict]:
   from tinygrad.uop.ops import Ops
   blob, rows = bytearray(buf.max_numel() * buf.dtype.itemsize), {}
@@ -72,7 +78,7 @@ def _evaluate(u, env):
     table, index = _root(u.src[0].src[0]), _evaluate(u.src[0].src[1], env)
     return struct.unpack_from(u.dtype.fmt, table.arg, index * u.dtype.itemsize)[0] if table.op is Ops.BINARY and index is not None else None
   if u.op in GroupOp.ALU and None not in (values := [_evaluate(x, env) for x in u.src]):
-    return exec_alu(u.op, u.dtype, values, False)
+    return exec_alu(u.op, u.dtype, values, *([False] if "truncate_output" in inspect.signature(exec_alu).parameters else []))
   return None
 
 
@@ -112,7 +118,7 @@ def convert(jit, kwargs: dict, frame: dict, target) -> tuple[dict, memoryview]:
   from tinygrad.uop.ops import Ops
   submit = jit.captured._linear.src[0].without_after
   kernels, topo = submit.arg.aux.kernels, list(jit.captured._linear.toposort(enter_calls=True))
-  allocs = {str(u.tag): u for u in topo if u.op is Ops.ALLOC and u.tag is not None}
+  allocs = {str(name): u for u in topo if u.op is Ops.ALLOC and (name := getattr(u, "tag", None) or u.arg.name) is not None}
   (commands, patched), kernargs = _patched(allocs["cmdbuf_compute_0"], topo), allocs["kernargs_compute_0"]
   argument_words, pokes = _patched(kernargs, topo)[1], _input_pokes(submit, kernargs)
   sh = {name: getattr(amd_gpu, f"reg{name}") + amd_gpu.GC_BASE__INST0_SEG0 - pm4.PACKET3_SET_SH_REG_START
@@ -158,7 +164,7 @@ def convert(jit, kwargs: dict, frame: dict, target) -> tuple[dict, memoryview]:
         args.append({"input": pokes[slot][0]})
       elif slot in argument_words:
         base, at, nbytes = _address(argument_words[slot])
-        args.append({"buffer": buffers.setdefault(base, len(buffers)), "offset": at, "nbytes": nbytes or base.arg.buffer.nbytes, "dtype": u8})
+        args.append({"buffer": buffers.setdefault(base, len(buffers)), "offset": at, "nbytes": nbytes or base.max_numel() * base.dtype.itemsize, "dtype": u8})
     outs, ins = kernel[6]
     calls.append({"kind": "program", "name": re.sub(r"\x1b\[[0-9;]*m", "", kernel[1]), "lib": programs[image_alloc], "target": dataclasses.asdict(target),
                   "signature": [[f"data{i}", i, u8, [a.get("nbytes", 1)]] for i, a in enumerate(args)], "global_size": grid, "local_size": local,
@@ -166,12 +172,13 @@ def convert(jit, kwargs: dict, frame: dict, target) -> tuple[dict, memoryview]:
     needs_init |= {args[i]["buffer"] for i in ins if "buffer" in args[i] and args[i]["buffer"] not in written}
     written |= {args[i]["buffer"] for i in outs if "buffer" in args[i]}
 
+  assert all(_buffer(b) is not None for b, i in buffers.items() if i in needs_init), "a kernel reads a scratch before any kernel writes it"
   names = list(jit.captured.expected_names)
   inputs = [{"slot": slot, "name": names[slot] if slot < len(names) else None, "dtype": repr(u.dtype), "numel": int(u.max_numel()), "device": u.device}
             for slot, u in enumerate(_prepare_jit_inputs((), kwargs)[0])]
   header = {"input_names": names, "calls": calls, "returns": {"kind": "NoneType", "tensors": []}, "inputs": inputs,
-            "buffers": [{"nbytes": b.arg.buffer.nbytes, "device": b.arg.buffer.device, "dtype": u8}
-                        | ({"init": blob.add(b.arg.buffer.ensure_allocated().as_memoryview())} if i in needs_init else {}) for b, i in buffers.items()],
+            "buffers": [{"nbytes": bb.nbytes if (bb := _buffer(b)) else b.max_numel() * b.dtype.itemsize, "device": bb.device if bb else b.device, "dtype": u8}
+                        | ({"init": blob.add(bb.ensure_allocated().as_memoryview())} if i in needs_init else {}) for b, i in buffers.items()],
             "check": {"inputs": [[path, store(array), device] for path, array, device in frame["inputs"]],
                       "outputs": [store(array) for array in frame["outputs"]], "output_container": frame["output_container"]}}
   with sink:

openpilot/sunnypilot/modeld_v2/helpers.py

@@ -250,6 +250,12 @@ def _allocate_buffers(header: dict, blob: memoryview, device_map: dict | None, s
   return buffers
 
 
+def _fields(what, /, **values):  # a tinygrad dataclass built or replaced with the fields it has today: a dropped field is fine, a new required one is not
+  known = {field.name for field in dataclasses.fields(what)}
+  values = {name: value for name, value in values.items() if name in known}
+  return what(**values) if isinstance(what, type) else dataclasses.replace(what, **values)
+
+
 def _program_uop(call: dict, lib: bytes):
   from tinygrad.helpers import Target
   from tinygrad.uop.ops import KernelInfo, Ops, ProgramInfo, UOp
@@ -257,9 +263,9 @@ def _program_uop(call: dict, lib: bytes):
   for name, compact_slot, dtype, shape in call["signature"]:
     parameter = UOp.param(call["globals"][compact_slot], _dtype(dtype), math.prod(shape))
     image = (shape[0], shape[1]) if len(shape) == 3 and shape[2] == 4 else None
-    parameters.append(parameter.replace(arg=dataclasses.replace(parameter.arg, name=name, image=image)))
-  info = ProgramInfo(global_size=tuple(call["global_size"]), local_size=tuple(call["local_size"]), vars=(), globals=tuple(call["globals"]),
-                     outs=tuple(call["outs"]), ins=tuple(call["ins"]), target=Target(**call["target"]))
+    parameters.append(parameter.replace(arg=_fields(parameter.arg, name=name, image=image)))
+  info = _fields(ProgramInfo, global_size=tuple(call["global_size"]), local_size=tuple(call["local_size"]), globals=tuple(call["globals"]),
+                 outs=tuple(call["outs"]), ins=tuple(call["ins"]), target=_fields(Target, **call["target"]))
   return UOp(Ops.PROGRAM, src=(UOp(Ops.SINK, arg=KernelInfo(name=call["name"])), UOp(Ops.LINEAR, src=tuple(parameters)),
                                UOp(Ops.SOURCE, arg=""), UOp(Ops.BINARY, arg=lib)), arg=info)
 

openpilot/sunnypilot/modeld_v2/modeld.py

@@ -7,6 +7,7 @@ See the LICENSE.md file in the root directory for more details.
 """
 
 from collections.abc import Callable
+from functools import partial
 import gc
 import os
 os.environ['GMMU'] = '0'
@@ -39,6 +40,7 @@ from openpilot.selfdrive.modeld.modeld import ChestnutGpuState
 
 from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output
 from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser
+from openpilot.sunnypilot.modeld_v2.blendful import Blendful
 from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
 from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants
 from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
@@ -228,7 +230,8 @@ class ModelState(ModelStateBase):
     return outputs
 
   def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
-                            lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action:
+                            lat_action_t: float, long_action_t: float, v_ego: float,
+                            blendful: Callable[[float], float] | None = None) -> log.ModelDataV2.Action:
     if 'action' not in model_output:
       plan = model_output['plan'][0]
       desired_accel = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS,
@@ -240,6 +243,8 @@ class ModelState(ModelStateBase):
     else:
       desired_accel = model_output['action'][0, 1]
       desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2
+    if blendful is not None:
+      desired_curvature += blendful(desired_curvature)
 
     stop = v_ego < 0.3 and desired_accel < 0.1
     desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
@@ -366,6 +371,8 @@ def main(demo=False):
   DH = DesireHelper()
   meta_constants = load_meta_constants()
   RELC = RoadEdgeLaneChangeController()
+  blendful = Blendful()
+  blendful_enabled = False
 
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -409,6 +416,7 @@ def main(demo=False):
       model.lat_delay = get_lat_delay(params, sm["lateralDelay"].lateralDelay)
       model.PLANPLUS_CONTROL = params.get("PlanplusControl", return_default=True)
       camera_offset_helper.set_offset(params.get("CameraOffset", return_default=True))
+      blendful_enabled = params.get_bool("Blendful")
     lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS
     if sm.updated["extrinsicsCalibration"] and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["extrinsicsCalibration"].rpyCalib, dtype=np.float32)
@@ -482,7 +490,9 @@ def main(demo=False):
 
       if not sm['carControl'].latActive:
         prev_action.desiredCurvature = sm['controlsState'].curvature
-      action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
+      blendful_active = desire == log.Desire.none and DH.lane_change_state == log.LaneChangeState.off
+      action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego,
+                                           partial(blendful.update, model_output, v_ego, blendful_active, enabled=blendful_enabled))
       prev_action = action
       fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
                      publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
@@ -499,6 +509,13 @@ def main(demo=False):
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
       mdv2sp_send.valid = modelv2_send.valid
       mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
+      mdv2sp_send.modelDataV2SP.blendfulCurvature = blendful.curvature
+      mdv2sp_send.modelDataV2SP.blendfulTrust = float(blendful.trust)
+      mdv2sp_send.modelDataV2SP.modelCurvature = float(blendful.model_curvature)
+      mdv2sp_send.modelDataV2SP.blendfulGoingAround = bool(blendful.going_around)
+      mdv2sp_send.modelDataV2SP.blendfulLeftProb = blendful.l_prob
+      mdv2sp_send.modelDataV2SP.blendfulRightProb = blendful.r_prob
+      mdv2sp_send.modelDataV2SP.blendfulLaneWidth = float(blendful.lane_width.x)
       drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
       drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
 

openpilot/sunnypilot/modeld_v2/plain_artifact_compiler.py

@@ -23,7 +23,7 @@ def record_kernel_linear():
   recorded, hcq_compile, compile_linear = [], realize.hcq_compile, jit.compile_linear
 
   def recording_compile_linear(*args, **kwargs):
-    with mock.patch.object(realize, "hcq_compile", lambda linear, *a, **k: (recorded.append(linear), hcq_compile(linear, *a, **k))[1]):
+    with mock.patch.object(realize, "hcq_compile", lambda linear, *a, **k: (linear.src and recorded.append(linear), hcq_compile(linear, *a, **k))[1]):
       return compile_linear(*args, **kwargs)
 
   with mock.patch.object(jit, "compile_linear", recording_compile_linear):
@@ -38,6 +38,18 @@ def capture_frame(jit, kwargs: dict, output_container: str | None = None):
   return {"inputs": inputs, "outputs": [tensor.numpy().copy() for tensor in outputs], "output_container": output_container}, result
 
 
+def kernel_signature(ast, elf) -> list:
+  from tinygrad.dtype import AddrSpace
+  if hasattr(ast, "kernel_params"):
+    if any(p.arg.addrspace is AddrSpace.ALU for p in ast.kernel_params):
+      raise ValueError(f"{elf.name} takes a variable, which plain artifacts don't support")
+    compact = {slot: i for i, slot in enumerate(ast.arg.globals)}
+    return [[p.arg.name, compact[p.arg.slot], repr(p.dtype), [int(x) for x in p._shape]] for p in ast.kernel_params]
+  if not all(isinstance(slot, int) for _, slot, _, _ in elf.signature):
+    raise ValueError("TinyELF.signature lost its slots and this tinygrad has no UOp.kernel_params: update kernel_signature")
+  return [[name, slot, repr(dtype), [int(x) for x in shape]] for name, slot, dtype, shape in elf.signature]
+
+
 def serialize_jit(jit, kernel_linear, check: dict | None = None) -> tuple[dict, memoryview]:
   from tinygrad.uop.ops import Ops
   sink = tempfile.TemporaryFile(dir=".")
@@ -51,6 +63,12 @@ def serialize_jit(jit, kernel_linear, check: dict | None = None) -> tuple[dict,
     if uop.op is Ops.PARAM:
       input_specs.setdefault(uop.arg.slot, {"dtype": repr(uop.dtype), "numel": int(uop.max_numel()), "device": uop.device})
       return {"input": uop.arg.slot}
+    root, offset = uop, 0
+    while root.op in (Ops.AFTER, Ops.BITCAST, Ops.SHRINK):
+      offset, root = offset + (root.src[1].arg * root.src[0].dtype.itemsize if root.op is Ops.SHRINK else 0), root.src[0]
+    if root.op is Ops.ALLOC:
+      base = bases.setdefault(id(root), (len(bases), root, root.dtype))
+      return {"buffer": base[0], "offset": offset, "nbytes": int(uop.max_numel()) * uop.dtype.itemsize, "dtype": repr(uop.dtype)}
     buffer = uop.buffer
     root = buffer._base if buffer._base is not None else buffer
     base = bases.setdefault(id(root), (len(bases), root, uop.base.dtype if buffer._base is None else uop.dtype))
@@ -63,7 +81,7 @@ def serialize_jit(jit, kernel_linear, check: dict | None = None) -> tuple[dict,
       elf, info = ast.to_elf(), ast.arg
       global_size, local_size = info.launch_dims({})
       calls.append({"kind": "program", "name": elf.name, "lib": blob.add(bytes(elf.lib)), "target": dataclasses.asdict(elf.target),
-                    "signature": [[name, slot, repr(dtype), [int(x) for x in shape]] for name, slot, dtype, shape in elf.signature],
+                    "signature": kernel_signature(ast, elf),
                     "global_size": [int(x) for x in global_size], "local_size": [int(x) for x in local_size],
                     "globals": list(info.globals), "outs": list(info.outs), "ins": list(info.ins), "args": arguments})
       outs, ins = info.outs, info.ins
@@ -77,7 +95,9 @@ def serialize_jit(jit, kernel_linear, check: dict | None = None) -> tuple[dict,
 
   ret = jit.captured.ret
   returns = {"kind": type(ret).__name__, "tensors": [describe(t.uop.base) | {"shape": [int(x) for x in t.shape]} for t in flatten(ret)]}
-  buffers = [{"nbytes": base.nbytes, "device": base.device, "dtype": repr(dtype)}
+  from tinygrad.device import Buffer
+  assert not [base for index, base, _ in bases.values() if index in needs_init and not isinstance(base, Buffer)], "a kernel reads a scratch before writing it"
+  buffers = [{"nbytes": base.nbytes if isinstance(base, Buffer) else int(base.max_numel()) * dtype.itemsize, "device": base.device, "dtype": repr(dtype)}
              | ({"init": blob.add(base.as_memoryview())} if index in needs_init else {}) for index, base, dtype in bases.values()]
   names = list(jit.captured.expected_names)
   if check is not None:

openpilot/sunnypilot/modeld_v2/tests/test_convert_upstream_pkl.py

@@ -0,0 +1,121 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import os
+import re
+import sys
+import tempfile
+import unittest
+from unittest import mock
+import numpy as np
+
+os.environ.setdefault("CHECK_OOB", "0")
+os.environ.setdefault("GMMU", "0")
+if sys.platform == "darwin" and os.environ.get("DEV", "").startswith("MOCKUSB"):  # the mock's VRAM is a memfd, which macOS lacks: an unlinked file is the same
+  from tinygrad.runtime.autogen import libc
+
+  def _memfd_create(name, flags):
+    fd, path = tempfile.mkstemp()
+    os.unlink(path)
+    return fd
+  libc.memfd_create = _memfd_create
+
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.sunnypilot.modeld_v2 import convert_upstream_pkl
+from openpilot.sunnypilot.modeld_v2.convert_upstream_pkl import convert
+from openpilot.sunnypilot.modeld_v2.helpers import PlainJit, load_model
+from openpilot.sunnypilot.modeld_v2.plain_artifact_compiler import capture_frame
+
+IMG, DESIRE, STATE, OUT = (6, 8, 8), (8,), (4, 16), (1, 32)
+
+
+class TestConvertUpstreamPkl(OpenpilotTestCase):
+  @classmethod
+  def setUpClass(cls):
+    super().setUpClass()
+    if not os.environ.get("TINYGRAD_TESTS"):
+      raise unittest.SkipTest("TINYGRAD_TESTS not set. skipping converter test")
+    from tinygrad import Device, Tensor
+    assert Device.DEFAULT == "AMD" and getattr(Device["AMD"], "is_usb", False), f"{Device.DEFAULT} is not an AMD device behind the USB bridge"
+    assert (Tensor([1, 2, 3]) * 2).tolist() == [2, 4, 6]
+    cls.tmp = tempfile.TemporaryDirectory()
+    cls.rng = np.random.default_rng(0)
+    cls.weights = Tensor(cls.rng.standard_normal((16, 32)).astype(np.float32)).realize()
+    cls.bias = Tensor(cls.rng.standard_normal((1, 32)).astype(np.float32)).realize()
+    cls.input_specs = {"input_imgs": (IMG, "uint8", Device.DEFAULT), "desire": (DESIRE, "float32", Device.DEFAULT),
+                       "features": (STATE, "float32", Device.DEFAULT)}
+    cls.output_specs = {"outputs": (OUT, "float32", Device.DEFAULT), "next_features": (STATE, "float32", Device.DEFAULT)}
+
+  @classmethod
+  def tearDownClass(cls):
+    cls.tmp.cleanup()
+    super().tearDownClass()
+
+  def model(self, input_imgs, desire, features, output_buffers):
+    from tinygrad import Tensor
+    feat = features.mean(axis=0).reshape(1, 16) + input_imgs.float().mean() + desire.sum()
+    Tensor.realize(output_buffers["outputs"].assign((feat @ self.weights + self.bias).relu()),
+                   output_buffers["next_features"].assign(features[1:].cat(feat * 2)))
+
+  def inputs(self, seed: int) -> dict:
+    from examples.openpilot.helpers import allocate_inputs
+    rng = np.random.default_rng(seed)
+
+    def initialize(arrays: dict):
+      for value in arrays.values():
+        value[...] = rng.standard_normal(value.shape) if value.dtype.kind == "f" else rng.integers(0, 256, value.shape)
+    return allocate_inputs(self.input_specs, initialize) | {"output_buffers": allocate_inputs(self.output_specs)}
+
+  def jit(self):
+    from tinygrad import TinyJit
+    run = TinyJit(self.model, prune=True)
+    kwargs = self.inputs(1)
+    run(**kwargs)
+    run(**kwargs)
+    return run
+
+  def assert_matches_eager(self, plain, seed: int):
+    fresh, expected = self.inputs(seed), self.inputs(seed)
+    plain(**fresh)
+    self.model(**expected)
+    for name in self.output_specs:
+      assert np.array_equal(fresh["output_buffers"][name].numpy(), expected["output_buffers"][name].numpy()), name
+
+  def convert(self, jit) -> tuple[dict, memoryview]:
+    from tinygrad import Device
+    kwargs = self.inputs(2)
+    frame, _ = capture_frame(jit, kwargs, "output_buffers")
+    header, blob = convert(jit, kwargs, frame, Device[Device.DEFAULT].renderer.target)
+    kernels = jit.captured._linear.src[0].without_after.arg.aux.kernels
+    assert [call["name"] for call in header["calls"]] == [re.sub(r"\x1b\[[0-9;]*m", "", kernel[1]) for kernel in kernels]
+    assert all(re.fullmatch(r"\w+", call["name"]) for call in header["calls"]), "kernel names: failure point 1"
+    assert [(tuple(call["outs"]), tuple(call["ins"])) for call in header["calls"]] == [tuple(kernel[6]) for kernel in kernels], "outs/ins: failure point 1"
+    bound = {argument["input"] for call in header["calls"] for argument in call["args"] if "input" in argument}
+    assert bound == {spec["slot"] for spec in header["inputs"]}, f"input slots bound by pokes {sorted(bound)}: failure point 4"
+    assert [spec["name"] for spec in header["inputs"]][:len(self.input_specs)] == sorted(self.input_specs)
+    return header, blob
+
+  def test_converted_jit_matches_eager(self):  # the submit decoded into plain calls, replayed against its own frame
+    jit = self.jit()
+    plain = PlainJit(*self.convert(jit))
+    self.assert_matches_eager(plain, 3)
+    self.assert_matches_eager(plain, 4)
+
+  def test_pickle_round_trip_through_main(self):
+    from examples.openpilot.helpers import dump_pickle, load_pickle
+    from tinygrad.engine.realize import lower_and_compile
+    pkl, out = os.path.join(self.tmp.name, "tiny_tinygrad.pkl"), os.path.join(self.tmp.name, "tiny_plain.pkl")
+    dump_pickle({"run": self.jit(), "input_specs": self.input_specs, "output_specs": self.output_specs, "metadata": {"name": "tiny"}}, pkl)
+
+    artifact = load_pickle(pkl, out_of_band=True)
+    artifact["run"].captured._linear = lower_and_compile(artifact["run"].captured._linear)
+    self.assert_matches_eager(PlainJit(*self.convert(artifact["run"])), 5)
+
+    with mock.patch.object(sys, "argv", ["convert_upstream_pkl", pkl, out]):
+      convert_upstream_pkl.main()
+    bundle = load_model(out, cache=False, verify=True)
+    assert bundle["metadata"] == {"name": "tiny"} and bundle["input_specs"] == self.input_specs and bundle["output_specs"] == self.output_specs
+    self.assert_matches_eager(bundle["run"], 6)

openpilot/sunnypilot/modeld_v2/tests/test_plain_artifact.py

@@ -0,0 +1,162 @@
+"""
+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 dataclasses
+import functools
+import inspect
+import os
+import sys
+import tempfile
+from unittest import mock
+import numpy as np
+import unittest
+
+import openpilot.sunnypilot.modeld_v2.helpers as helpers
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.sunnypilot.modeld_v2 import compile_warp
+from openpilot.sunnypilot.modeld_v2.frame_fusion import frame_part, pack_layout
+from openpilot.sunnypilot.modeld_v2.helpers import PlainJit, cache_is_fresh, load_model, read_parts, write_bundle
+from openpilot.sunnypilot.modeld_v2.plain_artifact_compiler import capture_frame, record_kernel_linear, serialize_jit
+
+CAM_W, CAM_H, MODEL_W, MODEL_H = 32, 16, 16, 8
+FRAME = f"{CAM_W},{CAM_H},{CAM_W},{CAM_H},{CAM_H // 2},{CAM_W * CAM_H * 3 // 2}"
+FRAME_SIZE = CAM_W * CAM_H * 3 // 2
+HOST = {"desire": (8,), "traffic_convention": (2,)}
+STATE, OUT = (4, 16), (1, 32)
+
+
+def compile_jit(fn, inputs: dict, output_container: str | None = None) -> tuple[dict, memoryview]:
+  from tinygrad import TinyJit
+  run = TinyJit(fn, prune=True)
+  with record_kernel_linear() as recorded:
+    run(**inputs)
+    run(**inputs)
+    check, _ = capture_frame(run, inputs, output_container)
+  assert len(recorded) == 1, f"expected one captured kernel list, got {len(recorded)}"
+  return serialize_jit(run, recorded[0], check)
+
+
+class TestPlainArtifact(OpenpilotTestCase):
+  @classmethod
+  def setUpClass(cls):
+    super().setUpClass()
+    if not os.environ.get("TINYGRAD_TESTS"):
+      raise unittest.SkipTest("TINYGRAD_TESTS env var not set, skipping artifact test.")
+    from tinygrad import Tensor
+    cls.tmp = tempfile.TemporaryDirectory()
+    cls.rng = np.random.default_rng(0)
+    cls.weights = Tensor(cls.rng.standard_normal((16, 32)).astype(np.float32)).realize()
+    cls.bias = Tensor(cls.rng.standard_normal((1, 32)).astype(np.float32)).realize()
+
+  @classmethod
+  def tearDownClass(cls):
+    cls.tmp.cleanup()
+    super().tearDownClass()
+
+  def model(self, x, h):  # a two input, one weight model: the shape of a policy without the size
+    return ((x @ self.weights + self.bias).relu() + h.mean()).contiguous()
+
+  def tensors(self, seed: int):
+    from tinygrad import Tensor
+    rng = np.random.default_rng(seed)
+    return {"x": Tensor(rng.standard_normal((1, 16)).astype(np.float32)).realize(), "h": Tensor(rng.standard_normal((4, 16)).astype(np.float32)).realize()}
+
+  def artifact(self, name: str = "model") -> str:
+    path = os.path.join(self.tmp.name, f"{name}_tinygrad.pkl")
+    if not os.path.exists(path):
+      write_bundle(path, {"run": compile_jit(self.model, self.tensors(1))}, {"metadata": {"name": name}})
+    return path
+
+  def test_round_trip_matches_jit(self):  # compiler -> bundle -> PlainJit, replay verified, then fresh inputs bit exact with eager tinygrad
+    bundle = load_model(self.artifact(), cache=False, verify=True)
+    assert bundle["metadata"] == {"name": "model"} and isinstance(bundle["run"], PlainJit)
+    fresh = self.tensors(2)
+    assert np.array_equal(bundle["run"](**fresh).numpy(), self.model(**fresh).numpy())
+
+  def test_replay_check_catches_a_changed_kernel(self):
+    parts, extras = read_parts(self.artifact())
+    header, blob = parts["run"]
+    poisoned = dict(header, check=dict(header["check"], outputs=[header["check"]["inputs"][0][1]]))
+    with self.assertRaises(helpers.ArtifactMismatch):
+      PlainJit(poisoned, blob)
+
+  def test_cache_round_trip_holds_no_weights_and_is_rebuilt_when_stale(self):
+    path = self.artifact("cached")
+    expected = load_model(path, cache=False, verify=False)["run"](**self.tensors(3)).numpy()
+    load_model(path)
+    assert cache_is_fresh(path)
+    with open(f"{path}.cache", "rb") as file:
+      cache = file.read()
+    assert self.weights.numpy().tobytes() not in cache and len(cache) < os.path.getsize(path)
+    with mock.patch.object(helpers, "PlainJit", side_effect=AssertionError("a fresh cache must not rebuild")):
+      cached = load_model(path)
+    assert np.array_equal(cached["run"](**self.tensors(3)).numpy(), expected)
+    with mock.patch.object(helpers, "tinygrad_identity", return_value="another tinygrad"):
+      assert not cache_is_fresh(path)
+      assert isinstance(load_model(path)["run"], PlainJit)
+      assert cache_is_fresh(path)
+    assert not cache_is_fresh(path)  # keyed on the tinygrad that wrote it, so the next boot after a bump rebuilds once
+    os.utime(path)
+    with mock.patch.object(helpers, "tinygrad_identity", return_value="another tinygrad"):
+      assert not cache_is_fresh(path)
+
+  def test_fused_with_warp_matches_warp_then_model(self):
+    from tinygrad import Tensor
+    warp_path = os.path.join(self.tmp.name, "warp_tinygrad.pkl")
+    with mock.patch.object(sys, "argv", ["compile_warp", "--frame", FRAME, "--warp-to", f"{MODEL_W}x{MODEL_H}", "--output", warp_path]):
+      compile_warp.main()
+    warp = load_model(warp_path, cache=False)["run"]
+
+    def native(new_img, desire, traffic_convention, state_feat_q, output_buffers):
+      feat = state_feat_q.mean(axis=0).reshape(1, 16) + new_img.float().mean() + desire.sum() + traffic_convention[0]
+      Tensor.realize(output_buffers["outputs"].assign((feat @ self.weights + self.bias).relu()),
+                     output_buffers["next_state_feat_q"].assign(state_feat_q[1:].cat(feat * 2)))
+
+    def model_inputs(new_img):
+      return {"new_img": new_img} | {name: Tensor(np.ones(shape, np.float32) * 0.5).realize() for name, shape in HOST.items()} | \
+             {"state_feat_q": Tensor(self.rng.standard_normal(STATE).astype(np.float32)).realize(),
+              "output_buffers": {"outputs": Tensor.zeros(OUT).contiguous().realize(), "next_state_feat_q": Tensor.zeros(STATE).contiguous().realize()}}
+    frames = Tensor(self.rng.integers(0, 256, (2, FRAME_SIZE), dtype=np.uint8)).realize()
+    tfm = Tensor(np.tile(np.eye(3, dtype=np.float32), (2, 1, 1))).realize()
+    path = os.path.join(self.tmp.name, "native_tinygrad.pkl")
+    part = compile_jit(native, model_inputs(warp(input_frame=frames, M_inv=tfm).realize()), "output_buffers")
+    extras = {"input_specs": {"new_img": ((2, 6, MODEL_H // 2, MODEL_W // 2), "|u1", "CPU"), "state_feat_q": (STATE, "<f4", "CPU")}
+              | {name: (shape, "<f4", "CPU") for name, shape in HOST.items()},
+              "output_specs": {"outputs": (OUT, "<f4", "CPU"), "next_state_feat_q": (STATE, "<f4", "CPU")},
+              "metadata": {"output_shapes": {"outputs": OUT, "next_state_feat_q": STATE}}}
+    write_bundle(path, {"run": part}, extras)
+
+    fused = load_model(path, cache=False, extend=functools.partial(frame_part, warp_path))
+    assert fused["run"].input_names == ["host", "state_feat_q"]
+    offsets, packed = pack_layout(HOST, FRAME_SIZE)
+    host = np.zeros(packed, np.uint8)
+    inputs = model_inputs(None)
+    for name, shape in ({"tfm": (2, 3, 3)} | HOST).items():
+      np.ndarray(shape, np.float32, host, offsets[name])[...] = tfm.numpy() if name == "tfm" else inputs[name].numpy()
+    host[offsets["frames"]:] = frames.numpy().ravel()
+    fused["run"](host=Tensor(host, device="NPY"), state_feat_q=inputs["state_feat_q"], output_buffers=inputs["output_buffers"])
+
+    expected = model_inputs(warp(input_frame=frames, M_inv=tfm).realize()) | {k: inputs[k] for k in ("state_feat_q",)}
+    native(**expected)
+    for name in ("outputs", "next_state_feat_q"):
+      assert np.array_equal(inputs["output_buffers"][name].numpy(), expected["output_buffers"][name].numpy()), name
+
+  def test_tinygrad_api_the_loader_reads(self):
+    import tinygrad.engine.jit as jit
+    from tinygrad.device import Buffer, TinyELF
+    from tinygrad.helpers import Target
+    from tinygrad.uop.ops import ParamArg, ProgramInfo, UOp
+    fields = {cls: {f.name for f in dataclasses.fields(cls)} for cls in (ProgramInfo, ParamArg, Target)}
+    assert fields[ProgramInfo] >= {"global_size", "local_size", "globals", "outs", "ins", "target"}, "ProgramInfo changed: update helpers._program_uop"
+    assert fields[ParamArg] >= {"slot", "dtype", "name", "image"}, "ParamArg changed: update helpers._program_uop"
+    assert fields[Target] >= {"device", "renderer", "arch"}, "Target changed: update the 'target' key in plain_artifact_compiler.serialize_jit"
+    assert "signature" in getattr(TinyELF, "__annotations__", {}) or hasattr(UOp, "kernel_params"), \
+      "kernel signatures moved: update plain_artifact_compiler.serialize_jit"
+    assert callable(getattr(UOp, "param", None)) and callable(getattr(UOp, "from_buffer", None)), "UOp.param/from_buffer: update helpers.PlainJit"
+    assert callable(getattr(UOp, "store_call", None)) and callable(getattr(UOp, "call", None)), "UOp.call/store_call: update helpers.PlainJit"
+    assert callable(getattr(Buffer, "view", None)) and callable(getattr(Buffer, "ensure_allocated", None)), "Buffer.view: update helpers._upload"
+    assert callable(getattr(jit, "compile_linear", None)) and callable(getattr(jit, "_prepare_jit_inputs", None)), "engine.jit: update helpers._Jit"
+    assert "expected_names" in inspect.signature(jit.CapturedJit).parameters, "CapturedJit: update helpers.PlainJit and plain_artifact_compiler.serialize_jit"

openpilot/sunnypilot/models/manager.py

@@ -34,6 +34,8 @@ TRANSIENT_HTTP_STATUS = frozenset({408, 429, 500, 502, 503, 504})
 REPORT_INTERVAL = 0.5  # seconds between progress publications
 # byte ranges count encoded bytes but iter_content yields decoded ones, so ask for an unencoded body
 NO_COMPRESSION = {"Accept-Encoding": "identity"}
+SPEED_SMOOTHING = 0.7  # weight of the previous speed sample in the published speed
+SPEED_FLOOR = 0.05 * 1024 ** 2  # bytes/s; anything slower would only display as 0.0 MB/s, so it reads as no rate
 
 
 class DownloadCancelled(Exception):
@@ -154,6 +156,7 @@ class ModelManagerSP:
         artifact.downloadProgress.status = source_artifact.downloadProgress.status
         artifact.downloadProgress.progress = source_artifact.downloadProgress.progress
         artifact.downloadProgress.eta = source_artifact.downloadProgress.eta
+        artifact.downloadProgress.speed = source_artifact.downloadProgress.speed
 
   def _calculate_eta(self, filename: str, progress: float) -> int:
     if filename not in self._download_start_times or progress <= 0:
@@ -167,28 +170,43 @@ class ModelManagerSP:
     eta = total_estimated_time - elapsed_time
     return max(1, int(eta))  # Return at least 1 second if download is ongoing
 
-  def _set_progress(self, artifact, status, progress: float, eta: int = 0) -> None:
+  def _set_progress(self, artifact, status, progress: float, eta: int = 0, speed: float = 0.0) -> None:
     artifact.downloadProgress.status = status
     artifact.downloadProgress.progress = progress
     artifact.downloadProgress.eta = eta
+    artifact.downloadProgress.speed = speed
     self._sync_artifact_progress(artifact)
     self._report_status()
 
-  def _publish_progress(self, artifact, done_bytes: int, total: int) -> None:
+  def _publish_progress(self, artifact, done_bytes: int, total: int, speed: float) -> None:
     # held at 99 until the file passes its hash check
     progress = min(99.0, done_bytes / total * 100) if total > 0 else 0.0
-    eta = self._calculate_eta(artifact.fileName, progress)
-    self._set_progress(artifact, custom.ModelManagerSP.DownloadStatus.downloading, progress, eta)
+    if speed > 0 and total > 0:
+      eta = max(1, int((total - done_bytes) / speed))
+    else:
+      eta = self._calculate_eta(artifact.fileName, progress)
+    self._set_progress(artifact, custom.ModelManagerSP.DownloadStatus.downloading, progress, eta, speed)
 
-  async def _report_until_done(self, tasks: list[asyncio.Future], artifact, progress: _Progress, total: int) -> None:
+  async def _report_until_done(self, tasks: list[asyncio.Future], artifact, progress: _Progress, total: int,
+                               started: tuple[int, float]) -> None:
     pending = set(tasks)
+    last_bytes, last_time = started
+    speed = 0.0
     while pending:
       done, pending = await asyncio.wait(pending, timeout=REPORT_INTERVAL)
       for task in done:
         task.result()  # re-raises a worker failure
       if self._download_interrupted():
         raise DownloadCancelled("Download cancelled")
-      self._publish_progress(artifact, progress.snapshot(), total)
+      now = time.monotonic()
+      done_bytes = progress.snapshot()
+      if (dt := now - last_time) > 0:
+        instant = max(0.0, (done_bytes - last_bytes) / dt)  # bytes/s this tick; clamped since retried pieces can lower the byte count
+        speed = instant if speed == 0 else SPEED_SMOOTHING * speed + (1 - SPEED_SMOOTHING) * instant
+        if speed < SPEED_FLOOR:  # a stall decays toward 0 without reaching it, and a near-0 speed overflows the eta
+          speed = 0.0
+      last_time, last_bytes = now, done_bytes
+      self._publish_progress(artifact, done_bytes, total, speed)
 
   async def _download_file(self, url: str, path: str, artifact) -> None:
     self._download_start_times[artifact.fileName] = time.monotonic()
@@ -199,12 +217,13 @@ class ModelManagerSP:
     await loop.run_in_executor(None, _prepare_target, path, total)
     progress = _Progress()
     cancel = threading.Event()
+    started = progress.snapshot(), time.monotonic()  # before any worker can land bytes
 
     with ThreadPoolExecutor(max_workers=MAX_CONCURRENT_PIECES) as pool:
       tasks = [loop.run_in_executor(pool, _fetch_piece, url, path, i, start, end, progress, cancel, self._block_size)
                for i, (start, end) in enumerate(pieces)]
       try:
-        await self._report_until_done(tasks, artifact, progress, total)
+        await self._report_until_done(tasks, artifact, progress, total, started)
       except BaseException as e:
         cancel.set()
         if isinstance(e, DownloadCancelled):
@@ -247,6 +266,7 @@ class ModelManagerSP:
       self._download_start_times.pop(artifact.fileName, None)
       artifact.downloadProgress.status = status.failed
       artifact.downloadProgress.eta = 0
+      artifact.downloadProgress.speed = 0
       self._sync_artifact_progress(artifact)
       if self.selected_bundle:
         self.selected_bundle.status = status.failed
@@ -259,6 +279,7 @@ class ModelManagerSP:
         os.remove(full_path)
       artifact.downloadProgress.status = status.failed
       artifact.downloadProgress.eta = 0
+      artifact.downloadProgress.speed = 0
       self._sync_artifact_progress(artifact)
       if self.selected_bundle:
         self.selected_bundle.status = status.failed
@@ -301,6 +322,7 @@ class ModelManagerSP:
           artifact.downloadProgress.status = custom.ModelManagerSP.DownloadStatus.cached
           artifact.downloadProgress.progress = 100
           artifact.downloadProgress.eta = 0
+          artifact.downloadProgress.speed = 0
         else:
           seen_artifacts.add(artifact.fileName)
           await self._process_artifact(artifact, destination_path)

openpilot/sunnypilot/models/tests/test_manager_download.py

@@ -6,6 +6,7 @@ See the LICENSE.md file in the root directory for more details.
 """
 
 import asyncio
+import concurrent.futures
 import contextlib
 import dataclasses
 import hashlib
@@ -129,6 +130,13 @@ def threaded_server(handler):
     thread.join()
 
 
+class InlineExecutor(concurrent.futures.Executor):
+  def submit(self, fn, /, *args, **kwargs):
+    future: concurrent.futures.Future = concurrent.futures.Future()
+    future.set_result(fn(*args, **kwargs))  # the piece lands before the reporter starts
+    return future
+
+
 class ManagerDownloadTestBase(OpenpilotTestCase):
   def setUp(self):
     super().setUp()
@@ -139,7 +147,7 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
     self.addCleanup(self._tmp.cleanup)
     self.dest = self._tmp.name
 
-    for name, value in (('PIECE_SIZE', PIECE_SIZE), ('REPORT_INTERVAL', 0.05), ('PIECE_BACKOFF', 0.01)):
+    for name, value in (('PIECE_SIZE', PIECE_SIZE), ('REPORT_INTERVAL', 0.05), ('PIECE_BACKOFF', 0.01), ('SPEED_FLOOR', 100)):
       self.enterContext(mock.patch.object(manager_module, name, value))
 
     self.reported: list[float] = []
@@ -229,6 +237,31 @@ class TestManagerDownload(ManagerDownloadTestBase):
     assert self.reported and self.reported == sorted(self.reported), "progress only rises"
     assert max(self.reported) <= 99.0, "progress holds below 100 until the hash check"
     assert artifact.fileName not in self.manager._download_start_times
+    assert artifact.downloadProgress.speed > 0, "a running download shows its speed"
+
+  def test_speed_counts_bytes_landed_before_the_first_report(self):
+    self.enterContext(mock.patch.object(manager_module, 'ThreadPoolExecutor', lambda max_workers: InlineExecutor()))
+    artifact = self.download_file()
+    assert artifact.downloadProgress.speed > 0
+
+  def test_eta_comes_from_speed(self):
+    artifact = self.make_artifact()
+    self.manager._publish_progress(artifact, done_bytes=400, total=1000, speed=100)
+    assert artifact.downloadProgress.eta == 6
+
+  def test_stalled_speed_drops_to_zero(self):
+    stall_event = self.stall(0)
+    seen_speed = threading.Event()
+
+    def release_once_zero(*args):
+      if self.artifact.downloadProgress.speed > 0:
+        seen_speed.set()
+      elif seen_speed.is_set():
+        stall_event.set()
+    self.pm.send.side_effect = release_once_zero
+    self.download_file()
+    assert self.server.stall_released_by_event is True, "the stalled speed never reached exactly 0"
+    assert self.artifact.downloadProgress.speed > 0, "the speed must recover once data flows again"
 
   def test_pieces_download_in_parallel(self):
     stall_event = self.stall(0)
@@ -303,7 +336,8 @@ class TestProcessArtifact(ManagerDownloadTestBase):
         self.download_bundle(source)
         assert ACTIVE_BUNDLE_KEYS[source] in store and ACTIVE_BUNDLE_KEYS[other] not in store
         assert self.manager.selected_bundle.status == ds.downloaded
-        assert self.artifact.downloadProgress.progress == 100 and self.artifact.downloadProgress.eta == 0
+        assert self.artifact.downloadProgress.progress == 100
+        assert self.artifact.downloadProgress.eta == 0 and self.artifact.downloadProgress.speed == 0
         assert self.read_path() == WHOLE_BODY
         os.remove(self.path)
 

openpilot/sunnypilot/selfdrive/car/interfaces.py

@@ -126,6 +126,9 @@ def initialize_params(params) -> list[dict[str, Any]]:
   keys.extend([
     "ToyotaEnforceStockLongitudinal",
     "ToyotaStopAndGoHack",
+    "ToyotaEnhancedBsm",
+    "ToyotaAngleSteering",
+    "ToyotaAngleSteeringHighOutput",
   ])
 
   return [{k: params.get(k, return_default=True)} for k in keys]

openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py

@@ -35,6 +35,12 @@ class ControlsExt(ModelStateBase):
     self.pm_services_ext = ['carControlSP']
 
   def initialize_lateral_control(self, lac, CI, dt):
+    # Angle and curvature cars must keep the controller that matches steerControlType: controlsd logs
+    # whatever lac_log this returns into the corresponding lateralControlState union field, so swapping in
+    # a torque controller (LateralTorqueState) raises a capnp type mismatch and kills controlsd on publish
+    if self.CP.steerControlType != structs.CarParams.SteerControlType.torque:
+      return lac
+
     enforce_torque_control = self.params.get_bool("EnforceTorqueControl")
     torque_versions = self.params.get("TorqueControlTune")
     if not enforce_torque_control:

openpilot/sunnypilot/selfdrive/controls/lib/accel_boost_ext.py

@@ -0,0 +1,65 @@
+"""
+Copyright (c) 2021-, 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.cereal import log
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.accel_boost import AccelBoost, ACCEL_BOOST_MAX
+from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
+
+SCALE_RELAXED = 0.3
+SCALE_STANDARD = 0.5
+SCALE_AGGRESSIVE = 0.7
+
+
+def get_accel_boost_scale(personality) -> float:
+  if personality == log.LongitudinalPersonality.aggressive:
+    return SCALE_AGGRESSIVE
+  if personality == log.LongitudinalPersonality.relaxed:
+    return SCALE_RELAXED
+  return SCALE_STANDARD
+
+
+def get_accel_boost_max(personality) -> float:
+  return ACCEL_BOOST_MAX * (1.0 + get_accel_boost_scale(personality))
+
+
+class AccelBoostExt:
+  def __init__(self, stock: AccelBoost | None = None):
+    self._stock = stock if stock is not None else AccelBoost()
+    self._extra = AccelBoost()
+    self.params = Params()
+    self.frame = -1
+    self.enabled = self.params.get_bool("FollowDistanceAccelBoost")
+
+    self.extra_boost = 0.0
+
+  @property
+  def total_boost(self) -> float:
+    return self._stock.total_boost + self.extra_boost
+
+  def _update_params(self) -> None:
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.enabled = self.params.get_bool("FollowDistanceAccelBoost")
+
+  def update(self, sm, output_a_target_e2e, output_a_target_mpc, a_cruise):
+    self.frame += 1
+    self._update_params()
+
+    if self.enabled and sm['selfdriveState'].enabled:
+      scale = get_accel_boost_scale(sm['selfdriveState'].personality)
+      extra_output = self._extra.update(sm, output_a_target_e2e, output_a_target_mpc, a_cruise)
+      self.extra_boost = (extra_output - output_a_target_e2e) * scale
+    else:
+      self.extra_boost = 0.0
+      self._extra.total_boost = 0.0
+      self._extra.boost_this_override = 0.0
+      self._extra.boost_eligible = False
+
+      return self._stock.update(sm, output_a_target_e2e, output_a_target_mpc, a_cruise)
+
+    target = output_a_target_e2e + self.extra_boost
+    return self._stock.update(sm, target, output_a_target_mpc, a_cruise)

openpilot/sunnypilot/selfdrive/controls/lib/auto_lane_change.py

@@ -10,6 +10,8 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
 
 
+
+
 class AutoLaneChangeMode:
   OFF = -1
   NUDGE = 0  # default
@@ -31,6 +33,7 @@ AUTO_LANE_CHANGE_TIMER = {
 }
 
 ONE_SECOND_DELAY = -1
+EDGE_DELAY = -2
 
 
 class AutoLaneChangeController:
@@ -48,6 +51,7 @@ class AutoLaneChangeController:
     self.prev_brake_pressed = False
     self.auto_lane_change_allowed = False
     self.prev_lane_change = False
+    self.lane_change_blocked = False
 
     self.read_params()
 
@@ -68,7 +72,7 @@ class AutoLaneChangeController:
       self.read_params()
     self.param_read_counter += 1
 
-  def update_lane_change_timers(self, blindspot_detected: bool) -> None:
+  def update_lane_change_timers(self, blindspot_detected: bool, edge_detected: bool) -> None:
     self.lane_change_delay = AUTO_LANE_CHANGE_TIMER.get(self.lane_change_set_timer,
                                                         AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGE])
 
@@ -80,6 +84,12 @@ class AutoLaneChangeController:
       else:
         self.lane_change_wait_timer = self.lane_change_delay + ONE_SECOND_DELAY
 
+    if edge_detected and self.lane_change_delay > 0:
+      if self.lane_change_delay == AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGELESS]:
+        self.lane_change_wait_timer = min(self.lane_change_wait_timer, EDGE_DELAY)
+      else:
+        self.lane_change_wait_timer = min(self.lane_change_wait_timer, self.lane_change_delay + EDGE_DELAY)
+
   def update_allowed(self) -> bool:
     # Auto lane change allowed if:
     # 1. A valid delay is set (non-zero)
@@ -97,14 +107,23 @@ class AutoLaneChangeController:
 
     return bool(self.lane_change_wait_timer > self.lane_change_delay)
 
-  def update_lane_change(self, blindspot_detected: bool, brake_pressed: bool) -> None:
+  def update_lane_change(self, blindspot_detected: bool, edge_detected: bool, brake_pressed: bool,
+                         torque_applied: bool = False) -> None:
     if brake_pressed and not self.prev_brake_pressed:
       self.prev_brake_pressed = brake_pressed
 
-    self.update_lane_change_timers(blindspot_detected)
+    self.update_lane_change_timers(blindspot_detected, edge_detected)
 
     self.auto_lane_change_allowed = self.update_allowed()
 
+    # BSM always blocks. Edge blocks auto mode but nudge can override.
+    if blindspot_detected:
+      self.lane_change_blocked = True
+    elif edge_detected:
+      self.lane_change_blocked = not torque_applied
+    else:
+      self.lane_change_blocked = False
+
   def update_state(self):
     if self.DH.lane_change_state == log.LaneChangeState.laneChangeStarting:
       self.prev_lane_change = True

openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -9,6 +9,7 @@ from openpilot.cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.common.constants import CV
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost_ext import AccelBoostExt
 from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist
@@ -32,6 +33,16 @@ class LongitudinalPlannerSP:
 
     self.output_v_target = 0.
     self.output_a_target = 0.
+    self._accel_boost: AccelBoostExt | None = None
+
+  @property
+  def accel_boost(self) -> AccelBoostExt:
+    assert self._accel_boost is not None
+    return self._accel_boost
+
+  @accel_boost.setter
+  def accel_boost(self, controller) -> None:
+    self._accel_boost = AccelBoostExt(controller)
 
   def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
     CS = sm['carState']

openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_boost_ext.py

@@ -0,0 +1,242 @@
+"""
+Copyright (c) 2021-, 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 types import SimpleNamespace
+from unittest import mock
+
+from openpilot.cereal import log
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.controls.lib.accel_boost import AccelBoost, ACCEL_BOOST_MAX, ACCEL_BOOST_PER_OVERRIDE, ACCEL_BOOST_RATE
+from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost_ext import AccelBoostExt, get_accel_boost_max
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP
+
+E2E_ACCEL = 0.5
+PLANNER_ACCEL = 2.0
+STEPS_PER_OVERRIDE = round(ACCEL_BOOST_PER_OVERRIDE / (ACCEL_BOOST_RATE * DT_MDL))
+EXPECTED_SCALE = {
+  log.LongitudinalPersonality.relaxed: 0.3,
+  log.LongitudinalPersonality.standard: 0.5,
+  log.LongitudinalPersonality.aggressive: 0.7,
+}
+
+
+class FakeParams:
+  def __init__(self, enabled=True):
+    self.enabled = enabled
+
+  def get_bool(self, key):
+    return self.enabled
+
+
+class FakeSubMaster:
+  def __init__(self, enabled=True, gas_pressed=False, v_ego=15.0,
+               personality=log.LongitudinalPersonality.standard):
+    self._messages = {
+      'selfdriveState': SimpleNamespace(enabled=enabled, personality=personality),
+      'carState': SimpleNamespace(gasPressed=gas_pressed, vEgo=v_ego),
+    }
+
+  def __getitem__(self, key):
+    return self._messages[key]
+
+
+def update(boost, sm, e2e=E2E_ACCEL):
+  return boost.update(sm, e2e, PLANNER_ACCEL, PLANNER_ACCEL)
+
+
+class PlannerStub(LongitudinalPlannerSP):
+  def __init__(self):
+    self._accel_boost = None
+
+
+class TestAccelBoostExt(OpenpilotTestCase):
+  def setUp(self):
+    super().setUp()
+    self.params = FakeParams()
+    patcher = mock.patch("openpilot.sunnypilot.selfdrive.controls.lib.accel_boost_ext.Params", return_value=self.params)
+    patcher.start()
+    self.addCleanup(patcher.stop)
+
+  def _boost_to_max(self, personality):
+    boost = AccelBoostExt()
+    sm = FakeSubMaster(personality=personality)
+    # Eligibility is latched on a frame without gas, then applied on the next override.
+    update(boost, sm)
+    for _ in range(10):
+      sm['carState'].gasPressed = True
+      for _ in range(STEPS_PER_OVERRIDE):
+        update(boost, sm)
+      sm['carState'].gasPressed = False
+      update(boost, sm)
+    return boost, sm
+
+  def _applied_extra(self, boost, sm):
+    update(boost, sm)
+    return boost.extra_boost
+
+  def test_off_matches_stock_boost_exactly(self):
+    self.params.enabled = False
+    sm = FakeSubMaster()
+    stock = AccelBoost()
+    ext = AccelBoostExt()
+
+    stock_out = update(stock, sm)
+    ext_out = update(ext, sm)
+    self.assertAlmostEqual(ext_out, stock_out)
+
+    for _ in range(10):
+      sm['carState'].gasPressed = True
+      for _ in range(STEPS_PER_OVERRIDE):
+        stock_out = update(stock, sm)
+        ext_out = update(ext, sm)
+        self.assertAlmostEqual(ext_out, stock_out)
+      sm['carState'].gasPressed = False
+      stock_out = update(stock, sm)
+      ext_out = update(ext, sm)
+      self.assertAlmostEqual(ext_out, stock_out)
+
+    self.assertAlmostEqual(ext.total_boost, stock.total_boost)
+    self.assertAlmostEqual(ext._stock.total_boost, stock_out - E2E_ACCEL)
+    self.assertEqual(ext.extra_boost, 0.0)
+    self.assertEqual(ext._extra.total_boost, 0.0)
+
+  def test_no_boost_without_gas(self):
+    ext = AccelBoostExt()
+    sm = FakeSubMaster(gas_pressed=False)
+    out = update(ext, sm)
+    self.assertEqual(ext.total_boost, 0.0)
+    self.assertEqual(ext._extra.total_boost, 0.0)
+    self.assertAlmostEqual(out, E2E_ACCEL)
+
+  def test_boost_requires_e2e_to_be_limiting(self):
+    ext = AccelBoostExt()
+    sm = FakeSubMaster(gas_pressed=False)
+    ext.update(sm, E2E_ACCEL, E2E_ACCEL, E2E_ACCEL)
+    sm['carState'].gasPressed = True
+    for _ in range(10):
+      ext.update(sm, E2E_ACCEL, E2E_ACCEL, E2E_ACCEL)
+    self.assertEqual(ext.total_boost, 0.0)
+    self.assertEqual(ext._stock.total_boost, 0.0)
+    self.assertEqual(ext.extra_boost, 0.0)
+    self.assertEqual(ext._extra.total_boost, 0.0)
+
+  def test_extra_layer_per_override_cap(self):
+    ext = AccelBoostExt()
+    sm = FakeSubMaster(gas_pressed=False)
+    update(ext, sm)
+    sm['carState'].gasPressed = True
+    for _ in range(1000):
+      update(ext, sm)
+
+    self.assertAlmostEqual(ext._extra.total_boost, ACCEL_BOOST_PER_OVERRIDE)
+    applied = self._applied_extra(ext, sm)
+    expected = ACCEL_BOOST_PER_OVERRIDE * EXPECTED_SCALE[log.LongitudinalPersonality.standard]
+    self.assertAlmostEqual(applied, expected)
+
+  def test_extra_scales_with_following_distance(self):
+    applied = {}
+    for personality in (log.LongitudinalPersonality.relaxed,
+                        log.LongitudinalPersonality.standard,
+                        log.LongitudinalPersonality.aggressive):
+      ext, sm = self._boost_to_max(personality)
+      sm['carState'].gasPressed = True
+      applied[personality] = self._applied_extra(ext, sm)
+
+    for personality, value in applied.items():
+      self.assertAlmostEqual(value, ACCEL_BOOST_MAX * EXPECTED_SCALE[personality])
+
+    self.assertLess(applied[log.LongitudinalPersonality.relaxed], applied[log.LongitudinalPersonality.standard])
+    self.assertLess(applied[log.LongitudinalPersonality.standard], applied[log.LongitudinalPersonality.aggressive])
+
+  def test_capnp_dynamic_enum_personality_scales(self):
+    # Real SubMaster messages expose personality as a capnp DynamicEnum; equality still matches the enum value.
+    msg = log.SelfdriveState.new_message()
+    msg.personality = log.LongitudinalPersonality.aggressive
+    ext, sm = self._boost_to_max(msg.personality)
+    sm['carState'].gasPressed = True
+    applied = self._applied_extra(ext, sm)
+    self.assertAlmostEqual(applied, ACCEL_BOOST_MAX * EXPECTED_SCALE[log.LongitudinalPersonality.aggressive])
+
+  def test_extra_feeds_into_stock_last(self):
+    ext, sm = self._boost_to_max(log.LongitudinalPersonality.standard)
+    sm['carState'].gasPressed = True
+    inputs = []
+    original_update = AccelBoost.update
+
+    def spy(boost, sm_, e2e, mpc, cruise):
+      inputs.append(e2e)
+      return original_update(boost, sm_, e2e, mpc, cruise)
+
+    with mock.patch.object(AccelBoost, "update", spy):
+      out = update(ext, sm)
+
+    # First call is our extra layer on the raw target, last call is stock on the boosted target.
+    self.assertAlmostEqual(inputs[0], E2E_ACCEL)
+    self.assertAlmostEqual(inputs[-1], E2E_ACCEL + ext.extra_boost)
+    self.assertAlmostEqual(out, inputs[-1] + ext._stock.total_boost)
+
+  def test_extra_is_added_on_top_of_stock_boost(self):
+    ext, sm = self._boost_to_max(log.LongitudinalPersonality.aggressive)
+    sm['carState'].gasPressed = True
+    out = update(ext, sm)
+    extra = out - E2E_ACCEL - ext._stock.total_boost
+    self.assertAlmostEqual(ext._stock.total_boost, ACCEL_BOOST_MAX)
+    self.assertAlmostEqual(ext.total_boost, ACCEL_BOOST_MAX + ext.extra_boost)
+    self.assertAlmostEqual(ext._stock.total_boost, ACCEL_BOOST_MAX)
+    self.assertAlmostEqual(ext.extra_boost, ACCEL_BOOST_MAX * EXPECTED_SCALE[log.LongitudinalPersonality.aggressive])
+    self.assertAlmostEqual(extra, ext.extra_boost)
+
+  def test_sp_planner_wraps_assigned_stock_controller(self):
+    planner = PlannerStub()
+    stock = AccelBoost()
+    planner.accel_boost = stock
+
+    self.assertIsInstance(planner.accel_boost, AccelBoostExt)
+    self.assertIs(planner.accel_boost._stock, stock)
+
+  def test_wraps_existing_stock_controller(self):
+    stock = AccelBoost()
+    sm = FakeSubMaster()
+    update(stock, sm)
+    sm['carState'].gasPressed = True
+    update(stock, sm)
+
+    ext = AccelBoostExt(stock)
+    self.assertIs(ext._stock, stock)
+    self.assertAlmostEqual(ext.total_boost, stock.total_boost)
+
+  def test_boost_bar_max_matches_profile_cap(self):
+    for personality, scale in EXPECTED_SCALE.items():
+      self.assertAlmostEqual(get_accel_boost_max(personality), ACCEL_BOOST_MAX * (1.0 + scale))
+
+  def test_total_boost_is_stock_accumulated_plus_extra_applied(self):
+    ext, sm = self._boost_to_max(log.LongitudinalPersonality.aggressive)
+    sm['carState'].gasPressed = True
+    update(ext, sm)
+    self.assertAlmostEqual(ext.total_boost, ext._stock.total_boost + ext.extra_boost)
+
+  def test_turning_toggle_off_restores_stock_only(self):
+    ext, sm = self._boost_to_max(log.LongitudinalPersonality.aggressive)
+    self.assertGreater(ext._extra.total_boost, 0.0)
+
+    self.params.enabled = False
+    sm['carState'].gasPressed = True
+    # Params are re-read every PARAMS_UPDATE_PERIOD; the extra layer must clear on the next poll.
+    for _ in range(int(PARAMS_UPDATE_PERIOD / DT_MDL) + 1):
+      out = update(ext, sm)
+
+    self.assertEqual(ext._extra.total_boost, 0.0)
+    self.assertEqual(ext.extra_boost, 0.0)
+    self.assertAlmostEqual(out, E2E_ACCEL + ext._stock.total_boost)
+
+  def test_relaxing_following_distance_applies_immediately(self):
+    ext, sm = self._boost_to_max(log.LongitudinalPersonality.aggressive)
+    sm['selfdriveState'].personality = log.LongitudinalPersonality.relaxed
+    sm['carState'].gasPressed = True
+    applied = self._applied_extra(ext, sm)
+    self.assertAlmostEqual(applied, ACCEL_BOOST_MAX * EXPECTED_SCALE[log.LongitudinalPersonality.relaxed])

openpilot/sunnypilot/selfdrive/controls/lib/tests/test_auto_lane_change.py

@@ -8,7 +8,7 @@ from openpilot.common.parameterized import parameterized
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeState, LaneChangeDirection
 from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeController, AutoLaneChangeMode, \
-  AUTO_LANE_CHANGE_TIMER, ONE_SECOND_DELAY
+  AUTO_LANE_CHANGE_TIMER, ONE_SECOND_DELAY, EDGE_DELAY
 from openpilot.common.test import OpenpilotTestCase
 
 AUTO_LANE_CHANGE_TIMER_COMBOS = [
@@ -61,7 +61,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
     # Update controller
     num_updates = int(5.0 / DT_MDL)
     for _ in range(num_updates):  # Run for 5 seconds
-      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     # Mode should not allow lane change immediately
     assert not self.alc.auto_lane_change_allowed
@@ -74,11 +74,11 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
     self.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGELESS
 
     # Update controller once to read params
-    self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+    self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     # Update multiple times to exceed the timer threshold
     for _ in range(1):  # Should exceed 0.1s with multiple DT_MDL updates
-      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     # Now lane change should be allowed
     assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay
@@ -91,7 +91,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
     self.alc.lane_change_set_timer = timer_state
 
     # Update controller once
-    self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+    self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     # The timer should still be below the threshold after one update
     assert not self.alc.auto_lane_change_allowed
@@ -99,7 +99,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
     # Update enough times to exceed the threshold (seconds / DT_MDL)
     num_updates = int(timer_delay / DT_MDL) + 1  # Add one extra updates to ensure we exceed the threshold
     for _ in range(num_updates):
-      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     # Now lane change should be allowed
     assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay
@@ -116,7 +116,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
 
     # Update with brake pressed for 1 second
     for _ in range(num_updates):
-      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=True)
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=True)
 
     # Even though it is an auto lane change mode, lane change should be disallowed due to brake pressed prior initiating lane change
     assert not self.alc.auto_lane_change_allowed
@@ -126,7 +126,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
 
     # Even releasing brake shouldn't allow auto lane change
     for _ in range(num_updates):
-      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     assert not self.alc.auto_lane_change_allowed
 
@@ -139,13 +139,13 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
     self.alc.lane_change_set_timer = timer_state
 
     # Update with blindspot detected - this should prevent auto lane change
-    self.alc.update_lane_change(blindspot_detected=True, brake_pressed=False)
+    self.alc.update_lane_change(blindspot_detected=True, edge_detected=False, brake_pressed=False)
     assert not self.alc.auto_lane_change_allowed
 
     # Keep updating with blindspot detected - should still prevent auto lane change
     num_updates = int(timer_delay / DT_MDL) + 1  # Add one extra updates to ensure we exceed the threshold
     for _ in range(num_updates):
-      self.alc.update_lane_change(blindspot_detected=True, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=True, edge_detected=False, brake_pressed=False)
     assert not self.alc.auto_lane_change_allowed
 
   @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
@@ -157,13 +157,13 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
     self.alc.lane_change_set_timer = timer_state
 
     # First update with blindspot detected to set the negative timer
-    self.alc.update_lane_change(blindspot_detected=True, brake_pressed=False)
+    self.alc.update_lane_change(blindspot_detected=True, edge_detected=False, brake_pressed=False)
     assert not self.alc.auto_lane_change_allowed
 
     # Now update with blindspot cleared - should start incrementing timer from negative value
     num_updates = int((timer_delay + abs(ONE_SECOND_DELAY)) / DT_MDL) + 1
     for _ in range(num_updates):
-      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     # After sufficient updates with no blindspot, auto lane change should be allowed
     assert self.alc.auto_lane_change_allowed
@@ -177,7 +177,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
 
     # Update enough times to exceed the threshold (seconds / DT_MDL)
     for _ in range(num_updates):
-      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     # Now lane change should be allowed
     assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay
@@ -193,7 +193,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
 
     # Update enough times to exceed the threshold (seconds / DT_MDL)
     for _ in range(num_updates):
-      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     assert not self.alc.auto_lane_change_allowed
 
@@ -206,7 +206,86 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
     # Simulate updates for a long period of time (e.g., 10 seconds)
     num_updates = int(10.0 / DT_MDL)
     for _ in range(num_updates):
-      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     # Lane change should never be allowed
     assert not self.alc.auto_lane_change_allowed
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_edge_detected_delays_auto_lane_change(self, timer_state, timer_delay):
+    """Test that road edge detection adds delay to auto lane change."""
+    self._reset_states()
+    self.alc.lane_change_bsm_delay = False
+    self.alc.lane_change_set_timer = timer_state
+
+    # Update with edge detected - should prevent auto lane change
+    self.alc.update_lane_change(blindspot_detected=False, edge_detected=True, brake_pressed=False)
+    assert not self.alc.auto_lane_change_allowed
+
+    # Keep updating with edge detected - timer keeps resetting, should stay disallowed
+    num_updates = int(timer_delay / DT_MDL) + 1
+    for _ in range(num_updates):
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=True, brake_pressed=False)
+    assert not self.alc.auto_lane_change_allowed
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_edge_detected_then_cleared_allows_after_delay(self, timer_state, timer_delay):
+    """Test that auto lane change is allowed after edge clears plus extra delay."""
+    self._reset_states()
+    self.alc.lane_change_bsm_delay = False
+    self.alc.lane_change_set_timer = timer_state
+
+    # First update with edge detected to set the negative timer
+    self.alc.update_lane_change(blindspot_detected=False, edge_detected=True, brake_pressed=False)
+    assert not self.alc.auto_lane_change_allowed
+
+    # Now update with edge cleared - should start incrementing timer from negative value
+    num_updates = int((timer_delay + abs(EDGE_DELAY)) / DT_MDL) + 1
+    for _ in range(num_updates):
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
+
+    # After sufficient updates with no edge, auto lane change should be allowed
+    assert self.alc.auto_lane_change_allowed
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_edge_and_bsm_combined_uses_worst_delay(self, timer_state, timer_delay):
+    """Test that when both BSM and edge are detected, the worst (longest) delay wins."""
+    self._reset_states()
+    self.alc.lane_change_bsm_delay = True
+    self.alc.lane_change_set_timer = timer_state
+
+    # Update with both blindspot and edge detected
+    self.alc.update_lane_change(blindspot_detected=True, edge_detected=True, brake_pressed=False)
+    assert not self.alc.auto_lane_change_allowed
+
+    # Clear both - need to wait out the worst delay
+    worst_delay = max(abs(ONE_SECOND_DELAY), abs(EDGE_DELAY))
+    num_updates = int((timer_delay + worst_delay) / DT_MDL) + 1
+    for _ in range(num_updates):
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
+
+    assert self.alc.auto_lane_change_allowed
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_nudge_overrides_edge_in_lane_change_blocked(self, timer_state, timer_delay):
+    """Test that lane_change_blocked is False when nudge applied with edge-only."""
+    self._reset_states()
+    self.alc.lane_change_set_timer = timer_state
+
+    # Edge detected, no nudge - should be blocked
+    self.alc.update_lane_change(blindspot_detected=False, edge_detected=True, brake_pressed=False, torque_applied=False)
+    assert self.alc.lane_change_blocked
+
+    # Edge detected, nudge applied - should NOT be blocked
+    self.alc.update_lane_change(blindspot_detected=False, edge_detected=True, brake_pressed=False, torque_applied=True)
+    assert not self.alc.lane_change_blocked
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_nudge_does_not_override_bsm_in_lane_change_blocked(self, timer_state, timer_delay):
+    """Test that lane_change_blocked stays True when BSM detected even with nudge."""
+    self._reset_states()
+    self.alc.lane_change_set_timer = timer_state
+
+    # BSM detected with nudge - should still be blocked
+    self.alc.update_lane_change(blindspot_detected=True, edge_detected=False, brake_pressed=False, torque_applied=True)
+    assert self.alc.lane_change_blocked

openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py

@@ -113,12 +113,56 @@ class TestDesireHelperIntegration(OpenpilotTestCase):
                 left_edge_detected=False, right_edge_detected=False)
     assert dh.desire == expected_desire
 
-  def test_edge_blocks_lane_change(self, set_lane_turn_params):
+  def test_nudge_overrides_edge_detection(self, set_lane_turn_params):
+    """Manual steering nudge should override road edge detection and allow lane change."""
     dh = DesireHelper()
     dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
     carstate = DummyCarState(vEgo=15, leftBlinker=True, steeringPressed=True, steeringTorque=1)
     for _ in range(10):
       dh.update(carstate, True, 1.0, left_edge_detected=True, right_edge_detected=False)
-    assert dh.lane_change_state == LaneChangeState.preLaneChange
+    assert dh.lane_change_state == LaneChangeState.laneChangeStarting
     assert dh.lane_change_direction == LaneChangeDirection.left
+    assert dh.desire == log.Desire.laneChangeLeft
+
+  def test_nudge_does_not_override_bsm(self, set_lane_turn_params):
+    """Manual steering nudge must NOT override BSM blindspot detection."""
+    dh = DesireHelper()
+    dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
+    carstate = DummyCarState(vEgo=15, leftBlinker=True, leftBlindspot=True, steeringPressed=True, steeringTorque=1)
+    for _ in range(10):
+      dh.update(carstate, True, 1.0, left_edge_detected=False, right_edge_detected=False)
+    assert dh.lane_change_state == LaneChangeState.preLaneChange
     assert dh.desire == log.Desire.none
+
+  def test_edge_blocks_auto_lane_change(self, set_lane_turn_params):
+    """Road edge detection should still block auto (nudgeless) lane changes."""
+    dh = DesireHelper()
+    dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGELESS
+    dh.alc.param_read_counter = 1
+    carstate = DummyCarState(vEgo=15, leftBlinker=True)
+    for _ in range(10):
+      dh.update(carstate, True, 1.0, left_edge_detected=True, right_edge_detected=False)
+    assert dh.lane_change_state == LaneChangeState.preLaneChange
+    assert dh.desire == log.Desire.none
+
+  def test_auto_lane_change_allowed_without_edge(self, set_lane_turn_params):
+    """Auto lane change should proceed when no edge and no BSM detected."""
+    dh = DesireHelper()
+    dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGELESS
+    dh.alc.param_read_counter = 1
+    carstate = DummyCarState(vEgo=15, leftBlinker=True)
+    for _ in range(10):
+      dh.update(carstate, True, 1.0, left_edge_detected=False, right_edge_detected=False)
+    assert dh.lane_change_state == LaneChangeState.laneChangeStarting
+    assert dh.desire == log.Desire.laneChangeLeft
+
+  def test_right_edge_does_not_block_left_lane_change(self, set_lane_turn_params):
+    """Edge on opposite side should not affect lane change direction."""
+    dh = DesireHelper()
+    dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGELESS
+    dh.alc.param_read_counter = 1
+    carstate = DummyCarState(vEgo=15, leftBlinker=True)
+    for _ in range(10):
+      dh.update(carstate, True, 1.0, left_edge_detected=False, right_edge_detected=True)
+    assert dh.lane_change_state == LaneChangeState.laneChangeStarting
+    assert dh.desire == log.Desire.laneChangeLeft

openpilot/sunnypilot/selfdrive/controls/tests/test_controlsd_ext.py

@@ -0,0 +1,47 @@
+import unittest
+
+from opendbc.car import structs
+from openpilot.sunnypilot.selfdrive.controls.controlsd_ext import ControlsExt
+
+
+class _TorqueLikeTuning:
+  def which(self) -> str:
+    return "torque"
+
+
+class _CPStub:
+  def __init__(self, steer_control_type):
+    self.steerControlType = steer_control_type
+    self.lateralTuning = _TorqueLikeTuning()
+
+
+class _ParamsStub:
+  """Fails loudly if the torque override logic is reached at all."""
+
+  def get_bool(self, key: str) -> bool:
+    raise AssertionError(f"params must not be read for non-torque cars, got {key}")
+
+  def get(self, key: str, **kwargs) -> str:
+    raise AssertionError(f"params must not be read for non-torque cars, got {key}")
+
+
+def _make_ext(steer_control_type) -> ControlsExt:
+  ext = ControlsExt.__new__(ControlsExt)
+  ext.CP = _CPStub(steer_control_type)
+  ext.params = _ParamsStub()
+  return ext
+
+
+class TestInitializeLateralControl(unittest.TestCase):
+  """A torque controller would log a LateralTorqueState, which controlsd cannot publish into
+  angleState/curvatureState (capnp type mismatch) and crashes controlsd."""
+
+  def test_angle_car_keeps_angle_controller(self):
+    lac = object()
+    ext = _make_ext(structs.CarParams.SteerControlType.angle)
+    self.assertIs(ControlsExt.initialize_lateral_control(ext, lac, None, 0.01), lac)
+
+  def test_curvature_car_keeps_curvature_controller(self):
+    lac = object()
+    ext = _make_ext(structs.CarParams.SteerControlType.curvature)
+    self.assertIs(ControlsExt.initialize_lateral_control(ext, lac, None, 0.01), lac)

openpilot/sunnypilot/sunnylink/capabilities.py

@@ -10,12 +10,13 @@ from openpilot.cereal import custom, messaging
 from opendbc.car.structs import car
 from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR, UNSUPPORTED_LONGITUDINAL_CAR
 from opendbc.car.subaru.values import CAR as SUBARU_CAR, SubaruFlags
+from opendbc.car.toyota.values import CAR as TOYOTA_CAR, ToyotaFlags
 from opendbc.sunnypilot.car.interfaces import has_bsm
 from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.hardware import HARDWARE
-
+from openpilot.sunnypilot.models.helpers import get_active_source, ACTIVE_BUNDLE_KEYS
 
 # Wire-protocol version for the capabilities payload. Bump on breaking changes
 # only; additive fields are backward-compatible and do not require a bump.
@@ -43,6 +44,9 @@ CAPABILITY_FIELDS = (
   "device_type",
   "subaru_has_sng",
   "hyundai_alpha_long_available",
+  "custom_model_active",
+  "toyota_enhanced_bsm_available",
+  "toyota_angle_steering_available",
 )
 
 CAPABILITY_LABELS: dict[str, str] = {
@@ -65,6 +69,9 @@ CAPABILITY_LABELS: dict[str, str] = {
   "device_type": "Device type",
   "subaru_has_sng": "Subaru Stop-and-Go available",
   "hyundai_alpha_long_available": "Hyundai Alpha Longitudinal available",
+  "custom_model_active": "Custom model selected",
+  "toyota_enhanced_bsm_available": "Toyota Enhanced BSM available",
+  "toyota_angle_steering_available": "Toyota LTA angle steering available",
 }
 
 # Explicit defaults for non-boolean capability fields
@@ -111,6 +118,18 @@ def _resolve_brand_capabilities(caps: dict, bundle_platform: str, CP) -> None:
       caps["subaru_has_sng"] = not bool(CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID))
       caps["has_stop_and_go"] = caps["subaru_has_sng"]
 
+  elif brand == "toyota":
+    if bundle_platform:
+      try:
+        flags = TOYOTA_CAR[bundle_platform].config.flags
+        caps["toyota_enhanced_bsm_available"] = bool(flags & ToyotaFlags.TSS2) and not bool(flags & ToyotaFlags.SECOC)
+        caps["toyota_angle_steering_available"] = bundle_platform == TOYOTA_CAR.TOYOTA_PRIUS_TSS2.value
+      except KeyError:
+        cloudlog.exception(f"capabilities: unknown toyota platform {bundle_platform!r}")
+    elif CP is not None:
+      caps["toyota_enhanced_bsm_available"] = bool(CP.flags & ToyotaFlags.TSS2) and not bool(CP.flags & ToyotaFlags.SECOC)
+      caps["toyota_angle_steering_available"] = CP.carFingerprint == TOYOTA_CAR.TOYOTA_PRIUS_TSS2
+
 
 def generate_capabilities(params: Params | None = None) -> dict:
   """Generate a SettingsCapabilities dict from CarParams + boolean params.
@@ -133,6 +152,12 @@ def generate_capabilities(params: Params | None = None) -> dict:
   caps["is_development"] = params.get_bool("IsDevelopmentBranch")
   caps["stock_longitudinal"] = params.get_bool("ToyotaEnforceStockLongitudinal")
 
+  # Model source based custom model flag
+  model_source = get_active_source(chestnut_active=params.get_bool("ChestnutActive"),
+                                   chestnut_loading=params.get_bool("ChestnutLoading"),
+                                   offroad=params.get_bool("IsOffroad"))
+  caps["custom_model_active"] = bool(params.get(ACTIVE_BUNDLE_KEYS[model_source]))
+
   bundle = params.get("CarPlatformBundle")
   bundle_brand = _bundle_field(bundle, "brand")
   bundle_platform = _bundle_field(bundle, "platform")

openpilot/sunnypilot/sunnylink/docs/README.md

@@ -156,7 +156,7 @@ The tables below describe the **compiled** `settings_ui.json` schema — what th
 
 **Enablement rules**: Grayed out (disabled) when rules fail. Frontend shows a contextual badge explaining why.
 
-**Capability fields** (referenced in rules): `has_longitudinal_control`, `has_icbm`, `icbm_available`, `torque_allowed`, `brand`, `pcm_cruise`, `alpha_long_available`, `steer_control_type`, `has_bsm`, `is_release`, `is_sp_release`, `is_development`, `tesla_has_vehicle_bus`, `has_stop_and_go`, `stock_longitudinal`
+**Capability fields** (referenced in rules): `has_longitudinal_control`, `has_icbm`, `icbm_available`, `torque_allowed`, `brand`, `pcm_cruise`, `alpha_long_available`, `steer_control_type`, `has_bsm`, `is_release`, `is_sp_release`, `is_development`, `tesla_has_vehicle_bus`, `has_stop_and_go`, `stock_longitudinal`, `toyota_enhanced_bsm_available`
 
 ---
 

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -620,6 +620,19 @@
                 }
               ]
             },
+            {
+              "key": "FollowDistanceAccelBoost",
+              "widget": "toggle",
+              "title": "Extra Acceleration Boost by Following Distance",
+              "description": "Adds extra acceleration on top of the stock gas-override boost. The extra boost follows the Following Distance setting, strongest when Aggressive and weakest when Relaxed. The stock boost remains active when this is off.",
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ]
+            },
             {
               "key": "IntelligentCruiseButtonManagement",
               "widget": "toggle",
@@ -2021,8 +2034,21 @@
         {
           "id": "model_behavior",
           "title": "Model Behavior",
-          "description": "Lane desire and lead-vehicle awareness tuning",
+          "description": "Lane desire, Blendful lane centering, and lead-vehicle awareness tuning",
           "items": [
+            {
+              "key": "Blendful",
+              "widget": "toggle",
+              "title": "Blendful Mode (Alpha)",
+              "description": "Gently pulls the model toward the lane center, as much as it trusts the lane lines, and yields around obstacles.",
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "custom_model_active",
+                  "equals": true
+                }
+              ]
+            },
             {
               "key": "LaneTurnDesire",
               "widget": "toggle",
@@ -2324,6 +2350,57 @@
               "equals": false
             }
           ]
+        },
+        {
+          "key": "ToyotaEnhancedBsm",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Enhanced Blind Spot Monitoring (Alpha)",
+          "description": "sunnypilot will poll the blind spot monitor diagnostics directly from the radar. Use this on vehicles whose blind spot monitors do not report blind spot status over the standard CAN signals. This is an alpha feature. Use at your own risk.",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            },
+            {
+              "type": "capability",
+              "field": "toyota_enhanced_bsm_available",
+              "equals": true
+            },
+            {
+              "type": "capability",
+              "field": "stock_longitudinal",
+              "equals": false
+            }
+          ]
+        },
+        {
+          "key": "ToyotaAngleSteering",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "LTA Angle Steering (Alpha)",
+          "description": "sunnypilot will command steering angle through Toyota Lane Tracing Assist (LTA) instead of steering torque. Intended for the Toyota Prius/Prime 2021-22 (TSS2). This is an alpha feature. Use at your own risk.",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            }
+          ]
+        },
+        {
+          "key": "ToyotaAngleSteeringHighOutput",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "LTA Angle Steering High-Output (Alpha)",
+          "description": "sunnypilot will use the wider angle envelope of an EPS patched for high-output mode, with up to 180 deg of target angle, a 250 deg/s angle rate and a 3200 torque monitor. Requires patched EPS firmware, the stock EPS only accepts 94.9461 deg. This is an alpha feature. Use at your own risk.",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            },
+            {
+              "type": "param",
+              "key": "ToyotaAngleSteering",
+              "equals": true
+            }
+          ]
         }
       ]
     }

openpilot/sunnypilot/sunnylink/settings_ui_src/_macros.yaml

@@ -63,3 +63,7 @@ macros:
   release_branches_hide:
     - type: not
       condition: {type: capability, field: is_sp_release, equals: true}
+
+  # Gate for custom model features based on active source.
+  custom_model:
+    - { type: capability, field: custom_model_active, equals: true }

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml

@@ -30,6 +30,14 @@ sections:
       label: Relaxed
     enablement:
     - $ref: '#/macros/longitudinal'
+  - key: FollowDistanceAccelBoost
+    widget: toggle
+    title: Extra Acceleration Boost by Following Distance
+    description: Adds extra acceleration on top of the stock gas-override boost. The extra boost follows
+      the Following Distance setting, strongest when Aggressive and weakest when Relaxed. The stock boost
+      remains active when this is off.
+    enablement:
+    - $ref: '#/macros/longitudinal'
   - key: IntelligentCruiseButtonManagement
     widget: toggle
     title: Intelligent Cruise Button Management (ICBM) (Alpha)

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml

@@ -9,8 +9,14 @@ description: Driving model behavior and camera calibration
 sections:
 - id: model_behavior
   title: Model Behavior
-  description: Lane desire and lead-vehicle awareness tuning
+  description: Lane desire, Blendful lane centering, and lead-vehicle awareness tuning
   items:
+  - key: Blendful
+    widget: toggle
+    title: Blendful Mode (Alpha)
+    description: Gently pulls the model toward the lane center, as much as it trusts the lane lines, and yields around obstacles.
+    enablement:
+      - $ref: '#/macros/custom_model'
   - key: LaneTurnDesire
     widget: toggle
     title: Use Lane Turn Desires

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml

@@ -101,3 +101,38 @@ sections:
     - type: param
       key: ToyotaEnforceStockLongitudinal
       equals: false
+  - key: ToyotaEnhancedBsm
+    widget: toggle
+    needs_onroad_cycle: true
+    title: Enhanced Blind Spot Monitoring (Alpha)
+    description: sunnypilot will poll the blind spot monitor diagnostics directly from the radar. Use this on vehicles whose
+      blind spot monitors do not report blind spot status over the standard CAN signals. This is an alpha feature. Use at
+      your own risk.
+    enablement:
+    - $ref: '#/macros/not_engaged'
+    - type: capability
+      field: toyota_enhanced_bsm_available
+      equals: true
+    - type: capability
+      field: stock_longitudinal
+      equals: false
+  - key: ToyotaAngleSteering
+    widget: toggle
+    needs_onroad_cycle: true
+    title: LTA Angle Steering (Alpha)
+    description: sunnypilot will command steering angle through Toyota Lane Tracing Assist (LTA) instead of steering torque.
+      Intended for the Toyota Prius/Prime 2021-22 (TSS2). This is an alpha feature. Use at your own risk.
+    enablement:
+    - $ref: '#/macros/not_engaged'
+  - key: ToyotaAngleSteeringHighOutput
+    widget: toggle
+    needs_onroad_cycle: true
+    title: LTA Angle Steering High-Output (Alpha)
+    description: sunnypilot will use the wider angle envelope of an EPS patched for high-output mode, with up to 180 deg
+      of target angle, a 250 deg/s angle rate and a 3200 torque monitor. Requires patched EPS firmware, the stock EPS only
+      accepts 94.9461 deg. This is an alpha feature. Use at your own risk.
+    enablement:
+    - $ref: '#/macros/not_engaged'
+    - type: param
+      key: ToyotaAngleSteering
+      equals: true

openpilot/sunnypilot/sunnylink/tests/test_capabilities.py

@@ -69,12 +69,24 @@ class TestOpaquePerBrandFlags(OpenpilotTestCase):
   def test_hyundai_alpha_long_available_field_present(self):
     assert "hyundai_alpha_long_available" in CAPABILITY_FIELDS
 
+  def test_toyota_enhanced_bsm_available_field_present(self):
+    assert "toyota_enhanced_bsm_available" in CAPABILITY_FIELDS
+
+  def test_toyota_angle_steering_available_field_present(self):
+    assert "toyota_angle_steering_available" in CAPABILITY_FIELDS
+
   def test_subaru_has_sng_default_false(self, caps):
     assert caps["subaru_has_sng"] is False
 
   def test_hyundai_alpha_long_available_default_false(self, caps):
     assert caps["hyundai_alpha_long_available"] is False
 
+  def test_toyota_enhanced_bsm_available_default_false(self, caps):
+    assert caps["toyota_enhanced_bsm_available"] is False
+
+  def test_toyota_angle_steering_available_default_false(self, caps):
+    assert caps["toyota_angle_steering_available"] is False
+
 
 class TestCapabilitiesShape(OpenpilotTestCase):
   def test_all_fields_present(self, caps):

openpilot/sunnypilot/sunnylink/tests/test_settings_changes.py

@@ -207,6 +207,9 @@ class TestNotEngagedReplacement(OpenpilotTestCase):
     "AlphaLongitudinalEnabled",
     "ToyotaEnforceStockLongitudinal",
     "ToyotaStopAndGoHack",
+    "ToyotaEnhancedBsm",
+    "ToyotaAngleSteering",
+    "ToyotaAngleSteeringHighOutput",
   ], names=["key"])
   def test_offroad_only_replaced_with_not_engaged(self, schema, key):
     """These items should use not_engaged, not offroad_only."""
@@ -215,3 +218,17 @@ class TestNotEngagedReplacement(OpenpilotTestCase):
     rule_types = _flatten_rule_types(item.get("enablement"))
     assert "offroad_only" not in rule_types, f"{key} still uses offroad_only"
     assert "not_engaged" in rule_types, f"{key} missing not_engaged"
+
+
+class TestToyotaAngleSteeringNoVehicleGate(OpenpilotTestCase):
+  @parameterized.expand([
+    "ToyotaAngleSteering",
+    "ToyotaAngleSteeringHighOutput",
+  ], names=["key"])
+  def test_not_gated_on_vehicle_platform(self, schema, key):
+    """Angle steering is alpha: it must stay toggleable regardless of platform detection."""
+    item = _find_item(schema, key)
+    assert item is not None, f"{key} not found"
+    rules = (item.get("visibility") or []) + (item.get("enablement") or [])
+    assert not _references_capability_field(rules, "toyota_angle_steering_available"), f"{key} still gated on car platform"
+    assert not _references_capability_field(rules, "brand"), f"{key} still gated on brand"

openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py

@@ -286,6 +286,9 @@ class TestKnownVehicleSettings(OpenpilotTestCase):
     keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("toyota"))}
     assert "ToyotaEnforceStockLongitudinal" in keys
     assert "ToyotaStopAndGoHack" in keys
+    assert "ToyotaEnhancedBsm" in keys
+    assert "ToyotaAngleSteering" in keys
+    assert "ToyotaAngleSteeringHighOutput" in keys
 
   def test_tesla_has_coop_steering(self, schema):
     keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("tesla"))}