wip

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -56,6 +56,7 @@ def calculate_lane_width(lane, current_lane, road_edge):
 
 class FrogPilotPlanner:
   def __init__(self, params, params_memory):
+    self.params_memory = params_memory
     self.cem = ConditionalExperimentalMode()
     self.mtsc = MapTurnSpeedController()
 
@@ -105,6 +106,8 @@ class FrogPilotPlanner:
     self.x_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, mpc.x_solution)
     self.x_desired_trajectory = self.x_desired_trajectory_full[:CONTROL_N]
 
+    self.params_memory.put_int("CSLCSpeed", int(round(self.v_cruise * CV.MS_TO_MPH)))
+
   def update_v_cruise(self, carState, controlsState, modelData, enabled, v_cruise, v_ego):
     # Pfeiferj's Map Turn Speed Controller
     if self.map_turn_speed_controller:
@@ -168,7 +171,10 @@ class FrogPilotPlanner:
     else:
       self.vtsc_target = v_cruise
 
-    v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0)
+    if self.CSLC:
+      v_ego_diff = 0
+    else:
+      v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0)
     return min(v_cruise, self.mtsc_target, self.slc_target, self.vtsc_target) - v_ego_diff
 
   def publish_lateral(self, sm, pm, DH):
@@ -206,6 +212,8 @@ class FrogPilotPlanner:
   def update_frogpilot_params(self, params, params_memory):
     self.is_metric = params.get_bool("IsMetric")
 
+    self.CSLC = params.get_bool("CSLCEnabled")
+
     self.blindspot_path = params.get_bool("CustomUI") and params.get_bool("BlindSpotPath")
 
     self.conditional_experimental_mode = params.get_bool("ConditionalExperimental")

selfdrive/frogpilot/ui/control_settings.cc

@@ -12,7 +12,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     {"CESlowerLead", "Slower Lead Detected Ahead", "Switch to 'Experimental Mode' when a slower lead vehicle is detected ahead.", ""},
     {"CEStopLights", "Stop Lights and Stop Signs", "Switch to 'Experimental Mode' when a stop light or stop sign is detected.", ""},
     {"CESignal", "Turn Signal When Below Highway Speeds", "Switch to 'Experimental Mode' when using turn signals below highway speeds to help assit with turns.", ""},
-
+    {"CSLCEnabled", "Custom Stock Longitudinal Control", "Use cruise control button spamming to adjust cruise set speed based on MTSC, VTSC, and SLC", "../frogpilot/assets/toggle_icons/icon_custom.png"},
     {"CustomPersonalities", "Custom Driving Personalities", "Customize the driving personality profiles to your driving style.", "../frogpilot/assets/toggle_icons/icon_custom.png"},
     {"DeviceShutdown", "Device Shutdown Timer", "Configure the timer for automatic device shutdown when offroad conserving energy and preventing battery drain.", "../frogpilot/assets/toggle_icons/icon_time.png"},
     {"ExperimentalModeActivation", "Experimental Mode Via", "Toggle Experimental Mode by double-clicking the 'Lane Departure'/'LKAS' button or double tapping screen.\n\nOverrides 'Conditional Experimental Mode'.", "../assets/img_experimental_white.svg"},