wip

selfdrive/car/hyundai/carcontroller.py

@@ -134,12 +134,16 @@ class CarController(CarControllerBase):
       # params_memory = Params("/dev/shm/params")
       params_memory = Params("/dev/shm/params")
       if params_memory.get_bool("CPTLkasButtonAction"):
+        if self.frame % 2 == 0:
+          can_sends.append(hyundaicanfd.create_acc_control_alt(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
+                                                           set_speed_in_units, hud_control))
+          self.accel_last = accel
         # if self.frame % 10 == 0:
           # for _ in range(20):
             # can_sends.append(hyundaicanfd.create_buttons_alt(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.SET_DECEL, CS.cruise_can_msg))
-        print("Decel")
+        # print("Decel")
         # can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
-        can_sends.append(hyundaicanfd.create_acc_set_speed(self.packer, self.CP, self.CAN, CS.cruise_info, 50))
+        # can_sends.append(hyundaicanfd.create_acc_set_speed(self.packer, self.CP, self.CAN, CS.cruise_info, 50))
         # can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, self.CAN, CS.cruise_info))
         # print("Debug cancel executed")
 

selfdrive/car/hyundai/hyundaicanfd.py

@@ -275,6 +275,37 @@ def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_ov
 
   return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
 
+
+def create_acc_control_alt(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control):
+  jerk = 5
+  jn = jerk / 50
+  if not enabled or gas_override:
+    a_val, a_raw = 0, 0
+  else:
+    a_raw = accel
+    a_val = clip(accel, accel_last - jn, accel_last + jn)
+
+  values = {
+    "ACCMode": 0 if not enabled else (2 if gas_override else 1),
+    "MainMode_ACC": 1,
+    "StopReq": 1 if stopping else 0,
+    "aReqValue": a_val,
+    "aReqRaw": a_raw,
+    "VSetDis": set_speed,
+    "JerkLowerLimit": jerk if enabled else 1,
+    "JerkUpperLimit": 3.0,
+
+    "ACC_ObjDist": 1,
+    "ObjValid": 0,
+    "OBJ_STATUS": 2,
+    "SET_ME_2": 0x4,
+    "SET_ME_3": 0x3,
+    "SET_ME_TMP_64": 0x64,
+    "DISTANCE_SETTING": hud_control.leadDistanceBars,
+  }
+
+  return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
+
 # Disabled blinker messages
 def create_spas_messages(packer, CAN, frame, left_blink, right_blink):
   ret = []

selfdrive/controls/controlsd.py

@@ -1133,13 +1133,13 @@ class Controls:
         self.drive_added = True
 
     # Clearpilot - todo: override conditional on cruise button tap
-    # if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents) and self.experimental_mode_via_lkas:
+    if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents) and self.experimental_mode_via_lkas:
-      # if self.frogpilot_variables.conditional_experimental_mode:
+      if self.frogpilot_variables.conditional_experimental_mode:
-      #   conditional_status = self.params_memory.get_int("CEStatus")
+        conditional_status = self.params_memory.get_int("CEStatus")
-      #   override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 3 if conditional_status >= 7 else 4
+        override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 3 if conditional_status >= 7 else 4
-      #   self.params_memory.put_int("CEStatus", override_value)
+        self.params_memory.put_int("CEStatus", override_value)
-      # else:
+      else:
-      #   self.params.put_bool_nonblocking("ExperimentalMode", not self.experimental_mode)
+        self.params.put_bool_nonblocking("ExperimentalMode", not self.experimental_mode)
 
     self.previously_enabled |= (self.enabled or self.FPCC.alwaysOnLateral) and CS.vEgo > CRUISING_SPEED
     self.previously_enabled &= self.driving_gear