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/messaging/bridge.cc

@@ -33,8 +33,7 @@ void zmq_to_msgq(const std::vector<std::string> &endpoints, const std::string &i
   for (auto endpoint : endpoints) {
     auto pub_sock = new MSGQPubSocket();
     auto sub_sock = new ZMQSubSocket();
-    size_t queue_size = services.at(endpoint).queue_size;
-    pub_sock->connect(pub_context.get(), endpoint, true, queue_size);
+    pub_sock->connect(pub_context.get(), endpoint);
     sub_sock->connect(sub_context.get(), endpoint, ip, false);
 
     poller->registerSocket(sub_sock);

cereal/messaging/msgq_to_zmq.cc

@@ -2,7 +2,6 @@
 
 #include <cassert>
 
-#include "cereal/services.h"
 #include "common/util.h"
 
 extern ExitHandler do_exit;
@@ -109,8 +108,7 @@ void MsgqToZmq::zmqMonitorThread() {
           if (++pair.connected_clients == 1) {
             // Create new MSGQ subscriber socket and map to ZMQ publisher
             pair.sub_sock = std::make_unique<MSGQSubSocket>();
-            size_t queue_size = services.at(pair.endpoint).queue_size;
-            pair.sub_sock->connect(msgq_context.get(), pair.endpoint, "127.0.0.1", false, true, queue_size);
+            pair.sub_sock->connect(msgq_context.get(), pair.endpoint, "127.0.0.1");
             sub2pub[pair.sub_sock.get()] = pair.pub_sock.get();
             registerSockets();
           }

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

@@ -3,7 +3,7 @@ import numpy as np
 from collections import deque
 
 from cereal import log
-from opendbc.car.lateral import get_friction, get_friction_threshold
+from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction
 from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
@@ -95,7 +95,7 @@ class LatControlTorque(LatControl):
       # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll
       ff -= self.torque_params.latAccelOffset
       # TODO jerk is weighted by lat_delay for legacy reasons, but should be made independent of it
-      ff += get_friction(error, lateral_accel_deadzone, get_friction_threshold(CS.vEgo), self.torque_params)
+      ff += get_friction(error, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
 
       freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
       output_lataccel = self.pid.update(pid_log.error,

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -35,14 +35,15 @@ X_EGO_OBSTACLE_COST = 3.
 X_EGO_COST = 0.
 V_EGO_COST = 0.
 A_EGO_COST = 0.
-J_EGO_COST = 10.0
-A_CHANGE_COST = 150.
+J_EGO_COST = 5.0
+A_CHANGE_COST = 200.
 DANGER_ZONE_COST = 100.
 CRASH_DISTANCE = .25
 LEAD_DANGER_FACTOR = 0.75
 LIMIT_COST = 1e6
 ACADOS_SOLVER_TYPE = 'SQP_RTI'
 
+
 # Fewer timestamps don't hurt performance and lead to
 # much better convergence of the MPC with low iterations
 N = 12
@@ -52,7 +53,7 @@ T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N) for idx in range(N+1
 T_IDXS = np.array(T_IDXS_LST)
 FCW_IDXS = T_IDXS < 5.0
 T_DIFFS = np.diff(T_IDXS, prepend=[0.])
-COMFORT_BRAKE = 2.0
+COMFORT_BRAKE = 2.5
 STOP_DISTANCE = 6.0
 CRUISE_MIN_ACCEL = -1.2
 CRUISE_MAX_ACCEL = 1.6
@@ -84,12 +85,20 @@ def get_stopped_equivalence_factor(v_lead):
 def get_safe_obstacle_distance(v_ego, t_follow):
   return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE
 
+def desired_follow_distance(v_ego, v_lead, t_follow=None):
+  if t_follow is None:
+    t_follow = get_T_FOLLOW()
+  return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead)
+
+
 def gen_long_model():
   model = AcadosModel()
   model.name = MODEL_NAME
 
-  # states
-  x_ego, v_ego, a_ego = SX.sym('x_ego'), SX.sym('v_ego'), SX.sym('a_ego')
+  # set up states & controls
+  x_ego = SX.sym('x_ego')
+  v_ego = SX.sym('v_ego')
+  a_ego = SX.sym('a_ego')
   model.x = vertcat(x_ego, v_ego, a_ego)
 
   # controls
@@ -117,6 +126,7 @@ def gen_long_model():
   model.f_expl_expr = f_expl
   return model
 
+
 def gen_long_ocp():
   ocp = AcadosOcp()
   ocp.model = gen_long_model()
@@ -212,31 +222,30 @@ def gen_long_ocp():
 
 
 class LongitudinalMpc:
-  def __init__(self, dt=DT_MDL):
+  def __init__(self, mode='acc', dt=DT_MDL):
+    self.mode = mode
     self.dt = dt
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.reset()
     self.source = SOURCES[2]
 
   def reset(self):
+    # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.solver.reset()
-
-    self.x_sol = np.zeros((N+1, X_DIM))
-    self.u_sol = np.zeros((N, 1))
+    # self.solver.options_set('print_level', 2)
     self.v_solution = np.zeros(N+1)
     self.a_solution = np.zeros(N+1)
-    self.j_solution = np.zeros(N)
     self.prev_a = np.array(self.a_solution)
+    self.j_solution = np.zeros(N)
     self.yref = np.zeros((N+1, COST_DIM))
-
     for i in range(N):
       self.solver.cost_set(i, "yref", self.yref[i])
     self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
-
+    self.x_sol = np.zeros((N+1, X_DIM))
+    self.u_sol = np.zeros((N,1))
     self.params = np.zeros((N+1, PARAM_DIM))
     for i in range(N+1):
       self.solver.set(i, 'x', np.zeros(X_DIM))
-
     self.last_cloudlog_t = 0
     self.status = False
     self.crash_cnt = 0.0
@@ -267,9 +276,16 @@ class LongitudinalMpc:
 
   def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
     jerk_factor = get_jerk_factor(personality)
-    a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
-    cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
-    constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
+    if self.mode == 'acc':
+      a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
+      cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
+      constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
+    elif self.mode == 'blended':
+      a_change_cost = 40.0 if prev_accel_constraint else 0
+      cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0]
+      constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
+    else:
+      raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set')
     self.set_cost_weights(cost_weights, constraint_cost_weights)
 
   def set_cur_state(self, v, a):
@@ -304,15 +320,17 @@ class LongitudinalMpc:
 
     # MPC will not converge if immediate crash is expected
     # Clip lead distance to what is still possible to brake for
-    min_x_lead = (v_ego + v_lead) * (v_ego - v_lead) / (-ACCEL_MIN * 2)
+    min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-ACCEL_MIN * 2)
     x_lead = np.clip(x_lead, min_x_lead, 1e8)
     v_lead = np.clip(v_lead, 0.0, 1e8)
     a_lead = np.clip(a_lead, -10., 5.)
     lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
     return lead_xv
 
-  def update(self, radarstate, v_cruise, 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
 
@@ -325,28 +343,56 @@ class LongitudinalMpc:
     lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
     lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
 
-    # 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)
-    # 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), v_lower, v_upper)
-    cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)
+    self.params[:,0] = ACCEL_MIN
+    self.params[:,1] = ACCEL_MAX
 
-    x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
-    self.source = SOURCES[np.argmin(x_obstacles[0])]
+    # Update in ACC mode or ACC/e2e blend
+    if self.mode == 'acc':
+      self.params[:,5] = LEAD_DANGER_FACTOR
 
-    self.yref[:,:] = 0.0
+      # Fake an obstacle for cruise, this ensures smooth acceleration to set speed
+      # when the leads are no factor.
+      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),
+                                 v_lower,
+                                 v_upper)
+      cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)
+      x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
+      self.source = SOURCES[np.argmin(x_obstacles[0])]
+
+      # These are not used in ACC mode
+      x[:], v[:], a[:], j[:] = 0.0, 0.0, 0.0, 0.0
+
+    elif self.mode == 'blended':
+      self.params[:,5] = 1.0
+
+      x_obstacles = np.column_stack([lead_0_obstacle,
+                                     lead_1_obstacle])
+      cruise_target = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0]
+      xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1])
+      x = np.cumsum(np.insert(xforward, 0, x[0]))
+
+      x_and_cruise = np.column_stack([x, cruise_target])
+      x = np.min(x_and_cruise, axis=1)
+
+      self.source = 'e2e' if x_and_cruise[1,0] < x_and_cruise[1,1] else 'cruise'
+
+    else:
+      raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner update')
+
+    self.yref[:,1] = x
+    self.yref[:,2] = v
+    self.yref[:,3] = a
+    self.yref[:,5] = j
     for i in range(N):
       self.solver.set(i, "yref", self.yref[i])
     self.solver.set(N, "yref", self.yref[N][:COST_E_DIM])
 
-    self.params[:,0] = ACCEL_MIN
-    self.params[:,1] = ACCEL_MAX
     self.params[:,2] = np.min(x_obstacles, axis=1)
     self.params[:,3] = np.copy(self.prev_a)
     self.params[:,4] = t_follow
-    self.params[:,5] = LEAD_DANGER_FACTOR
 
     self.run()
     if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and
@@ -355,7 +401,18 @@ class LongitudinalMpc:
     else:
       self.crash_cnt = 0
 
+    # Check if it got within lead comfort range
+    # TODO This should be done cleaner
+    if self.mode == 'blended':
+      if any((lead_0_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], t_follow))- self.x_sol[:,0] < 0.0):
+        self.source = 'lead0'
+      if any((lead_1_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], t_follow))- self.x_sol[:,0] < 0.0) and \
+         (lead_1_obstacle[0] - lead_0_obstacle[0]):
+        self.source = 'lead1'
+
   def run(self):
+    # t0 = time.monotonic()
+    # reset = 0
     for i in range(N+1):
       self.solver.set(i, 'p', self.params[i])
     self.solver.constraints_set(0, "lbx", self.x0)
@@ -367,6 +424,13 @@ class LongitudinalMpc:
     self.time_linearization = float(self.solver.get_stats('time_lin')[0])
     self.time_integrator = float(self.solver.get_stats('time_sim')[0])
 
