wip

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: