Merge remote-tracking branch 'origin/laneful-curves' into tempt

# Conflicts:
#	openpilot/selfdrive/ui/mici/onroad/model_renderer.py
#	openpilot/sunnypilot/modeld_v2/modeld.py

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

@@ -115,7 +115,7 @@ jobs:
           env ${TG_FLAGS} python3 ${{ github.workspace }}/${{ env.COMPILER }}/compile_onnx.py \
             ${{ github.workspace }}/${{ env.SMALL_ONNX }} \
             ${{ github.workspace }}/${{ env.SMALL_PKL }} \
-            --device-input "*" --out-of-band --benchmark-runs 1
+            --out-of-band --benchmark-runs 1
 
       - name: Prepare output
         env:
@@ -440,9 +440,10 @@ jobs:
           TG_FLAGS="DEV=QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1"
 
           taskset -c 7 env ${TG_FLAGS} python3 \
-            ${{ github.workspace }}/tinygrad_repo/examples/openpilot/compile3.py \
+            ${{ github.workspace }}/tinygrad_repo/examples/openpilot/compile_onnx.py \
             ${{ github.workspace }}/${{ env.DM_ONNX }} \
-            ${{ github.workspace }}/${{ env.DM_PKL }}
+            ${{ github.workspace }}/${{ env.DM_PKL }} \
+            --out-of-band --benchmark-runs 1
 
       - name: Compile DM warp
         run: |

.github/workflows/lfs-maintenance.yaml

@@ -33,7 +33,7 @@ jobs:
 
       - name: LFS Fetch
         run: |
-          git lfs fetch
+          git lfs fetch || echo "::warning::some objects are missing on commaai/openpilot-lfs; mirroring the rest"
 
       - name: Set up Git
         run: |
@@ -68,6 +68,7 @@ jobs:
           git config lfs.activitytimeout 300
           git config lfs.transfer.maxretries 5
           git config lfs.concurrenttransfers 4
+          git config lfs.allowincompletepush true
 
       - name: Push LFS
         id: sync-and-commit
@@ -81,3 +82,80 @@ jobs:
           done
           echo "All push attempts failed"
           exit 1