+    # qp_iter = self.solver.get_stats('statistics')[-1][-1] # SQP_RTI specific
+    # print(f"long_mpc timings: tot {self.solve_time:.2e}, qp {self.time_qp_solution:.2e}, lin {self.time_linearization:.2e}, \
+    # integrator {self.time_integrator:.2e}, qp_iter {qp_iter}")
+    # res = self.solver.get_residuals()
+    # print(f"long_mpc residuals: {res[0]:.2e}, {res[1]:.2e}, {res[2]:.2e}, {res[3]:.2e}")
+    # self.solver.print_statistics()
+
     for i in range(N+1):
       self.x_sol[i] = self.solver.get(i, 'x')
     for i in range(N):
@@ -384,8 +448,12 @@ class LongitudinalMpc:
         self.last_cloudlog_t = t
         cloudlog.warning(f"Long mpc reset, solution_status: {self.solution_status}")
       self.reset()
+      # reset = 1
+    # print(f"long_mpc timings: total internal {self.solve_time:.2e}, external: {(time.monotonic() - t0):.2e} qp {self.time_qp_solution:.2e}, \
+    # lin {self.time_linearization:.2e} qp_iter {qp_iter}, reset {reset}")
 
 
 if __name__ == "__main__":
   ocp = gen_long_ocp()
   AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
+  # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)

selfdrive/controls/lib/longitudinal_planner.py

@@ -9,7 +9,7 @@ from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, SOURCES
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_from_plan
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
@@ -28,12 +28,14 @@ MIN_ALLOW_THROTTLE_SPEED = 2.5
 _A_TOTAL_MAX_V = [1.7, 3.2]
 _A_TOTAL_MAX_BP = [20., 40.]
 
+
 def get_max_accel(v_ego):
   return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
 
 def get_coast_accel(pitch):
   return np.sin(pitch) * -5.65 - 0.3  # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py
 
