simple

* alert-dedup

* Update selfdrived.py

* Update selfdrived.py

* one liner

---------

Co-authored-by: James Vecellio-Grant <159560811+Discountchubbs@users.noreply.github.com>

.github/workflows/build-default-models.yaml

@@ -47,6 +47,7 @@ jobs:
           if [ "${{ inputs.target }}" = "big" ]; then
             NAME=$(python3 -c "from openpilot.sunnypilot.models.model_name import DEFAULT_BIG_MODEL; print(DEFAULT_BIG_MODEL)")
             HF_DEFAULTS_PATH="models/defaults/big"
+            ONNX_PATH="openpilot/selfdrive/modeld/models/big_driving_tinygrad.pkl"
           elif [ "${{ inputs.target }}" = "dm" ]; then
             ONNX_PATH="openpilot/selfdrive/modeld/models/dmonitoring_model.onnx"
             HF_DEFAULTS_PATH="models/defaults/dm"
@@ -185,7 +186,6 @@ jobs:
       - uses: actions/checkout@v4
         with:
           submodules: recursive
-          lfs: true
 
       - name: Pull big PKL via LFS
         run: git lfs pull -I "${{ env.BIG_PKL }}"
@@ -235,6 +235,34 @@ jobs:
           print(f'Chunked into {len(targets)} files')
           "
 
+      - name: Compile big warp
+        run: |
+          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
+          export PYTHONPATH="${PYTHONPATH}:${{ github.workspace }}/tinygrad_repo:${{ github.workspace }}"
+
+          CHESTNUT_TG_FLAGS="DEBUG=1 DEV=USB+AMD:LLVM FRAME_DEV=CPU FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0 TC_OPT=2 TC_MIN_GLOBALS=32"
+          COMPILER="${{ github.workspace }}/tinygrad_repo/examples/openpilot"
+          MODELS_DIR="${{ github.workspace }}/openpilot/selfdrive/modeld/models"
+          MODEL_SIZE=$(python3 -c "from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE as s; print(f'{s[0]}x{s[1]}')")
+
+          for res in $(python3 -c "from openpilot.common.transformations.camera import _ar_ox_fisheye as a, _os_fisheye as o; print(f'{a.width}x{a.height} {o.width}x{o.height}')"); do
+            CAM_W=$(echo $res | cut -d x -f1)
+            CAM_H=$(echo $res | cut -d x -f2)
+            STRIDE_INFO=$(python3 -c "
+          import sys
+          from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
+          s,y,u,sz = get_nv12_info(int(sys.argv[1]), int(sys.argv[2]))
+          print(f'{s},{y},{u},{sz}')
+          " $CAM_W $CAM_H)
+            WARP_PKL="${MODELS_DIR}/big_driving_warp_${res}_tinygrad.pkl"
+            env ${CHESTNUT_TG_FLAGS} python3 "${COMPILER}/compile_warp.py" \
+              --frame ${CAM_W},${CAM_H},${STRIDE_INFO} \
+              --warp-to ${MODEL_SIZE} \
+              --layout yuv420 \
+              --frames 2 \
+              --output "${WARP_PKL}"
+          done
+
       - name: Prepare output
         env:
           MODEL_NAME: ${{ needs.resolve.outputs.safe_model_name }}
@@ -249,7 +277,7 @@ jobs:
           cp "$MODELS_DIR/${PKL_BASE}".chunk* "$OUTPUT_DIR/"
           cp "$MODELS_DIR/${PKL_BASE}.chunkmanifest" "$OUTPUT_DIR/"
 
-          PKL_LFS_SHA256=$(sha256sum "$MODELS_DIR/${PKL_BASE}" | cut -d' ' -f1)
+          PKL_LFS_SHA256=$(git cat-file blob "$(git ls-files -s "${{ github.workspace }}/${{ env.BIG_PKL }}" | awk '{print $2}')" | grep '^oid sha256:' | cut -d: -f2)
 
           python3 "${{ github.workspace }}/release/ci/model_generator.py" \
             --model-dir "$MODELS_DIR" \
@@ -258,6 +286,8 @@ jobs:
             --upstream-branch "${{ needs.resolve.outputs.onnx_ref }}" \
             --onnx-sha256 "$PKL_LFS_SHA256"
 
+          cp "$MODELS_DIR"/big_driving_warp_* "$OUTPUT_DIR/" || true
+
           echo "model-${MODEL_NAME}-${{ github.run_number }}" > "$OUTPUT_DIR/artifact_name.txt"
 
       - name: Upload big model artifact
@@ -335,6 +365,8 @@ jobs:
             --hf-defaults-path "${{ needs.resolve.outputs.hf_defaults_path }}" \
             --artifact-name "$ARTIFACT_NAME" \
             --model-dir output \
+            --onnx-path "${{ needs.resolve.outputs.onnx_path }}" \
+            --onnx-ref "${{ needs.resolve.outputs.onnx_ref }}" \
             --model-name "${{ needs.resolve.outputs.model_name }}" \
             --tinygrad-ref "${{ needs.resolve.outputs.tinygrad_ref }}" \
             --run-number "${{ github.run_number }}"
@@ -514,4 +546,3 @@ jobs:
         run: |
           source ${UV_PROJECT_ENVIRONMENT}/bin/activate
           PYTHONPATH=$PYTHONPATH:${{ github.workspace }}/ ${{ github.workspace }}/scripts/manage-powersave.py --enable
-

.github/workflows/download-hf-model-chunks/action.yml

@@ -71,6 +71,11 @@ runs:
               ENCODED_URL=$(python3 -c "import urllib.parse; print(urllib.parse.quote('${BASE_URL}/${warp}', safe=':/'))")
               printf '%s\t%s\n' "$ENCODED_URL" "${DEST_DIR}/${warp}" >> "$DOWNLOAD_LIST"
             done
+          elif [ "$CANONICAL" = "big_driving_tinygrad.pkl" ]; then
+            for warp in big_driving_warp_1928x1208_tinygrad.pkl big_driving_warp_1344x760_tinygrad.pkl; do
+              ENCODED_URL=$(python3 -c "import urllib.parse; print(urllib.parse.quote('${BASE_URL}/${warp}', safe=':/'))")
+              printf '%s\t%s\n' "$ENCODED_URL" "${DEST_DIR}/${warp}" >> "$DOWNLOAD_LIST"
+            done
           fi
         }
 

openpilot/cereal/custom.capnp

@@ -196,7 +196,6 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
 }
 
 struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
-  dec @0 :DynamicExperimentalControl;
   longitudinalPlanSource @1 :LongitudinalPlanSource;
   smartCruiseControl @2 :SmartCruiseControl;
   speedLimit @3 :SpeedLimit;
@@ -205,7 +204,11 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   events @6 :List(OnroadEventSP.Event);
   e2eAlerts @7 :E2eAlerts;
 
