rr

Adjusted acceleration profiles and breakpoints for different personalities. Modified smoothing alpha for acceleration.

.gitmodules

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

cereal/custom.capnp

@@ -192,6 +192,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   aTarget @5 :Float32;
   events @6 :List(OnroadEventSP.Event);
   e2eAlerts @7 :E2eAlerts;
+  accelPersonality @8 :AccelerationPersonality;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -203,7 +204,11 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
       blended @1;
     }
   }
-
+  enum AccelerationPersonality {
+    sport @0;
+    normal @1;
+    eco @2;
+  }
   struct SmartCruiseControl {
     vision @0 :Vision;
     map @1 :Map;
@@ -288,6 +293,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     sccVision @1;
     sccMap @2;
     speedLimitAssist @3;
+    mapd @4;
   }
 
   struct E2eAlerts {
@@ -340,6 +346,7 @@ struct OnroadEventSP @0xda96579883444c35 {
     speedLimitChanged @21;
     speedLimitPending @22;
     e2eChime @23;
+    laneChangeRoadEdge @24;
   }
 }
 
@@ -446,6 +453,8 @@ struct LiveMapDataSP @0xf416ec09499d9d19 {
 
 struct ModelDataV2SP @0xa1680744031fdb2d {
   laneTurnDirection @0 :TurnDirection;
+  leftLaneChangeEdgeBlock @1 :Bool;
+  rightLaneChangeEdgeBlock @2 :Bool;
 
   enum TurnDirection {
     none @0;
@@ -475,11 +484,125 @@ struct CustomReserved15 @0xbd443b539493bc68 {
 struct CustomReserved16 @0xfc6241ed8877b611 {
 }
 
-struct CustomReserved17 @0xa30662f84033036c {
+struct MapdDownloadLocationDetails @0xff889853e7b0987f {
+  location @0 :Text;
+  totalFiles @1 :UInt32;
+  downloadedFiles @2 :UInt32;
 }
 
-struct CustomReserved18 @0xc86a3d38d13eb3ef {
+struct MapdDownloadProgress @0xfaa35dcac85073a2 {
+  active @0 :Bool;
+  cancelled @1 :Bool;
+  totalFiles @2 :UInt32;
+  downloadedFiles @3 :UInt32;
+  locations @4 :List(Text);
+  locationDetails @5 :List(MapdDownloadLocationDetails);
 }
 
-struct CustomReserved19 @0xa4f1eb3323f5f582 {
+struct MapdPathPoint @0xd6f78acca1bc3939 {
+  latitude @0 :Float64;
+  longitude @1 :Float64;
+  curvature @2 :Float32;
+  targetVelocity @3 :Float32;
+}
+
+struct MapdExtendedOut @0xa30662f84033036c {
+  downloadProgress @0 :MapdDownloadProgress;
+  settings @1 :Text;
+  path @2 :List(MapdPathPoint);
+}
+
+enum MapdInputType {
+  download @0;
+  setTargetLateralAccel @1;
+  setSpeedLimitOffset @2;
+  setSpeedLimitControl @3;
+  setMapCurveSpeedControl @4;
+  setVisionCurveSpeedControl @5;
+  setLogLevel @6;
+  setVisionCurveTargetLatA @7;
+  setVisionCurveMinTargetV @8;
+  reloadSettings @9;
+  saveSettings @10;
+  setEnableSpeed @11;
+  setVisionCurveUseEnableSpeed @12;
+  setMapCurveUseEnableSpeed @13;
+  setSpeedLimitUseEnableSpeed @14;
+  setHoldLastSeenSpeedLimit @15;
+  setTargetSpeedJerk @16;
+  setTargetSpeedAccel @17;
+  setTargetSpeedTimeOffset @18;
+  setDefaultLaneWidth @19;
+  setMapCurveTargetLatA @20;
+  loadDefaultSettings @21;
+  loadRecommendedSettings @22;
+  setSlowDownForNextSpeedLimit @23;
+  setSpeedUpForNextSpeedLimit @24;
+  setHoldSpeedLimitWhileChangingSetSpeed @25;
+  loadPersistentSettings @26;
+  cancelDownload @27;
+  setLogJson @28;
+  setLogSource @29;
+  setExternalSpeedLimitControl @30;
+  setExternalSpeedLimit @31;
+  setSpeedLimitPriority @32;
+  setSpeedLimitChangeRequiresAccept @33;
+  acceptSpeedLimit @34;
+  setPressGasToAcceptSpeedLimit @35;
+  setAdjustSetSpeedToAcceptSpeedLimit @36;
+  setAcceptSpeedLimitTimeout @37;
+  setPressGasToOverrideSpeedLimit @38;
+}
+
+enum WaySelectionType {
+  current @0;
+  predicted @1;
+  possible @2;
+  extended @3;
+  fail @4;
+}
+
+enum SpeedLimitOffsetType {
+  static @0;
+  percent @1;
+}
+
+struct MapdIn @0xc86a3d38d13eb3ef {
+  type @0 :MapdInputType;
+  float @1 :Float32;
+  str @2 :Text;
+  bool @3 :Bool;
+}
+
+enum RoadContext {
+  freeway @0;
+  city @1;
+  unknown @2;
+}
+
+struct MapdOut @0xa4f1eb3323f5f582 {
+  wayName @0 :Text;
+  wayRef @1 :Text;
+  roadName @2 :Text;
+  speedLimit @3 :Float32;
+  nextSpeedLimit @4 :Float32;
+  nextSpeedLimitDistance @5 :Float32;
+  hazard @6 :Text;
+  nextHazard @7 :Text;
+  nextHazardDistance @8 :Float32;
+  advisorySpeed @9 :Float32;
+  nextAdvisorySpeed @10 :Float32;
+  nextAdvisorySpeedDistance @11 :Float32;
+  oneWay @12 :Bool;
+  lanes @13 :UInt8;
+  tileLoaded @14 :Bool;
+  speedLimitSuggestedSpeed @15 :Float32;
+  suggestedSpeed @16 :Float32;
+  estimatedRoadWidth @17 :Float32;
+  roadContext @18 :RoadContext;
+  distanceFromWayCenter @19 :Float32;
+  visionCurveSpeed @20 :Float32;
+  mapCurveSpeed @21 :Float32;
+  waySelectionType @22 :WaySelectionType;
+  speedLimitAccepted @23 :Bool;
 }

cereal/log.capnp

@@ -2643,9 +2643,9 @@ struct Event {
     customReserved14 @140 :Custom.CustomReserved14;
     customReserved15 @141 :Custom.CustomReserved15;
     customReserved16 @142 :Custom.CustomReserved16;
-    customReserved17 @143 :Custom.CustomReserved17;
-    customReserved18 @144 :Custom.CustomReserved18;
-    customReserved19 @145 :Custom.CustomReserved19;
+    mapdExtendedOut @143 :Custom.MapdExtendedOut;
+    mapdIn @144 :Custom.MapdIn;
+    mapdOut @145 :Custom.MapdOut;
 
     # *********** legacy + deprecated ***********
     model @9 :Legacy.ModelData; # TODO: rename modelV2 and mark this as deprecated

cereal/services.py

@@ -98,9 +98,9 @@ _services: dict[str, tuple] = {
   "carParamsSP": (True, 0.02, 1),
   "carControlSP": (True, 100., 10),
   "carStateSP": (True, 100., 10),
-  "liveMapDataSP": (True, 1., 1),
   "modelDataV2SP": (True, 20., None, QueueSize.BIG),
   "liveLocationKalman": (True, 20.),
+  "mapdOut": (True, 20., 20, QueueSize.MEDIUM),
 
   # debug
   "uiDebug": (True, 0., 1),

common/params_keys.h

@@ -133,6 +133,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"Version", {PERSISTENT, STRING}},
 
     // --- sunnypilot params --- //
+    {"AccelPersonality", {PERSISTENT | BACKUP, INT, std::to_string(static_cast<int>(cereal::LongitudinalPlanSP::AccelerationPersonality::NORMAL))}},
+    {"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ApiCache_DriveStats", {PERSISTENT, JSON}},
     {"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}},
@@ -151,6 +153,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"CustomAccShortPressIncrement", {PERSISTENT | BACKUP, INT, "1"}},
     {"DeviceBootMode", {PERSISTENT | BACKUP, INT, "0"}},
     {"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}},
+    {"DynamicFollow", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
     {"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
     {"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
@@ -173,11 +176,18 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"RoadEdgeLaneChangeEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
 
+    // toyota specific params
+    {"ToyotaAutoHold", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"ToyotaEnhancedBsm", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"ToyotaTSS2Long", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"ToyotaDriveMode", {PERSISTENT | BACKUP, BOOL, "0"}},
+
     // MADS params
     {"Mads", {PERSISTENT | BACKUP, BOOL, "1"}},
     {"MadsMainCruiseAllowed", {PERSISTENT | BACKUP, BOOL, "1"}},
@@ -192,6 +202,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"ModelManager_LastSyncTime", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "0"}},
     {"ModelManager_ModelsCache", {PERSISTENT | BACKUP, JSON}},
 
+    {"MapdSettings", {PERSISTENT, JSON}},
+    {"Offroad_OSMUpdateRequired", {CLEAR_ON_MANAGER_START, JSON}},
+
     // Neural Network Lateral Control
     {"NeuralNetworkLateralControl", {PERSISTENT | BACKUP, BOOL, "0"}},
 
@@ -228,38 +241,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}},
     {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
 
-    // mapd
-    {"MapAdvisorySpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT}},
-    {"MapdVersion", {PERSISTENT, STRING}},
-    {"MapSpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT, "0.0"}},
-    {"NextMapSpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, JSON}},
-    {"Offroad_OSMUpdateRequired", {CLEAR_ON_MANAGER_START, JSON}},
-    {"OsmDbUpdatesCheck", {CLEAR_ON_MANAGER_START, BOOL}},  // mapd database update happens with device ON, reset on boot
-    {"OSMDownloadBounds", {PERSISTENT, STRING}},
-    {"OsmDownloadedDate", {PERSISTENT, STRING, "0.0"}},
-    {"OSMDownloadLocations", {PERSISTENT, JSON}},
-    {"OSMDownloadProgress", {CLEAR_ON_MANAGER_START, JSON}},
-    {"OsmLocal", {PERSISTENT, BOOL}},
-    {"OsmLocationName", {PERSISTENT, STRING}},
-    {"OsmLocationTitle", {PERSISTENT, STRING}},
-    {"OsmLocationUrl", {PERSISTENT, STRING}},
-    {"OsmStateName", {PERSISTENT, STRING, "All"}},
-    {"OsmStateTitle", {PERSISTENT, STRING}},
-    {"OsmWayTest", {PERSISTENT, STRING}},
-    {"RoadName", {CLEAR_ON_ONROAD_TRANSITION, STRING}},
-    {"RoadNameToggle", {PERSISTENT, STRING}},
-
-    // Speed Limit
-    {"SpeedLimitMode", {PERSISTENT | BACKUP, INT, "1"}},
-    {"SpeedLimitOffsetType", {PERSISTENT | BACKUP, INT, "0"}},
-    {"SpeedLimitPolicy", {PERSISTENT | BACKUP, INT, "3"}},
-    {"SpeedLimitValueOffset", {PERSISTENT | BACKUP, INT, "0"}},
-
-    // Smart Cruise Control
-    {"MapTargetVelocities", {CLEAR_ON_ONROAD_TRANSITION, STRING}},
-    {"SmartCruiseControlMap", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"SmartCruiseControlVision", {PERSISTENT | BACKUP, BOOL, "0"}},
-
     // Torque lateral control custom params
     {"CustomTorqueParams", {PERSISTENT | BACKUP , BOOL}},
     {"EnforceTorqueControl", {PERSISTENT | BACKUP, BOOL}},

selfdrive/car/card.py

@@ -10,7 +10,7 @@ from cereal import car, log, custom
 from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper
 from openpilot.common.swaglog import cloudlog, ForwardingHandler
-
+from opendbc.safety import ALTERNATIVE_EXPERIENCE
 from opendbc.car import DT_CTRL, structs
 from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable
 from opendbc.car.carlog import carlog
@@ -122,7 +122,13 @@ class Car:
       self.CI, self.CP, self.CP_SP = CI, CI.CP, CI.CP_SP
       self.RI = RI
 
+    # set alternative experiences from parameters
+    sp_toyota_auto_brake_hold = self.params.get_bool("ToyotaAutoHold")
     self.CP.alternativeExperience = 0
+    if sp_toyota_auto_brake_hold:
+      self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB
+
+
     # mads
     set_alternative_experience(self.CP, self.CP_SP, self.params)
     set_car_specific_params(self.CP, self.CP_SP, self.params)
@@ -213,7 +219,6 @@ class Car:
     if can_rcv_valid and REPLAY:
       self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
 
-    self.v_cruise_helper.update_speed_limit_assist(self.is_metric, self.sm['longitudinalPlanSP'])
     self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric)
     if self.sm['carControl'].enabled and not self.CC_prev.enabled:
       # Use CarState w/ buttons from the step selfdrived enables on

selfdrive/car/cruise.py

@@ -53,7 +53,6 @@ class VCruiseHelper(VCruiseHelperSP):
       if not self.CP.pcmCruise or (not self.CP_SP.pcmCruiseSpeed and _enabled):
         # if stock cruise is completely disabled, then we can use our own set speed logic
         self._update_v_cruise_non_pcm(CS, _enabled, is_metric)
-        self.update_speed_limit_assist_v_cruise_non_pcm()
         self.v_cruise_cluster_kph = self.v_cruise_kph
         self.update_button_timers(CS, enabled)
       else:
@@ -105,12 +104,6 @@ class VCruiseHelper(VCruiseHelperSP):
     if not self.button_change_states[button_type]["enabled"]:
       return
 
-    # Speed Limit Assist for Non PCM long cars.
-    # True: Disallow set speed changes when user confirmed the target set speed during preActive state
-    # False: Allow set speed changes as SLA is not requesting user confirmation
-    if self.update_speed_limit_assist_pre_active_confirmed(button_type):
-      return
-
     long_press, v_cruise_delta = VCruiseHelperSP.update_v_cruise_delta(self, long_press, v_cruise_delta)
     if long_press and self.v_cruise_kph % v_cruise_delta != 0:  # partial interval
       self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta

selfdrive/controls/lib/desire_helper.py

@@ -56,7 +56,7 @@ class DesireHelper:
   def get_lane_change_direction(CS):
     return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
 
-  def update(self, carstate, lateral_active, lane_change_prob):
+  def update(self, carstate, lateral_active, lane_change_prob, left_edge_detected, right_edge_detected):
     self.alc.update_params()
     self.lane_turn_controller.update_params()
     v_ego = carstate.vEgo
@@ -88,8 +88,8 @@ class DesireHelper:
                          ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
                           (carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
 
-        blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
-                              (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
+        blindspot_detected = (((carstate.leftBlindspot or left_edge_detected) and self.lane_change_direction == LaneChangeDirection.left) or
+                              ((carstate.rightBlindspot or right_edge_detected) and self.lane_change_direction == LaneChangeDirection.right))
 
         self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
 

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -327,8 +327,10 @@ class LongitudinalMpc:
     lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
     return lead_xv
 
-  def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard):
-    t_follow = get_T_FOLLOW(personality)
+  def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard, a_cruise_min_override=None, t_follow_override=None):
+    t_follow = t_follow_override if t_follow_override is not None else get_T_FOLLOW(personality)
+    a_cruise_min = a_cruise_min_override if a_cruise_min_override is not None else CRUISE_MIN_ACCEL
+
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 
@@ -350,7 +352,7 @@ class LongitudinalMpc:
 
       # Fake an obstacle for cruise, this ensures smooth acceleration to set speed
       # when the leads are no factor.
-      v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05)
+      v_lower = v_ego + (T_IDXS * a_cruise_min * 1.05)
       # TODO does this make sense when max_a is negative?
       v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05)
       v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),

selfdrive/controls/lib/longitudinal_planner.py

@@ -124,7 +124,11 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     prev_accel_constraint = not (reset_state or sm['carState'].standstill)
 
     if mode == 'acc':
-      accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
+      if sp_accel_clip := LongitudinalPlannerSP.get_accel_clip(self, v_ego, mode):
+        accel_clip = sp_accel_clip
+      else:
+        accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
+
       steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
       accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
     else:
@@ -154,7 +158,9 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
     self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
-    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality)
+    a_cruise_min_override = LongitudinalPlannerSP.get_cruise_min_accel(self, v_ego)
+    t_follow_override = LongitudinalPlannerSP.get_t_follow(self, v_ego)
+    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality, a_cruise_min_override=a_cruise_min_override, t_follow_override=t_follow_override)
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)

selfdrive/controls/plannerd.py

@@ -27,12 +27,11 @@ def main():
   longitudinal_planner = LongitudinalPlanner(CP, CP_SP)
   pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
   sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
-                            'liveMapDataSP', 'carStateSP', gps_location_service],
+                            'carStateSP', 'mapdOut', gps_location_service],
                            poll='carState')
 
   while True:
     sm.update()
-    longitudinal_planner.sla.update_car_state(sm['carState'])
     if sm.updated['modelV2']:
       longitudinal_planner.update(sm)
       longitudinal_planner.publish(sm, pm)

selfdrive/modeld/modeld.py

@@ -33,7 +33,7 @@ from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from
 
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase
-
+from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
 
 PROCESS_NAME = "selfdrive.modeld.modeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -43,7 +43,7 @@ POLICY_PKL_PATH = Path(__file__).parent / 'models/driving_policy_tinygrad.pkl'
 VISION_METADATA_PATH = Path(__file__).parent / 'models/driving_vision_metadata.pkl'
 POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.pkl'
 
-LAT_SMOOTH_SECONDS = 0.0
+LAT_SMOOTH_SECONDS = 0.1
 LONG_SMOOTH_SECONDS = 0.3
 MIN_LAT_CONTROL_SPEED = 0.3
 
@@ -298,6 +298,7 @@ def main(demo=False):
   prev_action = log.ModelDataV2.Action()
 
   DH = DesireHelper()
+  RELC = RoadEdgeLaneChangeController(params.get_bool("RoadEdgeLaneChangeEnabled"))
 
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -395,7 +396,10 @@ def main(demo=False):
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
       r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
       lane_change_prob = l_lane_change_prob + r_lane_change_prob
-      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
+      RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs)
+      mdv2sp_send.modelDataV2SP.leftLaneChangeEdgeBlock = RELC.left_edge_detected
+      mdv2sp_send.modelDataV2SP.rightLaneChangeEdgeBlock = RELC.right_edge_detected
+      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, RELC.left_edge_detected, RELC.right_edge_detected)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
       mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction

selfdrive/selfdrived/selfdrived.py

@@ -233,8 +233,8 @@ class SelfdriveD(CruiseHelper):
 
       # Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0
       if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \
-        (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \
-        (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)):
+            (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \
+            (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)):
         self.events.add(EventName.pedalPressed)
 
     # Create events for temperature, disk space, and memory
@@ -295,16 +295,22 @@ class SelfdriveD(CruiseHelper):
     # Handle lane change
     if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
       direction = self.sm['modelV2'].meta.laneChangeDirection
+      mdv2sp = self.sm['modelDataV2SP']
+
       if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
