wip

common/params.cc

@@ -244,6 +244,9 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"Compass", PERSISTENT},
     {"ConditionalExperimental", PERSISTENT},
     {"CurrentRandomEvent", PERSISTENT},
+    {"CSLCAvailable", PERSISTENT},
+    {"CSLCEnabled", PERSISTENT},
+    {"CSLCSpeed", PERSISTENT},
     {"CurveSensitivity", PERSISTENT},
     {"CustomColors", PERSISTENT},
     {"CustomIcons", PERSISTENT},

notes

@@ -6,6 +6,7 @@ Goals:
 3 - chirp when speed limit changes
 4 - Use LFA button to set speed of cruise to desired speed - 0-36 speed limit +3, 36-59 + 5, >= 60 +7
 5 - On experimental mode engage, adjust cruise speed to experimental model speed
+  - Verify pause lateral under 30 is enabled
 Wishlist:
 - When car reports seeing lanes, turn lane keep over to car, resume when car can't see lanes or turn signal is activated
 - Get hyundai speed limit reader merged into frogpilot

panda/board/safety/safety_hyundai.h

@@ -273,7 +273,8 @@ static bool hyundai_tx_hook(CANPacket_t *to_send) {
 
     bool allowed_resume = (button == 1) && controls_allowed;
     bool allowed_cancel = (button == 4) && cruise_engaged_prev;
-    if (!(allowed_resume || allowed_cancel)) {
+    bool allowed_cslc = (button == 1 || button == 2) && controls_allowed;
+    if (!(allowed_resume || allowed_cancel || allowed_cslc)) {
       tx = false;
     }
   }

panda/board/safety/safety_hyundai_canfd.h

@@ -247,8 +247,9 @@ static bool hyundai_canfd_tx_hook(CANPacket_t *to_send) {
     int button = GET_BYTE(to_send, 2) & 0x7U;
     bool is_cancel = (button == HYUNDAI_BTN_CANCEL);
     bool is_resume = (button == HYUNDAI_BTN_RESUME);
+    bool is_cslc = (button == HYUNDAI_BTN_RESUME || button == HYUNDAI_BTN_SET);
 
-    bool allowed = (is_cancel && cruise_engaged_prev) || (is_resume && controls_allowed);
+    bool allowed = (is_cancel && cruise_engaged_prev) || (is_resume && controls_allowed) || (is_cslc && controls_allowed);
     if (!allowed) {
       tx = false;
     }

selfdrive/car/hyundai/carcontroller.py

@@ -6,11 +6,15 @@ from opendbc.can.packer import CANPacker
 from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance
 from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican
 from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
-from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR
+from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR, LEGACY_SAFETY_MODE_CAR
+from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
+from openpilot.common.params import Params
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
+params_memory = Params("/dev/shm/params")
+
 # EPS faults if you apply torque while the steering angle is above 90 degrees for more than 1 second
 # All slightly below EPS thresholds to avoid fault
 MAX_ANGLE = 85
@@ -60,6 +64,10 @@ class CarController:
     actuators = CC.actuators
     hud_control = CC.hudControl
 
+    hud_v_cruise = hud_control.setSpeed
+    if hud_v_cruise > 70:
+      hud_v_cruise = 0
+
     # steering torque
     new_steer = int(round(actuators.steer * self.params.STEER_MAX))
     apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
@@ -146,6 +154,25 @@ class CarController:
       if not self.CP.openpilotLongitudinalControl:
         can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True))
 
+        # CSLC
+        if frogpilot_variables.CSLC and frogpilot_variables.CSLCA and CC.enabled and not CS.out.gasPressed: #and CS.cruise_buttons == Buttons.NONE:
+          cslcSetSpeed = get_set_speed(self, hud_v_cruise)
+          self.cruise_button = get_cslc_button(self, cslcSetSpeed, CS)
+          if self.cruise_button != Buttons.NONE:
+            if self.CP.carFingerprint in LEGACY_SAFETY_MODE_CAR:
+              send_freq = 1
+              # send resume at a max freq of 10Hz
+              if (self.frame - self.last_button_frame) * DT_CTRL > 0.1 * send_freq:
+                # send 25 messages at a time to increases the likelihood of cruise buttons being accepted
+                can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
+                if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15 * send_freq:
+                  self.last_button_frame = self.frame
+            elif self.frame % 2 == 0:
+              if self.CP.carFingerprint in CANFD_CAR:
+                can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, ((self.frame // 2) + 1) % 0x10, self.cruise_button))
+              else:
+                can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
+
       if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
         # TODO: unclear if this is needed
         jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
@@ -179,12 +206,14 @@ class CarController:
 
     if CS.custom_speed_down:
       CS.custom_speed_down = False
-      if self.CP.openpilotLongitudinalControl:
-        CC.cruiseControl.resume = True
-        self.CP.openpilotLongitudinalControl = False
-      else:
-        CC.cruiseControl.cancel = True
-        self.CP.openpilotLongitudinalControl = True
+      # Test me.
+      can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, 1, Buttons.RES_ACCEL))
+      # if self.CP.openpilotLongitudinalControl:
+        # CC.cruiseControl.resume = True
+        # self.CP.openpilotLongitudinalControl = False
+      # else:
+        # CC.cruiseControl.cancel = True
+        # self.CP.openpilotLongitudinalControl = True
 
     if use_clu11:
       if CC.cruiseControl.cancel:
@@ -221,3 +250,26 @@ class CarController:
       # can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.SET_DECEL, self.CP.carFingerprint)] * 25)
 
     return can_sends