-  struct DynamicExperimentalControl {
+  deprecated :group {
+    dec @0 :DynamicExperimentalControlDEPRECATED;
+  }
+
+  struct DynamicExperimentalControlDEPRECATED {
     state @0 :DynamicExperimentalControlState;
     enabled @1 :Bool;
     active @2 :Bool;

openpilot/common/params_keys.h

@@ -240,7 +240,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"ToyotaEnforceStockLongitudinal", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ToyotaStopAndGoHack", {PERSISTENT | BACKUP, BOOL, "0"}},
 
-    {"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
 
     // sunnypilot model params
@@ -250,6 +249,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"}},
+    {"LanefulStrength", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
     {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
 
     // mapd

openpilot/selfdrive/car/card.py

@@ -127,9 +127,6 @@ class Car:
     set_alternative_experience(self.CP, self.CP_SP, self.params)
     set_car_specific_params(self.CP, self.CP_SP, self.params)
 
-    # Dynamic Experimental Control
-    self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl")
-
     openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle")
     controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly
     self.CP.passive = not controller_available or self.CP.dashcamOnly
@@ -214,7 +211,7 @@ class Car:
     self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric)
     if self.sm['carControl'].enabled and not self.CC_prev.enabled:
       # Use CarState w/ buttons from the step selfdrived enables on
-      self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode, self.dynamic_experimental_control)
+      self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode)
 
     # TODO: mirror the carState.cruiseState struct?
     CS.vCruise = float(self.v_cruise_helper.v_cruise_kph)
@@ -302,7 +299,6 @@ class Car:
       self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
 
       # sunnypilot
-      self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl")
       self.v_cruise_helper.read_custom_set_speed_params()
 
       time.sleep(0.1)

openpilot/selfdrive/car/cruise.py

@@ -138,13 +138,12 @@ class VCruiseHelper(VCruiseHelperSP):
         self.button_timers[b.type.raw] = 1 if b.pressed else 0
         self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled}
 
-  def initialize_v_cruise(self, CS, experimental_mode: bool, dynamic_experimental_control: bool) -> None:
+  def initialize_v_cruise(self, CS, experimental_mode: bool) -> None:
     # initializing is handled by the PCM
     if self.CP.pcmCruise:
       return
 
-    initial_experimental_mode = experimental_mode and not dynamic_experimental_control
-    initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if initial_experimental_mode else V_CRUISE_INITIAL
+    initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL
 
     if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_initialized:
       self.v_cruise_kph = self.v_cruise_kph_last

openpilot/selfdrive/car/tests/test_cruise_speed.py

@@ -59,16 +59,16 @@ class TestVCruiseHelper(OpenpilotTestCase):
     for _ in range(2):
       self.v_cruise_helper.update_v_cruise(car.CarState(cruiseState={"available": False}), enabled=False, is_metric=False)
 
-  def enable(self, v_ego, experimental_mode, dynamic_experimental_control):
+  def enable(self, v_ego, experimental_mode):
     # Simulates user pressing set with a current speed
-    self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode, dynamic_experimental_control)
+    self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode)
 
   def test_adjust_speed(self):
     """
     Asserts speed changes on falling edges of buttons.
     """
 
-    self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False)
+    self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
 
     for btn in (ButtonType.accelCruise, ButtonType.decelCruise):
       for pressed in (True, False):
@@ -92,7 +92,7 @@ class TestVCruiseHelper(OpenpilotTestCase):
       CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=pressed)]
       self.v_cruise_helper.update_v_cruise(CS, enabled=enabled, is_metric=False)
       if pressed:
-        self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False)
+        self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
 
       # Expected diff on enabling. Speed should not change on falling edge of pressed
       assert (not pressed) == (self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last)
@@ -102,7 +102,7 @@ class TestVCruiseHelper(OpenpilotTestCase):
     Asserts we don't increment set speed if user presses resume/accel to exit cruise standstill.
     """
 
-    self.enable(0, False, False)
+    self.enable(0, False)
 
     for standstill in (True, False):
       for pressed in (True, False):
@@ -122,7 +122,7 @@ class TestVCruiseHelper(OpenpilotTestCase):
 
     for v_ego in np.linspace(0, 100, 101):
       self.reset_cruise_speed_state()
-      self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False)
+      self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
 
       # first decrement speed, then perform gas pressed logic
       expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT
@@ -144,11 +144,10 @@ class TestVCruiseHelper(OpenpilotTestCase):
     """
 
     for experimental_mode in (True, False):
-      for dynamic_experimental_control in (True, False):
-        for v_ego in np.linspace(0, 100, 101):
-          self.reset_cruise_speed_state()
-          assert not self.v_cruise_helper.v_cruise_initialized
+      for v_ego in np.linspace(0, 100, 101):
+        self.reset_cruise_speed_state()
+        assert not self.v_cruise_helper.v_cruise_initialized
 
-          self.enable(float(v_ego), experimental_mode, dynamic_experimental_control)
-          assert V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX
-          assert self.v_cruise_helper.v_cruise_initialized
+        self.enable(float(v_ego), experimental_mode)
+        assert V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX
+        assert self.v_cruise_helper.v_cruise_initialized

openpilot/selfdrive/controls/lib/desire_helper.py

@@ -64,19 +64,16 @@ 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 and self.lane_change_direction == LaneChangeDirection.left) or
-                              (carstate.rightBlindspot 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))
 
-        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)
+        self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
 
         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 self.alc.lane_change_blocked:
+        elif (torque_applied or self.alc.auto_lane_change_allowed) and not blindspot_detected:
           self.lane_change_state = LaneChangeState.laneChangeStarting
           self.lane_change_timer = 0.0
 

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -138,16 +138,16 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
-    is_e2e = self.is_e2e(sm)
+    experimental_mode = sm['selfdriveState'].experimentalMode
 
-    self.a_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego,
+    self.a_cruise = get_cruise_accel(experimental_mode, v_cruise, v_ego,
                                      self.a_cruise, steer_angle_without_offset, self.CP, self.dt,
                                      accel_coast, self.allow_throttle)
     cruise_should_stop = should_stop(v_ego, self.a_cruise)
 
     candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
                   (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)]
-    if is_e2e:
+    if experimental_mode:
       candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e))
 
     output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])

openpilot/selfdrive/selfdrived/selfdrived.py

@@ -577,7 +577,7 @@ class SelfdriveD(CruiseHelper):
     clear_event_types = set()
     if ET.WARNING not in self.state_machine.current_alert_types:
       clear_event_types.add(ET.WARNING)
-    if self.enabled:
+    if self.enabled or (ET.NO_ENTRY not in (c := self.state_machine.current_alert_types) and (ET.ENABLE in c or ET.USER_DISABLE in c)):
       clear_event_types.add(ET.NO_ENTRY)
 
     pers = LONGITUDINAL_PERSONALITY_MAP[self.personality]

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

@@ -181,6 +181,8 @@ class AugmentedRoadView(CameraView):
   def _handle_mouse_release(self, mouse_pos: MousePos):
     # Don't trigger click callback if bookmark was triggered
     if not self._bookmark_icon.interacting():
+      if self._model_renderer.laneful.handle_tap(mouse_pos):
+        return
       super()._handle_mouse_release(mouse_pos)
 
   def _render(self, _):

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

@@ -153,6 +153,7 @@ class ModelRenderer(Widget, ModelRendererSP):
     if ui_state.status != UIStatus.DISENGAGED:
       self._draw_lane_lines()
       self._draw_path(sm)
+      self.laneful.draw(self._rect)
 
     # if render_lead_indicator and radar_state:
     #   self._draw_lead_indicator()
@@ -204,6 +205,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.laneful.update(self, max_idx)
 
     # Update path using raw points
     if lead and lead.present:

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

@@ -82,14 +82,8 @@ class CruiseLayout(Widget):
       callback=lambda: self._set_current_panel(PanelType.SLA)
     )
 
