modeld_v2: plan braking toggle (port of comma's plan-long)

Adds a PlanLong toggle ("plan braking (alpha)" on the comma four Models
screen, custom models only). When on, the longitudinal accel keeps the
action head's speed level but slows early from the whole plan: it won't
run past where the plan expects to be from 2 s on, with 1.5 position
stds of slack and the actuator delay accounted for. This is the
longitudinal half of commaai/openpilot plan-long (Shane Smiskol,
1193b78db..1c2df3b3f). Its lane-center steering change and the 2.5 m/s
curvature divisor are not included; steering and the turn floor are
unchanged.

The accel boost is skipped while the toggle is on, since it would
cancel the plan's early slowing.

Rebuilds the prebuilt ARM64 libparams_c.so with the repo's params
rebuild tool so the new key is registered. Compared in the ARM64
container, the registry differs from the previous library only by
PlanLong (bool, default 0, persistent, backup).

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

openpilot/common/params_keys.h

@@ -265,6 +265,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"NavModelState", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, JSON}},
     {"Blendful", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
+    {"PlanLong", {PERSISTENT | BACKUP, BOOL, "0"}},
 
     // mapd
     {"MapAdvisorySpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT}},

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -109,7 +109,11 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     self.a_cruise = get_cruise_accel(v_cruise, v_ego, self.a_cruise, self.dt)
     cruise_should_stop = should_stop(v_ego, self.a_cruise)
 
-    output_a_target_e2e = self.accel_boost.update(sm, output_a_target_e2e, output_a_target_mpc, self.a_cruise)
+    if self.plan_long:
+      # the plan already slows early on its own; a boost would cancel that
+      self.accel_boost.total_boost = 0.0
+    else:
+      output_a_target_e2e = self.accel_boost.update(sm, output_a_target_e2e, output_a_target_mpc, self.a_cruise)
 
     candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
                   (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop),

openpilot/selfdrive/modeld/modeld.py

@@ -49,7 +49,7 @@ SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 
 LAT_SMOOTH_SECONDS = 0.0
 LONG_SMOOTH_SECONDS = 0.3
-MIN_LAT_CONTROL_SPEED = 1.6
+MIN_LAT_CONTROL_SPEED = 0.3
 BIG_MODEL_TIMEOUT = 60
 
 

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

@@ -109,6 +109,11 @@ class ModelsLayoutMici(NavScroller):
                                            description=tr("Adds a gentle nudge toward lane center on top of the model's own steering, stronger when " +
                                                           "lane lines are clear. Yields around obstacles. Not available with the Default model."))
 
+    self.plan_long_toggle = BigParamControl(tr("plan braking (alpha)"), "PlanLong",
+                                            description=tr("Starts slowing earlier for stops and curves the model's plan sees ahead, " +
+                                                           "while the model still sets the cruising speed. Turns off the accel boost. " +
+                                                           "Needs openpilot longitudinal. Not available with the Default model."))
+
     self.map_turn_toggle = BigParamControl(tr("signal navigation (experimental)"), "MapboxNextTurnEnabled",
       toggle_callback=self._enable_map_turns,
       description=tr("Signals can prepare a short street route while normal lane-change checks remain available. " +
@@ -118,7 +123,7 @@ class ModelsLayoutMici(NavScroller):
 
     self.main_items = [self.current_model_info, self.preparation_details_btn, self.select_model_btn, self.prepare_navigation_btn, self.cancel_download_btn,
                        self.refresh_btn, self.clear_cache_btn,
-                       self.blendful_toggle, self.map_turn_toggle]
+                       self.blendful_toggle, self.plan_long_toggle, self.map_turn_toggle]
     self._scroller.add_widgets(self.main_items)
 
   def _prepare_navigation_model(self):
@@ -271,6 +276,7 @@ class ModelsLayoutMici(NavScroller):
   def show_event(self):
     super().show_event()
     self.blendful_toggle.refresh()
+    self.plan_long_toggle.refresh()
 
   def _update_state(self):
     super()._update_state()
@@ -292,6 +298,11 @@ class ModelsLayoutMici(NavScroller):
     blendful_value = "" if blendful_allowed else tr("custom model only")
     if self.blendful_toggle.value != blendful_value:
       self.blendful_toggle.set_value(blendful_value)
+    # plan braking lives in the custom model's modeld, same as blendful
+    self.plan_long_toggle.set_checked(blendful_allowed and ui_state.params.get_bool("PlanLong"))
+    self.plan_long_toggle.set_enabled(blendful_allowed)
+    if self.plan_long_toggle.value != blendful_value:
+      self.plan_long_toggle.set_value(blendful_value)
 
     manager = self.model_manager
     self._download_frame += 1

openpilot/sunnypilot/modeld_v2/modeld.py

@@ -35,7 +35,7 @@ from openpilot.common.realtime import config_realtime_process, DT_MDL
 from openpilot.common.transformations.camera import DEVICE_CAMERAS
 from openpilot.common.transformations.model import get_warp_matrix
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
-from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value
+from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, get_curvature_from_plan, smooth_value
 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
@@ -49,6 +49,8 @@ from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.modeld_v2.helpers import load_model
 from openpilot.sunnypilot.modeld_v2.frame_fusion import fused_with_warp
 from openpilot.sunnypilot.modeld_v2.model_adapters import get_model_adapter
+from openpilot.sunnypilot.modeld_v2.turn_plan_floor import apply_turn_plan_floor, turn_plan_floor_scale
+from openpilot.sunnypilot.modeld_v2.plan_long import PlanLong
 from openpilot.sunnypilot.modeld_v2.navigation_input import (NavigationInputBuffer, NavigationStatePublisher, SignalRouteGuard,
                                                           destination_route_context, navigation_capability)
 from openpilot.sunnypilot.models.helpers import get_active_bundle
@@ -105,7 +107,7 @@ class ModelState(ModelStateBase):
 
     self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0"))
     self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0"))
-    self.MIN_LAT_CONTROL_SPEED = 1.6
+    self.MIN_LAT_CONTROL_SPEED = 0.3
     self.PLANPLUS_CONTROL: float = 1.0
     self.chestnut = chestnut
 
@@ -238,18 +240,31 @@ 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,
-                            blendful: Callable[[float], float] | None = None) -> log.ModelDataV2.Action:
+                            blendful: Callable[[float], float] | None = None, turn_sign: float = 0.,
+                            plan_long: PlanLong | 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,
-                                          action_t=long_action_t)
+      if plan_long is not None:
+        desired_accel = plan_long.update(plan, model_output['plan_stds'][0], self.constants.T_IDXS, long_action_t)
+      else:
+        desired_accel = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS,
+                                            action_t=long_action_t)
 
       curvature_plan = (plan + (self.PLANPLUS_CONTROL - 1.0) * model_output['planplus'][0]
                         if 'planplus' in model_output and self.PLANPLUS_CONTROL != 1.0 else plan)
       desired_curvature = get_curvature_from_output(model_output, curvature_plan, v_ego, lat_action_t, self.mlsim)
     else:
       desired_accel = model_output['action'][0, 1]