+
+      - name: Checkout sunnypilot
+        uses: actions/checkout@v4
+        with:
+          repository: 'sunnypilot/sunnypilot'
+          path: sunnypilot
+          fetch-depth: 0
+          filter: blob:limit=1k
+          lfs: false
+
+      - name: Reference objects on sunnypilot-lfs
+        env:
+          HF_TOKEN: ${{ secrets.HF_TOKEN }}
+        run: |
+          set -euo pipefail
+          GIT_LFS_SKIP_SMUDGE=1 git clone --quiet https://huggingface.co/sunnypilot/sunnypilot-lfs hf-lfs
+          mkdir -p hf-lfs/all
+          cat > scan_pointers.py <<'EOF'
+          import re, subprocess, sys, threading
+          repo = sys.argv[1]
+          chk = subprocess.run(["git", "-C", repo, "cat-file", "--batch-all-objects",
+                                "--batch-check=%(objectname) %(objecttype) %(objectsize)"],
+                               capture_output=True, text=True, check=True).stdout
+          small = [f[0] for f in (l.split() for l in chk.splitlines()) if f[1] == "blob" and int(f[2]) < 1024]
+          p = subprocess.Popen(["git", "-C", repo, "cat-file", "--batch=%(objectsize)"],
+                               stdin=subprocess.PIPE, stdout=subprocess.PIPE)
+          def feed():
+            for sha in small:
+              p.stdin.write(sha.encode() + b"\n")
+            p.stdin.close()
+          threading.Thread(target=feed, daemon=True).start()
+          rx = re.compile(rb"^version https://git-lfs\.github\.com/spec/v1\noid sha256:([0-9a-f]{64})\nsize ([0-9]+)\n")
+          seen = {}
+          for _ in small:
+            n = int(p.stdout.readline()); body = p.stdout.read(n); p.stdout.read(1)
+            m = rx.match(body)
+            if m:
+              seen.setdefault(m.group(1).decode(), m.group(2).decode())
+          for oid, size in sorted(seen.items()):
+            print(oid, size)
+          EOF
+          for repo in . sunnypilot; do
+            python3 scan_pointers.py "$repo"
+          done | sort -u | while read -r oid size; do
+            [ -e "hf-lfs/all/$oid" ] && continue
+            printf 'version https://git-lfs.github.com/spec/v1\noid sha256:%s\nsize %s\n' "$oid" "$size" > "hf-lfs/all/$oid"
+            echo "$oid"
+          done > new_oids.txt
+          echo "new pointer files: $(wc -l < new_oids.txt)"
+
+          dropped=0
+          while mapfile -t -n 100 chunk && ((${#chunk[@]})); do
+            body=$(for oid in "${chunk[@]}"; do
+                     size=$(awk '/^size /{print $2}' "hf-lfs/all/$oid")
+                     printf '{"oid":"%s","size":%s}\n' "$oid" "$size"
+                   done | jq -s '{operation:"download",transfers:["basic"],objects:.}')
+            for attempt in 1 2 3 4 5; do
+              resp=$(curl -sS -u "user:${HF_TOKEN}" -w '\n%{http_code}' \
+                       -H 'Accept: application/vnd.git-lfs+json' -H 'Content-Type: application/vnd.git-lfs+json' \
+                       --data "$body" "${LFS_URL}/objects/batch") && [ "$(tail -n1 <<<"$resp")" = 200 ] && break
+              echo "batch check attempt $attempt failed ($(tail -n1 <<<"$resp")), retrying in 60s"; sleep 60
+            done
+            [ "$(tail -n1 <<<"$resp")" = 200 ] || { echo "batch check failed"; exit 1; }
+            for oid in $(sed '$d' <<<"$resp" | jq -r '.objects[] | select(.error) | .oid'); do
+              rm -f "hf-lfs/all/$oid"; dropped=$((dropped+1))
+            done
+          done < new_oids.txt
+          echo "dropped (not on HF): $dropped"
+
+          cd hf-lfs
+          git add -A all
+          if git diff --cached --quiet; then
+            echo "nothing new to reference"
+            exit 0
+          fi
+          git commit -q -m "Reference $(git diff --cached --name-only | wc -l | tr -d ' ') new LFS objects"
+          git push origin HEAD:main

.github/workflows/sunnypilot-build-prebuilt.yaml

@@ -594,8 +594,14 @@ jobs:
       - name: Publish chestnut branch
         env:
           GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+          HF_TOKEN: ${{ secrets.HF_TOKEN }}
         run: |
           CHESTNUT_BRANCH="${{ needs.prepare_strategy.outputs.new_branch }}-chestnut"
+          cp .lfsconfig "${{ env.OUTPUT_DIR }}/"
+          echo 'openpilot/selfdrive/modeld/models/big_*_tinygrad.pkl filter=lfs diff=lfs merge=lfs -text' > "${{ env.OUTPUT_DIR }}/.gitattributes"
+          git lfs install
+          git config --global credential.helper store
+          echo "https://user:${HF_TOKEN}@huggingface.co" > ~/.git-credentials
 
           ${{ env.CI_DIR }}/publish.sh \
             "${{ github.workspace }}" \

.gitmodules

@@ -4,6 +4,7 @@
 [submodule "opendbc"]
   path = opendbc_repo
   url = https://github.com/sunnypilot/opendbc.git
+  branch = toyotabsmhack
 [submodule "msgq"]
   path = msgq_repo
   url = https://github.com/sunnypilot/msgq.git

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"}},
@@ -246,6 +247,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"TeslaMadsScreenButton", {PERSISTENT | BACKUP, INT, "0"}},
     {"ToyotaEnforceStockLongitudinal", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ToyotaStopAndGoHack", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"ToyotaEnhancedBsm", {PERSISTENT | BACKUP, BOOL, "0"}},
 
     {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
 
@@ -256,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

@@ -4,12 +4,16 @@ from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.common.pid import PIDController
 from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import LongControlSP
 
 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
+# Stopping is shaped as a constant-jerk taper so the brake is released as the car comes to rest.
+# The old behavior ramped towards CP.stopAccel while still rolling, making the last meter the hardest.
+STOPPING_JERK = 2.0  # m/s^3, rate the stopping decel is released at (higher eases off faster)
+STOPPING_HOLD_SPEED = 0.02  # m/s, below this command CP.stopAccel to hold the car
+
 
 def long_control_state_trans(CP_SP, active, long_control_state,
                              should_stop, brake_pressed, cruise_standstill):
@@ -40,8 +44,7 @@ def long_control_state_trans(CP_SP, active, long_control_state,
 
   return long_control_state
 
-
-class LongControl(LongControlSP):
+class LongControl:
   def __init__(self, CP, CP_SP):
     self.CP = CP
     self.CP_SP = CP_SP
@@ -53,6 +56,18 @@ class LongControl(LongControlSP):
   def reset(self):
     self.pid.reset()
 
+  def _stopping_accel(self, v_ego: float) -> float:
+    v_ego = max(float(v_ego), 0.0)
+    if v_ego < STOPPING_HOLD_SPEED:
+      return float(self.CP.stopAccel)
+
+    # Decel that reaches zero at standstill with constant jerk, never stronger than what we already have
+    taper = -np.sqrt(2.0 * STOPPING_JERK * v_ego)
+    goal = min(0.0, max(self.last_output_accel, taper))
+    output_accel = np.clip(goal, self.last_output_accel - STOPPING_JERK * DT_CTRL,
+                           self.last_output_accel + STOPPING_JERK * DT_CTRL)
+    return float(min(output_accel, 0.0))
+
   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]
@@ -66,11 +81,7 @@ class LongControl(LongControlSP):
       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 -= self.stopping_decel_rate(CS.vEgo) * DT_CTRL
+      output_accel = self._stopping_accel(CS.vEgo)
       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,10 +1,10 @@
-from types import SimpleNamespace
-from typing import cast
+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.sunnypilot.selfdrive.controls.lib.longcontrol import LongControlSP
+from openpilot.selfdrive.controls.lib.longcontrol import (LongControl, LongCtrlState, STOPPING_HOLD_SPEED,
+                                                           STOPPING_JERK, long_control_state_trans)
 
 
 class TestLongControlStateTransition(OpenpilotTestCase):
@@ -48,19 +48,73 @@ class TestLongControlStateTransition(OpenpilotTestCase):
     assert next_state == LongCtrlState.pid
 
 
-class TestStoppingHold:
-  def test_weak_brake_does_not_hold_stopping(self):
-    control = SimpleNamespace(last_output_accel=-0.109)
-    car_state = SimpleNamespace(vEgo=0.277, aEgo=-0.406)
-    assert not LongControlSP.should_hold_stopping(cast(LongControlSP, control), car_state, -0.072)
+class TestLongControlStopping(OpenpilotTestCase):
+  def _long_control(self, stop_accel=-2.0, entry_accel=-1.2, v_ego=0.3):
+    CP = car.CarParams.new_message()
+    CP.longitudinalTuning.kiBP = [0.0]
+    CP.longitudinalTuning.kiV = [0.0]
+    CP.stopAccel = stop_accel
+    lc = LongControl(CP, custom.CarParamsSP.new_message())
+    lc.long_control_state = LongCtrlState.stopping
+    lc.last_output_accel = entry_accel
 
-  def test_sufficient_brake_holds_stopping(self):
-    control = SimpleNamespace(last_output_accel=-0.543)
-    car_state = SimpleNamespace(vEgo=0.209, aEgo=-0.534)
-    assert LongControlSP.should_hold_stopping(cast(LongControlSP, control), car_state, -0.469)
+    CS = car.CarState.new_message()
+    CS.vEgo = float(v_ego)
+    CS.brakePressed = False
+    CS.cruiseState.standstill = False
+    return lc, CS
 
-  def test_stopping_decel_rate_is_smooth_while_rolling(self):
-    assert LongControlSP.stopping_decel_rate(0.1) == 0.3
+  def _run_stop(self, lc, CS, frames=500):
+    accels = []
+    distance = 0.0
+    for _ in range(frames):
+      accels.append(lc.update(True, CS, 0.0, True, (-4.0, 2.0)))
+      if CS.vEgo < STOPPING_HOLD_SPEED:
+        break
+      distance += CS.vEgo * DT_CTRL
+      CS.vEgo = float(max(0.0, CS.vEgo + accels[-1] * DT_CTRL))
+    return accels, distance
 
-  def test_stopping_decel_rate_builds_holding_brake_at_stop(self):
-    assert LongControlSP.stopping_decel_rate(0.0) == 2.0
+  def test_releases_braking_instead_of_ramping_to_stop_accel(self):
+    lc, CS = self._long_control(entry_accel=-1.2)
+    accels, _ = self._run_stop(lc, CS)
+    # the stop state must ease off the brake, not add more of it as the car slows
+    assert accels[0] > -1.2
+    assert accels[-2] > accels[0]
+    # and it must actually get gentle before standstill instead of stopping at the entry decel
+    assert accels[-2] > -0.8
+
+  def test_never_increases_decel_while_rolling(self):
+    lc, CS = self._long_control(entry_accel=-1.2)
+    accels, _ = self._run_stop(lc, CS)
+    rolling = accels[:-1]
+    assert all(b >= a - 1e-9 for a, b in zip(rolling[:-1], rolling[1:], strict=False)), rolling
+
+  def test_jerk_is_bounded(self):
+    lc, CS = self._long_control(entry_accel=-1.5)
+    accels, _ = self._run_stop(lc, CS)
+    rolling = accels[:-1]
+    max_step = max(abs(b - a) for a, b in zip(rolling[:-1], rolling[1:], strict=False))
+    assert max_step <= STOPPING_JERK * DT_CTRL + 1e-9
+
+  def test_holds_with_stop_accel_at_standstill(self):
+    lc, CS = self._long_control(stop_accel=-2.0)
+    accels, _ = self._run_stop(lc, CS)
+    assert CS.vEgo < STOPPING_HOLD_SPEED
+    assert accels[-1] == -2.0
+
+  def test_weak_entry_does_not_add_braking(self):
+    lc, CS = self._long_control(entry_accel=-0.2)
+    accels, _ = self._run_stop(lc, CS)
+    assert min(accels[:-1]) >= -0.2 - 1e-9
+
+  def test_never_commands_positive_accel(self):
+    lc, CS = self._long_control(entry_accel=0.5)
+    accels, _ = self._run_stop(lc, CS)
+    assert max(accels) <= 0.0
+
+  def test_stops_within_short_distance(self):
+    lc, CS = self._long_control(entry_accel=-1.2)
+    _, distance = self._run_stop(lc, CS)
+    assert CS.vEgo < STOPPING_HOLD_SPEED
+    assert distance < 0.25

openpilot/selfdrive/modeld/SConscript

@@ -48,8 +48,9 @@ def compile_model(onnx_path, pkl_path):
     Action(cmd, "  [ONNX] $TARGET"),
   )
 
-compile_model('models/dmonitoring_model.onnx', 'models/dmonitoring_model_tinygrad.pkl')
-compile_model('models/driving_supercombo.onnx', 'models/driving_tinygrad.pkl')
+if not os.getenv('SKIP_TINYGRAD_COMPILE'):
+  compile_model('models/dmonitoring_model.onnx', 'models/dmonitoring_model_tinygrad.pkl')
+  compile_model('models/driving_supercombo.onnx', 'models/driving_tinygrad.pkl')
 
 model_w, model_h = MEDMODEL_INPUT_SIZE
 for cam_w, cam_h in camera_configs:
@@ -62,7 +63,7 @@ for cam_w, cam_h in camera_configs:
   lenv.Command(warp_pkl_path, tinygrad_files + warp_deps + [Value(cmd)], cmd)
 
 dm_w, dm_h = DM_INPUT_SIZE
-for cam_w, cam_h in camera_configs:
+for cam_w, cam_h in [] if os.getenv('SKIP_TINYGRAD_COMPILE') else camera_configs:
   dm_pkl_path = File(f"models/dm_warp_{cam_w}x{cam_h}_tinygrad.pkl").abspath
   stride, y_height, uv_height, frame_size = get_nv12_info(cam_w, cam_h)
   cmd = (f'{tg_flags} {mac_brew_string} python3 "{compiler}/compile_warp.py" '

openpilot/selfdrive/modeld/modeld.py

@@ -7,6 +7,7 @@ from functools import cached_property
 import os
 os.environ['GMMU'] = '0' # for chestnut fast loading, noop for qcom
 os.environ.setdefault('AM_POWER_LIMIT', '100')
+os.environ.setdefault('AM_PERF_LEVEL', 'high')
 from tinygrad.device import Buffer, Device
 from tinygrad.dtype import DType, dtypes
 from tinygrad.engine.realize import lower_and_compile

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

@@ -8,7 +8,6 @@ from enum import IntEnum
 
 from openpilot.selfdrive.ui.sunnypilot.layouts.settings.cruise_sub_layouts.speed_limit_settings import SpeedLimitSettingsLayout
 from openpilot.selfdrive.ui.ui_state import ui_state
-from openpilot.sunnypilot.mapd import MAPD_AVAILABLE
 from openpilot.system.ui.lib.multilang import tr, tr_noop
 from openpilot.system.ui.sunnypilot.widgets.list_view import toggle_item_sp, option_item_sp, simple_button_item_sp
 from openpilot.system.ui.widgets import Widget
@@ -30,7 +29,11 @@ ACC_ENABLED_DESCRIPTION = tr_noop("Enable custom Short & Long press increments f
 ACC_NOLONG_DESCRIPTION = tr_noop("This feature can only be used with sunnypilot longitudinal control enabled.")
 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.")
-MAP_UNAVAILABLE_DESCRIPTION = tr_noop("Map data is temporarily unavailable.")
+
+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):
@@ -56,8 +59,7 @@ class CruiseLayout(Widget):
 
     self.scc_m_toggle = toggle_item_sp(
       title=tr("Smart Cruise Control - Map"),
-      description=(f"<b>{tr(MAP_UNAVAILABLE_DESCRIPTION)}</b>\n\n" if not MAPD_AVAILABLE else "") +
-                  tr("Use map data to estimate the appropriate speed to drive through turns ahead."),
+      description=tr("Use map data to estimate the appropriate speed to drive through turns ahead."),
       param="SmartCruiseControlMap")
 
     self.custom_acc_toggle = toggle_item_sp(
@@ -79,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,
@@ -92,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
@@ -107,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
@@ -142,7 +151,7 @@ class CruiseLayout(Widget):
       if has_long or has_icbm:
         self.custom_acc_toggle.action_item.set_enabled(((has_long and not ui_state.CP.pcmCruise) or has_icbm) and ui_state.is_offroad())
         self.scc_v_toggle.action_item.set_enabled(True)
-        self.scc_m_toggle.action_item.set_enabled(MAPD_AVAILABLE)
+        self.scc_m_toggle.action_item.set_enabled(True)
       else:
         ui_state.params.remove("CustomAccIncrementsEnabled")
         ui_state.params.remove("SmartCruiseControlVision")
@@ -151,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/layouts/settings/cruise_sub_layouts/speed_limit_policy.py

@@ -8,8 +8,6 @@ from collections.abc import Callable
 
 import pyray as rl
 from openpilot.selfdrive.ui.ui_state import ui_state
-from openpilot.sunnypilot.mapd import MAPD_AVAILABLE
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy
 from openpilot.system.ui.lib.multilang import tr
 from openpilot.system.ui.sunnypilot.widgets.list_view import multiple_button_item_sp
 from openpilot.system.ui.widgets.network import NavButton
@@ -45,8 +43,6 @@ class SpeedLimitPolicyLayout(Widget):
       param="SpeedLimitPolicy",
       button_width=250,
     )
-    if not MAPD_AVAILABLE:
-      self._speed_limit_policy.action_item.set_enabled_buttons({Policy.car_state_only})
 
     items = [
       self._speed_limit_policy
@@ -55,8 +51,6 @@ class SpeedLimitPolicyLayout(Widget):
 
   @staticmethod
   def _get_policy_description():
-    if not MAPD_AVAILABLE:
-      return f"<b>{tr('Map data is temporarily unavailable. Using Speed Limit data only from Car.')}</b>"
     return get_highlighted_description(ui_state.params, "SpeedLimitPolicy", SPEED_LIMIT_POLICY_DESCRIPTIONS)
 
   def _render(self, rect):

openpilot/selfdrive/ui/sunnypilot/layouts/settings/vehicle/brands/toyota.py

@@ -4,6 +4,8 @@ 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 opendbc.car.toyota.values import ToyotaFlags
+from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
 from openpilot.selfdrive.ui.sunnypilot.layouts.settings.vehicle.brands.base import BrandSettings
 from openpilot.selfdrive.ui.ui_state import ui_state
 from openpilot.system.ui.lib.application import gui_app
@@ -15,6 +17,10 @@ from openpilot.system.ui.sunnypilot.widgets.list_view import toggle_item_sp
 
 ONROAD_ONLY_DESCRIPTION = tr_noop("Start the vehicle to check vehicle compatibility.")
 SNG_HACK_UNAVAILABLE = tr_noop("sunnypilot Longitudinal Control must be available and enabled for your vehicle to use this feature.")
+BSM_UNAVAILABLE = tr_noop("Enhanced Blind Spot Monitoring requires a TSS2 Toyota/Lexus without SecOC.")
+BSM_STOCK_LONG_UNAVAILABLE = tr_noop("Enhanced Blind Spot Monitoring is not available while Enforce Factory Longitudinal Control is enabled.")
+BSM_REQUIRED = tr_noop("This vehicle does not report blind spot status over the standard CAN signals. " +
+                        "Enhanced Blind Spot Monitoring is required for blind spot detection.")
 
 DESCRIPTIONS = {
   'enforce_stock_longitudinal': tr_noop(
@@ -23,6 +29,10 @@ DESCRIPTIONS = {
   'stop_and_go_hack': tr_noop(
     'sunnypilot will allow some Toyota/Lexus cars to auto resume during stop and go traffic. ' +
     'This feature is only applicable to certain models that are able to use longitudinal control. This is an alpha feature. Use at your own risk.'
+  ),
+  'enhanced_bsm': tr_noop(
+    'sunnypilot will poll the blind spot monitor diagnostics directly from the radar. Use this on vehicles whose blind spot monitors ' +
+    'do not report blind spot status over the standard CAN signals. This is an alpha feature. Use at your own risk.'
   )
 }
 
@@ -47,9 +57,18 @@ class ToyotaSettings(BrandSettings):
       enabled=lambda: not ui_state.engaged,
     )
 
+    self.enhanced_bsm = toggle_item_sp(
+      lambda: tr("Enhanced Blind Spot Monitoring (Alpha)"),
+      description=lambda: tr(DESCRIPTIONS["enhanced_bsm"]),
+      initial_state=ui_state.params.get_bool("ToyotaEnhancedBsm"),
+      callback=self._on_enable_enhanced_bsm,
+      enabled=lambda: not ui_state.engaged,
+    )
+
     self.items = [
       self.enforce_stock_longitudinal,
       self.stop_and_go_hack,
+      self.enhanced_bsm,
     ]
 
   def _on_enable_enforce_stock_longitudinal(self, state: bool):
@@ -94,6 +113,25 @@ class ToyotaSettings(BrandSettings):
       ui_state.params.put_bool("ToyotaStopAndGoHack", False)
       ui_state.params.put_bool("OnroadCycleRequested", True)
 
+  def _on_enable_enhanced_bsm(self, state: bool):
+    if state:
+      def confirm_callback(result: int):
+        if result == DialogResult.CONFIRM:
+          ui_state.params.put_bool("ToyotaEnhancedBsm", True)
+          ui_state.params.put_bool("OnroadCycleRequested", True)
+        else:
+          self.enhanced_bsm.action_item.set_state(False)
+
+      content = (f"<h1>{self.enhanced_bsm.title}</h1><br>" +
+                 f"<p>{self.enhanced_bsm.description}</p>")
+
+      dlg = ConfirmDialog(content, tr("Enable"), rich=True, callback=confirm_callback)
+      gui_app.push_widget(dlg)
+
+    else:
+      ui_state.params.put_bool("ToyotaEnhancedBsm", False)
+      ui_state.params.put_bool("OnroadCycleRequested", True)
+
   def update_settings(self):
     if ui_state.CP is not None:
       longitudinal = ui_state.CP.openpilotLongitudinalControl
@@ -113,9 +151,36 @@ class ToyotaSettings(BrandSettings):
         self.stop_and_go_hack.set_description(new_desc)
         if show_desc:
           self.stop_and_go_hack.show_description(True)
+
+      # Enhanced BSM needs the radar diagnostic address, which is only in the TX allowlist for
+      # TSS2 cars without SecOC that are not enforcing factory longitudinal control
+      bsm_supported = bool(ui_state.CP.flags & ToyotaFlags.TSS2) and not bool(ui_state.CP.flags & ToyotaFlags.SECOC)
+      bsm_available = bsm_supported and not enforce_stock
+      self.enhanced_bsm.action_item.set_enabled(bsm_available and not ui_state.engaged)
+
+      bsm_desc = tr(DESCRIPTIONS["enhanced_bsm"])
+      if not bsm_supported:
+        self.enhanced_bsm.action_item.set_state(False)
+        bsm_desc = "<b>" + tr(BSM_UNAVAILABLE) + "</b>\n\n" + bsm_desc
+      elif enforce_stock:
+        self.enhanced_bsm.action_item.set_state(False)
+        bsm_desc = "<b>" + tr(BSM_STOCK_LONG_UNAVAILABLE) + "</b>\n\n" + bsm_desc
+      elif ui_state.CP_SP is not None and ui_state.CP_SP.flags & ToyotaFlagsSP.SP_NEED_DEBUG_BSM:
+        bsm_desc = "<b>" + tr(BSM_REQUIRED) + "</b>\n\n" + bsm_desc
+
+      if self.enhanced_bsm.description != bsm_desc:
+        self.enhanced_bsm.set_description(bsm_desc)
+        if not bsm_available:
+          self.enhanced_bsm.show_description(True)
     else:
       self.stop_and_go_hack.action_item.set_enabled(False)
       new_desc = "<b>" + tr(ONROAD_ONLY_DESCRIPTION) + "</b>\n\n" + tr(DESCRIPTIONS["stop_and_go_hack"])
       if self.stop_and_go_hack.description != new_desc:
         self.stop_and_go_hack.set_description(new_desc)
         self.stop_and_go_hack.show_description(True)
+
+      self.enhanced_bsm.action_item.set_enabled(False)
+      bsm_desc = "<b>" + tr(ONROAD_ONLY_DESCRIPTION) + "</b>\n\n" + tr(DESCRIPTIONS["enhanced_bsm"])
+      if self.enhanced_bsm.description != bsm_desc:
+        self.enhanced_bsm.set_description(bsm_desc)
+        self.enhanced_bsm.show_description(True)

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/mapd/__init__.py

@@ -3,7 +3,3 @@ from openpilot.common.basedir import BASEDIR
 
 MAPD_BIN_DIR = os.path.join(BASEDIR, 'openpilot', 'third_party/mapd_pfeiferj')
 MAPD_PATH = os.path.join(MAPD_BIN_DIR, 'mapd')
-
-# mapd still runs (OSM downloads) but has no position source since ubloxd owns LastGPSPosition;
-# map-based features stay off until mapd is rebuilt
-MAPD_AVAILABLE = False

openpilot/sunnypilot/mapd/live_map_data/osm_map_data.py

@@ -4,6 +4,7 @@ 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 json
 import math
 import platform
 
@@ -26,6 +27,21 @@ class OsmMapData(BaseMapData):
       self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
       self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
 
+    if self.last_position is None:
+      return
+
+    params = {
+      "latitude": self.last_position.latitude,
+      "longitude": self.last_position.longitude,
+    }
+
+    if self.last_bearing is not None:
+      params['bearing'] = self.last_bearing
+
+    # mapd reads its position from the memory params as JSON, so write it for the mapd binary.
+    # LastGPSPosition is registered as BYTES (ubloxd owns the persistent copy for pigeond),
+    # so the JSON payload has to be encoded here.
+    self.mem_params.put("LastGPSPosition", json.dumps(params).encode(), block=True)
 
   def get_current_speed_limit(self) -> float:
     return float(self.mem_params.get("MapSpeedLimit") or 0.0)

openpilot/sunnypilot/mapd/mapd_manager.py

@@ -121,6 +121,10 @@ def update_osm_db() -> None:
   if not mem_params.get("OSMDownloadBounds"):
     mem_params.put("OSMDownloadBounds", "", block=True)
 
+  # mapd reads LastGPSPosition from the memory params as JSON, and it is registered as BYTES upstream
+  if not mem_params.get("LastGPSPosition"):
+    mem_params.put("LastGPSPosition", b"{}", block=True)
+
 
 def main_thread():
   update_installed_version(VERSION, params)

openpilot/sunnypilot/mapd/tests/test_osm_map_data.py

@@ -0,0 +1,70 @@
+"""
+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 json
+import math
+import platform
+
+import openpilot.cereal.messaging as messaging
+from openpilot.cereal import log
+from openpilot.common.params import Params
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.sunnypilot.mapd.live_map_data.osm_map_data import OsmMapData
+
+
+class TestOsmMapData(OpenpilotTestCase):
+  """The mapd binary reads its position from the memory params as JSON, so mapd_manager has to feed it."""
+
+  LATITUDE = 32.875134
+  LONGITUDE = -117.209777
+  ALTITUDE = 42.0
+  BEARING = 90.0
+
+  def setup_method(self):
+    self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else Params()
+    self.mem_params.remove("LastGPSPosition")
+    self.map_data = OsmMapData()
+    self.pm = messaging.PubMaster(['liveLocationKalman'])
+
+  def location_message(self, status: log.LiveLocationKalman.Status, position_valid: bool):
+    msg = messaging.new_message('liveLocationKalman', valid=True)
+    location = msg.liveLocationKalman
+    location.status = status
+    location.positionGeodetic = {'value': [self.LATITUDE, self.LONGITUDE, self.ALTITUDE], 'valid': position_valid}
+    location.calibratedOrientationNED = {'value': [0.0, 0.0, math.radians(self.BEARING)], 'valid': True}
+    return msg
+
+  def send_location(self, status: log.LiveLocationKalman.Status, position_valid: bool) -> None:
+    # the first exchange synchronizes the PUB/SUB connection, the second one is actually received
+    self.pm.send('liveLocationKalman', self.location_message(status, position_valid))
+    self.map_data.sm.update(1000)
+    self.pm.send('liveLocationKalman', self.location_message(status, position_valid))
+    self.map_data.sm.update(1000)
+
+  def read_position(self) -> dict:
+    value = self.mem_params.get("LastGPSPosition")
+    self.assertIsNotNone(value, "LastGPSPosition was not written for mapd")
+    return json.loads(value)
+
+  def test_writes_position_for_mapd(self):
+    self.send_location(log.LiveLocationKalman.Status.valid, True)
+    self.map_data.tick()
+
+    position = self.read_position()
+    self.assertAlmostEqual(position['latitude'], self.LATITUDE, delta=1e-6)
+    self.assertAlmostEqual(position['longitude'], self.LONGITUDE, delta=1e-6)
+    self.assertAlmostEqual(position['bearing'], self.BEARING, delta=1e-6)
+
+  def test_invalid_localizer_keeps_last_known_position(self):
+    self.send_location(log.LiveLocationKalman.Status.valid, True)
+    self.map_data.tick()
+
+    self.send_location(log.LiveLocationKalman.Status.uninitialized, False)
+    self.map_data.tick()
+
+    position = self.read_position()
+    self.assertAlmostEqual(position['latitude'], self.LATITUDE, delta=1e-6)
+    self.assertAlmostEqual(position['longitude'], self.LONGITUDE, delta=1e-6)

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/car/interfaces.py

@@ -126,6 +126,7 @@ def initialize_params(params) -> list[dict[str, Any]]:
   keys.extend([
     "ToyotaEnforceStockLongitudinal",
     "ToyotaStopAndGoHack",
+    "ToyotaEnhancedBsm",
   ])
 
   return [{k: params.get(k, return_default=True)} for k in keys]

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/longcontrol.py

@@ -1,25 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-
-import numpy as np
-
-STOPPING_DISTANCE = 1.5
-STOPPED_SPEED = 0.02
-STOPPING_TIME = 2.5
-MIN_STOPPING_HOLD_ACCEL = -0.2
-STOPPING_DECEL_RATE = 0.3
-STOPPED_DECEL_RATE = 0.05
-
-
-class LongControlSP:
-  def should_hold_stopping(self, CS, a_target: float) -> bool:
-    return (self.last_output_accel <= MIN_STOPPING_HOLD_ACCEL and a_target >= self.last_output_accel and CS.vEgo > STOPPED_SPEED and CS.aEgo < 0.0
-            and CS.vEgo <= -CS.aEgo * STOPPING_TIME and CS.vEgo ** 2 <= -2.0 * CS.aEgo * STOPPING_DISTANCE)
-
-  @staticmethod
-  def stopping_decel_rate(v_ego: float) -> float:
-    return float(np.interp(v_ego, [0.0, 0.5, 1.5], [STOPPED_DECEL_RATE, 0.15, STOPPING_DECEL_RATE]))

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/smart_cruise_control/map_controller.py

@@ -9,7 +9,6 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
 from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
-from openpilot.sunnypilot.mapd import MAPD_AVAILABLE
 from openpilot.sunnypilot.navd.helpers import Coordinate
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
 
@@ -76,7 +75,7 @@ class SmartCruiseControlMap:
   def __init__(self):
     self.params = Params()
     self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
-    self.enabled = MAPD_AVAILABLE and self.params.get_bool("SmartCruiseControlMap")
+    self.enabled = self.params.get_bool("SmartCruiseControlMap")
     self.long_enabled = False
     self.long_override = False
     self.is_enabled = False
@@ -102,7 +101,7 @@ class SmartCruiseControlMap:
 
   def update_params(self):
     if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
-      self.enabled = MAPD_AVAILABLE and self.params.get_bool("SmartCruiseControlMap")
+      self.enabled = self.params.get_bool("SmartCruiseControlMap")
 
   def update_calculations(self, sm: messaging.SubMaster) -> None:
     gps = sm[self.gps_location_service]

openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_map_controller.py

@@ -8,7 +8,7 @@ import json
 import math
 import platform
 from types import SimpleNamespace
-from unittest.mock import MagicMock, patch
+from unittest.mock import MagicMock
 
 from openpilot.cereal import custom
 from openpilot.common.params import Params
@@ -23,8 +23,6 @@ MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
 class TestSmartCruiseControlMap(OpenpilotTestCase):
 
   def setup_method(self):
-    self.mapd_patch = patch("openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller.MAPD_AVAILABLE", True)
-    self.mapd_patch.start()
     self.params = Params()
     self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
     self.reset_params()
@@ -32,9 +30,6 @@ class TestSmartCruiseControlMap(OpenpilotTestCase):
     self.sm = MagicMock()
     self.sm.__getitem__.return_value = SimpleNamespace(latitude=0.0, longitude=0.0)
 
-  def teardown_method(self):
-    self.mapd_patch.stop()
-
   def reset_params(self):
     self.params.put_bool("SmartCruiseControlMap", True, block=True)
 
@@ -55,14 +50,6 @@ class TestSmartCruiseControlMap(OpenpilotTestCase):
     assert self.scc_m.state == VisionState.disabled
     assert not self.scc_m.is_active
 
-  def test_mapd_unavailable(self):
-    self.mapd_patch.stop()
-    self.scc_m = SmartCruiseControlMap()
-    for _ in range(int(10. / DT_MDL)):
-      self.scc_m.update(self.sm, True, False, 0., 0., 0.)
-    assert self.scc_m.state == VisionState.disabled
-    self.mapd_patch.start()
-
   def test_disabled(self):
     for _ in range(int(10. / DT_MDL)):
       self.scc_m.update(self.sm, False, False, 0., 0., 0.)

openpilot/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py

@@ -13,7 +13,6 @@ from openpilot.common.gps import get_gps_location_service
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
 from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD, get_sanitize_int_param
-from openpilot.sunnypilot.mapd import MAPD_AVAILABLE
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy, OffsetType
 
@@ -49,8 +48,6 @@ class SpeedLimitResolver:
       Policy.max().value,
       self.params
     )
-    if not MAPD_AVAILABLE:
-      self.policy = Policy.car_state_only
     self._policy_to_sources_map = {
       Policy.car_state_only: [SpeedLimitSource.car],
       Policy.map_data_only: [SpeedLimitSource.map],
@@ -94,7 +91,7 @@ class SpeedLimitResolver:
 
   def update_params(self):
     if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
-      self.policy = self.params.get("SpeedLimitPolicy", return_default=True) if MAPD_AVAILABLE else Policy.car_state_only
+      self.policy = self.params.get("SpeedLimitPolicy", return_default=True)
       self.is_metric = self.params.get_bool("IsMetric")
       self.offset_type = self.params.get("SpeedLimitOffsetType", return_default=True)
       self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)

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/capabilities.py

@@ -10,12 +10,12 @@ from openpilot.cereal import custom, messaging
 from opendbc.car.structs import car
 from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR, UNSUPPORTED_LONGITUDINAL_CAR
 from opendbc.car.subaru.values import CAR as SUBARU_CAR, SubaruFlags
+from opendbc.car.toyota.values import CAR as TOYOTA_CAR, ToyotaFlags
 from opendbc.sunnypilot.car.interfaces import has_bsm
 from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.hardware import HARDWARE
-from openpilot.sunnypilot.mapd import MAPD_AVAILABLE
 
 
 # Wire-protocol version for the capabilities payload. Bump on breaking changes
@@ -44,7 +44,7 @@ CAPABILITY_FIELDS = (
   "device_type",
   "subaru_has_sng",
   "hyundai_alpha_long_available",
-  "mapd_available",
+  "toyota_enhanced_bsm_available",
 )
 
 CAPABILITY_LABELS: dict[str, str] = {
@@ -67,7 +67,7 @@ CAPABILITY_LABELS: dict[str, str] = {
   "device_type": "Device type",
   "subaru_has_sng": "Subaru Stop-and-Go available",
   "hyundai_alpha_long_available": "Hyundai Alpha Longitudinal available",
-  "mapd_available": "Map data available",
+  "toyota_enhanced_bsm_available": "Toyota Enhanced BSM available",
 }
 
 # Explicit defaults for non-boolean capability fields
@@ -114,6 +114,16 @@ def _resolve_brand_capabilities(caps: dict, bundle_platform: str, CP) -> None:
       caps["subaru_has_sng"] = not bool(CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID))
       caps["has_stop_and_go"] = caps["subaru_has_sng"]
 
+  elif brand == "toyota":
+    if bundle_platform:
+      try:
+        flags = TOYOTA_CAR[bundle_platform].config.flags
+        caps["toyota_enhanced_bsm_available"] = bool(flags & ToyotaFlags.TSS2) and not bool(flags & ToyotaFlags.SECOC)
+      except KeyError:
+        cloudlog.exception(f"capabilities: unknown toyota platform {bundle_platform!r}")
+    elif CP is not None:
+      caps["toyota_enhanced_bsm_available"] = bool(CP.flags & ToyotaFlags.TSS2) and not bool(CP.flags & ToyotaFlags.SECOC)
+
 
 def generate_capabilities(params: Params | None = None) -> dict:
   """Generate a SettingsCapabilities dict from CarParams + boolean params.
@@ -135,7 +145,6 @@ def generate_capabilities(params: Params | None = None) -> dict:
   caps["is_sp_release"] = params.get_bool("IsReleaseSpBranch")
   caps["is_development"] = params.get_bool("IsDevelopmentBranch")
   caps["stock_longitudinal"] = params.get_bool("ToyotaEnforceStockLongitudinal")
-  caps["mapd_available"] = MAPD_AVAILABLE
 
   bundle = params.get("CarPlatformBundle")
   bundle_brand = _bundle_field(bundle, "brand")

openpilot/sunnypilot/sunnylink/docs/README.md

@@ -156,7 +156,7 @@ The tables below describe the **compiled** `settings_ui.json` schema — what th
 
 **Enablement rules**: Grayed out (disabled) when rules fail. Frontend shows a contextual badge explaining why.
 
-**Capability fields** (referenced in rules): `has_longitudinal_control`, `has_icbm`, `icbm_available`, `torque_allowed`, `brand`, `pcm_cruise`, `alpha_long_available`, `steer_control_type`, `has_bsm`, `is_release`, `is_sp_release`, `is_development`, `tesla_has_vehicle_bus`, `has_stop_and_go`, `stock_longitudinal`, `mapd_available`
+**Capability fields** (referenced in rules): `has_longitudinal_control`, `has_icbm`, `icbm_available`, `torque_allowed`, `brand`, `pcm_cruise`, `alpha_long_available`, `steer_control_type`, `has_bsm`, `is_release`, `is_sp_release`, `is_development`, `tesla_has_vehicle_bus`, `has_stop_and_go`, `stock_longitudinal`, `toyota_enhanced_bsm_available`
 
 ---
 

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",
@@ -857,47 +873,19 @@
                     },
                     {
                       "value": 1,
-                      "label": "Map Data Only",
-                      "enablement": [
-                        {
-                          "type": "capability",
-                          "field": "mapd_available",
-                          "equals": true
-                        }
-                      ]
+                      "label": "Map Data Only"
                     },
                     {
                       "value": 2,
-                      "label": "Car State Priority",
-                      "enablement": [
-                        {
-                          "type": "capability",
-                          "field": "mapd_available",
-                          "equals": true
-                        }
-                      ]
+                      "label": "Car State Priority"
                     },
                     {
                       "value": 3,
-                      "label": "Map Data Priority",
-                      "enablement": [
-                        {
-                          "type": "capability",
-                          "field": "mapd_available",
-                          "equals": true
-                        }
-                      ]
+                      "label": "Map Data Priority"
                     },
                     {
                       "value": 4,
-                      "label": "Combined",
-                      "enablement": [
-                        {
-                          "type": "capability",
-                          "field": "mapd_available",
-                          "equals": true
-                        }
-                      ]
+                      "label": "Combined"
                     }
                   ]
                 },
@@ -1029,11 +1017,6 @@
                 }
               ],
               "enablement": [
-                {
-                  "type": "capability",
-                  "field": "mapd_available",
-                  "equals": true
-                },
                 {
                   "type": "any",
                   "conditions": [
@@ -2357,6 +2340,28 @@
               "equals": false
             }
           ]
+        },
+        {
+          "key": "ToyotaEnhancedBsm",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Enhanced Blind Spot Monitoring (Alpha)",
+          "description": "sunnypilot will poll the blind spot monitor diagnostics directly from the radar. Use this on vehicles whose blind spot monitors do not report blind spot status over the standard CAN signals. This is an alpha feature. Use at your own risk.",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            },
+            {
+              "type": "capability",
+              "field": "toyota_enhanced_bsm_available",
+              "equals": true
+            },
+            {
+              "type": "capability",
+              "field": "stock_longitudinal",
+              "equals": false
+            }
+          ]
         }
       ]
     }

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)
@@ -167,28 +176,12 @@ sections:
         label: Car State Only
       - value: 1
         label: Map Data Only