+
 def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
   """
   This function returns a limited long acceleration allowed, depending on the existing lateral acceleration
@@ -68,6 +70,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     self.v_desired_trajectory = np.zeros(CONTROL_N)
     self.a_desired_trajectory = np.zeros(CONTROL_N)
     self.j_desired_trajectory = np.zeros(CONTROL_N)
+    self.solverExecutionTime = 0.0
 
   @staticmethod
   def parse_model(model_msg):
@@ -120,9 +123,16 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     # No change cost when user is controlling the speed, or when standstill
     prev_accel_constraint = not (reset_state or sm['carState'].standstill)
 
-    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)
+    if mode == 'acc':
+      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:
+      accel_clip = [ACCEL_MIN, ACCEL_MAX]
 
     if reset_state:
       self.v_desired_filter.x = v_ego
@@ -131,7 +141,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
     # Prevent divergence, smooth in current v_ego
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
-    _, _, _, _, throttle_prob = self.parse_model(sm['modelV2'])
+    x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'])
     # Don't clip at low speeds since throttle_prob doesn't account for creep
     self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
 
@@ -148,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, 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)
@@ -170,11 +182,12 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
-    output_a_target = min(output_a_target_mpc, output_a_target_e2e)
-    self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
-    if output_a_target_e2e < output_a_target_mpc:
-      self.mpc.source = SOURCES[3]
-
+    if mode == 'acc' or not self.mlsim:
+      output_a_target = output_a_target_mpc
+      self.output_should_stop = output_should_stop_mpc
+    else:
+      output_a_target = min(output_a_target_mpc, output_a_target_e2e)
+      self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
 
     for idx in range(2):
       accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)

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/controls/tests/test_following_distance.py

@@ -4,15 +4,10 @@ from parameterized import parameterized_class
 
 from cereal import log
 
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance, get_T_FOLLOW
 from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
 
 
-def desired_follow_distance(v_ego, v_lead, t_follow=None):
-  if t_follow is None:
-    t_follow = get_T_FOLLOW()
-  return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead)
-
 def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personality=0):
   man = Maneuver(
     '',

selfdrive/modeld/modeld.py

@@ -31,10 +31,9 @@ from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
 from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext
 from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
 
-from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
 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')
@@ -44,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
 
@@ -285,7 +284,6 @@ def main(demo=False):
   buf_main, buf_extra = None, None
   meta_main = FrameMeta()
   meta_extra = FrameMeta()
-  camera_offset_helper = CameraOffsetHelper()
 
 
   if demo:
@@ -300,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
@@ -341,14 +340,12 @@ def main(demo=False):
     v_ego = max(sm["carState"].vEgo, 0.)
     if sm.frame % 60 == 0:
       model.lat_delay = get_lat_delay(params, sm["liveDelay"].lateralDelay)
-      camera_offset_helper.set_offset(params.get("CameraOffset", return_default=True))
     lat_delay = model.lat_delay + LAT_SMOOTH_SECONDS
     if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
       dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
       model_transform_main = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics, False).astype(np.float32)
       model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics, True).astype(np.float32)
-      model_transform_main, model_transform_extra = camera_offset_helper.update(model_transform_main, model_transform_extra, sm, main_wide_camera)
       live_calib_seen = True
 
     traffic_convention = np.zeros(2)
@@ -399,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/SConscript

@@ -68,4 +68,4 @@ if GetOption('extras'):
       obj = raylib_env.Object(f"installer/installers/installer_{name}.o", ["installer/installer.cc"], CPPDEFINES=d)
       f = raylib_env.Program(f"installer/installers/installer_{name}", [obj, cont, inter, inter_bold, inter_light], LIBS=raylib_libs)
       # keep installers small
-      assert f[0].get_size() < 19000*1e3, f[0].get_size()
+      assert f[0].get_size() < 1900*1e3, f[0].get_size()

selfdrive/ui/layouts/settings/developer.py

@@ -1,3 +1,4 @@
+from openpilot.common.params import Params
 from openpilot.selfdrive.ui.widgets.ssh_key import ssh_key_item
 from openpilot.selfdrive.ui.ui_state import ui_state
 from openpilot.system.ui.widgets import Widget
@@ -34,7 +35,7 @@ DESCRIPTIONS = {
 class DeveloperLayout(Widget):
   def __init__(self):
     super().__init__()
-    self._params = ui_state.params
+    self._params = Params()
     self._is_release = self._params.get_bool("IsReleaseBranch")
 
     # Build items and keep references for callbacks/state updates

selfdrive/ui/layouts/settings/device.py

@@ -3,6 +3,7 @@ import math
 
 from cereal import messaging, log
 from openpilot.common.basedir import BASEDIR
+from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.ui.onroad.driver_camera_dialog import DriverCameraDialog
 from openpilot.selfdrive.ui.ui_state import ui_state
@@ -34,7 +35,7 @@ class DeviceLayout(Widget):
   def __init__(self):
     super().__init__()
 
-    self._params = ui_state.params
+    self._params = Params()
     self._select_language_dialog: MultiOptionDialog | None = None
     self._driver_camera: DriverCameraDialog | None = None
     self._pair_device_dialog: PairingDialog | None = None

selfdrive/ui/layouts/settings/toggles.py

@@ -1,5 +1,5 @@
 from cereal import log
-from openpilot.common.params import UnknownKeyName
+from openpilot.common.params import Params, UnknownKeyName
 from openpilot.system.ui.widgets import Widget
 from openpilot.system.ui.widgets.list_view import multiple_button_item, toggle_item
 from openpilot.system.ui.widgets.scroller_tici import Scroller
@@ -41,7 +41,7 @@ DESCRIPTIONS = {
 class TogglesLayout(Widget):
   def __init__(self):
     super().__init__()
-    self._params = ui_state.params
+    self._params = Params()
     self._is_release = self._params.get_bool("IsReleaseBranch")
 
     # param, title, desc, icon, needs_restart
@@ -198,6 +198,11 @@ class TogglesLayout(Widget):
 
     self._update_experimental_mode_icon()
 
+    # TODO: make a param control list item so we don't need to manage internal state as much here
+    # refresh toggles from params to mirror external changes
+    for param in self._toggle_defs:
+      self._toggles[param].action_item.set_state(self._params.get_bool(param))
+
     # these toggles need restart, block while engaged
     for toggle_def in self._toggle_defs:
       if self._toggle_defs[toggle_def][3] and toggle_def not in self._locked_toggles:

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/layouts/settings/models.py

@@ -1,214 +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 collections.abc import Callable
-
-from cereal import custom
-from openpilot.common.constants import CV
-from openpilot.selfdrive.ui.mici.widgets.button import BigButton, BigToggle
-from openpilot.selfdrive.ui.mici.widgets.dialog import BigConfirmationDialogV2
-from openpilot.selfdrive.ui.ui_state import ui_state
-from openpilot.system.ui.lib.application import gui_app
-from openpilot.system.ui.lib.multilang import tr
-from openpilot.system.ui.widgets import NavWidget, Widget
-from openpilot.system.ui.widgets.scroller import Scroller
-
-
-class ModelsLayoutMici(NavWidget):
-  def __init__(self, back_callback: Callable):
-    super().__init__()
-    self.set_back_callback(back_callback)
-    self.original_back_callback = back_callback
-    self.refresh_start_time = 0
-    self.focused_widget = None
-
-    self.current_model_btn = BigButton(tr("current model"), "", "")
-    self.current_model_btn.set_click_callback(self._show_folders)
-
-    self.refresh_btn = BigButton(tr("refresh model list"), "", "")
-    self.refresh_btn.set_click_callback(self._handle_refresh)
-
-    self.clear_cache_btn = BigButton(tr("clear model cache"), "", "")
-    self.clear_cache_btn.set_click_callback(self._handle_clear_cache)
-
-    self.cancel_download_btn = BigButton(tr("cancel download"), "", "")
-    self.cancel_download_btn.set_click_callback(lambda: ui_state.params.remove("ModelManager_DownloadIndex"))
-
-    self.lane_turn_toggle = BigToggle(text=tr("use lane turn desires"), initial_state=ui_state.params.get_bool("LaneTurnDesire"),
-                                      toggle_callback=lambda state: ui_state.params.put_bool("LaneTurnDesire", state))
-
-    self.lagd_toggle = BigToggle(text=tr("live learning steer delay"), initial_state=ui_state.params.get_bool("LagdToggle"),
-                                 toggle_callback=lambda state: ui_state.params.put_bool("LagdToggle", state))
-
-    self.lane_turn_value_btn = BigButton(tr("adjust lane turn speed"), "", "")
-    self.lane_turn_value_btn.set_click_callback(self._adjust_lane_turn)
-    self.delay_btn = BigButton(tr("adjust software delay"), "", "")
-    self.delay_btn.set_click_callback(self._adjust_delay)
-
-    self.main_items: list[Widget] = [self.current_model_btn, self.cancel_download_btn, self.refresh_btn, self.clear_cache_btn, self.lane_turn_toggle,
-                       self.lane_turn_value_btn, self.lagd_toggle, self.delay_btn]
-    self._scroller = Scroller(self.main_items, snap_items=False)
-
-  @property
-  def model_manager(self):
-    return ui_state.sm["modelManagerSP"]
-
-  def _get_grouped_bundles(self):
-    bundles = self.model_manager.availableBundles
-    folders = {}
-    for bundle in bundles:
-      folder = next((override.value for override in bundle.overrides if override.key == "folder"), "")
-      folders.setdefault(folder, []).append(bundle)
-    return folders
-
-  def _show_selection_view(self, items: list[Widget], back_callback: Callable):
-    self._scroller._items = items
-    for item in items:
-      item.set_touch_valid_callback(lambda: self._scroller.scroll_panel.is_touch_valid() and self._scroller.enabled)
-    self._scroller.scroll_panel.set_offset(0)
-    self.set_back_callback(back_callback)
-
-  def _show_folders(self):
-    self.focused_widget = self.current_model_btn
-    folders = self._get_grouped_bundles()
-    folder_buttons = []
-    default_btn = BigButton(tr("default model"), "", "")
-    default_btn.set_click_callback(self._select_default)
-    folder_buttons.append(default_btn)
-
-    for folder in sorted(folders.keys(), key=lambda f: max((bundle.index for bundle in folders[f]), default=-1), reverse=True):
-      if folder:
-        btn = BigButton(folder.lower(), "", "")
-        btn.set_click_callback(lambda f=folder: self._select_folder(f))
-        folder_buttons.append(btn)
-    self._show_selection_view(folder_buttons, self._reset_main_view)
-
-  def _handle_refresh(self):
-    self.refresh_btn.set_text(tr("refreshing..."))
-    self.refresh_start_time = time.monotonic()
-    ui_state.params.put("ModelManager_LastSyncTime", 0)
-
-  def _handle_clear_cache(self):
-    gui_app.set_modal_overlay(BigConfirmationDialogV2(tr("clear model cache?"), "icons_mici/settings/device/update.png",
-                                                      confirm_callback=lambda: ui_state.params.put_bool("ModelManager_ClearCache", True)))
-
-  def _select_model(self, bundle):
-    ui_state.params.put("ModelManager_DownloadIndex", bundle.index)
-    self._reset_main_view()
-
-  def _select_default(self):
-    ui_state.params.remove("ModelManager_ActiveBundle")
-    self._reset_main_view()
-
-  def _select_folder(self, folder_name):
-    folders = self._get_grouped_bundles()
-    bundles = sorted(folders.get(folder_name, []), key=lambda b: b.index, reverse=True)
-
-    btns = []
-    for bundle in bundles:
-      txt = bundle.displayName.lower()
-      if self.model_manager.activeBundle and self.model_manager.activeBundle.index == bundle.index:
-        txt += " (active)"
-      elif bundle.status in (custom.ModelManagerSP.DownloadStatus.downloaded, custom.ModelManagerSP.DownloadStatus.cached):
-        txt += " (cached)"
-
-      btn = BigButton(txt, "", "")
-      btn.set_click_callback(lambda b=bundle: self._select_model(b))
-      btns.append(btn)
-    self._show_selection_view(btns, self._show_folders)
-
-  def _reset_main_view(self):
-    self._scroller._items = self.main_items
-    self.set_back_callback(self.original_back_callback)
-    if self.focused_widget and self.focused_widget in self.main_items:
-      x = self._scroller._pad_start
-      for item in self.main_items:
-        if not item.is_visible:
-          continue
-        if item == self.focused_widget:
-          break
-        x += item.rect.width + self._scroller._spacing
-      self._scroller.scroll_panel.set_offset(0)
-      self._scroller.scroll_to(x)
-      self.focused_widget = None
-    else:
-      self._scroller.scroll_panel.set_offset(0)
-
-  def _create_buttons(self, values, current_val, label, callback):
-    buttons = []
-    for value in values:
-      suffix = " (current)" if value == current_val else ""
-      btn = BigButton(f"{label(value)}{suffix}", "", "")
-      btn.set_click_callback(lambda v=value: callback(v))
-      buttons.append(btn)
-    return buttons
-
-  def _adjust_lane_turn(self):
-    self.focused_widget = self.lane_turn_value_btn
-    lane_turn_value = float(ui_state.params.get("LaneTurnValue", return_default=True))
-    is_metric = ui_state.is_metric
-    cur = int(round(lane_turn_value * CV.MPH_TO_KPH)) if is_metric else int(round(lane_turn_value))
-    values = [8, 16, 24, 32] if is_metric else [5, 10, 15, 20]
-
-    btns = self._create_buttons(values, cur, lambda v: f"{v} {'km/h' if is_metric else 'mph'}", self._set_lane_turn)
-    self._show_selection_view(btns, self._reset_main_view)
-
-  def _set_lane_turn(self, value):
-    val = value / CV.MPH_TO_KPH if ui_state.is_metric else float(value)
-    ui_state.params.put("LaneTurnValue", val)
-    self._reset_main_view()
-
-  def _adjust_delay(self):
-    self.focused_widget = self.delay_btn
-    current_delay = float(ui_state.params.get("LagdToggleDelay", return_default=True))
-    values = [round(i * 0.01, 2) for i in range(10, 31)]
-    btns = self._create_buttons(values, current_delay, lambda v: f"{v:.2f}s", self._set_delay)
-    self._show_selection_view(btns, self._reset_main_view)
-
-  def _set_delay(self, value):
-    ui_state.params.put("LagdToggleDelay", value)
-    self._reset_main_view()
-
-  def _update_state(self):
-    super()._update_state()
-    if self.refresh_start_time > 0 and time.monotonic() - self.refresh_start_time > 1:
-      self.refresh_btn.set_text(tr("refresh model list"))
-      self.refresh_start_time = 0
-
-    manager = self.model_manager
-    if manager.selectedBundle and manager.selectedBundle.status == custom.ModelManagerSP.DownloadStatus.downloading:
-      self.current_model_btn.set_value(f"downloading {manager.selectedBundle.displayName.lower()}")
-      self.cancel_download_btn.set_visible(True)
-    else:
-      self.current_model_btn.set_value(manager.activeBundle.internalName.lower() if manager.activeBundle else tr("default model"))
-      self.cancel_download_btn.set_visible(False)
-    self.current_model_btn.set_enabled(ui_state.is_offroad())
-    self.current_model_btn.set_text(tr("current model"))
-
-    advanced_controls = ui_state.params.get_bool("ShowAdvancedControls")
-    turn_desires = ui_state.params.get_bool("LaneTurnDesire")
-    lagd_delay = ui_state.params.get_bool("LagdToggle")
-
-    self.lane_turn_value_btn.set_visible(turn_desires and advanced_controls)
-    if turn_desires and advanced_controls:
-      lane_turn_value = float(ui_state.params.get("LaneTurnValue", return_default=True))
-      val = int(round(lane_turn_value * CV.MPH_TO_KPH)) if ui_state.is_metric else int(round(lane_turn_value))
-      self.lane_turn_value_btn.set_text(tr("adjust lane turn speed"))
-      self.lane_turn_value_btn.set_value(f"{val} {'km/h' if ui_state.is_metric else 'mph'}")
-
-    self.delay_btn.set_visible(not lagd_delay and advanced_controls)
-    if not lagd_delay and advanced_controls:
-      toggle_delay = float(ui_state.params.get("LagdToggleDelay", return_default=True))
-      self.delay_btn.set_text(tr("adjust software delay"))
-      self.delay_btn.set_value(f"{toggle_delay:.2f}s")
-
-  def _render(self, rect):
-    self._scroller.render(rect)
-
-  def show_event(self):
-    super().show_event()
-    self._scroller.show_event()

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

@@ -226,7 +226,9 @@ class ModelsLayout(Widget):
     turn_desire: bool = ui_state.params.get_bool("LaneTurnDesire")
     live_delay: bool = ui_state.params.get_bool("LagdToggle")
 
+    self.lane_turn_desire_toggle.action_item.set_state(turn_desire)
     self.lane_turn_value_control.set_visible(turn_desire and advanced_controls)
+    self.lagd_toggle.action_item.set_state(live_delay)
     self.delay_control.set_visible(not live_delay and advanced_controls)
     new_step = int(round(100 / CV.MPH_TO_KPH)) if ui_state.is_metric else 100
     if self.lane_turn_value_control.action_item.value_change_step != new_step:

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/layouts/settings/sunnylink.py

@@ -336,6 +336,7 @@ class SunnylinkLayout(Widget):
     self._sunnylink_enabled = ui_state.params.get_bool("SunnylinkEnabled")
     self._sunnylink_toggle.set_right_value(tr("Dongle ID") + ": " + self._get_sunnylink_dongle_id())
     self._sunnylink_toggle.action_item.set_enabled(not ui_state.is_onroad())
+    self._sunnylink_toggle.action_item.set_state(self._sunnylink_enabled)
     self._sunnylink_uploader_toggle.action_item.set_enabled(self._sunnylink_enabled)
     self.handle_backup_restore_progress()
 

selfdrive/ui/sunnypilot/layouts/settings/vehicle/brands/hyundai.py

@@ -55,4 +55,5 @@ class HyundaiSettings(BrandSettings):
     self.longitudinal_tuning_item.action_item.set_enabled(not longitudinal_tuning_disabled)
     self.longitudinal_tuning_item.set_description(long_tuning_desc)
     self.longitudinal_tuning_item.show_description(True)
+    self.longitudinal_tuning_item.action_item.set_selected_button(tuning_param)
     self.longitudinal_tuning_item.set_visible(self.alpha_long_available)

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

@@ -9,7 +9,6 @@ from enum import IntEnum
 from openpilot.selfdrive.ui.mici.layouts.settings import settings as OP
 from openpilot.selfdrive.ui.mici.widgets.button import BigButton
 from openpilot.selfdrive.ui.sunnypilot.mici.layouts.sunnylink import SunnylinkLayoutMici
-from openpilot.selfdrive.ui.mici.layouts.settings.models import ModelsLayoutMici
 
 ICON_SIZE = 70
 
@@ -17,7 +16,6 @@ OP.PanelType = IntEnum(
   "PanelType",
   [es.name for es in OP.PanelType] + [
     "SUNNYLINK",
-    "MODELS",
   ],
   start=0,
 )
@@ -29,17 +27,13 @@ class SettingsLayoutSP(OP.SettingsLayout):
 
     sunnylink_btn = BigButton("sunnylink", "", "icons_mici/settings/developer/ssh.png")
     sunnylink_btn.set_click_callback(lambda: self._set_current_panel(OP.PanelType.SUNNYLINK))
-    models_btn = BigButton("models", "", "../../sunnypilot/selfdrive/assets/offroad/icon_models.png")
-    models_btn.set_click_callback(lambda: self._set_current_panel(OP.PanelType.MODELS))
     self._panels.update({
       OP.PanelType.SUNNYLINK: OP.PanelInfo("sunnylink", SunnylinkLayoutMici(back_callback=lambda: self._set_current_panel(None))),
-      OP.PanelType.MODELS: OP.PanelInfo("models", ModelsLayoutMici(back_callback=lambda: self._set_current_panel(None))),
     })
 
     items = self._scroller._items.copy()
 
     items.insert(1, sunnylink_btn)
-    items.insert(2, models_btn)
     self._scroller._items.clear()
     for item in items:
       self._scroller.add_widget(item)

selfdrive/ui/sunnypilot/ui_helpers.py

@@ -1,41 +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.
-"""
-
-
-def update_item_from_param(item, key, params):
-  if not (action := getattr(item, 'action_item', None)):
-    return
-
-  if hasattr(action, 'set_state'):
-    action.set_state(params.get_bool(key))
-  elif hasattr(action, 'set_value'):
-    action.set_value(params.get(key, return_default=True))
-  else:
-    try:
-      val = int(params.get(key, return_default=True))
-      if hasattr(action, 'selected_button'):
-        action.selected_button = val
-      if hasattr(action, 'current_value'):
-        action.current_value = val
-    except (ValueError, TypeError):
-      pass
-
-
-def sync_layout_params(layout, param_name, params):
-  targets = []
-  if toggles := getattr(layout, '_toggles', None):
-    targets.extend([(item, k) for k, item in toggles.items()])
-
-  items = getattr(layout, 'items', []) or getattr(getattr(layout, '_scroller', None), '_items', [])
-  for item in items:
-    action = getattr(item, 'action_item', None)
-    if key := getattr(action, 'param_key', None) or getattr(getattr(action, 'toggle', None), 'param_key', None):
-      targets.append((item, key))
-
-  for item, key in targets:
-    if param_name is None or key == param_name:
-      update_item_from_param(item, key, params)

