sunnypilot v2026.002.000

version: sunnypilot v2026.002.000 (staging)
date: 2026-07-21T07:44:02
master commit: 2c334ede443d7391d27575af8c854a095ba702a8

openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py

@@ -0,0 +1,119 @@
+"""
+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 time
+
+import openpilot.cereal.messaging as messaging
+from openpilot.cereal import log, custom
+
+from opendbc.car import structs
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
+from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
+from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
+from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_v0 import LatControlTorque as LatControlTorqueV0
+
+
+class ControlsExt(ModelStateBase):
+  def __init__(self, CP: structs.CarParams, params: Params):
+    ModelStateBase.__init__(self)
+    self.CP = CP
+    self.params = params
+    self._param_update_time: float = 0.0
+    self.blinker_pause_lateral = BlinkerPauseLateral()
+
+    cloudlog.info("controlsd_ext is waiting for CarParamsSP")
+    self.CP_SP = messaging.log_from_bytes(params.get("CarParamsSP", block=True), custom.CarParamsSP)
+    cloudlog.info("controlsd_ext got CarParamsSP")
+
+    self.sm_services_ext = ['radarState', 'selfdriveStateSP']
+    self.pm_services_ext = ['carControlSP']
+
+  def initialize_lateral_control(self, lac, CI, dt):
+    enforce_torque_control = self.params.get_bool("EnforceTorqueControl")
+    torque_versions = self.params.get("TorqueControlTune")
+    if not enforce_torque_control:
+      if self.CP.lateralTuning.which() == 'torque':
+        return LatControlTorqueV0(self.CP, self.CP_SP, CI, dt)  # FIXME-SP: revert when upstream fixes tuning issues with v1
+      return lac
+
+    if torque_versions == 0.0:  # v0
+      return LatControlTorqueV0(self.CP, self.CP_SP, CI, dt)
+    else:
+      return lac
+
+  def get_params_sp(self, sm: messaging.SubMaster) -> None:
+    if time.monotonic() - self._param_update_time > PARAMS_UPDATE_PERIOD:
+      self.blinker_pause_lateral.get_params()
+
+      if self.CP.lateralTuning.which() == 'torque':
+        self.lat_delay = get_lat_delay(self.params, sm["liveDelay"].lateralDelay)
+
+      self._param_update_time = time.monotonic()
+
+  def get_lat_active(self, sm: messaging.SubMaster) -> bool:
+    if self.blinker_pause_lateral.update(sm['carState']):
+      return False
+
+    ss_sp = sm['selfdriveStateSP']
+    if ss_sp.mads.available:
+      return bool(ss_sp.mads.active)
+
+    # MADS not available, use stock state to engage
+    return bool(sm['selfdriveState'].active)
+
+  @staticmethod
+  def get_lead_data(_lead, src: log.RadarState.LeadData) -> None:
+    _lead.dRel = src.dRel
+    _lead.yRel = src.yRel
+    _lead.vRel = src.vRel
+    _lead.aRel = src.deprecated.aRel
+    _lead.vLead = src.vLead
+    _lead.dPath = src.deprecated.dPath
+    _lead.vLat = src.deprecated.vLat
+    _lead.vLeadK = src.vLeadK
+    _lead.aLeadK = src.aLeadK
+    _lead.fcw = src.deprecated.fcw
+    _lead.status = src.present
+    _lead.aLeadTau = src.aLeadTau
+    _lead.modelProb = src.modelProb
+    _lead.radar = src.radar
+    _lead.radarTrackId = src.radarTrackId
+
+  def state_control_ext(self, sm: messaging.SubMaster) -> custom.CarControlSP:
+    CC_SP = custom.CarControlSP.new_message()
+
+    self.get_lead_data(CC_SP.leadOne, sm['radarState'].leadOne)
+    self.get_lead_data(CC_SP.leadTwo, sm['radarState'].leadTwo)
+
+    # MADS state
+    mads_src = sm['selfdriveStateSP'].mads
+    CC_SP.mads.state = mads_src.state
+    CC_SP.mads.enabled = mads_src.enabled
+    CC_SP.mads.active = mads_src.active
+    CC_SP.mads.available = mads_src.available
+
+    # ICBM state
+    icbm_src = sm['selfdriveStateSP'].intelligentCruiseButtonManagement
+    CC_SP.intelligentCruiseButtonManagement.state = icbm_src.state
+    CC_SP.intelligentCruiseButtonManagement.sendButton = icbm_src.sendButton
+    CC_SP.intelligentCruiseButtonManagement.vTarget = icbm_src.vTarget
+
+    return CC_SP
+
+  @staticmethod
+  def publish_ext(CC_SP: custom.CarControlSP, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
+    cc_sp_send = messaging.new_message('carControlSP')
+    cc_sp_send.valid = sm['carState'].canValid
+    cc_sp_send.carControlSP = CC_SP
+
+    pm.send('carControlSP', cc_sp_send)
+
+  def run_ext(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
+    CC_SP = self.state_control_ext(sm)
+    self.publish_ext(CC_SP, sm, pm)

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

@@ -0,0 +1,112 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from openpilot.cereal import log
+
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+
+
+class AutoLaneChangeMode:
+  OFF = -1
+  NUDGE = 0  # default
+  NUDGELESS = 1
+  HALF_SECOND = 2
+  ONE_SECOND = 3
+  TWO_SECONDS = 4
+  THREE_SECONDS = 5
+
+
+AUTO_LANE_CHANGE_TIMER = {
+  AutoLaneChangeMode.OFF: 0.0,            # Off
+  AutoLaneChangeMode.NUDGE: 0.0,          # Nudge
+  AutoLaneChangeMode.NUDGELESS: 0.05,     # Nudgeless
+  AutoLaneChangeMode.HALF_SECOND: 0.5,    # 0.5-second delay
+  AutoLaneChangeMode.ONE_SECOND: 1.0,     # 1-second delay
+  AutoLaneChangeMode.TWO_SECONDS: 2.0,    # 2-second delay
+  AutoLaneChangeMode.THREE_SECONDS: 3.0,  # 3-second delay
+}
+
+ONE_SECOND_DELAY = -1
+
+
+class AutoLaneChangeController:
+  def __init__(self, desire_helper):
+    self.DH = desire_helper
+    self.params = Params()
+
+    self.lane_change_wait_timer = 0.0
+    self.param_read_counter = 0
+    self.lane_change_delay = 0.0
+
+    self.lane_change_set_timer = self.params.get("AutoLaneChangeTimer", return_default=True)
+    self.lane_change_bsm_delay = False
+
+    self.prev_brake_pressed = False
+    self.auto_lane_change_allowed = False
+    self.prev_lane_change = False
+
+    self.read_params()
+
+  def reset(self) -> None:
+    # Auto reset if parent state indicates we should
+    if self.DH.lane_change_state == log.LaneChangeState.off and \
+       self.DH.lane_change_direction == log.LaneChangeDirection.none:
+      self.lane_change_wait_timer = 0.0
+      self.prev_brake_pressed = False
+      self.prev_lane_change = False
+
+  def read_params(self) -> None:
+    self.lane_change_bsm_delay = self.params.get_bool("AutoLaneChangeBsmDelay")
+    self.lane_change_set_timer = self.params.get("AutoLaneChangeTimer", return_default=True)
+
+  def update_params(self) -> None:
+    if self.param_read_counter % 50 == 0:
+      self.read_params()
+    self.param_read_counter += 1
+
+  def update_lane_change_timers(self, blindspot_detected: bool) -> None:
+    self.lane_change_delay = AUTO_LANE_CHANGE_TIMER.get(self.lane_change_set_timer,
+                                                        AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGE])
+
+    self.lane_change_wait_timer += DT_MDL
+
+    if self.lane_change_bsm_delay and blindspot_detected and self.lane_change_delay > 0:
+      if self.lane_change_delay == AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGELESS]:
+        self.lane_change_wait_timer = ONE_SECOND_DELAY
+      else:
+        self.lane_change_wait_timer = self.lane_change_delay + ONE_SECOND_DELAY
+
+  def update_allowed(self) -> bool:
+    # Auto lane change allowed if:
+    # 1. A valid delay is set (non-zero)
+    # 2. Brake wasn't previously pressed
+    # 3. We've waited long enough
+
+    if self.lane_change_set_timer in (AutoLaneChangeMode.OFF, AutoLaneChangeMode.NUDGE):
+      return False
+
+    if self.prev_brake_pressed:
+      return False
+
+    if self.prev_lane_change:
+      return False
+
+    return bool(self.lane_change_wait_timer > self.lane_change_delay)
+
+  def update_lane_change(self, blindspot_detected: bool, brake_pressed: bool) -> None:
+    if brake_pressed and not self.prev_brake_pressed:
+      self.prev_brake_pressed = brake_pressed
+
+    self.update_lane_change_timers(blindspot_detected)
+
+    self.auto_lane_change_allowed = self.update_allowed()
+
+  def update_state(self):
+    if self.DH.lane_change_state == log.LaneChangeState.laneChangeStarting:
+      self.prev_lane_change = True
+
+    self.reset()

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

@@ -0,0 +1,44 @@
+"""
+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.structs import car
+
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+
+
+class BlinkerPauseLateral:
+  def __init__(self):
+    self.params = Params()
+
+    self.enabled = self.params.get_bool("BlinkerPauseLateralControl")
+    self.is_metric = self.params.get_bool("IsMetric")
+    self.min_speed = 0
+    self.reengage_delay = 0
+    self.blinker_off_timer = 0.0
+
+  def get_params(self) -> None:
+    self.enabled = self.params.get_bool("BlinkerPauseLateralControl")
+    self.is_metric = self.params.get_bool("IsMetric")
+    self.min_speed = self.params.get("BlinkerMinLateralControlSpeed", return_default=True)
+    self.reengage_delay = self.params.get("BlinkerLateralReengageDelay", return_default=True)
+
+  def update(self, CS: car.CarState, DT_CTRL: float = 0.01) -> bool:
+    if not self.enabled:
+      return False
+
+    one_blinker = CS.leftBlinker != CS.rightBlinker
+    speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
+    min_speed_ms = self.min_speed * speed_factor
+
+    below_speed = CS.vEgo < min_speed_ms
+
+    if one_blinker and below_speed:
+      self.blinker_off_timer = self.reengage_delay
+    elif self.blinker_off_timer > 0:
+      self.blinker_off_timer -= DT_CTRL
+
+    return bool((one_blinker and below_speed) or self.blinker_off_timer > 0)

openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py

@@ -0,0 +1,17 @@
+class WMACConstants:
+  # Lead detection parameters
+  LEAD_WINDOW_SIZE = 6  # Stable detection window
+  LEAD_PROB = 0.45  # Balanced threshold for lead detection
+
+  # Slow down detection parameters
+  SLOW_DOWN_WINDOW_SIZE = 5  # Responsive but stable
+  SLOW_DOWN_PROB = 0.3  # Balanced threshold for slow down scenarios
+
+  # Optimized slow down distance curve - smooth and progressive
+  SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
+  SLOW_DOWN_DIST = [32., 46., 64., 86., 108., 130., 145., 165.]
+
+  # Slowness detection parameters
+  SLOWNESS_WINDOW_SIZE = 10  # Stable slowness detection
+  SLOWNESS_PROB = 0.55  # Clear threshold for slowness
+  SLOWNESS_CRUISE_OFFSET = 1.025  # Conservative cruise speed offset

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

@@ -0,0 +1,388 @@
+"""
+Copyright (c) 2021-, rav4kumar, 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.
+"""
+# Version = 2025-6-30
+
+from openpilot.cereal import messaging
+from opendbc.car import structs
+from numpy import interp
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
+from typing import Literal
+
+# d-e2e, from modeldata.h
+TRAJECTORY_SIZE = 33
+SET_MODE_TIMEOUT = 15
+
+# Define the valid mode types
+ModeType = Literal['acc', 'blended']
+
+
+class SmoothKalmanFilter:
+  """Enhanced Kalman filter with smoothing for stable decision making."""
+
+  def __init__(self, initial_value=0, measurement_noise=0.1, process_noise=0.01,
+               alpha=1.0, smoothing_factor=0.85):
+    self.x = initial_value
+    self.P = 1.0
+    self.R = measurement_noise
+    self.Q = process_noise
+    self.alpha = alpha
+    self.smoothing_factor = smoothing_factor
+    self.initialized = False
+    self.history = []
+    self.max_history = 10
+    self.confidence = 0.0
+
+  def add_data(self, measurement):
+    if len(self.history) >= self.max_history:
+      self.history.pop(0)
+    self.history.append(measurement)
+
+    if not self.initialized:
+      self.x = measurement
+      self.initialized = True
+      self.confidence = 0.1
+      return
+
+    self.P = self.alpha * self.P + self.Q
+
+    K = self.P / (self.P + self.R)
+    effective_K = K * (1.0 - self.smoothing_factor) + self.smoothing_factor * 0.1
+
+    innovation = measurement - self.x
+    self.x = self.x + effective_K * innovation
+    self.P = (1 - effective_K) * self.P
+
+    if abs(innovation) < 0.1:
+      self.confidence = min(1.0, self.confidence + 0.05)
+    else:
+      self.confidence = max(0.1, self.confidence - 0.02)
+
+  def get_value(self):
+    return self.x if self.initialized else None
+
+  def get_confidence(self):
+    return self.confidence
+
+  def reset_data(self):
+    self.initialized = False
+    self.history = []
+    self.confidence = 0.0
+
+
+class ModeTransitionManager:
+  """Manages smooth transitions between driving modes with hysteresis."""
+
+  def __init__(self):
+    self.current_mode: ModeType = 'acc'
+    self.mode_confidence = {'acc': 1.0, 'blended': 0.0}
+    self.transition_timeout = 0
+    self.min_mode_duration = 10
+    self.mode_duration = 0
+    self.emergency_override = False
+
+  def request_mode(self, mode: ModeType, confidence: float = 1.0, emergency: bool = False):
+    # Emergency override for critical situations (stops, collisions)
+    if emergency:
+      self.emergency_override = True
+      self.current_mode = mode
+      self.transition_timeout = SET_MODE_TIMEOUT
+      self.mode_duration = 0
+      return
+
+    self.mode_confidence[mode] = min(1.0, self.mode_confidence[mode] + 0.1 * confidence)
+    for m in self.mode_confidence:
+      if m != mode:
+        self.mode_confidence[m] = max(0.0, self.mode_confidence[m] - 0.05)
+
+    # Require minimum duration in current mode (unless emergency)
+    if self.mode_duration < self.min_mode_duration and not self.emergency_override:
+      return
+
+    # Hysteresis: higher threshold for mode changes
+    confidence_threshold = 0.6 if mode != self.current_mode else 0.3  # Lower threshold for faster response
+
+    if self.mode_confidence[mode] > confidence_threshold:
+      if mode != self.current_mode and self.transition_timeout == 0:
+        self.transition_timeout = SET_MODE_TIMEOUT
+        self.current_mode = mode
+        self.mode_duration = 0
+
+  def update(self):
+    if self.transition_timeout > 0:
+      self.transition_timeout -= 1
+    self.mode_duration += 1
+
+    # Reset emergency override after some time
+    if self.emergency_override and self.mode_duration > 20:
+      self.emergency_override = False
+
+    # Gradual confidence decay
+    for mode in self.mode_confidence:
+      self.mode_confidence[mode] *= 0.98
+
+  def get_mode(self) -> ModeType:
+    return self.current_mode
+
+
+class DynamicExperimentalController:
+  def __init__(self, CP: structs.CarParams, mpc, params=None):
+    self._CP = CP
+    self._mpc = mpc
+    self._params = params or Params()
+    self._enabled: bool = self._params.get_bool("DynamicExperimentalControl")
+    self._active: bool = False
+    self._frame: int = 0
+    self._urgency = 0.0
+
+    self._mode_manager = ModeTransitionManager()
+
+    # Smooth filters for stable decision making with faster response for critical scenarios
+    self._lead_filter = SmoothKalmanFilter(
+      measurement_noise=0.15,
+      process_noise=0.05,
+      alpha=1.02,
+      smoothing_factor=0.8
+    )
+
+    self._slow_down_filter = SmoothKalmanFilter(
+      measurement_noise=0.1,
+      process_noise=0.1,
+      alpha=1.05,
+      smoothing_factor=0.7
+    )
+
+    self._slowness_filter = SmoothKalmanFilter(
+      measurement_noise=0.1,
+      process_noise=0.06,
+      alpha=1.015,
+      smoothing_factor=0.92
+    )
+
+    self._mpc_fcw_filter = SmoothKalmanFilter(
+      measurement_noise=0.2,
+      process_noise=0.1,
+      alpha=1.1,
+      smoothing_factor=0.5
+    )
+    self._has_lead_filtered = False
+    self._has_slow_down = False
+    self._has_slowness = False
+    self._has_mpc_fcw = False
+    self._v_ego_kph = 0.0
+    self._v_cruise_kph = 0.0
+    self._has_standstill = False
+    self._mpc_fcw_crash_cnt = 0
+    self._standstill_count = 0
+    # debug
+    self._endpoint_x = float('inf')
+    self._expected_distance = 0.0
+    self._trajectory_valid = False
+
+  def _read_params(self) -> None:
+    if self._frame % int(1. / DT_MDL) == 0:
+      self._enabled = self._params.get_bool("DynamicExperimentalControl")
+
+  def mode(self) -> str:
+    return self._mode_manager.get_mode()
+
+  def enabled(self) -> bool:
+    return self._enabled
+
+  def active(self) -> bool:
+    return self._active
+
+  def set_mpc_fcw_crash_cnt(self) -> None:
+    """Set MPC FCW crash count"""
+    self._mpc_fcw_crash_cnt = self._mpc.crash_cnt
+
+  def _update_calculations(self, sm: messaging.SubMaster) -> None:
+    car_state = sm['carState']
+    lead_one = sm['radarState'].leadOne
+    md = sm['modelV2']
+
+    self._v_ego_kph = car_state.vEgo * 3.6
+    self._v_cruise_kph = car_state.vCruise
+    self._has_standstill = car_state.standstill
+
+    # standstill detection
+    if self._has_standstill:
+      self._standstill_count = min(20, self._standstill_count + 1)
+    else:
+      self._standstill_count = max(0, self._standstill_count - 1)
+
+    # Lead detection
+    self._lead_filter.add_data(float(lead_one.present))
+    lead_value = self._lead_filter.get_value() or 0.0
+    self._has_lead_filtered = lead_value > WMACConstants.LEAD_PROB
+
+    # MPC FCW detection
+    fcw_filtered_value = self._mpc_fcw_filter.get_value() or 0.0
+    self._mpc_fcw_filter.add_data(float(self._mpc_fcw_crash_cnt > 0))
+    self._has_mpc_fcw = fcw_filtered_value > 0.5
+
+    # Slow down detection
+    self._calculate_slow_down(md)
+
+    # Slowness detection
+    if not (self._standstill_count > 5) and not self._has_slow_down:
+      current_slowness = float(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET))
+      self._slowness_filter.add_data(current_slowness)
+      slowness_value = self._slowness_filter.get_value() or 0.0
+
+      # Hysteresis for slowness
+      threshold = WMACConstants.SLOWNESS_PROB * (0.8 if self._has_slowness else 1.1)
+      self._has_slowness = slowness_value > threshold
+
+  def _calculate_slow_down(self, md):
+    """Calculate urgency based on trajectory endpoint vs expected distance."""
+
+    # Reset to safe defaults
+    urgency = 0.0
+    self._endpoint_x = float('inf')
+    self._trajectory_valid = False
+
+    #Require exact trajectory size
+    position_valid = len(md.position.x) == TRAJECTORY_SIZE
+    orientation_valid = len(md.orientation.x) == TRAJECTORY_SIZE
+
+    if not (position_valid and orientation_valid):
+      # Invalid trajectory - this itself might indicate a stop scenario
+      # Apply moderate urgency for incomplete trajectories at speed
+      if self._v_ego_kph > 20.0:
+        urgency = 0.3
+
+      self._slow_down_filter.add_data(urgency)
+      urgency_filtered = self._slow_down_filter.get_value() or 0.0
+      self._has_slow_down = urgency_filtered > WMACConstants.SLOW_DOWN_PROB
+      self._urgency = urgency_filtered
+      return
+
+    # We have a valid full trajectory
+    self._trajectory_valid = True
+
+    # Use the exact endpoint (33rd point, index 32)
+    endpoint_x = md.position.x[TRAJECTORY_SIZE - 1]
+    self._endpoint_x = endpoint_x
+
+    # Get expected distance based on current speed using tuned constants
+    expected_distance = interp(self._v_ego_kph,
+                               WMACConstants.SLOW_DOWN_BP,
+                               WMACConstants.SLOW_DOWN_DIST)
+    self._expected_distance = expected_distance
+
+    # Calculate urgency based on trajectory shortage
+    if endpoint_x < expected_distance:
+      shortage = expected_distance - endpoint_x
+      shortage_ratio = shortage / expected_distance
+
+      # Base urgency on shortage ratio
+      urgency = min(1.0, shortage_ratio * 2.0)
+
+      # Increase urgency for very short trajectories (imminent stops)
+      critical_distance = expected_distance * 0.3
+      if endpoint_x < critical_distance:
+        urgency = min(1.0, urgency * 2.0)
+
+      # Speed-based urgency adjustment
+      if self._v_ego_kph > 25.0:
+        speed_factor = 1.0 + (self._v_ego_kph - 25.0) / 80.0
+        urgency = min(1.0, urgency * speed_factor)
+
+    # Apply filtering but with less smoothing for stops
+    self._slow_down_filter.add_data(urgency)
+    urgency_filtered = self._slow_down_filter.get_value() or 0.0
+
+    # Update state with lower threshold for better stop detection
+    self._has_slow_down = urgency_filtered > (WMACConstants.SLOW_DOWN_PROB * 0.8)
+    self._urgency = urgency_filtered
+
+  def _radarless_mode(self) -> None:
+    """Radarless mode decision logic with emergency handling."""
+
+    # EMERGENCY: MPC FCW - immediate blended mode
+    if self._has_mpc_fcw:
+      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
+      return
+
+    # Standstill: use blended
+    if self._standstill_count > 3:
+      self._mode_manager.request_mode('blended', confidence=0.9)
+      return
+
+    # Slow down scenarios: emergency for high urgency, normal for lower urgency
+    if self._has_slow_down:
+      if self._urgency > 0.7:
+        # Emergency: immediate blended mode for high urgency stops
+        self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
+      else:
+        # Normal: blended with urgency-based confidence
+        confidence = min(1.0, self._urgency * 1.5)
+        self._mode_manager.request_mode('blended', confidence=confidence)
+      return
+
+    # Driving slow: use ACC (but not if actively slowing down)
+    if self._has_slowness and not self._has_slow_down:
+      self._mode_manager.request_mode('acc', confidence=0.8)
+      return
+
+    # Default: ACC
+    self._mode_manager.request_mode('acc', confidence=0.7)
+
+  def _radar_mode(self) -> None:
+    """Radar mode with emergency handling."""
+
+    # EMERGENCY: MPC FCW - immediate blended mode
+    if self._has_mpc_fcw:
+      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
+      return
+
+    # If lead detected and not in standstill: always use ACC
+    if self._has_lead_filtered and not (self._standstill_count > 3):
+      self._mode_manager.request_mode('acc', confidence=1.0)
+      return
+
+    # Slow down scenarios: emergency for high urgency, normal for lower urgency
+    if self._has_slow_down:
+      if self._urgency > 0.7:
+        # Emergency: immediate blended mode for high urgency stops
+        self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
+      else:
+        # Normal: blended with urgency-based confidence
+        confidence = min(1.0, self._urgency * 1.3)
+        self._mode_manager.request_mode('blended', confidence=confidence)
+      return
+
+    # Standstill: use blended
+    if self._standstill_count > 3:
+      self._mode_manager.request_mode('blended', confidence=0.9)
+      return
+
+    # Driving slow: use ACC (but not if actively slowing down)
+    if self._has_slowness and not self._has_slow_down:
+      self._mode_manager.request_mode('acc', confidence=0.8)
+      return
+
+    # Default: ACC
+    self._mode_manager.request_mode('acc', confidence=0.7)
+
+  def update(self, sm: messaging.SubMaster) -> None:
+    self._read_params()
+
+    self.set_mpc_fcw_crash_cnt()
+
+    self._update_calculations(sm)
+
+    if self._CP.radarUnavailable:
+      self._radarless_mode()
+    else:
+      self._radar_mode()
+
+    self._mode_manager.update()
+    self._active = sm['selfdriveState'].experimentalMode and self._enabled
+    self._frame += 1

openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py

@@ -0,0 +1,94 @@
+import pytest
+
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
+
+class MockLeadOne:
+  def __init__(self, status=0.0):
+    self.status = status
+
+class MockRadarState:
+  def __init__(self, status=0.0):
+    self.leadOne = MockLeadOne(status=status)
+
+class MockCarState:
+  def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False):
+    self.vEgo = vEgo
+    self.vCruise = vCruise
+    self.standstill = standstill
+
+class MockModelData:
+  def __init__(self, valid=True):
+    size = 33 if valid else 10  # incomplete if invalid
+    self.position = type("Pos", (), {"x": [0.0] * size})()
+    self.orientation = type("Ori", (), {"x": [0.0] * size})()
+
+class MockSelfDriveState:
+  def __init__(self, experimentalMode=False):
+    self.experimentalMode = experimentalMode
+
+class MockParams:
+  def get_bool(self, name):
+    return True
+
+@pytest.fixture
+def default_sm():
+  sm = {
+    'carState': MockCarState(vEgo=10.0, vCruise=20.0),
+    'radarState': MockRadarState(status=1.0),
+    'modelV2': MockModelData(valid=True),
+    'selfdriveState': MockSelfDriveState(experimentalMode=True),
+  }
+  return sm
+
+@pytest.fixture
+def mock_cp():
+  class CP:
+    radarUnavailable = False
+  return CP()
+
+@pytest.fixture
+def mock_mpc():
+  class MPC:
+    crash_cnt = 0
+  return MPC()
+
+# Fake Kalman Filter that always returns a given value
+class FakeKalman:
+  def __init__(self, value=1.0):
+    self.value = value
+  def add_data(self, v): pass
+  def get_value(self): return self.value
+  def get_confidence(self): return 1.0
+  def reset_data(self): pass
+
+def test_initial_mode_is_acc(mock_cp, mock_mpc):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  assert controller.mode() == "acc"
+
+def test_standstill_triggers_blended(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['carState'].standstill = True
+  for _ in range(10):
+    controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+def test_emergency_blended_on_fcw(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  mock_mpc.crash_cnt = 1  # simulate FCW
+  for _ in range(2):
+    controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+def test_radarless_slowdown_triggers_blended(mock_cp, mock_mpc, default_sm):
+  mock_cp.radarUnavailable = True
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+
+  # Force conditions to simulate slowdown
+  controller._slow_down_filter = FakeKalman(value=1.0)  # Ensure urgency triggers slowdown
+  controller._v_ego_kph = 35.0
+  default_sm['modelV2'] = MockModelData(valid=False)  # Incomplete trajectory
+
+  for _ in range(3):
+    controller.update(default_sm)
+
+  assert controller.mode() == "blended"

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

@@ -0,0 +1,170 @@
+"""
+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 messaging, custom
+
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+
+GREEN_LIGHT_X_THRESHOLD = 30
+LEAD_DEPART_DIST_THRESHOLD = 1.0
+TRIGGER_TIMER_THRESHOLD = 0.3
+
+
+class E2EStates:
+  INACTIVE = 0
+  ARMED = 1
+  CONSUMED = 2
+
+
+class E2EAlertsHelper:
+  def __init__(self):
+    self._params = Params()
+    self.frame = -1
+    self.green_light_state = E2EStates.INACTIVE
+    self.prev_green_light_state = E2EStates.INACTIVE
+    self.lead_depart_state = E2EStates.INACTIVE
+    self.prev_lead_depart_state = E2EStates.INACTIVE
+
+    self.green_light_alert = False
+    self.green_light_alert_enabled = self._params.get_bool("GreenLightAlert")
+    self.lead_depart_alert = False
+    self.lead_depart_alert_enabled = self._params.get_bool("LeadDepartAlert")
+
+    self.green_light_trigger_timer = 0
+    self.lead_depart_trigger_timer = 0
+    self.last_lead_distance = -1
+    self.last_moving_frame = -1
+
+    self.allowed = False
+    self.last_allowed = False
+    self.has_lead = False
+
+    self.lead_depart_arm_timer = 0
+    self.lead_depart_confirmed_lead = False
+    self.lead_depart_armed = False
+
+  def _read_params(self) -> None:
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.green_light_alert_enabled = self._params.get_bool("GreenLightAlert")
+      self.lead_depart_alert_enabled = self._params.get_bool("LeadDepartAlert")
+
+  def update_alert_trigger(self, sm: messaging.SubMaster):
+    CS = sm['carState']
+    CC = sm['carControl']
+
+    model_x = sm['modelV2'].position.x
+    max_idx = len(model_x) - 1
+    self.has_lead = sm['radarState'].leadOne.present
+    lead_dRel = sm['radarState'].leadOne.dRel
+
+    standstill = CS.standstill
+    moving = not standstill and CS.vEgo > 0.1
+
+    if moving:
+      self.last_moving_frame = self.frame
+    recent_moving = self.last_moving_frame == -1 or (self.frame - self.last_moving_frame) * DT_MDL < 2.0
+
+    self.allowed = not moving and not CS.gasPressed and not CC.enabled and not recent_moving
+
+    # Green Light Alert
+    green_light_trigger = False
+    if self.green_light_state == E2EStates.ARMED:
+      if model_x[max_idx] > GREEN_LIGHT_X_THRESHOLD:
+        self.green_light_trigger_timer += 1
+      else:
+        self.green_light_trigger_timer = 0
+
+      if self.green_light_trigger_timer * DT_MDL > TRIGGER_TIMER_THRESHOLD:
+        green_light_trigger = True
+    elif self.green_light_state != E2EStates.ARMED:
+      self.green_light_trigger_timer = 0
+
+    # Lead Departure Alert
+    close_lead_valid = self.has_lead and lead_dRel < 8.0
+    if self.allowed and not self.last_allowed and close_lead_valid:
+      self.lead_depart_confirmed_lead = True
+    elif not self.allowed:
+      self.lead_depart_confirmed_lead = False
+
+    if self.allowed and self.lead_depart_confirmed_lead and close_lead_valid:
+      self.lead_depart_arm_timer += 1
+
+      if self.lead_depart_arm_timer * DT_MDL >= 1.0:
+        self.lead_depart_armed = True
+    else:
+      self.lead_depart_arm_timer = 0
+      self.lead_depart_armed = False
+
+    lead_depart_trigger = False
+    if self.lead_depart_state == E2EStates.ARMED:
+      if self.last_lead_distance == -1 or lead_dRel < self.last_lead_distance:
+        self.last_lead_distance = lead_dRel
+
+      if self.last_lead_distance != -1 and (lead_dRel - self.last_lead_distance > LEAD_DEPART_DIST_THRESHOLD):
+        self.lead_depart_trigger_timer += 1
+      else:
+        self.lead_depart_trigger_timer = 0
+
+      if self.lead_depart_trigger_timer * DT_MDL > TRIGGER_TIMER_THRESHOLD:
+        lead_depart_trigger = True
+    elif self.lead_depart_state != E2EStates.ARMED:
+      self.last_lead_distance = -1
+      self.lead_depart_trigger_timer = 0
+
+    self.last_allowed = self.allowed
+
+    return green_light_trigger, lead_depart_trigger
+
+  @staticmethod
+  def update_state_machine(state: int, enabled: bool, allowed: bool, triggered: bool) -> tuple[int, bool]:
+    if state != E2EStates.INACTIVE:
+      if not allowed or not enabled:
+        state = E2EStates.INACTIVE
+
+      else:
+        if state == E2EStates.ARMED:
+          if triggered:
+            state = E2EStates.CONSUMED
+
+        elif state == E2EStates.CONSUMED:
+          pass
+
+    elif state == E2EStates.INACTIVE:
+      if allowed and enabled:
+        state = E2EStates.ARMED
+
+    return state, triggered
+
+  def update(self, sm: messaging.SubMaster, events_sp: EventsSP) -> None:
+    self._read_params()
+
+    green_light_trigger, lead_depart_trigger = self.update_alert_trigger(sm)
+
+    self.prev_green_light_state = self.green_light_state
+    self.prev_lead_depart_state = self.lead_depart_state
+
+    self.green_light_state, self.green_light_alert = self.update_state_machine(
+      self.green_light_state,
+      self.green_light_alert_enabled,
+      self.allowed and not self.has_lead,
+      green_light_trigger
+    )
+
+    self.lead_depart_state, self.lead_depart_alert = self.update_state_machine(
+      self.lead_depart_state,
+      self.lead_depart_alert_enabled,
+      self.allowed and self.lead_depart_armed,
+      lead_depart_trigger
+    )
+
+    if self.green_light_alert or self.lead_depart_alert:
+      events_sp.add(custom.OnroadEventSP.EventName.e2eChime)
+
+    self.frame += 1

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

@@ -0,0 +1,47 @@
+"""
+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 custom
+
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+
+TurnDirection = custom.ModelDataV2SP.TurnDirection
+
+LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
+
+
+class LaneTurnController:
+  def __init__(self, desire_helper):
+    self.DH = desire_helper
+    self.turn_direction = TurnDirection.none
+    self.params = Params()
+    self.lane_turn_value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS
+    self.param_read_counter = 0
+    self.enabled = self.params.get_bool("LaneTurnDesire")
+
+  def read_params(self):
+    self.enabled = self.params.get_bool("LaneTurnDesire")
+    value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS
+    self.lane_turn_value = min(float(LANE_CHANGE_SPEED_MIN), value)
+
+  def update_params(self) -> None:
+    if self.param_read_counter % 50 == 0:
+      self.read_params()
+    self.param_read_counter += 1
+
+  def update_lane_turn(self, blindspot_left: bool, blindspot_right: bool, left_blinker: bool, right_blinker: bool, v_ego: float) -> None:
+    if left_blinker and not right_blinker and v_ego < self.lane_turn_value and not blindspot_left:
+      self.turn_direction = TurnDirection.turnLeft
+    elif right_blinker and not left_blinker and v_ego < self.lane_turn_value and not blindspot_right:
+      self.turn_direction = TurnDirection.turnRight
+    else:
+      self.turn_direction = TurnDirection.none
+
+  def get_turn_direction(self):
+    if not self.enabled:
+      return TurnDirection.none
+    return self.turn_direction

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

@@ -0,0 +1,38 @@
+"""
+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.sunnypilot.selfdrive.controls.lib.nnlc.nnlc import NeuralNetworkLateralControl
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_override import LatControlTorqueExtOverride
+
+
+class LatControlTorqueExt(NeuralNetworkLateralControl, LatControlTorqueExtOverride):
+  def __init__(self, lac_torque, CP, CP_SP, CI):
+    NeuralNetworkLateralControl.__init__(self, lac_torque, CP, CP_SP, CI)
+    LatControlTorqueExtOverride.__init__(self, CP)
+
+  def update(self, CS, VM, pid, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
+             desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel,
+             desired_curvature, actual_curvature, steer_limited_by_safety, output_torque):
+    self._ff = ff
+    self._pid = pid
+    self._pid_log = pid_log
+    self._setpoint = setpoint
+    self._measurement = measurement
+    self._roll_compensation = roll_compensation
+    self._lateral_accel_deadzone = lateral_accel_deadzone
+    self._desired_lateral_accel = desired_lateral_accel
+    self._actual_lateral_accel = actual_lateral_accel
+    self._desired_curvature = desired_curvature
+    self._actual_curvature = actual_curvature
+    self._gravity_adjusted_lateral_accel = gravity_adjusted_lateral_accel
+    self._steer_limited_by_safety = steer_limited_by_safety
+    self._output_torque = output_torque
+
+    self.update_calculations(CS, VM, desired_lateral_accel)
+    self.update_neural_network_feedforward(CS, params, calibrated_pose)
+
+    return self._pid_log, self._output_torque

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

@@ -0,0 +1,134 @@
+"""
+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 math
+import numpy as np
+
+from openpilot.common.pid import PIDController
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+from openpilot.selfdrive.modeld.constants import ModelConstants
+
+LAT_PLAN_MIN_IDX = 5
+LATERAL_LAG_MOD = 0.0  # seconds, modifies how far in the future we look ahead for the lateral plan
+
+KP = 1.0
+KI = 0.3
+
+
+def get_predicted_lateral_jerk(lat_accels, t_diffs):
+  # compute finite difference between subsequent model_v2.acceleration.y values
+  # this is just two calls of np.diff followed by an element-wise division
+  lat_accel_diffs = np.diff(lat_accels)
+  lat_jerk = lat_accel_diffs / t_diffs
+  # return as python list
+  return lat_jerk.tolist()
+
+
+def sign(x):
+  return 1.0 if x > 0.0 else (-1.0 if x < 0.0 else 0.0)
+
+
+def get_lookahead_value(future_vals, current_val):
+  if len(future_vals) == 0:
+    return current_val
+
+  same_sign_vals = [v for v in future_vals if sign(v) == sign(current_val)]
+
+  # if any future val has opposite sign of current val, return 0
+  if len(same_sign_vals) < len(future_vals):
+    return 0.0
+
+  # otherwise return the value with minimum absolute value
+  min_val = min(same_sign_vals + [current_val], key=lambda x: abs(x))
+  return min_val
+
+
+class LatControlTorqueExtBase:
+  def __init__(self, lac_torque, CP, CP_SP, CI):
+    self.model_v2 = None
+    self.model_valid = False
+    self.lac_torque = lac_torque
+
+    self.actual_lateral_jerk: float = 0.0
+    self.lateral_jerk_setpoint: float = 0.0
+    self.lateral_jerk_measurement: float = 0.0
+    self.lookahead_lateral_jerk: float = 0.0
+
+    self.torque_from_lateral_accel_in_torque_space = CI.torque_from_lateral_accel_in_torque_space()
+    self.torque_params = lac_torque.torque_params
+
+    self._ff = 0.0
+    self._pid = PIDController(KP, KI)
+    self._pid_log = None
+    self._setpoint = 0.0
+    self._measurement = 0.0
+    self._roll_compensation = 0.0
+    self._lateral_accel_deadzone = 0.0
+    self._desired_lateral_accel = 0.0
+    self._actual_lateral_accel = 0.0
+    self._desired_curvature = 0.0
+    self._actual_curvature = 0.0
+    self._gravity_adjusted_lateral_accel = 0.0
+    self._steer_limited_by_safety = False
+    self._output_torque = 0.0
+
+    # twilsonco's Lateral Neural Network Feedforward
+    # Instantaneous lateral jerk changes very rapidly, making it not useful on its own,
+    # however, we can "look ahead" to the future planned lateral jerk in order to gauge
+    # whether the current desired lateral jerk will persist into the future, i.e.
+    # whether it's "deliberate" or not. This allows us to simply ignore short-lived jerk.
+    # Note that LAT_PLAN_MIN_IDX is defined above and is used in order to prevent
+    # using a "future" value that is actually planned to occur before the "current" desired
+    # value, which is offset by the steerActuatorDelay.
+    # TODO-SP: Reevaluate lookahead v values that determines how low a desired lateral jerk signal needs to
+    #          persist in order to be used.
+    self.friction_look_ahead_v = [1.4, 2.0]  # how many seconds in the future to look ahead in [0, ~2.1] in 0.1 increments
+    self.friction_look_ahead_bp = [9.0, 30.0]  # corresponding speeds in m/s in [0, ~40] in 1.0 increments
+
+    # Scaling the lateral acceleration "friction response" could be helpful for some.
+    # Increase for a stronger response, decrease for a weaker response.
+    self.lat_jerk_friction_factor = 0.4
+    self.lat_accel_friction_factor = 0.7  # in [0, 3], in 0.05 increments. 3 is arbitrary safety limit
+
+    # precompute time differences between ModelConstants.T_IDXS
+    self.t_diffs = np.diff(ModelConstants.T_IDXS)
+    self.desired_lat_jerk_time = CP.steerActuatorDelay + LATERAL_LAG_MOD
+
+  def update_model_v2(self, model_v2):
+    self.model_v2 = model_v2
+    self.model_valid = self.model_v2 is not None and len(self.model_v2.orientation.x) >= CONTROL_N
+
+  def update_lateral_lag(self, lag):
+    self.desired_lat_jerk_time = max(0.01, lag) + LATERAL_LAG_MOD
+
+  def update_friction_input(self, val_1, val_2):
+    _error = val_1 - val_2
+    _value = self.lat_accel_friction_factor * _error + self.lat_jerk_friction_factor * self.lookahead_lateral_jerk
+
+    return _value
+
+  def update_calculations(self, CS, VM, desired_lateral_accel):
+    self.actual_lateral_jerk = 0.0
+    self.lateral_jerk_setpoint = 0.0
+    self.lateral_jerk_measurement = 0.0
+    self.lookahead_lateral_jerk = 0.0
+
+    actual_curvature_rate = -VM.calc_curvature(math.radians(CS.steeringRateDeg), CS.vEgo, 0.0)
+    self.actual_lateral_jerk = actual_curvature_rate * CS.vEgo ** 2
+
+    if self.model_valid:
+      # prepare "look-ahead" desired lateral jerk
+      lookahead = np.interp(CS.vEgo, self.friction_look_ahead_bp, self.friction_look_ahead_v)
+      friction_upper_idx = next((i for i, val in enumerate(ModelConstants.T_IDXS) if val > lookahead), 16)
+      predicted_lateral_jerk = get_predicted_lateral_jerk(self.model_v2.acceleration.y, self.t_diffs)
+      desired_lateral_jerk = (np.interp(self.desired_lat_jerk_time, ModelConstants.T_IDXS,
+                              self.model_v2.acceleration.y) - desired_lateral_accel) / self.desired_lat_jerk_time
+      self.lookahead_lateral_jerk = get_lookahead_value(predicted_lateral_jerk[LAT_PLAN_MIN_IDX:friction_upper_idx], desired_lateral_jerk)
+      if self.lookahead_lateral_jerk == 0.0:
+        self.actual_lateral_jerk = 0.0
+        self.lat_accel_friction_factor = 1.0
+      self.lateral_jerk_setpoint = self.lat_jerk_friction_factor * self.lookahead_lateral_jerk
+      self.lateral_jerk_measurement = self.lat_jerk_friction_factor * self.actual_lateral_jerk

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

@@ -0,0 +1,34 @@
+"""
+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.common.params import Params
+
+
+class LatControlTorqueExtOverride:
+  def __init__(self, CP):
+    self.CP = CP
+    self.params = Params()
+    self.enforce_torque_control_toggle = self.params.get_bool("EnforceTorqueControl")  # only during init
+    self.torque_override_enabled = self.params.get_bool("TorqueParamsOverrideEnabled")
+    self.frame = -1
+
+  def update_override_torque_params(self, torque_params) -> bool:
+    if not self.enforce_torque_control_toggle:
+      return False
+
+    self.frame += 1
+    if self.frame % 300 == 0:
+      self.torque_override_enabled = self.params.get_bool("TorqueParamsOverrideEnabled")
+
+      if not self.torque_override_enabled:
+        return False
+
+      torque_params.latAccelFactor = float(self.params.get("TorqueParamsOverrideLatAccelFactor", return_default=True))
+      torque_params.friction = float(self.params.get("TorqueParamsOverrideFriction", return_default=True))
+      return True
+
+    return False

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

@@ -0,0 +1,128 @@
+import math
+import numpy as np
+from collections import deque
+
+from openpilot.cereal import log
+from opendbc.car.lateral import get_friction
+from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+from openpilot.common.pid import PIDController
+
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext import LatControlTorqueExt
+
+# At higher speeds (25+mph) we can assume:
+# Lateral acceleration achieved by a specific car correlates to
+# torque applied to the steering rack. It does not correlate to
+# wheel slip, or to speed.
+
+# This controller applies torque to achieve desired lateral
+# accelerations. To compensate for the low speed effects the
+# proportional gain is increased at low speeds by the PID controller.
+# Additionally, there is friction in the steering wheel that needs
+# to be overcome to move it at all, this is compensated for too.
+
+KP = 1.0
+KI = 0.3
+KD = 0.0
+INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30]
+KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP]
+
+LP_FILTER_CUTOFF_HZ = 1.2
+LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
+FRICTION_THRESHOLD = 0.3
+VERSION = 0
+
+
+class LatControlTorque(LatControl):
+  def __init__(self, CP, CP_SP, CI, dt):
+    super().__init__(CP, CP_SP, CI, dt)
+    self.torque_params = CP.lateralTuning.torque.as_builder()
+    self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
+    self.lateral_accel_from_torque = CI.lateral_accel_from_torque()
+    self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, KD, rate=1/self.dt)
+    self.update_limits()
+    self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
+    self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
+    self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len)
+    self.previous_measurement = 0.0
+    self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
+
+    self.extension = LatControlTorqueExt(self, CP, CP_SP, CI)
+
+  def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
+    self.torque_params.latAccelFactor = latAccelFactor
+    self.torque_params.latAccelOffset = latAccelOffset
+    self.torque_params.friction = friction
+    self.update_limits()
+
+  def update_limits(self):
+    self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params),
+                        self.lateral_accel_from_torque(-self.steer_max, self.torque_params))
+
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay):
+    # Override torque params from extension
+    if self.extension.update_override_torque_params(self.torque_params):
+      self.update_limits()
+
+    pid_log = log.ControlsState.LateralTorqueState.new_message()
+    pid_log.version = VERSION
+    if not active:
+      output_torque = 0.0
+      pid_log.active = False
+    else:
+      measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
+      roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY
+      curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
+      lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
+
+      delay_frames = int(np.clip(lat_delay / self.dt, 1, self.lat_accel_request_buffer_len))
+      expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames]
+      # TODO factor out lateral jerk from error to later replace it with delay independent alternative
+      future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2
+      self.lat_accel_request_buffer.append(future_desired_lateral_accel)
+      gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
+      desired_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / lat_delay
+
+      measurement = measured_curvature * CS.vEgo ** 2
+      measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt)
+      self.previous_measurement = measurement
+
+      setpoint = lat_delay * desired_lateral_jerk + expected_lateral_accel
+      error = setpoint - measurement
+
+      # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly
+      pid_log.error = float(error)
+      ff = gravity_adjusted_future_lateral_accel
+      # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll
+      ff -= self.torque_params.latAccelOffset
+      # TODO jerk is weighted by lat_delay for legacy reasons, but should be made independent of it
+      ff += get_friction(error, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
+
+      freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
+      output_lataccel = self.pid.update(pid_log.error,
+                                       -measurement_rate,
+                                        feedforward=ff,
+                                        speed=CS.vEgo,
+                                        freeze_integrator=freeze_integrator)
+      output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
+
+      # Lateral acceleration torque controller extension updates
+      # Overrides pid_log.error and output_torque
+      pid_log, output_torque = self.extension.update(CS, VM, self.pid, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
+                                                     future_desired_lateral_accel, measurement, lateral_accel_deadzone, gravity_adjusted_future_lateral_accel,
+                                                     desired_curvature, measured_curvature, steer_limited_by_safety, output_torque)
+
+      pid_log.active = True
+      pid_log.p = float(self.pid.p)
+      pid_log.i = float(self.pid.i)
+      pid_log.d = float(self.pid.d)
+      pid_log.f = float(self.pid.f)
+      pid_log.output = float(-output_torque) # TODO: log lat accel?
+      pid_log.actualLateralAccel = float(measurement)
+      pid_log.desiredLateralAccel = float(setpoint)
+      pid_log.desiredLateralJerk = float(desired_lateral_jerk)
+      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
+
+    # TODO left is positive in this convention
+    return -output_torque, 0.0, pid_log

openpilot/sunnypilot/selfdrive/controls/lib/latcontrol_torque_versions.json

@@ -0,0 +1,8 @@
+{
+  "v1.0": {
+    "version": "1.0"
+  },
+  "v0.0": {
+    "version": "0.0"
+  }
+}

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

@@ -0,0 +1,141 @@
+"""
+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 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.dec.dec import DynamicExperimentalController
+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
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+from openpilot.sunnypilot.models.helpers import get_active_bundle
+
+DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
+LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
+
+
+class LongitudinalPlannerSP:
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
+    self.events_sp = EventsSP()
+    self.resolver = SpeedLimitResolver()
+    self.dec = DynamicExperimentalController(CP, mpc)
+    self.scc = SmartCruiseControl()
+    self.resolver = SpeedLimitResolver()
+    self.sla = SpeedLimitAssist(CP, CP_SP)
+    self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
+    self.source = LongitudinalPlanSource.cruise
+    self.e2e_alerts_helper = E2EAlertsHelper()
+
+    self.output_v_target = 0.
+    self.output_a_target = 0.
+
+  def is_e2e(self, sm: messaging.SubMaster) -> bool:
+    experimental_mode = sm['selfdriveState'].experimentalMode
+    if not self.dec.active():
+      return experimental_mode
+
+    return experimental_mode and self.dec.mode() == "blended"
+
+  def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
+    CS = sm['carState']
+    v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
+    v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS
+
+    long_enabled = sm['carControl'].enabled
+    long_override = sm['carControl'].cruiseControl.override
+
+    # Smart Cruise Control
+    self.scc.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)
+
+    # Speed Limit Resolver
+    self.resolver.update(v_ego, sm)
+
+    # Speed Limit Assist
+    has_speed_limit = self.resolver.speed_limit_valid or self.resolver.speed_limit_last_valid
+    self.sla.update(long_enabled, long_override, v_ego, a_ego, v_cruise_cluster, self.resolver.speed_limit,
+                    self.resolver.speed_limit_final_last, has_speed_limit, self.resolver.distance, self.events_sp)
+
+    targets = {
+      LongitudinalPlanSource.cruise: (v_cruise, a_ego),
+      LongitudinalPlanSource.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target),
+      LongitudinalPlanSource.sccMap: (self.scc.map.output_v_target, self.scc.map.output_a_target),
+      LongitudinalPlanSource.speedLimitAssist: (self.sla.output_v_target, self.sla.output_a_target),
+    }
+
+    self.source = min(targets, key=lambda k: targets[k][0])
+    self.output_v_target, self.output_a_target = targets[self.source]
+    return self.output_v_target, self.output_a_target
+
+  def update(self, sm: messaging.SubMaster) -> None:
+    self.events_sp.clear()
+    self.dec.update(sm)
+    self.e2e_alerts_helper.update(sm, self.events_sp)
+
+  def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
+    plan_sp_send = messaging.new_message('longitudinalPlanSP')
+
+    plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
+
+    longitudinalPlanSP = plan_sp_send.longitudinalPlanSP
+    longitudinalPlanSP.longitudinalPlanSource = self.source
+    longitudinalPlanSP.vTarget = float(self.output_v_target)
+    longitudinalPlanSP.aTarget = float(self.output_a_target)
+    longitudinalPlanSP.events = self.events_sp.to_msg()
+
+    # Dynamic Experimental Control
+    dec = longitudinalPlanSP.dec
+    dec.state = DecState.blended if self.dec.mode() == 'blended' else DecState.acc
+    dec.enabled = self.dec.enabled()
+    dec.active = self.dec.active()
+
+    # Smart Cruise Control
+    smartCruiseControl = longitudinalPlanSP.smartCruiseControl
+    # Vision Control
+    sccVision = smartCruiseControl.vision
+    sccVision.state = self.scc.vision.state
+    sccVision.vTarget = float(self.scc.vision.output_v_target)
+    sccVision.aTarget = float(self.scc.vision.output_a_target)
+    sccVision.currentLateralAccel = float(self.scc.vision.current_lat_acc)
+    sccVision.maxPredictedLateralAccel = float(self.scc.vision.max_pred_lat_acc)
+    sccVision.enabled = self.scc.vision.is_enabled
+    sccVision.active = self.scc.vision.is_active
+    # Map Control
+    sccMap = smartCruiseControl.map
+    sccMap.state = self.scc.map.state
+    sccMap.vTarget = float(self.scc.map.output_v_target)
+    sccMap.aTarget = float(self.scc.map.output_a_target)
+    sccMap.enabled = self.scc.map.is_enabled
+    sccMap.active = self.scc.map.is_active
+
+    # Speed Limit
+    speedLimit = longitudinalPlanSP.speedLimit
+    resolver = speedLimit.resolver
+    resolver.speedLimit = float(self.resolver.speed_limit)
+    resolver.speedLimitLast = float(self.resolver.speed_limit_last)
+    resolver.speedLimitFinal = float(self.resolver.speed_limit_final)
+    resolver.speedLimitFinalLast = float(self.resolver.speed_limit_final_last)
+    resolver.speedLimitValid = self.resolver.speed_limit_valid
+    resolver.speedLimitLastValid = self.resolver.speed_limit_last_valid
+    resolver.speedLimitOffset = float(self.resolver.speed_limit_offset)
+    resolver.distToSpeedLimit = float(self.resolver.distance)
+    resolver.source = self.resolver.source
+    assist = speedLimit.assist
+    assist.state = self.sla.state
+    assist.enabled = self.sla.is_enabled
+    assist.active = self.sla.is_active
+    assist.vTarget = float(self.sla.output_v_target)
+    assist.aTarget = float(self.sla.output_a_target)
+
+    # E2E Alerts
+    e2eAlerts = longitudinalPlanSP.e2eAlerts
+    e2eAlerts.greenLightAlert = self.e2e_alerts_helper.green_light_alert
+    e2eAlerts.leadDepartAlert = self.e2e_alerts_helper.lead_depart_alert
+
+    pm.send('longitudinalPlanSP', plan_sp_send)