-    self.dec_toggle = toggle_item_sp(
-      title=tr("Enable Dynamic Experimental Control"),
-      description=tr("Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal."),
-      param="DynamicExperimentalControl")
-
     items = [
       self.icbm_toggle,
-      self.dec_toggle,
       self.scc_v_toggle,
       self.scc_m_toggle,
       self.custom_acc_toggle,
@@ -144,16 +138,13 @@ class CruiseLayout(Widget):
 
       if has_long or has_icbm:
         self.custom_acc_toggle.action_item.set_enabled(((has_long and not ui_state.CP.pcmCruise) or has_icbm) and ui_state.is_offroad())
-        self.dec_toggle.action_item.set_enabled(has_long)
         self.scc_v_toggle.action_item.set_enabled(True)
         self.scc_m_toggle.action_item.set_enabled(True)
       else:
         ui_state.params.remove("CustomAccIncrementsEnabled")
-        ui_state.params.remove("DynamicExperimentalControl")
         ui_state.params.remove("SmartCruiseControlVision")
         ui_state.params.remove("SmartCruiseControlMap")
         self.custom_acc_toggle.action_item.set_enabled(False)
-        self.dec_toggle.action_item.set_enabled(False)
         self.scc_v_toggle.action_item.set_enabled(False)
         self.scc_m_toggle.action_item.set_enabled(False)
 

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

@@ -0,0 +1,39 @@
+"""
+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.selfdrive.ui.mici.widgets.button import BigMultiToggle
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.system.ui.widgets.scroller import NavScroller
+
+# above 1.0 the pull overshoots lane center and ping-pongs
+STRENGTHS = {"off": 0.0, "gentle": 0.5, "firm": 1.0}
+
+
+class LanefulStrengthToggle(BigMultiToggle):
+  """Tap to cycle LanefulStrength, like the driving personality toggle."""
+  def __init__(self):
+    super().__init__("laneful strength", list(STRENGTHS), select_callback=self._on_select,
+                     description="Nudges the car toward lane center.\n" +
+                                 "Firm centers faster than gentle.")
+    self.refresh()
+
+  def refresh(self):
+    strength = ui_state.params.get("LanefulStrength", return_default=True)
+    self.set_value(min(STRENGTHS, key=lambda k: abs(STRENGTHS[k] - strength)))
+
+  def _on_select(self, value: str):
+    ui_state.params.put("LanefulStrength", STRENGTHS[value])
+
+
+class LanefulLayoutMici(NavScroller):
+  def __init__(self):
+    super().__init__()
+    self._strength = LanefulStrengthToggle()
+    self._scroller.add_widget(self._strength)
+
+  def show_event(self):
+    super().show_event()
+    self._strength.refresh()

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

@@ -11,6 +11,7 @@ from openpilot.selfdrive.ui.mici.widgets.button import BigCircleButton
 from openpilot.selfdrive.ui.mici.widgets.dialog import BigConfirmationDialog, BigDialog
 from openpilot.selfdrive.ui.sunnypilot.mici.layouts.sunnylink import SunnylinkLayoutMici
 from openpilot.selfdrive.ui.sunnypilot.mici.layouts.models import ModelsLayoutMici
+from openpilot.selfdrive.ui.sunnypilot.mici.layouts.laneful import LanefulLayoutMici
 from openpilot.selfdrive.ui.ui_state import ui_state
 from openpilot.system.ui.lib.application import gui_app, FontWeight
 from openpilot.system.ui.lib.multilang import tr
@@ -50,6 +51,10 @@ class SettingsLayoutSP(OP.SettingsLayout):
     models_btn = SettingsBigButton(tr("models"), "", gui_app.texture("../../sunnypilot/selfdrive/assets/offroad/icon_models.png", ICON_SIZE, ICON_SIZE))
     models_btn.set_click_callback(lambda: gui_app.push_widget(models_panel))
 
+    laneful_panel = LanefulLayoutMici()
+    laneful_btn = SettingsBigButton(tr("laneful"), "", gui_app.texture("icons_mici/settings/device/lkas.png", ICON_SIZE, ICON_SIZE))
+    laneful_btn.set_click_callback(lambda: gui_app.push_widget(laneful_panel))
+
     # onroad: enable button sits at the front (left of toggles)
     self._enable_offroad_btn_onroad = BigCircleButton(self.icon_offroad_enable, red=True)
     self._enable_offroad_btn_onroad.set_click_callback(lambda: self._handle_always_offroad(True))
@@ -67,7 +72,8 @@ class SettingsLayoutSP(OP.SettingsLayout):
     items = self._scroller._items.copy()
 
     items.insert(1, models_btn)
-    items.insert(5, sunnylink_btn)
+    items.insert(2, laneful_btn)
+    items.insert(6, sunnylink_btn)
 
     # front slots (only one ever visible at a time): exit-always-offroad, then enable-onroad
     items.insert(0, self._enable_offroad_btn_onroad)

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

@@ -8,17 +8,24 @@ import pyray as rl
 
 from openpilot.selfdrive.ui.mici.onroad.hud_renderer import HudRenderer
 from openpilot.selfdrive.ui.sunnypilot.onroad.blind_spot_indicators import BlindSpotIndicators
+from openpilot.selfdrive.ui.ui_state import ui_state
 
 
 class HudRendererSP(HudRenderer):
   def __init__(self):
     super().__init__()
     self.blind_spot_indicators = BlindSpotIndicators()
+    self._sp_engaged: bool = False
 
   def _update_state(self) -> None:
     super()._update_state()
     self.blind_spot_indicators.update()
 
+    sp_engaged = ui_state.engaged
+    if sp_engaged != self._sp_engaged:
+      self._chestnut_fade_time = rl.get_time() if sp_engaged else 0
+    self._sp_engaged = sp_engaged
+
   def _render(self, rect: rl.Rectangle) -> None:
     super()._render(rect)
     self.blind_spot_indicators.render(rect)

openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.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.
+
+Laneful visuals: cyan lane-center target, thin white "after" path (model path bent by laneful's nudge),
+and a top-center readout of the pull. Mirrors the math in sunnypilot/modeld_v2/laneful.py.
+"""
+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.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
+
+CENTER_COLOR = (0, 200, 255)
+AFTER_COLOR = (255, 255, 255)
+# keep in sync with sunnypilot/modeld_v2/laneful.py
+LOOKAHEAD_TIME = 1.5
+MIN_LOOKAHEAD = 10.0
+MAX_ERROR = 0.5
+
+
+class LanefulRenderer:
+  def __init__(self):
+    self._center_points = np.empty((0, 2), dtype=np.float32)
+    self._after_points = np.empty((0, 2), dtype=np.float32)
+    self._trust = 0.0
+    self._strength = 0.0
+    self._frame = 0
+    self._font = gui_app.font(FontWeight.BOLD)
+    # smoothed copies for the readout so the numbers don't flicker
+    self._pull_display = FirstOrderFilter(0.0, 0.5, DT_MDL)
+    self._trust_display = FirstOrderFilter(0.0, 0.5, DT_MDL)
+    self._readout_rect: rl.Rectangle | None = None
+    self._settings_page = None
+
+  def update(self, renderer, max_idx: int) -> None:
+    """Called from the model renderer's update with its lane lines and path already in raw points."""
+    if self._frame % 60 == 0:
+      self._strength = ui_state.params.get("LanefulStrength", return_default=True)
+    self._frame += 1
+
+    left, right = renderer._lane_lines[1].raw_points, renderer._lane_lines[2].raw_points
+    stds = list(ui_state.sm['modelV2'].laneLineStds)
+    if self._strength <= 0.0 or len(left) == 0 or len(right) != len(left) or len(stds) < 3:
+      self._center_points = np.empty((0, 2), dtype=np.float32)
+      self._after_points = np.empty((0, 2), dtype=np.float32)
+      return
+
+    probs = renderer._lane_line_probs
+    l_prob = float(probs[1] * np.interp(stds[1], [0.15, 0.3], [1.0, 0.0]))
+    r_prob = float(probs[2] * np.interp(stds[2], [0.15, 0.3], [1.0, 0.0]))
+    width = np.clip(right[:, 1] - left[:, 1], 2.8, 4.5) if l_prob * r_prob > 0.5 else 3.7
+    center = left.copy()
+    center[:, 1] = (l_prob * (left[:, 1] + width / 2) + r_prob * (right[:, 1] - width / 2)) / (l_prob + r_prob + 1e-4)
+    self._trust = l_prob + r_prob - l_prob * r_prob
+    self._center_points = renderer._map_line_to_polygon(center, 0.1, 0.0, max_idx)
+
+    # "after" path: model path bent toward center the way laneful's pure-pursuit term steers, ending at the lookahead
+    path = renderer._path.raw_points
+    lookahead = max(ui_state.sm['carState'].vEgo * LOOKAHEAD_TIME, MIN_LOOKAHEAD)
+    error = float(np.clip(np.interp(lookahead, center[:, 0], center[:, 1]) - np.interp(lookahead, path[:, 0], path[:, 1]),
+                          -MAX_ERROR, MAX_ERROR))
+    self._pull_display.update(self._trust * error)  # what laneful actually pulls, 0 when lines are untrusted
+    self._trust_display.update(self._trust)
+    after = path.copy()
+    after[:, 1] += self._strength * self._trust * error * (path[:, 0] / lookahead) ** 2
+    self._after_points = renderer._map_line_to_polygon(after, 0.15, renderer._path_offset_z,
+                                                       renderer._get_path_length_idx(path[:, 0], lookahead))
+
+  def draw(self, rect: rl.Rectangle) -> None:
+    if self._center_points.size and self._trust > 0.05:
+      draw_polygon(rect, self._center_points, rl.Color(*CENTER_COLOR, int(np.clip(self._trust, 0.2, 1.0) * 255)))
+    if self._after_points.size:
+      draw_polygon(rect, self._after_points, rl.Color(*AFTER_COLOR, 200))
+    self._readout_rect = None
+    if self._strength > 0.0:
+      self._draw_readout(rect)
+
+  def handle_tap(self, pos) -> bool:
+    """Tapping the readout opens the laneful settings page. Returns True if the tap was consumed."""
+    r = self._readout_rect
+    if r is None or not (r.x - 20 <= pos.x <= r.x + r.width + 20 and r.y - 20 <= pos.y <= r.y + r.height + 20):
+      return False
+    if self._settings_page is None:
+      from openpilot.selfdrive.ui.sunnypilot.mici.layouts.laneful import LanefulLayoutMici
+      self._settings_page = LanefulLayoutMici()
+    gui_app.push_widget(self._settings_page)
+    return True
+
+  def _draw_readout(self, rect: rl.Rectangle) -> None:
+    """Top-center pill: pull toward lane center in cm (capped at 50), arrow = side,
+    bar grows from center toward the pull (full at the cap), brighter with more lane line trust."""
+    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
+    self._readout_rect = rl.Rectangle(x, y, w, h)
+    rl.draw_rectangle_rounded(self._readout_rect, 0.5, 12, rl.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, rl.WHITE)
+
+    active = rl.Color(*CENTER_COLOR, int(np.clip(abs(pull) / 0.1, 0.0, 1.0) * 255))
+    idle = rl.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, rl.Color(255, 255, 255, 40))
+    rl.draw_rectangle(int(cx - 1), int(bar.y - 2), 2, int(bar.height + 4), rl.Color(255, 255, 255, 120))
+    fill = bar.width / 2 * float(np.clip(abs(pull) / MAX_ERROR, 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,
+                                rl.Color(*CENTER_COLOR, int(np.clip(0.3 + 0.7 * trust, 0.0, 1.0) * 255)))

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.mici.onroad.laneful import LanefulRenderer
 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.laneful = LanefulRenderer()

openpilot/selfdrive/ui/sunnypilot/ui_state.py

@@ -221,10 +221,9 @@ class UIStateSP:
       self.params.remove("LateralJerkTorqueController")
       self.params.remove("AlphaLongitudinalEnabled")
 
-    # No longitudinal control: no experimental mode or DEC
+    # No longitudinal control: no experimental mode
     if not has_long:
       self.params.remove("ExperimentalMode")
-      self.params.remove("DynamicExperimentalControl")
 
     # ICBM: clear if not available or if full longitudinal control is active
     if self.CP_SP is not None:

openpilot/sunnypilot/modeld_v2/laneful.py

@@ -0,0 +1,79 @@
+"""
+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.
+
+Laneful: nudge the model's curvature toward the lane center seen by the lane lines.
+The model stays in charge; this only adds a pure-pursuit correction weighted by lane line confidence,
+and fades to zero (plain laneless) when the lines are weak.
+"""
+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, Plan
+
+LANE_WIDTH_DEFAULT = 3.7  # m
+LANE_WIDTH_CLIP = (2.8, 4.5)  # m
+LOOKAHEAD_TIME = 1.5  # s
+MIN_LOOKAHEAD = 10.0  # m
+MAX_ERROR = 0.5  # m, cap on how far laneful can pull
+MIN_SPEED = 9.0  # m/s
+
+
+class Laneful:
+  def __init__(self):
+    self.lane_width = FirstOrderFilter(LANE_WIDTH_DEFAULT, 10.0, DT_MDL)
+
+  def update(self, model_output: dict[str, np.ndarray], v_ego: float, strength: float, desire_active: bool) -> float:
+    """Returns a curvature offset to add to the model's raw desired curvature."""
+    if strength <= 0.0 or desire_active or v_ego < MIN_SPEED or 'lane_lines' not in model_output:
+      return 0.0
+
+    lookahead = max(v_ego * LOOKAHEAD_TIME, MIN_LOOKAHEAD)
+    lines = model_output['lane_lines'][0, :, :, 0]
+    left_y = np.interp(lookahead, ModelConstants.X_IDXS, lines[1])
+    right_y = np.interp(lookahead, ModelConstants.X_IDXS, lines[2])
+
+    # line confidence, discounted when the model is unsure where the line is
+    probs = model_output['lane_lines_prob'][0, 1::2]
+    stds = model_output['lane_lines_stds'][0, :, 0, 0]
+    l_prob = float(probs[1] * np.interp(stds[1], [0.15, 0.3], [1.0, 0.0]))
+    r_prob = float(probs[2] * np.interp(stds[2], [0.15, 0.3], [1.0, 0.0]))
+
+    if l_prob * r_prob > 0.5:
+      self.lane_width.update(float(np.clip(right_y - left_y, *LANE_WIDTH_CLIP)))
+    half_width = self.lane_width.x / 2
+
+    # one weak line -> hug the strong one at half a lane width
+    center_y = (l_prob * (left_y + half_width) + r_prob * (right_y - half_width)) / (l_prob + r_prob + 1e-4)
+    lane_trust = l_prob + r_prob - l_prob * r_prob
+
+    plan = model_output['plan'][0, :, Plan.POSITION]
+    plan_y = np.interp(lookahead, plan[:, 0], plan[:, 1])
+
+    error = np.clip(center_y - plan_y, -MAX_ERROR, MAX_ERROR)
+    return float(strength * lane_trust * 2.0 * error / lookahead ** 2)
+
+
+if __name__ == "__main__":
+  def fake_output(left_y, right_y, prob=0.9, std=0.1, plan_y=0.0):
+    x = np.array(ModelConstants.X_IDXS)
+    lines = np.zeros((1, 4, len(x), 2), dtype=np.float32)
+    lines[0, 1, :, 0], lines[0, 2, :, 0] = left_y, right_y
+    plan = np.zeros((1, len(x), 15), dtype=np.float32)
+    plan[0, :, 0], plan[0, :, 1] = x, plan_y
+    return {'lane_lines': lines, 'plan': plan,
+            'lane_lines_prob': np.full((1, 8), prob, dtype=np.float32),
+            'lane_lines_stds': np.full((1, 4, len(x), 2), std, dtype=np.float32)}
+
+  lf = Laneful()
+  assert abs(lf.update(fake_output(-1.85, 1.85), 25.0, 1.0, False)) < 1e-6, "centered -> no nudge"
+  assert lf.update(fake_output(-1.0, 2.7), 25.0, 1.0, False) > 0, "lane center to the +y side -> nudge toward +y"
+  assert lf.update(fake_output(-2.7, 1.0), 25.0, 1.0, False) < 0, "lane center to the -y side -> nudge toward -y"
+  assert lf.update(fake_output(-1.0, 2.7), 25.0, 1.0, True) == 0.0, "off during lane change/turn"
+  assert lf.update(fake_output(-1.0, 2.7, std=0.5), 25.0, 1.0, False) == 0.0, "blurry lines -> laneless"
+  assert lf.update(fake_output(-1.0, 2.7), 25.0, 0.0, False) == 0.0, "strength 0 -> off"
+  assert lf.update(fake_output(-1.0, 2.7), 25.0, 0.5, False) < lf.update(fake_output(-1.0, 2.7), 25.0, 1.0, False)
+  print("laneful self-check ok")

openpilot/sunnypilot/modeld_v2/modeld.py

@@ -37,6 +37,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.laneful import Laneful
 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
@@ -224,7 +225,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,
+                            laneful_curvature: float = 0.0) -> 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,
@@ -236,6 +238,7 @@ class ModelState(ModelStateBase):
     else:
       desired_accel = model_output['action'][0, 1]
       desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2
