longcontrol

This reverts commit fb95b72e23.

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;

openpilot/common/params_keys.h

@@ -170,6 +170,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}},
     {"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
     {"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
+    {"FollowDistanceAccelBoost", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}},
     {"HasAcceptedTermsSP", {PERSISTENT, STRING, "0"}},
@@ -257,6 +258,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"LagdValueCache", {PERSISTENT, FLOAT, "0.2"}},
     {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}},
+    {"Laneful", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
 
     // mapd

openpilot/selfdrive/controls/lib/longcontrol.py

@@ -9,6 +9,12 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
+STOPPING_JERK_MIN = 0.3  # m/s^3
+STOPPING_JERK_MAX = 2.0  # m/s^3
+STOPPING_HOLD_SPEED = 0.05  # m/s, below this ramp into the standstill hold
+STOPPING_HOLD_JERK = 1.0  # m/s^3, command ramp rate for the standstill hold
+STOPPING_EXIT_FRAMES = 15  # 0.15 s at 100 Hz, debounce should_stop at standstill
+
 
 def long_control_state_trans(CP_SP, active, long_control_state,
                              should_stop, brake_pressed, cruise_standstill):
@@ -47,28 +53,57 @@ class LongControl:
     self.pid = PIDController(0.0, (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
                              rate=1 / DT_CTRL)
     self.last_output_accel = 0.0
+    self.stopping_jerk = STOPPING_JERK_MIN
+    self.stopping_exit_frames = 0
 
   def reset(self):
     self.pid.reset()
 
+  def _stopping_accel(self, v_ego, standstill):
+    v_ego = max(float(v_ego), 0.0)
+    if standstill or v_ego < STOPPING_HOLD_SPEED:
+      goal = float(self.CP.stopAccel)
+      jerk = STOPPING_HOLD_JERK
+    else:
+      taper = -np.sqrt(2.0 * self.stopping_jerk * v_ego)
+      goal = max(self.last_output_accel, taper) if self.last_output_accel < 0.0 else taper
+      jerk = self.stopping_jerk
+
+    output_accel = np.clip(min(goal, 0.0), self.last_output_accel - jerk * DT_CTRL,
+                           self.last_output_accel + jerk * DT_CTRL)
+    return float(output_accel)
+
   def update(self, active, CS, a_target, should_stop, accel_limits):
     """Update longitudinal control. This updates the state machine and runs a PID loop"""
     self.pid.neg_limit = accel_limits[0]
     self.pid.pos_limit = accel_limits[1]
 
+    if self.long_control_state == LongCtrlState.stopping and not should_stop:
+      self.stopping_exit_frames += 1
+    else:
+      self.stopping_exit_frames = 0
+    should_stop_trans = should_stop or (self.long_control_state == LongCtrlState.stopping and
+                                        self.stopping_exit_frames < STOPPING_EXIT_FRAMES)
+
+    prev_state = self.long_control_state
     self.long_control_state = long_control_state_trans(self.CP_SP, active, self.long_control_state,
-                                                       should_stop, CS.brakePressed,
+                                                       should_stop_trans, CS.brakePressed,
                                                        CS.cruiseState.standstill)
+
+    if self.long_control_state == LongCtrlState.stopping and prev_state != LongCtrlState.stopping:
+      entry_v = max(float(CS.vEgo), STOPPING_HOLD_SPEED)
+      entry_a = min(float(self.last_output_accel), 0.0)
+      if entry_a == 0.0:
+        entry_a = min(float(a_target), 0.0)
+      self.stopping_jerk = float(np.clip(entry_a ** 2 / (2.0 * entry_v),
+                                         STOPPING_JERK_MIN, STOPPING_JERK_MAX))
+
     if self.long_control_state == LongCtrlState.off:
       self.reset()
       output_accel = 0.
 
     elif self.long_control_state == LongCtrlState.stopping:
-      output_accel = self.last_output_accel
-      if output_accel > self.CP.stopAccel:
-        output_accel = min(output_accel, 0.0)
-        # TODO: can we just go straight to stopAccel?
-        output_accel -= 1.0 * DT_CTRL  # m/s^2/s while trying to stop
+      output_accel = self._stopping_accel(CS.vEgo, CS.standstill)
       self.reset()
 
     else:  # LongCtrlState.pid

openpilot/selfdrive/controls/tests/test_accel_boost.py

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

openpilot/selfdrive/controls/tests/test_longcontrol.py

@@ -1,6 +1,8 @@
+from opendbc.car.structs import car
+from openpilot.common.realtime import DT_CTRL
 from openpilot.common.test import OpenpilotTestCase
 from openpilot.cereal import custom
-from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans
+from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState, long_control_state_trans
 
 
 class TestLongControlStateTransition(OpenpilotTestCase):
@@ -42,3 +44,112 @@ class TestLongControlStateTransition(OpenpilotTestCase):
     next_state = long_control_state_trans(CP_SP, active, current_state,
                              should_stop=False, brake_pressed=False, cruise_standstill=False)
     assert next_state == LongCtrlState.pid
+
+
+class TestLongControlStopping(OpenpilotTestCase):
+  ACCEL_LIMITS = (-2.0, 2.0)
+
+  def make_control(self):
+    CP = car.CarParams.new_message()
+    CP.stopAccel = -2.0
+    CP.longitudinalTuning.kiBP = [0.0]
+    CP.longitudinalTuning.kiV = [0.0]
+    CP_SP = custom.CarParamsSP.new_message()
+    return LongControl(CP, CP_SP)
+
+  def make_cs(self, v_ego, a_ego, standstill=False, cruise_standstill=None):
+    CS = car.CarState.new_message()
+    CS.vEgo = float(v_ego)
+    CS.aEgo = float(a_ego)
+    CS.standstill = standstill
+    CS.brakePressed = False
+    CS.cruiseState.standstill = standstill if cruise_standstill is None else cruise_standstill
+    return CS
+
+  def enter_stopping(self, loc, v_ego=0.23, a_ego=-0.55):
+    # Run in pid first so last_output_accel is a realistic decel, then enter stopping.
+    out = 0.0
+    for _ in range(100):
+      out = loc.update(True, self.make_cs(2.0, a_ego), a_ego, False, self.ACCEL_LIMITS)
+    assert loc.long_control_state == LongCtrlState.pid
+    out = loc.update(True, self.make_cs(v_ego, a_ego), a_ego, True, self.ACCEL_LIMITS)
+    assert loc.long_control_state == LongCtrlState.stopping
+    return out
+
+  def test_entry_is_continuous(self):
+    loc = self.make_control()
+    out = self.enter_stopping(loc)
+    assert abs(out - loc.last_output_accel) < 1e-9
+    assert abs(out - (-0.55)) < 0.02
+
+  def test_positive_entry_ramps_down(self):
+    loc = self.make_control()
+    loc.last_output_accel = 0.3
+    loc.stopping_jerk = 0.66
+    out = loc._stopping_accel(0.23, False)
+    assert abs(out - (0.3 - 0.66 * DT_CTRL)) < 1e-6  # no jump to zero
+
+  def test_taper_never_steps(self):
+    loc = self.make_control()
+    v_ego = 0.23
+    out = self.enter_stopping(loc)
+    prev = out
+    max_step = 0.0
+    for _ in range(400):
+      out = loc.update(True, self.make_cs(v_ego, out), -0.55, True, self.ACCEL_LIMITS)
+      max_step = max(max_step, abs(out - prev))
+      prev = out
+      v_ego = max(0.0, v_ego + out * DT_CTRL)
+
+    # bounded by the release jerk (and the 1.0 m/s^3 hold ramp), never a step to stopAccel
+    assert max_step <= 1.0 * DT_CTRL + 1e-6
+    assert -2.01 < out < -1.9
+
+  def test_taper_releases_before_standstill(self):
+    loc = self.make_control()
+    v_ego = 0.23
+    out = self.enter_stopping(loc)
+    while v_ego > 0.11:
+      out = loc.update(True, self.make_cs(v_ego, out), -0.55, True, self.ACCEL_LIMITS)
+      v_ego = max(0.0, v_ego + out * DT_CTRL)
+    assert out > -0.5  # noticeably released before the hold takes over
+
+  def test_hot_entry_still_bounded(self):
+    loc = self.make_control()
+    out = self.enter_stopping(loc, v_ego=0.25, a_ego=-1.2)
+    prev = out
+    assert abs(out - (-1.2)) < 0.021
+    for _ in range(30):
+      out = loc.update(True, self.make_cs(0.25, out), -1.2, True, self.ACCEL_LIMITS)
+      assert abs(out - prev) <= 2.0 * DT_CTRL + 1e-6
+      prev = out
+
+  def test_hold_ramps_instead_of_stepping(self):
+    loc = self.make_control()
+    loc.last_output_accel = -0.3
+    out = loc._stopping_accel(0.0, True)
+    assert out > -0.32  # first hold frame is a ramp, not stopAccel
+    for _ in range(300):
+      prev = out
+      loc.last_output_accel = out
+      out = loc._stopping_accel(0.0, True)
+      assert abs(out - prev) <= 1.0 * DT_CTRL + 1e-6
+    assert -2.01 < out < -1.9
+
+  def test_should_stop_debounce(self):
+    loc = self.make_control()
+    self.enter_stopping(loc)
+    # settle at standstill
+    out = -0.55
+    for _ in range(300):
+      out = loc.update(True, self.make_cs(0.0, 0.0, standstill=True), 0.2, True, self.ACCEL_LIMITS)
+    assert out < -1.9
+    # one dropped should_stop frame must not release the brake. cruiseState.standstill is
+    # false here, matching the observed standstill where should_stop briefly drops.
+    out = loc.update(True, self.make_cs(0.0, 0.0, standstill=True, cruise_standstill=False), 0.2, False, self.ACCEL_LIMITS)
+    assert loc.long_control_state == LongCtrlState.stopping
+    assert out < -1.9
+    # and it must eventually exit when the request to move persists
+    for _ in range(20):
+      out = loc.update(True, self.make_cs(0.0, 0.0, standstill=True, cruise_standstill=False), 0.2, False, self.ACCEL_LIMITS)
+    assert loc.long_control_state == LongCtrlState.pid

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

@@ -0,0 +1,86 @@
+import pyray as rl
+
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.selfdrive.ui.mici.onroad import SIDE_PANEL_WIDTH
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.system.ui.lib.application import FontWeight, gui_app
+from openpilot.system.ui.lib.text_measure import measure_text_cached
+from openpilot.system.ui.widgets import Widget
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost_ext import get_accel_boost_max
+
+BAR_WIDTH = 10
+VALUE_FONT_SIZE = 17
+LABEL_FONT_SIZE = 10
+
+COLOR_TRACK = rl.Color(255, 255, 255, 40)
+COLOR_TEXT = rl.Color(255, 255, 255, 235)
+
+PROFILE_STYLES = {
+  0: ("AGGR", rl.Color(255, 80, 60, 255)),
+  1: ("STD", rl.Color(0, 230, 120, 255)),
+  2: ("RELAX", rl.Color(70, 150, 255, 255)),
+}
+
+
+class AccelBoostIndicator(Widget):
+  def __init__(self):
+    super().__init__()
+    self._boost_filter = FirstOrderFilter(0.0, 0.15, 1 / gui_app.target_fps)
+    self._alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
+    self._font = gui_app.font(FontWeight.BOLD)
+    self._label_font = gui_app.font(FontWeight.MEDIUM)
+    self._profile_label = "STD"
+    self._profile_color = PROFILE_STYLES[1][1]
+    self._max_boost = 0.0
+
+  def _update_state(self):
+    sm = ui_state.sm
+    visible = (ui_state.has_longitudinal_control and sm.alive['selfdriveState'] and sm.alive['longitudinalPlan'] and
+               sm['selfdriveState'].enabled)
+    self._alpha_filter.update(1.0 if visible else 0.0)
+    self._boost_filter.update(float(sm['longitudinalPlan'].accelBoost) if visible else 0.0)
+
+    if sm.alive['selfdriveState']:
+      personality = sm['selfdriveState'].personality
+      self._profile_label, self._profile_color = PROFILE_STYLES.get(personality.raw, PROFILE_STYLES[1])
+      self._max_boost = get_accel_boost_max(personality)
+
+  def _render(self, _):
+    alpha = self._alpha_filter.x
+    if alpha < 0.01:
+      return
+
+    panel_x = self.rect.x + self.rect.width - SIDE_PANEL_WIDTH
+    margin = min(120.0, max(56.0, self.rect.height * 0.14))
+    track_top = self.rect.y + margin
+    track_bottom = self.rect.y + self.rect.height - margin
+    track_h = max(1.0, track_bottom - track_top)
+    cx = panel_x + SIDE_PANEL_WIDTH / 2
+    bar_x = cx - BAR_WIDTH / 2
+
+    # track
+    rl.draw_rectangle_rounded(rl.Rectangle(bar_x, track_top, BAR_WIDTH, track_h), 0.5, 12,
+                              rl.Color(COLOR_TRACK.r, COLOR_TRACK.g, COLOR_TRACK.b, int(COLOR_TRACK.a * alpha)))
+
+    if self._max_boost <= 0.0:
+      return
+
+    # Scale to the current profile's cap, so a maxed boost fills the bar.
+    boost = min(max(self._boost_filter.x, 0.0), self._max_boost)
+    fill_h = boost / self._max_boost * track_h
+    if fill_h > 0.0:
+      fill = rl.Color(self._profile_color.r, self._profile_color.g, self._profile_color.b, int(255 * alpha))
+      if fill_h < BAR_WIDTH:
+        rl.draw_circle(int(cx), int(track_bottom - fill_h / 2), max(1, int(BAR_WIDTH / 2)), fill)
+      else:
+        rl.draw_rectangle_rounded(rl.Rectangle(bar_x, track_bottom - fill_h, BAR_WIDTH, fill_h), 0.5, 12, fill)
+
+    label_size = measure_text_cached(self._label_font, self._profile_label, LABEL_FONT_SIZE)
+    rl.draw_text_ex(self._label_font, self._profile_label, rl.Vector2(cx - label_size.x / 2, track_top - 42),
+                    LABEL_FONT_SIZE, 0,
+                    rl.Color(self._profile_color.r, self._profile_color.g, self._profile_color.b, int(230 * alpha)))
+
+    value = f"+{boost:.2f}"
+    value_size = measure_text_cached(self._font, value, VALUE_FONT_SIZE)
+    rl.draw_text_ex(self._font, value, rl.Vector2(cx - value_size.x / 2, track_top - 30), VALUE_FONT_SIZE, 0,
+                    rl.Color(COLOR_TEXT.r, COLOR_TEXT.g, COLOR_TEXT.b, int(COLOR_TEXT.a * alpha)))

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

@@ -5,13 +5,13 @@ from opendbc.car.structs import car
 from openpilot.cereal.visionipc import VisionStreamType
 from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
 from openpilot.selfdrive.ui.mici.onroad import SIDE_PANEL_WIDTH
+from openpilot.selfdrive.ui.mici.onroad.accel_boost_indicator import AccelBoostIndicator
 from openpilot.selfdrive.ui.mici.onroad.alert_renderer import AlertRenderer
 from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer
 from openpilot.selfdrive.ui.mici.onroad.hud_renderer import HudRenderer
 from openpilot.selfdrive.ui.mici.onroad.long_indicator import LongIndicator
 from openpilot.selfdrive.ui.mici.onroad.model_renderer import ModelRenderer
 from openpilot.selfdrive.ui.mici.onroad.navigation import NavigationPanel, PANEL_WIDTH
-from openpilot.selfdrive.ui.mici.onroad.confidence_ball import ConfidenceBall
 from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
 from openpilot.system.ui.lib.application import FontWeight, gui_app, MousePos, MouseEvent, TextAlignment, TextAlignmentVertical
 from openpilot.system.ui.widgets.label import UnifiedLabel
@@ -180,7 +180,7 @@ class AugmentedRoadView(CameraView):
     self._long_indicator = LongIndicator()
     self._alert_renderer = AlertRenderer()
     self._driver_state_renderer = DriverStateRenderer()
-    self._confidence_ball = ConfidenceBall()
+    self._accel_boost_indicator = AccelBoostIndicator()
     self._offroad_label = UnifiedLabel("start the car to\nuse sunnypilot", 54, FontWeight.DISPLAY,
                                        text_color=rl.Color(255, 255, 255, int(255 * 0.9)),
                                        alignment=TextAlignment.CENTER,
@@ -206,6 +206,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, _):
@@ -275,7 +277,7 @@ class AugmentedRoadView(CameraView):
     if show_navigation:
       self._navigation.render(rl.Rectangle(self._content_rect.x + self._content_rect.width, self.rect.y, PANEL_WIDTH, self.rect.height))
     else:
-      self._confidence_ball.render(self.rect)
+      self._accel_boost_indicator.render(self.rect)
 
     self._bookmark_icon.render(self.rect)
 

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

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

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

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

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

@@ -0,0 +1,21 @@
+"""
+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 LanefulLayoutMici(NavScroller):
+  def __init__(self):
+    super().__init__()
+    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._toggle.refresh()

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

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

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

@@ -0,0 +1,117 @@
+"""
+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.
+"""
+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)
+
+
+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._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."""
+    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._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
+
+    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 by laneful'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._trust_display.update(self._trust)
+    after = path.copy()
+    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))
+
+  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))
+    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 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 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
+    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))
+
+    text = f"{abs(pull) * 100:.0f}"
+    text_w = measure_text_cached(self._font, text, 44, 0).x
+    rl.draw_text_ex(self._font, text, rl.Vector2(cx - text_w / 2, cy - 24), 44, 0, self._color((255, 255, 255), 255))
+
+    active = self._color(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, 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,
+                                self._color(CENTER_COLOR, np.clip(0.3 + 0.7 * trust, 0.0, 1.0) * 255))

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

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