-         (CS.rightBlindspot and direction == LaneChangeDirection.right):
+            (CS.rightBlindspot and direction == LaneChangeDirection.right):
         self.events.add(EventName.laneChangeBlocked)
+
+      elif mdv2sp.leftLaneChangeEdgeBlock or mdv2sp.rightLaneChangeEdgeBlock:
+        self.events_sp.add(custom.OnroadEventSP.EventName.laneChangeRoadEdge)
+
       else:
         if direction == LaneChangeDirection.left:
           self.events.add(EventName.preLaneChangeLeft)
         else:
           self.events.add(EventName.preLaneChangeRight)
     elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting,
-                                                    LaneChangeState.laneChangeFinishing):
+                                                     LaneChangeState.laneChangeFinishing):
       self.events.add(EventName.laneChange)
 
     # Handle lane turn
@@ -499,7 +505,7 @@ class SelfdriveD(CruiseHelper):
 
     # All pandas not in silent mode must have controlsAllowed when openpilot is enabled
     if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates']
-           if ps.safetyModel not in IGNORED_SAFETY_MODES):
+                            if ps.safetyModel not in IGNORED_SAFETY_MODES):
       self.mismatch_counter += 1
 
     return CS

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-b259f6f8f099a9d82e4c65dd5deae2e4e293007b
+b508f43fb0481bce0859c9b6ab4f45ee690b8dab

selfdrive/ui/mici/layouts/main.py

@@ -4,6 +4,7 @@ import cereal.messaging as messaging
 from openpilot.selfdrive.ui.mici.layouts.home import MiciHomeLayout
 from openpilot.selfdrive.ui.mici.layouts.settings.settings import SettingsLayout
 from openpilot.selfdrive.ui.mici.layouts.offroad_alerts import MiciOffroadAlerts
+from openpilot.selfdrive.ui.mici.layouts.mapd_panel import MapdInfoPanel
 from openpilot.selfdrive.ui.mici.onroad.augmented_road_view import AugmentedRoadView
 from openpilot.selfdrive.ui.ui_state import device, ui_state
 from openpilot.selfdrive.ui.mici.layouts.onboarding import OnboardingWindow
@@ -40,17 +41,19 @@ class MiciMainLayout(Widget):
     self._alerts_layout = MiciOffroadAlerts()
     self._settings_layout = SettingsLayout()
     self._onroad_layout = AugmentedRoadView(bookmark_callback=self._on_bookmark_clicked)
+    self._mapd_panel = MapdInfoPanel()
 
     # Initialize widget rects
-    for widget in (self._home_layout, self._settings_layout, self._alerts_layout, self._onroad_layout):
+    for widget in (self._home_layout, self._settings_layout, self._alerts_layout, self._onroad_layout, self._mapd_panel):
       # TODO: set parent rect and use it if never passed rect from render (like in Scroller)
       widget.set_rect(rl.Rectangle(0, 0, gui_app.width, gui_app.height))
 
     self._scroller = Scroller([
-      self._alerts_layout,
-      self._home_layout,
-      self._onroad_layout,
-    ], spacing=0, pad_start=0, pad_end=0)
+        self._alerts_layout,
+        self._home_layout,
+        self._mapd_panel,
+        self._onroad_layout,
+      ], spacing=0, pad_start=0, pad_end=0)
     self._scroller.set_reset_scroll_at_show(False)
 
     # Disable scrolling when onroad is interacting with bookmark
@@ -107,6 +110,14 @@ class MiciMainLayout(Widget):
       self._layouts[mode].show_event()
       self._current_mode = mode
 
+  def _is_on_side_panel(self) -> bool:
+    onroad_x = self._onroad_layout.rect.x
+    current_scroll = self._scroller.scroll_panel.get_offset()
+    return abs(current_scroll - onroad_x) > self._rect.width / 2
+
+  def _is_on_mapd_panel(self) -> bool:
+    return self._is_on_side_panel()
+
   def _handle_transitions(self):
     if ui_state.started != self._prev_onroad:
       self._prev_onroad = ui_state.started
@@ -123,15 +134,16 @@ class MiciMainLayout(Widget):
 
     CS = ui_state.sm["carState"]
     if not CS.standstill and self._prev_standstill:
-      self._set_mode(MainState.MAIN)
-      self._scroll_to(self._onroad_layout)
+      if not self._is_on_mapd_panel():
+        self._set_mode(MainState.MAIN)
+        self._scroll_to(self._onroad_layout)
     self._prev_standstill = CS.standstill
 
   def _set_mode_for_started(self, onroad_transition: bool = False):
     if ui_state.started:
       CS = ui_state.sm["carState"]
       # Only go onroad if car starts or is not at a standstill
-      if not CS.standstill or onroad_transition:
+      if (not CS.standstill or onroad_transition) and not self._is_on_mapd_panel():
         self._set_mode(MainState.MAIN)
         self._scroll_to(self._onroad_layout)
     else:

selfdrive/ui/mici/layouts/mapd_panel.py