+    desired_curvature += laneful_curvature
 
     stop = v_ego < 0.3 and desired_accel < 0.1
     desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
@@ -350,6 +353,8 @@ def main(demo=False):
   DH = DesireHelper()
   meta_constants = load_meta_constants()
   RELC = RoadEdgeLaneChangeController()
+  laneful = Laneful()
+  laneful_strength = 0.0
 
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -393,6 +398,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))
+      laneful_strength = params.get("LanefulStrength", return_default=True)
     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)
@@ -464,7 +470,9 @@ def main(demo=False):
       posenet_send = messaging.new_message('cameraOdometry')
       mdv2sp_send = messaging.new_message('modelDataV2SP')
 
-      action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
+      laneful_curvature = laneful.update(model_output, v_ego, laneful_strength,
+                                         desire != log.Desire.none or 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, laneful_curvature)
       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,

openpilot/sunnypilot/selfdrive/car/interfaces.py

@@ -81,7 +81,6 @@ def _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsS
 
   if not CP.openpilotLongitudinalControl and CP_SP.pcmCruiseSpeed:
     cloudlog.warning("openpilot Longitudinal Control and ICBM not available, cleaning up params")
-    params.remove("DynamicExperimentalControl")
     params.remove("CustomAccIncrementsEnabled")
     params.remove("SmartCruiseControlVision")
     params.remove("SmartCruiseControlMap")