+      if plan_long is not None:
+        desired_accel = plan_long.update(model_output['plan'][0], model_output['plan_stds'][0], self.constants.T_IDXS, long_action_t,
+                                         action_accel=desired_accel)
       desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2
+      if turn_sign and (floor_scale := turn_plan_floor_scale(v_ego)) > 0.:
+        # the action head under-commits in slow turns while the model's own plan has the turn; use the plan as a floor
+        plan = model_output['plan'][0]
+        plan_curvature = get_curvature_from_plan(plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan[:, Plan.ORIENTATION_RATE][:, 2],
+                                                 self.constants.T_IDXS, v_ego, lat_action_t)
+        desired_curvature = apply_turn_plan_floor(desired_curvature, plan_curvature, turn_sign, floor_scale)
     if blendful is not None:
       desired_curvature += blendful(desired_curvature)
 
@@ -389,6 +404,8 @@ def main(demo=False):
   RELC = RoadEdgeLaneChangeController()
   blendful = Blendful()
   blendful_enabled = False
+  plan_long = PlanLong()
+  plan_long_enabled = False
 
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -433,6 +450,9 @@ def main(demo=False):
       model.PLANPLUS_CONTROL = params.get("PlanplusControl", return_default=True)
       camera_offset_helper.set_offset(params.get("CameraOffset", return_default=True))
       blendful_enabled = params.get_bool("Blendful")