openpilot/selfdrive/ui/sunnypilot/ui_state.py

@@ -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()

openpilot/sunnypilot/modeld_v2/laneful.py

@@ -0,0 +1,117 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import numpy as np
+
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
+
+LANE_WIDTH_DEFAULT = 3.6  # m (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
+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, 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:
+    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
+
+  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 = 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]))
+    # 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:
+      self.trust = 0.0
+      return 0.0
+
+    win = (X_IDXS >= 0.5 * lookahead) & (X_IDXS <= 1.5 * lookahead)
+    x = X_IDXS[win]
+    half_width = self.lane_width.x / 2
+    if l_prob * r_prob > 0.5:
+      width = np.interp(lookahead, X_IDXS, lines[2]) - np.interp(lookahead, X_IDXS, lines[1])
+      self.lane_width.update(float(np.clip(width, *LANE_WIDTH_CLIP)))
+      half_width = np.clip(lines[2, win] - lines[1, win], *LANE_WIDTH_CLIP) / 2
+    l_var = stds[1, win] ** 2 / max(l_prob, 1e-4)
+    r_var = stds[2, win] ** 2 / max(r_prob, 1e-4)
+    center = (r_var * (lines[1, win] + half_width) + l_var * (lines[2, win] - half_width)) / (l_var + r_var)
+    weight = 1.0 / (l_var * r_var / (l_var + r_var) + POINT_STD ** 2)
+
+    # Fit curvature to place the plant in center: e ~ dk x^2 / 2
+    plan_x = np.interp(x, model_output['plan'][0, :, 0], model_output['plan'][0, :, 1])
+    plan_share = np.interp(abs(model_curvature) * lookahead ** 2 / 2, [0.5, 2.0], [0.3, 0.5]) # blending up to 50% plan with action during sharp curvature
+    error = np.clip(center - ((1 - plan_share) * model_curvature * x ** 2 / 2 + plan_share * plan_x), -1.0, 1.0)
+    dk = 2.0 * np.sum(weight * error * x ** 2) / np.sum(weight * x ** 4)
+
+    # A model planning to end up further from its usual spot in the lane than the plant is now is likely going around something
+    hw_now = np.clip(lines[2, 0] - lines[1, 0], *LANE_WIDTH_CLIP) / 2 if l_prob * r_prob > 0.5 else self.lane_width.x / 2
+    center_now = (l_prob * (lines[1, 0] + hw_now) + r_prob * (lines[2, 0] - hw_now)) / (l_prob + r_prob)
+    plan_y = np.interp(lookahead, model_output['plan'][0, :, 0], model_output['plan'][0, :, 1])
+    offset = plan_y - np.interp(lookahead, x, center)
+    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)

