diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index b00fec8..241b157 100755 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/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") diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index dc37bc1..9d878c9 100755 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/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 = [] diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 1cf6863..46bc44b 100755 --- a/selfdrive/controls/controlsd.py +++ b/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 self.frogpilot_variables.conditional_experimental_mode: - # 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 - # self.params_memory.put_int("CEStatus", override_value) - # else: - # self.params.put_bool_nonblocking("ExperimentalMode", not self.experimental_mode) + 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: + 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 + self.params_memory.put_int("CEStatus", override_value) + else: + 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