+      plan_long_enabled = params.get_bool("PlanLong")
+      if not plan_long_enabled:
+        plan_long.reset()
     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)
@@ -545,8 +565,10 @@ def main(demo=False):
       if not sm['carControl'].latActive:
         prev_action.desiredCurvature = sm['controlsState'].curvature
       blendful_active = desire == log.Desire.none and DH.lane_change_state == log.LaneChangeState.off
+      turn_sign = 1. if desire == log.Desire.turnRight else -1. if desire == log.Desire.turnLeft else 0.  # curvature + is right
       action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego,
-                                           partial(blendful.update, model_output, v_ego, blendful_active, enabled=blendful_enabled))
+                                           partial(blendful.update, model_output, v_ego, blendful_active, enabled=blendful_enabled),
+                                           turn_sign=turn_sign, plan_long=plan_long if plan_long_enabled else None)
       prev_action = action
       fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
                      publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,

openpilot/sunnypilot/modeld_v2/plan_long.py

@@ -0,0 +1,50 @@
+"""
+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.selfdrive.controls.lib.drive_helpers import get_accel_from_plan
+from openpilot.sunnypilot.modeld_v2.constants import Plan
+
+# Longitudinal half of comma's plan-long experiment (commaai/openpilot plan-long, 1193b78db..1c2df3b3f, Shane Smiskol).
+# The action head is on-policy but slow to see stops and curves coming; the plan sees them seconds ahead but is
+# off-policy and runs above the speed the road needs. So the action sets the speed level and the plan adds early slowing.
+PLAN_OFFSET_RC = 3.0  # s, how fast the plan's accel level is pulled to the action's
+HORIZON_START_T = 2.0  # s, plan points from here on can ask for early slowing
+X_STD_SLACK = 1.5  # plan position stds the car may run past where the plan expects it
+
+
+def get_accel_from_plan_horizon(a_now, speeds, accels, x, y, x_stds, t_idxs, action_t):
+  # distance along the path, x alone is short in curves
+  dist = x[0] + np.concatenate(([0.0], np.cumsum(np.hypot(np.diff(x), np.diff(y)))))
+  ahead = t_idxs >= HORIZON_START_T
+  t = t_idxs[ahead]
+  # where we get to if the current accel holds until the new one acts after action_t
+  x_hold = speeds[0] * t + accels[0] * action_t * (t - action_t / 2)
+  # slow down early enough to not pass where the plan expects to be, with its uncertainty as slack
+  a_slow = min(np.min(2 * (dist[ahead] + X_STD_SLACK * x_stds[ahead] - x_hold) / (t - action_t) ** 2), 0.0)
+  # stop accelerating first if the plan slows down later
+  return float(max(a_now, 0.0) * np.interp(-a_slow, [0.0, 0.3], [1.0, 0.0]) + min(a_now, a_slow))
+
+
+class PlanLong:
+  def __init__(self):
+    self.plan_offset = FirstOrderFilter(0.0, PLAN_OFFSET_RC, DT_MDL)
+
+  def reset(self):
+    self.plan_offset.x = 0.0
+
+  def update(self, plan, plan_stds, t_idxs, action_t, action_accel=None) -> float:
+    t_idxs = np.asarray(t_idxs)
+    speeds, accels = plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0]
+    a_plan = get_accel_from_plan(speeds, accels, t_idxs, action_t=action_t)
+    if action_accel is not None:
+      # keep the plan's changes, but the action's level
+      a_plan -= self.plan_offset.update(a_plan - action_accel)
+    return get_accel_from_plan_horizon(a_plan, speeds, accels, plan[:, Plan.POSITION][:, 0], plan[:, Plan.POSITION][:, 1],
+                                       plan_stds[:, Plan.POSITION][:, 0], t_idxs, action_t)

openpilot/sunnypilot/modeld_v2/tests/test_plan_long.py

@@ -0,0 +1,111 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from typing import Any
+
+import numpy as np
+import pytest
+
+from openpilot.cereal import log
+from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
+from openpilot.sunnypilot.modeld_v2.modeld import ModelState
+from openpilot.sunnypilot.modeld_v2.plan_long import PlanLong, get_accel_from_plan_horizon
+
+T = np.array(ModelConstants.T_IDXS)
+ACTION_T = 0.3
+X_STD = 0.5
+
+
+def make_plan(speeds, x=None, y=None):
+  plan = np.zeros((len(T), ModelConstants.PLAN_WIDTH), dtype=np.float32)
+  plan[:, Plan.VELOCITY.start] = speeds
+  plan[:, Plan.ACCELERATION.start] = np.gradient(speeds, T)
+  plan[:, Plan.POSITION.start] = np.concatenate(([0.], np.cumsum(np.diff(T) * (speeds[1:] + speeds[:-1]) / 2))) if x is None else x
+  if y is not None:
+    plan[:, Plan.POSITION.start + 1] = y
+  stds = np.full_like(plan, X_STD)
+  return plan, stds
+
+
+def cruise_then_stop(v0=15., cruise_t=3., decel=3.):
+  # holds speed, then brakes hard to a stop: the plan sees the stop well before its first seconds do
+  return np.where(T < cruise_t, v0, np.maximum(v0 - decel * (T - cruise_t), 0.))
+
+
+def horizon(a_now, plan, stds):
+  return get_accel_from_plan_horizon(a_now, plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], plan[:, Plan.POSITION][:, 0],
+                                     plan[:, Plan.POSITION][:, 1], stds[:, Plan.POSITION][:, 0], T, ACTION_T)
+
+
+@pytest.mark.parametrize("a_now", [-0.5, 0., 0.5])
+def test_constant_speed_plan_leaves_accel_alone(a_now):
+  plan, stds = make_plan(np.full(len(T), 15.))
+  assert horizon(a_now, plan, stds) == pytest.approx(a_now)
+
+
+def test_slows_early_for_a_stop_the_plan_sees_ahead():
+  plan, stds = make_plan(cruise_then_stop())
+  assert horizon(0., plan, stds) < -1.0
+
+
+def test_stops_accelerating_when_the_plan_slows_later():
+  plan, stds = make_plan(cruise_then_stop())
+  assert horizon(0.5, plan, stds) < 0.
+
+
+def test_never_weakens_harder_braking():
+  plan, stds = make_plan(cruise_then_stop())
+  assert horizon(-2.5, plan, stds) == -2.5
+
+
+def test_curved_path_is_not_mistaken_for_slowing():
+  # x alone is short in a curve; the distance along the path is what the speed covers
+  v, radius = 10., 100.
+  speeds = np.full(len(T), v)
+  angle = v * T / radius
+  plan, stds = make_plan(speeds, x=radius * np.sin(angle), y=radius * (1 - np.cos(angle)))
+  assert horizon(0., plan, stds) == pytest.approx(0., abs=1e-3)
+
+
+def test_action_sets_the_speed_level():
+  # the plan's level drifts above what the road needs; after a few seconds the action's level wins
+  plan, stds = make_plan(np.full(len(T), 15.))
+  plan_long = PlanLong()
+  for _ in range(int(15. / 0.05)):
+    accel = plan_long.update(plan, stds, T, ACTION_T, action_accel=-0.3)
+  assert accel == pytest.approx(-0.3, abs=0.01)
+
+
+def test_reset_clears_the_level_offset():
+  plan, stds = make_plan(np.full(len(T), 15.))
+  plan_long = PlanLong()
+  for _ in range(100):
+    plan_long.update(plan, stds, T, ACTION_T, action_accel=-0.3)
+  plan_long.reset()
+  assert plan_long.update(plan, stds, T, ACTION_T) == pytest.approx(0.)
+
+
+class MockStruct:
+  def __init__(self, **kwargs):
+    for k, v in kwargs.items():
+      setattr(self, k, v)
+
+
+def run_action(plan_long):
+  state: Any = MockStruct(PLANPLUS_CONTROL=1.0, LONG_SMOOTH_SECONDS=0.3, LAT_SMOOTH_SECONDS=0.1, MIN_LAT_CONTROL_SPEED=0.3, mlsim=False,
+                          generation=12, constants=MockStruct(T_IDXS=ModelConstants.T_IDXS))
+  plan, stds = make_plan(cruise_then_stop())
+  model_output = {'plan': plan[None], 'plan_stds': stds[None], 'action': np.array([[0., 0.]], dtype=np.float32)}
+  return ModelState.get_action_from_model(state, model_output, log.ModelDataV2.Action(), 0.3, ACTION_T, 15.,  # type: ignore[arg-type]
+                                          plan_long=plan_long)
+
+
+def test_toggle_off_uses_the_action_accel():
+  assert run_action(None).desiredAcceleration == 0.
+
+
+def test_toggle_on_brakes_early_from_the_plan():
+  assert run_action(PlanLong()).desiredAcceleration < 0.

openpilot/sunnypilot/modeld_v2/tests/test_turn_plan_floor.py

@@ -0,0 +1,53 @@
+"""
+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 pytest
+
+from openpilot.sunnypilot.modeld_v2.turn_plan_floor import TURN_PLAN_FLOOR_BP, TURN_PLAN_FLOOR_V, apply_turn_plan_floor, \
+  turn_plan_floor_scale
+
+RIGHT, LEFT, NONE = 1., -1., 0.
+LOW_SPEED_SCALE = TURN_PLAN_FLOOR_V[0]
+
+
+def test_no_turn_leaves_action_alone():
+  assert apply_turn_plan_floor(0.01, 0.2, NONE, LOW_SPEED_SCALE) == 0.01
+
+
+@pytest.mark.parametrize("sign", [RIGHT, LEFT])
+def test_weak_action_is_raised_to_scaled_plan(sign):
+  # curvature is + to the right, so a left turn is negative
+  assert apply_turn_plan_floor(sign * 0.03, sign * 0.10, sign, LOW_SPEED_SCALE) == pytest.approx(sign * LOW_SPEED_SCALE * 0.10)
+
+
+@pytest.mark.parametrize("sign", [RIGHT, LEFT])
+def test_strong_action_is_kept(sign):
+  assert apply_turn_plan_floor(sign * 0.12, sign * 0.10, sign, LOW_SPEED_SCALE) == sign * 0.12
+
+
+@pytest.mark.parametrize("sign", [RIGHT, LEFT])
+def test_plan_against_the_turn_never_weakens_or_reverses(sign):
+  assert apply_turn_plan_floor(sign * 0.05, -sign * 0.10, sign, LOW_SPEED_SCALE) == sign * 0.05
+
+
+def test_floor_only_pushes_toward_the_requested_side():
+  # a right-turn request must never add left curvature, even if action and plan both point left
+  assert apply_turn_plan_floor(-0.02, -0.10, RIGHT, LOW_SPEED_SCALE) == -0.02
+
+
+@pytest.mark.parametrize("sign", [RIGHT, LEFT])
+def test_zero_scale_is_off_even_for_a_counter_steer(sign):
+  # above the fade-out speed the action head steers alone, including small corrections against the turn
+  assert apply_turn_plan_floor(-sign * 0.01, sign * 0.10, sign, 0.) == -sign * 0.01
+
+
+def test_scale_is_full_at_low_speed_and_fades_out():
+  low, high = TURN_PLAN_FLOOR_BP
+  assert turn_plan_floor_scale(0.) == LOW_SPEED_SCALE
+  assert turn_plan_floor_scale(low) == LOW_SPEED_SCALE
+  assert turn_plan_floor_scale((low + high) / 2) == pytest.approx(LOW_SPEED_SCALE / 2)
+  assert turn_plan_floor_scale(high) == 0.
+  assert turn_plan_floor_scale(30.) == 0.