+  
+def get_set_speed(self, hud_v_cruise):
+  v_cruise_kph = min(hud_v_cruise * CV.MS_TO_KPH, V_CRUISE_MAX)
+  v_cruise = int(round(v_cruise_kph * CV.KPH_TO_MPH))
+
+  v_cruise_slc: int = 0
+  v_cruise_slc = params_memory.get_int("CSLCSpeed")
+
+  if v_cruise_slc > 0:
+    v_cruise = v_cruise_slc
+  return v_cruise
+
+def get_cslc_button(self, cslcSetSpeed, CS):
+  cruiseBtn = Buttons.NONE
+  speedSetPoint = int(round(CS.out.cruiseState.speed * CV.MS_TO_MPH))
+
+  if cslcSetSpeed < speedSetPoint and speedSetPoint > 25:
+    cruiseBtn = Buttons.SET_DECEL
+  elif cslcSetSpeed > speedSetPoint:
+    cruiseBtn = Buttons.RES_ACCEL
+  else:
+    cruiseBtn = Buttons.NONE
+  return cruiseBtn

selfdrive/car/hyundai/hyundaicanfd.py

@@ -116,7 +116,8 @@ def create_acc_cancel(packer, CP, CAN, cruise_info_copy):
 def create_lfahda_cluster(packer, CAN, enabled):
   values = {
     "HDA_ICON": 1 if enabled else 0,
-    "LFA_ICON": 2 if enabled else 0,
+    "LFA_ICON": 0 if enabled else 0,
+    # "LFA_ICON": 2 if enabled else 0,
   }
   return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)
 

selfdrive/car/hyundai/interface.py

@@ -9,6 +9,9 @@ from openpilot.selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
 from openpilot.selfdrive.car import create_button_events, get_safety_config
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase
 from openpilot.selfdrive.car.disable_ecu import disable_ecu
+from openpilot.common.params import Params
+
+params_memory = Params("/dev/shm/params")
 
 Ecu = car.CarParams.Ecu
 SafetyModel = car.CarParams.SafetyModel
@@ -35,6 +38,7 @@ class CarInterface(CarInterfaceBase):
   def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
     ret.carName = "hyundai"
     ret.radarUnavailable = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None
+    params_memory.put_bool("CSLCAvailable", True)
 
     # These cars have been put into dashcam only due to both a lack of users and test coverage.
     # These cars likely still work fine. Once a user confirms each car works and a test route is
@@ -50,6 +54,7 @@ class CarInterface(CarInterfaceBase):
       if hda2:
         if 0x110 in fingerprint[CAN.CAM]:
           ret.flags |= HyundaiFlags.CANFD_HDA2_ALT_STEERING.value
+          params_memory.put_bool("CSLCAvailable", False)
       else:
         # non-HDA2
         if 0x1cf not in fingerprint[CAN.ECAN]:
@@ -315,6 +320,7 @@ class CarInterface(CarInterfaceBase):
       ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC
     if ret.openpilotLongitudinalControl:
       ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_LONG
+      params_memory.put_bool("CSLCAvailable", False)
     if candidate in HYBRID_CAR:
       ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_HYBRID_GAS
     elif candidate in EV_CAR:

selfdrive/controls/controlsd.py

@@ -604,7 +604,7 @@ class Controls:
           else:
             self.state = State.enabled
           self.current_alert_types.append(ET.ENABLE)
-          self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.conditional_experimental_mode)
+          self.v_cruise_helper.initialize_v_cruise(CS, self.frogpilot_variables, self.experimental_mode, self.conditional_experimental_mode)
 
     # Check if openpilot is engaged and actuators are enabled
     self.enabled = self.state in ENABLED_STATES
@@ -1002,6 +1002,8 @@ class Controls:
 
     self.average_desired_curvature = self.params.get_bool("AverageCurvature")
     self.conditional_experimental_mode = self.params.get_bool("ConditionalExperimental")
+    self.frogpilot_variables.CSLC = self.params.get_bool("CSLCEnabled")
+    self.frogpilot_variables.CSLCA = self.params.get_bool("CSLCAvailable")
 
     custom_theme = self.params.get_bool("CustomTheme")
     custom_sounds = self.params.get_int("CustomSounds") if custom_theme else 0

selfdrive/controls/lib/drive_helpers.py

@@ -56,7 +56,7 @@ class VCruiseHelper:
     self.v_cruise_kph_last = self.v_cruise_kph
 
     if CS.cruiseState.available:
-      if not self.CP.pcmCruise:
+      if not self.CP.pcmCruise or frogpilot_variables.CSLC:
         # 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, reverse_cruise_increase, set_speed_offset)
         self.v_cruise_cluster_kph = self.v_cruise_kph
@@ -134,9 +134,14 @@ class VCruiseHelper:
         self.button_timers[b.type.raw] = 1 if b.pressed else 0
         self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled}
 
-  def initialize_v_cruise(self, CS, experimental_mode: bool, conditional_experimental_mode) -> None:
+  def initialize_v_cruise(self, CS, frogpilot_variables, experimental_mode: bool, conditional_experimental_mode) -> None:
     # initializing is handled by the PCM
-    if self.CP.pcmCruise:
+    if self.CP.pcmCruise and not frogpilot_variables.CSLC:
+      return
+    
+    if frogpilot_variables.CSLC:
+      self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
+      self.v_cruise_cluster_kph = self.v_cruise_kph
       return
 
     initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode and not conditional_experimental_mode else V_CRUISE_INITIAL

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,6 +171,9 @@ class FrogPilotPlanner:
     else:
       self.vtsc_target = v_cruise
 
+    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
 
@@ -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"},