openpilot/sunnypilot/selfdrive/controls/lib/nnlc/helpers.py

@@ -0,0 +1,68 @@
+"""
+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 os
+import tomllib
+from difflib import SequenceMatcher
+
+from opendbc.car import structs
+from openpilot.common.basedir import BASEDIR
+
+TORQUE_NN_MODEL_PATH = os.path.join(BASEDIR, "openpilot", "sunnypilot", "neural_network_data", "neural_network_lateral_control")
+TORQUE_NN_MODEL_SUBSTITUTE_PATH = os.path.join(BASEDIR, "opendbc_repo", "opendbc", "car", "torque_data/substitute.toml")
+MOCK_MODEL_PATH = os.path.join(TORQUE_NN_MODEL_PATH, "MOCK.json")
+
+
+def similarity(s1: str, s2: str) -> float:
+  return SequenceMatcher(None, s1, s2).ratio()
+
+
+def get_nn_model_path(CP: structs.CarParams) -> tuple[str, str, bool]:
+  car_fingerprint = CP.carFingerprint
+  eps_fw = str(next((fw.fwVersion for fw in CP.carFw if fw.ecu == "eps"), ""))
+
+  def check_nn_path(_nn_candidate):
+    _model_path = None
+    _max_similarity = -1.0
+    for f in os.listdir(TORQUE_NN_MODEL_PATH):
+      if f.endswith(".json"):
+        model = os.path.splitext(f)[0]
+        similarity_score = similarity(model, _nn_candidate)
+        if similarity_score > _max_similarity:
+          _max_similarity = similarity_score
+          _model_path = os.path.join(TORQUE_NN_MODEL_PATH, f)
+    return _model_path, _max_similarity
+
+  if len(eps_fw) > 3:
+    eps_fw = eps_fw.replace("\\", "")
+    nn_candidate = f"{car_fingerprint} {eps_fw}"
+  else:
+    nn_candidate = car_fingerprint
+
+  model_path, max_similarity = check_nn_path(nn_candidate)
+  exact_match = max_similarity >= 0.99
+
+  if car_fingerprint not in model_path or 0.0 <= max_similarity < 0.9:
+    nn_candidate = car_fingerprint
+    model_path, max_similarity = check_nn_path(nn_candidate)
+    exact_match = max_similarity >= 0.99
+
+    if 0.0 <= max_similarity < 0.9:
+      with open(TORQUE_NN_MODEL_SUBSTITUTE_PATH, 'rb') as f:
+        sub = tomllib.load(f)
+      sub_candidate = sub.get(car_fingerprint, car_fingerprint)
+
+      for candidate in [car_fingerprint, sub_candidate]:
+        model_path, max_similarity = check_nn_path(candidate)
+
+      exact_match = False
+
+  if CP.steerControlType == structs.CarParams.SteerControlType.angle:
+    model_path = MOCK_MODEL_PATH
+
+  model_name = os.path.splitext(os.path.basename(model_path))[0]
+
+  return model_path, model_name, exact_match

openpilot/sunnypilot/selfdrive/controls/lib/nnlc/model.py

@@ -0,0 +1,83 @@
+"""
+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 json import load
+import numpy as np
+
+from openpilot.selfdrive.modeld.parse_model_outputs import safe_exp
+
+# dict used to rename activation functions whose names aren't valid python identifiers
+ACTIVATION_FUNCTION_NAMES = {'σ': 'sigmoid'}
+
+
+class NNTorqueModel:
+  def __init__(self, params_file, zero_bias=False):
+    with open(params_file) as f:
+      params = load(f)
+
+    self.input_size = params["input_size"]
+    self.output_size = params["output_size"]
+    self.input_mean = np.array(params["input_mean"], dtype=np.float32).T
+    self.input_std = np.array(params["input_std"], dtype=np.float32).T
+    self.layers = []
+    self.friction_override = False
+
+    for layer_params in params["layers"]:
+      W = np.array(layer_params[next(key for key in layer_params.keys() if key.endswith('_W'))], dtype=np.float32).T
+      b = np.array(layer_params[next(key for key in layer_params.keys() if key.endswith('_b'))], dtype=np.float32).T
+      if zero_bias:
+        b = np.zeros_like(b)
+      activation = layer_params["activation"]
+      for k, v in ACTIVATION_FUNCTION_NAMES.items():
+        activation = activation.replace(k, v)
+      self.layers.append((W, b, activation))
+
+    self.validate_layers()
+    self.check_for_friction_override()
+
+  # Begin activation functions.
+  # These are called by name using the keys in the model json file
+  @staticmethod
+  def sigmoid(x):
+    return 1 / (1 + safe_exp(-x))
+
+  @staticmethod
+  def identity(x):
+    return x
+  # End activation functions
+
+  def forward(self, x):
+    for W, b, activation in self.layers:
+      x = getattr(self, activation)(x.dot(W) + b)
+    return x
+
+  def evaluate(self, input_array):
+    in_len = len(input_array)
+    if in_len != self.input_size:
+      # If the input is length 2-4, then it's a simplified evaluation.
+      # In that case, need to add on zeros to fill out the input array to match the correct length.
+      if 2 <= in_len:
+        input_array = input_array + [0] * (self.input_size - in_len)
+      else:
+        raise ValueError(f"Input array length {len(input_array)} must be length 2 or greater")
+
+    input_array = np.array(input_array, dtype=np.float32)
+
+    # Rescale the input array using the input_mean and input_std
+    input_array = (input_array - self.input_mean) / self.input_std
+
+    output_array = self.forward(input_array)
+
+    return float(output_array[0, 0])
+
+  def validate_layers(self):
+    for _, _, activation in self.layers:
+      if not hasattr(self, activation):
+        raise ValueError(f"Unknown activation: {activation}")
+
+  def check_for_friction_override(self):
+    y = self.evaluate([10.0, 0.0, 0.2])
+    self.friction_override = (y < 0.1)

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