@@ -0,0 +1,217 @@
+"""
+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 pyray as rl
+from dataclasses import dataclass
+from openpilot.common.constants import CV
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.system.ui.lib.application import gui_app, FontWeight
+from openpilot.system.ui.lib.multilang import tr
+from openpilot.system.ui.lib.text_measure import measure_text_cached
+from openpilot.system.ui.widgets import Widget
+
+METER_TO_KM = 0.001
+METER_TO_MILE = 0.000621371
+
+@dataclass(frozen=True)
+class MapdPanelColors:
+  white: rl.Color = rl.WHITE
+  black: rl.Color = rl.BLACK
+  red: rl.Color = rl.Color(255, 0, 0, 255)
+  green: rl.Color = rl.Color(0, 255, 0, 255)
+  grey: rl.Color = rl.Color(145, 155, 149, 241)
+  light_grey: rl.Color = rl.Color(200, 200, 200, 255)
+  dark_grey: rl.Color = rl.Color(100, 100, 100, 255)
+  bg_dark: rl.Color = rl.Color(30, 30, 30, 255)
+  card_bg: rl.Color = rl.Color(50, 50, 50, 200)
+
+COLORS = MapdPanelColors()
+
+class MapdInfoPanel(Widget):
+  def __init__(self):
+    super().__init__()
+    self.speed_limit: float = 0.0
+    self.speed_limit_valid: bool = False
+    self.next_speed_limit: float = 0.0
+    self.next_speed_limit_distance: float = 0.0
+    self.road_name: str = ""
+    self.current_speed: float = 0.0
+    self.lanes: int = 0
+    self.road_context: str = ""
+    self.hazard: str = ""
+    self.vision_curve_speed: float = 0.0
+    self.map_curve_speed: float = 0.0
+    self.tile_loaded: bool = False
+
+    self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD)
+    self._font_semi_bold: rl.Font = gui_app.font(FontWeight.SEMI_BOLD)
+    self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM)
+
+  def _update_state(self) -> None:
+    sm = ui_state.sm
+    speed_conv = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
+    if sm.valid["mapdOut"]:
+      mapd = sm["mapdOut"]
+      self.speed_limit = mapd.speedLimit * speed_conv
+      self.speed_limit_valid = mapd.speedLimit > 0
+      self.next_speed_limit = mapd.nextSpeedLimit * speed_conv
+      self.next_speed_limit_distance = mapd.nextSpeedLimitDistance
+      self.road_name = mapd.roadName
+      self.lanes = mapd.lanes
+      self.hazard = mapd.hazard
+      self.vision_curve_speed = mapd.visionCurveSpeed * speed_conv
+      self.map_curve_speed = mapd.mapCurveSpeed * speed_conv
+      self.tile_loaded = mapd.tileLoaded
+
+      road_ctx = mapd.roadContext
+      if hasattr(road_ctx, 'raw'):
+        ctx_val = road_ctx.raw
+      else:
+        ctx_val = int(road_ctx)
+      self.road_context = ["Freeway", "City", "Unknown"][ctx_val] if ctx_val < 3 else "Unknown"
+    else:
+      self.speed_limit_valid = False
+
+    if sm.updated["carState"]:
+      self.current_speed = sm["carState"].vEgo * speed_conv
+
+  def _render(self, rect: rl.Rectangle) -> None:
+    self._update_state()
+
+    rl.draw_rectangle(int(rect.x), int(rect.y), int(rect.width), int(rect.height), COLORS.bg_dark)
+    margin = 15
+    left_width = 130
+    right_x = rect.x + left_width + margin + 10
+    right_width = rect.width - left_width - margin * 3
+    sign_x = rect.x + margin + 5
+    sign_y = rect.y + (rect.height - 140) / 2
+    self._draw_speed_limit_sign(sign_x, sign_y)
+    info_y = rect.y + 25
+
+    # Road name
+    self._draw_road_name(right_x, info_y, right_width)
+    info_y += 55
+
+    # Road context (Freeway/City) and lanes
+    self._draw_road_details(right_x, info_y, right_width)
+    info_y += 45
+
+    # Next speed limit
+    self._draw_next_speed_limit(right_x, info_y, right_width)
+    info_y += 45
+
+    # Curve speeds
+    self._draw_curve_speeds(right_x, info_y, right_width)
+    info_y += 45
+
+    # Hazard warning Beta
+    if self.hazard:
+      self._draw_hazard(right_x, info_y, right_width)
+
+    status_text = tr("Map Loaded") if self.tile_loaded else tr("No Map Data")
+    status_color = COLORS.green if self.tile_loaded else COLORS.red
+    status_size = measure_text_cached(self._font_medium, status_text, 18)
+    status_pos = rl.Vector2(rect.x + (rect.width - status_size.x) / 2, rect.y + rect.height - 30)
+    rl.draw_text_ex(self._font_medium, status_text, status_pos, 18, 0, status_color)
+
+  def _draw_speed_limit_sign(self, x: float, y: float) -> None:
+    sign_width = 110 if ui_state.is_metric else 100
+    sign_height = 120 if ui_state.is_metric else 110
+
+    speed_str = str(round(self.speed_limit)) if self.speed_limit_valid else "--"
+    speed_color = COLORS.black if not self.speed_limit_valid or self.current_speed <= self.speed_limit else COLORS.red
+
+    if ui_state.is_metric:
+      self._draw_vienna_sign(x, y, sign_width, sign_height, speed_str, speed_color)
+    else:
+      self._draw_mutcd_sign(x, y, sign_width, sign_height, speed_str, speed_color)
+
+  def _draw_road_name(self, x: float, y: float, width: float) -> None:
+    road_display = self.road_name if self.road_name else "--"
+    road_size = measure_text_cached(self._font_semi_bold, road_display, 34)
+    if road_size.x > width:
+      road_display = road_display[:20] + "..."
+    rl.draw_text_ex(self._font_semi_bold, road_display, rl.Vector2(x, y), 34, 0, COLORS.white)
+
+  def _draw_road_details(self, x: float, y: float, width: float) -> None:
+    details = []
+    if self.road_context and self.road_context != "Unknown":
+      details.append(self.road_context)
+    if self.lanes > 0:
+      details.append(f"{self.lanes} {tr('lanes')}")
+    details_text = " • ".join(details) if details else "--"
+    rl.draw_text_ex(self._font_medium, details_text, rl.Vector2(x, y), 26, 0, COLORS.light_grey)
+
+  def _draw_next_speed_limit(self, x: float, y: float, width: float) -> None:
+    if self.next_speed_limit > 0 and self.next_speed_limit != self.speed_limit:
+      next_text = f"{tr('Next')}: {round(self.next_speed_limit)} ({self._format_distance(self.next_speed_limit_distance)})"
+    else:
+      next_text = f"{tr('Next')}: --"
+    rl.draw_text_ex(self._font_medium, next_text, rl.Vector2(x, y), 26, 0, COLORS.light_grey)
+
+  def _draw_curve_speeds(self, x: float, y: float, width: float) -> None:
+    unit = tr("km/h") if ui_state.is_metric else tr("mph")
+    vision_val = str(round(self.vision_curve_speed)) if self.vision_curve_speed > 0 else "--"
+    map_val = str(round(self.map_curve_speed)) if self.map_curve_speed > 0 else "--"
+    curve_text = f"{tr('Curve')}: V:{vision_val} M:{map_val} {unit}"
+    rl.draw_text_ex(self._font_medium, curve_text, rl.Vector2(x, y), 24, 0, COLORS.light_grey)
+
+  def _draw_hazard(self, x: float, y: float, width: float) -> None:
+    hazard_text = f"⚠ {self.hazard}"
+    rl.draw_text_ex(self._font_semi_bold, hazard_text, rl.Vector2(x, y), 26, 0, COLORS.red)
+
+  def _draw_vienna_sign(self, x: float, y: float, width: float, height: float, speed_str: str, speed_color: rl.Color) -> None:
+    center = rl.Vector2(x + width / 2, y + height / 2)
+    outer_radius = min(width, height) / 2
+
+    rl.draw_circle_v(center, outer_radius, COLORS.white)
+    ring_width = outer_radius * 0.18
+    rl.draw_ring(center, outer_radius - ring_width, outer_radius, 0, 360, 36, COLORS.red)
+
+    font_size = outer_radius * (0.7 if len(speed_str) >= 3 else 0.9)
+    text_size = measure_text_cached(self._font_bold, speed_str, int(font_size))
+    text_pos = rl.Vector2(center.x - text_size.x / 2, center.y - text_size.y / 2)
+    rl.draw_text_ex(self._font_bold, speed_str, text_pos, font_size, 0, speed_color)
+
+  def _draw_mutcd_sign(self, x: float, y: float, width: float, height: float, speed_str: str, speed_color: rl.Color) -> None:
+    sign_rect = rl.Rectangle(x, y, width, height)
+    rl.draw_rectangle_rounded(sign_rect, 0.2, 10, COLORS.white)
+    rl.draw_rectangle_rounded_lines_ex(sign_rect, 0.2, 10, 3, COLORS.black)
+
+    speed_label = tr("SPEED")
+    limit_label = tr("LIMIT")
+    label_size = 16
+
+    speed_text_size = measure_text_cached(self._font_semi_bold, speed_label, label_size)
+    speed_pos = rl.Vector2(x + (width - speed_text_size.x) / 2, y + 8)
+    rl.draw_text_ex(self._font_semi_bold, speed_label, speed_pos, label_size, 0, COLORS.black)
+
+    limit_text_size = measure_text_cached(self._font_semi_bold, limit_label, label_size)
+    limit_pos = rl.Vector2(x + (width - limit_text_size.x) / 2, y + 26)
+    rl.draw_text_ex(self._font_semi_bold, limit_label, limit_pos, label_size, 0, COLORS.black)
+
+    font_size = 40 if len(speed_str) >= 3 else 50
+    num_size = measure_text_cached(self._font_bold, speed_str, font_size)
+    num_pos = rl.Vector2(x + (width - num_size.x) / 2, y + 45)
+    rl.draw_text_ex(self._font_bold, speed_str, num_pos, font_size, 0, speed_color)
+
+  def _format_distance(self, distance: float) -> str:
+    if ui_state.is_metric:
+      if distance < 50:
+        return tr("Near")
+      if distance >= 1000:
+        return f"{distance * METER_TO_KM:.1f}" + tr("km")
+      if distance < 200:
+        rounded = max(10, int(distance / 10) * 10)
+      else:
+        rounded = int(distance / 100) * 100
+      return str(rounded) + tr("m")
+    else:
+      distance_mi = distance * METER_TO_MILE
+      if distance_mi < 0.1:
+        return tr("Near")
+      return f"{distance_mi:.1f}" + tr("mi")

selfdrive/ui/mici/onroad/hud_renderer.py

@@ -121,12 +121,17 @@ class HudRenderer(Widget):
     self._txt_wheel: rl.Texture = gui_app.texture('icons_mici/wheel.png', 50, 50)
     self._txt_wheel_critical: rl.Texture = gui_app.texture('icons_mici/wheel_critical.png', 50, 50)
     self._txt_exclamation_point: rl.Texture = gui_app.texture('icons_mici/exclamation_point.png', 44, 44)
+    self._txt_blind_spot_left: rl.Texture = gui_app.texture('icons_mici/onroad/blind_spot_left.png', 108, 128)
+    self._txt_blind_spot_right: rl.Texture = gui_app.texture('icons_mici/onroad/blind_spot_right.png', 108, 128)
 
     self._wheel_alpha_filter = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps)
     self._wheel_y_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps)
 
     self._set_speed_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
 
+    self._blind_spot_left_alpha_filter = FirstOrderFilter(0, 0.15, 1 / gui_app.target_fps)
+    self._blind_spot_right_alpha_filter = FirstOrderFilter(0, 0.15, 1 / gui_app.target_fps)
+
   def set_wheel_critical_icon(self, critical: bool):
     """Set the wheel icon to critical or normal state."""
     self._show_wheel_critical = critical
@@ -175,19 +180,79 @@ class HudRenderer(Widget):
     if ui_state.sm['controlsState'].lateralControlState.which() != 'angleState':
       self._torque_bar.render(rect)
 
+    draw_set_speed = False
     if self.is_cruise_set:
+      alpha = self._set_speed_alpha_filter.update(
+        0 < rl.get_time() - self._set_speed_changed_time < SET_SPEED_PERSISTENCE and
+        self._can_draw_top_icons and self._engaged
+      )
+      draw_set_speed = alpha >= 1e-2
+
+    if draw_set_speed:
       self._draw_set_speed(rect)
 
     self._draw_steering_wheel(rect)
 
+    self._draw_blind_spot_indicators(rect)
+
+  def _draw_blind_spot_indicators(self, rect: rl.Rectangle) -> None:
+    sm = ui_state.sm
+    car_state = sm['carState']
+
+    try:
+      left_detected = car_state.leftBlindspot
+    except AttributeError:
+      left_detected = False
+
+    try:
+      right_detected = car_state.rightBlindspot
+    except AttributeError:
+      right_detected = False
+
+    self._blind_spot_left_alpha_filter.update(1.0 if left_detected else 0.0)
+    self._blind_spot_right_alpha_filter.update(1.0 if right_detected else 0.0)
+
+    BLIND_SPOT_MARGIN_X = 20  # Distance from edge of screen
+    BLIND_SPOT_Y_OFFSET = 100  # Distance from top of screen
+
+    if self._blind_spot_left_alpha_filter.x > 0.01:
+      pos_x = int(rect.x + BLIND_SPOT_MARGIN_X)
+      pos_y = int(rect.y + BLIND_SPOT_Y_OFFSET)
+      alpha = int(255 * self._blind_spot_left_alpha_filter.x)
+      color = rl.Color(255, 255, 255, alpha)
+      rl.draw_texture(self._txt_blind_spot_left, pos_x, pos_y, color)
+
+    if self._blind_spot_right_alpha_filter.x > 0.01:
+      pos_x = int(rect.x + rect.width - BLIND_SPOT_MARGIN_X - self._txt_blind_spot_right.width)
+      pos_y = int(rect.y + BLIND_SPOT_Y_OFFSET)
+      alpha = int(255 * self._blind_spot_right_alpha_filter.x)
+      color = rl.Color(255, 255, 255, alpha)
+      rl.draw_texture(self._txt_blind_spot_right, pos_x, pos_y, color)
+
+  def _has_blind_spot_detected(self) -> bool:
+    car_state = ui_state.sm['carState']
+    try:
+      left_detected = car_state.leftBlindspot
+    except AttributeError:
+      left_detected = False
+
+    try:
+      right_detected = car_state.rightBlindspot
+    except AttributeError:
+      right_detected = False
+
+    return left_detected or right_detected
+
   def _draw_steering_wheel(self, rect: rl.Rectangle) -> None:
     wheel_txt = self._txt_wheel_critical if self._show_wheel_critical else self._txt_wheel
 
+    has_blind_spot = self._has_blind_spot_detected()
+
     if self._show_wheel_critical:
       self._wheel_alpha_filter.update(255)
       self._wheel_y_filter.update(0)
     else:
-      if ui_state.status == UIStatus.DISENGAGED:
+      if ui_state.status == UIStatus.DISENGAGED or has_blind_spot:
         self._wheel_alpha_filter.update(0)
         self._wheel_y_filter.update(wheel_txt.height / 2)
       else:

selfdrive/ui/mici/onroad/speed_limit_ui.py

@@ -0,0 +1,177 @@
+import pyray as rl
+from dataclasses import dataclass
+from openpilot.common.constants import CV
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.system.ui.lib.application import gui_app, FontWeight
+from openpilot.system.ui.lib.multilang import tr
+from openpilot.system.ui.lib.text_measure import measure_text_cached
+from openpilot.system.ui.widgets import Widget
+
+METER_TO_KM = 0.001
+METER_TO_MILE = 0.000621371
+
+SPEED_LIMIT_WIDTH_METRIC = 200
+SPEED_LIMIT_WIDTH_IMPERIAL = 172
+SPEED_LIMIT_HEIGHT = 204
+
+
+@dataclass(frozen=True)
+class SpeedLimitColors:
+  white: rl.Color = rl.WHITE
+  black: rl.Color = rl.BLACK
+  red: rl.Color = rl.Color(255, 0, 0, 255)
+  grey: rl.Color = rl.Color(145, 155, 149, 241)
+  green: rl.Color = rl.Color(0, 255, 0, 255)
+  light_grey: rl.Color = rl.Color(200, 200, 200, 255)
+  dark_grey: rl.Color = rl.Color(180, 180, 180, 255)
+  border_grey: rl.Color = rl.Color(77, 77, 77, 255)
+  black_translucent: rl.Color = rl.Color(0, 0, 0, 166)
+  border_translucent: rl.Color = rl.Color(255, 255, 255, 75)
+
+
+COLORS = SpeedLimitColors()
+
+
+class SpeedLimitRenderer(Widget):
+  def __init__(self):
+    super().__init__()
+    self.speed_limit: float = 0.0
+    self.speed_limit_valid: bool = False
+    self.next_speed_limit: float = 0.0
+    self.next_speed_limit_distance: float = 0.0
+    self.road_name: str = ""
+    self.current_speed: float = 0.0
+
+    self._font_semi_bold: rl.Font = gui_app.font(FontWeight.SEMI_BOLD)
+    self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD)
+    self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM)
+
+  def _update_state(self) -> None:
+    sm = ui_state.sm
+    if sm.recv_frame["carState"] < ui_state.started_frame:
+      return
+
+    speed_conv = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
+    if sm.valid["mapdOut"]:
+      mapd = sm["mapdOut"]
+      self.speed_limit = mapd.speedLimit * speed_conv
+      self.speed_limit_valid = mapd.speedLimit > 0
+      self.next_speed_limit = mapd.nextSpeedLimit * speed_conv
+      self.next_speed_limit_distance = mapd.nextSpeedLimitDistance
+      self.road_name = mapd.roadName
+    else:
+      self.speed_limit_valid = False
+
+    if sm.updated["carState"]:
+      self.current_speed = sm["carState"].vEgo * speed_conv
+
+  def _render(self, rect: rl.Rectangle) -> None:
+    self._update_state()
+
+    if not self.speed_limit_valid:
+      return
+
+    sign_width = SPEED_LIMIT_WIDTH_METRIC if ui_state.is_metric else SPEED_LIMIT_WIDTH_IMPERIAL
+    sign_height = SPEED_LIMIT_HEIGHT
+    sign_margin = 12
+    sign_x = rect.x + sign_margin
+    sign_y = rect.y + 45
+
+    sign_rect = rl.Rectangle(sign_x, sign_y, sign_width, sign_height)
+
+    speed_str = str(round(self.speed_limit))
+    speed_color = COLORS.white if self.current_speed <= self.speed_limit else COLORS.red
+
+    if ui_state.is_metric:
+      self._draw_vienna_sign(sign_rect, speed_str, "", speed_color, True)
+    else:
+      self._draw_mutcd_sign(sign_rect, speed_str, "", speed_color, True)
+    if self.next_speed_limit > 0 and self.next_speed_limit != self.speed_limit:
+      self._draw_upcoming_speed_limit(sign_rect)
+
+  def _draw_vienna_sign(self, sign_rect: rl.Rectangle, speed_str: str, sub_text: str, speed_color: rl.Color, has_speed_limit: bool) -> None:
+    """Draw EU/Vienna sign."""
+    center = rl.Vector2(sign_rect.x + sign_rect.width / 2, sign_rect.y + sign_rect.height / 2)
+    outer_radius = min(sign_rect.width, sign_rect.height) / 2
+    circle_size = outer_radius * 2
+
+    rl.draw_circle_v(center, outer_radius, COLORS.white)
+    ring_width = outer_radius * 0.18
+    rl.draw_ring(center, outer_radius - ring_width, outer_radius, 0, 360, 36, COLORS.red)
+
+    font_size = circle_size * (0.35 if len(speed_str) >= 3 else 0.45)
+    text_size = measure_text_cached(self._font_bold, speed_str, int(font_size))
+    text_pos = rl.Vector2(center.x - text_size.x / 2, center.y - text_size.y / 2)
+    rl.draw_text_ex(self._font_bold, speed_str, text_pos, font_size, 0, speed_color)
+
+  def _draw_mutcd_sign(self, sign_rect: rl.Rectangle, speed_str: str, sub_text: str, speed_color: rl.Color, has_speed_limit: bool) -> None:
+    """Draw US/Canada MUTCD sign."""
+    padding = 8
+    inner_rect = rl.Rectangle(sign_rect.x + padding, sign_rect.y + padding, sign_rect.width - (padding * 2), sign_rect.height - (padding * 2))
+    rl.draw_rectangle_rounded(inner_rect, 0.35, 10, COLORS.white)
+    rl.draw_rectangle_rounded_lines_ex(inner_rect, 0.35, 10, 4, COLORS.black)
+
+    border_width = 4
+    border_rect = rl.Rectangle(
+      inner_rect.x + border_width, inner_rect.y + border_width, inner_rect.width - (border_width * 2), inner_rect.height - (border_width * 2)
+    )
+    speed_text = tr("SPEED")
+    limit_text = tr("LIMIT")
+
+    speed_size = measure_text_cached(self._font_semi_bold, speed_text, 40)
+    limit_size = measure_text_cached(self._font_semi_bold, limit_text, 40)
+
+    speed_pos = rl.Vector2(border_rect.x + (border_rect.width - speed_size.x) / 2, sign_rect.y + 27)
+    limit_pos = rl.Vector2(border_rect.x + (border_rect.width - limit_size.x) / 2, sign_rect.y + 65)
+
+    rl.draw_text_ex(self._font_semi_bold, speed_text, speed_pos, 40, 0, COLORS.black)
+    rl.draw_text_ex(self._font_semi_bold, limit_text, limit_pos, 40, 0, COLORS.black)
+
+    font_size = 70 if len(speed_str) >= 3 else 90
+    speed_value_size = measure_text_cached(self._font_bold, speed_str, font_size)
+    speed_value_pos = rl.Vector2(border_rect.x + (border_rect.width - speed_value_size.x) / 2, sign_rect.y + 100)
+    rl.draw_text_ex(self._font_bold, speed_str, speed_value_pos, font_size, 0, speed_color)
+
+  def _draw_upcoming_speed_limit(self, sign_rect: rl.Rectangle) -> None:
+    """Draw upcoming speed limit."""
+    speed_str = str(round(self.next_speed_limit))
+    distance_str = self._format_distance(self.next_speed_limit_distance)
+
+    ahead_width = 150
+    ahead_height = 100
+    ahead_x = sign_rect.x + (sign_rect.width - ahead_width) / 2
+    ahead_y = sign_rect.y + sign_rect.height + 6
+    ahead_rect = rl.Rectangle(ahead_x, ahead_y, ahead_width, ahead_height)
+
+    rl.draw_rectangle_rounded(ahead_rect, 0.2, 10, rl.Color(0, 0, 0, 180))
+    rl.draw_rectangle_rounded_lines_ex(ahead_rect, 0.2, 10, 3, rl.Color(255, 255, 255, 100))
+
+    ahead_label = tr("AHEAD")
+    ahead_size = measure_text_cached(self._font_semi_bold, ahead_label, 28)
+    ahead_pos = rl.Vector2(ahead_rect.x + (ahead_rect.width - ahead_size.x) / 2, ahead_rect.y + 4)
+    rl.draw_text_ex(self._font_semi_bold, ahead_label, ahead_pos, 28, 0, COLORS.light_grey)
+
+    speed_size = measure_text_cached(self._font_bold, speed_str, 48)
+    speed_pos = rl.Vector2(ahead_rect.x + (ahead_rect.width - speed_size.x) / 2, ahead_rect.y + 26)
+    rl.draw_text_ex(self._font_bold, speed_str, speed_pos, 48, 0, COLORS.white)
+
+    distance_size = measure_text_cached(self._font_medium, distance_str, 28)
+    distance_pos = rl.Vector2(ahead_rect.x + (ahead_rect.width - distance_size.x) / 2, ahead_rect.y + 70)
+    rl.draw_text_ex(self._font_medium, distance_str, distance_pos, 28, 0, COLORS.dark_grey)
+
+  def _format_distance(self, distance: float) -> str:
+    if ui_state.is_metric:
+      if distance < 50:
+        return tr("Near")
+      if distance >= 1000:
+        return f"{distance * METER_TO_KM:.1f}" + tr("km")
+      if distance < 200:
+        rounded = max(10, int(distance / 10) * 10)
+      else:
+        rounded = int(distance / 100) * 100
+      return str(rounded) + tr("m")
+    else:
+      distance_mi = distance * METER_TO_MILE
+      if distance_mi < 0.1:
+        return tr("Near")
+      return f"{distance_mi:.1f}" + tr("mi")

selfdrive/ui/onroad/hud_renderer.py

@@ -2,6 +2,7 @@ import pyray as rl
 from dataclasses import dataclass
 from openpilot.common.constants import CV
 from openpilot.selfdrive.ui.onroad.exp_button import ExpButton
+from openpilot.selfdrive.ui.onroad.speed_limit_ui import SpeedLimitRenderer
 from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
 from openpilot.system.ui.lib.application import gui_app, FontWeight
 from openpilot.system.ui.lib.multilang import tr
@@ -71,6 +72,7 @@ class HudRenderer(Widget):
     self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM)
 
     self._exp_button: ExpButton = ExpButton(UI_CONFIG.button_size, UI_CONFIG.wheel_icon_size)
+    self._speed_limit_renderer: SpeedLimitRenderer = SpeedLimitRenderer()
 
   def _update_state(self) -> None:
     """Update HUD state based on car state and controls state."""
@@ -112,6 +114,8 @@ class HudRenderer(Widget):
       COLORS.HEADER_GRADIENT_END,
     )
 
+    self._speed_limit_renderer.render(rect)
+
     if self.is_cruise_available:
       self._draw_set_speed(rect)
 

selfdrive/ui/onroad/speed_limit_ui.py

@@ -0,0 +1,235 @@
+"""
+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 pyray as rl
+from dataclasses import dataclass
+from openpilot.common.constants import CV
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.system.ui.lib.application import gui_app, FontWeight
+from openpilot.system.ui.lib.multilang import tr
+from openpilot.system.ui.lib.text_measure import measure_text_cached
+from openpilot.system.ui.widgets import Widget
+
+METER_TO_KM = 0.001
+METER_TO_MILE = 0.000621371
+
+
+@dataclass(frozen=True)
+class SpeedLimitColors:
+  white: rl.Color = rl.WHITE
+  black: rl.Color = rl.BLACK
+  red: rl.Color = rl.Color(255, 0, 0, 255)
+  green: rl.Color = rl.Color(0, 255, 0, 255)
+  grey: rl.Color = rl.Color(145, 155, 149, 241)
+  light_grey: rl.Color = rl.Color(200, 200, 200, 255)
+  dark_grey: rl.Color = rl.Color(100, 100, 100, 255)
+  bg_dark: rl.Color = rl.Color(20, 20, 20, 230)
+  card_bg: rl.Color = rl.Color(50, 50, 50, 200)
+
+
+COLORS = SpeedLimitColors()
+
+
+class SpeedLimitRenderer(Widget):
+  def __init__(self):
+    super().__init__()
+    self.speed_limit: float = 0.0
+    self.speed_limit_valid: bool = False
+    self.next_speed_limit: float = 0.0
+    self.next_speed_limit_distance: float = 0.0
+    self.road_name: str = ""
+    self.current_speed: float = 0.0
+    self.lanes: int = 0
+    self.road_context: str = ""
+    self.hazard: str = ""
+    self.vision_curve_speed: float = 0.0
+    self.map_curve_speed: float = 0.0
+    self.tile_loaded: bool = False
+
+    self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD)
+    self._font_semi_bold: rl.Font = gui_app.font(FontWeight.SEMI_BOLD)
+    self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM)
+
+  def _update_state(self) -> None:
+    sm = ui_state.sm
+    if sm.recv_frame["carState"] < ui_state.started_frame:
+      return
+
+    speed_conv = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
+
+    if sm.valid["mapdOut"]:
+      mapd = sm["mapdOut"]
+      self.speed_limit = mapd.speedLimit * speed_conv
+      self.speed_limit_valid = mapd.speedLimit > 0
+      self.next_speed_limit = mapd.nextSpeedLimit * speed_conv
+      self.next_speed_limit_distance = mapd.nextSpeedLimitDistance
+      self.road_name = mapd.roadName
+      self.lanes = mapd.lanes
+      self.hazard = mapd.hazard
+      self.vision_curve_speed = mapd.visionCurveSpeed * speed_conv
+      self.map_curve_speed = mapd.mapCurveSpeed * speed_conv
+      self.tile_loaded = mapd.tileLoaded
+
+      road_ctx = mapd.roadContext
+      if hasattr(road_ctx, 'raw'):
+        ctx_val = road_ctx.raw
+      else:
+        ctx_val = int(road_ctx)
+      self.road_context = ["Freeway", "City", "Unknown"][ctx_val] if ctx_val < 3 else "Unknown"
+    else:
+      self.speed_limit_valid = False
+      self.tile_loaded = False
+
+    if sm.updated["carState"]:
+      self.current_speed = sm["carState"].vEgo * speed_conv
+
+  def _render(self, rect: rl.Rectangle) -> None:
+    self._update_state()
+    panel_width = 420
+    panel_height = 280
+    panel_margin = 20
+    panel_x = rect.x + panel_margin
+    panel_y = rect.y + (rect.height - panel_height) / 2
+
+    panel_rect = rl.Rectangle(panel_x, panel_y, panel_width, panel_height)
+    rl.draw_rectangle_rounded(panel_rect, 0.08, 10, COLORS.bg_dark)
+    rl.draw_rectangle_rounded_lines_ex(panel_rect, 0.08, 10, 2, rl.Color(80, 80, 80, 200))
+
+    padding = 20
+    sign_width = 140 if ui_state.is_metric else 130
+    sign_height = 150 if ui_state.is_metric else 140
+    sign_x = panel_x + padding
+    sign_y = panel_y + padding
+
+    self._draw_speed_limit_sign(sign_x, sign_y, sign_width, sign_height)
+    info_x = sign_x + sign_width + 20
+    info_width = panel_width - sign_width - padding * 2 - 20
+    info_y = panel_y + padding + 5
+
+    # Road name
+    self._draw_road_name(info_x, info_y, info_width)
+    info_y += 50
+
+    # Road context and lanes
+    self._draw_road_details(info_x, info_y, info_width)
+    info_y += 40
+
+    # Next speed limit
+    self._draw_next_speed_limit(info_x, info_y, info_width)
+    info_y += 40
+
+    # Curve speeds
+    self._draw_curve_speeds(info_x, info_y, info_width)
+
+    # Hazard warning beta
+    if self.hazard:
+      hazard_y = panel_y + panel_height - 60
+      self._draw_hazard(panel_x + padding, hazard_y, panel_width - padding * 2)
+
+    status_text = tr("Map Loaded") if self.tile_loaded else tr("No Map Data")
+    status_color = COLORS.green if self.tile_loaded else COLORS.red
+    status_size = measure_text_cached(self._font_medium, status_text, 22)
+    status_pos = rl.Vector2(panel_x + (panel_width - status_size.x) / 2, panel_y + panel_height - 30)
+    rl.draw_text_ex(self._font_medium, status_text, status_pos, 22, 0, status_color)
+
+  def _draw_speed_limit_sign(self, x: float, y: float, width: float, height: float) -> None:
+    speed_str = str(round(self.speed_limit)) if self.speed_limit_valid else "--"
+    speed_color = COLORS.black if not self.speed_limit_valid or self.current_speed <= self.speed_limit else COLORS.red
+
+    if ui_state.is_metric:
+      self._draw_vienna_sign(x, y, width, height, speed_str, speed_color)
+    else:
+      self._draw_mutcd_sign(x, y, width, height, speed_str, speed_color)
+
+  def _draw_vienna_sign(self, x: float, y: float, width: float, height: float, speed_str: str, speed_color: rl.Color) -> None:
+    center = rl.Vector2(x + width / 2, y + height / 2)
+    outer_radius = min(width, height) / 2
+
+    rl.draw_circle_v(center, outer_radius, COLORS.white)
+    ring_width = outer_radius * 0.18
+    rl.draw_ring(center, outer_radius - ring_width, outer_radius, 0, 360, 36, COLORS.red)
+
+    font_size = outer_radius * (0.75 if len(speed_str) >= 3 else 0.95)
+    text_size = measure_text_cached(self._font_bold, speed_str, int(font_size))
+    text_pos = rl.Vector2(center.x - text_size.x / 2, center.y - text_size.y / 2)
+    rl.draw_text_ex(self._font_bold, speed_str, text_pos, font_size, 0, speed_color)
+
+  def _draw_mutcd_sign(self, x: float, y: float, width: float, height: float, speed_str: str, speed_color: rl.Color) -> None:
+    sign_rect = rl.Rectangle(x, y, width, height)
+    rl.draw_rectangle_rounded(sign_rect, 0.2, 10, COLORS.white)
+    rl.draw_rectangle_rounded_lines_ex(sign_rect, 0.2, 10, 4, COLORS.black)
+
+    speed_label = tr("SPEED")
+    limit_label = tr("LIMIT")
+    label_size = 22
+
+    speed_text_size = measure_text_cached(self._font_semi_bold, speed_label, label_size)
+    speed_pos = rl.Vector2(x + (width - speed_text_size.x) / 2, y + 12)
+    rl.draw_text_ex(self._font_semi_bold, speed_label, speed_pos, label_size, 0, COLORS.black)
+
+    limit_text_size = measure_text_cached(self._font_semi_bold, limit_label, label_size)
+    limit_pos = rl.Vector2(x + (width - limit_text_size.x) / 2, y + 36)
+    rl.draw_text_ex(self._font_semi_bold, limit_label, limit_pos, label_size, 0, COLORS.black)
+
+    font_size = 55 if len(speed_str) >= 3 else 70
+    num_size = measure_text_cached(self._font_bold, speed_str, font_size)
+    num_pos = rl.Vector2(x + (width - num_size.x) / 2, y + 65)
+    rl.draw_text_ex(self._font_bold, speed_str, num_pos, font_size, 0, speed_color)
+
+  def _draw_road_name(self, x: float, y: float, width: float) -> None:
+    road_display = self.road_name if self.road_name else "--"
+    font_size = 32
+    road_size = measure_text_cached(self._font_semi_bold, road_display, font_size)
+    if road_size.x > width:
+      while len(road_display) > 3 and measure_text_cached(self._font_semi_bold, road_display + "...", font_size).x > width:
+        road_display = road_display[:-1]
+      road_display = road_display + "..."
+    rl.draw_text_ex(self._font_semi_bold, road_display, rl.Vector2(x, y), font_size, 0, COLORS.white)
+
+  def _draw_road_details(self, x: float, y: float, width: float) -> None:
+    details = []
+    if self.road_context and self.road_context != "Unknown":
+      details.append(self.road_context)
+    if self.lanes > 0:
+      details.append(f"{self.lanes} {tr('lanes')}")
+    details_text = " • ".join(details) if details else "--"
+    rl.draw_text_ex(self._font_medium, details_text, rl.Vector2(x, y), 26, 0, COLORS.light_grey)
+
+  def _draw_next_speed_limit(self, x: float, y: float, width: float) -> None:
+    if self.next_speed_limit > 0 and self.next_speed_limit != self.speed_limit:
+      next_text = f"{tr('Next')}: {round(self.next_speed_limit)} ({self._format_distance(self.next_speed_limit_distance)})"
+    else:
+      next_text = f"{tr('Next')}: --"
+    rl.draw_text_ex(self._font_medium, next_text, rl.Vector2(x, y), 26, 0, COLORS.light_grey)
+
+  def _draw_curve_speeds(self, x: float, y: float, width: float) -> None:
+    unit = tr("km/h") if ui_state.is_metric else tr("mph")
+    vision_val = str(round(self.vision_curve_speed)) if self.vision_curve_speed > 0 else "--"
+    map_val = str(round(self.map_curve_speed)) if self.map_curve_speed > 0 else "--"
+    curve_text = f"{tr('Curve')}: V:{vision_val} M:{map_val} {unit}"
+    rl.draw_text_ex(self._font_medium, curve_text, rl.Vector2(x, y), 24, 0, COLORS.light_grey)
+
+  def _draw_hazard(self, x: float, y: float, width: float) -> None:
+    hazard_text = f"⚠ {self.hazard}"
+    rl.draw_text_ex(self._font_semi_bold, hazard_text, rl.Vector2(x, y), 28, 0, COLORS.red)
+
+  def _format_distance(self, distance: float) -> str:
+    if ui_state.is_metric:
+      if distance < 50:
+        return tr("Near")
+      if distance >= 1000:
+        return f"{distance * METER_TO_KM:.1f}" + tr("km")
+      if distance < 200:
+        rounded = max(10, int(distance / 10) * 10)
+      else:
+        rounded = int(distance / 100) * 100
+      return str(rounded) + tr("m")
+    else:
+      distance_mi = distance * METER_TO_MILE
+      if distance_mi < 0.1:
+        return tr("Near")
+      return f"{distance_mi:.1f}" + tr("mi")

selfdrive/ui/sunnypilot/layouts/settings/osm.py

@@ -1,232 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import datetime
-import os
-import platform
-import requests
-import shutil
-import threading
-from pathlib import Path
-from time import monotonic
-
-from openpilot.common.params import Params
-from openpilot.selfdrive.ui.ui_state import device, ui_state
-from openpilot.selfdrive.ui.layouts.settings.software import time_ago
-from openpilot.system.hardware.hw import Paths
-from openpilot.system.ui.lib.application import gui_app
-from openpilot.system.ui.lib.multilang import tr
-from openpilot.system.ui.widgets import DialogResult, Widget
-from openpilot.system.ui.widgets.confirm_dialog import ConfirmDialog
-from openpilot.system.ui.widgets.list_view import text_item
-from openpilot.system.ui.widgets.scroller_tici import Scroller
-
-from openpilot.system.ui.sunnypilot.lib.utils import NoElideButtonAction
-from openpilot.system.ui.sunnypilot.widgets.list_view import ListItemSP
-from openpilot.system.ui.sunnypilot.widgets.tree_dialog import TreeFolder, TreeNode, TreeOptionDialog
-from openpilot.system.ui.sunnypilot.widgets.progress_bar import progress_item
-
-MAP_PATH = Path(Paths.mapd_root()) / "offline"
-
-
-class OSMLayout(Widget):
-  def __init__(self):
-    super().__init__()
-    self._current_percent = 0
-    self._last_map_size_update = 0
-    self._mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else ui_state.params
-    self._initialize_items()
-    self._update_map_size()
-    self._progress.set_visible(False)
-    self._state_btn.set_visible(False)
-    self._mapd_version.action_item.set_text(ui_state.params.get("MapdVersion") or "Loading...")
-    self._scroller = Scroller(self.items, line_separator=True, spacing=0)
-
-  def _initialize_items(self):
-    self._mapd_version = text_item(tr("Mapd Version"), lambda: ui_state.params.get("MapdVersion") or "Loading...")
-    self._delete_maps_btn = ListItemSP(tr("Downloaded Maps"), action_item=NoElideButtonAction(tr("DELETE"), enabled=True), callback=self._delete_maps)
-    self._progress = progress_item(tr("Downloading Map"))
-    self._update_btn = ListItemSP(tr("Database Update"), action_item=NoElideButtonAction(tr("CHECK"), enabled=True), callback=self._update_db)
-    self._country_btn = ListItemSP(tr("Country"), action_item=NoElideButtonAction(tr("SELECT"), enabled=True), callback=lambda: self._select_region("Country"))
-    self._state_btn = ListItemSP(tr("State"), action_item=NoElideButtonAction(tr("SELECT"), enabled=True), callback=lambda: self._select_region("State"))
-
-    self.items = [self._mapd_version, self._delete_maps_btn, self._progress, self._update_btn, self._country_btn, self._state_btn]
-
-  def _show_confirm(self, msg, confirm_text, func):
-    gui_app.set_modal_overlay(ConfirmDialog(msg, confirm_text), lambda res: func() if res == DialogResult.CONFIRM else None)
-
-  def calculate_size(self):
-    total_size = 0
-    directories_to_scan = [MAP_PATH] if MAP_PATH.exists() else []
-    while directories_to_scan:
-      try:
-        for entry in os.scandir(directories_to_scan.pop()):
-          if entry.is_file():
-            total_size += entry.stat().st_size
-          elif entry.is_dir():
-            directories_to_scan.append(entry.path)
-      except OSError:
-        pass
-    self._delete_maps_btn.action_item.set_value(f"{total_size / 1024 ** 2:.2f} MB" if total_size < 1024 ** 3 else f"{total_size / 1024 ** 3:.2f} GB")
-
-  def _update_map_size(self):
-    threading.Thread(target=self.calculate_size, daemon=True).start()
-
-  def _do_delete_maps(self):
-    if MAP_PATH.exists():
-      shutil.rmtree(MAP_PATH)
-
-    for param in ("OsmDownloadedDate", "OsmLocal", "OsmLocationName", "OsmLocationTitle", "OsmStateName", "OsmStateTitle"):
-      ui_state.params.remove(param)
-
-    self._delete_maps_btn.action_item.set_enabled(True)
-    self._delete_maps_btn.action_item.set_text(tr("DELETE"))
-    self._update_map_size()
-
-  def _on_confirm_delete_maps(self):
-    self._delete_maps_btn.action_item.set_enabled(False)
-    self._delete_maps_btn.action_item.set_text("DELETING...")
-    threading.Thread(target=self._do_delete_maps).start()
-
-  def _delete_maps(self):
-    self._show_confirm(tr("This will delete ALL downloaded maps\n\nAre you sure you want to delete all maps?"),
-                       tr("Yes, delete all maps"), self._on_confirm_delete_maps)
-
-  def _update_db(self):
-    self._show_confirm(tr("This will start the download process and it might take a while to complete."), tr("Start Download"),
-                       lambda: ui_state.params.put_bool("OsmDbUpdatesCheck", True))
-
-  def _select_region(self, region_type):
-    is_country = region_type == "Country"
-    btn = self._country_btn if is_country else self._state_btn
-    btn.action_item.set_enabled(False)
-    btn.action_item.set_text(tr("FETCHING..."))
-    threading.Thread(target=self._do_select_region, args=(region_type, btn)).start()
-
-  def _handle_region_selection(self, region_type, locations, key, res, ref):
-    if res != DialogResult.CONFIRM or not ref:
-      if region_type == "State" and res == DialogResult.CANCEL:
-        if ui_state.params.get("OsmLocationName") == "US" and not ui_state.params.get("OsmStateName"):
-          ui_state.params.remove("OsmLocationName")
-          ui_state.params.remove("OsmLocationTitle")
-          ui_state.params.remove("OsmLocal")
-          self._update_labels()
-      return
-
-    if region_type == "Country":
-      ui_state.params.put_bool("OsmLocal", True)
-      ui_state.params.remove("OsmStateName")
-      ui_state.params.remove("OsmStateTitle")
-
-    ui_state.params.put(f"{key}Name", ref)
-    name = next((n.data['display_name'] for n in locations if n.ref == ref), ref)
-    ui_state.params.put(f"{key}Title", name)
-
-    if ref == "US" and region_type == "Country":
-      self._select_region("State")
-    else:
-      self._update_db()
-
-  def _do_select_region(self, region_type, btn):
-    base_url = "https://raw.githubusercontent.com/pfeiferj/openpilot-mapd/main/"
-    url = base_url + ("nation_bounding_boxes.json" if region_type == "Country" else "us_states_bounding_boxes.json")
-    try:
-      data = requests.get(url, timeout=10).json()
-      locations = sorted([TreeNode(ref=k, data={'display_name': v['full_name']}) for k, v in data.items()], key=lambda n: n.data['display_name'])
-    except Exception:
-      locations = []
-
-    if region_type == "State":
-      locations.insert(0, TreeNode(ref="All", data={'display_name': tr("All states (~6.0 GB)")}))
-
-    btn.action_item.set_enabled(True)
-    btn.action_item.set_text(tr("SELECT"))
-
-    key = "OsmLocation" if region_type == "Country" else "OsmState"
-    current = ui_state.params.get(f"{key}Name") or ""
-
-    dialog = TreeOptionDialog(tr(f"Select {region_type}"), [TreeFolder(folder="", nodes=locations)], current_ref=current, search_prompt="Perform a search")
-    dialog.on_exit = lambda res: self._handle_region_selection(region_type, locations, key, res, dialog.selection_ref)
-    gui_app.set_modal_overlay(dialog, callback=lambda res: self._handle_region_selection(region_type, locations, key, res, dialog.selection_ref))
-
-  def _update_labels(self):
-    downloading = bool(self._mem_params.get("OSMDownloadLocations"))
-    self._country_btn.set_enabled(not downloading)
-    self._state_btn.set_enabled(not downloading)
-    self._state_btn.set_visible(ui_state.params.get("OsmLocationName") == "US")
-    self._update_btn.set_visible(bool(ui_state.params.get("OsmLocationName")))
-
-    self._country_btn.action_item.set_value(ui_state.params.get("OsmLocationTitle") or "")
-    self._state_btn.action_item.set_value(ui_state.params.get("OsmStateTitle") or "")
-
-    pending = ui_state.params.get_bool("OsmDbUpdatesCheck")
-    if downloading or pending:
-      if downloading:
-        device._reset_interactive_timeout()
-        self._update_map_size()
-      self._progress.set_visible(True)
-      progress = ui_state.params.get("OSMDownloadProgress")
-      total = progress.get('total_files', 0) if progress else 0
-      done = progress.get('downloaded_files', 0) if progress else 0
-      failed = total > 0 and not downloading and done < total
-
-      if total > 0:
-        progress_perc = max(0.0, min(100.0, (done / total) * 100.0))
-      else:
-        progress_perc = 0.0
-
-      if failed:
-        text = "0% - Downloading Maps"
-        btn_text = tr("Error: Invalid download. Retry.")
-        self._current_percent = 0.0
-      elif total > 0 and downloading:
-        self._current_percent = progress_perc
-        perc_int = int(progress_perc)
-        text = f"{perc_int}% - Downloading Maps"
-        btn_text = f"{done}/{total} ({perc_int}%)"
-      else:
-        self._current_percent = 0.0
-        text = "0% - Downloading Maps"
-        btn_text = tr("Downloading Maps...")
-
-      self._progress.action_item.update(self._current_percent, text, show_progress=total > 0 and downloading and not failed)
-      self._update_btn.action_item.set_enabled(not downloading)  # TODO-SP: introduce CANCEL database download with mapd
-      self._update_btn.action_item.set_value(btn_text)
-      self._country_btn.action_item.set_enabled(not downloading)
-      self._state_btn.action_item.set_enabled(not downloading)
-      self._delete_maps_btn.action_item.set_enabled(not downloading)
-    else:
-      self._progress.set_visible(False)
-      self._update_btn.action_item.set_enabled(True)
-      self._country_btn.action_item.set_enabled(True)
-      self._state_btn.action_item.set_enabled(True)
-      self._delete_maps_btn.action_item.set_enabled(True)
-
-      ts = ui_state.params.get("OsmDownloadedDate")
-      dt: datetime.datetime | None = None
-
-      if ts:
-        try:
-          ts_f = float(ts)
-          if ts_f > 0:
-            dt = datetime.datetime.fromtimestamp(ts_f, tz=datetime.UTC)
-        except (ValueError, TypeError):
-          dt = None
-
-      formatted = time_ago(dt)
-      self._update_btn.action_item.set_value(tr("Last checked {}").format(formatted))
-
-  def show_event(self):
-    self._scroller.show_event()
-
-  def _update_state(self):
-    now = monotonic()
-    if now - self._last_map_size_update >= 1.0:
-      self._last_map_size_update = now
-      self._update_labels()
-
-  def _render(self, rect):
-    self._scroller.render(rect)

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

@@ -17,7 +17,6 @@ from openpilot.selfdrive.ui.sunnypilot.layouts.settings.device import DeviceLayo
 from openpilot.selfdrive.ui.sunnypilot.layouts.settings.display import DisplayLayout
 from openpilot.selfdrive.ui.sunnypilot.layouts.settings.models import ModelsLayout
 from openpilot.selfdrive.ui.sunnypilot.layouts.settings.network import NetworkUISP
-from openpilot.selfdrive.ui.sunnypilot.layouts.settings.osm import OSMLayout
 from openpilot.selfdrive.ui.sunnypilot.layouts.settings.software import SoftwareLayoutSP
 from openpilot.selfdrive.ui.sunnypilot.layouts.settings.steering import SteeringLayout
 from openpilot.selfdrive.ui.sunnypilot.layouts.settings.sunnylink import SunnylinkLayout
