From 80494ec3cf21ff639dfb21339592789cd0ce5e80 Mon Sep 17 00:00:00 2001 From: royjr Date: Fri, 16 Oct 2026 16:50:38 -0400 Subject: [PATCH 01/54] init --- openpilot/common/params_keys.h | 1 + .../ui/mici/onroad/model_renderer.py | 2 + .../ui/sunnypilot/mici/onroad/laneful.py | 107 ++++++++++++++++++ .../sunnypilot/mici/onroad/model_renderer.py | 2 + openpilot/sunnypilot/modeld_v2/laneful.py | 79 +++++++++++++ openpilot/sunnypilot/modeld_v2/modeld.py | 12 +- 6 files changed, 201 insertions(+), 2 deletions(-) create mode 100644 openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py create mode 100644 openpilot/sunnypilot/modeld_v2/laneful.py diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index 82725e807e..019f3090ae 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -249,6 +249,7 @@ inline static std::unordered_map 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 diff --git a/openpilot/selfdrive/ui/mici/onroad/model_renderer.py b/openpilot/selfdrive/ui/mici/onroad/model_renderer.py index 805bdcc335..eaabd5f630 100644 --- a/openpilot/selfdrive/ui/mici/onroad/model_renderer.py +++ b/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: diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py new file mode 100644 index 0000000000..e0c43951c9 --- /dev/null +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py @@ -0,0 +1,107 @@ +""" +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) + + 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)) + if self._strength > 0.0: + self._draw_readout(rect) + + 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 + rl.draw_rectangle_rounded(rl.Rectangle(x, y, w, h), 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))) diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py index e3de9401ce..b3471042f4 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py +++ b/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() diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py new file mode 100644 index 0000000000..c6ec77a79b --- /dev/null +++ b/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") diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index 2fd1baf999..161b9c9a37 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/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, From f54f2d462e1f22035bd0acb15725a957981d7d3f Mon Sep 17 00:00:00 2001 From: royjr Date: Sat, 17 Oct 2026 13:31:59 -0400 Subject: [PATCH 02/54] settings --- .../ui/mici/onroad/augmented_road_view.py | 2 + .../ui/sunnypilot/mici/layouts/laneful.py | 45 +++++++++++++++++++ .../ui/sunnypilot/mici/layouts/settings.py | 8 +++- .../ui/sunnypilot/mici/onroad/laneful.py | 17 ++++++- 4 files changed, 70 insertions(+), 2 deletions(-) create mode 100644 openpilot/selfdrive/ui/sunnypilot/mici/layouts/laneful.py diff --git a/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py b/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py index 71ea7e8f8f..bcdf183ef2 100644 --- a/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py +++ b/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, _): diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/laneful.py b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/laneful.py new file mode 100644 index 0000000000..4425428200 --- /dev/null +++ b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/laneful.py @@ -0,0 +1,45 @@ +""" +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 BigButton, BigMultiToggle, GreyBigButton +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, "0.25": 0.25, "0.5": 0.5, "0.75": 0.75, "1.0": 1.0} + + +class LanefulStrengthToggle(BigMultiToggle): + """Tap to cycle LanefulStrength. Value shown as text, so no option pills.""" + def __init__(self): + super().__init__("laneful strength", list(STRENGTHS), select_callback=self._on_select) + 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]) + + def _draw_content(self, btn_y: float): + BigButton._draw_content(self, btn_y) + + +class LanefulLayoutMici(NavScroller): + def __init__(self): + super().__init__() + self._strength = LanefulStrengthToggle() + self._scroller.add_widgets([ + self._strength, + GreyBigButton("", "Nudges the car toward the center of the lane lines."), + GreyBigButton("", "Fades out when the lines are unclear and during lane changes."), + GreyBigButton("", "0.5 is gentle. 1.0 centers fastest."), + ]) + + def show_event(self): + super().show_event() + self._strength.refresh() diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/settings.py b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/settings.py index 4c0eba41ea..4351d9bc20 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/settings.py +++ b/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) diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py index e0c43951c9..2a9ed0da26 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py @@ -35,6 +35,8 @@ class LanefulRenderer: # 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.""" @@ -75,9 +77,21 @@ class LanefulRenderer: 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.""" @@ -85,7 +99,8 @@ class LanefulRenderer: 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, rl.Color(0, 0, 0, 170)) + 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 From bf93807e7b62e9127caec4f7d8126afcd75e2d81 Mon Sep 17 00:00:00 2001 From: royjr Date: Sun, 18 Oct 2026 01:05:59 -0400 Subject: [PATCH 03/54] simple --- .../ui/sunnypilot/mici/layouts/laneful.py | 20 +++++++------------ 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/laneful.py b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/laneful.py index 4425428200..404165ccf5 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/laneful.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/laneful.py @@ -4,18 +4,20 @@ 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 BigButton, BigMultiToggle, GreyBigButton +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, "0.25": 0.25, "0.5": 0.5, "0.75": 0.75, "1.0": 1.0} +STRENGTHS = {"off": 0.0, "gentle": 0.5, "firm": 1.0} class LanefulStrengthToggle(BigMultiToggle): - """Tap to cycle LanefulStrength. Value shown as text, so no option pills.""" + """Tap to cycle LanefulStrength, like the driving personality toggle.""" def __init__(self): - super().__init__("laneful strength", list(STRENGTHS), select_callback=self._on_select) + 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): @@ -25,20 +27,12 @@ class LanefulStrengthToggle(BigMultiToggle): def _on_select(self, value: str): ui_state.params.put("LanefulStrength", STRENGTHS[value]) - def _draw_content(self, btn_y: float): - BigButton._draw_content(self, btn_y) - class LanefulLayoutMici(NavScroller): def __init__(self): super().__init__() self._strength = LanefulStrengthToggle() - self._scroller.add_widgets([ - self._strength, - GreyBigButton("", "Nudges the car toward the center of the lane lines."), - GreyBigButton("", "Fades out when the lines are unclear and during lane changes."), - GreyBigButton("", "0.5 is gentle. 1.0 centers fastest."), - ]) + self._scroller.add_widget(self._strength) def show_event(self): super().show_event() From 9c7655c45e7bb87efb360f7a6eb3e7af582aebae Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Mon, 19 Oct 2026 12:11:27 -0700 Subject: [PATCH 04/54] modeld_v2: blended laneful using weighted stds --- openpilot/cereal/custom.capnp | 1 + .../ui/sunnypilot/mici/onroad/laneful.py | 18 ++--- openpilot/selfdrive/ui/sunnypilot/ui_state.py | 2 +- openpilot/sunnypilot/modeld_v2/laneful.py | 71 ++++++++----------- openpilot/sunnypilot/modeld_v2/modeld.py | 13 ++-- 5 files changed, 47 insertions(+), 58 deletions(-) diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index b8c833ed80..c281fec3af 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -465,6 +465,7 @@ struct ModelDataV2SP @0xa1680744031fdb2d { laneTurnDirection @0 :TurnDirection; leftLaneChangeEdgeBlock @1 :Bool; rightLaneChangeEdgeBlock @2 :Bool; + lanefulCurvature @3 :Float32; enum TurnDirection { none @0; diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py index 2a9ed0da26..94c5b9cba4 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py @@ -5,23 +5,20 @@ 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. +and a top-center readout of the pull, from the curvature modeld publishes in modelDataV2SP.lanefulCurvature. """ import numpy as np import pyray as rl from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.sunnypilot.modeld_v2.laneful import LOOKAHEAD_TIME, MIN_LOOKAHEAD, MAX_ERROR 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: @@ -60,15 +57,14 @@ class LanefulRenderer: 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 + # "after" path: model path bent by laneful's curvature, 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 + laneful_curvature = ui_state.sm['modelDataV2SP'].lanefulCurvature + self._pull_display.update(laneful_curvature * lookahead ** 2 / 2) # how far laneful moves the car at the lookahead self._trust_display.update(self._trust) after = path.copy() - after[:, 1] += self._strength * self._trust * error * (path[:, 0] / lookahead) ** 2 + after[:, 1] += laneful_curvature * path[:, 0] ** 2 / 2 self._after_points = renderer._map_line_to_polygon(after, 0.15, renderer._path_offset_z, renderer._get_path_length_idx(path[:, 0], lookahead)) @@ -93,7 +89,7 @@ class LanefulRenderer: return True def _draw_readout(self, rect: rl.Rectangle) -> None: - """Top-center pill: pull toward lane center in cm (capped at 50), arrow = side, + """Top-center pill: pull toward lane center in cm at the lookahead, 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 diff --git a/openpilot/selfdrive/ui/sunnypilot/ui_state.py b/openpilot/selfdrive/ui/sunnypilot/ui_state.py index e5ddd70400..f6d44fa952 100644 --- a/openpilot/selfdrive/ui/sunnypilot/ui_state.py +++ b/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() diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index c6ec77a79b..d4c5190d1f 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -12,68 +12,57 @@ 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 +from openpilot.sunnypilot.modeld_v2.constants import ModelConstants 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 +MAX_ERROR = 1.0 # m, cap on how far laneful can pull MIN_SPEED = 9.0 # m/s +LINE_STD = (0.15, 0.4) +POINT_STD = 0.1 +X_IDXS = np.array(ModelConstants.X_IDXS) class Laneful: def __init__(self): self.lane_width = FirstOrderFilter(LANE_WIDTH_DEFAULT, 10.0, DT_MDL) + self.curvature = 0.0 - 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.""" + def update(self, model_output: dict[str, np.ndarray], v_ego: float, strength: float, desire_active: bool, + model_curvature: float) -> float: + self.curvature = 0.0 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]) + stds = model_output['lane_lines_stds'][0, :, :, 0] - # 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])) + l_prob = float(probs[1] * np.interp(np.interp(lookahead, X_IDXS, stds[1]), LINE_STD, [1.0, 0.0])) + r_prob = float(probs[2] * np.interp(np.interp(lookahead, X_IDXS, stds[2]), LINE_STD, [1.0, 0.0])) + lane_trust = l_prob + r_prob - l_prob * r_prob + if lane_trust <= 0.0: + return 0.0 if l_prob * r_prob > 0.5: - self.lane_width.update(float(np.clip(right_y - left_y, *LANE_WIDTH_CLIP))) + 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 = 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 + win = (X_IDXS >= 0.5 * lookahead) & (X_IDXS <= 1.5 * lookahead) + x = X_IDXS[win] + 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) - 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") + # the error is against the arc the car will drive on the model's curvature, not the model's plan: the plan already + # bends back to where the model wants to be while the car follows the slower action, so a plan error vanishes exactly + # when the car is drifting, worst on curves. Fit the curvature change that lays that arc onto the center: e ~ dk x^2 / 2 + error = np.clip(center - model_curvature * x ** 2 / 2, -MAX_ERROR, MAX_ERROR) + dk = 2.0 * np.sum(weight * error * x ** 2) / np.sum(weight * x ** 4) + self.curvature = float(strength * lane_trust * dk) + return self.curvature diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index 8ccc8f223a..7102882b5f 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/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' @@ -230,7 +231,7 @@ class ModelState(ModelStateBase): 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, - laneful_curvature: float = 0.0) -> log.ModelDataV2.Action: + laneful: 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, @@ -242,7 +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 - desired_curvature += laneful_curvature + if laneful is not None: + desired_curvature += laneful(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) @@ -488,9 +490,9 @@ def main(demo=False): if not sm['carControl'].latActive: prev_action.desiredCurvature = sm['controlsState'].curvature - 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) + desire_active = 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, + partial(laneful.update, model_output, v_ego, laneful_strength, desire_active)) 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, @@ -507,6 +509,7 @@ 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.lanefulCurvature = laneful.curvature drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction From e64f67ec7e21115d2bc284bffb3db0992dcfc443 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Mon, 19 Oct 2026 12:36:35 -0700 Subject: [PATCH 05/54] red diff that --- openpilot/common/params_keys.h | 2 +- .../ui/sunnypilot/mici/layouts/laneful.py | 30 ++++------------ .../ui/sunnypilot/mici/onroad/laneful.py | 35 +++++++++---------- openpilot/sunnypilot/modeld_v2/modeld.py | 8 ++--- 4 files changed, 28 insertions(+), 47 deletions(-) diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index b03ecba18c..fb19536c7f 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -256,7 +256,7 @@ inline static std::unordered_map keys = { {"LagdValueCache", {PERSISTENT, FLOAT, "0.2"}}, {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}}, {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}}, - {"LanefulStrength", {PERSISTENT | BACKUP, FLOAT, "0.0"}}, + {"Laneful", {PERSISTENT | BACKUP, BOOL, "0"}}, {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}}, // mapd diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/laneful.py b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/laneful.py index 404165ccf5..7960fc6f0a 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/laneful.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/laneful.py @@ -4,36 +4,18 @@ 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.selfdrive.ui.mici.widgets.button import BigParamControl 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) + self._toggle = BigParamControl("laneful", "Laneful", + description="Keeps the car centered in its lane.\n" + + "Leans on the lane lines when they are clear, drives laneless when they are not.") + self._scroller.add_widget(self._toggle) def show_event(self): super().show_event() - self._strength.refresh() + self._toggle.refresh() diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py index 94c5b9cba4..19275a5724 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py @@ -26,24 +26,22 @@ class LanefulRenderer: 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._alpha = FirstOrderFilter(0.0, 0.3, DT_MDL) # fades that hoe 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 + laneful_curvature = ui_state.sm['modelDataV2SP'].lanefulCurvature + self._alpha.update(float(laneful_curvature != 0.0)) 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: + 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) self._after_points = np.empty((0, 2), dtype=np.float32) return @@ -60,7 +58,6 @@ class LanefulRenderer: # "after" path: model path bent by laneful's curvature, ending at the lookahead path = renderer._path.raw_points lookahead = max(ui_state.sm['carState'].vEgo * LOOKAHEAD_TIME, MIN_LOOKAHEAD) - laneful_curvature = ui_state.sm['modelDataV2SP'].lanefulCurvature self._pull_display.update(laneful_curvature * lookahead ** 2 / 2) # how far laneful moves the car at the lookahead self._trust_display.update(self._trust) after = path.copy() @@ -68,13 +65,15 @@ class LanefulRenderer: self._after_points = renderer._map_line_to_polygon(after, 0.15, renderer._path_offset_z, renderer._get_path_length_idx(path[:, 0], lookahead)) + def _color(self, rgb, alpha: float) -> rl.Color: + return rl.Color(*rgb, int(alpha * self._alpha.x)) + def draw(self, rect: rl.Rectangle) -> None: if self._center_points.size and self._trust > 0.05: - draw_polygon(rect, self._center_points, 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)) + draw_polygon(rect, self._center_points, self._color(CENTER_COLOR, np.clip(self._trust, 0.2, 1.0) * 255)) self._readout_rect = None - if self._strength > 0.0: + if self._after_points.size: + draw_polygon(rect, self._after_points, self._color(AFTER_COLOR, 200)) self._draw_readout(rect) def handle_tap(self, pos) -> bool: @@ -96,23 +95,23 @@ class LanefulRenderer: 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)) + rl.draw_rectangle_rounded(self._readout_rect, 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, rl.WHITE) + rl.draw_text_ex(self._font, text, rl.Vector2(cx - text_w / 2, cy - 24), 44, 0, self._color((255, 255, 255), 255)) - active = rl.Color(*CENTER_COLOR, int(np.clip(abs(pull) / 0.1, 0.0, 1.0) * 255)) - idle = rl.Color(255, 255, 255, 40) + active = self._color(CENTER_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, 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)) + 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.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))) + self._color(CENTER_COLOR, np.clip(0.3 + 0.7 * trust, 0.0, 1.0) * 255)) diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index 7102882b5f..6be1919eb5 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/openpilot/sunnypilot/modeld_v2/modeld.py @@ -372,7 +372,7 @@ def main(demo=False): meta_constants = load_meta_constants() RELC = RoadEdgeLaneChangeController() laneful = Laneful() - laneful_strength = 0.0 + laneful_enabled = False while True: # Keep receiving frames until we are at least 1 frame ahead of previous extra frame @@ -416,7 +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)) - laneful_strength = params.get("LanefulStrength", return_default=True) + laneful_enabled = params.get_bool("Laneful") 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) @@ -490,9 +490,9 @@ def main(demo=False): if not sm['carControl'].latActive: prev_action.desiredCurvature = sm['controlsState'].curvature - desire_active = desire != log.Desire.none or DH.lane_change_state != log.LaneChangeState.off + laneful_active = laneful_enabled and 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(laneful.update, model_output, v_ego, laneful_strength, desire_active)) + partial(laneful.update, model_output, v_ego, laneful_active)) 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, From 6c674810eec10befc8c399b5ba2017515eaae063 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Mon, 19 Oct 2026 12:38:16 -0700 Subject: [PATCH 06/54] default to 1.0 while i test variable weighting --- openpilot/sunnypilot/modeld_v2/laneful.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index d4c5190d1f..fbb4366e4f 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -30,10 +30,9 @@ class Laneful: self.lane_width = FirstOrderFilter(LANE_WIDTH_DEFAULT, 10.0, DT_MDL) self.curvature = 0.0 - def update(self, model_output: dict[str, np.ndarray], v_ego: float, strength: float, desire_active: bool, - model_curvature: float) -> float: + def update(self, model_output: dict[str, np.ndarray], v_ego: float, active: bool, model_curvature: float) -> float: self.curvature = 0.0 - if strength <= 0.0 or desire_active or v_ego < MIN_SPEED or 'lane_lines' not in model_output: + if not active or v_ego < MIN_SPEED or 'lane_lines' not in model_output: return 0.0 lookahead = max(v_ego * LOOKAHEAD_TIME, MIN_LOOKAHEAD) @@ -64,5 +63,5 @@ class Laneful: # when the car is drifting, worst on curves. Fit the curvature change that lays that arc onto the center: e ~ dk x^2 / 2 error = np.clip(center - model_curvature * x ** 2 / 2, -MAX_ERROR, MAX_ERROR) dk = 2.0 * np.sum(weight * error * x ** 2) / np.sum(weight * x ** 4) - self.curvature = float(strength * lane_trust * dk) + self.curvature = float(lane_trust * dk) return self.curvature From 54a22bd8bd394dbc1e3a4d2f17bd34fbd38662b5 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Mon, 19 Oct 2026 12:54:53 -0700 Subject: [PATCH 07/54] blended correction over .3 s --- openpilot/sunnypilot/modeld_v2/laneful.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index fbb4366e4f..6522ab7e77 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -1,12 +1,8 @@ """ -Copyright (c) 2021-, sunnypilot, and a number of other contributors. +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. - -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 @@ -20,6 +16,7 @@ LOOKAHEAD_TIME = 1.5 # s MIN_LOOKAHEAD = 10.0 # m MAX_ERROR = 1.0 # m, cap on how far laneful can pull MIN_SPEED = 9.0 # m/s +SMOOTH_TAU = 0.3 # s LINE_STD = (0.15, 0.4) POINT_STD = 0.1 X_IDXS = np.array(ModelConstants.X_IDXS) @@ -28,11 +25,12 @@ X_IDXS = np.array(ModelConstants.X_IDXS) class Laneful: def __init__(self): self.lane_width = FirstOrderFilter(LANE_WIDTH_DEFAULT, 10.0, DT_MDL) + self.correction = FirstOrderFilter(0.0, SMOOTH_TAU, DT_MDL) # correcting over .3s reduces jerk self.curvature = 0.0 def update(self, model_output: dict[str, np.ndarray], v_ego: float, active: bool, model_curvature: float) -> float: - self.curvature = 0.0 if not active or v_ego < MIN_SPEED or 'lane_lines' not in model_output: + self.curvature = self.correction.x = 0.0 return 0.0 lookahead = max(v_ego * LOOKAHEAD_TIME, MIN_LOOKAHEAD) @@ -44,6 +42,7 @@ class Laneful: r_prob = float(probs[2] * np.interp(np.interp(lookahead, X_IDXS, stds[2]), LINE_STD, [1.0, 0.0])) lane_trust = l_prob + r_prob - l_prob * r_prob if lane_trust <= 0.0: + self.curvature = self.correction.x = 0.0 return 0.0 if l_prob * r_prob > 0.5: @@ -63,5 +62,5 @@ class Laneful: # when the car is drifting, worst on curves. Fit the curvature change that lays that arc onto the center: e ~ dk x^2 / 2 error = np.clip(center - model_curvature * x ** 2 / 2, -MAX_ERROR, MAX_ERROR) dk = 2.0 * np.sum(weight * error * x ** 2) / np.sum(weight * x ** 4) - self.curvature = float(lane_trust * dk) + self.curvature = float(self.correction.update(lane_trust * dk)) return self.curvature From 0b3459fbdc7018c312d0431f89f7a9e312f9b3da Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Mon, 19 Oct 2026 13:10:03 -0700 Subject: [PATCH 08/54] decay trust when model intends to move plant off lane. think going around cones and stuff --- openpilot/sunnypilot/modeld_v2/laneful.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index 6522ab7e77..109953f517 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -17,6 +17,7 @@ MIN_LOOKAHEAD = 10.0 # m MAX_ERROR = 1.0 # m, cap on how far laneful can pull MIN_SPEED = 9.0 # m/s SMOOTH_TAU = 0.3 # s +YIELD = (0.5, 1.0) # m LINE_STD = (0.15, 0.4) POINT_STD = 0.1 X_IDXS = np.array(ModelConstants.X_IDXS) @@ -57,10 +58,14 @@ class Laneful: 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) - # the error is against the arc the car will drive on the model's curvature, not the model's plan: the plan already - # bends back to where the model wants to be while the car follows the slower action, so a plan error vanishes exactly - # when the car is drifting, worst on curves. Fit the curvature change that lays that arc onto the center: e ~ dk x^2 / 2 + # Fit curvature to place the plant in center: e ~ dk x^2 / 2 error = np.clip(center - model_curvature * x ** 2 / 2, -MAX_ERROR, MAX_ERROR) dk = 2.0 * np.sum(weight * error * x ** 2) / np.sum(weight * x ** 4) + + # A model planning to end up further off center than the plant is now is likely going around something + center_now = (l_prob * (lines[1, 0] + half_width) + r_prob * (lines[2, 0] - half_width)) / (l_prob + r_prob) + plan_y = np.interp(lookahead, model_output['plan'][0, :, 0], model_output['plan'][0, :, 1]) + away = abs(plan_y - np.interp(lookahead, x, center)) - abs(center_now) + lane_trust *= np.interp(away, YIELD, [1.0, 0.0]) self.curvature = float(self.correction.update(lane_trust * dk)) return self.curvature From 563fa3c7825ed1d18f4b4afde2f793a86895c788 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Mon, 19 Oct 2026 13:38:52 -0700 Subject: [PATCH 09/54] sharp curvies have some issues :O --- openpilot/sunnypilot/modeld_v2/laneful.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index 109953f517..5c582b1c3d 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -18,7 +18,7 @@ MAX_ERROR = 1.0 # m, cap on how far laneful can pull MIN_SPEED = 9.0 # m/s SMOOTH_TAU = 0.3 # s YIELD = (0.5, 1.0) # m -LINE_STD = (0.15, 0.4) +LINE_STD = (0.2, 0.6) POINT_STD = 0.1 X_IDXS = np.array(ModelConstants.X_IDXS) @@ -39,8 +39,8 @@ class Laneful: stds = model_output['lane_lines_stds'][0, :, :, 0] probs = model_output['lane_lines_prob'][0, 1::2] - l_prob = float(probs[1] * np.interp(np.interp(lookahead, X_IDXS, stds[1]), LINE_STD, [1.0, 0.0])) - r_prob = float(probs[2] * np.interp(np.interp(lookahead, X_IDXS, stds[2]), LINE_STD, [1.0, 0.0])) + l_prob = float(probs[1] * np.interp(np.interp(0.5 * lookahead, X_IDXS, stds[1]), LINE_STD, [1.0, 0.0])) + r_prob = float(probs[2] * np.interp(np.interp(0.5 * lookahead, X_IDXS, stds[2]), LINE_STD, [1.0, 0.0])) lane_trust = l_prob + r_prob - l_prob * r_prob if lane_trust <= 0.0: self.curvature = self.correction.x = 0.0 @@ -65,7 +65,7 @@ class Laneful: # A model planning to end up further off center than the plant is now is likely going around something center_now = (l_prob * (lines[1, 0] + half_width) + r_prob * (lines[2, 0] - half_width)) / (l_prob + r_prob) plan_y = np.interp(lookahead, model_output['plan'][0, :, 0], model_output['plan'][0, :, 1]) - away = abs(plan_y - np.interp(lookahead, x, center)) - abs(center_now) + away = abs(plan_y - np.interp(lookahead, x, center)) - abs(center_now) - 0.15 * abs(model_curvature) * lookahead ** 2 / 2 lane_trust *= np.interp(away, YIELD, [1.0, 0.0]) self.curvature = float(self.correction.update(lane_trust * dk)) return self.curvature From 8a508606483d2615e53a23744c3f69582b01a854 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Mon, 19 Oct 2026 14:16:46 -0700 Subject: [PATCH 10/54] quicker entry --- openpilot/sunnypilot/modeld_v2/laneful.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index 5c582b1c3d..1f2d29a027 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -16,7 +16,7 @@ LOOKAHEAD_TIME = 1.5 # s MIN_LOOKAHEAD = 10.0 # m MAX_ERROR = 1.0 # m, cap on how far laneful can pull MIN_SPEED = 9.0 # m/s -SMOOTH_TAU = 0.3 # s +SMOOTH_TAU = 0.15 # s YIELD = (0.5, 1.0) # m LINE_STD = (0.2, 0.6) POINT_STD = 0.1 @@ -65,7 +65,7 @@ class Laneful: # A model planning to end up further off center than the plant is now is likely going around something center_now = (l_prob * (lines[1, 0] + half_width) + r_prob * (lines[2, 0] - half_width)) / (l_prob + r_prob) plan_y = np.interp(lookahead, model_output['plan'][0, :, 0], model_output['plan'][0, :, 1]) - away = abs(plan_y - np.interp(lookahead, x, center)) - abs(center_now) - 0.15 * abs(model_curvature) * lookahead ** 2 / 2 + away = abs(plan_y - np.interp(lookahead, x, center)) - abs(center_now) - 0.25 * abs(model_curvature) * lookahead ** 2 / 2 lane_trust *= np.interp(away, YIELD, [1.0, 0.0]) self.curvature = float(self.correction.update(lane_trust * dk)) return self.curvature From 46c0b319975c64d46318a7f44f2755bb1b2a3ace Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Mon, 19 Oct 2026 16:28:07 -0700 Subject: [PATCH 11/54] blend 30% plan with action projeciton --- openpilot/sunnypilot/modeld_v2/laneful.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index 1f2d29a027..5cf6c218c7 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -59,7 +59,8 @@ class Laneful: 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 - error = np.clip(center - model_curvature * x ** 2 / 2, -MAX_ERROR, MAX_ERROR) + plan_x = np.interp(x, model_output['plan'][0, :, 0], model_output['plan'][0, :, 1]) + error = np.clip(center - (0.7 * model_curvature * x ** 2 / 2 + 0.3 * plan_x), -MAX_ERROR, MAX_ERROR) dk = 2.0 * np.sum(weight * error * x ** 2) / np.sum(weight * x ** 4) # A model planning to end up further off center than the plant is now is likely going around something From a3ed6bd2f08bd27e30b73143d81af2a457bcd842 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Mon, 19 Oct 2026 16:49:48 -0700 Subject: [PATCH 12/54] blending 50% on sharp curvaruerqw --- openpilot/sunnypilot/modeld_v2/laneful.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index 5cf6c218c7..5bb3452b7a 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -60,7 +60,8 @@ class Laneful: # 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]) - error = np.clip(center - (0.7 * model_curvature * x ** 2 / 2 + 0.3 * plan_x), -MAX_ERROR, MAX_ERROR) + plan_share = np.interp(abs(model_curvature) * lookahead ** 2 / 2, [0.5, 2.0], [0.3, 0.5]) # blending up to 50% plan with action during sharp curvature + error = np.clip(center - ((1 - plan_share) * model_curvature * x ** 2 / 2 + plan_share * plan_x), -MAX_ERROR, MAX_ERROR) dk = 2.0 * np.sum(weight * error * x ** 2) / np.sum(weight * x ** 4) # A model planning to end up further off center than the plant is now is likely going around something From ed4e8aeea685a174af5a9db9b87ede8bdbbf9847 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Mon, 19 Oct 2026 20:20:45 -0700 Subject: [PATCH 13/54] jerk limiting correctin strength so that hecker doesnt spin spin yo wheel --- openpilot/sunnypilot/modeld_v2/laneful.py | 26 ++++++++++++++--------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index 5bb3452b7a..22b50906c0 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -16,7 +16,7 @@ LOOKAHEAD_TIME = 1.5 # s MIN_LOOKAHEAD = 10.0 # m MAX_ERROR = 1.0 # m, cap on how far laneful can pull MIN_SPEED = 9.0 # m/s -SMOOTH_TAU = 0.15 # s +JERK_LIMIT = 0.5 # m/s^3 YIELD = (0.5, 1.0) # m LINE_STD = (0.2, 0.6) POINT_STD = 0.1 @@ -26,24 +26,31 @@ X_IDXS = np.array(ModelConstants.X_IDXS) class Laneful: def __init__(self): self.lane_width = FirstOrderFilter(LANE_WIDTH_DEFAULT, 10.0, DT_MDL) - self.correction = FirstOrderFilter(0.0, SMOOTH_TAU, DT_MDL) # correcting over .3s reduces jerk + self.line_probs = FirstOrderFilter(np.zeros(2), 0.5, DT_MDL) self.curvature = 0.0 def update(self, model_output: dict[str, np.ndarray], v_ego: float, active: bool, model_curvature: float) -> float: - if not active or v_ego < MIN_SPEED or 'lane_lines' not in model_output: - self.curvature = self.correction.x = 0.0 + if not active: + self.curvature = 0.0 + return 0.0 + target = self._target(model_output, v_ego, model_curvature) + step = JERK_LIMIT / max(v_ego, 1.0) ** 2 * DT_MDL + self.curvature = float(np.clip(target, self.curvature - step, self.curvature + step)) + return self.curvature + + def _target(self, model_output: dict[str, np.ndarray], v_ego: float, model_curvature: float) -> float: + if 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] stds = model_output['lane_lines_stds'][0, :, :, 0] - probs = model_output['lane_lines_prob'][0, 1::2] - l_prob = float(probs[1] * np.interp(np.interp(0.5 * lookahead, X_IDXS, stds[1]), LINE_STD, [1.0, 0.0])) - r_prob = float(probs[2] * np.interp(np.interp(0.5 * lookahead, X_IDXS, stds[2]), LINE_STD, [1.0, 0.0])) + probs = self.line_probs.update(model_output['lane_lines_prob'][0, 1::2][1:3]) + 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])) lane_trust = l_prob + r_prob - l_prob * r_prob if lane_trust <= 0.0: - self.curvature = self.correction.x = 0.0 return 0.0 if l_prob * r_prob > 0.5: @@ -69,5 +76,4 @@ class Laneful: plan_y = np.interp(lookahead, model_output['plan'][0, :, 0], model_output['plan'][0, :, 1]) away = abs(plan_y - np.interp(lookahead, x, center)) - abs(center_now) - 0.25 * abs(model_curvature) * lookahead ** 2 / 2 lane_trust *= np.interp(away, YIELD, [1.0, 0.0]) - self.curvature = float(self.correction.update(lane_trust * dk)) - return self.curvature + return float(lane_trust * dk) From 61154655cdfcae298dcec91f3e32f2a564758e20 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Mon, 19 Oct 2026 20:25:29 -0700 Subject: [PATCH 14/54] =?UTF-8?q?handle=20one=20line=20flicker=20by=20trus?= =?UTF-8?q?t=C3=B72?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- openpilot/sunnypilot/modeld_v2/laneful.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index 22b50906c0..6f5f126f3a 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -49,7 +49,7 @@ class Laneful: probs = self.line_probs.update(model_output['lane_lines_prob'][0, 1::2][1:3]) 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])) - lane_trust = l_prob + r_prob - l_prob * r_prob + lane_trust = (l_prob + r_prob) / 2 if lane_trust <= 0.0: return 0.0 From e9fc7a24653121f8dba7dc05e1da02c26ec6a1ee Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Mon, 19 Oct 2026 21:43:35 -0700 Subject: [PATCH 15/54] less is more --- openpilot/sunnypilot/modeld_v2/laneful.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index 6f5f126f3a..40b869d92d 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -15,7 +15,7 @@ LANE_WIDTH_CLIP = (2.8, 4.5) # m LOOKAHEAD_TIME = 1.5 # s MIN_LOOKAHEAD = 10.0 # m MAX_ERROR = 1.0 # m, cap on how far laneful can pull -MIN_SPEED = 9.0 # m/s +MIN_SPEED = 5.0 # m/s JERK_LIMIT = 0.5 # m/s^3 YIELD = (0.5, 1.0) # m LINE_STD = (0.2, 0.6) From 4d2d3f69f514f707c159e5e476059c23fa38bf4e Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Mon, 19 Oct 2026 23:11:22 -0700 Subject: [PATCH 16/54] winner takes all trust system in lane lines --- openpilot/sunnypilot/modeld_v2/laneful.py | 28 +++++++++++++++-------- 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index 40b869d92d..02f7331b6a 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -10,22 +10,21 @@ 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.7 # m +LANE_WIDTH_DEFAULT = 3.2914 # m (averaged narrow lane in downtown streets, ~10.8ft usable) LANE_WIDTH_CLIP = (2.8, 4.5) # m LOOKAHEAD_TIME = 1.5 # s MIN_LOOKAHEAD = 10.0 # m -MAX_ERROR = 1.0 # m, cap on how far laneful can pull MIN_SPEED = 5.0 # m/s -JERK_LIMIT = 0.5 # m/s^3 -YIELD = (0.5, 1.0) # m LINE_STD = (0.2, 0.6) POINT_STD = 0.1 X_IDXS = np.array(ModelConstants.X_IDXS) -class Laneful: +class Laneful: # TODO-SP: Rename this to blendful or something similar because it is not pure laneful anymore. def __init__(self): self.lane_width = FirstOrderFilter(LANE_WIDTH_DEFAULT, 10.0, DT_MDL) + 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.curvature = 0.0 @@ -34,7 +33,8 @@ class Laneful: self.curvature = 0.0 return 0.0 target = self._target(model_output, v_ego, model_curvature) - step = JERK_LIMIT / max(v_ego, 1.0) ** 2 * DT_MDL + releasing = abs(target) < abs(self.curvature) or target * self.curvature < 0 + step = (2.0 if releasing else 0.5) / max(v_ego, 1.0) ** 2 * DT_MDL # jerk over 1 second self.curvature = float(np.clip(target, self.curvature - step, self.curvature + step)) return self.curvature @@ -47,9 +47,19 @@ class Laneful: stds = model_output['lane_lines_stds'][0, :, :, 0] probs = self.line_probs.update(model_output['lane_lines_prob'][0, 1::2][1:3]) + gone = self.line_gone.update((model_output['lane_lines_prob'][0, 1::2][1:3] < 0.3).astype(float)) + dropping = bool(np.any(probs < 0.6 * self.line_probs_slow.update(model_output['lane_lines_prob'][0, 1::2][1:3]))) 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])) - lane_trust = (l_prob + r_prob) / 2 + # one line steadily on its own still centers at half say conf. a line that is disappearing gives control back to laneless + if gone[1] > 0.8 and l_prob > 0.5: + lane_trust = l_prob / 2 + elif gone[0] > 0.8 and r_prob > 0.5: + lane_trust = r_prob / 2 + elif dropping: + lane_trust = min(l_prob, r_prob) + else: + lane_trust = l_prob + r_prob - l_prob * r_prob if lane_trust <= 0.0: return 0.0 @@ -68,12 +78,12 @@ class Laneful: # Fit curvature to place the plant in center: e ~ dk x^2 / 2 plan_x = np.interp(x, model_output['plan'][0, :, 0], model_output['plan'][0, :, 1]) plan_share = np.interp(abs(model_curvature) * lookahead ** 2 / 2, [0.5, 2.0], [0.3, 0.5]) # blending up to 50% plan with action during sharp curvature - error = np.clip(center - ((1 - plan_share) * model_curvature * x ** 2 / 2 + plan_share * plan_x), -MAX_ERROR, MAX_ERROR) + 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 off center than the plant is now is likely going around something center_now = (l_prob * (lines[1, 0] + half_width) + r_prob * (lines[2, 0] - half_width)) / (l_prob + r_prob) plan_y = np.interp(lookahead, model_output['plan'][0, :, 0], model_output['plan'][0, :, 1]) away = abs(plan_y - np.interp(lookahead, x, center)) - abs(center_now) - 0.25 * abs(model_curvature) * lookahead ** 2 / 2 - lane_trust *= np.interp(away, YIELD, [1.0, 0.0]) + lane_trust *= np.interp(away, (0.5, 1.0), [1.0, 0.0]) return float(lane_trust * dk) From 77d0c2d665680b078cc2b5a3a92768b31dd86b55 Mon Sep 17 00:00:00 2001 From: James Vecellio-Grant <159560811+Discountchubbs@users.noreply.github.com> Date: Tue, 20 Oct 2026 07:16:14 -0700 Subject: [PATCH 17/54] Ui fix --- openpilot/sunnypilot/modeld_v2/laneful.py | 1 + 1 file changed, 1 insertion(+) diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index 02f7331b6a..d25582d838 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -17,6 +17,7 @@ MIN_LOOKAHEAD = 10.0 # m MIN_SPEED = 5.0 # m/s LINE_STD = (0.2, 0.6) POINT_STD = 0.1 +MAX_ERROR = 1.0 X_IDXS = np.array(ModelConstants.X_IDXS) From 0af369cae0ed6f4be4bb1c5b2a17e14f62b19707 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Tue, 20 Oct 2026 13:28:20 -0700 Subject: [PATCH 18/54] blendful --- openpilot/sunnypilot/modeld_v2/laneful.py | 61 ++++++++++++++++------- 1 file changed, 44 insertions(+), 17 deletions(-) diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index d25582d838..7a39343612 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -10,7 +10,7 @@ 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.2914 # m (averaged narrow lane in downtown streets, ~10.8ft usable) +LANE_WIDTH_DEFAULT = 3.6 # m (reseeded to model's lane width upon initialization) LANE_WIDTH_CLIP = (2.8, 4.5) # m LOOKAHEAD_TIME = 1.5 # s MIN_LOOKAHEAD = 10.0 # m @@ -18,23 +18,30 @@ MIN_SPEED = 5.0 # m/s LINE_STD = (0.2, 0.6) POINT_STD = 0.1 MAX_ERROR = 1.0 +YIELD = (0.2, 0.45) X_IDXS = np.array(ModelConstants.X_IDXS) class Laneful: # TODO-SP: Rename this to blendful or something similar because it is not pure laneful anymore. def __init__(self): - self.lane_width = FirstOrderFilter(LANE_WIDTH_DEFAULT, 10.0, DT_MDL) + 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.trust = 0.0 self.curvature = 0.0 def update(self, model_output: dict[str, np.ndarray], v_ego: float, active: bool, model_curvature: float) -> float: - if not active: - self.curvature = 0.0 - return 0.0 - target = self._target(model_output, v_ego, model_curvature) - releasing = abs(target) < abs(self.curvature) or target * self.curvature < 0 + fading = not active or v_ego < MIN_SPEED + if fading: + self.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) + releasing = not fading and (abs(target) < abs(self.curvature) or target * self.curvature < 0) step = (2.0 if releasing else 0.5) / max(v_ego, 1.0) ** 2 * DT_MDL # jerk over 1 second self.curvature = float(np.clip(target, self.curvature - step, self.curvature + step)) return self.curvature @@ -62,15 +69,16 @@ class Laneful: # TODO-SP: Rename this to blendful or something similar because i else: lane_trust = l_prob + r_prob - l_prob * r_prob if lane_trust <= 0.0: + self.trust = 0.0 return 0.0 - 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 = self.lane_width.x / 2 - 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) @@ -82,9 +90,28 @@ class Laneful: # TODO-SP: Rename this to blendful or something similar because i 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 off center than the plant is now is likely going around something - center_now = (l_prob * (lines[1, 0] + half_width) + r_prob * (lines[2, 0] - half_width)) / (l_prob + r_prob) + # 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]) - away = abs(plan_y - np.interp(lookahead, x, center)) - abs(center_now) - 0.25 * abs(model_curvature) * lookahead ** 2 / 2 - lane_trust *= np.interp(away, (0.5, 1.0), [1.0, 0.0]) - return float(lane_trust * dk) + offset = plan_y - np.interp(lookahead, x, center) + 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 + if inside_curve: + self.going_around = False + elif away > YIELD[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]) + + self.trust = lane_trust if lane_trust < self.trust else self.trust + (lane_trust - self.trust) * DT_MDL / (1.5 + DT_MDL) + return float(self.trust * dk) From 7847dbf148e196eed3ba0bd01d2bf5815b8139c4 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Tue, 20 Oct 2026 15:47:46 -0700 Subject: [PATCH 19/54] sm logging + qlog logging --- openpilot/cereal/custom.capnp | 3 +++ openpilot/cereal/services.py | 2 +- openpilot/sunnypilot/modeld_v2/laneful.py | 2 ++ openpilot/sunnypilot/modeld_v2/modeld.py | 3 +++ 4 files changed, 9 insertions(+), 1 deletion(-) diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index c281fec3af..0a5f431545 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -466,6 +466,9 @@ struct ModelDataV2SP @0xa1680744031fdb2d { leftLaneChangeEdgeBlock @1 :Bool; rightLaneChangeEdgeBlock @2 :Bool; lanefulCurvature @3 :Float32; + lanefulTrust @4 :Float32; + modelCurvature @5 :Float32; + lanefulGoingAround @6 :Bool; enum TurnDirection { none @0; diff --git a/openpilot/cereal/services.py b/openpilot/cereal/services.py index 541a06298a..449778ba2a 100755 --- a/openpilot/cereal/services.py +++ b/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 diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index 7a39343612..1ef2777da3 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -31,6 +31,7 @@ class Laneful: # TODO-SP: Rename this to blendful or something similar because i self.usual_offset = FirstOrderFilter(0.0, 10.0, DT_MDL, initialized=False) self.going_around = False self.trust = 0.0 + self.model_curvature = 0.0 self.curvature = 0.0 def update(self, model_output: dict[str, np.ndarray], v_ego: float, active: bool, model_curvature: float) -> float: @@ -40,6 +41,7 @@ class Laneful: # TODO-SP: Rename this to blendful or something similar because i self.usual_offset.initialized = False self.lane_width.initialized = False self.going_around = False + self.model_curvature = model_curvature target = 0.0 if fading else self._target(model_output, v_ego, model_curvature) releasing = not fading and (abs(target) < abs(self.curvature) or target * self.curvature < 0) step = (2.0 if releasing else 0.5) / max(v_ego, 1.0) ** 2 * DT_MDL # jerk over 1 second diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index 6be1919eb5..2804f8b8e0 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/openpilot/sunnypilot/modeld_v2/modeld.py @@ -510,6 +510,9 @@ def main(demo=False): mdv2sp_send.valid = modelv2_send.valid mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction mdv2sp_send.modelDataV2SP.lanefulCurvature = laneful.curvature + mdv2sp_send.modelDataV2SP.lanefulTrust = laneful.trust + mdv2sp_send.modelDataV2SP.modelCurvature = laneful.model_curvature + mdv2sp_send.modelDataV2SP.lanefulGoingAround = laneful.going_around drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction From 644e23f8ef2f3af51ed04c9b85d08ce70e2fb65f Mon Sep 17 00:00:00 2001 From: James Vecellio-Grant <159560811+Discountchubbs@users.noreply.github.com> Date: Tue, 20 Oct 2026 17:55:15 -0700 Subject: [PATCH 20/54] Update modeld.py --- openpilot/sunnypilot/modeld_v2/modeld.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index 2804f8b8e0..0d6341460d 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/openpilot/sunnypilot/modeld_v2/modeld.py @@ -511,7 +511,7 @@ def main(demo=False): mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction mdv2sp_send.modelDataV2SP.lanefulCurvature = laneful.curvature mdv2sp_send.modelDataV2SP.lanefulTrust = laneful.trust - mdv2sp_send.modelDataV2SP.modelCurvature = laneful.model_curvature + mdv2sp_send.modelDataV2SP.modelCurvature = float(laneful.model_curvature) mdv2sp_send.modelDataV2SP.lanefulGoingAround = laneful.going_around drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction From 11603b2de7489b16afe4855681f22871bcca2aed Mon Sep 17 00:00:00 2001 From: James Vecellio-Grant <159560811+Discountchubbs@users.noreply.github.com> Date: Tue, 20 Oct 2026 18:02:52 -0700 Subject: [PATCH 21/54] Update modeld.py --- openpilot/sunnypilot/modeld_v2/modeld.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index 0d6341460d..ba22ccec71 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/openpilot/sunnypilot/modeld_v2/modeld.py @@ -510,9 +510,9 @@ def main(demo=False): mdv2sp_send.valid = modelv2_send.valid mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction mdv2sp_send.modelDataV2SP.lanefulCurvature = laneful.curvature - mdv2sp_send.modelDataV2SP.lanefulTrust = laneful.trust + mdv2sp_send.modelDataV2SP.lanefulTrust = float(laneful.trust) mdv2sp_send.modelDataV2SP.modelCurvature = float(laneful.model_curvature) - mdv2sp_send.modelDataV2SP.lanefulGoingAround = laneful.going_around + mdv2sp_send.modelDataV2SP.lanefulGoingAround = bool(laneful.going_around) drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction From e87a605fb2829b62c86b972104f0f5812c679e12 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Tue, 20 Oct 2026 18:39:44 -0700 Subject: [PATCH 22/54] relearn ideal sim pose --- openpilot/sunnypilot/modeld_v2/laneful.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index 1ef2777da3..62222f7080 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -97,6 +97,8 @@ class Laneful: # TODO-SP: Rename this to blendful or something similar because i 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) From 7707a3e3842a22395585dd60f8d6056a557ff321 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Tue, 20 Oct 2026 22:56:10 -0700 Subject: [PATCH 23/54] interpolate smoothness on straights to reduce mr kumar and his mdv3 jiggle shennanigans --- openpilot/sunnypilot/modeld_v2/laneful.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index 62222f7080..59ca6f2518 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -32,6 +32,7 @@ class Laneful: # TODO-SP: Rename this to blendful or something similar because i self.going_around = False self.trust = 0.0 self.model_curvature = 0.0 + self.smooth = 0.0 # smoothens lanelines below .53 lat accel self.curvature = 0.0 def update(self, model_output: dict[str, np.ndarray], v_ego: float, active: bool, model_curvature: float) -> float: @@ -43,6 +44,7 @@ class Laneful: # TODO-SP: Rename this to blendful or something similar because i self.going_around = False self.model_curvature = model_curvature 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.25, 0.0])) releasing = not fading and (abs(target) < abs(self.curvature) or target * self.curvature < 0) step = (2.0 if releasing else 0.5) / max(v_ego, 1.0) ** 2 * DT_MDL # jerk over 1 second self.curvature = float(np.clip(target, self.curvature - step, self.curvature + step)) From 62cf10ff05d9755e0a2ed82bbf5a14219377f229 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Wed, 21 Oct 2026 08:17:55 -0700 Subject: [PATCH 24/54] let unwind be handled by laneless --- openpilot/sunnypilot/modeld_v2/laneful.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index 59ca6f2518..d30336e168 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -30,6 +30,7 @@ class Laneful: # TODO-SP: Rename this to blendful or something similar because i 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.model_curvature = 0.0 self.smooth = 0.0 # smoothens lanelines below .53 lat accel @@ -106,9 +107,10 @@ class Laneful: # TODO-SP: Rename this to blendful or something similar because i 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]: + elif away > YIELD[0] and self.since_turning > 2.0: self.going_around = True elif swerve < YIELD[0] / 2: self.going_around = False From ee7bf203fe2088b1c40fe7b4997fea6fce4b5bd4 Mon Sep 17 00:00:00 2001 From: nayan Date: Wed, 21 Oct 2026 17:20:08 -0400 Subject: [PATCH 25/54] ffs --- openpilot/sunnypilot/sunnylink/settings_ui.json | 8 +++++++- .../sunnylink/settings_ui_src/pages/models.yaml | 8 +++++++- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index c9d4c826f4..4799456354 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -2021,8 +2021,14 @@ { "id": "model_behavior", "title": "Model Behavior", - "description": "Lane desire and lead-vehicle awareness tuning", + "description": "Lane desire, blended laneful, and lead-vehicle awareness tuning", "items": [ + { + "key": "Laneful", + "widget": "toggle", + "title": "Use Blended Laneful Mode", + "description": "Nudges the model's path toward lane center with a smoothly ramped correction that scales with lane-line confidence. It fades out automatically when lines are unclear, mid-lane-change, during turns, or when the model is going around something, so you keep laneless behavior plus extra centering." + }, { "key": "LaneTurnDesire", "widget": "toggle", diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml index bcb8b895b9..3820467c7c 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml +++ b/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, blended laneful, and lead-vehicle awareness tuning items: + - key: Laneful + widget: toggle + title: Use Blended Laneful Mode + description: Nudges the model's path toward lane center with a smoothly ramped correction that scales with + lane-line confidence. It fades out automatically when lines are unclear, mid-lane-change, during turns, + or when the model is going around something, so you keep laneless behavior plus extra centering. - key: LaneTurnDesire widget: toggle title: Use Lane Turn Desires From 5854346c2ffd5fdcaca0ebd1e15e3f3addb40401 Mon Sep 17 00:00:00 2001 From: royjr Date: Thu, 22 Oct 2026 17:39:50 -0400 Subject: [PATCH 26/54] blendful --- openpilot/cereal/custom.capnp | 6 ++--- openpilot/common/params_keys.h | 2 +- .../ui/mici/onroad/augmented_road_view.py | 2 +- .../ui/mici/onroad/model_renderer.py | 4 +-- .../mici/layouts/{laneful.py => blendful.py} | 4 +-- .../ui/sunnypilot/mici/layouts/settings.py | 10 +++---- .../mici/onroad/{laneful.py => blendful.py} | 24 ++++++++--------- .../sunnypilot/mici/onroad/model_renderer.py | 4 +-- .../modeld_v2/{laneful.py => blendful.py} | 2 +- openpilot/sunnypilot/modeld_v2/modeld.py | 26 +++++++++---------- .../sunnypilot/sunnylink/settings_ui.json | 6 ++--- .../settings_ui_src/pages/models.yaml | 6 ++--- 12 files changed, 48 insertions(+), 48 deletions(-) rename openpilot/selfdrive/ui/sunnypilot/mici/layouts/{laneful.py => blendful.py} (88%) rename openpilot/selfdrive/ui/sunnypilot/mici/onroad/{laneful.py => blendful.py} (85%) rename openpilot/sunnypilot/modeld_v2/{laneful.py => blendful.py} (98%) diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index 9e28948352..ca4cee003f 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -468,10 +468,10 @@ struct ModelDataV2SP @0xa1680744031fdb2d { laneTurnDirection @0 :TurnDirection; leftLaneChangeEdgeBlock @1 :Bool; rightLaneChangeEdgeBlock @2 :Bool; - lanefulCurvature @3 :Float32; - lanefulTrust @4 :Float32; + blendfulCurvature @3 :Float32; + blendfulTrust @4 :Float32; modelCurvature @5 :Float32; - lanefulGoingAround @6 :Bool; + blendfulGoingAround @6 :Bool; enum TurnDirection { none @0; diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index fb19536c7f..bed8e93b6a 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -256,7 +256,7 @@ inline static std::unordered_map keys = { {"LagdValueCache", {PERSISTENT, FLOAT, "0.2"}}, {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}}, {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}}, - {"Laneful", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"Blendful", {PERSISTENT | BACKUP, BOOL, "0"}}, {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}}, // mapd diff --git a/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py b/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py index 1ec48ad981..7d102306a1 100644 --- a/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py +++ b/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py @@ -206,7 +206,7 @@ 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): + if self._model_renderer.blendful.handle_tap(mouse_pos): return super()._handle_mouse_release(mouse_pos) diff --git a/openpilot/selfdrive/ui/mici/onroad/model_renderer.py b/openpilot/selfdrive/ui/mici/onroad/model_renderer.py index c9a524ecc3..d86b31aff5 100644 --- a/openpilot/selfdrive/ui/mici/onroad/model_renderer.py +++ b/openpilot/selfdrive/ui/mici/onroad/model_renderer.py @@ -141,7 +141,7 @@ class ModelRenderer(Widget, ModelRendererSP): if ui_state.status != UIStatus.DISENGAGED: self._draw_lane_lines() self._draw_path() - self.laneful.draw(self._rect) + self.blendful.draw(self._rect) if render_lead_indicator: self._update_leads(sm) @@ -220,7 +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.laneful.update(self, max_idx) + self.blendful.update(self, max_idx) # Update path using raw points if lead and lead.present: diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/laneful.py b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/blendful.py similarity index 88% rename from openpilot/selfdrive/ui/sunnypilot/mici/layouts/laneful.py rename to openpilot/selfdrive/ui/sunnypilot/mici/layouts/blendful.py index 7960fc6f0a..f06676299e 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/laneful.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/blendful.py @@ -8,10 +8,10 @@ from openpilot.selfdrive.ui.mici.widgets.button import BigParamControl from openpilot.system.ui.widgets.scroller import NavScroller -class LanefulLayoutMici(NavScroller): +class BlendfulLayoutMici(NavScroller): def __init__(self): super().__init__() - self._toggle = BigParamControl("laneful", "Laneful", + self._toggle = BigParamControl("blendful", "Blendful", description="Keeps the car centered in its lane.\n" + "Leans on the lane lines when they are clear, drives laneless when they are not.") self._scroller.add_widget(self._toggle) diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/settings.py b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/settings.py index 7471cff7d1..46559931c2 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/settings.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/settings.py @@ -11,7 +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.sunnypilot.mici.layouts.blendful import BlendfulLayoutMici 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 @@ -51,9 +51,9 @@ 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)) + blendful_panel = BlendfulLayoutMici() + blendful_btn = SettingsBigButton(tr("blendful"), "", gui_app.texture("icons_mici/settings/device/lkas.png", ICON_SIZE, ICON_SIZE)) + blendful_btn.set_click_callback(lambda: gui_app.push_widget(blendful_panel)) # onroad: enable button sits at the front (left of toggles) self._enable_offroad_btn_onroad = BigCircleButton(self.icon_offroad_enable, red=True) @@ -72,7 +72,7 @@ class SettingsLayoutSP(OP.SettingsLayout): items = self._scroller._items.copy() items.insert(1, models_btn) - items.insert(2, laneful_btn) + items.insert(2, blendful_btn) items.insert(6, sunnylink_btn) # front slots (only one ever visible at a time): exit-always-offroad, then enable-onroad diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py similarity index 85% rename from openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py rename to openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py index 19275a5724..fc639e3054 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py @@ -4,15 +4,15 @@ 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, from the curvature modeld publishes in modelDataV2SP.lanefulCurvature. +Blendful visuals: cyan lane-center target, thin white "after" path (model path bent by blendful's nudge), +and a top-center readout of the pull, from the curvature modeld publishes in modelDataV2SP.blendfulCurvature. """ import numpy as np import pyray as rl from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.sunnypilot.modeld_v2.laneful import LOOKAHEAD_TIME, MIN_LOOKAHEAD, MAX_ERROR +from openpilot.sunnypilot.modeld_v2.blendful import LOOKAHEAD_TIME, MIN_LOOKAHEAD, MAX_ERROR 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 @@ -21,7 +21,7 @@ CENTER_COLOR = (0, 200, 255) AFTER_COLOR = (255, 255, 255) -class LanefulRenderer: +class BlendfulRenderer: def __init__(self): self._center_points = np.empty((0, 2), dtype=np.float32) self._after_points = np.empty((0, 2), dtype=np.float32) @@ -36,8 +36,8 @@ class LanefulRenderer: 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.""" - laneful_curvature = ui_state.sm['modelDataV2SP'].lanefulCurvature - self._alpha.update(float(laneful_curvature != 0.0)) + blendful_curvature = ui_state.sm['modelDataV2SP'].blendfulCurvature + self._alpha.update(float(blendful_curvature != 0.0)) left, right = renderer._lane_lines[1].raw_points, renderer._lane_lines[2].raw_points stds = list(ui_state.sm['modelV2'].laneLineStds) @@ -55,13 +55,13 @@ class LanefulRenderer: 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 by laneful's curvature, ending at the lookahead + # "after" path: model path bent by blendful's curvature, ending at the lookahead path = renderer._path.raw_points lookahead = max(ui_state.sm['carState'].vEgo * LOOKAHEAD_TIME, MIN_LOOKAHEAD) - self._pull_display.update(laneful_curvature * lookahead ** 2 / 2) # how far laneful moves the car at the lookahead + self._pull_display.update(blendful_curvature * lookahead ** 2 / 2) # how far blendful moves the car at the lookahead self._trust_display.update(self._trust) after = path.copy() - after[:, 1] += laneful_curvature * path[:, 0] ** 2 / 2 + after[:, 1] += blendful_curvature * path[:, 0] ** 2 / 2 self._after_points = renderer._map_line_to_polygon(after, 0.15, renderer._path_offset_z, renderer._get_path_length_idx(path[:, 0], lookahead)) @@ -77,13 +77,13 @@ class LanefulRenderer: 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.""" + """Tapping the readout opens the blendful 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() + from openpilot.selfdrive.ui.sunnypilot.mici.layouts.blendful import BlendfulLayoutMici + self._settings_page = BlendfulLayoutMici() gui_app.push_widget(self._settings_page) return True diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py index b3471042f4..3ea80e3259 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py @@ -6,7 +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.mici.onroad.blendful import BlendfulRenderer from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath LANE_LINE_COLORS_SP = { @@ -18,4 +18,4 @@ LANE_LINE_COLORS_SP = { class ModelRendererSP: def __init__(self): self.rainbow_path = RainbowPath() - self.laneful = LanefulRenderer() + self.blendful = BlendfulRenderer() diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/blendful.py similarity index 98% rename from openpilot/sunnypilot/modeld_v2/laneful.py rename to openpilot/sunnypilot/modeld_v2/blendful.py index d30336e168..93718fc595 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/blendful.py @@ -22,7 +22,7 @@ YIELD = (0.2, 0.45) X_IDXS = np.array(ModelConstants.X_IDXS) -class Laneful: # TODO-SP: Rename this to blendful or something similar because it is not pure laneful anymore. +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) diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index ba22ccec71..f8fbecc7ab 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/openpilot/sunnypilot/modeld_v2/modeld.py @@ -40,7 +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.laneful import Laneful +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 @@ -231,7 +231,7 @@ class ModelState(ModelStateBase): 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, - laneful: Callable[[float], float] | None = None) -> log.ModelDataV2.Action: + 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, @@ -243,8 +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 laneful is not None: - desired_curvature += laneful(desired_curvature) + 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) @@ -371,8 +371,8 @@ def main(demo=False): DH = DesireHelper() meta_constants = load_meta_constants() RELC = RoadEdgeLaneChangeController() - laneful = Laneful() - laneful_enabled = False + blendful = Blendful() + blendful_enabled = False while True: # Keep receiving frames until we are at least 1 frame ahead of previous extra frame @@ -416,7 +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)) - laneful_enabled = params.get_bool("Laneful") + 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) @@ -490,9 +490,9 @@ def main(demo=False): if not sm['carControl'].latActive: prev_action.desiredCurvature = sm['controlsState'].curvature - laneful_active = laneful_enabled and desire == log.Desire.none and DH.lane_change_state == log.LaneChangeState.off + blendful_active = blendful_enabled and 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(laneful.update, model_output, v_ego, laneful_active)) + partial(blendful.update, model_output, v_ego, blendful_active)) 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, @@ -509,10 +509,10 @@ 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.lanefulCurvature = laneful.curvature - mdv2sp_send.modelDataV2SP.lanefulTrust = float(laneful.trust) - mdv2sp_send.modelDataV2SP.modelCurvature = float(laneful.model_curvature) - mdv2sp_send.modelDataV2SP.lanefulGoingAround = bool(laneful.going_around) + 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) drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index 4799456354..6bd156d250 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -2021,12 +2021,12 @@ { "id": "model_behavior", "title": "Model Behavior", - "description": "Lane desire, blended laneful, and lead-vehicle awareness tuning", + "description": "Lane desire, blendful, and lead-vehicle awareness tuning", "items": [ { - "key": "Laneful", + "key": "Blendful", "widget": "toggle", - "title": "Use Blended Laneful Mode", + "title": "Use Blendful Mode", "description": "Nudges the model's path toward lane center with a smoothly ramped correction that scales with lane-line confidence. It fades out automatically when lines are unclear, mid-lane-change, during turns, or when the model is going around something, so you keep laneless behavior plus extra centering." }, { diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml index 3820467c7c..123a71d7e5 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml @@ -9,11 +9,11 @@ description: Driving model behavior and camera calibration sections: - id: model_behavior title: Model Behavior - description: Lane desire, blended laneful, and lead-vehicle awareness tuning + description: Lane desire, blendful, and lead-vehicle awareness tuning items: - - key: Laneful + - key: Blendful widget: toggle - title: Use Blended Laneful Mode + title: Use Blendful Mode description: Nudges the model's path toward lane center with a smoothly ramped correction that scales with lane-line confidence. It fades out automatically when lines are unclear, mid-lane-change, during turns, or when the model is going around something, so you keep laneless behavior plus extra centering. From afd0ce66c9193b0a8456d7dacae70be0ad423668 Mon Sep 17 00:00:00 2001 From: royjr Date: Thu, 22 Oct 2026 17:41:10 -0400 Subject: [PATCH 27/54] no tap --- .../ui/mici/onroad/augmented_road_view.py | 2 -- .../ui/sunnypilot/mici/onroad/blendful.py | 17 +---------------- 2 files changed, 1 insertion(+), 18 deletions(-) diff --git a/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py b/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py index 7d102306a1..ffd7d94b2e 100644 --- a/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py +++ b/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py @@ -206,8 +206,6 @@ 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.blendful.handle_tap(mouse_pos): - return super()._handle_mouse_release(mouse_pos) def _render(self, _): diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py index fc639e3054..b4cdf0ca01 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py @@ -31,8 +31,6 @@ class BlendfulRenderer: 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) # fades that hoe - 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.""" @@ -71,22 +69,10 @@ class BlendfulRenderer: def draw(self, rect: rl.Rectangle) -> None: if self._center_points.size and self._trust > 0.05: draw_polygon(rect, self._center_points, self._color(CENTER_COLOR, np.clip(self._trust, 0.2, 1.0) * 255)) - self._readout_rect = None if self._after_points.size: draw_polygon(rect, self._after_points, self._color(AFTER_COLOR, 200)) self._draw_readout(rect) - def handle_tap(self, pos) -> bool: - """Tapping the readout opens the blendful 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.blendful import BlendfulLayoutMici - self._settings_page = BlendfulLayoutMici() - 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 at the lookahead, arrow = side, bar grows from center toward the pull (full at the cap), brighter with more lane line trust.""" @@ -94,8 +80,7 @@ class BlendfulRenderer: 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, self._color((0, 0, 0), 170)) + 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 From cb71cdc54dbacf83b97955aa3e24f258ef46946e Mon Sep 17 00:00:00 2001 From: royjr Date: Thu, 22 Oct 2026 17:46:26 -0400 Subject: [PATCH 28/54] use constants --- openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py index b4cdf0ca01..b3c134cbc6 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py @@ -12,7 +12,7 @@ import pyray as rl from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.sunnypilot.modeld_v2.blendful import LOOKAHEAD_TIME, MIN_LOOKAHEAD, MAX_ERROR +from openpilot.sunnypilot.modeld_v2.blendful import LANE_WIDTH_CLIP, LANE_WIDTH_DEFAULT, LINE_STD, LOOKAHEAD_TIME, MIN_LOOKAHEAD, MAX_ERROR 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 @@ -45,9 +45,9 @@ class BlendfulRenderer: 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 + l_prob = float(probs[1] * np.interp(stds[1], LINE_STD, [1.0, 0.0])) + r_prob = float(probs[2] * np.interp(stds[2], LINE_STD, [1.0, 0.0])) + width = np.clip(right[:, 1] - left[:, 1], *LANE_WIDTH_CLIP) if l_prob * r_prob > 0.5 else LANE_WIDTH_DEFAULT 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 From fd39f4f820efa9e79c55667f553bba6a918e16ec Mon Sep 17 00:00:00 2001 From: royjr Date: Thu, 22 Oct 2026 17:46:44 -0400 Subject: [PATCH 29/54] fade on trust --- openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py index b3c134cbc6..97af853e50 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py @@ -35,7 +35,7 @@ class BlendfulRenderer: 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.""" blendful_curvature = ui_state.sm['modelDataV2SP'].blendfulCurvature - self._alpha.update(float(blendful_curvature != 0.0)) + self._alpha.update(float(ui_state.sm['modelDataV2SP'].blendfulTrust > 0.0)) left, right = renderer._lane_lines[1].raw_points, renderer._lane_lines[2].raw_points stds = list(ui_state.sm['modelV2'].laneLineStds) From 21f45673fda8240b5b8941ccb29458dce65cc1ce Mon Sep 17 00:00:00 2001 From: royjr Date: Thu, 22 Oct 2026 17:47:36 -0400 Subject: [PATCH 30/54] use blendful trust --- openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py index 97af853e50..7e4f4dce63 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py @@ -34,8 +34,9 @@ class BlendfulRenderer: 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.""" - blendful_curvature = ui_state.sm['modelDataV2SP'].blendfulCurvature - self._alpha.update(float(ui_state.sm['modelDataV2SP'].blendfulTrust > 0.0)) + blendful = ui_state.sm['modelDataV2SP'] + blendful_curvature = blendful.blendfulCurvature + self._alpha.update(float(blendful.blendfulTrust > 0.0)) left, right = renderer._lane_lines[1].raw_points, renderer._lane_lines[2].raw_points stds = list(ui_state.sm['modelV2'].laneLineStds) @@ -50,7 +51,7 @@ class BlendfulRenderer: width = np.clip(right[:, 1] - left[:, 1], *LANE_WIDTH_CLIP) if l_prob * r_prob > 0.5 else LANE_WIDTH_DEFAULT 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._trust = blendful.blendfulTrust self._center_points = renderer._map_line_to_polygon(center, 0.1, 0.0, max_idx) # "after" path: model path bent by blendful's curvature, ending at the lookahead From a3ef498512175d497885d68d7373f299f1e24183 Mon Sep 17 00:00:00 2001 From: royjr Date: Thu, 22 Oct 2026 17:53:47 -0400 Subject: [PATCH 31/54] dont guess --- openpilot/cereal/custom.capnp | 3 +++ .../selfdrive/ui/sunnypilot/mici/onroad/blendful.py | 12 ++++++------ openpilot/sunnypilot/modeld_v2/blendful.py | 4 ++++ openpilot/sunnypilot/modeld_v2/modeld.py | 3 +++ 4 files changed, 16 insertions(+), 6 deletions(-) diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index ca4cee003f..430aa140d9 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -472,6 +472,9 @@ struct ModelDataV2SP @0xa1680744031fdb2d { blendfulTrust @4 :Float32; modelCurvature @5 :Float32; blendfulGoingAround @6 :Bool; + blendfulLeftProb @7 :Float32; + blendfulRightProb @8 :Float32; + blendfulLaneWidth @9 :Float32; enum TurnDirection { none @0; diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py index 7e4f4dce63..32d683bae2 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py @@ -12,7 +12,7 @@ import pyray as rl from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.sunnypilot.modeld_v2.blendful import LANE_WIDTH_CLIP, LANE_WIDTH_DEFAULT, LINE_STD, LOOKAHEAD_TIME, MIN_LOOKAHEAD, MAX_ERROR +from openpilot.sunnypilot.modeld_v2.blendful import LANE_WIDTH_CLIP, LOOKAHEAD_TIME, MIN_LOOKAHEAD, MAX_ERROR 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 @@ -45,12 +45,12 @@ class BlendfulRenderer: 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], LINE_STD, [1.0, 0.0])) - r_prob = float(probs[2] * np.interp(stds[2], LINE_STD, [1.0, 0.0])) - width = np.clip(right[:, 1] - left[:, 1], *LANE_WIDTH_CLIP) if l_prob * r_prob > 0.5 else LANE_WIDTH_DEFAULT + l_prob, r_prob = blendful.blendfulLeftProb, blendful.blendfulRightProb + 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] = (l_prob * (left[:, 1] + width / 2) + r_prob * (right[:, 1] - width / 2)) / (l_prob + r_prob + 1e-4) + center[:, 1] = (r_var * (left[:, 1] + half_width) + l_var * (right[:, 1] - half_width)) / (l_var + r_var) self._trust = blendful.blendfulTrust self._center_points = renderer._map_line_to_polygon(center, 0.1, 0.0, max_idx) diff --git a/openpilot/sunnypilot/modeld_v2/blendful.py b/openpilot/sunnypilot/modeld_v2/blendful.py index 93718fc595..f4388de62e 100644 --- a/openpilot/sunnypilot/modeld_v2/blendful.py +++ b/openpilot/sunnypilot/modeld_v2/blendful.py @@ -32,11 +32,14 @@ class Blendful: self.going_around = False self.since_turning = 0.0 self.trust = 0.0 + self.l_prob = 0.0 + self.r_prob = 0.0 self.model_curvature = 0.0 self.smooth = 0.0 # smoothens lanelines below .53 lat accel self.curvature = 0.0 def update(self, model_output: dict[str, np.ndarray], v_ego: float, active: bool, model_curvature: float) -> float: + self.l_prob = self.r_prob = 0.0 fading = not active or v_ego < MIN_SPEED if fading: self.trust = 0.0 @@ -64,6 +67,7 @@ class Blendful: dropping = bool(np.any(probs < 0.6 * self.line_probs_slow.update(model_output['lane_lines_prob'][0, 1::2][1:3]))) 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 half say conf. a line that is disappearing gives control back to laneless if gone[1] > 0.8 and l_prob > 0.5: lane_trust = l_prob / 2 diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index f8fbecc7ab..5ad1732437 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/openpilot/sunnypilot/modeld_v2/modeld.py @@ -513,6 +513,9 @@ def main(demo=False): 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 From d45d7e2de7e8768751fc1ca144f13c581e044c0f Mon Sep 17 00:00:00 2001 From: royjr Date: Thu, 22 Oct 2026 17:54:31 -0400 Subject: [PATCH 32/54] white is model path --- .../ui/sunnypilot/mici/onroad/blendful.py | 20 +++++++++---------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py index 32d683bae2..57b3dffe8c 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py @@ -4,7 +4,7 @@ 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. -Blendful visuals: cyan lane-center target, thin white "after" path (model path bent by blendful's nudge), +Blendful visuals: cyan lane-center target, thin white line down the middle of the model path, and a top-center readout of the pull, from the curvature modeld publishes in modelDataV2SP.blendfulCurvature. """ import numpy as np @@ -18,13 +18,13 @@ 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) +PATH_COLOR = (255, 255, 255) class BlendfulRenderer: def __init__(self): self._center_points = np.empty((0, 2), dtype=np.float32) - self._after_points = np.empty((0, 2), dtype=np.float32) + self._path_points = np.empty((0, 2), dtype=np.float32) self._trust = 0.0 self._font = gui_app.font(FontWeight.BOLD) # smoothed copies for the readout so the numbers don't flicker @@ -42,7 +42,7 @@ class BlendfulRenderer: 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) - self._after_points = np.empty((0, 2), dtype=np.float32) + self._path_points = np.empty((0, 2), dtype=np.float32) return l_prob, r_prob = blendful.blendfulLeftProb, blendful.blendfulRightProb @@ -54,15 +54,13 @@ class BlendfulRenderer: self._trust = blendful.blendfulTrust self._center_points = renderer._map_line_to_polygon(center, 0.1, 0.0, max_idx) - # "after" path: model path bent by blendful's curvature, ending at the lookahead + # middle of the model path, ending at the lookahead path = renderer._path.raw_points lookahead = max(ui_state.sm['carState'].vEgo * LOOKAHEAD_TIME, MIN_LOOKAHEAD) self._pull_display.update(blendful_curvature * lookahead ** 2 / 2) # how far blendful moves the car at the lookahead self._trust_display.update(self._trust) - after = path.copy() - after[:, 1] += blendful_curvature * path[:, 0] ** 2 / 2 - self._after_points = renderer._map_line_to_polygon(after, 0.15, renderer._path_offset_z, - renderer._get_path_length_idx(path[:, 0], lookahead)) + self._path_points = renderer._map_line_to_polygon(path, 0.15, renderer._path_offset_z, + renderer._get_path_length_idx(path[:, 0], lookahead)) def _color(self, rgb, alpha: float) -> rl.Color: return rl.Color(*rgb, int(alpha * self._alpha.x)) @@ -70,8 +68,8 @@ class BlendfulRenderer: def draw(self, rect: rl.Rectangle) -> None: if self._center_points.size and self._trust > 0.05: draw_polygon(rect, self._center_points, self._color(CENTER_COLOR, np.clip(self._trust, 0.2, 1.0) * 255)) - if self._after_points.size: - draw_polygon(rect, self._after_points, self._color(AFTER_COLOR, 200)) + if self._path_points.size: + draw_polygon(rect, self._path_points, self._color(PATH_COLOR, 200)) self._draw_readout(rect) def _draw_readout(self, rect: rl.Rectangle) -> None: From 6e0ce4a6f90ee4efaa2c8282f8aad74b224d5b60 Mon Sep 17 00:00:00 2001 From: royjr Date: Thu, 22 Oct 2026 18:03:21 -0400 Subject: [PATCH 33/54] match width --- openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py index 57b3dffe8c..8aaad6d4e7 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py @@ -52,7 +52,7 @@ class BlendfulRenderer: center = left.copy() center[:, 1] = (r_var * (left[:, 1] + half_width) + l_var * (right[:, 1] - half_width)) / (l_var + r_var) self._trust = blendful.blendfulTrust - self._center_points = renderer._map_line_to_polygon(center, 0.1, 0.0, max_idx) + self._center_points = renderer._map_line_to_polygon(center, 0.15, 0.0, max_idx) # middle of the model path, ending at the lookahead path = renderer._path.raw_points From 15bf3dcd94d1c83d3b3a23d622c99bfb1ae9191b Mon Sep 17 00:00:00 2001 From: royjr Date: Thu, 22 Oct 2026 18:04:10 -0400 Subject: [PATCH 34/54] fade out blue --- .../ui/sunnypilot/mici/onroad/blendful.py | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py index 8aaad6d4e7..22c8cf73dc 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py @@ -45,14 +45,15 @@ class BlendfulRenderer: self._path_points = np.empty((0, 2), dtype=np.float32) return - l_prob, r_prob = blendful.blendfulLeftProb, blendful.blendfulRightProb - 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._trust = blendful.blendfulTrust - self._center_points = renderer._map_line_to_polygon(center, 0.15, 0.0, max_idx) + if blendful.blendfulTrust > 0.0: + l_prob, r_prob = blendful.blendfulLeftProb, blendful.blendfulRightProb + 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._trust = blendful.blendfulTrust + self._center_points = renderer._map_line_to_polygon(center, 0.15, 0.0, max_idx) # middle of the model path, ending at the lookahead path = renderer._path.raw_points From c4f1dc179f2b2e98b6f3d92c3ece61f1f7c51770 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Thu, 22 Oct 2026 20:17:03 -0700 Subject: [PATCH 35/54] dead --- openpilot/sunnypilot/modeld_v2/blendful.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/openpilot/sunnypilot/modeld_v2/blendful.py b/openpilot/sunnypilot/modeld_v2/blendful.py index f4388de62e..90783d3e3d 100644 --- a/openpilot/sunnypilot/modeld_v2/blendful.py +++ b/openpilot/sunnypilot/modeld_v2/blendful.py @@ -55,9 +55,6 @@ class Blendful: return self.curvature def _target(self, model_output: dict[str, np.ndarray], v_ego: float, model_curvature: float) -> float: - if 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] stds = model_output['lane_lines_stds'][0, :, :, 0] From 2ffb986f3b8690c907eecf73030c1f720476de52 Mon Sep 17 00:00:00 2001 From: royjr Date: Thu, 22 Oct 2026 22:42:07 -0400 Subject: [PATCH 36/54] blue only --- .../ui/sunnypilot/mici/onroad/blendful.py | 21 ++++++------------- 1 file changed, 6 insertions(+), 15 deletions(-) diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py index 22c8cf73dc..8eab6da489 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py @@ -4,8 +4,7 @@ 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. -Blendful visuals: cyan lane-center target, thin white line down the middle of the model path, -and a top-center readout of the pull, from the curvature modeld publishes in modelDataV2SP.blendfulCurvature. +Blendful visuals: cyan lane-center target and a top-center readout of the pull, from the curvature modeld publishes in modelDataV2SP.blendfulCurvature. """ import numpy as np import pyray as rl @@ -17,14 +16,12 @@ 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) -PATH_COLOR = (255, 255, 255) +BLENDFUL_COLOR = (0, 200, 255) class BlendfulRenderer: def __init__(self): self._center_points = np.empty((0, 2), dtype=np.float32) - self._path_points = np.empty((0, 2), dtype=np.float32) self._trust = 0.0 self._font = gui_app.font(FontWeight.BOLD) # smoothed copies for the readout so the numbers don't flicker @@ -42,7 +39,6 @@ class BlendfulRenderer: 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) - self._path_points = np.empty((0, 2), dtype=np.float32) return if blendful.blendfulTrust > 0.0: @@ -55,22 +51,17 @@ class BlendfulRenderer: self._trust = blendful.blendfulTrust self._center_points = renderer._map_line_to_polygon(center, 0.15, 0.0, max_idx) - # middle of the model path, ending at the lookahead - path = renderer._path.raw_points lookahead = max(ui_state.sm['carState'].vEgo * LOOKAHEAD_TIME, MIN_LOOKAHEAD) self._pull_display.update(blendful_curvature * lookahead ** 2 / 2) # how far blendful moves the car at the lookahead self._trust_display.update(self._trust) - self._path_points = renderer._map_line_to_polygon(path, 0.15, renderer._path_offset_z, - renderer._get_path_length_idx(path[:, 0], lookahead)) def _color(self, rgb, alpha: float) -> rl.Color: return rl.Color(*rgb, int(alpha * self._alpha.x)) def draw(self, rect: rl.Rectangle) -> None: if self._center_points.size and self._trust > 0.05: - draw_polygon(rect, self._center_points, self._color(CENTER_COLOR, np.clip(self._trust, 0.2, 1.0) * 255)) - if self._path_points.size: - draw_polygon(rect, self._path_points, self._color(PATH_COLOR, 200)) + draw_polygon(rect, self._center_points, self._color(BLENDFUL_COLOR, np.clip(self._trust, 0.2, 1.0) * 255)) + if self._center_points.size: self._draw_readout(rect) def _draw_readout(self, rect: rl.Rectangle) -> None: @@ -86,7 +77,7 @@ class BlendfulRenderer: 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(CENTER_COLOR, np.clip(abs(pull) / 0.1, 0.0, 1.0) * 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) @@ -99,4 +90,4 @@ class BlendfulRenderer: 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(CENTER_COLOR, np.clip(0.3 + 0.7 * trust, 0.0, 1.0) * 255)) + self._color(BLENDFUL_COLOR, np.clip(0.3 + 0.7 * trust, 0.0, 1.0) * 255)) From d904487eedca0e98bc288144c3dd0a86d2c5d554 Mon Sep 17 00:00:00 2001 From: royjr Date: Thu, 22 Oct 2026 22:46:56 -0400 Subject: [PATCH 37/54] sqrt bar --- openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py index 8eab6da489..10d8c9c9b4 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py @@ -86,7 +86,7 @@ class BlendfulRenderer: 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.clip(abs(pull) / MAX_ERROR, 0.0, 1.0)) + fill = bar.width / 2 * float(np.sqrt(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, From 5de324ab962ee272f4df05a6226dfb1640c5ff35 Mon Sep 17 00:00:00 2001 From: royjr Date: Thu, 22 Oct 2026 23:04:59 -0400 Subject: [PATCH 38/54] ill take it from here thanks --- openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py | 5 +++-- openpilot/sunnypilot/modeld_v2/blendful.py | 1 - 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py index 10d8c9c9b4..02cb8883de 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py @@ -11,12 +11,13 @@ import pyray as rl from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.sunnypilot.modeld_v2.blendful import LANE_WIDTH_CLIP, LOOKAHEAD_TIME, MIN_LOOKAHEAD, MAX_ERROR +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: @@ -86,7 +87,7 @@ class BlendfulRenderer: 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) / MAX_ERROR, 0.0, 1.0))) + 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, diff --git a/openpilot/sunnypilot/modeld_v2/blendful.py b/openpilot/sunnypilot/modeld_v2/blendful.py index 90783d3e3d..3f7a0a94b1 100644 --- a/openpilot/sunnypilot/modeld_v2/blendful.py +++ b/openpilot/sunnypilot/modeld_v2/blendful.py @@ -17,7 +17,6 @@ MIN_LOOKAHEAD = 10.0 # m MIN_SPEED = 5.0 # m/s LINE_STD = (0.2, 0.6) POINT_STD = 0.1 -MAX_ERROR = 1.0 YIELD = (0.2, 0.45) X_IDXS = np.array(ModelConstants.X_IDXS) From 4dbb49430b53aa954a894d2b7b2f9381b8e5c579 Mon Sep 17 00:00:00 2001 From: royjr Date: Thu, 22 Oct 2026 23:20:10 -0400 Subject: [PATCH 39/54] tizi --- openpilot/selfdrive/ui/onroad/model_renderer.py | 2 ++ .../selfdrive/ui/sunnypilot/layouts/settings/models.py | 9 ++++++++- .../ui/sunnypilot/mici/onroad/model_renderer.py | 2 +- .../ui/sunnypilot/{mici => }/onroad/blendful.py | 9 +++++---- .../selfdrive/ui/sunnypilot/onroad/model_renderer.py | 2 ++ 5 files changed, 18 insertions(+), 6 deletions(-) rename openpilot/selfdrive/ui/sunnypilot/{mici => }/onroad/blendful.py (95%) diff --git a/openpilot/selfdrive/ui/onroad/model_renderer.py b/openpilot/selfdrive/ui/onroad/model_renderer.py index 6db7d348a2..84573b7732 100644 --- a/openpilot/selfdrive/ui/onroad/model_renderer.py +++ b/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: diff --git a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/models.py b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/models.py index 22cc818ff4..2e804f469d 100644 --- a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/models.py +++ b/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"), + tr("Keeps the car centered in its lane. " + + "Leans on the lane lines when they are clear, drives laneless when they are not."), + 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,7 @@ 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) + self.blendful_toggle.action_item.set_state(ui_state.params.get_bool("Blendful")) 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: diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py index 3ea80e3259..b185693584 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py @@ -6,7 +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.blendful import BlendfulRenderer +from openpilot.selfdrive.ui.sunnypilot.onroad.blendful import BlendfulRenderer from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath LANE_LINE_COLORS_SP = { diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py similarity index 95% rename from openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py rename to openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py index 02cb8883de..0de64b6ae5 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/onroad/blendful.py +++ b/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py @@ -21,7 +21,8 @@ FULL_BAR = 1.0 class BlendfulRenderer: - def __init__(self): + def __init__(self, readout: bool = True): + self._readout = readout self._center_points = np.empty((0, 2), dtype=np.float32) self._trust = 0.0 self._font = gui_app.font(FontWeight.BOLD) @@ -30,7 +31,7 @@ class BlendfulRenderer: self._trust_display = FirstOrderFilter(0.0, 0.5, DT_MDL) self._alpha = FirstOrderFilter(0.0, 0.3, DT_MDL) # fades that hoe - def update(self, renderer, max_idx: int) -> None: + def update(self, renderer, max_idx: int, *map_args) -> None: """Called from the model renderer's update with its lane lines and path already in raw points.""" blendful = ui_state.sm['modelDataV2SP'] blendful_curvature = blendful.blendfulCurvature @@ -50,7 +51,7 @@ class BlendfulRenderer: center = left.copy() center[:, 1] = (r_var * (left[:, 1] + half_width) + l_var * (right[:, 1] - half_width)) / (l_var + r_var) self._trust = blendful.blendfulTrust - self._center_points = renderer._map_line_to_polygon(center, 0.15, 0.0, max_idx) + self._center_points = renderer._map_line_to_polygon(center, 0.15, 0.0, max_idx, *map_args) lookahead = max(ui_state.sm['carState'].vEgo * LOOKAHEAD_TIME, MIN_LOOKAHEAD) self._pull_display.update(blendful_curvature * lookahead ** 2 / 2) # how far blendful moves the car at the lookahead @@ -62,7 +63,7 @@ class BlendfulRenderer: def draw(self, rect: rl.Rectangle) -> None: if self._center_points.size and self._trust > 0.05: draw_polygon(rect, self._center_points, self._color(BLENDFUL_COLOR, np.clip(self._trust, 0.2, 1.0) * 255)) - if self._center_points.size: + if self._center_points.size and self._readout: self._draw_readout(rect) def _draw_readout(self, rect: rl.Rectangle) -> None: diff --git a/openpilot/selfdrive/ui/sunnypilot/onroad/model_renderer.py b/openpilot/selfdrive/ui/sunnypilot/onroad/model_renderer.py index 4be82bef4a..db868d777a 100644 --- a/openpilot/selfdrive/ui/sunnypilot/onroad/model_renderer.py +++ b/openpilot/selfdrive/ui/sunnypilot/onroad/model_renderer.py @@ -6,6 +6,7 @@ See the LICENSE.md file in the root directory for more details. """ from openpilot.common.filter_simple import FirstOrderFilter from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus +from openpilot.selfdrive.ui.sunnypilot.onroad.blendful import BlendfulRenderer from openpilot.selfdrive.ui.sunnypilot.onroad.chevron_metrics import ChevronMetrics from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath from openpilot.selfdrive.ui.sunnypilot.ui_state import MADSState @@ -16,6 +17,7 @@ class ModelRendererSP: def __init__(self): self.rainbow_path = RainbowPath() self.chevron_metrics = ChevronMetrics() + self.blendful = BlendfulRenderer(readout=False) self._width_filter = FirstOrderFilter(0.9, 0.1, 1 / gui_app.target_fps) @property From 6ac88a7be0b7a586f0a790a6fade173d1414fba8 Mon Sep 17 00:00:00 2001 From: royjr Date: Thu, 22 Oct 2026 23:21:53 -0400 Subject: [PATCH 40/54] move to models --- .../ui/sunnypilot/mici/layouts/blendful.py | 21 ------------------- .../ui/sunnypilot/mici/layouts/models.py | 13 ++++++++++-- .../ui/sunnypilot/mici/layouts/settings.py | 8 +------ 3 files changed, 12 insertions(+), 30 deletions(-) delete mode 100644 openpilot/selfdrive/ui/sunnypilot/mici/layouts/blendful.py diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/blendful.py b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/blendful.py deleted file mode 100644 index f06676299e..0000000000 --- a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/blendful.py +++ /dev/null @@ -1,21 +0,0 @@ -""" -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 BigParamControl -from openpilot.system.ui.widgets.scroller import NavScroller - - -class BlendfulLayoutMici(NavScroller): - def __init__(self): - super().__init__() - self._toggle = BigParamControl("blendful", "Blendful", - description="Keeps the car centered in its lane.\n" + - "Leans on the lane lines when they are clear, drives laneless when they are not.") - self._scroller.add_widget(self._toggle) - - def show_event(self): - super().show_event() - self._toggle.refresh() diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py index c1f1f09155..98c1fef33c 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py @@ -11,7 +11,7 @@ import pyray as rl from openpilot.cereal import custom from openpilot.selfdrive.ui.mici.widgets.dialog import BigConfirmationDialog, BigDialog from openpilot.sunnypilot.models.helpers import ACTIVE_BUNDLE_KEYS, get_selected_bundle -from openpilot.selfdrive.ui.mici.widgets.button import BigButton +from openpilot.selfdrive.ui.mici.widgets.button import BigButton, BigParamControl from openpilot.selfdrive.ui.ui_state import ui_state, device from openpilot.selfdrive.ui.sunnypilot.model_info import (active_source, big_model_state, bundles_for_source, carrying_model, default_model_name, model_cache_size_mb, model_info, queued_name, @@ -95,7 +95,12 @@ class ModelsLayoutMici(NavScroller): self.clear_cache_btn.set_click_callback(self._confirm_clear_cache) self._cache_size_time = 0.0 - self.main_items = [self.current_model_info, self.select_model_btn, self.cancel_download_btn, self.refresh_btn, self.clear_cache_btn] + self.blendful_toggle = BigParamControl("blendful", "Blendful", + description="Keeps the car centered in its lane.\n" + + "Leans on the lane lines when they are clear, drives laneless when they are not.") + + 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() diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/settings.py b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/settings.py index 46559931c2..d38e7596fa 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/settings.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/settings.py @@ -11,7 +11,6 @@ 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.blendful import BlendfulLayoutMici 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 @@ -51,10 +50,6 @@ 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)) - blendful_panel = BlendfulLayoutMici() - blendful_btn = SettingsBigButton(tr("blendful"), "", gui_app.texture("icons_mici/settings/device/lkas.png", ICON_SIZE, ICON_SIZE)) - blendful_btn.set_click_callback(lambda: gui_app.push_widget(blendful_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)) @@ -72,8 +67,7 @@ class SettingsLayoutSP(OP.SettingsLayout): items = self._scroller._items.copy() items.insert(1, models_btn) - items.insert(2, blendful_btn) - items.insert(6, sunnylink_btn) + items.insert(5, 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) From 9c391b48422c7ae8983646a81d8f1ac9a4b67326 Mon Sep 17 00:00:00 2001 From: royjr Date: Thu, 22 Oct 2026 23:46:50 -0400 Subject: [PATCH 41/54] cleanup --- .../selfdrive/ui/sunnypilot/layouts/settings/models.py | 3 +-- .../selfdrive/ui/sunnypilot/mici/layouts/models.py | 4 ++-- openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py | 10 ++-------- openpilot/sunnypilot/sunnylink/settings_ui.json | 6 +++--- .../sunnylink/settings_ui_src/pages/models.yaml | 8 +++----- 5 files changed, 11 insertions(+), 20 deletions(-) diff --git a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/models.py b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/models.py index 2e804f469d..28a83598b8 100644 --- a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/models.py +++ b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/models.py @@ -103,8 +103,7 @@ class ModelsLayout(Widget): self.lagd_toggle = toggle_item_sp(tr("Live Learning Steer Delay"), "", param="LagdToggle") self.blendful_toggle = toggle_item_sp(tr("Blendful"), - tr("Keeps the car centered in its lane. " + - "Leans on the lane lines when they are clear, drives laneless when they are not."), + tr("Nudges the car toward the lane center when the lane lines are clear. Drives laneless when they are not."), param="Blendful") self.camera_offset = option_item_sp(tr("Adjust Camera Offset"), "CameraOffset", -35, 35, diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py index 98c1fef33c..6440accd0c 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py @@ -96,8 +96,8 @@ class ModelsLayoutMici(NavScroller): self._cache_size_time = 0.0 self.blendful_toggle = BigParamControl("blendful", "Blendful", - description="Keeps the car centered in its lane.\n" + - "Leans on the lane lines when they are clear, drives laneless when they are not.") + description="Nudges the car toward the lane center when the lane lines are clear.\n" + + "Drives laneless when they are not.") 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] diff --git a/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py index 0de64b6ae5..9d06202766 100644 --- a/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py +++ b/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py @@ -3,8 +3,6 @@ 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. - -Blendful visuals: cyan lane-center target and a top-center readout of the pull, from the curvature modeld publishes in modelDataV2SP.blendfulCurvature. """ import numpy as np import pyray as rl @@ -26,13 +24,11 @@ class BlendfulRenderer: self._center_points = np.empty((0, 2), dtype=np.float32) self._trust = 0.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._alpha = FirstOrderFilter(0.0, 0.3, DT_MDL) # fades that hoe + self._alpha = FirstOrderFilter(0.0, 0.3, DT_MDL) def update(self, renderer, max_idx: int, *map_args) -> None: - """Called from the model renderer's update with its lane lines and path already in raw points.""" blendful = ui_state.sm['modelDataV2SP'] blendful_curvature = blendful.blendfulCurvature self._alpha.update(float(blendful.blendfulTrust > 0.0)) @@ -54,7 +50,7 @@ class BlendfulRenderer: self._center_points = renderer._map_line_to_polygon(center, 0.15, 0.0, max_idx, *map_args) lookahead = max(ui_state.sm['carState'].vEgo * LOOKAHEAD_TIME, MIN_LOOKAHEAD) - self._pull_display.update(blendful_curvature * lookahead ** 2 / 2) # how far blendful moves the car at the lookahead + self._pull_display.update(blendful_curvature * lookahead ** 2 / 2) self._trust_display.update(self._trust) def _color(self, rgb, alpha: float) -> rl.Color: @@ -67,8 +63,6 @@ class BlendfulRenderer: self._draw_readout(rect) def _draw_readout(self, rect: rl.Rectangle) -> None: - """Top-center pill: pull toward lane center in cm at the lookahead, 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 diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index 6bd156d250..fe6d631b27 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -2021,13 +2021,13 @@ { "id": "model_behavior", "title": "Model Behavior", - "description": "Lane desire, blendful, and lead-vehicle awareness tuning", + "description": "Lane desire, Blendful lane centering, and lead-vehicle awareness tuning", "items": [ { "key": "Blendful", "widget": "toggle", - "title": "Use Blendful Mode", - "description": "Nudges the model's path toward lane center with a smoothly ramped correction that scales with lane-line confidence. It fades out automatically when lines are unclear, mid-lane-change, during turns, or when the model is going around something, so you keep laneless behavior plus extra centering." + "title": "Blendful", + "description": "Nudges the car toward the lane center when the lane lines are clear. Drives laneless when they are not." }, { "key": "LaneTurnDesire", diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml index 123a71d7e5..dbad2f0692 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml @@ -9,14 +9,12 @@ description: Driving model behavior and camera calibration sections: - id: model_behavior title: Model Behavior - description: Lane desire, blendful, and lead-vehicle awareness tuning + description: Lane desire, Blendful lane centering, and lead-vehicle awareness tuning items: - key: Blendful widget: toggle - title: Use Blendful Mode - description: Nudges the model's path toward lane center with a smoothly ramped correction that scales with - lane-line confidence. It fades out automatically when lines are unclear, mid-lane-change, during turns, - or when the model is going around something, so you keep laneless behavior plus extra centering. + title: Blendful + description: Nudges the car toward the lane center when the lane lines are clear. Drives laneless when they are not. - key: LaneTurnDesire widget: toggle title: Use Lane Turn Desires From 8efe325a716e189c4e047864de5901c6ab99ca5d Mon Sep 17 00:00:00 2001 From: royjr Date: Thu, 22 Oct 2026 23:51:03 -0400 Subject: [PATCH 42/54] gate like rainbow --- openpilot/selfdrive/ui/onroad/model_renderer.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/openpilot/selfdrive/ui/onroad/model_renderer.py b/openpilot/selfdrive/ui/onroad/model_renderer.py index 84573b7732..f9c1dc161e 100644 --- a/openpilot/selfdrive/ui/onroad/model_renderer.py +++ b/openpilot/selfdrive/ui/onroad/model_renderer.py @@ -123,7 +123,8 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP): # Draw elements self._draw_lane_lines() self._draw_path() - self.blendful.draw(self._rect) + if self._lateral_active: + self.blendful.draw(self._rect) if render_lead_indicator and radar_state: self._draw_lead_indicator() From 16b17e051ffc5f9501363323382d6f65469e85a3 Mon Sep 17 00:00:00 2001 From: royjr Date: Fri, 23 Oct 2026 00:03:15 -0400 Subject: [PATCH 43/54] tick on new data --- openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py index 9d06202766..f2e61b0401 100644 --- a/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py +++ b/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py @@ -30,8 +30,9 @@ class BlendfulRenderer: def update(self, renderer, max_idx: int, *map_args) -> None: blendful = ui_state.sm['modelDataV2SP'] - blendful_curvature = blendful.blendfulCurvature - self._alpha.update(float(blendful.blendfulTrust > 0.0)) + new_data = ui_state.sm.updated['modelDataV2SP'] + if new_data: + self._alpha.update(float(blendful.blendfulTrust > 0.0)) left, right = renderer._lane_lines[1].raw_points, renderer._lane_lines[2].raw_points stds = list(ui_state.sm['modelV2'].laneLineStds) @@ -49,9 +50,10 @@ class BlendfulRenderer: self._trust = blendful.blendfulTrust self._center_points = renderer._map_line_to_polygon(center, 0.15, 0.0, max_idx, *map_args) - lookahead = max(ui_state.sm['carState'].vEgo * LOOKAHEAD_TIME, MIN_LOOKAHEAD) - self._pull_display.update(blendful_curvature * lookahead ** 2 / 2) - self._trust_display.update(self._trust) + if new_data: + 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) def _color(self, rgb, alpha: float) -> rl.Color: return rl.Color(*rgb, int(alpha * self._alpha.x)) From f179460982cf357495547ffbbeb458a066ac227f Mon Sep 17 00:00:00 2001 From: royjr Date: Fri, 23 Oct 2026 00:05:04 -0400 Subject: [PATCH 44/54] so I can sleep at night --- openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py index f2e61b0401..1df175a04c 100644 --- a/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py +++ b/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py @@ -50,7 +50,7 @@ class BlendfulRenderer: self._trust = blendful.blendfulTrust self._center_points = renderer._map_line_to_polygon(center, 0.15, 0.0, max_idx, *map_args) - if new_data: + if new_data and 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) From d28e027943049e3794b0a224b52b3ecf29b27c56 Mon Sep 17 00:00:00 2001 From: royjr Date: Fri, 23 Oct 2026 00:12:41 -0400 Subject: [PATCH 45/54] keep pill current --- .../selfdrive/ui/sunnypilot/onroad/blendful.py | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py index 1df175a04c..f50fbea0c8 100644 --- a/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py +++ b/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py @@ -30,9 +30,14 @@ class BlendfulRenderer: def update(self, renderer, max_idx: int, *map_args) -> None: blendful = ui_state.sm['modelDataV2SP'] - new_data = ui_state.sm.updated['modelDataV2SP'] - if new_data: + 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) @@ -47,14 +52,8 @@ class BlendfulRenderer: 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._trust = blendful.blendfulTrust self._center_points = renderer._map_line_to_polygon(center, 0.15, 0.0, max_idx, *map_args) - if new_data and 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) - def _color(self, rgb, alpha: float) -> rl.Color: return rl.Color(*rgb, int(alpha * self._alpha.x)) From 02c7b6060067e08e94b2f85cc7b39af2b997b9ef Mon Sep 17 00:00:00 2001 From: royjr Date: Fri, 23 Oct 2026 00:35:06 -0400 Subject: [PATCH 46/54] fine --- openpilot/sunnypilot/sunnylink/settings_ui.json | 2 +- .../sunnypilot/sunnylink/settings_ui_src/pages/models.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index fe6d631b27..d7854560f3 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -2026,7 +2026,7 @@ { "key": "Blendful", "widget": "toggle", - "title": "Blendful", + "title": "Blendful Mode", "description": "Nudges the car toward the lane center when the lane lines are clear. Drives laneless when they are not." }, { diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml index dbad2f0692..741b348019 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml @@ -13,7 +13,7 @@ sections: items: - key: Blendful widget: toggle - title: Blendful + title: Blendful Mode description: Nudges the car toward the lane center when the lane lines are clear. Drives laneless when they are not. - key: LaneTurnDesire widget: toggle From aa559e48a207a1b2d17c1c8dcbb137003d5f8604 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Thu, 22 Oct 2026 23:51:09 -0700 Subject: [PATCH 47/54] smoother release authority --- openpilot/sunnypilot/modeld_v2/blendful.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/openpilot/sunnypilot/modeld_v2/blendful.py b/openpilot/sunnypilot/modeld_v2/blendful.py index 3f7a0a94b1..e302e1a667 100644 --- a/openpilot/sunnypilot/modeld_v2/blendful.py +++ b/openpilot/sunnypilot/modeld_v2/blendful.py @@ -48,9 +48,10 @@ class Blendful: self.model_curvature = model_curvature 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.25, 0.0])) - releasing = not fading and (abs(target) < abs(self.curvature) or target * self.curvature < 0) - step = (2.0 if releasing else 0.5) / max(v_ego, 1.0) ** 2 * DT_MDL # jerk over 1 second - self.curvature = float(np.clip(target, self.curvature - step, self.curvature + step)) + 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 + c = self.curvature + self.curvature = float(np.clip(target, max(c - release, -build) if c > 0 else c - build, min(c + release, build) if c < 0 else c + build)) return self.curvature def _target(self, model_output: dict[str, np.ndarray], v_ego: float, model_curvature: float) -> float: From cda505ea6dd654a5686df4f4167033d552170f66 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Thu, 22 Oct 2026 23:54:18 -0700 Subject: [PATCH 48/54] satisfy royjr --- openpilot/sunnypilot/modeld_v2/blendful.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/openpilot/sunnypilot/modeld_v2/blendful.py b/openpilot/sunnypilot/modeld_v2/blendful.py index e302e1a667..4430be00be 100644 --- a/openpilot/sunnypilot/modeld_v2/blendful.py +++ b/openpilot/sunnypilot/modeld_v2/blendful.py @@ -14,7 +14,7 @@ LANE_WIDTH_DEFAULT = 3.6 # m (reseeded to model's lane width upon initializatio LANE_WIDTH_CLIP = (2.8, 4.5) # m LOOKAHEAD_TIME = 1.5 # s MIN_LOOKAHEAD = 10.0 # m -MIN_SPEED = 5.0 # m/s +MIN_SPEED = 1.0 # m/s LINE_STD = (0.2, 0.6) POINT_STD = 0.1 YIELD = (0.2, 0.45) From 0ce9fca933fdf59ba14edce67e49726a907a8fd1 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Fri, 23 Oct 2026 11:54:58 -0700 Subject: [PATCH 49/54] decay one line and give full authority in curvy curves; kappa=curv --- openpilot/sunnypilot/modeld_v2/blendful.py | 32 +++++++++++++++------- 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/openpilot/sunnypilot/modeld_v2/blendful.py b/openpilot/sunnypilot/modeld_v2/blendful.py index 4430be00be..725861e8d8 100644 --- a/openpilot/sunnypilot/modeld_v2/blendful.py +++ b/openpilot/sunnypilot/modeld_v2/blendful.py @@ -10,7 +10,7 @@ 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 (reseeded to model's lane width upon initialization) +LANE_WIDTH_DEFAULT = 3.6 # m LANE_WIDTH_CLIP = (2.8, 4.5) # m LOOKAHEAD_TIME = 1.5 # s MIN_LOOKAHEAD = 10.0 # m @@ -31,6 +31,8 @@ class Blendful: 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 @@ -41,7 +43,7 @@ class Blendful: self.l_prob = self.r_prob = 0.0 fading = not active or v_ego < MIN_SPEED if fading: - self.trust = 0.0 + self.trust = self.line_trust = 0.0 self.usual_offset.initialized = False self.lane_width.initialized = False self.going_around = False @@ -50,8 +52,8 @@ class Blendful: 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.25, 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 - c = self.curvature - self.curvature = float(np.clip(target, max(c - release, -build) if c > 0 else c - build, min(c + release, build) if c < 0 else c + build)) + 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: @@ -59,24 +61,33 @@ class Blendful: 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)) - dropping = bool(np.any(probs < 0.6 * self.line_probs_slow.update(model_output['lane_lines_prob'][0, 1::2][1:3]))) + 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 half say conf. a line that is disappearing gives control back to laneless - if gone[1] > 0.8 and l_prob > 0.5: - lane_trust = l_prob / 2 - elif gone[0] > 0.8 and r_prob > 0.5: - lane_trust = r_prob / 2 + # 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] @@ -123,4 +134,5 @@ class Blendful: 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]) 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) From 9d98fe43764dbccfbba58bba3b897d8658e708e6 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Fri, 23 Oct 2026 11:56:25 -0700 Subject: [PATCH 50/54] he does not like 1ms --- openpilot/sunnypilot/modeld_v2/blendful.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/openpilot/sunnypilot/modeld_v2/blendful.py b/openpilot/sunnypilot/modeld_v2/blendful.py index 725861e8d8..a74e78fa33 100644 --- a/openpilot/sunnypilot/modeld_v2/blendful.py +++ b/openpilot/sunnypilot/modeld_v2/blendful.py @@ -14,7 +14,7 @@ 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 = 1.0 # m/s +MIN_SPEED = 5.0 # m/s LINE_STD = (0.2, 0.6) POINT_STD = 0.1 YIELD = (0.2, 0.45) From 10b508929d0d223d5e1dab84db4a1e7c7906480c Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Fri, 23 Oct 2026 23:17:33 -0700 Subject: [PATCH 51/54] smoother --- openpilot/sunnypilot/modeld_v2/blendful.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/openpilot/sunnypilot/modeld_v2/blendful.py b/openpilot/sunnypilot/modeld_v2/blendful.py index a74e78fa33..32209f45c6 100644 --- a/openpilot/sunnypilot/modeld_v2/blendful.py +++ b/openpilot/sunnypilot/modeld_v2/blendful.py @@ -49,7 +49,7 @@ class Blendful: self.going_around = False self.model_curvature = model_curvature 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.25, 0.0])) + 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 @@ -133,6 +133,8 @@ class Blendful: 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) From e063ce146e4d28908c81777784bc42a6fee9a496 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sat, 24 Oct 2026 11:25:51 -0700 Subject: [PATCH 52/54] force tg --- .github/workflows/build-single-tinygrad-model.yaml | 5 +++++ .github/workflows/sunnypilot-build-model.yaml | 11 ++++++++++- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/.github/workflows/build-single-tinygrad-model.yaml b/.github/workflows/build-single-tinygrad-model.yaml index 077077a714..ef505f92df 100644 --- a/.github/workflows/build-single-tinygrad-model.yaml +++ b/.github/workflows/build-single-tinygrad-model.yaml @@ -117,6 +117,10 @@ on: required: false type: string default: 'sunnypilot/sunnypilot-models' + tinygrad_ref: + description: 'Tinygrad commit to convert a precompiled upstream pkl with, when it differs from the upstream pin' + required: false + type: string env: RECOMPILED_DIR: recompiled${{ inputs.recompiled_dir }} JSON_FILE: docs/docs/driving_models_${{ inputs.target_hardware == 'chestnut' && 'chestnut_v' || 'v' }}${{ inputs.json_version }}.json @@ -130,6 +134,7 @@ jobs: is_20hz: ${{ inputs.is_20hz }} artifact_suffix: ${{ inputs.artifact_suffix }} target_hardware: ${{ inputs.target_hardware }} + tinygrad_ref: ${{ inputs.tinygrad_ref }} secrets: inherit publish_model: diff --git a/.github/workflows/sunnypilot-build-model.yaml b/.github/workflows/sunnypilot-build-model.yaml index 8276c40136..28899b13f1 100644 --- a/.github/workflows/sunnypilot-build-model.yaml +++ b/.github/workflows/sunnypilot-build-model.yaml @@ -35,6 +35,10 @@ on: required: false type: string default: 'qcom' + tinygrad_ref: + description: 'Tinygrad commit to convert a precompiled upstream pkl with, when it differs from the upstream pin' + required: false + type: string workflow_dispatch: inputs: upstream_branch: @@ -58,6 +62,10 @@ on: options: - qcom - chestnut + tinygrad_ref: + description: 'Tinygrad commit to convert a precompiled upstream pkl with, when it differs from the upstream pin' + required: false + type: string default: 'qcom' @@ -224,11 +232,12 @@ jobs: if [ -f "${{ env.MODELS_DIR }}/upstream_pkl.txt" ]; then read -r TG_URL TG_SHA _ < "${{ env.MODELS_DIR }}/upstream_pkl.txt" + if [ -n "${{ inputs.tinygrad_ref }}" ]; then TG_SHA="${{ inputs.tinygrad_ref }}"; fi echo "Converting commaai's precompiled model with tinygrad $TG_SHA" UPSTREAM_TG="${{ runner.temp }}/upstream_tinygrad" rm -rf "$UPSTREAM_TG" && git init -q "$UPSTREAM_TG" && git -C "$UPSTREAM_TG" fetch -q --depth 1 "$TG_URL" "$TG_SHA" && git -C "$UPSTREAM_TG" checkout -q FETCH_HEAD CONVERTED="${{ runner.temp }}/big_driving_tinygrad_plain.pkl" - env DEV=USB+AMD PYTHONPATH="$UPSTREAM_TG:${{ github.workspace }}" taskset -c 7 python3 openpilot/sunnypilot/modeld_v2/convert_upstream_pkl.py \ + env DEV=USB+AMD PYTHONPATH="$UPSTREAM_TG:${{ github.workspace }}" taskset -c 7 python3 -u -X faulthandler openpilot/sunnypilot/modeld_v2/convert_upstream_pkl.py \ "${{ env.MODELS_DIR }}/big_driving_tinygrad.pkl" "$CONVERTED" mv "$CONVERTED" "$OUTPUT_PKL" elif NEW_ARCH_ONNX=$(find "${{ env.MODELS_DIR }}" -maxdepth 1 -name "${{ inputs.target_hardware == 'chestnut' && 'big_' || '' }}*driving*.onnx" -print -quit) && From c2f980f769a4b78369434a7cded9726e11d27963 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sat, 24 Oct 2026 11:31:06 -0700 Subject: [PATCH 53/54] oop --- .github/workflows/build-single-tinygrad-model.yaml | 2 +- .github/workflows/sunnypilot-build-model.yaml | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build-single-tinygrad-model.yaml b/.github/workflows/build-single-tinygrad-model.yaml index ef505f92df..5b10d8dd9f 100644 --- a/.github/workflows/build-single-tinygrad-model.yaml +++ b/.github/workflows/build-single-tinygrad-model.yaml @@ -118,7 +118,7 @@ on: type: string default: 'sunnypilot/sunnypilot-models' tinygrad_ref: - description: 'Tinygrad commit to convert a precompiled upstream pkl with, when it differs from the upstream pin' + description: 'Full tinygrad commit sha to convert a precompiled upstream pkl with, when it differs from the upstream pin' required: false type: string env: diff --git a/.github/workflows/sunnypilot-build-model.yaml b/.github/workflows/sunnypilot-build-model.yaml index 28899b13f1..c1ae211e44 100644 --- a/.github/workflows/sunnypilot-build-model.yaml +++ b/.github/workflows/sunnypilot-build-model.yaml @@ -36,7 +36,7 @@ on: type: string default: 'qcom' tinygrad_ref: - description: 'Tinygrad commit to convert a precompiled upstream pkl with, when it differs from the upstream pin' + description: 'Full tinygrad commit sha to convert a precompiled upstream pkl with, when it differs from the upstream pin' required: false type: string workflow_dispatch: @@ -63,7 +63,7 @@ on: - qcom - chestnut tinygrad_ref: - description: 'Tinygrad commit to convert a precompiled upstream pkl with, when it differs from the upstream pin' + description: 'Full tinygrad commit sha to convert a precompiled upstream pkl with, when it differs from the upstream pin' required: false type: string default: 'qcom' @@ -235,7 +235,8 @@ jobs: if [ -n "${{ inputs.tinygrad_ref }}" ]; then TG_SHA="${{ inputs.tinygrad_ref }}"; fi echo "Converting commaai's precompiled model with tinygrad $TG_SHA" UPSTREAM_TG="${{ runner.temp }}/upstream_tinygrad" - rm -rf "$UPSTREAM_TG" && git init -q "$UPSTREAM_TG" && git -C "$UPSTREAM_TG" fetch -q --depth 1 "$TG_URL" "$TG_SHA" && git -C "$UPSTREAM_TG" checkout -q FETCH_HEAD + rm -rf "$UPSTREAM_TG" && git init -q "$UPSTREAM_TG" && git -C "$UPSTREAM_TG" fetch -q --depth 1 "$TG_URL" "$TG_SHA" && git -C "$UPSTREAM_TG" checkout -q FETCH_HEAD \ + || { echo "::error::could not fetch tinygrad $TG_SHA from $TG_URL (a full 40-character commit sha is required)"; exit 1; } CONVERTED="${{ runner.temp }}/big_driving_tinygrad_plain.pkl" env DEV=USB+AMD PYTHONPATH="$UPSTREAM_TG:${{ github.workspace }}" taskset -c 7 python3 -u -X faulthandler openpilot/sunnypilot/modeld_v2/convert_upstream_pkl.py \ "${{ env.MODELS_DIR }}/big_driving_tinygrad.pkl" "$CONVERTED" From 99700587d8b153b9520312f8665fd474ae7d5d89 Mon Sep 17 00:00:00 2001 From: royjr Date: Sun, 25 Oct 2026 08:52:02 -0400 Subject: [PATCH 54/54] hold blue --- openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py b/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py index f50fbea0c8..bd4b8511fa 100644 --- a/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py +++ b/openpilot/selfdrive/ui/sunnypilot/onroad/blendful.py @@ -45,8 +45,8 @@ class BlendfulRenderer: self._center_points = np.empty((0, 2), dtype=np.float32) return - if blendful.blendfulTrust > 0.0: - l_prob, r_prob = blendful.blendfulLeftProb, blendful.blendfulRightProb + 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)