@@ -0,0 +1,164 @@
+"""
+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 collections import deque
+import math
+import numpy as np
+
+from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction
+from opendbc.sunnypilot.car.interfaces import LatControlInputs
+from opendbc.sunnypilot.car.lateral_ext import get_friction as get_friction_in_torque_space
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.params import Params
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_base import LatControlTorqueExtBase, sign
+from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import MOCK_MODEL_PATH
+from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.model import NNTorqueModel
+
+LOW_SPEED_X = [0, 10, 20, 30]
+LOW_SPEED_Y = [12, 3, 1, 0]
+
+
+# At a given roll, if pitch magnitude increases, the
+# gravitational acceleration component starts pointing
+# in the longitudinal direction, decreasing the lateral
+# acceleration component. Here we do the same thing
+# to the roll value itself, then passed to nnff.
+def roll_pitch_adjust(roll, pitch):
+  return roll * math.cos(pitch)
+
+
+class NeuralNetworkLateralControl(LatControlTorqueExtBase):
+  def __init__(self, lac_torque, CP, CP_SP, CI):
+    super().__init__(lac_torque, CP, CP_SP, CI)
+    self.params = Params()
+    self.enabled = self.params.get_bool("NeuralNetworkLateralControl")
+    self.has_nn_model = CP_SP.neuralNetworkLateralControl.model.path != MOCK_MODEL_PATH
+
+    # NN model takes current v_ego, lateral_accel, lat accel/jerk error, roll, and past/future/planned data
+    # of lat accel and roll
+    # Past value is computed using previous desired lat accel and observed roll
+    self.model = NNTorqueModel(CP_SP.neuralNetworkLateralControl.model.path)
+
+    self.pitch = FirstOrderFilter(0.0, 0.5, 0.01)
+    self.pitch_last = 0.0
+
+    # setup future time offsets
+    self.future_times = [0.3, 0.6, 1.0, 1.5] # seconds in the future
+    self.nn_future_times = [i + self.desired_lat_jerk_time for i in self.future_times]
+
+    # setup past time offsets
+    self.past_times = [-0.3, -0.2, -0.1]
+    history_check_frames = [int(abs(i)*100) for i in self.past_times]
+    self.history_frame_offsets = [history_check_frames[0] - i for i in history_check_frames]
+    self.lateral_accel_desired_deque = deque(maxlen=history_check_frames[0])
+    self.roll_deque = deque(maxlen=history_check_frames[0])
+    self.error_deque = deque(maxlen=history_check_frames[0])
+    self.past_future_len = len(self.past_times) + len(self.nn_future_times)
+
+  @property
+  def _nnlc_enabled(self):
+    return self.enabled and self.model_valid and self.has_nn_model
+
+  def update_limits(self):
+    if not self._nnlc_enabled:
+      return
+
+    self._pid.set_limits(self.lac_torque.steer_max, -self.lac_torque.steer_max)
+
+  def update_lateral_lag(self, lag):
+    super().update_lateral_lag(lag)
+    self.nn_future_times = [t + self.desired_lat_jerk_time for t in self.future_times]
+
+  def update_feedforward_torque_space(self, CS):
+    torque_from_setpoint = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._setpoint, self._roll_compensation, CS.vEgo, CS.aEgo),
+                                                                          self.torque_params, gravity_adjusted=False)
+    torque_from_measurement = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._measurement, self._roll_compensation, CS.vEgo, CS.aEgo),
+                                                                             self.torque_params, gravity_adjusted=False)
+    self._pid_log.error = float(torque_from_setpoint - torque_from_measurement)
+    self._ff = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._gravity_adjusted_lateral_accel, self._roll_compensation,
+                                                                               CS.vEgo, CS.aEgo), self.torque_params, gravity_adjusted=True)
+    self._ff += get_friction_in_torque_space(self._desired_lateral_accel - self._actual_lateral_accel, self._lateral_accel_deadzone,
+                                             FRICTION_THRESHOLD, self.torque_params)
+
+  def update_output_torque(self, CS):
+    freeze_integrator = self._steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
+    self._output_torque = self._pid.update(self._pid_log.error,
+                                           feedforward=self._ff,
+                                           speed=CS.vEgo,
+                                           freeze_integrator=freeze_integrator)
+
+  def update_neural_network_feedforward(self, CS, params, calibrated_pose) -> None:
+    if not self._nnlc_enabled:
+      return
+
+    self.update_feedforward_torque_space(CS)
+
+    low_speed_factor = float(np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)) ** 2
+    self._setpoint = self._desired_lateral_accel + low_speed_factor * self._desired_curvature
+    self._measurement = self._actual_lateral_accel + low_speed_factor * self._actual_curvature
+
+    # update past data
+    roll = params.roll
+    if calibrated_pose is not None:
+      pitch = self.pitch.update(calibrated_pose.orientation.pitch)
+      roll = roll_pitch_adjust(roll, pitch)
+      self.pitch_last = pitch
+    self.roll_deque.append(roll)
+    self.lateral_accel_desired_deque.append(self._desired_lateral_accel)
+
+    # prepare past and future values
+    # adjust future times to account for longitudinal acceleration
+    adjusted_future_times = [t + 0.5 * CS.aEgo * (t / max(CS.vEgo, 1.0)) for t in self.nn_future_times]
+    past_rolls = [self.roll_deque[min(len(self.roll_deque) - 1, i)] for i in self.history_frame_offsets]
+    future_rolls = [roll_pitch_adjust(np.interp(t, ModelConstants.T_IDXS, self.model_v2.orientation.x) + roll,
+                                      np.interp(t, ModelConstants.T_IDXS, self.model_v2.orientation.y) + self.pitch_last) for t in
+                    adjusted_future_times]
+    past_lateral_accels_desired = [self.lateral_accel_desired_deque[min(len(self.lateral_accel_desired_deque) - 1, i)]
+                                   for i in self.history_frame_offsets]
+    future_planned_lateral_accels = [np.interp(t, ModelConstants.T_IDXS, self.model_v2.acceleration.y) for t in
+                                     adjusted_future_times]
+
+    # compute NNFF error response
+    nnff_setpoint_input = [CS.vEgo, self._setpoint, self.lateral_jerk_setpoint, roll] \
+                          + [self._setpoint] * self.past_future_len \
+                          + past_rolls + future_rolls
+    # past lateral accel error shouldn't count, so use past desired like the setpoint input
+    nnff_measurement_input = [CS.vEgo, self._measurement, self.lateral_jerk_measurement, roll] \
+                             + [self._measurement] * self.past_future_len \
+                             + past_rolls + future_rolls
+    torque_from_setpoint = self.model.evaluate(nnff_setpoint_input)
+    torque_from_measurement = self.model.evaluate(nnff_measurement_input)
+    self._pid_log.error = torque_from_setpoint - torque_from_measurement
+
+    # The "pure" NNLC error response can be too weak for cars whose models were trained
+    # with a lack of high-magnitude lateral acceleration data, for which the NNLC model
+    # torque response flattens out at high lateral accelerations.
+    # This workaround blends in a guaranteed stronger error response only when the
+    # desired lateral acceleration is high enough to warrant it, by using the lateral acceleration
+    # error as the input to the NNLC model. This is not ideal, and potentially degrades the NNLC
+    # accuracy for cars that don't have this issue, but it's necessary until a better NNLC model
+    # structure is used that doesn't create this issue when high-magnitude data is missing.
+    error_blend_factor = float(np.interp(abs(self._desired_lateral_accel), [1.0, 2.0], [0.0, 1.0]))
+    if error_blend_factor > 0.0:  # blend in stronger error response when in high lat accel
+      # NNFF inputs 5+ are optional, and if left out are replaced with 0.0 inside the NNFF class
+      nnff_error_input = [CS.vEgo, self._setpoint - self._measurement, self.lateral_jerk_setpoint - self.lateral_jerk_measurement, 0.0]
+      torque_from_error = self.model.evaluate(nnff_error_input)
+      if sign(self._pid_log.error) == sign(torque_from_error) and abs(self._pid_log.error) < abs(torque_from_error):
+        self._pid_log.error = self._pid_log.error * (1.0 - error_blend_factor) + torque_from_error * error_blend_factor
+
+    # compute feedforward (same as nn setpoint output)
+    friction_input = self.update_friction_input(self._setpoint, self._measurement)
+    nn_input = [CS.vEgo, self._desired_lateral_accel, friction_input, roll] \
+               + past_lateral_accels_desired + future_planned_lateral_accels \
+               + past_rolls + future_rolls
+    self._ff = self.model.evaluate(nn_input)
+
+    # apply friction override for cars with low NN friction response
+    if self.model.friction_override:
+      self._pid_log.error += get_friction(friction_input, self._lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
+
+    self.update_output_torque(CS)

openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_fingerprint.py

@@ -0,0 +1,43 @@
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.honda.values import CAR as HONDA
+from opendbc.car.hyundai.values import CAR as HYUNDAI
+from opendbc.car.nissan.values import CAR as NISSAN
+from opendbc.car.toyota.values import CAR as TOYOTA
+from opendbc.car.tesla.values import CAR as TESLA
+from openpilot.common.parameterized import parameterized
+from openpilot.common.params import Params
+from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
+
+
+FINGERPRINT_EXACT_MATCH = [HONDA.HONDA_CIVIC_BOSCH, TOYOTA.TOYOTA_RAV4_TSS2_2022, HYUNDAI.HYUNDAI_IONIQ_5]
+FINGERPRINT_FUZZY_MATCH = [HONDA.HONDA_CIVIC_BOSCH_DIESEL, HYUNDAI.GENESIS_G70_2020, HYUNDAI.HYUNDAI_IONIQ_6]
+FINGERPRINT_ANGLE_NO_MATCH = [TOYOTA.TOYOTA_RAV4_TSS2_2023, NISSAN.NISSAN_LEAF, TESLA.TESLA_MODEL_3]
+
+
+class TestNNLCFingerprintBase:
+
+  @staticmethod
+  def _setup_platform(car_name):
+    CarInterface = interfaces[car_name]
+    CP = CarInterface.get_non_essential_params(car_name)
+    CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
+    CI = CarInterface(CP, CP_SP)
+
+    sunnypilot_interfaces.setup_interfaces(CI, Params())
+
+    return CI
+
+  @parameterized.expand(FINGERPRINT_EXACT_MATCH)
+  def test_exact_fingerprint(self, car_name):
+    CI = self._setup_platform(car_name)
+    assert CI.CP_SP.neuralNetworkLateralControl.model.name != "MOCK" and not CI.CP_SP.neuralNetworkLateralControl.fuzzyFingerprint
+
+  @parameterized.expand(FINGERPRINT_FUZZY_MATCH)
+  def test_fuzzy_fingerprint(self, car_name):
+    CI = self._setup_platform(car_name)
+    assert CI.CP_SP.neuralNetworkLateralControl.model.name != "MOCK" and CI.CP_SP.neuralNetworkLateralControl.fuzzyFingerprint
+
+  @parameterized.expand(FINGERPRINT_ANGLE_NO_MATCH)
+  def test_no_fingerprint(self, car_name):
+    CI = self._setup_platform(car_name)
+    assert CI.CP_SP.neuralNetworkLateralControl.model.name == "MOCK"

openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_load_model.py

@@ -0,0 +1,31 @@
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.honda.values import CAR as HONDA
+from opendbc.car.hyundai.values import CAR as HYUNDAI
+from opendbc.car.toyota.values import CAR as TOYOTA
+from openpilot.common.parameterized import parameterized
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.car.helpers import convert_to_capnp
+from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
+
+
+class TestNNTorqueModel:
+
+  @parameterized.expand([HONDA.HONDA_CIVIC, TOYOTA.TOYOTA_RAV4, HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN])
+  def test_load_model(self, car_name):
+    params = Params()
+    params.put_bool("NeuralNetworkLateralControl", True, block=True)
+
+    CarInterface = interfaces[car_name]
+    CP = CarInterface.get_non_essential_params(car_name)
+    CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
+    CI = CarInterface(CP, CP_SP)
+
+    sunnypilot_interfaces.setup_interfaces(CI, params)
+
+    CP_SP = convert_to_capnp(CP_SP)
+
+    controller = LatControlTorque(CP.as_reader(), CP_SP.as_reader(), CI, DT_CTRL)
+
+    assert controller.extension.has_nn_model

openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py

@@ -0,0 +1,103 @@
+import numpy as np
+
+from openpilot.cereal import log, messaging
+from opendbc.car.structs import car
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.gm.values import CAR as GM
+from opendbc.car.honda.values import CAR as HONDA
+from opendbc.car.hyundai.values import CAR as HYUNDAI
+from opendbc.car.toyota.values import CAR as TOYOTA
+from opendbc.car.vehicle_model import VehicleModel
+from openpilot.common.parameterized import parameterized
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.car.helpers import convert_to_capnp
+from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from openpilot.selfdrive.locationd.helpers import Pose
+from openpilot.common.mock.generators import generate_livePose
+from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
+from openpilot.selfdrive.modeld.constants import ModelConstants
+
+
+def generate_modelV2():
+  model = messaging.new_message('modelV2')
+  position = log.XYZTData.new_message()
+  speed = 30
+  position.x = [float(x) for x in (speed + 0.5) * np.array(ModelConstants.T_IDXS)]
+  model.modelV2.position = position
+  orientation = log.XYZTData.new_message()
+  curvature = 0.05
+  orientation.x = [float(curvature) for _ in ModelConstants.T_IDXS]
+  orientation.y = [0.0 for _ in ModelConstants.T_IDXS]
+  model.modelV2.orientation = orientation
+  velocity = log.XYZTData.new_message()
+  velocity.x = [float(x) for x in (speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
+  velocity.x[0] = float(speed)  # always start at current speed
+  model.modelV2.velocity = velocity
+  acceleration = log.XYZTData.new_message()
+  acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
+  acceleration.y = [float(y) for y in np.zeros_like(ModelConstants.T_IDXS)]
+  model.modelV2.acceleration = acceleration
+
+  return model
+
+
+class TestNeuralNetworkLateralControl:
+
+  @parameterized.expand([HONDA.HONDA_CIVIC, TOYOTA.TOYOTA_RAV4, HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN, GM.CHEVROLET_BOLT_EUV])
+  def test_saturation(self, car_name):
+    params = Params()
+    params.put_bool("NeuralNetworkLateralControl", True, block=True)
+
+    CarInterface = interfaces[car_name]
+    CP = CarInterface.get_non_essential_params(car_name)
+    CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
+    CI = CarInterface(CP, CP_SP)
+
+    sunnypilot_interfaces.setup_interfaces(CI, params)
+
+    CP_SP = convert_to_capnp(CP_SP)
+    VM = VehicleModel(CP)
+
+    controller = LatControlTorque(CP.as_reader(), CP_SP.as_reader(), CI, DT_CTRL)
+    torque_params = CP.lateralTuning.torque
+
+    CS = car.CarState.new_message()
+    CS.vEgo = 30
+    CS.steeringPressed = False
+
+    params = log.LiveParametersData.new_message()
+
+    lp = generate_livePose()
+    pose = Pose.from_live_pose(lp.livePose)
+
+    mdl = generate_modelV2()
+    sm = {'modelV2': mdl.modelV2}
+    model_v2 = sm['modelV2']
+    controller.extension.model_v2 = model_v2
+
+    # Saturate for curvature limited and controller limited
+    test_lag = 0.3
+    for _ in range(1000):
+      controller.extension.update_model_v2(model_v2)
+      controller.extension.update_lateral_lag(test_lag)
+      controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction)
+      controller.extension.update_limits()
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True, 0.2)
+    assert lac_log.saturated
+
+    for _ in range(1000):
+      controller.extension.update_model_v2(model_v2)
+      controller.extension.update_lateral_lag(test_lag)
+      controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction)
+      controller.extension.update_limits()
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False, 0.2)
+    assert not lac_log.saturated
+
+    for _ in range(1000):
+      controller.extension.update_model_v2(model_v2)
+      controller.extension.update_lateral_lag(test_lag)
+      controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction)
+      controller.extension.update_limits()
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False, 0.2)
+    assert lac_log.saturated

openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/__init__.py

@@ -0,0 +1,9 @@
+"""
+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.common.constants import CV
+
+MIN_V = 20 * CV.KPH_TO_MS  # Do not operate under 20 km/h

openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py