selfdrive/ui/sunnypilot/ui_state.py

@@ -7,13 +7,10 @@ See the LICENSE.md file in the root directory for more details.
 from enum import Enum
 
 from cereal import messaging, log, custom
-from openpilot.common.swaglog import cloudlog
-
-from openpilot.sunnypilot.common.params import Params
+from openpilot.common.params import Params
 from openpilot.selfdrive.ui.sunnypilot.layouts.settings.display import OnroadBrightness
 from openpilot.sunnypilot.sunnylink.sunnylink_state import SunnylinkState
 from openpilot.system.ui.lib.application import gui_app
-from openpilot.selfdrive.ui.sunnypilot.ui_helpers import sync_layout_params
 
 OpenpilotState = log.SelfdriveState.OpenpilotState
 MADSState = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
@@ -30,25 +27,14 @@ class OnroadTimerStatus(Enum):
 class UIStateSP:
   def __init__(self):
     self.params = Params()
-    self.params.add_watcher(self.on_param_change)
-    self.params.start()
     self.sm_services_ext = [
       "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
-      "gpsLocation", "liveTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "liveDelay"
+      "gpsLocation", "liveTorqueParameters", "carStateSP", "carParamsSP", "liveDelay",
+      "mapdOut"
     ]
 
     self.sunnylink_state = SunnylinkState()
     self.update_params()
-    self.active_layout = None
-    self.changed_params = set()
-
-  def set_active_layout(self, layout):
-    self.active_layout = layout
-    if layout:
-      sync_layout_params(layout, None, self.params)
-
-  def on_param_change(self, param_name):
-    self.changed_params.add(param_name)
 
     self.onroad_brightness_timer: int = 0
     self.custom_interactive_timeout: int = self.params.get("InteractivityTimeout", return_default=True)
@@ -94,18 +80,6 @@ class UIStateSP:
   def auto_onroad_brightness(self) -> bool:
     return self.onroad_brightness in (OnroadBrightness.AUTO, OnroadBrightness.AUTO_DARK)
 
-    if not self.params.is_watching():
-      cloudlog.warning("ParamWatcher thread died, restarting...")
-      self.params.start()
-
-    if self.changed_params:
-      while self.changed_params:
-        self.changed_params.pop()
-
-      if self.active_layout:
-        sync_layout_params(self.active_layout, None, self.params)
-
-
   @staticmethod
   def update_status(ss, ss_sp, onroad_evt) -> str:
     state = ss.state

selfdrive/ui/ui_state.py

@@ -6,6 +6,7 @@ from collections.abc import Callable
 from enum import Enum
 from cereal import messaging, car, log
 from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.ui.lib.prime_state import PrimeState
 from openpilot.system.ui.lib.application import gui_app
@@ -35,6 +36,7 @@ class UIState(UIStateSP):
 
   def _initialize(self):
     UIStateSP.__init__(self)