@@ -46,7 +45,6 @@ OP.PanelType = IntEnum(
     "CRUISE",
     "VISUALS",
     "DISPLAY",
-    "OSM",
     "NAVIGATION",
     "TRIPS",
     "VEHICLE",
@@ -120,7 +118,6 @@ class SettingsLayoutSP(OP.SettingsLayout):
       OP.PanelType.CRUISE: PanelInfo(tr_noop("Cruise"), CruiseLayout(), icon="icons/speed_limit.png"),
       OP.PanelType.VISUALS: PanelInfo(tr_noop("Visuals"), VisualsLayout(), icon="../../sunnypilot/selfdrive/assets/offroad/icon_visuals.png"),
       OP.PanelType.DISPLAY: PanelInfo(tr_noop("Display"), DisplayLayout(), icon="../../sunnypilot/selfdrive/assets/offroad/icon_display.png"),
-      OP.PanelType.OSM: PanelInfo(tr_noop("OSM"), OSMLayout(), icon="../../sunnypilot/selfdrive/assets/offroad/icon_map.png"),
       # OP.PanelType.NAVIGATION: PanelInfo(tr_noop("Navigation"), NavigationLayout(), icon="../../sunnypilot/selfdrive/assets/offroad/icon_map.png"),
       OP.PanelType.TRIPS: PanelInfo(tr_noop("Trips"), TripsLayout(), icon="../../sunnypilot/selfdrive/assets/offroad/icon_trips.png"),
       OP.PanelType.VEHICLE: PanelInfo(tr_noop("Vehicle"), VehicleLayout(), icon="../../sunnypilot/selfdrive/assets/offroad/icon_vehicle.png"),

selfdrive/ui/sunnypilot/ui_state.py

@@ -29,7 +29,8 @@ class UIStateSP:
     self.params = Params()
     self.sm_services_ext = [
       "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
-      "gpsLocation", "liveTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "liveDelay"
+      "gpsLocation", "liveTorqueParameters", "carStateSP", "carParamsSP", "liveDelay",
+      "mapdOut"
     ]
 
     self.sunnylink_state = SunnylinkState()

sunnypilot/mapd/__init__.py

@@ -1,5 +0,0 @@
-import os
-from openpilot.common.basedir import BASEDIR
-
-MAPD_BIN_DIR = os.path.join(BASEDIR, 'third_party/mapd_pfeiferj')
-MAPD_PATH = os.path.join(MAPD_BIN_DIR, 'mapd')

sunnypilot/mapd/live_map_data/__init__.py

@@ -1,22 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-from openpilot.common.swaglog import cloudlog
-
-LOOK_AHEAD_HORIZON_TIME = 15.  # s. Time horizon for look ahead of turn speed sections to provide on liveMapDataSP msg.
-_DEBUG = False
-_CLOUDLOG_DEBUG = False
-ROAD_NAME_TIMEOUT = 30  # secs
-R = 6373000.0  # approximate radius of earth in mts
-QUERY_RADIUS = 3000  # mts. Radius to use on OSM data queries.
-QUERY_RADIUS_OFFLINE = 2250  # mts. Radius to use on offline OSM data queries.
-
-
-def get_debug(msg, log_to_cloud=True):
-  if _CLOUDLOG_DEBUG and log_to_cloud:
-    cloudlog.debug(msg)
-  if _DEBUG:
-    print(msg)

sunnypilot/mapd/live_map_data/base_map_data.py

@@ -1,65 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-from abc import abstractmethod, ABC
-
-import cereal.messaging as messaging
-from openpilot.common.params import Params
-from openpilot.common.constants import CV
-from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
-from openpilot.sunnypilot.navd.helpers import coordinate_from_param
-
-MAX_SPEED_LIMIT = V_CRUISE_UNSET * CV.KPH_TO_MS
-
-
-class BaseMapData(ABC):
-  def __init__(self):
-    self.params = Params()
-
-    self.sm = messaging.SubMaster(['liveLocationKalman'])
-    self.pm = messaging.PubMaster(['liveMapDataSP'])
-
-    self.localizer_valid = False
-    self.last_bearing = None
-    self.last_position = coordinate_from_param("LastGPSPositionLLK", self.params)
-
-  @abstractmethod
-  def update_location(self) -> None:
-    pass
-
-  @abstractmethod
-  def get_current_speed_limit(self) -> float:
-    pass
-
-  @abstractmethod
-  def get_next_speed_limit_and_distance(self) -> tuple[float, float]:
-    pass
-
-  @abstractmethod
-  def get_current_road_name(self) -> str:
-    pass
-
-  def publish(self) -> None:
-    speed_limit = self.get_current_speed_limit()
-    next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance()
-
-    mapd_sp_send = messaging.new_message('liveMapDataSP')
-    mapd_sp_send.valid = self.sm['liveLocationKalman'].gpsOK
-    live_map_data = mapd_sp_send.liveMapDataSP
-
-    live_map_data.speedLimitValid = bool(MAX_SPEED_LIMIT > speed_limit > 0)
-    live_map_data.speedLimit = speed_limit
-    live_map_data.speedLimitAheadValid = bool(MAX_SPEED_LIMIT > next_speed_limit > 0)
-    live_map_data.speedLimitAhead = next_speed_limit
-    live_map_data.speedLimitAheadDistance = next_speed_limit_distance
-    live_map_data.roadName = self.get_current_road_name()
-
-    self.pm.send('liveMapDataSP', mapd_sp_send)
-
-  def tick(self) -> None:
-    self.sm.update(0)
-    self.update_location()
-    self.publish()

sunnypilot/mapd/live_map_data/debug.py

@@ -1,56 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-# DISCLAIMER: This code is intended principally for development and debugging purposes.
-# Although it provides a standalone entry point to the program, users should refer
-# to the actual implementations for consumption. Usage outside of development scenarios
-# is not advised and could lead to unpredictable results.
-
-import threading
-import traceback
-
-from cereal import messaging
-from openpilot.common.gps import get_gps_location_service
-from openpilot.common.params import Params
-from openpilot.common.realtime import config_realtime_process
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Policy
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver
-from openpilot.sunnypilot.mapd.live_map_data import get_debug
-
-
-def excepthook(args):
-  get_debug(f'MapD: Threading exception:\n{args}')
-  traceback.print_exception(args.exc_type, args.exc_value, args.exc_traceback)
-
-
-def live_map_data_sp_thread():
-  config_realtime_process([0, 1, 2, 3], 5)
-
-  params = Params()
-  gps_location_service = get_gps_location_service(params)
-
-  while True:
-    live_map_data_sp_thread_debug(gps_location_service)
-
-
-def live_map_data_sp_thread_debug(gps_location_service):
-  _sub_master = messaging.SubMaster(['carState', 'livePose', 'liveMapDataSP', 'longitudinalPlanSP', 'carStateSP', gps_location_service])
-  _sub_master.update()
-
-  v_ego = _sub_master['carState'].vEgo
-  _resolver = SpeedLimitResolver()
-  _resolver.policy = Policy.car_state_priority
-  _resolver.update(v_ego, _sub_master)
-  print(_resolver.speed_limit, _resolver.distance, _resolver.source)
-
-
-def main():
-  threading.excepthook = excepthook
-  live_map_data_sp_thread()
-
-
-if __name__ == "__main__":
-  main()

sunnypilot/mapd/live_map_data/osm_map_data.py

@@ -1,61 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import json
-import math
-import platform
-
-from cereal import log
-from openpilot.common.params import Params
-from openpilot.sunnypilot.mapd.live_map_data.base_map_data import BaseMapData
-from openpilot.sunnypilot.navd.helpers import Coordinate
-
-
-class OsmMapData(BaseMapData):
-  def __init__(self):
-    super().__init__()
-    self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
-
-  def update_location(self) -> None:
-    location = self.sm['liveLocationKalman']
-    self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
-
-    if self.localizer_valid:
-      self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
-      self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
-
-    if self.last_position is None:
-      return
-
-    params = {
-      "latitude": self.last_position.latitude,
-      "longitude": self.last_position.longitude,
-    }
-
-    if self.last_bearing is not None:
-      params['bearing'] = self.last_bearing
-
-    self.mem_params.put("LastGPSPosition", json.dumps(params))
-
-  def get_current_speed_limit(self) -> float:
-    return float(self.mem_params.get("MapSpeedLimit") or 0.0)
-
-  def get_current_road_name(self) -> str:
-    return str(self.mem_params.get("RoadName") or "")
-
-  def get_next_speed_limit_and_distance(self) -> tuple[float, float]:
-    next_speed_limit_section_str = self.mem_params.get("NextMapSpeedLimit")
-    next_speed_limit_section = next_speed_limit_section_str if next_speed_limit_section_str else {}
-    next_speed_limit = next_speed_limit_section.get('speedlimit', 0.0)
-    next_speed_limit_latitude = next_speed_limit_section.get('latitude')
-    next_speed_limit_longitude = next_speed_limit_section.get('longitude')
-    next_speed_limit_distance = 0.0
-
-    if next_speed_limit_latitude and next_speed_limit_longitude:
-      next_speed_limit_coordinates = Coordinate(next_speed_limit_latitude, next_speed_limit_longitude)
-      next_speed_limit_distance = (self.last_position or Coordinate(0, 0)).distance_to(next_speed_limit_coordinates)
-
-    return next_speed_limit, next_speed_limit_distance

sunnypilot/mapd/live_map_data/standalone.py

@@ -1,42 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-# DISCLAIMER: This code is intended principally for development and debugging purposes.
-# Although it provides a standalone entry point to the program, users should refer
-# to the actual implementations for consumption. Usage outside of development scenarios
-# is not advised and could lead to unpredictable results.
-
-import threading
-import traceback
-
-from openpilot.common.realtime import Ratekeeper, config_realtime_process
-from openpilot.sunnypilot.mapd.live_map_data import get_debug
-from openpilot.sunnypilot.mapd.live_map_data.osm_map_data import OsmMapData
-
-
-def excepthook(args):
-  get_debug(f'MapD: Threading exception:\n{args}')
-  traceback.print_exception(args.exc_type, args.exc_value, args.exc_traceback)
-
-
-def live_map_data_sp_thread():
-  config_realtime_process([0, 1, 2, 3], 5)
-
-  live_map_sp = OsmMapData()
-  rk = Ratekeeper(1, print_delay_threshold=None)
-
-  while True:
-    live_map_sp.tick()
-    rk.keep_time()
-
-
-def main():
-  threading.excepthook = excepthook
-  live_map_data_sp_thread()
-
-
-if __name__ == "__main__":
-  main()

sunnypilot/mapd/mapd_installer.py

@@ -1,154 +0,0 @@
-#!/usr/bin/env python3
-"""
-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 logging
-import os
-import stat
-import time
-import traceback
-import requests
-from pathlib import Path
-from urllib.request import urlopen
-
-from cereal import messaging
-from openpilot.common.params import Params
-from openpilot.system.hardware.hw import Paths
-from openpilot.common.spinner import Spinner
-from openpilot.system.version import is_prebuilt
-from openpilot.sunnypilot.mapd import MAPD_PATH, MAPD_BIN_DIR
-import openpilot.system.sentry as sentry
-
-VERSION = "v1.12.0"
-URL = f"https://github.com/pfeiferj/openpilot-mapd/releases/download/{VERSION}/mapd"
-
-
-def update_installed_version(version: str, params: Params = None) -> None:
-  if params is None:
-    params = Params()
-
-  params.put("MapdVersion", version)
-
-
-class MapdInstallManager:
-  def __init__(self, spinner_ref: Spinner):
-    self._spinner = spinner_ref
-    self._params = Params()
-
-  def download(self) -> None:
-    self.ensure_directories_exist()
-    self._download_file()
-    update_installed_version(VERSION, self._params)
-
-  def check_and_download(self) -> None:
-    if self.download_needed():
-      self.download()
-
-  def download_needed(self) -> bool:
-    return not os.path.exists(MAPD_PATH) or self.get_installed_version() != VERSION
-
-  @staticmethod
-  def ensure_directories_exist() -> None:
-    if not os.path.exists(Paths.mapd_root()):
-      os.makedirs(Paths.mapd_root())
-    if not os.path.exists(MAPD_BIN_DIR):
-      os.makedirs(MAPD_BIN_DIR)
-
-  @staticmethod
-  def _safe_write_and_set_executable(file_path: Path, content: bytes) -> None:
-    with open(file_path, 'wb') as output:
-      output.write(content)
-      output.flush()
-      os.fsync(output.fileno())
-    current_permissions = stat.S_IMODE(os.lstat(file_path).st_mode)
-    os.chmod(file_path, current_permissions | stat.S_IEXEC)
-
-  def _download_file(self, num_retries=5) -> None:
-    temp_file = Path(MAPD_PATH + ".tmp")
-    download_timeout = 60
-    for cnt in range(num_retries):
-      try:
-        response = requests.get(URL, stream=True, timeout=download_timeout)
-        response.raise_for_status()
-        self._safe_write_and_set_executable(temp_file, response.content)
-        # No exceptions encountered. Safe to replace original file.
-        temp_file.replace(MAPD_PATH)
-        return
-      except requests.exceptions.ReadTimeout:
-        self._spinner.update(f"ReadTimeout caught. Timeout is [{download_timeout}]. Retrying download... [{cnt}]")
-        time.sleep(0.5)
-      except requests.exceptions.RequestException as e:
-        self._spinner.update(f"RequestException caught: {e}. Retrying download... [{cnt}]")
-        time.sleep(0.5)
-
-    # Delete temp file if the process was not successful.
-    if temp_file.exists():
-      temp_file.unlink()
-    logging.error("Failed to download file after all retries")
-
-  def get_installed_version(self) -> str:
-    return str(self._params.get("MapdVersion") or "")
-
-  def wait_for_internet_connection(self, return_on_failure: bool = False) -> bool:
-    max_retries = 10
-    for retries in range(max_retries + 1):
-      self._spinner.update(f"Waiting for internet connection... [{retries}/{max_retries}]")
-      time.sleep(2)
-      try:
-        _ = urlopen('https://sentry.io', timeout=10)
-        return True
-      except Exception as e:
-        print(f'Wait for internet failed: {e}')
-        if return_on_failure and retries == max_retries:
-          return False
-
-    return False
-
-  def non_prebuilt_install(self) -> None:
-    sm = messaging.SubMaster(['deviceState'])
-    metered = sm['deviceState'].networkMetered
-
-    if metered:
-      self._spinner.update("Can't proceed with mapd install since network is metered!")
-      time.sleep(5)
-      return
-
-    try:
-      self.ensure_directories_exist()
-      if not self.download_needed():
-        self._spinner.update("Mapd is good!")
-        time.sleep(0.1)
-        return
-
-      if self.wait_for_internet_connection(return_on_failure=True):
-        self._spinner.update(f"Downloading pfeiferj's mapd [{self.get_installed_version()}] => [{VERSION}].")
-        time.sleep(0.1)
-        self.check_and_download()
-      self._spinner.close()
-
-    except Exception:
-      for i in range(6):
-        self._spinner.update("Failed to download OSM maps won't work until properly downloaded!" +
-                             "Try again manually rebooting. " +
-                             f"Boot will continue in {5 - i}s...")
-        time.sleep(1)
-
-      sentry.init(sentry.SentryProject.SELFDRIVE)
-      traceback.print_exc()
-      sentry.capture_exception()
-
-
-if __name__ == "__main__":
-  spinner = Spinner()
-  install_manager = MapdInstallManager(spinner)
-  install_manager.ensure_directories_exist()
-  if is_prebuilt():
-    debug_msg = f"[DEBUG] This is prebuilt, no mapd install required. VERSION: [{VERSION}], Param [{install_manager.get_installed_version()}]"
-    spinner.update(debug_msg)
-    update_installed_version(VERSION)
-  else:
-    spinner.update(f"Checking if mapd is installed and valid. Prebuilt [{is_prebuilt()}]")
-    install_manager.non_prebuilt_install()

sunnypilot/mapd/mapd_manager.py

@@ -1,144 +0,0 @@
-#!/usr/bin/env python3
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import json
-import platform
-import os
-import glob
-import shutil
-from datetime import datetime
-
-from openpilot.common.params import Params
-from openpilot.common.realtime import Ratekeeper, config_realtime_process
-from openpilot.common.swaglog import cloudlog
-from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
-from openpilot.sunnypilot.mapd.live_map_data.osm_map_data import OsmMapData
-from openpilot.system.hardware.hw import Paths
-from openpilot.sunnypilot.mapd import MAPD_PATH
-from openpilot.sunnypilot.mapd.mapd_installer import VERSION, update_installed_version
-
-# PFEIFER - MAPD {{
-params = Params()
-mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else params
-# }} PFEIFER - MAPD
-
-
-def get_files_for_cleanup() -> list[str]:
-  paths = [
-    f"{Paths.mapd_root()}/db",
-    f"{Paths.mapd_root()}/v*"
-  ]
-  files_to_remove = []
-  for path in paths:
-    if os.path.exists(path):
-      files = glob.glob(path + '/**', recursive=True)
-      files_to_remove.extend(files)
-  # check for version and mapd files
-  if not os.path.isfile(MAPD_PATH):
-    files_to_remove.append(MAPD_PATH)
-  return files_to_remove
-
-
-def cleanup_old_osm_data(files_to_remove: list[str]) -> None:
-  for file in files_to_remove:
-    # Remove trailing slash if path is file
-    if file.endswith('/') and os.path.isfile(file[:-1]):
-      file = file[:-1]
-    # Try to remove as file or symbolic link first
-    if os.path.islink(file) or os.path.isfile(file):
-      os.remove(file)
-    elif os.path.isdir(file):  # If it's a directory
-      shutil.rmtree(file, ignore_errors=False)
-
-
-def request_refresh_osm_location_data(nations: list[str], states: list[str] | None = None) -> None:
-  params.put("OsmDownloadedDate", str(datetime.now().timestamp()))
-  params.put_bool("OsmDbUpdatesCheck", False)
-
-  osm_download_locations = {
-    "nations": nations,
-    "states": states or []
-  }
-
-  print(f"Downloading maps for {json.dumps(osm_download_locations)}")
-  mem_params.put("OSMDownloadLocations", osm_download_locations)
-
-
-def filter_nations_and_states(nations: list[str], states: list[str] | None = None) -> tuple[list[str], list[str]]:
-  """Filters and prepares nation and state data for OSM map download.
-
-  If the nation is 'US' and a specific state is provided, the nation 'US' is removed from the list.
-  If the nation is 'US' and the state is 'All', the 'All' is removed from the list.
-  The idea behind these filters is that if a specific state in the US is provided,
-  there's no need to download map data for the entire US. Conversely,
-  if the state is unspecified (i.e., 'All'), we intend to download map data for the whole US,
-  and 'All' isn't a valid state name, so it's removed.
-
-  Parameters:
-  nations (list): A list of nations for which the map data is to be downloaded.
-  states (list, optional): A list of states for which the map data is to be downloaded. Defaults to None.
-
-  Returns:
-  tuple: Two lists. The first list is filtered nations and the second list is filtered states.
-  """
-
-  if "US" in nations and states and not any(x.lower() == "all" for x in states):
-    # If a specific state in the US is provided, remove 'US' from nations
-    nations.remove("US")
-  elif "US" in nations and states and any(x.lower() == "all" for x in states):
-    # If 'All' is provided as a state (case invariant), remove those instances from states
-    states = [x for x in states if x.lower() != "all"]
-  elif "US" not in nations and states and any(x.lower() == "all" for x in states):
-    states.remove("All")
-  return nations, states or []
-
-
-def update_osm_db() -> None:
-  if params.get_bool("OsmDbUpdatesCheck"):
-    cleanup_old_osm_data(get_files_for_cleanup())
-    country = params.get("OsmLocationName", return_default=True)
-    state = params.get("OsmStateName", return_default=True)
-    filtered_nations, filtered_states = filter_nations_and_states([country], [state])
-    request_refresh_osm_location_data(filtered_nations, filtered_states)
-
-  if not mem_params.get("OSMDownloadBounds"):
-    mem_params.put("OSMDownloadBounds", "")
-
-  if not mem_params.get("LastGPSPosition"):
-    mem_params.put("LastGPSPosition", "{}")
-
-
-def main_thread():
-  update_installed_version(VERSION, params)
-  config_realtime_process([0, 1, 2, 3], 5)
-
-  rk = Ratekeeper(1, print_delay_threshold=None)
-  live_map_sp = OsmMapData()
-
-  # Create folder needed for OSM
-  try:
-    os.mkdir(Paths.mapd_root())
-  except FileExistsError:
-    pass
-  except PermissionError:
-    cloudlog.exception(f"mapd: failed to make {Paths.mapd_root()}")
-
-  while True:
-    show_alert = get_files_for_cleanup() and params.get_bool("OsmLocal")
-    set_offroad_alert("Offroad_OSMUpdateRequired", show_alert, "This alert will be cleared when new maps are downloaded.")
-
-    update_osm_db()
-    live_map_sp.tick()
-    rk.keep_time()
-
-
-def main():
-  main_thread()
-
-
-if __name__ == "__main__":
-  main()

sunnypilot/mapd/tests/mapd_hash

@@ -1 +0,0 @@
-fdb3b49ee19956e6ce09fdc3373cbba557f1263b2180e9f344c1d4053852284b

sunnypilot/mapd/tests/test_mapd_version.py

@@ -1,19 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-from openpilot.sunnypilot import get_file_hash
-from openpilot.sunnypilot.mapd import MAPD_PATH
-from openpilot.sunnypilot.mapd.update_version import MAPD_HASH_PATH
-
-
-class TestMapdVersion:
-  def test_compare_versions(self):
-    mapd_hash = get_file_hash(MAPD_PATH)
-
-    with open(MAPD_HASH_PATH) as f:
-      current_hash = f.read().strip()
-
-    assert current_hash == mapd_hash, "Run sunnypilot/mapd/update_version.py to update the current mapd version and hash"

sunnypilot/mapd/update_version.py

@@ -1,97 +0,0 @@
-#!/usr/bin/env python3
-"""
-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 argparse
-import os
-import re
-
-from openpilot.sunnypilot import get_file_hash
-from openpilot.common.basedir import BASEDIR
-from openpilot.sunnypilot.mapd import MAPD_PATH
-
-MAPD_HASH_PATH = os.path.join(BASEDIR, "sunnypilot", "mapd", "tests", "mapd_hash")
-MAPD_VERSION_PATH = os.path.join(BASEDIR, "sunnypilot", "mapd", "mapd_installer.py")
-
-
-def update_mapd_hash():
-  mapd_hash = get_file_hash(MAPD_PATH)
-
-  with open(MAPD_HASH_PATH, "w") as f:
-    f.write(mapd_hash)
-
-  print(f"Generated and updated new mapd hash to {MAPD_HASH_PATH}")
-
-
-def get_current_mapd_version(path: str) -> str:
-  print("[GET CURRENT MAPD VERSION]")
-  with open(path) as f:
-    for line in f:
-      if line.strip().startswith("VERSION"):
-        # Match VERSION = 'v1.11.0' or VERSION="v1.11.0" (with optional spaces)
-        match = re.search(r'VERSION\s*=\s*[\'"]([^\'"]+)[\'"]', line)
-        if match:
-          ver = match.group(1)
-          print(f'Current mapd version: "{ver}"')
-          return ver
-        else:
-          print("[ERROR] VERSION line found but no quoted value detected.")
-          return ""
-  print("[ERROR] VERSION not found in file!")
-  return ""
-
-
-def update_mapd_version(ver: str, path: str):
-  print("[CHANGE CURRENT MAPD VERSION]")
-
-  with open(path) as f:
-    lines = f.readlines()
-
-  found = False
-  new_lines = []
-  for line in lines:
-    if not found and line.startswith("VERSION ="):
-      new_lines.append(f'VERSION = "{ver}"\n')
-      found = True
-      new_lines.extend(lines[lines.index(line) + 1:])
-      break
-    else:
-      new_lines.append(line)
-
-  if not found:
-    print("[ERROR] VERSION line not found! Aborting without writing.")
-    return
-
-  with open(path, "w") as f:
-    f.writelines(new_lines)
-
-  print(f'New mapd version: "{ver}"')
-  print("[DONE]")
-
-
-if __name__ == "__main__":
-  parser = argparse.ArgumentParser(description="Update mapd version and hash")
-  parser.add_argument("--new_ver", type=str, help="New mapd version")
-  args = parser.parse_args()
-
-  if not args.new_ver:
-    print("Warning: No new mapd version provided. Use --new_ver to specify")
-    print("Example:")
-    print("  python sunnypilot/mapd/update_version.py --new_ver \"v1.12.0\"")
-    print("Current mapd version and hash will not be updated! (aborted)")
-    exit(0)
-
-  current_ver = get_current_mapd_version(MAPD_VERSION_PATH)
-  new_ver = f"{args.new_ver}"
-  if current_ver == new_ver:
-    print(f'Proposed mapd version: "{new_ver}"')
-    confirm = input("Proposed mapd version is the same as the current mapd version. Confirm? (y/n): ").upper().strip()
-    if confirm != "Y":
-      print("Current mapd version and hash will not be updated! (aborted)")
-      exit(0)
-
-  update_mapd_version(new_ver, MAPD_VERSION_PATH)
-  update_mapd_hash()

sunnypilot/modeld/modeld.py

@@ -29,6 +29,7 @@ from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan
 from openpilot.sunnypilot.models.helpers import get_active_bundle, get_model_path, load_metadata, prepare_inputs, load_meta_constants
 from openpilot.sunnypilot.modeld.models.commonmodel_pyx import ModelFrame, CLContext
 from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase
+from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
 
 
 PROCESS_NAME = "selfdrive.modeld.modeld_snpe"
@@ -211,6 +212,7 @@ def main(demo=False):
   prev_action = log.ModelDataV2.Action()
 
   DH = DesireHelper()
+  RELC = RoadEdgeLaneChangeController(params.get_bool("RoadEdgeLaneChangeEnabled"))
 
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -318,7 +320,10 @@ def main(demo=False):
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
       r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
       lane_change_prob = l_lane_change_prob + r_lane_change_prob
-      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
+      RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs)
+      mdv2sp_send.modelDataV2SP.leftLaneChangeEdgeBlock = RELC.left_edge_detected
+      mdv2sp_send.modelDataV2SP.rightLaneChangeEdgeBlock = RELC.right_edge_detected
+      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, RELC.left_edge_detected, RELC.right_edge_detected)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
       mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction

sunnypilot/modeld_v2/modeld.py

@@ -27,6 +27,7 @@ from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase
 from openpilot.sunnypilot.models.helpers import get_active_bundle
 from openpilot.sunnypilot.models.runners.helpers import get_model_runner
+from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
 
 PROCESS_NAME = "selfdrive.modeld.modeld_tinygrad"
 
@@ -245,6 +246,9 @@ def main(demo=False):
   prev_action = log.ModelDataV2.Action()
 
   DH = DesireHelper()
+  RELC = RoadEdgeLaneChangeController(params.get_bool("RoadEdgeLaneChangeEnabled"))
+
+
 
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -348,7 +352,10 @@ def main(demo=False):
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
       r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
       lane_change_prob = l_lane_change_prob + r_lane_change_prob
-      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
+      RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs)
+      mdv2sp_send.modelDataV2SP.leftLaneChangeEdgeBlock = RELC.left_edge_detected
+      mdv2sp_send.modelDataV2SP.rightLaneChangeEdgeBlock = RELC.right_edge_detected
+      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, RELC.left_edge_detected, RELC.right_edge_detected)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
       mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction

sunnypilot/selfdrive/car/cruise_ext.py

@@ -6,16 +6,12 @@ See the LICENSE.md file in the root directory for more details.
 """
 import numpy as np
 
-from cereal import car, custom
+from cereal import car
 from opendbc.car import structs
-from openpilot.common.constants import CV
 from openpilot.common.params import Params
 from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import ACTIVE_STATES as SLA_ACTIVE_STATES
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target
 
 ButtonType = car.CarState.ButtonEvent.Type
-SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
 
 CRUISE_BUTTON_TIMER = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0,
                        ButtonType.setCruise: 0, ButtonType.resumeCruise: 0,
@@ -54,16 +50,6 @@ class VCruiseHelperSP:
 
     self.enable_button_timers = CRUISE_BUTTON_TIMER
 
-    # Speed Limit Assist
-    self.sla_state = SpeedLimitAssistState.disabled
-    self.prev_sla_state = SpeedLimitAssistState.disabled
-    self.has_speed_limit = False
-    self.speed_limit_final_last = 0.
-    self.speed_limit_final_last_kph = 0.
-    self.prev_speed_limit_final_last_kph = 0.
-    self.req_plus = False
-    self.req_minus = False
-
   def read_custom_set_speed_params(self) -> None:
     self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled")
     self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True)
@@ -106,33 +92,3 @@ class VCruiseHelperSP:
       return enabled and self.enabled_prev
 
     return enabled
-
-  def update_speed_limit_assist(self, is_metric, LP_SP: custom.LongitudinalPlanSP) -> None:
-    resolver = LP_SP.speedLimit.resolver
-    self.has_speed_limit = resolver.speedLimitValid or resolver.speedLimitLastValid
-    self.speed_limit_final_last = LP_SP.speedLimit.resolver.speedLimitFinalLast
-    self.speed_limit_final_last_kph = self.speed_limit_final_last * CV.MS_TO_KPH
-    self.sla_state = LP_SP.speedLimit.assist.state
-    self.req_plus, self.req_minus = compare_cluster_target(self.v_cruise_cluster_kph * CV.KPH_TO_MS,
-                                                           self.speed_limit_final_last, is_metric)
-
-  @property
-  def update_speed_limit_final_last_changed(self) -> bool:
-    return self.has_speed_limit and bool(self.speed_limit_final_last_kph != self.prev_speed_limit_final_last_kph)
-
-  def update_speed_limit_assist_pre_active_confirmed(self, button_type: car.CarState.ButtonEvent.Type) -> bool:
-    if self.sla_state == SpeedLimitAssistState.preActive or self.prev_sla_state == SpeedLimitAssistState.preActive:
-      if button_type == ButtonType.decelCruise and self.req_minus:
-        return True
-      if button_type == ButtonType.accelCruise and self.req_plus:
-        return True
-
-    return False
-
-  def update_speed_limit_assist_v_cruise_non_pcm(self) -> None:
-    if self.sla_state in SLA_ACTIVE_STATES and (self.prev_sla_state not in SLA_ACTIVE_STATES or
-                                                self.update_speed_limit_final_last_changed):
-      self.v_cruise_kph = np.clip(round(self.speed_limit_final_last_kph, 1), self.v_cruise_min, V_CRUISE_MAX)
-
-    self.prev_sla_state = self.sla_state
-    self.prev_speed_limit_final_last_kph = self.speed_limit_final_last_kph

sunnypilot/selfdrive/car/interfaces.py

@@ -11,7 +11,6 @@ from opendbc.car.interfaces import CarInterfaceBase
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
 from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import get_nn_model_path
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import set_speed_limit_assist_availability
 
 import openpilot.system.sentry as sentry
 
@@ -86,10 +85,6 @@ def _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsS
     cloudlog.warning("openpilot Longitudinal Control and ICBM not available, cleaning up params")
     params.remove("DynamicExperimentalControl")
     params.remove("CustomAccIncrementsEnabled")
-    params.remove("SmartCruiseControlVision")
-    params.remove("SmartCruiseControlMap")
-
-  set_speed_limit_assist_availability(CP, CP_SP, params)
 
 
 def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None:

sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py

@@ -0,0 +1,139 @@
+"""
+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.
+"""
+
+from cereal import custom
+import numpy as np
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.params import Params
+
+AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
+ACCEL_PERSONALITY_OPTIONS = [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]
+
+# Acceleration Profiles
+MAX_ACCEL_PROFILES = {
+  AccelPersonality.eco:      [1.85, 1.80, 1.55, 0.94, 0.72, 0.58, 0.34, 0.120, 0.09, 0.07],
+  AccelPersonality.normal:   [2.00, 1.95, 1.80, 1.06, 0.81, 0.69, 0.42, 0.160, 0.10, 0.08],
+  AccelPersonality.sport:    [2.00, 1.99, 1.95, 1.45, 1.10, 0.82, 0.53, 0.240, 0.13, 0.09],
+  #AccelPersonality.eco:      [1.30, 1.25, 1.15, 0.69, 0.60, 0.49, 0.28, 0.107, 0.08, 0.06],
+  #AccelPersonality.normal:   [1.85, 1.80, 1.55, 0.94, 0.72, 0.58, 0.34, 0.120, 0.09, 0.07],
+  #AccelPersonality.sport:    [2.00, 1.95, 1.80, 1.06, 0.81, 0.69, 0.42, 0.160, 0.10, 0.08],
+}
+MAX_ACCEL_BREAKPOINTS =      [0.,   3.,   5.,   8.,   12.,  18.,  24.,  32.,  42.,  55.]
+
+# Braking Profiles
+MIN_ACCEL_PROFILES = {
+  AccelPersonality.eco:    [-.002, -0.24, -0.34, -0.44, -1.2],
+  AccelPersonality.normal: [-.003, -0.26, -0.36, -0.46, -1.3],
+  AccelPersonality.sport:  [-.004, -0.28, -0.38, -0.48, -1.4],
+}
+MIN_ACCEL_BREAKPOINTS =    [3,     4.5,   7.,    9.,     25]
+
+
+DECEL_SMOOTH_ALPHA = 0.40  # Very aggressive smoothing for decel (lower = smoother)
+ACCEL_SMOOTH_ALPHA = 0.90  # Less aggressive for accel (higher = more responsive)
+
+# Asymmetric rate limiting
+MAX_DECEL_INCREASE_RATE = 1.3  # When braking harder (m/s² per second)
+MAX_DECEL_DECREASE_RATE = 1.0  # When releasing brake (m/s² per second)
+
+
+
+class AccelPersonalityController:
+  def __init__(self):
+    self.params = Params()
+    self.frame = 0
+    self.last_max_accel = 2.0
+    self.last_min_accel = -0.01
+    self.first_run = True
+    self._accel_personality = self.params.get('AccelPersonality') or AccelPersonality.normal
+    self._enabled = self.params.get_bool('AccelPersonalityEnabled')
+
+  def update(self, sm=None):
+    self.frame += 1
+    if self.frame % int(1.0 / DT_MDL) == 0:
+      self._accel_personality = self.params.get('AccelPersonality') or AccelPersonality.normal
+      self._enabled = self.params.get_bool('AccelPersonalityEnabled')
+
+  @property
+  def accel_personality(self) -> int:
+    return self._accel_personality
+
+  def get_accel_personality(self) -> int:
+    return int(self._accel_personality)
+
+  def set_accel_personality(self, personality: int):
+    if personality in ACCEL_PERSONALITY_OPTIONS:
+      self._accel_personality = personality
+      self.params.put('AccelPersonality', personality)
+
+  def cycle_accel_personality(self) -> int:
+    current = self._accel_personality
+    next_personality = ACCEL_PERSONALITY_OPTIONS[(ACCEL_PERSONALITY_OPTIONS.index(current) + 1) % len(ACCEL_PERSONALITY_OPTIONS)]
+    self.set_accel_personality(next_personality)
+    return int(next_personality)
+
+  def get_accel_limits(self, v_ego: float) -> tuple[float, float]:
+    v_ego = max(0.0, v_ego)
+    target_max = np.interp(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self.accel_personality])
+    target_min = np.interp(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[self.accel_personality])
+
+    if self.first_run:
+      self.last_max_accel, self.last_min_accel = target_max, target_min
+      self.first_run = False
+      return float(target_min), float(target_max)
+
+    # Smoothing
+    self.last_max_accel = ACCEL_SMOOTH_ALPHA * target_max + (1 - ACCEL_SMOOTH_ALPHA) * self.last_max_accel
+    smoothed_decel = DECEL_SMOOTH_ALPHA * target_min + (1 - DECEL_SMOOTH_ALPHA) * self.last_min_accel
+
+    # Rate Limiting (Asymmetric)
+    raw_change = smoothed_decel - self.last_min_accel
+
+    if raw_change < 0:
+      limit = MAX_DECEL_INCREASE_RATE * DT_MDL
+      decel_change = np.clip(raw_change, -limit, limit)
+    else:
+      limit = MAX_DECEL_DECREASE_RATE * DT_MDL
+      decel_change = np.clip(raw_change, -limit, limit)
+
+    self.last_min_accel += decel_change
+
+    # Dynamic Safety Corridor: Ensure min is always strictly less than max.
+    # We maintain a gap of at least 0.1, or 5% of the current max acceleration.
+    # This scaling gap prevents solver crashes at high acceleration values.
+    gap = max(0.1, abs(self.last_max_accel) * 0.05)
+
+    if self.last_min_accel > self.last_max_accel - gap:
+      self.last_min_accel = self.last_max_accel - gap
+
+    return float(self.last_min_accel), float(self.last_max_accel)
+
+  def get_min_accel(self, v_ego: float) -> float:
+    return self.get_accel_limits(v_ego)[0]
+
+  def get_max_accel(self, v_ego: float) -> float:
+    return self.get_accel_limits(v_ego)[1]
+
+  def is_enabled(self) -> bool:
+    return self._enabled
+
+  def set_enabled(self, enabled: bool):
+    self._enabled = enabled
+    self.params.put_bool('AccelPersonalityEnabled', enabled)
+
+  def toggle_enabled(self) -> bool:
+    current = self._enabled
+    self.set_enabled(not current)
+    return not current
+
+  def reset(self):
+    self._accel_personality = AccelPersonality.normal
+    self.params.put('AccelPersonality', AccelPersonality.normal)
+    self.frame = 0
+    self.last_max_accel = 2.0
+    self.last_min_accel = -0.01
+    self.first_run = True

sunnypilot/selfdrive/controls/lib/dynamic_personality/dynamic_follow.py

@@ -0,0 +1,106 @@
+"""
+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.
+"""
+
+from cereal import log
+import numpy as np
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.params import Params
+
+LongPersonality = log.LongitudinalPersonality
+
+# Follow distance profiles mapped to LongPersonality
+FOLLOW_PROFILES = {
+  LongPersonality.relaxed:    [1.70, 1.70, 1.75, 1.75, 1.80, 1.80, 1.80],
+  LongPersonality.standard:   [1.40, 1.40, 1.45, 1.45, 1.50, 1.50, 1.50],
+  LongPersonality.aggressive: [1.05, 1.05, 1.15, 1.15, 1.20, 1.20, 1.20],
+}
+
+FOLLOW_BREAKPOINTS =          [0.,   3.,    6.,  30.,  40.,  50.,  60.]
+
+SMOOTHING_BASE = 0.55  # Base smoothing factor (higher = smoother)
+SMOOTHING_RANGE = 0.20  # Additional smoothing at high speeds
+SMOOTHING_SPEED_THRESHOLD = 36.0  # m/s (~80 mph) for max smoothing
+PERSONALITY_CHANGE_COOLDOWN_S = 2.0
+
+
+class FollowDistanceController:
+  def __init__(self):
+    self.params = Params()
+    self.frame = 0
+    self.current_multiplier = None
+    self.first_run = True
+    self.personality_change_cooldown = 0
+    self.personality_cooldown_frames = int(PERSONALITY_CHANGE_COOLDOWN_S / DT_MDL)
+    self._personality = self.params.get('LongitudinalPersonality') or LongPersonality.standard
+    self._enabled = self.params.get_bool('DynamicFollow')
+
+  def _get_smoothing_factor(self, v_ego: float) -> float:
+    speed_factor = np.clip(v_ego / SMOOTHING_SPEED_THRESHOLD, 0.3, 1.0)
+    return SMOOTHING_BASE + (SMOOTHING_RANGE * speed_factor)
+
+  def is_enabled(self) -> bool:
+    return self._enabled
+
+  def set_enabled(self, enabled: bool):
+    self._enabled = enabled
+    self.params.put_bool('DynamicFollow', enabled)
+
+  def toggle(self) -> bool:
+    current = self._enabled
+    self.set_enabled(not current)
+    return not current
+
+  @property
+  def personality(self) -> int:
+    return self._personality
+
+  def get_personality(self) -> int:
+    return int(self._personality)
+
+  def set_personality(self, personality: int):
+    if personality not in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]:
+      return
+    self._personality = personality
+    self.params.put('LongitudinalPersonality', personality)
+    self.personality_change_cooldown = self.personality_cooldown_frames
+
+  def cycle_personality(self) -> int:
+    personalities = [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]
+    current_idx = personalities.index(self._personality)
+    next_personality = personalities[(current_idx + 1) % len(personalities)]
+    self.set_personality(next_personality)
+    return int(next_personality)
+
+  def get_follow_distance_multiplier(self, v_ego: float) -> float:
+    v_ego = max(0.0, v_ego)
+    target = float(np.interp(v_ego, FOLLOW_BREAKPOINTS, FOLLOW_PROFILES[self._personality]))
+
+    if self.first_run:
+      self.current_multiplier = target
+      self.first_run = False
+      return self.current_multiplier
+
+    # exponential smoothing with speedadaptive factor
+    alpha = self._get_smoothing_factor(v_ego)
+    self.current_multiplier = alpha * self.current_multiplier + (1.0 - alpha) * target
+    return self.current_multiplier
+
+  def reset(self):
+    self._personality = LongPersonality.standard
+    self.params.put('LongitudinalPersonality', LongPersonality.standard)
+    self.frame = 0
+    self.current_multiplier = None
+    self.first_run = True
+    self.personality_change_cooldown = 0
+
+  def update(self):
+    self.frame += 1
+    if self.personality_change_cooldown > 0:
+      self.personality_change_cooldown -= 1
+    if self.frame % int(1.0 / DT_MDL) == 0:
+      self._personality = self.params.get('LongitudinalPersonality') or LongPersonality.standard
+      self._enabled = self.params.get_bool('DynamicFollow')

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -7,16 +7,15 @@ See the LICENSE.md file in the root directory for more details.
 
 from 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
 
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelPersonalityController
+from openpilot.sunnypilot.selfdrive.controls.lib.dynamic_personality.dynamic_follow import FollowDistanceController
+from opendbc.car.interfaces import ACCEL_MIN
+
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
 LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
@@ -24,17 +23,15 @@ 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.accel_controller = AccelPersonalityController()
+    self.dynamic_follow = FollowDistanceController()
     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.
+    self.output_v_target = 0.0
+    self.output_a_target = 0.0
 
   @property
   def mlsim(self) -> bool:
@@ -47,40 +44,39 @@ class LongitudinalPlannerSP:
 
     return self.dec.mode()
 
+  def get_accel_clip(self, v_ego: float, mode: str) -> list[float] | None:
+    if mode == 'acc' and self.accel_controller.is_enabled():
+      return [ACCEL_MIN, self.accel_controller.get_max_accel(v_ego)]
+    return None
+
+  def get_cruise_min_accel(self, v_ego: float) -> float | None:
+    if self.accel_controller.is_enabled():
+      return self.accel_controller.get_min_accel(v_ego)
+    return None
+
+  def get_t_follow(self, v_ego: float) -> float | None:
+    if self.dynamic_follow.is_enabled():
+      return self.dynamic_follow.get_follow_distance_multiplier(v_ego)
+    return None
+
   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
+    self.source = LongitudinalPlanSource.cruise
+    self.output_v_target = v_cruise
+    self.output_a_target = a_ego
 
-    long_enabled = sm['carControl'].enabled
-    long_override = sm['carControl'].cruiseControl.override
+    if sm.valid['mapdOut']:
+      suggested_speed = sm['mapdOut'].suggestedSpeed
+      if suggested_speed > 0 and suggested_speed < self.output_v_target:
+        self.output_v_target = suggested_speed
+        self.source = LongitudinalPlanSource.mapd
 
-    # 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)
+    self.accel_controller.update(sm)
 
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
     plan_sp_send = messaging.new_message('longitudinalPlanSP')
@@ -99,43 +95,7 @@ class LongitudinalPlannerSP:
     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)
+    longitudinalPlanSP.accelPersonality = int(self.accel_controller.get_accel_personality())
 
     # E2E Alerts
     e2eAlerts = longitudinalPlanSP.e2eAlerts

sunnypilot/selfdrive/controls/lib/relc.py

@@ -0,0 +1,113 @@
+"""
+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.
+"""
+
+import numpy as np
+from cereal import log
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.params import Params
+
+NEARSIDE_PROB = 0.2
+EDGE_PROB = 0.35
+EDGE_REACTION_TIME = 1.0
+
+class RoadEdgeLaneChangeController:
+  def __init__(self, desire_helper):
+    self.desire_helper = desire_helper
+    self.params = Params()
+    self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
+    self.left_edge_detected = False
+    self.right_edge_detected = False
+    self.left_edge_timer = 0.0
+    self.right_edge_timer = 0.0
+    self._frame = 0
+
+  def set_enabled(self, enabled):
+    self.enabled = enabled
+    if not enabled:
+      self._reset_state()
+
+  def _read_params(self) -> None:
+    if self._frame % int(1. / DT_MDL) == 0:
+      self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
+
+  def _reset_state(self):
+    self.left_edge_detected = False
+    self.right_edge_detected = False
+    self.left_edge_timer = 0.0
+    self.right_edge_timer = 0.0
+
+  def _update_edge_detection(self, road_edge_stds, lane_line_probs):
+    if not self.enabled:
+      return
+
+    left_road_edge_prob = np.clip(1.0 - road_edge_stds[0], 0.0, 1.0)
+    right_road_edge_prob = np.clip(1.0 - road_edge_stds[1], 0.0, 1.0)
+
+    # Lane line probabilities: [left_outer, left_inner, right_inner, right_outer]
+    left_lane_nearside_prob = lane_line_probs[0] if len(lane_line_probs) > 0 else 0.0
+    right_lane_nearside_prob = lane_line_probs[3] if len(lane_line_probs) > 3 else 0.0
+
+    left_edge_conditions = (
+          left_road_edge_prob > EDGE_PROB and
+          left_lane_nearside_prob < NEARSIDE_PROB and
+          (len(lane_line_probs) <= 3 or right_lane_nearside_prob >= left_lane_nearside_prob)
+    )
+    right_edge_conditions = (
+          right_road_edge_prob > EDGE_PROB and
+          right_lane_nearside_prob < NEARSIDE_PROB and
+          (len(lane_line_probs) <= 0 or left_lane_nearside_prob >= right_lane_nearside_prob)
+    )
+
+    if left_edge_conditions:
+      self.left_edge_timer += DT_MDL
+      self.left_edge_detected = self.left_edge_timer > EDGE_REACTION_TIME
+    else:
+      self.left_edge_timer = 0.0
+      self.left_edge_detected = False
+
+    if right_edge_conditions:
+      self.right_edge_timer += DT_MDL
+      self.right_edge_detected = self.right_edge_timer > EDGE_REACTION_TIME
+    else:
+      self.right_edge_timer = 0.0
+      self.right_edge_detected = False
+
+  def update(self, road_edge_stds, lane_line_probs):
+    self._read_params()
+
+    if not self.enabled:
+      self._frame += 1
+      return
+
+    self._update_edge_detection(road_edge_stds, lane_line_probs)
+    self._frame += 1
+
+  def should_trigger_lane_change(self, carstate, lateral_active):
+    if not self.enabled:
+      return False, log.LaneChangeDirection.none
+    return False, log.LaneChangeDirection.none
+
+  def is_lane_change_blocked(self, direction):
+    if not self.enabled:
+      return False
+
+    if direction == log.LaneChangeDirection.left:
+      return self.left_edge_detected
+    elif direction == log.LaneChangeDirection.right:
+      return self.right_edge_detected
+
+    return False
+
+  def can_change_lane_left(self):
+    return not self.left_edge_detected if self.enabled else True
+
+  def can_change_lane_right(self):
+    return not self.right_edge_detected if self.enabled else True
+
+  @property
+  def edge_detected(self):
+    return self.left_edge_detected or self.right_edge_detected

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

@@ -1,9 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-from openpilot.common.constants import CV
-
-MIN_V = 20 * CV.KPH_TO_MS  # Do not operate under 20 km/h

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

@@ -1,261 +0,0 @@
-import json
-import math
-import platform
-
-from 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

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

@@ -1,19 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import 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)

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

@@ -1,58 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import platform
-
-from 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)
-
-    # TODO-SP: mock data from gpsLocation
-    self.params.put("LastGPSPosition", "{}")
-    self.params.put("MapTargetVelocities", "{}")
-
-  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)
-    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

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

@@ -1,104 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import numpy as np
-
-import cereal.messaging as messaging
-from 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.vision_controller import SmartCruiseControlVision
-
-VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
-
-
-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)
-
-  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)
-    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
-
-  # TODO-SP: mock modelV2 data to test other states

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

@@ -1,203 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import numpy as np
-
-import cereal.messaging as messaging
-from 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.amax(predicted_lat_accels)
-
-      # 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

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

@@ -1,19 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-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
-}

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

@@ -1,29 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-
-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

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

@@ -1,44 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-
-from cereal import custom, 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))
-
-  return allowed

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

@@ -1,414 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import time
-
-from cereal import custom, 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

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

@@ -1,190 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import time
-
-import cereal.messaging as messaging
-from 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

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

@@ -1,278 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-
-import pytest
-
-from 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)
-    self.params.put("SpeedLimitMode", int(Mode.assist))
-    self.params.put_bool("IsMetric", False)
-    self.params.put("SpeedLimitOffsetType", 0)
-    self.params.put("SpeedLimitValueOffset", 0)
-
-  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_GEN1, 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))
-    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))
-    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

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

@@ -1,144 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import random
-import time
-
-import pytest
-from pytest_mock import MockerFixture
-
-from 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.

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

@@ -5,6 +5,8 @@ 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
+from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
+
 
 TurnDirection = custom.ModelDataV2SP.TurnDirection
 
@@ -106,8 +108,10 @@ def set_lane_turn_params():
   (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
+    dh = DesireHelper()
+    relc = RoadEdgeLaneChangeController(dh)
+    relc.set_enabled(True)
+    dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
+    for _ in range(10):
+        dh.update(carstate, lateral_active, lane_change_prob, left_edge_detected=relc.left_edge_detected, right_edge_detected=relc.right_edge_detected)
+    assert dh.desire == expected_desire  # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers

sunnypilot/selfdrive/controls/lib/tests/test_relc.py

@@ -0,0 +1,190 @@
+"""
+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.
+"""
+
+import pytest
+from cereal import log
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController, EDGE_REACTION_TIME
+
+@pytest.fixture
+def relc_controller(mocker):
+  mock_params = mocker.patch("openpilot.sunnypilot.selfdrive.controls.lib.relc.Params")
+  mock_params.return_value.get_bool.return_value = True
+
+  DH = DesireHelper()
+  relc = RoadEdgeLaneChangeController(DH)
+  relc.set_enabled(True)
+  return relc
+
+
+def test_disable_resets_state(relc_controller):
+  relc = relc_controller
+  relc.left_edge_detected = True
+  relc.right_edge_detected = True
+  relc.left_edge_timer = 5.0
+  relc.right_edge_timer = 5.0
+
+  relc.set_enabled(False)
+
+  assert not relc.left_edge_detected
+  assert not relc.right_edge_detected
+  assert relc.left_edge_timer == 0.0
+  assert relc.right_edge_timer == 0.0
+
+
+def test_lane_change_blocked_left(relc_controller):
+  relc = relc_controller
+  relc.left_edge_detected = True
+  assert relc.is_lane_change_blocked(log.LaneChangeDirection.left)
+
+
+def test_lane_change_blocked_right(relc_controller):
+  relc = relc_controller
+  relc.right_edge_detected = True
+  assert relc.is_lane_change_blocked(log.LaneChangeDirection.right)
+
+
+def test_lane_change_not_blocked_opposite_side(relc_controller):
+  relc = relc_controller
+  relc.left_edge_detected = True
+  assert not relc.is_lane_change_blocked(log.LaneChangeDirection.right)
+
+  relc.left_edge_detected = False
+  relc.right_edge_detected = True
+  assert not relc.is_lane_change_blocked(log.LaneChangeDirection.left)
+
+
+def test_lane_change_not_blocked_when_disabled(relc_controller):
+  relc = relc_controller
+  relc.set_enabled(False)
+  relc.left_edge_detected = True
+  relc.right_edge_detected = True
+
+  assert not relc.is_lane_change_blocked(log.LaneChangeDirection.left)
+  assert not relc.is_lane_change_blocked(log.LaneChangeDirection.right)
+
+
+def test_can_change_lane_left(relc_controller):
+  relc = relc_controller
+  assert relc.can_change_lane_left()
+
+  relc.left_edge_detected = True
+  assert not relc.can_change_lane_left()
+
+
+def test_can_change_lane_right(relc_controller):
+  relc = relc_controller
+  assert relc.can_change_lane_right()
+
+  relc.right_edge_detected = True
+  assert not relc.can_change_lane_right()
+
+
+def test_can_change_lane_when_disabled(relc_controller):
+  relc = relc_controller
+  relc.set_enabled(False)
+  relc.left_edge_detected = True
+  relc.right_edge_detected = True
+
+  assert relc.can_change_lane_left()
+  assert relc.can_change_lane_right()
+
+
+def test_edge_detected_property(relc_controller):
+  relc = relc_controller
+  assert not relc.edge_detected
+
+  relc.left_edge_detected = True
+  assert relc.edge_detected
+
+  relc.left_edge_detected = False
+  relc.right_edge_detected = True
+  assert relc.edge_detected
+
+  relc.left_edge_detected = True
+  assert relc.edge_detected
+
+
+def test_should_trigger_lane_change(relc_controller):
+  relc = relc_controller
+  should_trigger, direction = relc.should_trigger_lane_change(None, True)
+  assert not should_trigger
+  assert direction == log.LaneChangeDirection.none
+
+
+def test_update_increments_frame(relc_controller):
+  relc = relc_controller
+  initial = relc._frame
+  relc.update([0.5, 0.5], [0.5, 0.5, 0.5, 0.5])
+  assert relc._frame == initial + 1
+
+
+def test_left_edge_detection(relc_controller):
+  relc = relc_controller
+  road_edge_stds = [0.0, 0.9]
+  lane_line_probs = [0.0, 0.8, 0.8, 0.8]
+
+  num_updates = int(EDGE_REACTION_TIME / DT_MDL) + 5
+  for _ in range(num_updates):
+    relc.update(road_edge_stds, lane_line_probs)
+
+  assert relc.left_edge_detected
+
+
+def test_right_edge_detection(relc_controller):
+  relc = relc_controller
+  road_edge_stds = [0.9, 0.0]
+  lane_line_probs = [0.8, 0.8, 0.8, 0.0]
+
+  num_updates = int(EDGE_REACTION_TIME / DT_MDL) + 5
+  for _ in range(num_updates):
+    relc.update(road_edge_stds, lane_line_probs)
+
+  assert relc.right_edge_detected
+
+
+def test_edge_detection_requires_time(relc_controller):
+  relc = relc_controller
+  road_edge_stds = [0.0, 0.9]
+  lane_line_probs = [0.0, 0.8, 0.8, 0.8]
+
+  num_updates = int(EDGE_REACTION_TIME / DT_MDL) - 1
+  for _ in range(num_updates):
+    relc.update(road_edge_stds, lane_line_probs)
+
+  assert not relc.left_edge_detected
+
+
+def test_edge_detection_clears(relc_controller):
+  relc = relc_controller
+  road_edge_stds = [0.0, 0.9]
+  lane_line_probs = [0.0, 0.8, 0.8, 0.8]
+
+  num_updates = int(EDGE_REACTION_TIME / DT_MDL) + 5
+  for _ in range(num_updates):
+    relc.update(road_edge_stds, lane_line_probs)
+  assert relc.left_edge_detected
+
+  road_edge_stds = [0.9, 0.9]
+  relc.update(road_edge_stds, lane_line_probs)
+
+  assert not relc.left_edge_detected
+  assert relc.left_edge_timer == 0.0
+
+
+def test_both_edges_detected(relc_controller):
+  relc = relc_controller
+  road_edge_stds = [0.0, 0.0]
+  lane_line_probs = [0.0, 0.8, 0.8, 0.0]
+
+  num_updates = int(EDGE_REACTION_TIME / DT_MDL) + 5
+  for _ in range(num_updates):
+    relc.update(road_edge_stds, lane_line_probs)
+
+  assert relc.left_edge_detected
+  assert relc.right_edge_detected

sunnypilot/selfdrive/controls/lib/vibe_personality/tests/test_vibe.py

@@ -0,0 +1,314 @@
+import pytest
+
+# Import the actual modules
+from cereal import log, custom
+from openpilot.common.realtime import DT_MDL
+
+# Import the enums we need for testing
+LongPersonality = log.LongitudinalPersonality
+AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
+
+
+class MockParams:
+  """Simple mock for Params class"""
+  def __init__(self):
+    self.data = {}
+    self.bool_data = {
+      'VibePersonalityEnabled': True,
+      'VibeAccelPersonalityEnabled': True,
+      'VibeFollowPersonalityEnabled': True
+    }
+
+  def get(self, key, encoding=None):
+    return self.data.get(key)
+
+  def get_bool(self, key):
+    return self.bool_data.get(key, True)
+
+  def put(self, key, value):
+    self.data[key] = value
+
+  def put_bool(self, key, value):
+    self.bool_data[key] = value
+
+  def reset_mock(self):
+    self.call_count = 0
+
+  @property
+  def call_count(self):
+    return getattr(self, '_call_count', 0)
+
+  @call_count.setter
+  def call_count(self, value):
+    self._call_count = value
+
+
+@pytest.fixture
+def mock_params():
+  """Create mock params instance"""
+  return MockParams()
+
+
+@pytest.fixture
+def controller(mock_params, monkeypatch):
+  """Create controller instance with mocked Params"""
+  # Patch the Params import in the controller module
+  monkeypatch.setattr('openpilot.sunnypilot.selfdrive.controls.lib.vibe_personality.vibe_personality.Params',
+                      lambda: mock_params)
+
+  from openpilot.sunnypilot.selfdrive.controls.lib.vibe_personality.vibe_personality import VibePersonalityController
+  return VibePersonalityController()
+
+
+class TestVibePersonalityController:
+
+  def test_initialization(self, controller):
+    """Test controller initializes with correct defaults"""
+    assert controller.frame == 0
+    assert controller.accel_personality == AccelPersonality.normal
+    assert controller.long_personality == LongPersonality.standard
+    assert 'accel_personality' in controller.param_keys
+    assert 'long_personality' in controller.param_keys
+
+  def test_frame_increment(self, controller):
+    """Test frame counter increments correctly"""
+    initial_frame = controller.frame
+    controller.update()
+    assert controller.frame == initial_frame + 1
+
+    controller.update()
+    assert controller.frame == initial_frame + 2
+
+  def test_parameter_reading_throttled(self, controller, mock_params):
+    """Test parameters are only read every DT_MDL frames"""
+    # Track calls manually
+    original_get = mock_params.get
+    call_count = 0
+
+    def counting_get(*args, **kwargs):
+      nonlocal call_count
+      call_count += 1
+      return original_get(*args, **kwargs)
+
+    mock_params.get = counting_get
+
+    # First call should read params (frame 0)
+    controller._update_from_params()
+
+    # Reset counter
+    call_count = 0
+
+    # Advance frame but not to threshold
+    controller.frame = 5  # Less than int(1/DT_MDL)
+    controller._update_from_params()
+    assert call_count == 0  # Should not read params
+
+    # Advance to threshold
+    controller.frame = int(1. / DT_MDL)  # Equal to threshold
+    controller._update_from_params()
+    assert call_count >= 2  # Should read both personality params
+
+  def test_accel_personality_management(self, controller, mock_params):
+    """Test acceleration personality setting and cycling"""
+    # Test setting valid personality
+    assert controller.set_accel_personality(AccelPersonality.eco)
+    assert controller.accel_personality == AccelPersonality.eco
+
+    assert controller.set_accel_personality(AccelPersonality.sport)
+    assert controller.accel_personality == AccelPersonality.sport
+
+    # Test setting invalid personality
+    assert not controller.set_accel_personality(999)
+    assert controller.accel_personality == AccelPersonality.sport  # Should remain unchanged
+
+    # Test cycling
+    controller.accel_personality = AccelPersonality.eco
+    next_personality = controller.cycle_accel_personality()
+    assert next_personality == AccelPersonality.normal  # should cycle to normal
+    assert controller.accel_personality == AccelPersonality.normal
+
+    next_personality = controller.cycle_accel_personality()
+    assert next_personality == AccelPersonality.sport  # should cycle to sport
+
+    next_personality = controller.cycle_accel_personality()
+    assert next_personality == AccelPersonality.eco  # should cycle back to eco
+
+  def test_long_personality_management(self, controller, mock_params):
+    """Test longitudinal personality setting and cycling"""
+    # Test setting valid personality
+    assert controller.set_long_personality(LongPersonality.relaxed)
+    assert controller.long_personality == LongPersonality.relaxed
+
+    assert controller.set_long_personality(LongPersonality.aggressive)
+    assert controller.long_personality == LongPersonality.aggressive
+
+    # Test setting invalid personality
+    assert not controller.set_long_personality(999)
+    assert controller.long_personality == LongPersonality.aggressive  # Should remain unchanged
+
+    # Test cycling
+    controller.long_personality = LongPersonality.standard
+    next_personality = controller.cycle_long_personality()
+    assert next_personality == LongPersonality.aggressive  # should cycle to aggressive
+    assert controller.long_personality == LongPersonality.aggressive
+
+    next_personality = controller.cycle_long_personality()
+    assert next_personality == LongPersonality.relaxed  # should cycle to relaxed
+
+    next_personality = controller.cycle_long_personality()
+    assert next_personality == LongPersonality.standard  # should cycle back to standard
+
+  def test_toggle_functions(self, controller, mock_params):
+    """Test toggle functionality"""
+    # Set initial state to False
+    mock_params.bool_data['VibePersonalityEnabled'] = False
+
+    result = controller.toggle_personality()
+    assert result  # Should toggle to True
+    assert mock_params.bool_data['VibePersonalityEnabled']
+
+    # Set initial state to True
+    mock_params.bool_data['VibeAccelPersonalityEnabled'] = True
+
+    result = controller.toggle_accel_personality()
+    assert not result  # Should toggle to False
+    assert not mock_params.bool_data['VibeAccelPersonalityEnabled']
+
+  def test_enable_checks(self, controller, mock_params):
+    """Test various enable state checks"""
+    # All enabled
+    mock_params.bool_data = {
+      'VibePersonalityEnabled': True,
+      'VibeAccelPersonalityEnabled': True,
+      'VibeFollowPersonalityEnabled': True
+    }
+
+    assert controller.is_enabled()
+    assert controller.is_accel_enabled()
+    assert controller.is_follow_enabled()
+
+    # Main toggle disabled
+    mock_params.bool_data['VibePersonalityEnabled'] = False
+
+    assert not controller.is_enabled()
+    assert not controller.is_accel_enabled()
+    assert not controller.is_follow_enabled()
+
+  def test_accel_limits_calculation(self, controller, mock_params):
+    """Test acceleration limits calculation"""
+    # Enable all features through mock_params bool_data
+    mock_params.bool_data = {
+      'VibePersonalityEnabled': True,
+      'VibeAccelPersonalityEnabled': True,
+      'VibeFollowPersonalityEnabled': True
+    }
+
+    # Test with different speeds and personalities
+    controller.accel_personality = 1  # normal
+    controller.long_personality = 1  # standard
+
+    limits = controller.get_accel_limits(10.0)  # 10 m/s
+    assert limits is not None
+    min_a, max_a = limits
+    assert isinstance(min_a, float)
+    assert isinstance(max_a, float)
+    assert min_a < 0  # Should be negative (braking)
+    assert max_a > 0  # Should be positive (acceleration)
+
+    # Test with disabled controller
+    mock_params.bool_data['VibePersonalityEnabled'] = False
+    limits = controller.get_accel_limits(10.0)
+    assert limits is None
+
+  def test_follow_distance_multiplier(self, controller, mock_params):
+    """Test following distance multiplier calculation"""
+    # Enable controller
+    mock_params.bool_data['VibePersonalityEnabled'] = True
+    mock_params.bool_data['VibeFollowPersonalityEnabled'] = True
+
+    # Test with different speeds and personalities
+    controller.long_personality = LongPersonality.relaxed
+
+    multiplier = controller.get_follow_distance_multiplier(15.0)  # 15 m/s
+    assert multiplier is not None
+    assert isinstance(multiplier, float)
+    assert multiplier > 0
+
+    # Test with different personality - aggressive should have shorter distance
+    controller.long_personality = LongPersonality.aggressive
+    aggressive_multiplier = controller.get_follow_distance_multiplier(15.0)
+    assert aggressive_multiplier is not None
+    assert aggressive_multiplier < multiplier  # Aggressive should have shorter distance
+
+    # Test with disabled controller
+    mock_params.bool_data['VibeFollowPersonalityEnabled'] = False
+    multiplier = controller.get_follow_distance_multiplier(15.0)
+    assert multiplier is None
+
+  def test_personality_differences(self, controller, mock_params):
+    """Test that different personalities actually produce different values"""
+    # Enable controller
+    mock_params.bool_data['VibePersonalityEnabled'] = True
+    mock_params.bool_data['VibeAccelPersonalityEnabled'] = True
+    mock_params.bool_data['VibeFollowPersonalityEnabled'] = True
+
+    # Test acceleration differences - sport should have higher max acceleration than eco
+    controller.accel_personality = AccelPersonality.eco
+    eco_limits = controller.get_accel_limits(20.0)
+
+    controller.accel_personality = AccelPersonality.sport
+    sport_limits = controller.get_accel_limits(20.0)
+
+    assert sport_limits[1] > eco_limits[1]  # Sport should have higher max acceleration
+
+    # Test following distance differences - relaxed should have longer distance than aggressive
+    controller.long_personality = LongPersonality.relaxed
+    relaxed_dist = controller.get_follow_distance_multiplier(20.0)
+
+    controller.long_personality = LongPersonality.aggressive
+    aggressive_dist = controller.get_follow_distance_multiplier(20.0)
+
+    assert relaxed_dist > aggressive_dist  # Relaxed should have longer following distance
+
+  def test_reset(self, controller):
+    """Test reset functionality"""
+    # Change some values
+    controller.accel_personality = AccelPersonality.sport
+    controller.long_personality = LongPersonality.relaxed
+    controller.frame = 100
+
+    # Reset
+    controller.reset()
+
+    # Check defaults are restored
+    assert controller.accel_personality == AccelPersonality.normal
+    assert controller.long_personality == LongPersonality.standard
+    assert controller.frame == 0
+
+  def test_edge_cases(self, controller, mock_params):
+    """Test edge cases and error handling"""
+    # Enable all features
+    mock_params.bool_data = {
+      'VibePersonalityEnabled': True,
+      'VibeAccelPersonalityEnabled': True,
+      'VibeFollowPersonalityEnabled': True
+    }
+
+    # Test with zero speed
+    limits = controller.get_accel_limits(0.0)
+    assert limits is not None
+
+    multiplier = controller.get_follow_distance_multiplier(0.0)
+    assert multiplier is not None
+
+    # Test with very high speed
+    limits = controller.get_accel_limits(100.0)
+    assert limits is not None
+
+    multiplier = controller.get_follow_distance_multiplier(100.0)
+    assert multiplier is not None
+
+    # Test interpolation works correctly
+    low_speed_limits = controller.get_accel_limits(5.0)
+    high_speed_limits = controller.get_accel_limits(50.0)
+    assert low_speed_limits[1] > high_speed_limits[1]  # Max accel should decrease with speed

sunnypilot/selfdrive/controls/lib/vibe_personality/vibe_personality.py

@@ -0,0 +1,144 @@
+"""
+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.
+"""
+
+from cereal import log, custom
+import numpy as np
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.params import Params
+
+LongPersonality = log.LongitudinalPersonality
+AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
+
+# Acceleration Profiles mapped to AccelPersonality (eco/normal/sport)
+MAX_ACCEL_PROFILES = {
+    AccelPersonality.eco:       [2.0,  1.99,  1.88, 1.10, .500, .292, .15, .10],   # eco
+    AccelPersonality.normal:    [2.0,  2.00,  1.94, 1.22, .635, .33, .22, .16],   # normal
+    AccelPersonality.sport:     [2.0,  2.00,  2.00, 1.85, .800, .54, .32, .22],    # sport
+}
+MAX_ACCEL_BREAKPOINTS =         [0.,   4.,   6.,   9.,   16.,  25.,  30., 55.]
+
+# Braking profiles mapped to LongPersonality (relaxed/standard/aggressive)
+MIN_ACCEL_PROFILES = {
+    LongPersonality.relaxed:    [-.0006, -.0006, -.010,  -.30,  -1.20],  # gentler braking
+    LongPersonality.standard:   [-.0007, -.0007, -.012,  -.35,  -1.20],  # normal braking
+    LongPersonality.aggressive: [-.0020, -.0008, -.014,  -.40,  -1.20],  # more aggressive braking
+}
+MIN_ACCEL_BREAKPOINTS =         [0.,   3.0,    11.,    14,  50.]
+
+# Follow distance profiles mapped to LongPersonality (relaxed/standard/aggressive)
+FOLLOW_PROFILES = {
+    LongPersonality.relaxed:    [1.55, 1.65, 1.65, 1.80],  # more spread out
+    LongPersonality.standard:   [1.45, 1.45, 1.45, 1.55],  # balanced
+    LongPersonality.aggressive: [1.20, 1.25, 1.28, 1.35],  # tighter
+}
+FOLLOW_BREAKPOINTS =            [0.,   6.,   18.,  36.]
+
+
+class VibePersonalityController:
+    """Controller for acceleration and distance personalities"""
+
+    def __init__(self):
+        self.params = Params()
+        self.frame = 0
+        self.accel_personality = AccelPersonality.normal
+        self.long_personality = LongPersonality.standard
+        self.param_keys = {
+            'accel_personality': 'AccelPersonality',
+            'long_personality': 'LongitudinalPersonality',
+            'enabled': 'VibePersonalityEnabled',
+            'accel_enabled': 'VibeAccelPersonalityEnabled',
+            'follow_enabled': 'VibeFollowPersonalityEnabled'
+        }
+
+    def _update_from_params(self):
+        """Update personalities from params"""
+        if self.frame % int(1. / DT_MDL) != 0:
+            return
+
+        accel_personality_int = int(self.params.get(self.param_keys['accel_personality']))
+        self.accel_personality = accel_personality_int
+
+        long_personality_int = int(self.params.get(self.param_keys['long_personality']))
+        self.long_personality = long_personality_int
+
+    def _get_toggle_state(self, key: str) -> bool:
+        return self.params.get_bool(self.param_keys[key])
+
+    def _set_toggle_state(self, key: str, value: bool):
+        self.params.put_bool(self.param_keys[key], value)
+
+    def set_accel_personality(self, personality: int) -> bool:
+        self.accel_personality = personality
+        self.params.put(self.param_keys['accel_personality'], str(personality))
+        return True
+
+    def cycle_accel_personality(self) -> int:
+        personalities = [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]
+        current_idx = personalities.index(self.accel_personality)
+        next_personality = personalities[(current_idx + 1) % len(personalities)]
+        self.set_accel_personality(next_personality)
+        return int(next_personality)
+
+    def get_accel_personality(self) -> int:
+        self._update_from_params()
+        return int(self.accel_personality)
+
+    def set_long_personality(self, personality: int) -> bool:
+        self.long_personality = personality
+        self.params.put(self.param_keys['long_personality'], str(personality))
+        return True
+
+    def cycle_long_personality(self) -> int:
+        personalities = [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]
+        current_idx = personalities.index(self.long_personality)
+        next_personality = personalities[(current_idx + 1) % len(personalities)]
+        self.set_long_personality(next_personality)
+        return int(next_personality)
+
+    def get_long_personality(self) -> int:
+        self._update_from_params()
+        return int(self.long_personality)
+
+    def toggle_personality(self): return self._toggle_flag('enabled')
+    def toggle_accel_personality(self): return self._toggle_flag('accel_enabled')
+    def toggle_follow_distance_personality(self): return self._toggle_flag('follow_enabled')
+
+    def _toggle_flag(self, key):
+        current = self._get_toggle_state(key)
+        self._set_toggle_state(key, not current)
+        return not current
+
+    def set_personality_enabled(self, enabled: bool): self._set_toggle_state('enabled', enabled)
+    def is_accel_enabled(self) -> bool: return self._get_toggle_state('enabled') and self._get_toggle_state('accel_enabled')
+    def is_follow_enabled(self) -> bool: return self._get_toggle_state('enabled') and self._get_toggle_state('follow_enabled')
+    def is_enabled(self) -> bool: return self._get_toggle_state('enabled') and (self._get_toggle_state('accel_enabled') or self._get_toggle_state('follow_enabled'))
+
+    def get_accel_limits(self, v_ego: float) -> tuple[float, float]:
+        """Get acceleration limits based on current personalities."""
+        self._update_from_params()
+        max_a = np.interp(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self.accel_personality])
+        min_a = np.interp(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[self.long_personality])
+        return float(min_a), float(max_a)
+
+    def get_follow_distance_multiplier(self, v_ego: float) -> float:
+        """Get dynamic following distance based on speed and personality"""
+        self._update_from_params()
+        return float(np.interp(v_ego, FOLLOW_BREAKPOINTS, FOLLOW_PROFILES[self.long_personality]))
+
+    def get_min_accel(self, v_ego: float) -> float:
+        return self.get_accel_limits(v_ego)[0]
+
+    def get_max_accel(self, v_ego: float) -> float:
+        return self.get_accel_limits(v_ego)[1]
+
+    def reset(self):
+        self.accel_personality = AccelPersonality.normal
+        self.long_personality = LongPersonality.standard
+        self.frame = 0
+
+    def update(self):
+        self.frame += 1

sunnypilot/selfdrive/selfdrived/events.py

@@ -1,9 +1,7 @@
-import cereal.messaging as messaging
 from cereal import log, car, custom
-from openpilot.common.constants import CV
+
 from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \
   NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD
 
 
 AlertSize = log.SelfdriveState.AlertSize
@@ -18,43 +16,6 @@ EventNameSP = custom.OnroadEventSP.EventName
 EVENT_NAME_SP = {v: k for k, v in EventNameSP.schema.enumerants.items()}
 
 
-def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
-  speedLimit = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimit
-  speed = round(speedLimit * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))
-  message = f'Adjusting to {speed} {"km/h" if metric else "mph"} speed limit'
-  return Alert(
-    message,
-    "",
-    AlertStatus.normal, AlertSize.small,
-    Priority.LOW, VisualAlert.none, AudibleAlert.none, 4.)
-
-
-def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
-  speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH
-  speed_limit_final_last = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimitFinalLast
-  speed_limit_final_last_conv = round(speed_limit_final_last * speed_conv)
-  alert_1_str = ""
-  alert_2_str = ""
-  alert_size = AlertSize.none
-
-  if CP.openpilotLongitudinalControl and CP.pcmCruise:
-    # PCM long
-    cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[metric]
-    pcm_long_required_max = cst_low if speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[metric] else cst_high
-    pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv)
-    speed_unit = "km/h" if metric else "mph"
-
-    alert_1_str = "Speed Limit Assist: Activation Required"
-    alert_2_str = f"Manually change set speed to {pcm_long_required_max_set_speed_conv} {speed_unit} to activate"
-    alert_size = AlertSize.mid
-
-  return Alert(
-    alert_1_str,
-    alert_2_str,
-    AlertStatus.normal, alert_size,
-    Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleLow, .1)
-
-
 class EventsSP(EventsBase):
   def __init__(self):
     super().__init__()
@@ -191,34 +152,6 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
       Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
   },
 
-  EventNameSP.speedLimitActive: {
-    ET.WARNING: Alert(
-      "Automatically adjusting to the posted speed limit",
-      "",
-      AlertStatus.normal, AlertSize.small,
-      Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleHigh, 5.),
-  },
-
-  EventNameSP.speedLimitChanged: {
-    ET.WARNING: Alert(
-      "Set speed changed",
-      "",
-      AlertStatus.normal, AlertSize.small,
-      Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleHigh, 5.),
-  },
-
-  EventNameSP.speedLimitPreActive: {
-    ET.WARNING: speed_limit_pre_active_alert,
-  },
-
-  EventNameSP.speedLimitPending: {
-    ET.WARNING: Alert(
-      "Automatically adjusting to the last speed limit",
-      "",
-      AlertStatus.normal, AlertSize.small,
-      Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleHigh, 5.),
-  },
-
   EventNameSP.e2eChime: {
     ET.PERMANENT: Alert(
       "",
@@ -226,4 +159,12 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
       AlertStatus.normal, AlertSize.none,
       Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
   },
+
+  EventNameSP.laneChangeRoadEdge: {
+    ET.WARNING: Alert(
+      "Lane Change Unavailable: Road Edge",
+      "",
+      AlertStatus.userPrompt, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.1),
+  },
 }

sunnypilot/sunnylink/params_metadata.json

@@ -1253,15 +1253,27 @@
     "max": 5.0,
     "step": 0.1,
     "unit": "m/s²"
+  },
+    "ToyotaAutoHold": {
+    "title": "Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS",
+    "description": ""
+  },
+    "ToyotaEnhancedBsm": {
+    "title": "Toyota: Prius TSS2 BSM and some tssp",
+    "description": ""
+  },
+    "ToyotaTSS2Long": {
+    "title": "Toyota: custom longitudinal for TSS2",
+    "description": ""
+  },
+    "ToyotaDriveMode": {
+    "title": "Enable drive mode btn link",
+    "description": ""
   },
   "ToyotaEnforceStockLongitudinal": {
     "title": "Toyota: Enforce Factory Longitudinal Control",
     "description": "When enabled, sunnypilot will not take over control of gas and brakes. Factory Toyota longitudinal control will be used."
   },
-  "TrainingVersion": {
-    "title": "Training Version",
-    "description": ""
-  },
   "TrueVEgoUI": {
     "title": "True vEgo UI",
     "description": ""

system/manager/process_config.py

@@ -8,7 +8,6 @@ from openpilot.system.hardware import PC, TICI
 from openpilot.system.manager.process import PythonProcess, NativeProcess, DaemonProcess
 from openpilot.system.hardware.hw import Paths
 
-from openpilot.sunnypilot.mapd.mapd_manager import MAPD_PATH
 
 from openpilot.sunnypilot.models.helpers import get_active_model_runner
 from openpilot.sunnypilot.sunnylink.utils import sunnylink_need_register, sunnylink_ready, use_sunnylink_uploader
@@ -92,8 +91,6 @@ def is_stock_model(started, params, CP: car.CarParams) -> bool:
   """Check if the active model runner is stock."""
   return bool(get_active_model_runner(params, not started) == custom.ModelManagerSP.Runner.stock)
 
-def mapd_ready(started: bool, params: Params, CP: car.CarParams) -> bool:
-  return bool(os.path.exists(Paths.mapd_root()))
 
 def uploader_ready(started: bool, params: Params, CP: car.CarParams) -> bool:
   if not params.get_bool("OnroadUploads"):
@@ -177,11 +174,10 @@ procs += [
   PythonProcess("backup_manager", "sunnypilot.sunnylink.backups.manager", and_(only_offroad, sunnylink_ready_shim)),
 
   # mapd
-  NativeProcess("mapd", Paths.mapd_root(), ["bash", "-c", f"{MAPD_PATH} > /dev/null 2>&1"], mapd_ready),
-  PythonProcess("mapd_manager", "sunnypilot.mapd.mapd_manager", always_run),
+  NativeProcess("mapd", "selfdrive", ["./mapd"], always_run),
 
   # locationd
-  NativeProcess("locationd_llk", "sunnypilot/selfdrive/locationd", ["./locationd"], only_onroad),
+  #NativeProcess("locationd_llk", "sunnypilot/selfdrive/locationd", ["./locationd"], only_onroad),
 ]
 
 if os.path.exists("./github_runner.sh"):

third_party/mapd_pfeiferj/README.md

@@ -1,2 +0,0 @@
-# MAPD implementation by pfeiferj
-https://github.com/pfeiferj/openpilot-mapd/releases/