@@ -0,0 +1,261 @@
+import json
+import math
+import platform
+
+from openpilot.cereal import custom
+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.navd.helpers import coordinate_from_param, Coordinate
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
+
+MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
+
+ACTIVE_STATES = (MapState.turning, )
+ENABLED_STATES = (MapState.enabled, MapState.overriding, *ACTIVE_STATES)
+
+R = 6373000.0  # approximate radius of earth in meters
+TO_RADIANS = math.pi / 180
+TO_DEGREES = 180 / math.pi
+TARGET_JERK = -0.6  # m/s^3 There's some jounce limits that are not consistent so we're fudging this some
+TARGET_ACCEL = -1.2  # m/s^2 should match up with the long planner limit
+TARGET_OFFSET = 1.0  # seconds - This controls how soon before the curve you reach the target velocity. It also helps
+                     # reach the target velocity when inaccuracies in the distance modeling logic would cause overshoot.
+                     # The value is multiplied against the target velocity to determine the additional distance. This is
+                     # done to keep the distance calculations consistent but results in the offset actually being less
+                     # time than specified depending on how much of a speed differential there is between v_ego and the
+                     # target velocity.
+
+
+def velocities_from_param(param: str, params: Params):
+  if params is None:
+    params = Params()
+
+  json_str = params.get(param)
+  if json_str is None:
+    return None
+
+  velocities = json.loads(json_str)
+
+  return velocities
+
+
+def calculate_accel(t, target_jerk, a_ego):
+  return a_ego + target_jerk * t
+
+
+def calculate_velocity(t, target_jerk, a_ego, v_ego):
+  return v_ego + a_ego * t + target_jerk/2 * (t ** 2)
+
+
+def calculate_distance(t, target_jerk, a_ego, v_ego):
+  return t * v_ego + a_ego/2 * (t ** 2) + target_jerk/6 * (t ** 3)
+
+
+# points should be in radians
+# output is meters
+def distance_to_point(ax, ay, bx, by):
+  a = math.sin((bx-ax)/2)*math.sin((bx-ax)/2) + math.cos(ax) * math.cos(bx)*math.sin((by-ay)/2)*math.sin((by-ay)/2)
+  c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
+
+  return R * c  # in meters
+
+
+class SmartCruiseControlMap:
+  v_target: float = 0
+  a_target: float = 0.
+  v_ego: float = 0.
+  a_ego: float = 0.
+  output_v_target: float = V_CRUISE_UNSET
+  output_a_target: float = 0.
+
+  def __init__(self):
+    self.params = Params()
+    self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
+    self.enabled = self.params.get_bool("SmartCruiseControlMap")
+    self.long_enabled = False
+    self.long_override = False
+    self.is_enabled = False
+    self.is_active = False
+    self.state = MapState.disabled
+    self.v_cruise = 0
+    self.target_lat = 0.0
+    self.target_lon = 0.0
+    self.frame = -1
+
+    self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0)
+    self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or []
+
+  def get_v_target_from_control(self) -> float:
+    if self.is_active:
+      return max(self.v_target, MIN_V)
+
+    return V_CRUISE_UNSET
+
+  def get_a_target_from_control(self) -> float:
+    return self.a_ego
+
+  def update_params(self):
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.enabled = self.params.get_bool("SmartCruiseControlMap")
+
+  def update_calculations(self) -> None:
+    self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0)
+    lat = self.last_position.latitude
+    lon = self.last_position.longitude
+
+    self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or []
+
+    if self.last_position is None or self.target_velocities is None:
+      return
+
+    min_dist = 1000
+    min_idx = 0
+    distances = []
+
+    # find our location in the path
+    for i in range(len(self.target_velocities)):
+      target_velocity = self.target_velocities[i]
+      tlat = target_velocity["latitude"]
+      tlon = target_velocity["longitude"]
+      d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, tlat * TO_RADIANS, tlon * TO_RADIANS)
+      distances.append(d)
+      if d < min_dist:
+        min_dist = d
+        min_idx = i
+
+    # only look at values from our current position forward
+    forward_points = self.target_velocities[min_idx:]
+    forward_distances = distances[min_idx:]
+
+    # find velocities that we are within the distance we need to adjust for
+    valid_velocities = []
+    for i in range(len(forward_points)):
+      target_velocity = forward_points[i]
+      tlat = target_velocity["latitude"]
+      tlon = target_velocity["longitude"]
+      tv = target_velocity["velocity"]
+      if tv > self.v_ego:
+        continue
+
+      d = forward_distances[i]
+
+      a_diff = (self.a_ego - TARGET_ACCEL)
+      accel_t = abs(a_diff / TARGET_JERK)
+      min_accel_v = calculate_velocity(accel_t, TARGET_JERK, self.a_ego, self.v_ego)
+
+      max_d = 0
+      if tv > min_accel_v:
+        # calculate time needed based on target jerk
+        a = 0.5 * TARGET_JERK
+        b = self.a_ego
+        c = self.v_ego - tv
+        t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a
+        t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a
+        if not isinstance(t_a, complex) and t_a > 0:
+          t = t_a
+        else:
+          t = t_b
+        if isinstance(t, complex):
+          continue
+
+        max_d = max_d + calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego)
+      else:
+        t = accel_t
+        max_d = calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego)
+
+        # calculate additional time needed based on target accel
+        t = abs((min_accel_v - tv) / TARGET_ACCEL)
+        max_d += calculate_distance(t, 0, TARGET_ACCEL, min_accel_v)
+
+      if d < max_d + tv * TARGET_OFFSET:
+        valid_velocities.append((float(tv), tlat, tlon))
+
+    # Find the smallest velocity we need to adjust for
+    min_v = 100.0
+    target_lat = 0.0
+    target_lon = 0.0
+    for tv, lat, lon in valid_velocities:
+      if tv < min_v:
+        min_v = tv
+        target_lat = lat
+        target_lon = lon
+
+    if self.v_target < min_v and not (self.target_lat == 0 and self.target_lon == 0):
+      for i in range(len(forward_points)):
+        target_velocity = forward_points[i]
+        tlat = target_velocity["latitude"]
+        tlon = target_velocity["longitude"]
+        tv = target_velocity["velocity"]
+        if tv > self.v_ego:
+          continue
+
+        if tlat == self.target_lat and tlon == self.target_lon and tv == self.v_target:
+          return
+
+      # not found so let's reset
+      self.v_target = 0.0
+      self.target_lat = 0.0
+      self.target_lon = 0.0
+
+    self.v_target = min_v
+    self.target_lat = target_lat
+    self.target_lon = target_lon
+
+  def _update_state_machine(self) -> tuple[bool, bool]:
+    # ENABLED, TURNING
+    if self.state != MapState.disabled:
+      if not self.long_enabled or not self.enabled:
+        self.state = MapState.disabled
+      elif self.long_override:
+        self.state = MapState.overriding
+
+      else:
+        # ENABLED
+        if self.state == MapState.enabled:
+          if self.v_cruise > self.v_target != 0:
+            self.state = MapState.turning
+
+        # TURNING
+        elif self.state == MapState.turning:
+          if self.v_cruise <= self.v_target or self.v_target == 0:
+            self.state = MapState.enabled
+
+        # OVERRIDING
+        elif self.state == MapState.overriding:
+          if not self.long_override:
+            if self.v_cruise > self.v_target != 0:
+              self.state = MapState.turning
+            else:
+              self.state = MapState.enabled
+
+    # DISABLED
+    elif self.state == MapState.disabled:
+      if self.long_enabled and self.enabled:
+        if self.long_override:
+          self.state = MapState.overriding
+        else:
+          self.state = MapState.enabled
+
+    enabled = self.state in ENABLED_STATES
+    active = self.state in ACTIVE_STATES
+
+    return enabled, active
+
+  def update(self, long_enabled: bool, long_override: bool, v_ego, a_ego, v_cruise) -> None:
+    self.long_enabled = long_enabled
+    self.long_override = long_override
+    self.v_ego = v_ego
+    self.a_ego = a_ego
+    self.v_cruise = v_cruise
+
+    self.update_params()
+    self.update_calculations()
+
+    self.is_enabled, self.is_active = self._update_state_machine()
+
+    self.output_v_target = self.get_v_target_from_control()
+    self.output_a_target = self.get_a_target_from_control()
+
+    self.frame += 1

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

@@ -0,0 +1,19 @@
+"""
+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 openpilot.cereal.messaging as messaging
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
+
+
+class SmartCruiseControl:
+  def __init__(self):
+    self.vision = SmartCruiseControlVision()
+    self.map = SmartCruiseControlMap()
+
+  def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise: float) -> None:
+    self.map.update(long_enabled, long_override, v_ego, a_ego, v_cruise)
+    self.vision.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)

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

@@ -0,0 +1,58 @@
+"""
+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 platform
+
+from openpilot.cereal import custom
+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.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
+
+MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
+
+
+class TestSmartCruiseControlMap:
+
+  def setup_method(self):
+    self.params = Params()
+    self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
+    self.reset_params()
+    self.scc_m = SmartCruiseControlMap()
+
+  def reset_params(self):
+    self.params.put_bool("SmartCruiseControlMap", True, block=True)
+
+    # TODO-SP: mock data from gpsLocation
+    self.params.put("LastGPSPosition", "{}", block=True)
+    self.params.put("MapTargetVelocities", "{}", block=True)
+
+  def test_initial_state(self):
+    assert self.scc_m.state == VisionState.disabled
+    assert not self.scc_m.is_active
+    assert self.scc_m.output_v_target == V_CRUISE_UNSET
+    assert self.scc_m.output_a_target == 0.
+
+  def test_system_disabled(self):
+    self.params.put_bool("SmartCruiseControlMap", False, block=True)
+    self.scc_m.enabled = self.params.get_bool("SmartCruiseControlMap")
+
+    for _ in range(int(10. / DT_MDL)):
+      self.scc_m.update(True, False, 0., 0., 0.)
+    assert self.scc_m.state == VisionState.disabled
+    assert not self.scc_m.is_active
+
+  def test_disabled(self):
+    for _ in range(int(10. / DT_MDL)):
+      self.scc_m.update(False, False, 0., 0., 0.)
+    assert self.scc_m.state == VisionState.disabled
+
+  def test_transition_disabled_to_enabled(self):
+    for _ in range(int(10. / DT_MDL)):
+      self.scc_m.update(True, False, 0., 0., 0.)
+    assert self.scc_m.state == VisionState.enabled
+
+  # TODO-SP: mock data from modelV2 to test other states

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

@@ -0,0 +1,214 @@
+"""
+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
+import pytest
+
+import openpilot.cereal.messaging as messaging
+from openpilot.cereal import custom, log
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision, _ENTERING_PRED_LAT_ACC_TH
+
+VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
+
+
+def _th_above_f32(th: float) -> float:
+  """
+  Return the next representable float32 *above* `th`.
+  This avoids flaky comparisons around thresholds due to float32 rounding.
+  """
+  th32 = np.float32(th)
+  above32 = np.nextafter(th32, np.float32(np.inf), dtype=np.float32)
+  return float(above32)
+
+
+def _build_single_spike_filtered(n: int, base: float = 1.0) -> np.ndarray:
+  """
+  Create an array where max() is >= threshold but p97 is < threshold.
+  This demonstrates the behavior difference vs np.amax().
+
+  Note: We intentionally construct using float32-representable values to match
+  the data path through cereal/capnp.
+  """
+  th = float(_ENTERING_PRED_LAT_ACC_TH)
+  th32 = float(np.float32(th))
+
+  # numpy percentile default is linear interpolation: idx=(n-1)*p/100
+  idx = (n - 1) * 0.97
+  w = float(idx - np.floor(idx))
+
+  base32 = float(np.float32(base))
+
+  # Choose spike so that p97 = base + w*(spike-base) < th
+  # -> spike < base + (th-base)/w. Use a margin (0.9) and ensure spike >= th.
+  if w == 0.0:
+    spike = th32 + 1.0
+  else:
+    spike = base32 + (th32 - base32) / w * 0.9
+    spike = max(spike, th32 + 0.01)
+
+  arr = np.full(n, base32, dtype=np.float32)
+  arr[-1] = np.float32(spike)
+  return arr
+
+
+def generate_modelV2():
+  model = messaging.new_message('modelV2')
+  position = log.XYZTData.new_message()
+  speed = 30
+  position.x = [float(x) for x in (speed + 0.5) * np.array(ModelConstants.T_IDXS)]
+  model.modelV2.position = position
+  orientation = log.XYZTData.new_message()
+  curvature = 0.05
+  orientation.x = [float(curvature) for _ in ModelConstants.T_IDXS]
+  orientation.y = [0.0 for _ in ModelConstants.T_IDXS]
+  model.modelV2.orientation = orientation
+  orientationRate = log.XYZTData.new_message()
+  orientationRate.z = [float(z) for z in ModelConstants.T_IDXS]
+  model.modelV2.orientationRate = orientationRate
+  velocity = log.XYZTData.new_message()
+  velocity.x = [float(x) for x in (speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
+  velocity.x[0] = float(speed)  # always start at current speed
+  model.modelV2.velocity = velocity
+  acceleration = log.XYZTData.new_message()
+  acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
+  acceleration.y = [float(y) for y in np.zeros_like(ModelConstants.T_IDXS)]
+  model.modelV2.acceleration = acceleration
+
+  return model
+
+
+def generate_carState():
+  car_state = messaging.new_message('carState')
+  speed = 30
+  v_cruise = 50
+  car_state.carState.vEgo = float(speed)
+  car_state.carState.standstill = False
+  car_state.carState.vCruise = float(v_cruise * 3.6)
+
+  return car_state
+
+
+def generate_controlsState():
+  controls_state = messaging.new_message('controlsState')
+  controls_state.controlsState.curvature = 0.05
+
+  return controls_state
+
+
+class TestSmartCruiseControlVision:
+
+  def setup_method(self):
+    self.params = Params()
+    self.reset_params()
+    self.scc_v = SmartCruiseControlVision()
+
+    mdl = generate_modelV2()
+    cs = generate_carState()
+    controls_state = generate_controlsState()
+    self.sm = {'modelV2': mdl.modelV2, 'carState': cs.carState, 'controlsState': controls_state.controlsState}
+
+  def reset_params(self):
+    self.params.put_bool("SmartCruiseControlVision", True, block=True)
+
+  def test_initial_state(self):
+    assert self.scc_v.state == VisionState.disabled
+    assert not self.scc_v.is_active
+    assert self.scc_v.output_v_target == V_CRUISE_UNSET
+    assert self.scc_v.output_a_target == 0.
+
+  def test_system_disabled(self):
+    self.params.put_bool("SmartCruiseControlVision", False, block=True)
+    self.scc_v.enabled = self.params.get_bool("SmartCruiseControlVision")
+
+    for _ in range(int(10. / DT_MDL)):
+      self.scc_v.update(self.sm, True, False, 0., 0., 0.)
+    assert self.scc_v.state == VisionState.disabled
+    assert not self.scc_v.is_active
+
+  def test_disabled(self):
+    for _ in range(int(10. / DT_MDL)):
+      self.scc_v.update(self.sm, False, False, 0., 0., 0.)
+    assert self.scc_v.state == VisionState.disabled
+
+  def test_transition_disabled_to_enabled(self):
+    for _ in range(int(10. / DT_MDL)):
+      self.scc_v.update(self.sm, True, False, 0., 0., 0.)
+    assert self.scc_v.state == VisionState.enabled
+
+  @pytest.mark.parametrize(
+    "case, should_enter",
+    [
+      ("p97_just_above_threshold", True),
+      ("single_spike_filtered", False),
+      ("persistent_high_values", True),
+    ],
+    ids=[
+      "p97>threshold_enters",
+      "single_spike_max_large_but_p97_below_threshold",
+      "high_values_persist_trigger_entering",
+    ],
+  )
+  def test_max_pred_lat_acc_uses_p97_and_threshold(self, case, should_enter):
+    n = len(ModelConstants.T_IDXS)
+    th = float(_ENTERING_PRED_LAT_ACC_TH)
+
+    if case == "p97_just_above_threshold":
+      # Use the next representable float32 above threshold to avoid float32 rounding flakiness.
+      val = _th_above_f32(th)
+      pred_lat_accels = np.full(n, np.float32(val), dtype=np.float32)
+
+    elif case == "single_spike_filtered":
+      pred_lat_accels = _build_single_spike_filtered(n, base=1.0)
+
+    elif case == "persistent_high_values":
+      # Make enough "high" samples so p97 is driven by the persistent trend, not a single outlier.
+      high_count = max(2, int(np.ceil(n * 0.03)) + 1)
+      pred_lat_accels = np.full(n, np.float32(1.0), dtype=np.float32)
+      pred_lat_accels[-high_count:] = np.float32(2.0)
+      pred_lat_accels[-1] = np.float32(8.0)  # keep one big outlier too
+
+    else:
+      raise AssertionError(f"Unknown case: {case}")
+
+    # Override model predictions so:
+    # predicted_lat_accels = abs(orientationRate.z) * velocity.x == pred_lat_accels
+    mdl = generate_modelV2()
+    mdl.modelV2.velocity.x = [1.0 for _ in range(n)]
+    mdl.modelV2.orientationRate.z = [float(x) for x in pred_lat_accels]
+    self.sm["modelV2"] = mdl.modelV2
+
+    v_ego = float(MIN_V + 5.0)
+
+    # 1st update: disabled -> enabled
+    self.scc_v.update(self.sm, True, False, v_ego, 0.0, 0.0)
+    # 2nd update: evaluate entering condition from enabled state
+    self.scc_v.update(self.sm, True, False, v_ego, 0.0, 0.0)
+
+    # Controller does percentile on numpy float64 arrays (values already quantized by capnp),
+    # so compute expected in float64 to match behavior and avoid interpolation/rounding deltas.
+    expected_p97 = float(np.percentile(pred_lat_accels.astype(np.float64), 97))
+
+    # allow tiny numeric differences due to float conversions/interpolation
+    assert np.isclose(self.scc_v.max_pred_lat_acc, expected_p97, rtol=1e-6, atol=1e-5)
+
+    if should_enter:
+      # We assert entering primarily by state (this is the actual intended behavior).
+      assert self.scc_v.state == VisionState.entering
+      # Optional sanity: should be >= threshold with some margin (since we used nextafter above threshold).
+      assert self.scc_v.max_pred_lat_acc > th
+
+    else:
+      # Difference vs np.amax(): max can be above threshold, but p97 stays below it.
+      assert float(np.max(pred_lat_accels)) >= th
+      assert self.scc_v.max_pred_lat_acc < th
+      assert self.scc_v.state == VisionState.enabled
+
+  # TODO-SP: mock modelV2 data to test other states

openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py

@@ -0,0 +1,203 @@
+"""
+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
+
+import openpilot.cereal.messaging as messaging
+from openpilot.cereal import custom
+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.selfdrive.controls.lib.smart_cruise_control import MIN_V
+
+VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
+
+ACTIVE_STATES = (VisionState.entering, VisionState.turning, VisionState.leaving)
+ENABLED_STATES = (VisionState.enabled, VisionState.overriding, *ACTIVE_STATES)
+
+_ENTERING_PRED_LAT_ACC_TH = 1.3  # Predicted Lat Acc threshold to trigger entering turn state.
+_ABORT_ENTERING_PRED_LAT_ACC_TH = 1.1  # Predicted Lat Acc threshold to abort entering state if speed drops.
+
+_TURNING_LAT_ACC_TH = 1.6  # Lat Acc threshold to trigger turning state.
+
+_LEAVING_LAT_ACC_TH = 1.3  # Lat Acc threshold to trigger leaving turn state.
+_FINISH_LAT_ACC_TH = 1.1  # Lat Acc threshold to trigger the end of the turn cycle.
+
+_A_LAT_REG_MAX = 2.  # Maximum lateral acceleration
+
+_NO_OVERSHOOT_TIME_HORIZON = 4.  # s. Time to use for velocity desired based on a_target when not overshooting.
+
+# Lookup table for the minimum smooth deceleration during the ENTERING state
+# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead.
+_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.]  # min decel value allowed on ENTERING state
+_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.]  # absolute value of lat acc ahead
+
+# Lookup table for the acceleration for the TURNING state
+# depending on the current lateral acceleration of the vehicle.
+_TURNING_ACC_V = [0.5, 0., -0.4]  # acc value
+_TURNING_ACC_BP = [1.5, 2.3, 3.]  # absolute value of current lat acc
+
+_LEAVING_ACC = 0.5  # Conformable acceleration to regain speed while leaving a turn.
+
+
+class SmartCruiseControlVision:
+  v_target: float = 0
+  a_target: float = 0.
+  v_ego: float = 0.
+  a_ego: float = 0.
+  output_v_target: float = V_CRUISE_UNSET
+  output_a_target: float = 0.
+
+  def __init__(self):
+    self.params = Params()
+    self.frame = -1
+    self.long_enabled = False
+    self.long_override = False
+    self.is_enabled = False
+    self.is_active = False
+    self.enabled = self.params.get_bool("SmartCruiseControlVision")
+    self.v_cruise_setpoint = 0.
+
+    self.state = VisionState.disabled
+    self.current_lat_acc = 0.
+    self.max_pred_lat_acc = 0.
+
+  def get_a_target_from_control(self) -> float:
+    return self.a_target
+
+  def get_v_target_from_control(self) -> float:
+    if self.is_active:
+      return max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON
+
+    return V_CRUISE_UNSET
+
+  def _update_params(self) -> None:
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.enabled = self.params.get_bool("SmartCruiseControlVision")
+
+  def _update_calculations(self, sm: messaging.SubMaster) -> None:
+    if not self.long_enabled:
+      return
+    else:
+      rate_plan = np.array(np.abs(sm['modelV2'].orientationRate.z))
+      vel_plan = np.array(sm['modelV2'].velocity.x)
+
+      self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature)
+
+      # get the maximum lat accel from the model
+      predicted_lat_accels = rate_plan * vel_plan
+      self.max_pred_lat_acc = np.percentile(predicted_lat_accels, 97)
+
+      # get the maximum curve based on the current velocity
+      v_ego = max(self.v_ego, 0.1)  # ensure a value greater than 0 for calculations
+      max_curve = self.max_pred_lat_acc / (v_ego**2)
+
+      # Get the target velocity for the maximum curve
+      self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5
+
+  def _update_state_machine(self) -> tuple[bool, bool]:
+    # ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING
+    if self.state != VisionState.disabled:
+      # longitudinal and feature disable always have priority in a non-disabled state
+      if not self.long_enabled or not self.enabled:
+        self.state = VisionState.disabled
+      elif self.long_override:
+        self.state = VisionState.overriding
+
+      else:
+        # ENABLED
+        if self.state == VisionState.enabled:
+          # Do not enter a turn control cycle if the speed is low.
+          if self.v_ego <= MIN_V:
+            pass
+          # If significant lateral acceleration is predicted ahead, then move to Entering turn state.
+          elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
+            self.state = VisionState.entering
+
+        # OVERRIDING
+        elif self.state == VisionState.overriding:
+          if not self.long_override:
+            self.state = VisionState.enabled
+
+        # ENTERING
+        elif self.state == VisionState.entering:
+          # Transition to Turning if current lateral acceleration is over the threshold.
+          if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
+            self.state = VisionState.turning
+          # Abort if the predicted lateral acceleration drops
+          elif self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH:
+            self.state = VisionState.enabled
+
+        # TURNING
+        elif self.state == VisionState.turning:
+          # Transition to Leaving if current lateral acceleration drops below a threshold.
+          if self.current_lat_acc <= _LEAVING_LAT_ACC_TH:
+            self.state = VisionState.leaving
+
+        # LEAVING
+        elif self.state == VisionState.leaving:
+          # Transition back to Turning if current lateral acceleration goes back over the threshold.
+          if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
+            self.state = VisionState.turning
+          # Finish if current lateral acceleration goes below a threshold.
+          elif self.current_lat_acc < _FINISH_LAT_ACC_TH:
+            self.state = VisionState.enabled
+
+    # DISABLED
+    elif self.state == VisionState.disabled:
+      if self.long_enabled and self.enabled:
+        if self.long_override:
+          self.state = VisionState.overriding
+        else:
+          self.state = VisionState.enabled
+
+    enabled = self.state in ENABLED_STATES
+    active = self.state in ACTIVE_STATES
+
+    return enabled, active
+
+  def _update_solution(self) -> float:
+    # DISABLED, ENABLED, OVERRIDING
+    if self.state not in ACTIVE_STATES:
+      # when not overshooting, calculate v_turn as the speed at the prediction horizon when following
+      # the smooth deceleration.
+      a_target = self.a_ego
+    # ENTERING
+    elif self.state == VisionState.entering:
+      # when not overshooting, target a smooth deceleration in preparation for a sharp turn to come.
+      a_target = np.interp(self.max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V)
+    # TURNING
+    elif self.state == VisionState.turning:
+      # When turning, we provide a target acceleration that is comfortable for the lateral acceleration felt.
+      a_target = np.interp(self.current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V)
+    # LEAVING
+    elif self.state == VisionState.leaving:
+      # When leaving, we provide a comfortable acceleration to regain speed.
+      a_target = _LEAVING_ACC
+    else:
+      raise NotImplementedError(f"SCC-V state not supported: {self.state}")
+
+    return a_target
+
+  def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float,
+             v_cruise_setpoint: float) -> None:
+    self.long_enabled = long_enabled
+    self.long_override = long_override
+    self.v_ego = v_ego
+    self.a_ego = a_ego
+    self.v_cruise_setpoint = v_cruise_setpoint
+
+    self._update_params()
+    self._update_calculations(sm)
+
+    self.is_enabled, self.is_active = self._update_state_machine()
+    self.a_target = self._update_solution()
+
+    self.output_v_target = self.get_v_target_from_control()
+    self.output_a_target = self.get_a_target_from_control()
+
+    self.frame += 1

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