+    self.params = Params()
     self.sm = messaging.SubMaster(
       [
         "modelV2",

sunnypilot/common/README.md

@@ -1,146 +0,0 @@
-<center>
-
-# Comparative Analysis of Parameter Getter Methods: Params vs. ParamWatcher
-
-James (sunnypilot Developer) <br>
-December 13, 2025
-
-</center>
-
-
-## <br><br> Abstract
-
-This research report examines the inefficiencies in standard parameter access methods within sunnypilot and proposes an optimized alternative, ParamWatcher. The standard `Params::get()` method incurs significant CPU and memory overhead due to repeated file I/O operations (sunnypilot, 2025). ParamWatcher utilizes OS-level file system events (inotify on Linux, FSEvents on macOS) to maintain an in-memory cache, reducing I/O to near zero (Linux man-pages, 2025-c; Apple Inc., n.d.-a). Empirical benchmarks with ~10 million parameter accesses demonstrate a 14.5x CPU speedup and flat memory usage (514.7 KB vs. 497.7 KB for base Params, with only 17 KB overhead). The implementation employs a process-local singleton pattern for efficiency in multi-process architectures (Gamma et al., 1994). Results indicate ParamWatcher eliminates UI stutters and GC pauses, enhancing system responsiveness without compromising data freshness.
-
-**Keywords:** parameter access, file I/O optimization, event-driven caching, autonomous driving systems, performance benchmarking
-
-## Introduction
-
-In sunnypilot, efficient parameter management is important for real-time system access. The standard `Params::get()` method, implemented in C++ and wrapped in Python, performs full file I/O cycles for each access, leading to high CPU overhead and memory churn (sunnypilot, 2025). This is particularly problematic in UI loops where parameters are queried frequently (e.g., 50 toggles at 20 FPS equates to ~1,000 reads/second).
-
-This inefficiency stems from architectural mismatches: C++ streams are designed for throughput, not latency (cppreference.com, n.d.-a). Each call triggers kernel mode switches, heap allocations, and garbage collection in Python, causing UI stutters (Linux man-pages, 2025-a; Linux man-pages, 2025-b).
-
-### Inefficiencies in Standard Parameter Access
-The standard `Params::get()` method executes a full file I/O lifecycle—opening, allocating, reading, and closing—for every function call. This results in significant CPU overhead and memory churn due to the frequency of these operations in the user interface loop.
-
-#### System Overhead Analysis
-- **System Call Overhead**: Every read operation requires context switches into kernel mode. The `Params::get` function calls `util::read_file` (sunnypilot, 2025), which subsequently invokes `std::ifstream` (sunnypilot, 2025).
-- **Impact**: Frequent context switching degrades performance (Linux man-pages, 2025-a; Linux man-pages, 2025-b).
-- **C++ Stream Overhead**: The use of `std::ifstream` introduces additional overhead for maintaining stream state and buffering compared to raw file descriptors (cppreference.com, n.d.-a; Codezup, 2025).
-- **Memory Churn**: The instantiation of `std::string result(size, '\0');` forces heap allocation and deallocation during every call (sunnypilot, 2025). This stresses the memory allocator and can lead to fragmentation (cppreference.com, n.d.-b).
-
-This report introduces ParamWatcher, an event-driven caching solution using OS file system events. It shifts from polling to notifications, caching converted values in static RAM. I propose that ParamWatcher achieves minimum 10x+ CPU gains with bounded non increasing memory, improving sunnypilot's performance both on latency and responsiveness.
-
-## Method
-
-### Materials
-- **System:** sunnypilot, running on macOS, Ubuntu/Linux, comma 3x, and comma four.
-- **Parameters:** 231 defined keys in `param_keys.h`.
-- **Tools:** Python 3, tracemalloc for memory profiling, time.perf_counter for CPU timing, ctypes for OS integration (Python Software Foundation, 2025).
-
-### Implementation Details
-ParamWatcher provides cross-platform file system monitoring using ctypes for direct OS integration (Python Software Foundation, 2025).
-
-#### Linux Implementation
-On Linux, ParamWatcher uses the inotify subsystem for efficient file change detection (Linux man-pages, 2025-c). It loads `libc.so.6` to access system calls, initializes an inotify instance, and watches the parameters directory for events like `IN_MODIFY` and `IN_CLOSE_WRITE` (Linux Kernel Organization, 2005). Events are polled with `select.epoll()` and parsed using `struct.unpack_from()` to avoid ctypes overhead. Filenames are extracted and passed to cache invalidation, ensuring real-time updates without polling (Codezup, 2025).
-
-- **Library Loading**: `libc = ctypes.CDLL('libc.so.6')` loads the standard C library to access system calls.
-- **Initialization**: `inotify_init()` is called to create a new inotify instance, returning a file descriptor.
-- **Watch Setup**: `inotify_add_watch(fd, path, mask)` registers the parameters directory. The mask includes `IN_MODIFY | IN_CREATE | IN_DELETE | IN_MOVED_TO | IN_CLOSE_WRITE` (Linux Kernel Organization, 2005) to capture all relevant file changes.
-- **Event Loop**:
-  - **Polling**: `select.epoll()` is used to efficiently wait for activity on the file descriptor without busy-waiting.
-  - **Reading**: When events occur, `os.read(fd, 1024)` retrieves the raw binary event data.
-  - **Parsing**: The code uses Python's `struct` module (`struct.unpack_from("iIII", ...)`) to parse the C-style `inotify_event` structures directly from the buffer, avoiding the overhead of defining `ctypes` structures.
-  - **Handling**: Extracted filenames are passed to `_trigger_callbacks`, which invalidates the specific cache entry (`self._cache.pop(path, None)`), forcing a fresh read on the next access.
-
-#### macOS Implementation
-On macOS, ParamWatcher leverages FSEvents from CoreServices for directory monitoring (Apple Inc., n.d.-a). It defines a C-compatible callback using `CFUNCTYPE`, creates an `FSEventStream` with `kFSEventStreamCreateFlagFileEvents`, and schedules it on the run loop (Apple Inc., n.d.-b). Events are filtered for modifications, creations, and renames (Apple Inc., n.d.-c), triggering cache invalidation for affected parameters.
-
-- **Framework Loading**: `ctypes.cdll.LoadLibrary` loads `CoreServices` and `CoreFoundation`.
-- **Callback Definition**: `CFUNCTYPE` is used to define a C-compatible callback function. This function is invoked by the OS whenever a change occurs in the watched directory.
-- **Stream Creation**: `FSEventStreamCreate` creates a stream for the target directory. The `kFSEventStreamCreateFlagFileEvents` flag is used to request file-level granularity where available.
-- **Event Filtering**: The callback filters events using flags such as `kFSEventStreamEventFlagItemCreated` and `kFSEventStreamEventFlagItemModified` to ensure only relevant file changes trigger updates (Apple Inc., n.d.-c).
-- **Scheduling**: `FSEventStreamScheduleWithRunLoop` attaches the stream to the current thread's run loop (Apple Inc., n.d.-b).
-- **Execution**: `CFRunLoopRun()` starts the event loop. This passes control to the OS, which wakes the thread only when necessary.
-- **Handling**: Inside the callback, the code iterates through the changed paths provided by the OS. It extracts the filename and calls `_trigger_callbacks` to invalidate the cache for that specific parameter.
-
-### Procedure
-Benchmarks simulated heavy load:
-- **Memory Test:** ~10 million gets (43,290 loops over 231 keys), measured with tracemalloc.
-- **CPU Test:** Same load, timed with perf_counter.
-- Comparisons: Base Params vs. ParamWatcher.
-
-## Results
-
-### Memory Usage
-Using tracemalloc for peak memory measurement during ~10 million parameter accesses (43,290 loops over 231 keys), base Params peaked at 497.7 KB, while ParamWatcher peaked at 514.7 KB (17 KB overhead). ParamWatcher's memory remained flat post-initialization, preventing churn.
-
-| Condition | Memory (KB) | Overhead |
-|-----------|-------------|----------|
-| Base Params | 497.7 | - |
-| ParamWatcher | 514.7 | 17 KB |
-### CPU Performance
-ParamWatcher was 14.5x faster: 4.52s vs. 65.43s to complete ~10 million param gets.
-
-| Condition | Time (s) | Speedup |
-|-----------|----------|---------|
-| Base Params | 65.43 | 1x |
-| ParamWatcher | 4.52 | 14.5x |
-
-### Scalability
-No degradation at scale; cache invalidation maintained freshness.
-
-See Appendix A for visual graphs of memory usage over a 30-minute time span, captured on comma four. These two routes are of equal conditions: each route started completely unplugged to two minutes offroad, followed by onroad state, with ambient temperature at 75 degrees fahrenheit. These routes are direct comparisons of pre-ParamWatcher and ParamWatcher implementations. Appendix A also includes I/O capture graphs for those 30-minute routes, demonstrating reductions in file system activity post-ParamWatcher.
-
-## Discussion
-
-ParamWatcher successfully optimizes parameter access, delivering substantial CPU gains with minimal memory overhead. The event-driven approach eliminates I/O bottlenecks, reducing GC pressure and UI stutters (cppreference.com, n.d.-b). The 17 KB memory overhead is negligible compared to the megabytes of churn from base Params, ensuring bounded usage in multi-process environments via the singleton pattern (Gamma et al., 1994).
-
-Results demonstrate scalability without degradation, with cache invalidation maintaining data freshness. This optimization enhances system responsiveness.
-Limitations include potential event latency in high-load scenarios (<10 ms, imperceptible for UI) and increased complexity from background threads.
-Trade-offs: Static RAM (~17 KB) vs. dynamic churn; benefits outweigh costs for param-heavy workloads.
-
-## <br> Appendix A: Memory Usage Graphs
-
-### Base Params Memory Usage
-![Base Params Memory Usage](assets/memory_usage_pre_paramwatcher.png)
-
-### ParamWatcher Memory Usage
-![ParamWatcher Memory Usage](assets/memory_usage_00000025--d50a4a3471.png)
-
-### Base Params IO Usage
-![Base Params IO Usage](assets/io_usage_pre_paramwatcher.png)
-
-### ParamWatcher IO Usage
-![Base Params IO Usage](assets/io_usage_param_watcher.png)
-
-
-## <br>References
-
-Apple Inc. (n.d.-a). *File System Events*. Retrieved from https://developer.apple.com/documentation/coreservices/file_system_events
-
-Apple Inc. (n.d.-b). *CFRunLoop*. Retrieved from https://developer.apple.com/documentation/corefoundation/cfrunloop
-
-Apple Inc. (n.d.-c). *FSEventStreamEventFlags*. Retrieved from https://developer.apple.com/documentation/coreservices/1455361-fseventstreameventflags
-
-Codezup. (2025). *Efficient File I/O in C++*. Retrieved from https://codezup.com/efficient-file-io-cpp-best-practices/
-
-cppreference.com. (n.d.-a). *std::basic_ifstream*. Retrieved from https://en.cppreference.com/w/cpp/io/basic_ifstream
-
-cppreference.com. (n.d.-b). *std::basic_string*. Retrieved from https://en.cppreference.com/w/cpp/string/basic_string/basic_string
-
-Gamma, E., Helm, R., Johnson, R., & Vlissides, J. (1994). *Design Patterns: Elements of Reusable Object-Oriented Software*. Addison-Wesley.
-
-Linux Kernel Organization. (2005). *include/uapi/linux/inotify.h*. Retrieved from https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git/tree/include/uapi/linux/inotify.h
-
-Linux man-pages. (2025-a). *open(2)*. Retrieved from https://man7.org/linux/man-pages/man2/open.2.html
-
-Linux man-pages. (2025-b). *read(2)*. Retrieved from https://man7.org/linux/man-pages/man2/read.2.html
-
-Linux man-pages. (2025-c). *inotify(7)*. Retrieved from https://man7.org/linux/man-pages/man7/inotify.7.html
-
-Python Software Foundation. (2025). *ctypes — A foreign function library for Python*. Retrieved from https://docs.python.org/3/library/ctypes.html
-
-sunnypilot. (2025). *common/params.cc* [Source code]. GitHub. https://github.com/sunnypilot/sunnypilot/blob/master/common/params.cc#L180C1-L206C2
-
-sunnypilot. (2025). *common/util.cc* [Source code]. GitHub. https://github.com/sunnypilot/sunnypilot/blob/master/common/util.cc#L79C1-L117C2

sunnypilot/common/assets/io_usage_param_watcher.png

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:b94f93515223b41fba5250ffb7c795ebb1640c524de5296393b02bd36a202a18
-size 566837

sunnypilot/common/assets/io_usage_pre_paramwatcher.png

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:696dc900066c78de38e64f0636716a588f68f2fa4a3d4fbfdb5ca3f8ab49e922
-size 291424

sunnypilot/common/assets/memory_usage_00000025--d50a4a3471.png

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:abb4a5ec3108d337cb4aab0d009c5dd7c03f601f725188fcb933f7dc4cd1b1fa
-size 248658

sunnypilot/common/assets/memory_usage_pre_paramwatcher.png

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:a27f8ba0633e806083ce41bb51fbabcd027f8ad713534dc04298cc2f350b3389
-size 311370

sunnypilot/common/param_watcher.py

@@ -1,193 +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 os
-import platform
-import struct
-import select
-import threading
-import time
-import ctypes
-import ctypes.util
-import traceback
-from ctypes import c_void_p, c_size_t, POINTER, c_uint32, c_uint64
-
-from openpilot.common.params import Params
-from openpilot.common.swaglog import cloudlog
-from openpilot.system.hardware.hw import Paths
-
-IN_MODIFY = 0x00000002
-IN_CREATE = 0x00000100
-IN_DELETE = 0x00000200
-IN_MOVED_TO = 0x00000080
-IN_CLOSE_WRITE = 0x00000008
-
-
-class ParamWatcher(Params):
-  _instance = None
-
-  def __new__(cls):
-    if cls._instance is None:
-      cls._instance = super().__new__(cls)
-      cls._instance._initialized = False
-    return cls._instance
-
-  def __init__(self):
-    if self._initialized:
-      return
-    super().__init__()
-    cloudlog.warning("ParamWatcher initialized")
-    self._cache = {}
-    self._last_trigger = {}
-    self._version = {}
-    self._lock = threading.Lock()
-    self._callbacks = []
-    self.last_accessed_param = None
-    self._initialized = True
-    self.start()
-
-  def start(self):
-    if getattr(self, '_thread', None) and self._thread.is_alive():
-      return
-    self._thread = threading.Thread(target=self._run_watcher, daemon=True)
-    self._thread.start()
-
-  def is_watching(self):
-    return getattr(self, '_thread', None) and self._thread.is_alive()
-
-  def add_watcher(self, callback):
-    if callback not in self._callbacks:
-      self._callbacks.append(callback)
-
-  def _trigger_callbacks(self, path):
-    with self._lock:
-      if (now := time.monotonic()) - self._last_trigger.get(path, 0) < 0.1:
-        return
-      self._last_trigger[path] = now
-      self._version[path] = self._version.get(path, 0) + 1
-      self._cache.pop(path, None)
-
-    for callback in self._callbacks:
-      try:
-        callback(path)
-      except Exception:
-        cloudlog.exception("Param watcher callback failed")
-
-  def _get_cached(self, key, getter, sig):
-    k = str(key)
-    with self._lock:
-      bucket = self._cache.get(k)
-      if bucket and sig in bucket:
-        if bucket[sig][0] == self._version.get(k, 0):
-          return bucket[sig][1]
-
-    start_ver = self._version.get(k, 0)
-    val = getter()
-    with self._lock:
-      if self._version.get(k, 0) != start_ver:
-        val = getter()
-      self._cache.setdefault(k, {})[sig] = (self._version.get(k, 0), val)
-    return val
-
-  def get(self, key, block=False, return_default=False):
-    self.last_accessed_param = key
-    if block:
-      return super().get(key, block, return_default)
-    fetcher = super().get
-    return self._get_cached(key, lambda: fetcher(key, block, return_default), (block, return_default))
-
-  def get_bool(self, key, block=False):
-    self.last_accessed_param = key
-    if block:
-      return super().get_bool(key, block)
-    fetcher = super().get_bool
-    return self._get_cached(key, lambda: fetcher(key, block), ("bool", block))
-
-  def _run_watcher(self):
-    system = platform.system()
-    while True:
-      try:
-        if system == "Linux":
-          self._run_linux()
-        elif system == "Darwin":
-          self._run_darwin()
-      except Exception:
-        cloudlog.exception("Param watcher crashed")
-        time.sleep(2)
-
-  def _run_linux(self):
-    path = Paths.params_root()
-    libc = ctypes.CDLL('libc.so.6')
-    fd = libc.inotify_init()
-    libc.inotify_add_watch(fd, path.encode(), IN_MODIFY | IN_CREATE | IN_DELETE | IN_MOVED_TO | IN_CLOSE_WRITE)
-
-    try:
-      poll = select.epoll()
-      poll.register(fd, select.EPOLLIN)
-      while True:
-        for fileno, _ in poll.poll():
-          if fileno == fd:
-            buffer = os.read(fd, 2048)
-            i = 0
-            while i + 16 <= len(buffer):
-              _, mask, _, name_len = struct.unpack_from("iIII", buffer, i)
-              if mask & (IN_MODIFY | IN_CREATE | IN_DELETE | IN_MOVED_TO | IN_CLOSE_WRITE):
-                name = buffer[i+16:i+16+name_len].rstrip(b"\0").decode()
-                if not name.startswith("."):
-                  self._trigger_callbacks(name)
-              i += 16 + name_len
-    finally:
-      if 'poll' in locals():
-        poll.unregister(fd)
-        poll.close()
-      os.close(fd)
-
-  def _run_darwin(self):
-    CS = ctypes.cdll.LoadLibrary(ctypes.util.find_library("CoreServices"))
-    CF = ctypes.cdll.LoadLibrary(ctypes.util.find_library("CoreFoundation"))
-
-    kCFAllocatorDefault = c_void_p(0)
-    kCFStringEncodingUTF8 = 0x08000100
-    kFSEventStreamCreateFlagFileEvents = 0x00000010
-    kFSEventStreamEventFlagItemCreated = 0x00000100
-    kFSEventStreamEventFlagItemRemoved = 0x00000200
-    kFSEventStreamEventFlagItemRenamed = 0x00000800
-    kFSEventStreamEventFlagItemModified = 0x00001000
-
-    CF.CFStringCreateWithCString.restype = c_void_p
-    CF.CFStringCreateWithCString.argtypes = [c_void_p, ctypes.c_char_p, c_uint32]
-    CF.CFArrayCreate.restype = c_void_p
-    CF.CFArrayCreate.argtypes = [c_void_p, POINTER(c_void_p), c_size_t, c_void_p]
-    CS.FSEventStreamCreate.restype = c_void_p
-    CS.FSEventStreamCreate.argtypes = [c_void_p, c_void_p, c_void_p, c_void_p, c_uint64, ctypes.c_double, c_uint32]
-    CS.FSEventStreamScheduleWithRunLoop.argtypes = [c_void_p, c_void_p, c_void_p]
-    CS.FSEventStreamStart.argtypes = [c_void_p]
-    CF.CFRunLoopGetCurrent.restype = c_void_p
-
-    def _cb(stream, ctx, num, paths, flags, ids):
-      try:
-        paths_arr = ctypes.cast(paths, POINTER(c_void_p))
-        flags_arr = ctypes.cast(flags, POINTER(c_uint32))
-        for i in range(num):
-          path = ctypes.cast(paths_arr[i], ctypes.c_char_p).value
-          if path and (flags_arr[i] & (kFSEventStreamEventFlagItemCreated | kFSEventStreamEventFlagItemRemoved |
-                                       kFSEventStreamEventFlagItemRenamed | kFSEventStreamEventFlagItemModified)):
-            self._trigger_callbacks(os.path.basename(path.decode('utf-8').rstrip('/')))
-      except Exception:
-        traceback.print_exc()
-
-    self._darwin_cb = ctypes.CFUNCTYPE(None, c_void_p, c_void_p, c_size_t, c_void_p, POINTER(c_uint32), POINTER(c_uint64))(_cb)
-
-    path_str = Paths.params_root().encode('utf-8')
-    cf_path = CF.CFStringCreateWithCString(kCFAllocatorDefault, path_str, kCFStringEncodingUTF8)
-    cf_paths = CF.CFArrayCreate(kCFAllocatorDefault, (c_void_p * 1)(cf_path), 1, None)
-    stream = CS.FSEventStreamCreate(kCFAllocatorDefault, self._darwin_cb, None, cf_paths, -1, 0.05, kFSEventStreamCreateFlagFileEvents)
-
-    run_loop = CF.CFRunLoopGetCurrent()
-    kDefaultMode = CF.CFStringCreateWithCString(kCFAllocatorDefault, b"kCFRunLoopDefaultMode", kCFStringEncodingUTF8)
-    CS.FSEventStreamScheduleWithRunLoop(stream, run_loop, kDefaultMode)
-    CS.FSEventStreamStart(stream)
-    CF.CFRunLoopRun()

sunnypilot/common/params.py

@@ -1,4 +0,0 @@
-from openpilot.common.params_pyx import ParamKeyFlag, ParamKeyType, UnknownKeyName
-from openpilot.sunnypilot.common.param_watcher import ParamWatcher as Params
-
-__all__ = ["Params", "ParamKeyFlag", "ParamKeyType", "UnknownKeyName"]

sunnypilot/common/tests/test_param_watcher.py

@@ -1,94 +0,0 @@
-import time
-import pytest
-import threading
-import tracemalloc
-
-from openpilot.common.params import Params
-from openpilot.common.params_pyx import UnknownKeyName
-
-from openpilot.sunnypilot.common.param_watcher import ParamWatcher
-
-
-class TestParamWatcher:
-  BYTES_KEYS = ["LocationFilterInitialState", "UpdaterCurrentReleaseNotes", "UpdaterNewReleaseNotes"]
-  BOOL_KEYS = [
-    "IsMetric", "AdbEnabled", "AlwaysOnDM", "ExperimentalMode",
-    "ExperimentalModeConfirmed", "DisengageOnAccelerator",
-    "OpenpilotEnabledToggle", "RecordAudio", "RecordFront"
-  ]
-  _key_counter = 0
-
-  @pytest.fixture(autouse=True)
-  def setup_method(self):
-    self.params = Params()
-    self.key_index = TestParamWatcher._key_counter
-    TestParamWatcher._key_counter += 1
-    self.bytes_key = self.BYTES_KEYS[self.key_index % len(self.BYTES_KEYS)]
-    self.bool_key = self.BOOL_KEYS[self.key_index % len(self.BOOL_KEYS)]
-
-  @pytest.fixture
-  def param_watcher(self):
-    ParamWatcher._instance = None
-    param_watch = ParamWatcher()
-    param_watch.start()
-    assert param_watch.is_watching(), "ParamWatcher thread died"
-    return param_watch
-
-  def teardown_method(self):
-    for key in (self.bytes_key, self.bool_key):
-      try:
-        self.params.remove(key)
-      except UnknownKeyName:
-        pass
-
-  def test_watcher_detects_change(self, param_watcher):
-    val = b"123"
-    self.params.put(self.bytes_key, val)
-    assert param_watcher.get(self.bytes_key) == val
-
-  def test_watcher_get_bool(self, param_watcher):
-    self.params.put_bool(self.bool_key, True)
-    assert param_watcher.get_bool(self.bool_key) is True  # First read should populate internal cache
-
-  def test_performance_comparison(self, param_watcher):
-    plain_params = self.params
-
-    for key in self.BYTES_KEYS:
-      plain_params.put(key, b"x" * 10000)
-      param_watcher.get(key)
-    for key in self.BOOL_KEYS:
-      plain_params.put_bool(key, True)
-      param_watcher.get_bool(key)
-
-    def bench(get_bytes, get_bool):
-      tracemalloc.start()
-      start_time = time.process_time()
-      for _ in range(1000):
-        for key in self.BYTES_KEYS:
-          get_bytes(key)
-        for key in self.BOOL_KEYS:
-          get_bool(key)
-      duration = time.process_time() - start_time
-      _, memory = tracemalloc.get_traced_memory()
-      tracemalloc.stop()
-      return duration, memory
-
-    plain_cpu, plain_memory = bench(plain_params.get, plain_params.get_bool)
-    watcher_cpu, watcher_memory = bench(param_watcher.get, param_watcher.get_bool)
-
-    # ParamWatcher *should* be significantly faster and use less memory than Params()
-    assert watcher_cpu < plain_cpu * 0.6, f"PW CPU ({watcher_cpu:.4f}s) should be < 60% of Param call ({plain_cpu:.4f}s)"
-    assert watcher_memory < plain_memory * 0.5, f"PW Memory ({watcher_memory}B) should be < 50% of Param call ({plain_memory}B)"
-
-  def test_cache_invalidation_simulation(self, param_watcher):
-    self.params.put(self.bytes_key, b"old")
-    assert param_watcher.get(self.bytes_key) == b"old"
-    time.sleep(0.2)
-
-    event = threading.Event()
-    param_watcher.add_watcher(lambda key: event.set())
-    param_watcher._trigger_callbacks(self.bytes_key)
-    assert event.wait(timeout=2), "Callback not triggered"
-
-    self.params.put(self.bytes_key, b"new")
-    assert param_watcher.get(self.bytes_key) == b"new"

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
@@ -286,7 +290,6 @@ def main(demo=False):
     v_ego = max(sm["carState"].vEgo, 0.)
     if sm.frame % 60 == 0:
       model.lat_delay = get_lat_delay(params, sm["liveDelay"].lateralDelay)
-      camera_offset_helper.set_offset(params.get("CameraOffset", return_default=True))
       model.PLANPLUS_CONTROL = params.get("PlanplusControl", return_default=True)
       camera_offset_helper.set_offset(params.get("CameraOffset", return_default=True))
     lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS
@@ -349,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/nnlc/nnlc.py

@@ -8,7 +8,7 @@ from collections import deque
 import math
 import numpy as np
 
-from opendbc.car.lateral import get_friction, get_friction_threshold
+from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction
 from opendbc.sunnypilot.car.interfaces import LatControlInputs
 from opendbc.sunnypilot.car.lateral_ext import get_friction as get_friction_in_torque_space
 from openpilot.common.filter_simple import FirstOrderFilter
@@ -82,7 +82,7 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase):
     self._ff = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._gravity_adjusted_lateral_accel, self._roll_compensation,
                                                                                CS.vEgo, CS.aEgo), self.lac_torque.torque_params, gravity_adjusted=True)
     self._ff += get_friction_in_torque_space(self._desired_lateral_accel - self._actual_lateral_accel, self._lateral_accel_deadzone,
-                                             get_friction_threshold(CS.vEgo), self.lac_torque.torque_params)
+                                             FRICTION_THRESHOLD, self.lac_torque.torque_params)
 
   def update_output_torque(self, CS):
     freeze_integrator = self._steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