openpilot/sunnypilot/modeld_v2/turn_plan_floor.py

@@ -0,0 +1,30 @@
+"""
+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
+
+# Share of the plan-derived curvature the command may not fall below during a requested turn, by speed (m/s).
+# The action head falls short of the plan by a roughly fixed lateral acceleration (~0.2 m/s^2), which costs the most
+# curvature at low speed: logged turns put it at ~0.65-0.8x the plan below 4 m/s and ~0.85-0.9x above 6 m/s.
+# The plan reads tight against driven turns, so the floor stays below 1 and fades out where the action head is close.
+TURN_PLAN_FLOOR_BP = [4., 8.]
+TURN_PLAN_FLOOR_V = [0.85, 0.]
+
+
+def turn_plan_floor_scale(v_ego: float) -> float:
+  return float(np.interp(v_ego, TURN_PLAN_FLOOR_BP, TURN_PLAN_FLOOR_V))
+
+
+def apply_turn_plan_floor(action_curvature: float, plan_curvature: float, turn_sign: float, scale: float) -> float:
+  """Keep the steering command from falling below a scaled share of the model's own plan during a requested turn.
+
+  Curvature is + to the right. turn_sign is +1 for a right turn, -1 for a left turn and 0 when no turn is requested.
+  The floor only acts in the turn direction: it can strengthen a turn the action head under-commits to, but never
+  weakens or reverses the action. A scale of 0 turns the floor off.
+  """
+  if turn_sign == 0. or scale <= 0.:
+    return action_curvature
+  return turn_sign * max(turn_sign * action_curvature, scale * turn_sign * plan_curvature)

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

@@ -8,6 +8,9 @@ See the LICENSE.md file in the root directory for more details.
 from openpilot.cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
 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
@@ -33,6 +36,10 @@ class LongitudinalPlannerSP:
     self.output_v_target = 0.
     self.output_a_target = 0.
 
+    self.params = Params()
+    self.params_frame = 0
+    self.plan_long = self.params.get_bool("PlanLong")
+
   def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
     CS = sm['carState']
     v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
@@ -64,6 +71,9 @@ class LongitudinalPlannerSP:
     return self.output_v_target, self.output_a_target
 
   def update(self, sm: messaging.SubMaster) -> None:
+    self.params_frame += 1
+    if self.params_frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.plan_long = self.params.get_bool("PlanLong")
     self.events_sp.clear()
     self.e2e_alerts_helper.update(sm, self.events_sp)
 

openpilot/tools/params/README.md

@@ -22,7 +22,9 @@ The checked-in `build_provenance.json` describes the shipped feature build.
 The previous build preserved all 272 original key names, types and raw defaults
 and added five Mapbox keys. The navigation build adds only `NavModelState`;
 all 277 previous registry entries were compared against the real ARM64 library
-and remain identical. The status JSON clears on manager startup and going offroad. The C API exports and dynamic
+and remain identical. The plan braking build adds only the `PlanLong` bool (persistent, backup, default
+`0`); all 278 previous registry entries were compared against the real ARM64 library and remain identical.
+The status JSON clears on manager startup and going offroad. The C API exports and dynamic
 library dependencies match the base release, with no increased symbol-version
 requirements. The `params_keys_by_flag` correction returns distinct buffers
 owned by the handle instead of aliasing one temporary string.

openpilot/tools/params/build_provenance.json

@@ -1,6 +1,6 @@
 {
   "schemaVersion": 1,
-  "baseCommit": "3b1816ae6c1521b91f98907e2034f7ef1bda8c8b",
+  "baseCommit": "26b7da53e64ba81a2f98eea24191c706d2a4d03e",
   "sourceTreeIncludesFeatureChanges": true,
   "builder": {
     "architecture": "native aarch64 Linux",
@@ -14,7 +14,7 @@
     "openpilot/common/params.h": "3c7b87d050c79275a401597dc7d15ae9fa57c2316c816ff5472cbdd84588a6e6",
     "openpilot/common/params.cc": "0074bdf766e9bf410f21820b8f730ed9403117c54725b6193176f28a14233737",
     "openpilot/common/params_c.cc": "7c3c86301ab3d65aa0eecc3f7b61c3baf4730baf72a9166cbb4de4237419a48d",
-    "openpilot/common/params_keys.h": "738e4fa928acbd2980a9aac590320f1b8bed4fe1a98ed06db45b890114f37fdc",
+    "openpilot/common/params_keys.h": "1884bbd57bfa98ab7230292e069f7e9aea4c81aede45edce94ed2735fc663a9e",
     "openpilot/common/queue.h": "f7c75c847ae63dfd6e864406e722248540a79908270e85ca2674b43407408fcf",
     "openpilot/common/timing.h": "66a580e240fa4fe4af03bc3073ddd0f4db5d1027720e8adfa92ba448f7c8bc71",
     "openpilot/common/util.cc": "4479ecf72465e8f453d8af78447f7715f02d9397c58a49048f2bbc87a96d6b8a",
@@ -57,7 +57,7 @@
   ],
   "rebuildTools": {
     "Dockerfile": "c60c96043ea7b6d8ad94ab4e9181033606ae35e4fb43e034ab38b7505d867cff",
-    "README.md": "65b95ef339e8a0183c7030cf759d042c128c0ba846080fecfb5c19740db1cda7",
+    "README.md": "ac2b563868aeea9dc5fa101b97d990b661d0fca428b566867b2a0688231ae5aa",
     "verify.py": "b068578579c1694c7b1594deca244917894ba43d69a80651058280c9a60a1b9f",
     "rebuild.py": "62f25ff2e6bd7daa9a635d6a6f49b7fdf03533370f2f75296d77003bed01d765"
   },
@@ -94,8 +94,8 @@
     ],
     "compiler": "g++ (Debian 12.2.0-14+deb12u1) 12.2.0",
     "machine": "aarch64",
-    "sha256": "4391c55759edb28c41987914e5b02c7fa3df7a4c499a884d67be1f1b16cac511",
-    "bytes": 4132776
+    "sha256": "abc2c0086d078521c63d8d028aa69ca681f1a9e50149c85600cc55908679154a",
+    "bytes": 4137880
   },
   "verification": {
     "originalTestMethods": [
@@ -181,7 +181,7 @@
       ]
     },
     "rebuilt": {
-      "sha256": "4391c55759edb28c41987914e5b02c7fa3df7a4c499a884d67be1f1b16cac511",
+      "sha256": "abc2c0086d078521c63d8d028aa69ca681f1a9e50149c85600cc55908679154a",
       "needed": [
         "libstdc++.so.6",
         "libgcc_s.so.1",
@@ -220,27 +220,39 @@
     "Original Params method bodies use isolated test-directory scaffolding and a logging stub.",
     "No model, steering, Tesla, cooperative steering, CAN or safety binaries were rebuilt."
   ],
-  "previousRegistryCompatibility": {
-    "originalKeyCount": 272,
-    "rebuiltKeyCount": 277,
-    "exactAddedKeys": [
-      "MapboxNextTurnEnabled",
-      "MapboxTurnContext",
-      "MapboxTurnCue",
-      "MapboxTurnIntent",
-      "MapboxTurnLeadDistance"
-    ],
-    "allOriginalNamesTypesDefaultsUnchanged": true,
-    "executionArchitecture": "nativeaarch64Linux"
-  },
+  "previousRegistryCompatibility": [
+    {
+      "originalKeyCount": 272,
+      "rebuiltKeyCount": 277,
+      "exactAddedKeys": [
+        "MapboxNextTurnEnabled",
+        "MapboxTurnContext",
+        "MapboxTurnCue",
+        "MapboxTurnIntent",
+        "MapboxTurnLeadDistance"
+      ],
+      "allOriginalNamesTypesDefaultsUnchanged": true,
+      "executionArchitecture": "nativeaarch64Linux"
+    },
+    {
+      "originalKeyCount": 277,
+      "rebuiltKeyCount": 278,
+      "exactAddedKeys": [
+        "NavModelState"
+      ],
+      "allOriginalNamesTypesDefaultsUnchanged": true,
+      "executionArchitecture": "nativeaarch64Linux",
+      "baselineCommit": "3b1816ae6c1521b91f98907e2034f7ef1bda8c8b"
+    }
+  ],
   "registryCompatibility": {
-    "originalKeyCount": 277,
-    "rebuiltKeyCount": 278,
+    "originalKeyCount": 278,
+    "rebuiltKeyCount": 279,
     "exactAddedKeys": [
-      "NavModelState"
+      "PlanLong"
     ],
     "allOriginalNamesTypesDefaultsUnchanged": true,
     "executionArchitecture": "nativeaarch64Linux",
-    "baselineCommit": "3b1816ae6c1521b91f98907e2034f7ef1bda8c8b"
+    "baselineCommit": "26b7da53e64ba81a2f98eea24191c706d2a4d03e"
   }
 }