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