@@ -159,6 +159,6 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase):
 
     # apply friction override for cars with low NN friction response
     if self.model.friction_override:
-      self._pid_log.error += get_friction(friction_input, self._lateral_accel_deadzone, get_friction_threshold(CS.vEgo), self.lac_torque.torque_params)
+      self._pid_log.error += get_friction(friction_input, self._lateral_accel_deadzone, FRICTION_THRESHOLD, self.lac_torque.torque_params)
 
     self.update_output_torque(CS)

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,204 +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 = 0.8  # Predicted Lat Acc threshold to trigger entering turn state.
-_ABORT_ENTERING_PRED_LAT_ACC_TH = 0.6  # Predicted Lat Acc threshold to abort entering state if speed drops.
-
-_TURNING_LAT_ACC_TH = 1.4  # Lat Acc threshold to trigger turning state.
-
-_LEAVING_LAT_ACC_TH = 1.0  # Lat Acc threshold to trigger leaving turn state.
-_FINISH_LAT_ACC_TH = 0.6  # Lat Acc threshold to trigger the end of the turn cycle.
-
-_A_LAT_REG_MAX = 2.0  # 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.1, -0.4]  # min decel value allowed on ENTERING state
-_ENTERING_SMOOTH_DECEL_BP = [0.8, 2.5]  # 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.0, 2.25]  # absolute value of current lat acc
-
-_LEAVING_ACC_V = [0.5, 0.1]  # Conformable acceleration to regain speed while leaving a turn.
-_LEAVING_ACC_BP = [0.6, 1.2]
-
-
-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 = np.interp(self.current_lat_acc, _LEAVING_ACC_BP, _LEAVING_ACC_V)
-    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": ""