-        enablement:
-        - type: capability
-          field: mapd_available
-          equals: true
       - value: 2
         label: Car State Priority
-        enablement:
-        - type: capability
-          field: mapd_available
-          equals: true
       - value: 3
         label: Map Data Priority
-        enablement:
-        - type: capability
-          field: mapd_available
-          equals: true
       - value: 4
         label: Combined
-        enablement:
-        - type: capability
-          field: mapd_available
-          equals: true
     - key: SpeedLimitOffsetType
       widget: multiple_button
       title: Speed Limit Offset Type
@@ -262,9 +255,6 @@ sections:
         field: has_icbm
         equals: true
     enablement:
-    - type: capability
-      field: mapd_available
-      equals: true
     - type: any
       conditions:
       - type: capability

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

@@ -101,3 +101,18 @@ sections:
     - type: param
       key: ToyotaEnforceStockLongitudinal
       equals: false
+  - key: ToyotaEnhancedBsm
+    widget: toggle
+    needs_onroad_cycle: true
+    title: Enhanced Blind Spot Monitoring (Alpha)
+    description: sunnypilot will poll the blind spot monitor diagnostics directly from the radar. Use this on vehicles whose
+      blind spot monitors do not report blind spot status over the standard CAN signals. This is an alpha feature. Use at
+      your own risk.
+    enablement:
+    - $ref: '#/macros/not_engaged'
+    - type: capability
+      field: toyota_enhanced_bsm_available
+      equals: true
+    - type: capability
+      field: stock_longitudinal
+      equals: false