openpilot/sunnypilot/modeld_v2/modeld.py

@@ -7,6 +7,7 @@ See the LICENSE.md file in the root directory for more details.
 """
 
 from collections.abc import Callable
+from functools import partial
 import gc
 import os
 os.environ['GMMU'] = '0'
@@ -39,6 +40,7 @@ from openpilot.selfdrive.modeld.modeld import ChestnutGpuState
 
 from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output
 from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser
+from openpilot.sunnypilot.modeld_v2.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
@@ -228,7 +230,8 @@ class ModelState(ModelStateBase):
     return outputs
 
   def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
-                            lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action:
+                            lat_action_t: float, long_action_t: float, v_ego: float,
+                            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,
@@ -240,6 +243,8 @@ class ModelState(ModelStateBase):
     else:
       desired_accel = model_output['action'][0, 1]
       desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2
+    if 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)
@@ -366,6 +371,8 @@ def main(demo=False):
   DH = DesireHelper()
   meta_constants = load_meta_constants()
   RELC = RoadEdgeLaneChangeController()
+  laneful = Laneful()
+  laneful_enabled = False
 
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -409,6 +416,7 @@ def main(demo=False):
       model.lat_delay = get_lat_delay(params, sm["lateralDelay"].lateralDelay)
       model.PLANPLUS_CONTROL = params.get("PlanplusControl", return_default=True)
       camera_offset_helper.set_offset(params.get("CameraOffset", return_default=True))
+      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)
@@ -482,7 +490,9 @@ def main(demo=False):
 
       if not sm['carControl'].latActive:
         prev_action.desiredCurvature = sm['controlsState'].curvature
-      action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
+      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_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,
@@ -499,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
 

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

@@ -0,0 +1,67 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from openpilot.cereal import log
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.accel_boost import AccelBoost, ACCEL_BOOST_MAX
+from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
+
+SCALE_RELAXED = 0.3
+SCALE_STANDARD = 0.5
+SCALE_AGGRESSIVE = 0.7
+
+
+def get_accel_boost_scale(personality) -> float:
+  if personality == log.LongitudinalPersonality.aggressive:
+    return SCALE_AGGRESSIVE
+  if personality == log.LongitudinalPersonality.relaxed:
+    return SCALE_RELAXED
+  return SCALE_STANDARD
+
+
+def get_accel_boost_max(personality) -> float:
+  """Maximum reported boost: the stock cap plus the profile-scaled extra cap."""
+  return ACCEL_BOOST_MAX * (1.0 + get_accel_boost_scale(personality))
+
+
+class AccelBoostExt:
+  def __init__(self, stock: AccelBoost | None = None):
+    self._stock = stock if stock is not None else AccelBoost()
+    self._extra = AccelBoost()
+    self.params = Params()
+    self.frame = -1
+    self.enabled = self.params.get_bool("FollowDistanceAccelBoost")
+
+    self.stock_boost = 0.0
+    self.extra_boost = 0.0
+
+  @property
+  def total_boost(self) -> float:
+    return self._stock.total_boost + self.extra_boost
+
+  def _update_params(self) -> None:
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.enabled = self.params.get_bool("FollowDistanceAccelBoost")
+
+  def update(self, sm, output_a_target_e2e, output_a_target_mpc, a_cruise):
+    self.frame += 1
+    self._update_params()
+
+    if self.enabled and sm['selfdriveState'].enabled:
+      scale = get_accel_boost_scale(sm['selfdriveState'].personality)
+      extra_output = self._extra.update(sm, output_a_target_e2e, output_a_target_mpc, a_cruise)
+      self.extra_boost = (extra_output - output_a_target_e2e) * scale
+    else:
+      self.extra_boost = 0.0
+      self._extra.total_boost = 0.0
+      self._extra.boost_this_override = 0.0
+      self._extra.boost_eligible = False
+
+    target = output_a_target_e2e + self.extra_boost
+    output = self._stock.update(sm, target, output_a_target_mpc, a_cruise)
+    self.stock_boost = output - target
+    return output

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

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

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

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

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -620,6 +620,22 @@
                 }
               ]
             },
+            {
+              "key": "FollowDistanceAccelBoost",
+              "widget": "toggle",
+              "title": "Extra Acceleration Boost by Following Distance",
+              "description": "Adds extra acceleration on top of the stock gas-override boost. The extra boost follows the Following Distance setting, strongest when Aggressive and weakest when Relaxed. The stock boost remains active when this is off.",
+              "enablement": [
+                {
+                  "type": "offroad_only"
+                },
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ]
+            },
             {
               "key": "IntelligentCruiseButtonManagement",
               "widget": "toggle",

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

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