openpilot/sunnypilot/selfdrive/car/tests/test_custom_cruise.py

@@ -57,7 +57,7 @@ class TestCustomAccIncrements(TestVCruiseHelper):
   def test_default_behavior_when_disabled(self):
     """Test that default increments are used when custom ACC is disabled"""
     self.set_custom_increments(enabled=False, short_inc=5, long_inc=10)
-    self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False)
+    self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
 
     initial_speed = self.v_cruise_helper.v_cruise_kph
 
@@ -69,7 +69,7 @@ class TestCustomAccIncrements(TestVCruiseHelper):
   def test_custom_short_press_increments(self, increment):
     """Test custom short press increments (1-10)"""
     self.set_custom_increments(enabled=True, short_inc=increment, long_inc=5)
-    self.enable(50 * CV.KPH_TO_MS, False, False)
+    self.enable(50 * CV.KPH_TO_MS, False)
 
     initial_speed = self.v_cruise_helper.v_cruise_kph
     self.press_button_short(ButtonType.accelCruise)
@@ -86,7 +86,7 @@ class TestCustomAccIncrements(TestVCruiseHelper):
   def test_custom_long_press_increments(self, increment):
     """Test custom long press increments (1, 5, 10)"""
     self.set_custom_increments(enabled=True, short_inc=1, long_inc=increment)
-    self.enable(50 * CV.KPH_TO_MS, False, False)
+    self.enable(50 * CV.KPH_TO_MS, False)
 
     initial_speed = self.v_cruise_helper.v_cruise_kph
     self.press_button_long(ButtonType.accelCruise)
@@ -103,7 +103,7 @@ class TestCustomAccIncrements(TestVCruiseHelper):
   def test_accel_decel_symmetry(self, button_type):
     """Test that acceleration and deceleration work symmetrically"""
     self.set_custom_increments(enabled=True, short_inc=3, long_inc=5)
-    self.enable(50 * CV.KPH_TO_MS, False, False)
+    self.enable(50 * CV.KPH_TO_MS, False)
 
     initial_speed = self.v_cruise_helper.v_cruise_kph
     self.press_button_short(button_type)
@@ -123,7 +123,7 @@ class TestCustomAccIncrements(TestVCruiseHelper):
     for initial, increment, expected in test_cases:
       self.set_custom_increments(enabled=True, short_inc=increment, long_inc=increment)
       self.reset_cruise_speed_state()
-      self.enable(initial * CV.KPH_TO_MS, False, False)
+      self.enable(initial * CV.KPH_TO_MS, False)
 
       self.press_button_short(ButtonType.accelCruise)
       assert self.v_cruise_helper.v_cruise_kph == expected
@@ -132,7 +132,7 @@ class TestCustomAccIncrements(TestVCruiseHelper):
     """Test that invalid values fallback to safe defaults"""
     # Test invalid short increment
     self.set_custom_increments(enabled=True, short_inc=-1, long_inc=5)
-    self.enable(50 * CV.KPH_TO_MS, False, False)
+    self.enable(50 * CV.KPH_TO_MS, False)
 
     initial_speed = self.v_cruise_helper.v_cruise_kph
     self.press_button_short(ButtonType.accelCruise)
@@ -141,7 +141,7 @@ class TestCustomAccIncrements(TestVCruiseHelper):
     # Test invalid long increment
     self.reset_cruise_speed_state()
     self.set_custom_increments(enabled=True, short_inc=1, long_inc=99)
-    self.enable(50 * CV.KPH_TO_MS, False, False)
+    self.enable(50 * CV.KPH_TO_MS, False)
 
     initial_speed = self.v_cruise_helper.v_cruise_kph
     self.press_button_long(ButtonType.accelCruise)

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

@@ -10,8 +10,6 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
 
 
-
-
 class AutoLaneChangeMode:
   OFF = -1
   NUDGE = 0  # default
@@ -33,7 +31,6 @@ AUTO_LANE_CHANGE_TIMER = {
 }
 
 ONE_SECOND_DELAY = -1
-EDGE_DELAY = -2
 
 
 class AutoLaneChangeController:
@@ -51,7 +48,6 @@ 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()
 
@@ -72,7 +68,7 @@ class AutoLaneChangeController:
       self.read_params()
     self.param_read_counter += 1
 
-  def update_lane_change_timers(self, blindspot_detected: bool, edge_detected: bool) -> None:
+  def update_lane_change_timers(self, blindspot_detected: bool) -> None:
     self.lane_change_delay = AUTO_LANE_CHANGE_TIMER.get(self.lane_change_set_timer,
                                                         AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGE])
 
@@ -84,12 +80,6 @@ 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)
@@ -107,23 +97,14 @@ class AutoLaneChangeController:
 
     return bool(self.lane_change_wait_timer > self.lane_change_delay)
 
-  def update_lane_change(self, blindspot_detected: bool, edge_detected: bool, brake_pressed: bool,
-                         torque_applied: bool = False) -> None:
+  def update_lane_change(self, blindspot_detected: bool, brake_pressed: bool) -> None:
     if brake_pressed and not self.prev_brake_pressed:
       self.prev_brake_pressed = brake_pressed
 
