controls: Blendful lateral policy (alpha) (#2073)

* init

* settings

* simple

* modeld_v2: blended laneful using weighted stds

* red diff that

* default to 1.0 while i test variable weighting

* blended correction over .3 s

* decay trust when model intends to move plant off lane. think going around cones and stuff

* sharp curvies have some issues :O

* quicker entry

* blend 30% plan with action  projeciton

* blending 50% on sharp curvaruerqw

* jerk limiting correctin strength so that hecker doesnt spin spin yo wheel

* handle one line flicker by trust÷2

* less is more

* winner takes all trust system in lane lines

* Ui fix

* blendful

* sm logging + qlog logging

* Update modeld.py

* Update modeld.py

* relearn ideal sim pose

* interpolate smoothness on straights to reduce mr kumar and his mdv3 jiggle shennanigans

* let unwind be handled by laneless

* ffs

* blendful

* no tap

* use constants

* fade on trust

* use blendful trust

* dont guess

* white is model path

* match width

* fade out blue

* dead

* blue only

* sqrt bar

* ill take it from here thanks

* tizi

* move to models

* cleanup

* gate like rainbow

* tick on new data

* so I can sleep at night

* keep pill current

* fine

* smoother release authority

* satisfy royjr

* decay one line and give full authority in curvy curves; kappa=curv

* he does not like 1ms

* smoother

* force tg

* oop

* hold blue

* rm

* early return

* add custom model gate & Alpha tag

* more accurate

* more

* reduce

* better

* more accurate

* ci

* small regression test

* ui: tighten Blendful gating and settings copy

* ui: clarify Blendful toggle description

* ui: gate Blendful on the launched model runner

---------

Co-authored-by: royjr <royjr96@gmail.com>
Co-authored-by: nayan <nayan8teen@gmail.com>
Co-authored-by: Jason Wen <haibin.wen3@gmail.com>

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

@@ -257,6 +257,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/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/mici/onroad/torque_bar.py

@@ -208,7 +208,7 @@ class TorqueBar(Widget):
     # draw curved line polygon torque bar
     torque_line_radius = 1200 * self._scale
     top_angle = -90
-    torque_bg_angle_span = self._torque_line_alpha_filter.x * TORQUE_ANGLE_SPAN * rect.width / 476
+    torque_bg_angle_span = self._torque_line_alpha_filter.x * TORQUE_ANGLE_SPAN * min(1.0, rect.width / (476 * self._scale))
     torque_start_angle = top_angle - torque_bg_angle_span / 2
     torque_end_angle = top_angle + torque_bg_angle_span / 2
     # centerline radius & center (you already have these values)

openpilot/selfdrive/ui/onroad/model_renderer.py

@@ -123,6 +123,7 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
     # Draw elements
     self._draw_lane_lines()
     self._draw_path()
+    self.blendful.draw(self._rect)
 
     if render_lead_indicator and radar_state:
       self._draw_lead_indicator()
@@ -172,6 +173,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/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("Adds a gentle nudge toward lane center on top of the model's own steering, stronger when lane lines are clear." +
+                                             " Yields around obstacles. Not available with the Default model."),
+                                          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. " +
@@ -322,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_runner_tinygrad
+    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/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(tr("blendful (alpha)"), "Blendful",
+                                           description=tr("Adds a gentle nudge toward lane center on top of the model's own steering, stronger when " +
+                                                          "lane lines are clear. Yields around obstacles. Not available with the Default model."))
+
+    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_runner_tinygrad
+    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

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(rect_relative=True)

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

@@ -0,0 +1,95 @@
+"""
+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.selfdrive.ui.sunnypilot.ui_state import UIStateSP
+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, rect_relative: bool = False):
+    self._readout = readout
+    self._rect_relative = rect_relative
+    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 not UIStateSP.lateral_active(ui_state):
+      return
+    if self._center_points.size and self._trust > 0.05:
+      points = self._center_points + (rect.x, rect.y) if self._rect_relative else self._center_points
+      draw_polygon(rect, 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

@@ -5,10 +5,11 @@ This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
 from openpilot.common.filter_simple import FirstOrderFilter
-from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
+from openpilot.selfdrive.ui.ui_state import ui_state
+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
+from openpilot.selfdrive.ui.sunnypilot.ui_state import UIStateSP
 from openpilot.system.ui.lib.application import gui_app
 
 
@@ -16,16 +17,12 @@ 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
   def _lateral_active(self) -> bool:
-    sm = ui_state.sm
-    if sm.valid["selfdriveStateSP"]:
-      mads = sm["selfdriveStateSP"].mads
-      if mads.available:
-        return mads.enabled and mads.state != MADSState.paused
-    return ui_state.status in (UIStatus.ENGAGED, UIStatus.LAT_ONLY)
+    return UIStateSP.lateral_active(ui_state)
 
   def _get_path_half_width(self) -> float:
     target = 0.9 if self._lateral_active else 0.40

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()
@@ -46,6 +46,7 @@ class UIStateSP:
 
     self.active_bundle = None
     self.model_runner_tinygrad: bool = False
+    self.active_runner_tinygrad: bool = False
     self.blindspot: bool = False
     self.chevron_metrics = None
     self.custom_interactive_timeout: int = 0
@@ -109,6 +110,14 @@ class UIStateSP:
   def auto_onroad_brightness(self) -> bool:
     return self.onroad_brightness in (OnroadBrightness.AUTO, OnroadBrightness.AUTO_DARK)
 
+  @staticmethod
+  def lateral_active(_ui_state) -> bool:
+    if _ui_state.sm.valid["selfdriveStateSP"]:
+      mads = _ui_state.sm["selfdriveStateSP"].mads
+      if mads.available:
+        return mads.enabled and mads.state != MADSState.paused
+    return _ui_state.status.value in ("engaged", "lat_only")
+
   @staticmethod
   def update_status(ss, ss_sp, onroad_evt) -> str:
     state = ss.state
@@ -157,6 +166,8 @@ class UIStateSP:
                                chestnut_loading=self.chestnut_loading, offroad=self.is_offroad())
     self.active_bundle = self.params.get(ACTIVE_BUNDLE_KEYS[source])
     self.model_runner_tinygrad = self.active_bundle is not None and self.active_bundle.get("runner") == "tinygrad"
+    # runner the manager launches (process_config.is_tinygrad_model); with a chestnut the big slot decides
+    self.active_runner_tinygrad = self.params.get("ModelRunnerTypeCache") == custom.ModelManagerSP.Runner.tinygrad
     # stock only counts the default big model's compiled pkl. a downloaded big bundle runs on the
     # chestnut just the same, so ChestnutState has to see it as available too.
     self.chestnut_compiled = self.chestnut_compiled or self.model_runner_tinygrad

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 # smooths the correction target 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]) # up to 50% plan y vs curvature parabola in sharp curves
+    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/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/tests/test_blendful.py

@@ -0,0 +1,112 @@
+"""
+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.realtime import DT_MDL
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.sunnypilot.modeld_v2.blendful import Blendful
+from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
+
+X = np.array(ModelConstants.X_IDXS)
+BUILD, RELEASE = 0.5, 2.0
+
+
+def frame(center=0.0, plan=0.0, k=0.0, probs=(0.9, 0.9), stds=(0.1, 0.1), width=3.5):
+  road = k * X ** 2 / 2
+  lines = np.zeros((1, 4, 33, 2))
+  lane_stds = np.ones((1, 4, 33, 2))
+  for i, off in enumerate((-1.5 * width, -width / 2, width / 2, 1.5 * width)):
+    lines[0, i, :, 0] = road + center + off
+  lane_stds[0, 1, :, 0], lane_stds[0, 2, :, 0] = stds
+  lane_probs = np.zeros((1, 8))
+  lane_probs[0, 3], lane_probs[0, 5] = probs
+  path = np.zeros((1, 33, 3))
+  path[0, :, 0] = X
+  path[0, :, 1] = road + plan * np.clip(X / 10, 0, 1)
+  return {'lane_lines': lines, 'lane_lines_stds': lane_stds, 'lane_lines_prob': lane_probs, 'plan': path}
+
+
+def drive(bf, seconds, v=20.0, k=0.0, **kw):
+  out = [bf.update(frame(k=k, **kw), v, True, k) for _ in range(int(seconds / DT_MDL))]
+  return np.array(out) * v ** 2  # m/s^2
+
+
+class TestBlendful(OpenpilotTestCase):
+  def setup_method(self):
+    self.bf = Blendful()
+
+  def test_centered_adds_nothing(self):
+    push = drive(self.bf, 5.0)
+    assert self.bf.trust > 0.9
+    assert np.abs(push).max() < 1e-3
+
+  def test_pulls_toward_lane_center(self):
+    push = drive(self.bf, 5.0, center=0.5)
+    assert push[-1] > 0.02  # + y side, where the lane center is
+    assert self.bf.trust > 0.9
+    assert np.all(np.diff(push) <= BUILD * DT_MDL + 1e-9)
+
+  def test_flipping_sides_builds_slowly(self):
+    k = 0.002  # 0.8 m/s^2 at 20 m/s
+    drive(self.bf, 5.0, k=k, center=0.5)
+    push = drive(self.bf, 5.0, k=k, center=-0.5)
+    rate = np.diff(push) / DT_MDL
+    assert push[-1] < 0
+    assert np.all(rate >= -RELEASE - 1e-6)
+    assert np.all(rate[push[:-1] <= 0] >= -BUILD - 1e-6)  # once past zero it builds
+    crossing = push[1:][(push[:-1] > 0) & (push[1:] <= 0)]
+    assert len(crossing) and np.all(crossing >= -BUILD * DT_MDL - 1e-9)
+
+  def test_weak_lines_drive_laneless(self):
+    clear = drive(Blendful(), 5.0, center=0.5)[-1]
+    faint = Blendful()
+    push = drive(faint, 5.0, center=0.5, probs=(0.1, 0.1))
+    assert faint.trust < 0.25 and abs(push[-1]) < 0.25 * clear  # faint lines = trust and push scale down
+    blurry = Blendful()
+    push = drive(blurry, 5.0, center=0.5, stds=(0.8, 0.8))
+    assert blurry.trust == 0.0 and np.abs(push).max() < 1e-6  # lines too uncertain = laneless
+
+  def test_lone_line_holds_through_a_blink_in_a_curve(self):
+    k, v = 1 / 20, 6.0
+    drive(self.bf, 6.0, v=v, k=k, plan=0.4, probs=(0.9, 0.05))
+    before = self.bf.trust
+    drive(self.bf, 0.25, v=v, k=k, plan=0.4, probs=(0.1, 0.05), stds=(1.0, 0.1))
+    bottom = self.bf.trust
+    drive(self.bf, 2.0, v=v, k=k, plan=0.4, probs=(0.9, 0.05))
+    assert before > 0.5
+    assert bottom > 0.5 * before  # keeps latched through flicker
+    assert self.bf.trust > 0.85 * before
+
+  def test_yields_when_the_model_goes_around(self):
+    drive(self.bf, 6.0)
+    push = drive(self.bf, 3.0, plan=0.8)
+    assert self.bf.going_around
+    assert self.bf.trust < 0.05
+    assert abs(push[-1]) < 0.01
+
+  def test_fit_does_not_grow_while_trust_falls(self):
+    drive(self.bf, 5.0, center=0.3)
+    for i in range(40):
+      dk, trust = self.bf.dk, self.bf.trust
+      p = 0.9 - 0.02 * i
+      self.bf.update(frame(center=0.3, plan=-0.02 * i, probs=(p, p)), 20.0, True, 0.0)
+      if self.bf.trust < trust:
+        assert abs(self.bf.dk) <= abs(dk) + 1e-12
+
+  def test_toggle_off_returns_zero_and_resets(self):
+    drive(self.bf, 5.0, center=0.5)
+    assert self.bf.curvature != 0.0
+    assert self.bf.update(frame(center=0.5), 20.0, True, 0.0, enabled=False) == 0.0
+    assert self.bf.curvature == 0.0 and self.bf.trust == 0.0
+
+  def test_fades_below_min_speed(self):
+    drive(self.bf, 5.0, v=10.0, center=0.5)
+    start = self.bf.curvature
+    out = np.array([self.bf.update(frame(center=0.5), 4.0, True, 0.0) for _ in range(100)])
+    assert self.bf.trust == 0.0
+    assert abs(out[0]) < abs(start)
+    assert abs(out[-1]) < 1e-6

openpilot/sunnypilot/sunnylink/capabilities.py

@@ -45,6 +45,7 @@ CAPABILITY_FIELDS = (
   "subaru_has_sng",
   "hyundai_alpha_long_available",
   "toyota_tss2",
+  "custom_model_active",
 )
 
 CAPABILITY_LABELS: dict[str, str] = {
@@ -68,6 +69,7 @@ CAPABILITY_LABELS: dict[str, str] = {
   "subaru_has_sng": "Subaru Stop-and-Go available",
   "hyundai_alpha_long_available": "Hyundai Alpha Longitudinal available",
   "toyota_tss2": "Toyota TSS2 platform",
+  "custom_model_active": "Non-Default model selected",
 }
 
 # Explicit defaults for non-boolean capability fields
@@ -145,6 +147,9 @@ def generate_capabilities(params: Params | None = None) -> dict:
   caps["is_development"] = params.get_bool("IsDevelopmentBranch")
   caps["stock_longitudinal"] = params.get_bool("ToyotaEnforceStockLongitudinal")
 
+  # runner the manager launches (process_config.is_tinygrad_model); with a chestnut the big slot decides
+  caps["custom_model_active"] = params.get("ModelRunnerTypeCache") == custom.ModelManagerSP.Runner.tinygrad
+
   bundle = params.get("CarPlatformBundle")
   bundle_brand = _bundle_field(bundle, "brand")
   bundle_platform = _bundle_field(bundle, "platform")

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -2021,8 +2021,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 (Alpha)",
+              "description": "Adds a gentle nudge toward lane center on top of the model's own steering, stronger when lane lines are clear. Yields around obstacles. Not available with the Default model.",
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "custom_model_active",
+                  "equals": true
+                }
+              ]
+            },
             {
               "key": "LaneTurnDesire",
               "widget": "toggle",

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/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 (Alpha)
+    description: Adds a gentle nudge toward lane center on top of the model's own steering, stronger when lane lines are clear. Yields around obstacles. Not available with the Default model.
+    enablement:
+      - $ref: '#/macros/custom_model'
   - key: LaneTurnDesire
     widget: toggle
     title: Use Lane Turn Desires