sunnypilot/tools/memory_profiler/mem_usage.py

@@ -1,160 +0,0 @@
-import matplotlib.pyplot as plt
-import os
-import sys
-import argparse
-import numpy as np
-import base64
-import io
-
-from openpilot.tools.lib.logreader import LogReader, ReadMode
-
-def extract_mem_cpu_data(lr):
-  times, mems, cpus = [], [], []
-  start_time = None
-
-  for msg in lr:
-    if msg.which() == 'procLog':
-      if start_time is None:
-        start_time = msg.logMonoTime
-      mem = msg.procLog.mem
-      mem_usage = (mem.total - mem.available) / mem.total * 100
-      cpu_usages = [(total - cpu.idle) / total * 100 for cpu in msg.procLog.cpuTimes
-                    if (total := cpu.idle + cpu.user + cpu.system + cpu.nice + cpu.iowait + cpu.irq + cpu.softirq) > 0]
-      avg_cpu = sum(cpu_usages) / len(cpu_usages) if cpu_usages else 0
-      times.append((msg.logMonoTime - start_time) / 1e9)
-      mems.append(mem_usage)
-      cpus.append(avg_cpu)
-  return times, mems, cpus
-
-def process_segment(lr):
-  return [extract_mem_cpu_data(lr)]
-
-def calculate_r_squared(y_true, y_pred):
-  ss_res = np.sum((y_true - y_pred) ** 2)
-  ss_tot = np.sum((y_true - np.mean(y_true)) ** 2)
-  return 1 - (ss_res / ss_tot) if ss_tot != 0 else 0
-
-def plot_results(segments, segment_data, route_name):
-  valid_data = [d for d in segment_data if d and d[0]]
-  if not valid_data:
-    print("No valid data to plot")
-    return
-
-  avg_mems = [np.mean(d[1]) for d in valid_data]
-  avg_cpus = [np.mean(d[2]) for d in valid_data]
-  valid_segments = [segments[i] for i, d in enumerate(segment_data) if d and d[0]]
-
-  height = max(10, 5 + len(valid_segments) * 0.4)
-  fig1, ax1 = plt.subplots(1, 1, figsize=(12, height), dpi=150)
-
-  y_pos = range(len(valid_segments))
-  ax1.barh([y - 0.2 for y in y_pos], avg_mems, height=0.4, color="dodgerblue", alpha=0.8, label="Avg Mem %")
-  ax1.barh([y + 0.2 for y in y_pos], avg_cpus, height=0.4, color="green", alpha=0.8, label="Avg CPU %")
-
-  for i, (mem, cpu) in enumerate(zip(avg_mems, avg_cpus, strict=True)):
-    ax1.text(mem, i - 0.2, f"{mem:.1f}%", va="center", fontsize=8, color="#005a9e", fontweight="bold")
-    ax1.text(cpu, i + 0.2, f"{cpu:.1f}%", va="center", fontsize=8, color="#005a9e", fontweight="bold")
-
-  ax1.set_yticks(y_pos)
-  ax1.set_yticklabels([f"Seg {s}" for s in valid_segments])
-  ax1.set_xlabel("Usage (%)")
-  ax1.set_title("Average Memory and CPU Usage by Segment")
-  ax1.legend()
-  ax1.grid(axis="x", linestyle="--", alpha=0.5)
-  ax1.invert_yaxis()
-
-  fig2, ax2 = plt.subplots(1, 1, figsize=(12, 8), dpi=150)
-  combined_times, combined_mems, combined_cpus = [], [], []
-  time_offset = 0.0
-  for times, mems, cpus in valid_data:
-    if times:
-      combined_times.extend([t + time_offset for t in times])
-      combined_mems.extend(mems)
-      combined_cpus.extend(cpus)
-      time_offset += max(times)
-
-  ax2.plot(combined_times, combined_mems, color="red", label="Memory Usage", alpha=0.6)
-  ax2.plot(combined_times, combined_cpus, color="blue", label="CPU Usage", alpha=0.6)
-
-  warmup_sec = 60
-  if len(combined_times) > 1 and combined_times[-1] > warmup_sec:
-      mask = np.array(combined_times) > warmup_sec
-      x_reg = np.array(combined_times)[mask]
-
-      y_mem_reg = np.array(combined_mems)[mask]
-      slope_mem, intercept_mem = np.polyfit(x_reg, y_mem_reg, 1)
-      trend_mem = slope_mem * x_reg + intercept_mem
-      r2_mem = calculate_r_squared(y_mem_reg, trend_mem)
-      ax2.plot(x_reg, trend_mem, color="darkred", linestyle="--", linewidth=2.5,
-               label=f"Mem Trend (Slope: {slope_mem:.4f} %/s, R²: {r2_mem:.2f})")
-
-      y_cpu_reg = np.array(combined_cpus)[mask]
-      slope_cpu, intercept_cpu = np.polyfit(x_reg, y_cpu_reg, 1)
-      trend_cpu = slope_cpu * x_reg + intercept_cpu
-      r2_cpu = calculate_r_squared(y_cpu_reg, trend_cpu)
-      ax2.plot(x_reg, trend_cpu, color="navy", linestyle="--", linewidth=2.5,
-               label=f"CPU Trend (Slope: {slope_cpu:.4f} %/s, R²: {r2_cpu:.2f})")
-
-  ax2.set_xlabel("Time (s)")
-  ax2.set_ylabel("Usage (%)")
-  ax2.set_title("Memory and CPU Usage Over Time")
-  ax2.legend(loc='lower left', fontsize='small', framealpha=0.9)
-  ax2.grid(True, linestyle="--", alpha=0.5)
-
-  buffer1 = io.BytesIO()
-  fig1.savefig(buffer1, format='webp', bbox_inches='tight', pad_inches=1.0)
-  buffer1.seek(0)
-  img1 = base64.b64encode(buffer1.getvalue()).decode()
-
-  buffer2 = io.BytesIO()
-  fig2.savefig(buffer2, format='webp', bbox_inches='tight', pad_inches=1.0)
-  buffer2.seek(0)
-  img2 = base64.b64encode(buffer2.getvalue()).decode()
-
-  filename = f"memory_usage_{route_name}.html"
-  save_path = os.path.join(os.path.dirname(__file__), "plots", filename)
-  os.makedirs(os.path.dirname(save_path), exist_ok=True)
-
-  html_template = (
-    "<style>body{font-family:Arial,sans-serif;margin:20px}" +
-    "h1,h2,h3{text-align:center;margin:5px 0}h2{margin-bottom:10px}" +
-    "img{width:100%;max-width:800px;height:auto;display:block;margin:0 auto}</style>" +
-    f"<h1>Memory Profile Report</h1><h3>Route: {route_name.replace('_', '/')}</h3>" +
-    f"<img src='data:image/webp;base64,{img1}'>" +
-    f"<img src='data:image/webp;base64,{img2}'>"
-  )
-
-  plt.close(fig1)
-  plt.close(fig2)
-
-  with open(save_path, "w") as f:
-    f.write(html_template)
-
-  print(f"Report saved to {save_path}")
-
-def main():
-  parser = argparse.ArgumentParser(description='Extract memory usage from route logs.')
-  parser.add_argument('route_or_segment_name', help='Route or segment name from comma connect')
-  args = parser.parse_args()
-
-  try:
-    print(f"Fetching logs for {args.route_or_segment_name}")
-    lr = LogReader(args.route_or_segment_name, default_mode=ReadMode.QLOG)
-    segment_data = lr.run_across_segments(24, process_segment)
-    segments = list(range(len(segment_data)))
-    route_name = args.route_or_segment_name.replace('/', '_')
-    plot_results(segments, segment_data, route_name)
-  except Exception as e:
-    print(f"Error: {e}")
-    sys.exit(1)
-
-if __name__ == "__main__":
-  main()
-  """
-  To run this script:
-
-  source .venv/bin/activate &&
-  python sunnypilot/tools/memory_profiler/mem_usage.py {route_or_segment_name}
-
-  replace {route_or_segment_name} with full comma connect route. e.g., e1c2f3718946cc1/00000015--5888108fd9/7
-  """