-    self.update_lane_change_timers(blindspot_detected, edge_detected)
+    self.update_lane_change_timers(blindspot_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/dec/constants.py

@@ -1,17 +0,0 @@
-class WMACConstants:
-  # Lead detection parameters
-  LEAD_WINDOW_SIZE = 6  # Stable detection window
-  LEAD_PROB = 0.45  # Balanced threshold for lead detection
-
-  # Slow down detection parameters
-  SLOW_DOWN_WINDOW_SIZE = 5  # Responsive but stable
-  SLOW_DOWN_PROB = 0.3  # Balanced threshold for slow down scenarios
-
-  # Optimized slow down distance curve - smooth and progressive
-  SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
-  SLOW_DOWN_DIST = [32., 46., 64., 86., 108., 130., 145., 165.]
-
-  # Slowness detection parameters
-  SLOWNESS_WINDOW_SIZE = 10  # Stable slowness detection
-  SLOWNESS_PROB = 0.55  # Clear threshold for slowness
-  SLOWNESS_CRUISE_OFFSET = 1.025  # Conservative cruise speed offset

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

@@ -1,388 +0,0 @@
-"""
-Copyright (c) 2021-, rav4kumar, 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.
-"""
-# Version = 2025-6-30
-
-from openpilot.cereal import messaging
-from opendbc.car import structs
-from numpy import interp
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
-from typing import Literal
-
-# d-e2e, from modeldata.h
-TRAJECTORY_SIZE = 33
-SET_MODE_TIMEOUT = 15
-
-# Define the valid mode types
-ModeType = Literal['acc', 'blended']
-
-
-class SmoothKalmanFilter:
-  """Enhanced Kalman filter with smoothing for stable decision making."""
-
-  def __init__(self, initial_value=0, measurement_noise=0.1, process_noise=0.01,
-               alpha=1.0, smoothing_factor=0.85):
-    self.x = initial_value
-    self.P = 1.0
-    self.R = measurement_noise
-    self.Q = process_noise
-    self.alpha = alpha
-    self.smoothing_factor = smoothing_factor
-    self.initialized = False
-    self.history = []
-    self.max_history = 10
-    self.confidence = 0.0
-
-  def add_data(self, measurement):
-    if len(self.history) >= self.max_history:
-      self.history.pop(0)
-    self.history.append(measurement)
-
-    if not self.initialized:
-      self.x = measurement
-      self.initialized = True
-      self.confidence = 0.1
-      return
-
-    self.P = self.alpha * self.P + self.Q
-
-    K = self.P / (self.P + self.R)
-    effective_K = K * (1.0 - self.smoothing_factor) + self.smoothing_factor * 0.1
-
-    innovation = measurement - self.x
-    self.x = self.x + effective_K * innovation
-    self.P = (1 - effective_K) * self.P
-
-    if abs(innovation) < 0.1:
-      self.confidence = min(1.0, self.confidence + 0.05)
-    else:
-      self.confidence = max(0.1, self.confidence - 0.02)
-
-  def get_value(self):
-    return self.x if self.initialized else None
-
-  def get_confidence(self):
-    return self.confidence
-
-  def reset_data(self):
-    self.initialized = False
-    self.history = []
-    self.confidence = 0.0
-
-
-class ModeTransitionManager:
-  """Manages smooth transitions between driving modes with hysteresis."""
-
-  def __init__(self):
-    self.current_mode: ModeType = 'acc'
-    self.mode_confidence = {'acc': 1.0, 'blended': 0.0}
-    self.transition_timeout = 0
-    self.min_mode_duration = 10
-    self.mode_duration = 0
-    self.emergency_override = False
-
-  def request_mode(self, mode: ModeType, confidence: float = 1.0, emergency: bool = False):
-    # Emergency override for critical situations (stops, collisions)
-    if emergency:
-      self.emergency_override = True
-      self.current_mode = mode
-      self.transition_timeout = SET_MODE_TIMEOUT
-      self.mode_duration = 0
-      return
-
-    self.mode_confidence[mode] = min(1.0, self.mode_confidence[mode] + 0.1 * confidence)
-    for m in self.mode_confidence:
-      if m != mode:
-        self.mode_confidence[m] = max(0.0, self.mode_confidence[m] - 0.05)
-
-    # Require minimum duration in current mode (unless emergency)
-    if self.mode_duration < self.min_mode_duration and not self.emergency_override:
-      return
-
-    # Hysteresis: higher threshold for mode changes
-    confidence_threshold = 0.6 if mode != self.current_mode else 0.3  # Lower threshold for faster response
-
-    if self.mode_confidence[mode] > confidence_threshold:
-      if mode != self.current_mode and self.transition_timeout == 0:
-        self.transition_timeout = SET_MODE_TIMEOUT
-        self.current_mode = mode
-        self.mode_duration = 0
-
-  def update(self):
-    if self.transition_timeout > 0:
-      self.transition_timeout -= 1
-    self.mode_duration += 1
-
-    # Reset emergency override after some time
-    if self.emergency_override and self.mode_duration > 20:
-      self.emergency_override = False
-
-    # Gradual confidence decay
-    for mode in self.mode_confidence:
-      self.mode_confidence[mode] *= 0.98
-
-  def get_mode(self) -> ModeType:
-    return self.current_mode
-
-
-class DynamicExperimentalController:
-  def __init__(self, CP: structs.CarParams, mpc, params=None):
-    self._CP = CP
-    self._mpc = mpc
-    self._params = params or Params()
-    self._enabled: bool = self._params.get_bool("DynamicExperimentalControl")
-    self._active: bool = False
-    self._frame: int = 0
-    self._urgency = 0.0
-
-    self._mode_manager = ModeTransitionManager()
-
-    # Smooth filters for stable decision making with faster response for critical scenarios
-    self._lead_filter = SmoothKalmanFilter(
-      measurement_noise=0.15,
-      process_noise=0.05,
-      alpha=1.02,
-      smoothing_factor=0.8
-    )
-
-    self._slow_down_filter = SmoothKalmanFilter(
-      measurement_noise=0.1,
-      process_noise=0.1,
-      alpha=1.05,
-      smoothing_factor=0.7
-    )
-
-    self._slowness_filter = SmoothKalmanFilter(
-      measurement_noise=0.1,
-      process_noise=0.06,
-      alpha=1.015,
-      smoothing_factor=0.92
-    )
-
-    self._mpc_fcw_filter = SmoothKalmanFilter(
-      measurement_noise=0.2,
-      process_noise=0.1,
-      alpha=1.1,
-      smoothing_factor=0.5
-    )
-    self._has_lead_filtered = False
-    self._has_slow_down = False
-    self._has_slowness = False
-    self._has_mpc_fcw = False
-    self._v_ego_kph = 0.0
-    self._v_cruise_kph = 0.0
-    self._has_standstill = False
-    self._mpc_fcw_crash_cnt = 0
-    self._standstill_count = 0
-    # debug
-    self._endpoint_x = float('inf')
-    self._expected_distance = 0.0
-    self._trajectory_valid = False
-
-  def _read_params(self) -> None:
-    if self._frame % int(1. / DT_MDL) == 0:
-      self._enabled = self._params.get_bool("DynamicExperimentalControl")
-
-  def mode(self) -> str:
-    return self._mode_manager.get_mode()
-
-  def enabled(self) -> bool:
-    return self._enabled
-
-  def active(self) -> bool:
-    return self._active
-
-  def set_mpc_fcw_crash_cnt(self) -> None:
-    """Set MPC FCW crash count"""
-    self._mpc_fcw_crash_cnt = self._mpc.crash_cnt
-
-  def _update_calculations(self, sm: messaging.SubMaster) -> None:
-    car_state = sm['carState']
-    lead_one = sm['radarState'].leadOne
-    md = sm['modelV2']
-
-    self._v_ego_kph = car_state.vEgo * 3.6
-    self._v_cruise_kph = car_state.vCruise
-    self._has_standstill = car_state.standstill
-
-    # standstill detection
-    if self._has_standstill:
-      self._standstill_count = min(20, self._standstill_count + 1)
-    else:
-      self._standstill_count = max(0, self._standstill_count - 1)
-
-    # Lead detection
-    self._lead_filter.add_data(float(lead_one.present))
-    lead_value = self._lead_filter.get_value() or 0.0
-    self._has_lead_filtered = lead_value > WMACConstants.LEAD_PROB
-
-    # MPC FCW detection
-    fcw_filtered_value = self._mpc_fcw_filter.get_value() or 0.0
-    self._mpc_fcw_filter.add_data(float(self._mpc_fcw_crash_cnt > 0))
-    self._has_mpc_fcw = fcw_filtered_value > 0.5
-
-    # Slow down detection
-    self._calculate_slow_down(md)
-
-    # Slowness detection
-    if not (self._standstill_count > 5) and not self._has_slow_down:
-      current_slowness = float(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET))
-      self._slowness_filter.add_data(current_slowness)
-      slowness_value = self._slowness_filter.get_value() or 0.0
-
-      # Hysteresis for slowness
-      threshold = WMACConstants.SLOWNESS_PROB * (0.8 if self._has_slowness else 1.1)
-      self._has_slowness = slowness_value > threshold
-
-  def _calculate_slow_down(self, md):
-    """Calculate urgency based on trajectory endpoint vs expected distance."""
-
-    # Reset to safe defaults
-    urgency = 0.0
-    self._endpoint_x = float('inf')
-    self._trajectory_valid = False
-
-    #Require exact trajectory size
-    position_valid = len(md.position.x) == TRAJECTORY_SIZE
-    orientation_valid = len(md.orientation.x) == TRAJECTORY_SIZE
-
-    if not (position_valid and orientation_valid):
-      # Invalid trajectory - this itself might indicate a stop scenario
-      # Apply moderate urgency for incomplete trajectories at speed
-      if self._v_ego_kph > 20.0:
-        urgency = 0.3
-
-      self._slow_down_filter.add_data(urgency)
-      urgency_filtered = self._slow_down_filter.get_value() or 0.0
-      self._has_slow_down = urgency_filtered > WMACConstants.SLOW_DOWN_PROB
-      self._urgency = urgency_filtered
-      return
-
-    # We have a valid full trajectory
-    self._trajectory_valid = True
-
-    # Use the exact endpoint (33rd point, index 32)
-    endpoint_x = md.position.x[TRAJECTORY_SIZE - 1]
-    self._endpoint_x = endpoint_x
-
-    # Get expected distance based on current speed using tuned constants
-    expected_distance = interp(self._v_ego_kph,
-                               WMACConstants.SLOW_DOWN_BP,
-                               WMACConstants.SLOW_DOWN_DIST)
-    self._expected_distance = expected_distance
-
-    # Calculate urgency based on trajectory shortage
-    if endpoint_x < expected_distance:
-      shortage = expected_distance - endpoint_x
-      shortage_ratio = shortage / expected_distance
-
-      # Base urgency on shortage ratio
-      urgency = min(1.0, shortage_ratio * 2.0)
-
-      # Increase urgency for very short trajectories (imminent stops)
-      critical_distance = expected_distance * 0.3
-      if endpoint_x < critical_distance:
-        urgency = min(1.0, urgency * 2.0)
-
-      # Speed-based urgency adjustment
-      if self._v_ego_kph > 25.0:
-        speed_factor = 1.0 + (self._v_ego_kph - 25.0) / 80.0
-        urgency = min(1.0, urgency * speed_factor)
-
-    # Apply filtering but with less smoothing for stops
-    self._slow_down_filter.add_data(urgency)
-    urgency_filtered = self._slow_down_filter.get_value() or 0.0
-
-    # Update state with lower threshold for better stop detection
-    self._has_slow_down = urgency_filtered > (WMACConstants.SLOW_DOWN_PROB * 0.8)
-    self._urgency = urgency_filtered
-
-  def _radarless_mode(self) -> None:
-    """Radarless mode decision logic with emergency handling."""
-
-    # EMERGENCY: MPC FCW - immediate blended mode
-    if self._has_mpc_fcw:
-      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      return
-
-    # Standstill: use blended
-    if self._standstill_count > 3:
-      self._mode_manager.request_mode('blended', confidence=0.9)
-      return
-
-    # Slow down scenarios: emergency for high urgency, normal for lower urgency
-    if self._has_slow_down:
-      if self._urgency > 0.7:
-        # Emergency: immediate blended mode for high urgency stops
-        self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      else:
-        # Normal: blended with urgency-based confidence
-        confidence = min(1.0, self._urgency * 1.5)
-        self._mode_manager.request_mode('blended', confidence=confidence)
-      return
-
-    # Driving slow: use ACC (but not if actively slowing down)
-    if self._has_slowness and not self._has_slow_down:
-      self._mode_manager.request_mode('acc', confidence=0.8)
-      return
-
-    # Default: ACC
-    self._mode_manager.request_mode('acc', confidence=0.7)
-
-  def _radar_mode(self) -> None:
-    """Radar mode with emergency handling."""
-
-    # EMERGENCY: MPC FCW - immediate blended mode
-    if self._has_mpc_fcw:
-      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      return
-
-    # If lead detected and not in standstill: always use ACC
-    if self._has_lead_filtered and not (self._standstill_count > 3):
-      self._mode_manager.request_mode('acc', confidence=1.0)
-      return
-
-    # Slow down scenarios: emergency for high urgency, normal for lower urgency
-    if self._has_slow_down:
-      if self._urgency > 0.7:
-        # Emergency: immediate blended mode for high urgency stops
-        self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      else:
-        # Normal: blended with urgency-based confidence
-        confidence = min(1.0, self._urgency * 1.3)
-        self._mode_manager.request_mode('blended', confidence=confidence)
-      return
-
-    # Standstill: use blended
-    if self._standstill_count > 3:
-      self._mode_manager.request_mode('blended', confidence=0.9)
-      return
-
-    # Driving slow: use ACC (but not if actively slowing down)
-    if self._has_slowness and not self._has_slow_down:
-      self._mode_manager.request_mode('acc', confidence=0.8)
-      return
-
-    # Default: ACC
-    self._mode_manager.request_mode('acc', confidence=0.7)
-
-  def update(self, sm: messaging.SubMaster) -> None:
-    self._read_params()
-
-    self.set_mpc_fcw_crash_cnt()
-
-    self._update_calculations(sm)
-
-    if self._CP.radarUnavailable:
-      self._radarless_mode()
-    else:
-      self._radar_mode()
-
-    self._mode_manager.update()
-    self._active = sm['selfdriveState'].experimentalMode and self._enabled
-    self._frame += 1

openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py

@@ -1,112 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-from typing import cast
-
-from openpilot.cereal import custom, messaging
-from opendbc.car import structs
-from openpilot.common.test import OpenpilotTestCase
-from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, LongitudinalPlanSource
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
-
-V_EGO = 20.0
-E2E_ACCEL = -3.0  # low enough that e2e wins the min() whenever it is a candidate
-
-
-class MockDec:
-  def __init__(self, active: bool, mode: str):
-    self._active = active
-    self._mode = mode
-
-  def update(self, sm):
-    pass
-
-  def active(self) -> bool:
-    return self._active
-
-  def mode(self) -> str:
-    return self._mode
-
-  def enabled(self) -> bool:
-    return True
-
-
-class MockSubMaster(dict):
-  def __init__(self, services: dict):
-    super().__init__(services)
-    self.valid = dict.fromkeys(services, True)
-    self.logMonoTime = dict.fromkeys(services, 0)
-    self.updated = dict.fromkeys(services, True)
-    self.recv_frame = dict.fromkeys(services, 1)
-
-  def all_checks(self, service_list=None) -> bool:
-    return True
-
-
-def build_sm(experimental_mode: bool) -> MockSubMaster:
-  services = {}
-  for service in ("radarState", "controlsState", "vehicleParameters", "carStateSP",
-                  "liveMapDataSP", "gpsLocationExternal", "gpsLocation"):
-    services[service] = getattr(messaging.new_message(service), service)
-
-  car_state = messaging.new_message('carState')
-  car_state.carState.vEgo = V_EGO
-  car_state.carState.vCruise = 100.0
-  car_state.carState.vCruiseCluster = 100.0
-  services['carState'] = car_state.carState.as_reader()
-
-  selfdrive_state = messaging.new_message('selfdriveState')
-  selfdrive_state.selfdriveState.experimentalMode = experimental_mode
-  selfdrive_state.selfdriveState.enabled = True
-  services['selfdriveState'] = selfdrive_state.selfdriveState.as_reader()
-
-  car_control = messaging.new_message('carControl')
-  car_control.carControl.enabled = True
-  services['carControl'] = car_control.carControl.as_reader()
-
-  model = messaging.new_message('modelV2')
-  model.modelV2.orientationRate.z = [0.01] * 33  # nonzero: a straight path divides by zero in SCC vision
-  model.modelV2.velocity.x = [V_EGO] * 33
-  model.modelV2.position.x = [float(i) for i in range(33)]
-  model.modelV2.action.desiredAcceleration = E2E_ACCEL
-  services['modelV2'] = model.modelV2.as_reader()
-
-  return MockSubMaster(services)
-
-
-def build_planner(dec_active: bool, dec_mode: str) -> LongitudinalPlanner:
-  CP = structs.CarParams()
-  CP.steerRatio = 15.0
-  CP.wheelbase = 2.7
-  CP.longitudinalActuatorDelay = 0.2
-  CP_SP = custom.CarParamsSP.new_message().as_reader()
-
-  planner = LongitudinalPlanner(CP, CP_SP, init_v=V_EGO)
-  planner.dec = cast(DynamicExperimentalController, MockDec(dec_active, dec_mode))
-  return planner
-
-
-class TestDecPlannerGate(OpenpilotTestCase):
-  """The e2e candidate must be gated on is_e2e(), not raw experimentalMode."""
-
-  def _source(self, experimental_mode: bool, dec_active: bool, dec_mode: str) -> LongitudinalPlanSource:
-    planner = build_planner(dec_active, dec_mode)
-    planner.update(build_sm(experimental_mode))
-    return planner.mpc.source
-
-  def test_no_e2e_when_experimental_mode_off(self):
-    assert self._source(False, False, 'acc') != LongitudinalPlanSource.e2e
-
-  def test_e2e_when_dec_inactive(self):
-    # DEC off: behavior must match upstream
-    assert self._source(True, False, 'acc') == LongitudinalPlanSource.e2e
-
-  def test_e2e_when_dec_blended(self):
-    assert self._source(True, True, 'blended') == LongitudinalPlanSource.e2e
-
-  def test_no_e2e_when_dec_holds_acc(self):
-    # the regression
-    assert self._source(True, True, 'acc') != LongitudinalPlanSource.e2e

openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py