@@ -0,0 +1,19 @@
+"""
+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.
+"""
+LIMIT_ADAPT_ACC = -1.  # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits.
+LIMIT_MAX_MAP_DATA_AGE = 10.  # s Maximum time to hold to map data, then consider it invalid inside limits controllers.
+
+# Speed Limit Assist constants
+PCM_LONG_REQUIRED_MAX_SET_SPEED = {
+  True: (33.3333, 36.1111),  # km/h, (120, 130)
+  False: (31.2928, 35.7632),  # mph, (70, 80)
+}
+
+CONFIRM_SPEED_THRESHOLD = {
+  True: 80,   # km/h
+  False: 50,  # mph
+}

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

@@ -0,0 +1,29 @@
+"""
+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.sunnypilot import IntEnumBase
+
+
+class Policy(IntEnumBase):
+  car_state_only = 0
+  map_data_only = 1
+  car_state_priority = 2
+  map_data_priority = 3
+  combined = 4
+
+
+class OffsetType(IntEnumBase):
+  off = 0
+  fixed = 1
+  percentage = 2
+
+
+class Mode(IntEnumBase):
+  off = 0
+  information = 1
+  warning = 2
+  assist = 3

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

@@ -0,0 +1,45 @@
+"""
+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 custom
+from opendbc.car.structs import car
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode as SpeedLimitMode
+
+
+def compare_cluster_target(v_cruise_cluster: float, target_set_speed: float, is_metric: bool) -> tuple[bool, bool]:
+  speed_conv = CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH
+  v_cruise_cluster_conv = round(v_cruise_cluster * speed_conv)
+  target_set_speed_conv = round(target_set_speed * speed_conv)
+
+  req_plus = v_cruise_cluster_conv < target_set_speed_conv
+  req_minus = v_cruise_cluster_conv > target_set_speed_conv
+
+  return req_plus, req_minus
+
+
+def set_speed_limit_assist_availability(CP: car.CarParams, CP_SP: custom.CarParamsSP, params: Params = None) -> bool:
+  if params is None:
+    params = Params()
+
+  is_release = params.get_bool("IsReleaseSpBranch")
+  disallow_in_release = CP.brand == "tesla" and is_release
+  always_disallow = CP.brand == "rivian"
+  allowed = True
+
+  if disallow_in_release or always_disallow:
+    allowed = False
+
+  if not CP.openpilotLongitudinalControl and CP_SP.pcmCruiseSpeed:
+    allowed = False
+
+  if not allowed:
+    if params.get("SpeedLimitMode", return_default=True) == SpeedLimitMode.assist:
+      params.put("SpeedLimitMode", int(SpeedLimitMode.warning), block=True)
+
+  return allowed

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

@@ -0,0 +1,415 @@
+"""
+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 time
+
+from openpilot.cereal import custom
+from opendbc.car.structs import car
+from openpilot.common.params import Params
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target, set_speed_limit_assist_availability
+
+ButtonType = car.CarState.ButtonEvent.Type
+EventNameSP = custom.OnroadEventSP.EventName
+SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
+
+ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting)
+ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, *ACTIVE_STATES)
+
+DISABLED_GUARD_PERIOD = 0.5  # secs.
+# secs. Time to wait after activation before considering temp deactivation signal.
+PRE_ACTIVE_GUARD_PERIOD = {
+  True: 15,
+  False: 5,
+}
+SPEED_LIMIT_CHANGED_HOLD_PERIOD = 1  # secs. Time to wait after speed limit change before switching to preActive.
+
+LIMIT_MIN_ACC = -1.5  # m/s^2 Maximum deceleration allowed for limit controllers to provide.
+LIMIT_MAX_ACC = 1.0   # m/s^2 Maximum acceleration allowed for limit controllers to provide while active.
+LIMIT_MIN_SPEED = 8.33  # m/s, Minimum speed limit to provide as solution on limit controllers.
+LIMIT_SPEED_OFFSET_TH = -1.  # m/s Maximum offset between speed limit and current speed for adapting state.
+V_CRUISE_UNSET = 255.
+
+CRUISE_BUTTONS_PLUS = (ButtonType.accelCruise, ButtonType.resumeCruise)
+CRUISE_BUTTONS_MINUS = (ButtonType.decelCruise, ButtonType.setCruise)
+CRUISE_BUTTON_CONFIRM_HOLD = 0.5  # secs.
+
+
+class SpeedLimitAssist:
+  _speed_limit_final_last: float
+  _distance: float
+  v_ego: float
+  a_ego: float
+  v_offset: float
+
+  def __init__(self, CP: car.CarParams, CP_SP: custom.CarParamsSP):
+    self.params = Params()
+    self.CP = CP
+    self.CP_SP = CP_SP
+    self.frame = -1
+    self.long_engaged_timer = 0
+    self.pre_active_timer = 0
+    self.is_metric = self.params.get_bool("IsMetric")
+    set_speed_limit_assist_availability(self.CP, self.CP_SP, self.params)
+    self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
+    self.long_enabled = False
+    self.long_enabled_prev = False
+    self.is_enabled = False
+    self.is_active = False
+    self.output_v_target = V_CRUISE_UNSET
+    self.output_a_target = 0.
+    self.v_ego = 0.
+    self.a_ego = 0.
+    self.v_offset = 0.
+    self.target_set_speed_conv = 0
+    self.prev_target_set_speed_conv = 0
+    self.v_cruise_cluster = 0.
+    self.v_cruise_cluster_prev = 0.
+    self.v_cruise_cluster_conv = 0
+    self.prev_v_cruise_cluster_conv = 0
+    self._has_speed_limit = False
+    self._speed_limit = 0.
+    self._speed_limit_final_last = 0.
+    self.speed_limit_prev = 0.
+    self.speed_limit_final_last_conv = 0
+    self.prev_speed_limit_final_last_conv = 0
+    self._distance = 0.
+    self.state = SpeedLimitAssistState.disabled
+    self._state_prev = SpeedLimitAssistState.disabled
+    self.pcm_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise
+
+    self._plus_hold = 0.
+    self._minus_hold = 0.
+    self._last_carstate_ts = 0.
+
+    # TODO-SP: SLA's own output_a_target for planner
+    # Solution functions mapped to respective states
+    self.acceleration_solutions = {
+      SpeedLimitAssistState.disabled: self.get_current_acceleration_as_target,
+      SpeedLimitAssistState.inactive: self.get_current_acceleration_as_target,
+      SpeedLimitAssistState.preActive: self.get_current_acceleration_as_target,
+      SpeedLimitAssistState.pending: self.get_current_acceleration_as_target,
+      SpeedLimitAssistState.adapting: self.get_adapting_state_target_acceleration,
+      SpeedLimitAssistState.active: self.get_active_state_target_acceleration,
+    }
+
+  @property
+  def speed_limit_changed(self) -> bool:
+    return self._has_speed_limit and bool(self._speed_limit != self.speed_limit_prev)
+
+  @property
+  def v_cruise_cluster_changed(self) -> bool:
+    return bool(self.v_cruise_cluster_conv != self.prev_v_cruise_cluster_conv)
+
+  @property
+  def target_set_speed_confirmed(self) -> bool:
+    return bool(self.v_cruise_cluster_conv == self.target_set_speed_conv)
+
+  @property
+  def v_cruise_cluster_below_confirm_speed_threshold(self) -> bool:
+    return bool(self.v_cruise_cluster_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric])
+
+  def update_active_event(self, events_sp: EventsSP) -> None:
+    if self.v_cruise_cluster_below_confirm_speed_threshold:
+      events_sp.add(EventNameSP.speedLimitChanged)
+    else:
+      events_sp.add(EventNameSP.speedLimitActive)
+
+  def get_v_target_from_control(self) -> float:
+    if self._has_speed_limit:
+      if self.pcm_op_long and self.is_enabled:
+        return self._speed_limit_final_last
+      if not self.pcm_op_long and self.is_active:
+        return self._speed_limit_final_last
+
+    # Fallback
+    return V_CRUISE_UNSET
+
+  # TODO-SP: SLA's own output_a_target for planner
+  def get_a_target_from_control(self) -> float:
+    return self.a_ego
+
+  def update_params(self) -> None:
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.is_metric = self.params.get_bool("IsMetric")
+      set_speed_limit_assist_availability(self.CP, self.CP_SP, self.params)
+      self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
+
+  def update_car_state(self, CS: car.CarState) -> None:
+    now = time.monotonic()
+    self._last_carstate_ts = now
+
+    for b in CS.buttonEvents:
+      if not b.pressed:
+        if b.type in CRUISE_BUTTONS_PLUS:
+          self._plus_hold = max(self._plus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
+        elif b.type in CRUISE_BUTTONS_MINUS:
+          self._minus_hold = max(self._minus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
+
+  def _get_button_release(self, req_plus: bool, req_minus: bool) -> bool:
+    now = time.monotonic()
+    if req_plus and now <= self._plus_hold:
+      self._plus_hold = 0.
+      return True
+    elif req_minus and now <= self._minus_hold:
+      self._minus_hold = 0.
+      return True
+
+    # expired
+    if now > self._plus_hold:
+      self._plus_hold = 0.
+    if now > self._minus_hold:
+      self._minus_hold = 0.
+    return False
+
+  def update_calculations(self, v_cruise_cluster: float) -> None:
+    speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH
+    self.v_cruise_cluster = v_cruise_cluster
+
+    # Update current velocity offset (error)
+    self.v_offset = self._speed_limit_final_last - self.v_ego
+
+    self.speed_limit_final_last_conv = round(self._speed_limit_final_last * speed_conv)
+    self.v_cruise_cluster_conv = round(self.v_cruise_cluster * speed_conv)
+
+    cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.is_metric]
+    pcm_long_required_max = cst_low if self._has_speed_limit and self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric] else \
+                            cst_high
+    pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv)
+
+    self.target_set_speed_conv = pcm_long_required_max_set_speed_conv if self.pcm_op_long else self.speed_limit_final_last_conv
+
+  @property
+  def apply_confirm_speed_threshold(self) -> bool:
+    # below CST: always require user confirmation
+    if self.v_cruise_cluster_below_confirm_speed_threshold:
+      return True
+
+    # at/above CST:
+    # - new speed limit >= CST: auto change
+    # - new speed limit < CST: user confirmation required
+    return bool(self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric])
+
+  def get_current_acceleration_as_target(self) -> float:
+    return self.a_ego
+
+  def get_adapting_state_target_acceleration(self) -> float:
+    if self._distance > 0:
+      return (self._speed_limit_final_last ** 2 - self.v_ego ** 2) / (2. * self._distance)
+
+    return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
+
+  def get_active_state_target_acceleration(self) -> float:
+    return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
+
+  def _update_confirmed_state(self):
+    if self._has_speed_limit:
+      if self.v_offset < LIMIT_SPEED_OFFSET_TH:
+        self.state = SpeedLimitAssistState.adapting
+      else:
+        self.state = SpeedLimitAssistState.active
+    else:
+      self.state = SpeedLimitAssistState.pending
+
+  def _update_non_pcm_long_confirmed_state(self) -> bool:
+    if self.target_set_speed_confirmed:
+      return True
+
+    if self.state != SpeedLimitAssistState.preActive:
+      return False
+
+    req_plus, req_minus = compare_cluster_target(self.v_cruise_cluster, self._speed_limit_final_last, self.is_metric)
+
+    return self._get_button_release(req_plus, req_minus)
+
+  def update_state_machine_pcm_op_long(self):
+    self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
+    self.pre_active_timer = max(0, self.pre_active_timer - 1)
+
+    # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE
+    if self.state != SpeedLimitAssistState.disabled:
+      if not self.long_enabled or not self.enabled:
+        self.state = SpeedLimitAssistState.disabled
+
+      else:
+        # ACTIVE
+        if self.state == SpeedLimitAssistState.active:
+          if self.v_cruise_cluster_changed:
+            self.state = SpeedLimitAssistState.inactive
+          elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
+            self.state = SpeedLimitAssistState.preActive
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
+          elif self._has_speed_limit and self.v_offset < LIMIT_SPEED_OFFSET_TH:
+            self.state = SpeedLimitAssistState.adapting
+
+        # ADAPTING
+        elif self.state == SpeedLimitAssistState.adapting:
+          if self.v_cruise_cluster_changed:
+            self.state = SpeedLimitAssistState.inactive
+          elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
+            self.state = SpeedLimitAssistState.preActive
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
+          elif self.v_offset >= LIMIT_SPEED_OFFSET_TH:
+            self.state = SpeedLimitAssistState.active
+
+        # PENDING
+        elif self.state == SpeedLimitAssistState.pending:
+          if self.target_set_speed_confirmed:
+            self._update_confirmed_state()
+          elif self.speed_limit_changed:
+            self.state = SpeedLimitAssistState.preActive
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
+
+        # PRE_ACTIVE
+        elif self.state == SpeedLimitAssistState.preActive:
+          if self.target_set_speed_confirmed:
+            self._update_confirmed_state()
+          elif self.pre_active_timer <= 0:
+            # Timeout - session ended
+            self.state = SpeedLimitAssistState.inactive
+
+        # INACTIVE
+        elif self.state == SpeedLimitAssistState.inactive:
+          pass
+
+    # DISABLED
+    elif self.state == SpeedLimitAssistState.disabled:
+      if self.long_enabled and self.enabled:
+        # start or reset preActive timer if initially enabled or manual set speed change detected
+        if not self.long_enabled_prev or self.v_cruise_cluster_changed:
+          self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
+
+        elif self.long_engaged_timer <= 0:
+          if self.target_set_speed_confirmed:
+            self._update_confirmed_state()
+          elif self._has_speed_limit:
+            self.state = SpeedLimitAssistState.preActive
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
+          else:
+            self.state = SpeedLimitAssistState.pending
+
+    enabled = self.state in ENABLED_STATES
+    active = self.state in ACTIVE_STATES
+
+    return enabled, active
+
+  def update_state_machine_non_pcm_long(self):
+    self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
+    self.pre_active_timer = max(0, self.pre_active_timer - 1)
+
+    # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE
+    if self.state != SpeedLimitAssistState.disabled:
+      if not self.long_enabled or not self.enabled:
+        self.state = SpeedLimitAssistState.disabled
+
+      else:
+        # ACTIVE
+        if self.state == SpeedLimitAssistState.active:
+          if self.v_cruise_cluster_changed:
+            self.state = SpeedLimitAssistState.inactive
+
+          elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
+            self.state = SpeedLimitAssistState.preActive
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
+
+        # PRE_ACTIVE
+        elif self.state == SpeedLimitAssistState.preActive:
+          if self._update_non_pcm_long_confirmed_state():
+            self.state = SpeedLimitAssistState.active
+          elif self.pre_active_timer <= 0:
+            # Timeout - session ended
+            self.state = SpeedLimitAssistState.inactive
+
+        # INACTIVE
+        elif self.state == SpeedLimitAssistState.inactive:
+          if self.speed_limit_changed:
+            self.state = SpeedLimitAssistState.preActive
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
+          elif self._update_non_pcm_long_confirmed_state():
+            self.state = SpeedLimitAssistState.active
+
+    # DISABLED
+    elif self.state == SpeedLimitAssistState.disabled:
+      if self.long_enabled and self.enabled:
+        # start or reset preActive timer if initially enabled or manual set speed change detected
+        if not self.long_enabled_prev or self.v_cruise_cluster_changed:
+          self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
+
+        elif self.long_engaged_timer <= 0:
+          if self._update_non_pcm_long_confirmed_state():
+            self.state = SpeedLimitAssistState.active
+          elif self._has_speed_limit:
+            self.state = SpeedLimitAssistState.preActive
+            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
+          else:
+            self.state = SpeedLimitAssistState.inactive
+
+    enabled = self.state in ENABLED_STATES
+    active = self.state in ACTIVE_STATES
+
+    return enabled, active
+
+  def update_events(self, events_sp: EventsSP) -> None:
+    if self.state == SpeedLimitAssistState.preActive:
+      events_sp.add(EventNameSP.speedLimitPreActive)
+
+    if self.state == SpeedLimitAssistState.pending and self._state_prev != SpeedLimitAssistState.pending:
+      events_sp.add(EventNameSP.speedLimitPending)
+
+    if self.is_active:
+      if self._state_prev not in ACTIVE_STATES:
+        self.update_active_event(events_sp)
+
+      # only notify if we acquire a valid speed limit
+      # do not check has_speed_limit here
+      elif self._speed_limit != self.speed_limit_prev:
+        if self.speed_limit_prev <= 0:
+          self.update_active_event(events_sp)
+        elif self.speed_limit_prev > 0 and self._speed_limit > 0:
+          self.update_active_event(events_sp)
+
+  def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_cluster: float, speed_limit: float,
+             speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP) -> None:
+    self.long_enabled = long_enabled
+    self.v_ego = v_ego
+    self.a_ego = a_ego
+
+    self._has_speed_limit = has_speed_limit
+    self._speed_limit = speed_limit
+    self._speed_limit_final_last = speed_limit_final_last
+    self._distance = distance
+
+    self.update_params()
+    self.update_calculations(v_cruise_cluster)
+
+    self._state_prev = self.state
+    if self.pcm_op_long:
+      self.is_enabled, self.is_active = self.update_state_machine_pcm_op_long()
+    else:
+      self.is_enabled, self.is_active = self.update_state_machine_non_pcm_long()
+
+    self.update_events(events_sp)
+
+    # Update change tracking variables
+    self.speed_limit_prev = self._speed_limit
+    self.v_cruise_cluster_prev = self.v_cruise_cluster
+    self.long_enabled_prev = self.long_enabled
+    self.prev_target_set_speed_conv = self.target_set_speed_conv
+    self.prev_v_cruise_cluster_conv = self.v_cruise_cluster_conv
+    self.prev_speed_limit_final_last_conv = self.speed_limit_final_last_conv
+
+    self.output_v_target = self.get_v_target_from_control()
+    self.output_a_target = self.get_a_target_from_control()
+
+    self.frame += 1

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

@@ -0,0 +1,190 @@
+"""
+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 time
+
+import openpilot.cereal.messaging as messaging
+from openpilot.cereal import custom
+from openpilot.common.constants import CV
+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.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
+
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
+
+ALL_SOURCES = tuple(SpeedLimitSource.schema.enumerants.values())
+
+
+class SpeedLimitResolver:
+  limit_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float]
+  distance_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float]
+  v_ego: float
+  speed_limit: float
+  speed_limit_last: float
+  speed_limit_final: float
+  speed_limit_final_last: float
+  distance: float
+  source: custom.LongitudinalPlanSP.SpeedLimit.Source
+  speed_limit_offset: float
+
+  def __init__(self):
+    self.params = Params()
+    self.frame = -1
+
+    self._gps_location_service = get_gps_location_service(self.params)
+    self.limit_solutions = {}  # Store for speed limit solutions from different sources
+    self.distance_solutions = {}  # Store for distance to current speed limit start for different sources
+
+    self.policy = self.params.get("SpeedLimitPolicy", return_default=True)
+    self.policy = get_sanitize_int_param(
+      "SpeedLimitPolicy",
+      Policy.min().value,
+      Policy.max().value,
+      self.params
+    )
+    self._policy_to_sources_map = {
+      Policy.car_state_only: [SpeedLimitSource.car],
+      Policy.map_data_only: [SpeedLimitSource.map],
+      Policy.car_state_priority: [SpeedLimitSource.car, SpeedLimitSource.map],
+      Policy.map_data_priority: [SpeedLimitSource.map, SpeedLimitSource.car],
+      Policy.combined: [SpeedLimitSource.car, SpeedLimitSource.map],
+    }
+    self.source = SpeedLimitSource.none
+    for source in ALL_SOURCES:
+      self._reset_limit_sources(source)
+
+    self.is_metric = self.params.get_bool("IsMetric")
+    self.offset_type = get_sanitize_int_param(
+      "SpeedLimitOffsetType",
+      OffsetType.min().value,
+      OffsetType.max().value,
+      self.params
+    )
+    self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
+
+    self.speed_limit = 0.
+    self.speed_limit_last = 0.
+    self.speed_limit_final = 0.
+    self.speed_limit_final_last = 0.
+    self.speed_limit_offset = 0.
+
+  def update_speed_limit_states(self) -> None:
+    self.speed_limit_final = self.speed_limit + self.speed_limit_offset
+
+    if self.speed_limit > 0.:
+      self.speed_limit_last = self.speed_limit
+      self.speed_limit_final_last = self.speed_limit_final
+
+  @property
+  def speed_limit_valid(self) -> bool:
+    return self.speed_limit > 0.
+
+  @property
+  def speed_limit_last_valid(self) -> bool:
+    return self.speed_limit_last > 0.
+
+  def update_params(self):
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      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)
+
+  def _get_speed_limit_offset(self) -> float:
+    if self.offset_type == OffsetType.off:
+      return 0
+    elif self.offset_type == OffsetType.fixed:
+      return float(self.offset_value * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS))
+    elif self.offset_type == OffsetType.percentage:
+      return float(self.offset_value * 0.01 * self.speed_limit)
+    else:
+      raise NotImplementedError("Offset not supported")
+
+  def _reset_limit_sources(self, source: custom.LongitudinalPlanSP.SpeedLimit.Source) -> None:
+    self.limit_solutions[source] = 0.
+    self.distance_solutions[source] = 0.
+
+  def _get_from_car_state(self, sm: messaging.SubMaster) -> None:
+    self._reset_limit_sources(SpeedLimitSource.car)
+    self.limit_solutions[SpeedLimitSource.car] = sm['carStateSP'].speedLimit
+    self.distance_solutions[SpeedLimitSource.car] = 0.
+
+  def _get_from_map_data(self, sm: messaging.SubMaster) -> None:
+    self._reset_limit_sources(SpeedLimitSource.map)
+    self._process_map_data(sm)
+
+  def _process_map_data(self, sm: messaging.SubMaster) -> None:
+    gps_data = sm[self._gps_location_service]
+    map_data = sm['liveMapDataSP']
+
+    gps_fix_age = time.monotonic() - gps_data.unixTimestampMillis * 1e-3
+    if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE:
+      return
+
+    speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0.
+    next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0.
+
+    self._calculate_map_data_limits(sm, speed_limit, next_speed_limit)
+
+  def _calculate_map_data_limits(self, sm: messaging.SubMaster, speed_limit: float, next_speed_limit: float) -> None:
+    gps_data = sm[self._gps_location_service]
+    map_data = sm['liveMapDataSP']
+
+    distance_since_fix = self.v_ego * (time.monotonic() - gps_data.unixTimestampMillis * 1e-3)
+    distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix)
+
+    self.limit_solutions[SpeedLimitSource.map] = speed_limit
+    self.distance_solutions[SpeedLimitSource.map] = 0.
+
+    # FIXME-SP: this is not working as expected
+    if 0. < next_speed_limit < self.v_ego:
+      adapt_time = (next_speed_limit - self.v_ego) / LIMIT_ADAPT_ACC
+      adapt_distance = self.v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time ** 2
+
+      if distance_to_speed_limit_ahead <= adapt_distance:
+        self.limit_solutions[SpeedLimitSource.map] = next_speed_limit
+        self.distance_solutions[SpeedLimitSource.map] = distance_to_speed_limit_ahead
+
+  def _get_source_solution_according_to_policy(self) -> custom.LongitudinalPlanSP.SpeedLimit.Source:
+    sources_for_policy = self._policy_to_sources_map[self.policy]
+
+    if self.policy != Policy.combined:
+      # They are ordered in the order of preference, so we pick the first that's non-zero
+      for source in sources_for_policy:
+        if self.limit_solutions[source] > 0.:
+          return source
+      return SpeedLimitSource.none
+
+    sources_with_limits = [(s, limit) for s, limit in [(s, self.limit_solutions[s]) for s in sources_for_policy] if limit > 0.]
+    if sources_with_limits:
+      return min(sources_with_limits, key=lambda x: x[1])[0]
+
+    return SpeedLimitSource.none
+
+  def _resolve_limit_sources(self, sm: messaging.SubMaster) -> tuple[float, float, custom.LongitudinalPlanSP.SpeedLimit.Source]:
+    """Get limit solutions from each data source"""
+    self._get_from_car_state(sm)
+    self._get_from_map_data(sm)
+
+    source = self._get_source_solution_according_to_policy()
+    speed_limit = self.limit_solutions[source] if source else 0.
+    distance = self.distance_solutions[source] if source else 0.
+
+    return speed_limit, distance, source
+
+  def update(self, v_ego: float, sm: messaging.SubMaster) -> None:
+    self.v_ego = v_ego
+    self.update_params()
+
+    self.speed_limit, self.distance, self.source = self._resolve_limit_sources(sm)
+    self.speed_limit_offset = self._get_speed_limit_offset()
+
+    self.update_speed_limit_states()
+
+    self.frame += 1