sunnypilot/tools/pull_footage.py

@@ -1,130 +0,0 @@
-#!/usr/bin/env python3
-import argparse
-import os
-import shutil
-import subprocess
-import sys
-import requests
-from openpilot.tools.lib.route import Route
-
-
-def get_segments(source, route_id, camera, seg_range):
-  if "@" in source or "comma-" or "sunny-" in source:  # SSH
-    if not route_id:
-      raise ValueError("route_id required for SSH")
-    cmd = ["ssh", source, f"ls -d /data/media/0/realdata/{route_id.split('--')[0]}--*"]
-    output = subprocess.check_output(cmd, stderr=subprocess.DEVNULL).decode("utf-8").strip()
-    return [{
-      "type": "ssh",
-      "host": source,
-      "src": os.path.join(path, camera),
-      "num": int(path.split("--")[-1])
-    } for path in sorted(output.split("\n"), key=lambda x: int(x.split("--")[-1])) if path]
-  else:  # URL
-    route = Route(route_id)
-    cameras = [camera]
-    if camera == "fcamera.hevc":
-      cameras.extend([c for c in ["ecamera.hevc", "qcamera.ts"] if c != camera])
-
-    for cam in cameras:
-      attr_name = "camera_paths" if cam == "fcamera.hevc" else f"{cam.split('.')[0]}_paths"
-      paths = getattr(route, attr_name)()
-      if any(paths):
-        return [{"type": "url", "src": url, "num": idx, "cam": cam} for idx, url in enumerate(paths) if url]
-
-    raise ValueError(f"No footage found for {route_id}")
-
-
-def download(job, out_dir):
-  destination = os.path.join(out_dir, f"{job['num']}_{os.path.basename(job.get('cam', job.get('src')))}")
-  if os.path.exists(destination) and os.path.getsize(destination) > 0:
-    return destination
-
-  print(f"Downloading segment {job['num']}")
-  if job["type"] == "ssh":
-    subprocess.check_call(["scp", f"{job['host']}:{job['src']}", destination])
-  else:
-    with requests.get(job["src"], stream=True) as r:
-      r.raise_for_status()
-      with open(destination, 'wb') as f:
-        shutil.copyfileobj(r.raw, f)
-  return destination
-
-
-def mux(files, output_file, codec):
-  list_filename = f"{output_file}.list.txt"
-  with open(list_filename, 'w') as f:
-    f.write('\n'.join([f"file '{os.path.abspath(name)}'" for name in files]))
-
-  try:
-    cmd = [
-      "ffmpeg", "-y", "-probesize", "100M", "-analyzeduration", "100M", "-f", "concat",
-      "-safe", "0", "-r", "20", "-i", list_filename, "-c", "copy", "-tag:v", codec, output_file
-    ]
-    subprocess.check_call(cmd)
-    print(f"Saved: {output_file} ({os.path.getsize(output_file) / 1048576:.2f} MB)")
-    if sys.platform == "darwin":
-      subprocess.run(["open", "-R", output_file])
-  finally:
-    if os.path.exists(list_filename):
-      os.remove(list_filename)
-
-
-def main():
-  parser = argparse.ArgumentParser()
-  parser.add_argument("source")
-  parser.add_argument("route_id", nargs='?')
-  parser.add_argument("--output", "-o", default="output.mp4")
-  parser.add_argument("--camera", "-c", default="fcamera.hevc")
-  parser.add_argument("--keep-segments", action="store_true")
-  args = parser.parse_args()
-
-  try:
-    route_id_str = args.route_id or args.source
-    segment_range = None
-    if "/" in route_id_str:
-      route_id_str, range_str = route_id_str.rsplit("/", 1)
-      if ":" in range_str or range_str.isdigit():
-        segment_range = range_str
-
-    is_ssh = "@" in args.source or "comma-" in args.source
-    if not is_ssh and len(route_id_str.split("--")) > 2:
-      route_id_str = "--".join(route_id_str.split("--")[:2])
-
-    segments = get_segments(args.source, route_id_str, args.camera, segment_range)
-    if segment_range:
-      if ":" in segment_range:
-        parts = segment_range.split(":")
-        start_idx = int(parts[0]) if parts[0] else None
-        end_idx = int(parts[1]) if parts[1] else None
-      else:
-        start_idx = int(segment_range)
-        end_idx = start_idx + 1
-
-      segments = [
-        segment for segment in segments
-        if (start_idx is None or segment['num'] >= start_idx) and (end_idx is None or segment['num'] < end_idx)
-      ]
-
-    download_dir = f"{route_id_str}_segments"
-    os.makedirs(download_dir, exist_ok=True)
-
-    downloaded_files = sorted(
-      [download(segment, download_dir) for segment in segments],
-      key=lambda x: int(os.path.basename(x).split("_")[0])
-    )
-
-    camera_name = segments[0].get('cam', args.camera)
-    codec = "hvc1" if camera_name.endswith("hevc") else "avc1"
-    mux(downloaded_files, f"{route_id_str}--{args.output}", codec)
-
-    if not args.keep_segments:
-      shutil.rmtree(download_dir)
-
-  except Exception as e:
-    print(f"Error: {e}")
-    sys.exit(1)
-
-
-if __name__ == "__main__":
-  main()

system/hardware/hw.py

@@ -95,10 +95,3 @@ class Paths:
       return str(Path(Paths.comma_home()) / "media" / "0" / "osm")
     else:
       return "/data/media/0/osm"
-
-  @staticmethod
-  def params_root() -> str:
-    if PC:
-      return str(Path(Paths.comma_home()) / "params" / "d")
-    else:
-      return "/data/params/d"

system/manager/manager.py

@@ -215,8 +215,7 @@ def main() -> None:
 
 
 if __name__ == "__main__":
-  if sys.platform != 'darwin':
-    unblock_stdout()
+  unblock_stdout()
 
   try:
     main()

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"):

system/ui/lib/scroll_panel2.py

@@ -10,7 +10,7 @@ from collections import deque
 
 MIN_VELOCITY = 10  # px/s, changes from auto scroll to steady state
 MIN_VELOCITY_FOR_CLICKING = 2 * 60  # px/s, accepts clicks while auto scrolling below this velocity
-MIN_DRAG_PIXELS = 5
+MIN_DRAG_PIXELS = 12
 AUTO_SCROLL_TC_SNAP = 0.025
 AUTO_SCROLL_TC = 0.18
 BOUNCE_RETURN_RATE = 10.0
@@ -221,4 +221,5 @@ class GuiScrollPanel2:
     return self._state
 
   def is_touch_valid(self) -> bool:
-    return bool(self._state in (ScrollState.STEADY, ScrollState.PRESSED))
+    # MIN_VELOCITY_FOR_CLICKING is checked in auto-scroll state
+    return bool(self._state != ScrollState.MANUAL_SCROLL)