openpilot/sunnypilot/sunnylink/tests/test_capabilities.py

@@ -69,12 +69,18 @@ class TestOpaquePerBrandFlags(OpenpilotTestCase):
   def test_hyundai_alpha_long_available_field_present(self):
     assert "hyundai_alpha_long_available" in CAPABILITY_FIELDS
 
+  def test_toyota_enhanced_bsm_available_field_present(self):
+    assert "toyota_enhanced_bsm_available" in CAPABILITY_FIELDS
+
   def test_subaru_has_sng_default_false(self, caps):
     assert caps["subaru_has_sng"] is False
 
   def test_hyundai_alpha_long_available_default_false(self, caps):
     assert caps["hyundai_alpha_long_available"] is False
 
+  def test_toyota_enhanced_bsm_available_default_false(self, caps):
+    assert caps["toyota_enhanced_bsm_available"] is False
+
 
 class TestCapabilitiesShape(OpenpilotTestCase):
   def test_all_fields_present(self, caps):

openpilot/sunnypilot/sunnylink/tests/test_settings_changes.py

@@ -207,6 +207,7 @@ class TestNotEngagedReplacement(OpenpilotTestCase):
     "AlphaLongitudinalEnabled",
     "ToyotaEnforceStockLongitudinal",
     "ToyotaStopAndGoHack",
+    "ToyotaEnhancedBsm",
   ], names=["key"])
   def test_offroad_only_replaced_with_not_engaged(self, schema, key):
     """These items should use not_engaged, not offroad_only."""

openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py

@@ -286,6 +286,7 @@ class TestKnownVehicleSettings(OpenpilotTestCase):
     keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("toyota"))}
     assert "ToyotaEnforceStockLongitudinal" in keys
     assert "ToyotaStopAndGoHack" in keys
+    assert "ToyotaEnhancedBsm" in keys
 
   def test_tesla_has_coop_steering(self, schema):
     keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("tesla"))}