openpilot/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py

@@ -0,0 +1,278 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import pytest
+
+from openpilot.cereal import custom
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.rivian.values import CAR as RIVIAN
+from opendbc.car.tesla.values import CAR as TESLA
+from opendbc.car.toyota.values import CAR as TOYOTA
+from openpilot.common.constants import CV
+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.selfdrive.car import interfaces as sunnypilot_interfaces
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist, \
+  PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+
+SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
+
+ALL_STATES = tuple(SpeedLimitAssistState.schema.enumerants.values())
+
+SPEED_LIMITS = {
+  'residential': 25 * CV.MPH_TO_MS,  # 25 mph
+  'city': 35 * CV.MPH_TO_MS,         # 35 mph
+  'highway': 65 * CV.MPH_TO_MS,      # 65 mph
+  'freeway': 80 * CV.MPH_TO_MS,      # 80 mph
+}
+
+DEFAULT_CAR = TOYOTA.TOYOTA_RAV4_TSS2
+
+
+@pytest.fixture
+def car_name(request):
+  return getattr(request, "param", DEFAULT_CAR)
+
+
+@pytest.fixture(autouse=True)
+def set_car_name_on_instance(request, car_name):
+  instance = getattr(request, "instance", None)
+  if instance:
+    instance.car_name = car_name
+
+
+class TestSpeedLimitAssist:
+
+  def setup_method(self, method):
+    self.params = Params()
+    self.reset_custom_params()
+    self.events_sp = EventsSP()
+    CI = self._setup_platform(self.car_name)
+    self.sla = SpeedLimitAssist(CI.CP, CI.CP_SP)
+    self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.sla.pcm_op_long] / DT_MDL)
+    self.pcm_long_max_set_speed = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric][1]  # use 80 MPH for now
+    self.speed_conv = CV.MS_TO_KPH if self.sla.is_metric else CV.MS_TO_MPH
+
+  def teardown_method(self, method):
+    self.reset_state()
+
+  def _setup_platform(self, car_name):
+    CarInterface = interfaces[car_name]
+    CP = CarInterface.get_non_essential_params(car_name)
+    CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
+    CI = CarInterface(CP, CP_SP)
+    CI.CP.openpilotLongitudinalControl = True  # always assume it's openpilot longitudinal
+    sunnypilot_interfaces.setup_interfaces(CI, self.params)
+    return CI
+
+  def reset_custom_params(self):
+    self.params.put("IsReleaseSpBranch", True, block=True)
+    self.params.put("SpeedLimitMode", int(Mode.assist), block=True)
+    self.params.put_bool("IsMetric", False, block=True)
+    self.params.put("SpeedLimitOffsetType", 0, block=True)
+    self.params.put("SpeedLimitValueOffset", 0, block=True)
+
+  def reset_state(self):
+    self.sla.state = SpeedLimitAssistState.disabled
+    self.sla.frame = -1
+    self.sla.last_op_engaged_frame = 0
+    self.sla.op_engaged = False
+    self.sla.op_engaged_prev = False
+    self.sla._speed_limit = 0.
+    self.sla.speed_limit_prev = 0.
+    self.sla.last_valid_speed_limit_offsetted = 0.
+    self.sla._distance = 0.
+    self.events_sp.clear()
+
+  def initialize_active_state(self, initialize_v_cruise):
+    self.sla.state = SpeedLimitAssistState.active
+    self.sla.v_cruise_cluster = initialize_v_cruise
+    self.sla.v_cruise_cluster_prev = initialize_v_cruise
+    self.sla.prev_v_cruise_cluster_conv = round(initialize_v_cruise * self.speed_conv)
+
+  def test_initial_state(self):
+    assert self.sla.state == SpeedLimitAssistState.disabled
+    assert not self.sla.is_enabled
+    assert not self.sla.is_active
+    assert V_CRUISE_UNSET == self.sla.get_v_target_from_control()
+
+  @pytest.mark.parametrize("car_name", [RIVIAN.RIVIAN_R1, TESLA.TESLA_MODEL_Y], indirect=True)
+  def test_disallowed_brands(self, car_name):
+    """
+      Speed Limit Assist is disabled for the following brands and conditions:
+      - All Tesla and is a release branch;
+      - All Rivian
+    """
+    assert not self.sla.enabled
+
+    # stay disallowed even when the param may have changed from somewhere else
+    self.params.put("SpeedLimitMode", int(Mode.assist), block=True)
+    for _ in range(int(PARAMS_UPDATE_PERIOD / DT_MDL)):
+      self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'],
+                      SPEED_LIMITS['city'], True, 0, self.events_sp)
+    assert not self.sla.enabled
+
+  def test_disabled(self):
+    self.params.put("SpeedLimitMode", int(Mode.off), block=True)
+    for _ in range(int(10. / DT_MDL)):
+      self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.disabled
+
+  def test_transition_disabled_to_preactive(self):
+    for _ in range(int(3. / DT_MDL)):
+      self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.preActive
+    assert self.sla.is_enabled and not self.sla.is_active
+
+  def test_transition_disabled_to_pending_no_speed_limit_not_max_initial_set_speed(self):
+    for _ in range(int(3. / DT_MDL)):
+      self.sla.update(True, False, SPEED_LIMITS['highway'], 0, SPEED_LIMITS['city'], 0, 0, False, 0, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.pending
+    assert self.sla.is_enabled and not self.sla.is_active
+
+  def test_preactive_to_active_with_max_speed_confirmation(self):
+    self.sla.state = SpeedLimitAssistState.preActive
+    self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
+                    SPEED_LIMITS['highway'], True, 0, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.active
+    assert self.sla.is_enabled and self.sla.is_active
+    assert self.sla.output_v_target == SPEED_LIMITS['highway']
+
+  def test_preactive_timeout_to_inactive(self):
+    self.sla.state = SpeedLimitAssistState.preActive
+    self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
+
+    for _ in range(int(PRE_ACTIVE_GUARD_PERIOD[self.sla.pcm_op_long] / DT_MDL)):
+      self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.inactive
+
+  def test_preactive_to_pending_no_speed_limit(self):
+    self.sla.state = SpeedLimitAssistState.preActive
+    self.sla.update(True, False, SPEED_LIMITS['highway'], 0, self.pcm_long_max_set_speed, 0, 0, False, 0, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.pending
+    assert self.sla.is_enabled and not self.sla.is_active
+
+  def test_pending_to_active_when_speed_limit_available(self):
+    self.sla.state = SpeedLimitAssistState.pending
+    self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
+    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
+
+    self.sla.update(True, False, SPEED_LIMITS['highway'], 0, self.pcm_long_max_set_speed,
+                    SPEED_LIMITS['highway'], SPEED_LIMITS['highway'], True, 0, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.active
+
+  def test_pending_to_adapting_when_below_speed_limit(self):
+    self.sla.state = SpeedLimitAssistState.pending
+    self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
+    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
+
+    self.sla.update(True, False, SPEED_LIMITS['highway'] + 5, 0, self.pcm_long_max_set_speed,
+                    SPEED_LIMITS['highway'], SPEED_LIMITS['highway'], True, 0, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.adapting
+    assert self.sla.is_enabled and self.sla.is_active
+
+  def test_active_to_adapting_transition(self):
+    self.initialize_active_state(self.pcm_long_max_set_speed)
+
+    self.sla.update(True, False, SPEED_LIMITS['highway'] + 2, 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
+                    SPEED_LIMITS['highway'], True, 0, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.adapting
+
+  def test_adapting_to_active_transition(self):
+    self.sla.state = SpeedLimitAssistState.adapting
+    self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
+    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
+
+    self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
+                    SPEED_LIMITS['highway'], True, 0, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.active
+
+  def test_manual_cruise_change_detection(self):
+    self.sla.state = SpeedLimitAssistState.active
+    expected_cruise = SPEED_LIMITS['highway']
+    self.sla.v_cruise_cluster_prev = expected_cruise
+
+    different_cruise = SPEED_LIMITS['highway'] + 5
+    self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.inactive
+
+  # TODO-SP: test lower CST cases
+  def test_rapid_speed_limit_changes(self):
+    self.initialize_active_state(self.pcm_long_max_set_speed)
+    speed_limits = [SPEED_LIMITS['highway'], SPEED_LIMITS['freeway']]
+
+    for _, speed_limit in enumerate(speed_limits):
+      self.sla.update(True, False, speed_limit, 0, self.pcm_long_max_set_speed, speed_limit, speed_limit, True, 0, self.events_sp)
+    assert self.sla.state in ACTIVE_STATES
+
+  def test_invalid_speed_limits_handling(self):
+    self.initialize_active_state(self.pcm_long_max_set_speed)
+
+    invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS]
+
+    for invalid_limit in invalid_limits:
+      self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, invalid_limit, SPEED_LIMITS['city'], True, 0, self.events_sp)
+      assert isinstance(self.sla.output_v_target, (int, float))
+      assert self.sla.output_v_target == V_CRUISE_UNSET or self.sla.output_v_target > 0
+
+  def test_stale_data_handling(self):
+    self.initialize_active_state(self.pcm_long_max_set_speed)
+    old_speed_limit = SPEED_LIMITS['city']
+
+    self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, 0, old_speed_limit, True, 0, self.events_sp)
+    assert self.sla.state in ACTIVE_STATES
+    assert self.sla.output_v_target == old_speed_limit
+
+  def test_distance_based_adapting(self):
+    self.sla.state = SpeedLimitAssistState.adapting
+    self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
+    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
+
+    distance = 100.0
+    current_speed = SPEED_LIMITS['freeway']
+    target_speed = SPEED_LIMITS['highway']
+
+    self.sla.update(True, False, current_speed, 0, self.pcm_long_max_set_speed, target_speed, target_speed, True, distance, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.adapting
+    assert self.sla.output_v_target == target_speed  # TODO-SP: assert expected accel, need to enable self.acceleration_solutions
+
+  def test_long_disengaged_to_disabled(self):
+    self.initialize_active_state(self.pcm_long_max_set_speed)
+
+    self.sla.update(False, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'],
+                    SPEED_LIMITS['city'], True, 0, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.disabled
+    assert self.sla.output_v_target == V_CRUISE_UNSET
+
+  def test_maintain_states_with_no_changes(self):
+    """Test that states are maintained when no significant changes occur"""
+    test_states = [
+      SpeedLimitAssistState.preActive,
+      SpeedLimitAssistState.pending,
+      SpeedLimitAssistState.active,
+      SpeedLimitAssistState.adapting
+    ]
+
+    for state in test_states:
+      self.sla.state = state
+      self.sla.op_engaged = True
+
+      initial_state = state
+
+      self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
+
+      assert self.sla.state in ALL_STATES  # Sanity check
+
+      if initial_state == SpeedLimitAssistState.preActive:
+        assert self.sla.state in [SpeedLimitAssistState.preActive, SpeedLimitAssistState.active]
+      elif initial_state in ACTIVE_STATES:
+        assert self.sla.state in ACTIVE_STATES

openpilot/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_resolver.py

@@ -0,0 +1,144 @@
+"""
+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 random
+import time
+
+import pytest
+from pytest_mock import MockerFixture
+
+from openpilot.cereal import custom
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE
+
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver, ALL_SOURCES
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy
+
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
+
+
+def create_mock(properties, mocker: MockerFixture):
+  mock = mocker.MagicMock()
+  for _property, value in properties.items():
+    setattr(mock, _property, value)
+  return mock
+
+
+def setup_sm_mock(mocker: MockerFixture):
+  cruise_speed_limit = random.uniform(0, 120)
+  live_map_data_limit = random.uniform(0, 120)
+
+  car_state = create_mock({
+    'gasPressed': False,
+    'brakePressed': False,
+    'standstill': False,
+  }, mocker)
+  car_state_sp = create_mock({
+    'speedLimit': cruise_speed_limit,
+  }, mocker)
+  live_map_data = create_mock({
+    'speedLimit': live_map_data_limit,
+    'speedLimitValid': True,
+    'speedLimitAhead': 0.,
+    'speedLimitAheadValid': 0.,
+    'speedLimitAheadDistance': 0.,
+  }, mocker)
+  gps_data = create_mock({
+    'unixTimestampMillis': time.monotonic() * 1e3,
+  }, mocker)
+  sm_mock = mocker.MagicMock()
+  sm_mock.__getitem__.side_effect = lambda key: {
+    'carState': car_state,
+    'liveMapDataSP': live_map_data,
+    'carStateSP': car_state_sp,
+    'gpsLocation': gps_data,
+  }[key]
+  return sm_mock
+
+
+parametrized_policies = pytest.mark.parametrize(
+  "policy, sm_key, function_key", [
+    (Policy.car_state_only, 'carStateSP', SpeedLimitSource.car),
+    (Policy.car_state_priority, 'carStateSP', SpeedLimitSource.car),
+    (Policy.map_data_only, 'liveMapDataSP', SpeedLimitSource.map),
+    (Policy.map_data_priority, 'liveMapDataSP', SpeedLimitSource.map),
+  ],
+  ids=lambda val: val.name if hasattr(val, 'name') else str(val)
+)
+
+
+@pytest.mark.parametrize("resolver_class", [SpeedLimitResolver])
+class TestSpeedLimitResolverValidation:
+
+  @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
+  def test_initial_state(self, resolver_class, policy):
+    resolver = resolver_class()
+    resolver.policy = policy
+    for source in ALL_SOURCES:
+      if source in resolver.limit_solutions:
+        assert resolver.limit_solutions[source] == 0.
+        assert resolver.distance_solutions[source] == 0.
+
+  @parametrized_policies
+  def test_resolver(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
+    resolver = resolver_class()
+    resolver.policy = policy
+    sm_mock = setup_sm_mock(mocker)
+    source_speed_limit = sm_mock[sm_key].speedLimit
+
+    # Assert the resolver
+    resolver.update(source_speed_limit, sm_mock)
+    assert resolver.speed_limit == source_speed_limit
+    assert resolver.source == ALL_SOURCES[function_key]
+
+  def test_resolver_combined(self, resolver_class, mocker: MockerFixture):
+    resolver = resolver_class()
+    resolver.policy = Policy.combined
+    sm_mock = setup_sm_mock(mocker)
+    socket_to_source = {'carStateSP': SpeedLimitSource.car, 'liveMapDataSP': SpeedLimitSource.map}
+    minimum_key, minimum_speed_limit = min(
+      ((key, sm_mock[key].speedLimit) for key in
+       socket_to_source.keys()), key=lambda x: x[1])
+
+    # Assert the resolver
+    resolver.update(minimum_speed_limit, sm_mock)
+    assert resolver.speed_limit == minimum_speed_limit
+    assert resolver.source == socket_to_source[minimum_key]
+
+  @parametrized_policies
+  def test_parser(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
+    resolver = resolver_class()
+    resolver.policy = policy
+    sm_mock = setup_sm_mock(mocker)
+    source_speed_limit = sm_mock[sm_key].speedLimit
+
+    # Assert the parsing
+    resolver.update(source_speed_limit, sm_mock)
+    assert resolver.limit_solutions[ALL_SOURCES[function_key]] == source_speed_limit
+    assert resolver.distance_solutions[ALL_SOURCES[function_key]] == 0.
+
+  @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
+  def test_resolve_interaction_in_update(self, resolver_class, policy, mocker: MockerFixture):
+    v_ego = 50
+    resolver = resolver_class()
+    resolver.policy = policy
+
+    sm_mock = setup_sm_mock(mocker)
+    resolver.update(v_ego, sm_mock)
+
+    # After resolution
+    assert resolver.speed_limit is not None
+    assert resolver.distance is not None
+    assert resolver.source is not None
+
+  @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
+  def test_old_map_data_ignored(self, resolver_class, policy, mocker: MockerFixture):
+    resolver = resolver_class()
+    resolver.policy = policy
+    sm_mock = mocker.MagicMock()
+    sm_mock['gpsLocation'].unixTimestampMillis = (time.monotonic() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3
+    resolver._get_from_map_data(sm_mock)
+    assert resolver.limit_solutions[SpeedLimitSource.map] == 0.
+    assert resolver.distance_solutions[SpeedLimitSource.map] == 0.

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

@@ -0,0 +1,211 @@
+"""
+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.common.parameterized import parameterized
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeState, LaneChangeDirection
+from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeController, AutoLaneChangeMode, \
+  AUTO_LANE_CHANGE_TIMER, ONE_SECOND_DELAY
+
+AUTO_LANE_CHANGE_TIMER_COMBOS = [
+  (AutoLaneChangeMode.NUDGELESS, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGELESS]),
+  (AutoLaneChangeMode.HALF_SECOND, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.HALF_SECOND]),
+  (AutoLaneChangeMode.ONE_SECOND, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.ONE_SECOND]),
+  (AutoLaneChangeMode.TWO_SECONDS, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.TWO_SECONDS]),
+  (AutoLaneChangeMode.THREE_SECONDS, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.THREE_SECONDS])
+]
+
+
+class TestAutoLaneChangeController:
+  def setup_method(self):
+    self.DH = DesireHelper()
+    self.alc = AutoLaneChangeController(self.DH)
+
+  def _reset_states(self):
+    self.alc.lane_change_bsm_delay = False
+    self.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
+    self.lane_change_wait_timer = 0.0
+    self.prev_brake_pressed = False
+    self.prev_lane_change = False
+
+  def test_reset(self):
+    """Test that reset correctly sets timers back to default."""
+    # Set some non-default values
+    self.alc.lane_change_wait_timer = 2.0
+    self.alc.prev_brake_pressed = True
+
+    # Set the DesireHelper to trigger a reset
+    self.DH.lane_change_state = LaneChangeState.off
+    self.DH.lane_change_direction = LaneChangeDirection.none
+
+    # Call reset
+    self.alc.reset()
+
+    # Check values were reset
+    assert self.alc.lane_change_wait_timer == 0.0
+    assert not self.alc.prev_brake_pressed
+
+  @parameterized.expand([(AutoLaneChangeMode.OFF, ), (AutoLaneChangeMode.NUDGE, )])
+
+  def test_off_and_nudge_mode(self, timer_state):
+    """Test the default OFF and NUDGE mode behavior."""
+    self._reset_states()
+    # Setup mode
+    self.alc.lane_change_bsm_delay = False  # BSM delay off
+    self.alc.lane_change_set_timer = timer_state
+
+    # Update controller
+    num_updates = int(5.0 / DT_MDL)
+    for _ in range(num_updates):  # Run for 5 seconds
+      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+    # Mode should not allow lane change immediately
+    assert not self.alc.auto_lane_change_allowed
+
+  def test_nudgeless_mode(self):
+    """Test the NUDGELESS mode behavior."""
+    self._reset_states()
+    # Setup NUDGELESS mode
+    self.alc.lane_change_bsm_delay = False  # BSM delay off
+    self.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGELESS
+
+    # Update controller once to read params
+    self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+    # Update multiple times to exceed the timer threshold
+    for _ in range(1):  # Should exceed 0.1s with multiple DT_MDL updates
+      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+    # Now lane change should be allowed
+    assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay
+    assert self.alc.auto_lane_change_allowed
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_timers(self, timer_state, timer_delay):
+      self._reset_states()
+      self.alc.lane_change_bsm_delay = False  # BSM delay off
+      self.alc.lane_change_set_timer = timer_state
+
+      # Update controller once
+      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+      # The timer should still be below the threshold after one update
+      assert not self.alc.auto_lane_change_allowed
+
+      # Update enough times to exceed the threshold (seconds / DT_MDL)
+      num_updates = int(timer_delay / DT_MDL) + 1  # Add one extra updates to ensure we exceed the threshold
+      for _ in range(num_updates):
+        self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+      # Now lane change should be allowed
+      assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay
+      assert self.alc.auto_lane_change_allowed
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_brake_pressed_disables_auto_lane_change(self, timer_state, timer_delay):
+    """Test that pressing the brake disables auto lane change."""
+    self._reset_states()
+    # Setup auto lane change mode
+    self.alc.lane_change_bsm_delay = False
+    self.alc.lane_change_set_timer = timer_state
+    num_updates = int(timer_delay / DT_MDL) + 1  # Add one extra updates to ensure we exceed the threshold
+
+    # Update with brake pressed for 1 second
+    for _ in range(num_updates):
+      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=True)
+
+    # Even though it is an auto lane change mode, lane change should be disallowed due to brake pressed prior initiating lane change
+    assert not self.alc.auto_lane_change_allowed
+
+    # Check that prev_brake_pressed is saved
+    assert self.alc.prev_brake_pressed
+
+    # Even releasing brake shouldn't allow auto lane change
+    for _ in range(num_updates):
+      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+    assert not self.alc.auto_lane_change_allowed
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_blindspot_detected_with_bsm_delay(self, timer_state, timer_delay):
+    """Test behavior when blindspot is detected with BSM delay enabled."""
+    # Blindspot detected - should prevent auto lane change
+    self._reset_states()
+    self.alc.lane_change_bsm_delay = True  # BSM delay on
+    self.alc.lane_change_set_timer = timer_state
+
+    # Update with blindspot detected - this should prevent auto lane change
+    self.alc.update_lane_change(blindspot_detected=True, brake_pressed=False)
+    assert not self.alc.auto_lane_change_allowed
+
+    # Keep updating with blindspot detected - should still prevent auto lane change
+    num_updates = int(timer_delay / DT_MDL) + 1  # Add one extra updates to ensure we exceed the threshold
+    for _ in range(num_updates):
+      self.alc.update_lane_change(blindspot_detected=True, brake_pressed=False)
+    assert not self.alc.auto_lane_change_allowed
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_blindspot_detected_then_undetected_with_bsm_delay(self, timer_state, timer_delay):
+    """Test behavior when blindspot is detected then undetected with BSM delay enabled."""
+    # Blindspot clears - should allow auto lane change after sufficient time
+    self._reset_states()
+    self.alc.lane_change_bsm_delay = True
+    self.alc.lane_change_set_timer = timer_state
+
+    # First update with blindspot detected to set the negative timer
+    self.alc.update_lane_change(blindspot_detected=True, brake_pressed=False)
+    assert not self.alc.auto_lane_change_allowed
+
+    # Now update with blindspot cleared - should start incrementing timer from negative value
+    num_updates = int((timer_delay + abs(ONE_SECOND_DELAY)) / DT_MDL) + 1
+    for _ in range(num_updates):
+      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+    # After sufficient updates with no blindspot, auto lane change should be allowed
+    assert self.alc.auto_lane_change_allowed
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_disallow_continuous_auto_lane_change(self, timer_state, timer_delay):
+      self._reset_states()
+      self.alc.lane_change_bsm_delay = False  # BSM delay off
+      self.alc.lane_change_set_timer = timer_state
+      num_updates = int(timer_delay / DT_MDL) + 1  # Add one extra updates to ensure we exceed the threshold
+
+      # Update enough times to exceed the threshold (seconds / DT_MDL)
+      for _ in range(num_updates):
+        self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+      # Now lane change should be allowed
+      assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay
+      assert self.alc.auto_lane_change_allowed
+
+      # Simulate lane change is initiated
+      self.DH.lane_change_state = LaneChangeState.laneChangeStarting
+      self.alc.update_state()
+
+      # Simulate lane change is completed, and one_blinker stays on
+      self.DH.lane_change_state = LaneChangeState.preLaneChange
+      self.alc.update_state()
+
+      # Update enough times to exceed the threshold (seconds / DT_MDL)
+      for _ in range(num_updates):
+        self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+      assert not self.alc.auto_lane_change_allowed
+
+  def test_auto_lane_change_mode_off_disallows_lane_change(self):
+    """Test that OFF mode never allows auto lane change."""
+    self._reset_states()
+    self.alc.lane_change_bsm_delay = False
+    self.alc.lane_change_set_timer = AutoLaneChangeMode.OFF
+
+    # Simulate updates for a long period of time (e.g., 10 seconds)
+    num_updates = int(10.0 / DT_MDL)
+    for _ in range(num_updates):
+      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+    # Lane change should never be allowed
+    assert not self.alc.auto_lane_change_allowed

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