@@ -1,91 +0,0 @@
-from openpilot.common.test import OpenpilotTestCase
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
-
-class MockLeadOne:
-  def __init__(self, present=0.0):
-    self.present = present
-
-class MockRadarState:
-  def __init__(self, present=0.0):
-    self.leadOne = MockLeadOne(present=present)
-
-class MockCarState:
-  def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False):
-    self.vEgo = vEgo
-    self.vCruise = vCruise
-    self.standstill = standstill
-
-class MockModelData:
-  def __init__(self, valid=True):
-    size = 33 if valid else 10  # incomplete if invalid
-    self.position = type("Pos", (), {"x": [0.0] * size})()
-    self.orientation = type("Ori", (), {"x": [0.0] * size})()
-
-class MockSelfDriveState:
-  def __init__(self, experimentalMode=False):
-    self.experimentalMode = experimentalMode
-
-class MockParams:
-  def get_bool(self, name):
-    return True
-
-def default_sm():
-  sm = {
-    'carState': MockCarState(vEgo=10.0, vCruise=20.0),
-    'radarState': MockRadarState(present=1.0),
-    'modelV2': MockModelData(valid=True),
-    'selfdriveState': MockSelfDriveState(experimentalMode=True),
-  }
-  return sm
-
-def mock_cp():
-  class CP:
-    radarUnavailable = False
-  return CP()
-
-def mock_mpc():
-  class MPC:
-    crash_cnt = 0
-  return MPC()
-
-# Fake Kalman Filter that always returns a given value
-class FakeKalman:
-  def __init__(self, value=1.0):
-    self.value = value
-  def add_data(self, v): pass
-  def get_value(self): return self.value
-  def get_confidence(self): return 1.0
-  def reset_data(self): pass
-
-class TestDynamicExperimentalController(OpenpilotTestCase):
-  def test_initial_mode_is_acc(self, mock_cp, mock_mpc):
-    controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-    assert controller.mode() == "acc"
-
-  def test_standstill_triggers_blended(self, mock_cp, mock_mpc, default_sm):
-    controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-    default_sm['carState'].standstill = True
-    for _ in range(10):
-      controller.update(default_sm)
-    assert controller.mode() == "blended"
-
-  def test_emergency_blended_on_fcw(self, mock_cp, mock_mpc, default_sm):
-    controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-    mock_mpc.crash_cnt = 1  # simulate FCW
-    for _ in range(2):
-      controller.update(default_sm)
-    assert controller.mode() == "blended"
-
-  def test_radarless_slowdown_triggers_blended(self, mock_cp, mock_mpc, default_sm):
-    mock_cp.radarUnavailable = True
-    controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-
-    # Force conditions to simulate slowdown
-    controller._slow_down_filter = FakeKalman(value=1.0)  # ty: ignore[invalid-assignment]
-    controller._v_ego_kph = 35.0
-    default_sm['modelV2'] = MockModelData(valid=False)  # Incomplete trajectory
-
-    for _ in range(3):
-      controller.update(default_sm)
-
-    assert controller.mode() == "blended"

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

@@ -9,7 +9,6 @@ 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.dec.dec import DynamicExperimentalController
 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
@@ -17,7 +16,6 @@ from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolve
 from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 from openpilot.sunnypilot.models.helpers import get_active_bundle
 
-DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
 LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
 
@@ -25,7 +23,6 @@ class LongitudinalPlannerSP:
   def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
     self.events_sp = EventsSP()
     self.resolver = SpeedLimitResolver()
-    self.dec = DynamicExperimentalController(CP, mpc)
     self.scc = SmartCruiseControl()
     self.resolver = SpeedLimitResolver()
     self.sla = SpeedLimitAssist(CP, CP_SP)
@@ -36,13 +33,6 @@ class LongitudinalPlannerSP:
     self.output_v_target = 0.
     self.output_a_target = 0.
 
-  def is_e2e(self, sm: messaging.SubMaster) -> bool:
-    experimental_mode = sm['selfdriveState'].experimentalMode
-    if not self.dec.active():
-      return experimental_mode
-
-    return experimental_mode and self.dec.mode() == "blended"
-
   def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
     CS = sm['carState']
     v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
@@ -75,7 +65,6 @@ class LongitudinalPlannerSP:
 
   def update(self, sm: messaging.SubMaster) -> None:
     self.events_sp.clear()
-    self.dec.update(sm)
     self.e2e_alerts_helper.update(sm, self.events_sp)
 
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
@@ -89,12 +78,6 @@ class LongitudinalPlannerSP:
     longitudinalPlanSP.aTarget = float(self.output_a_target)
     longitudinalPlanSP.events = self.events_sp.to_msg()
 
-    # Dynamic Experimental Control
-    dec = longitudinalPlanSP.dec
-    dec.state = DecState.blended if self.dec.mode() == 'blended' else DecState.acc
-    dec.enabled = self.dec.enabled()
-    dec.active = self.dec.active()
-
     # Smart Cruise Control
     smartCruiseControl = longitudinalPlanSP.smartCruiseControl
     # Vision Control

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, EDGE_DELAY
+  AUTO_LANE_CHANGE_TIMER, ONE_SECOND_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, edge_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_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, edge_detected=False, brake_pressed=False)
+    self.alc.update_lane_change(blindspot_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, edge_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_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, edge_detected=False, brake_pressed=False)
+    self.alc.update_lane_change(blindspot_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, edge_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_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, edge_detected=False, brake_pressed=True)
+      self.alc.update_lane_change(blindspot_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, edge_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_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, edge_detected=False, brake_pressed=False)
+    self.alc.update_lane_change(blindspot_detected=True, 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, edge_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=True, 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, edge_detected=False, brake_pressed=False)
+    self.alc.update_lane_change(blindspot_detected=True, 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, edge_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_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, edge_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_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, edge_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
 
     assert not self.alc.auto_lane_change_allowed
 
@@ -206,86 +206,7 @@ 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, edge_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_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,56 +113,12 @@ class TestDesireHelperIntegration(OpenpilotTestCase):
                 left_edge_detected=False, right_edge_detected=False)
     assert dh.desire == expected_desire
 
-  def test_nudge_overrides_edge_detection(self, set_lane_turn_params):
-    """Manual steering nudge should override road edge detection and allow lane change."""
+  def test_edge_blocks_lane_change(self, set_lane_turn_params):
     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.laneChangeStarting
+    assert dh.lane_change_state == LaneChangeState.preLaneChange
     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/sunnylink/docs/README.md

@@ -367,8 +367,8 @@ enablement:
 enablement:
   - type: any
     conditions:
-      - {type: param, key: ExperimentalMode, equals: true}
-      - {type: param, key: DynamicExperimentalControl, equals: true}
+      - {type: param, key: SmartCruiseControlVision, equals: true}
+      - {type: param, key: SmartCruiseControlMap, equals: true}
 ```
 
 **Mixed: capability AND param** (only on longitudinal cars when ShowAdvancedControls is on):

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -599,26 +599,6 @@
                 }
               ]
             },
-            {
-              "key": "DynamicExperimentalControl",
-              "widget": "toggle",
-              "title": "Dynamic Experimental Control",
-              "description": "Let the model decide when to use sunnypilot ACC or sunnypilot End to End Longitudinal.",
-              "visibility": [
-                {
-                  "type": "capability",
-                  "field": "has_longitudinal_control",
-                  "equals": true
-                }
-              ],
-              "enablement": [
-                {
-                  "type": "capability",
-                  "field": "has_longitudinal_control",
-                  "equals": true
-                }
-              ]
-            },
             {
               "key": "DisengageOnAccelerator",
               "widget": "toggle",

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

@@ -16,14 +16,6 @@ sections:
     title: Experimental Mode
     enablement:
     - $ref: '#/macros/longitudinal'
-  - key: DynamicExperimentalControl
-    widget: toggle
-    title: Dynamic Experimental Control
-    description: Let the model decide when to use sunnypilot ACC or sunnypilot End to End Longitudinal.
-    visibility:
-    - $ref: '#/macros/longitudinal'
-    enablement:
-    - $ref: '#/macros/longitudinal'
   - key: DisengageOnAccelerator
     widget: toggle
     title: Disengage Cruise on Accelerator Pedal

openpilot/sunnypilot/sunnylink/tests/test_settings_changes.py

@@ -201,11 +201,6 @@ class TestSpuriousOffroadGatesDropped(OpenpilotTestCase):
     assert item is not None
     assert "offroad_only" not in _flatten_rule_types(item.get("enablement"))
 
-  def test_dynamic_experimental_has_no_offroad_only(self, schema):
-    item = _find_item(schema, "DynamicExperimentalControl")
-    assert item is not None
-    assert "offroad_only" not in _flatten_rule_types(item.get("enablement"))
-
 
 class TestNotEngagedReplacement(OpenpilotTestCase):
   @parameterized.expand([