@@ -0,0 +1,144 @@
+"""
+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.structs import car
+
+from openpilot.common.constants import CV
+from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral
+
+
+class TestBlinkerPauseLateral:
+
+  def setup_method(self):
+    self.blinker_pause_lateral = BlinkerPauseLateral()
+    self._reset_states()
+
+  def _reset_states(self):
+    self.blinker_pause_lateral.enabled = True
+    self.blinker_pause_lateral.is_metric = False
+    self.blinker_pause_lateral.min_speed = 20  # MPH
+    self.blinker_pause_lateral.reengage_delay = 0
+    self.blinker_pause_lateral.blinker_off_timer = 0.0
+
+    self.CS = car.CarState.new_message()
+    self.CS.vEgo = 0
+    self.CS.leftBlinker = False
+    self.CS.rightBlinker = False
+
+  def _test_should_blinker_pause_lateral(self, expected_results) -> None:
+    for left in (True, False):
+      for right in (True, False):
+        self.CS.leftBlinker = left
+        self.CS.rightBlinker = right
+
+        result = self.blinker_pause_lateral.update(self.CS)
+        assert result == expected_results[(left, right)]
+
+  def test_below_min_speed_blinker(self):
+    self.CS.vEgo = 4.5  # ~10 MPH
+
+    expected_results = {
+      (False, False): False,
+      (True, False): True,
+      (False, True): True,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_reengage_delay(self):
+    self.blinker_pause_lateral.reengage_delay = 2 # seconds
+    self.CS.vEgo = 4.5 # ~10 MPH
+
+    expected_results = {
+      (False, False): True,
+      (True, False): True,
+      (False, True): True,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_above_min_speed_blinker(self):
+    self.CS.vEgo = 13.4  # ~30 MPH
+
+    expected_results = {
+      (False, False): False,
+      (True, False): False,
+      (False, True): False,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_just_below_min_speed(self):
+    self.CS.vEgo = (20 * CV.MPH_TO_MS) - 0.01
+
+    expected_results = {
+      (False, False): False,
+      (True, False): True,
+      (False, True): True,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_disabled(self):
+    self.blinker_pause_lateral.enabled = False
+    self.CS.vEgo = 4.5  # ~10 MPH
+
+    expected_results = {
+      (False, False): False,
+      (True, False): False,
+      (False, True): False,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_metric_units_below_min_speed(self):
+    self.blinker_pause_lateral.is_metric = True
+    self.CS.vEgo = 5.0  # ~18 km/h
+
+    expected_results = {
+      (False, False): False,
+      (True, False): True,
+      (False, True): True,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_metric_units_above_threshold(self):
+    self.blinker_pause_lateral.is_metric = True
+    self.CS.vEgo = 6.0  # ~21.6 km/h
+
+    expected_results = {
+      (False, False): False,
+      (True, False): False,
+      (False, True): False,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_change_min_speed_threshold(self):
+    self.blinker_pause_lateral.min_speed = 30  # MPH
+
+    # below min speed
+    self.CS.vEgo = 11.2  # ~25 MPH
+
+    expected_results = {
+      (False, False): False,
+      (True, False): True,
+      (False, True): True,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+    # above min speed
+    self.CS.vEgo = 15.6  # ~35 MPH
+
+    expected_results = {
+      (False, False): False,
+      (True, False): False,
+      (False, True): False,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)

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

@@ -0,0 +1,113 @@
+import pytest
+from openpilot.cereal import log, custom
+from openpilot.common.params import Params
+
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
+from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
+
+TurnDirection = custom.ModelDataV2SP.TurnDirection
+
+
+@pytest.mark.parametrize("left_blinker,right_blinker,v_ego,blindspot_left,blindspot_right,expected", [
+  (True, False, 5, False, False, TurnDirection.turnLeft),
+  (False, True, 6, False, False, TurnDirection.turnRight),
+  (True, False, 9, False, False, TurnDirection.none),
+  (True, False, 7, True, False, TurnDirection.none),
+  (False, True, 6, False, True, TurnDirection.none),
+  (False, False, 5, False, False, TurnDirection.none),
+  (True, True, 5, False, False, TurnDirection.none),
+])
+def test_lane_turn_desire_conditions(left_blinker, right_blinker, v_ego, blindspot_left, blindspot_right, expected):
+  dh = DesireHelper()
+  controller = LaneTurnController(dh)
+  controller.enabled = True
+  controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
+  controller.turn_direction = TurnDirection.none
+  controller.update_lane_turn(blindspot_left, blindspot_right, left_blinker, right_blinker, v_ego)
+  assert controller.get_turn_direction() == expected
+
+
+def test_lane_turn_desire_disabled():
+  dh = DesireHelper()
+  controller = LaneTurnController(dh)
+  controller.enabled = False
+  controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
+  controller.turn_direction = TurnDirection.none
+  controller.update_lane_turn(False, False, True, False, 7)
+  assert controller.get_turn_direction() == TurnDirection.none
+
+
+def test_lane_turn_overrides_lane_change():
+  dh = DesireHelper()
+  controller = LaneTurnController(dh)
+  controller.enabled = True
+  controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
+  controller.turn_direction = TurnDirection.none
+  # left turn desire
+  controller.update_lane_turn(False, False, True, False, 5)
+  assert controller.get_turn_direction() == TurnDirection.turnLeft
+  # right turn desire
+  controller.update_lane_turn(False, False, False, True, 6)
+  assert controller.get_turn_direction() == TurnDirection.turnRight
+  # no turn
+  controller.update_lane_turn(False, False, False, False, 7)
+  assert controller.get_turn_direction() == TurnDirection.none
+
+
+@pytest.mark.parametrize("v_ego,expected", [
+  (8.93, TurnDirection.turnLeft),  # just below threshold
+  (8.96, TurnDirection.none),  # above threshold
+  (8.95, TurnDirection.none),  # just above threshold
+])
+def test_lane_turn_desire_speed_boundary(v_ego, expected):
+  dh = DesireHelper()
+  controller = LaneTurnController(dh)
+  controller.enabled = True
+  controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
+  controller.turn_direction = TurnDirection.none
+  controller.update_lane_turn(False, True, True, False, v_ego)
+  assert controller.get_turn_direction() == expected
+
+
+class DummyCarState:
+  def __init__(self, vEgo=0, leftBlinker=False, rightBlinker=False, leftBlindspot=False, rightBlindspot=False,
+               steeringPressed=False, steeringTorque=0, brakePressed=False):
+    self.vEgo = vEgo
+    self.leftBlinker = leftBlinker
+    self.rightBlinker = rightBlinker
+    self.leftBlindspot = leftBlindspot
+    self.rightBlindspot = rightBlindspot
+    self.steeringPressed = steeringPressed
+    self.steeringTorque = steeringTorque
+    self.brakePressed = brakePressed
+
+
+@pytest.fixture
+def set_lane_turn_params():
+  params = Params()
+  params.put("LaneTurnDesire", True)
+  params.put("LaneTurnValue", 20.0)
+
+
+@pytest.mark.parametrize("carstate, lateral_active, lane_change_prob, expected_desire", [
+  # Lane turn desire overrides lane change desire
+  (DummyCarState(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False), True, 1.0,
+   log.Desire.turnLeft),
+  (DummyCarState(vEgo=7, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False), True, 1.0,
+   log.Desire.turnRight),
+  # Lane change desire only (no turn desires)
+  (DummyCarState(vEgo=9, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False,
+                 steeringPressed=True, steeringTorque=1), True, 1.0, log.Desire.laneChangeLeft),
+  (DummyCarState(vEgo=9, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False,
+                 steeringPressed=True, steeringTorque=-1), True, 1.0, log.Desire.laneChangeRight),
+  # No desire (inactive)
+  (DummyCarState(vEgo=9, leftBlinker=False, rightBlinker=False), False, 1.0, log.Desire.none),
+  (DummyCarState(vEgo=4, leftBlinker=False, rightBlinker=False), True, 1.0, log.Desire.none),  # No blinkers? no desire!
+])
+def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params):
+  dh = DesireHelper()
+  dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
+  for _ in range(10):
+    dh.update(carstate, lateral_active, lane_change_prob)
+  assert dh.desire == expected_desire  # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers