diff --git a/opendbc b/opendbc index 526e21da66..b3dc569994 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 526e21da666aeeabcf2369c66903a5675fdf933b +Subproject commit b3dc569994fd10e4de04afd650980c51ddfce5e1 diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 913f683e2c..2f944edc0f 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -49,6 +49,7 @@ class CarController: self.angle_limit_counter = 0 self.frame = 0 + self.accel_last = 0 self.apply_steer_last = 0 self.car_fingerprint = CP.carFingerprint self.last_button_frame = 0 @@ -123,8 +124,9 @@ class CarController: if self.CP.openpilotLongitudinalControl: can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.frame)) if self.frame % 2 == 0: - can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CP, CC.enabled, accel, stopping, CC.cruiseControl.override, + can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CP, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, set_speed_in_units)) + self.accel_last = accel else: # button presses if (self.frame - self.last_button_frame) * DT_CTRL > 0.25: diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 2e4cb0d25a..3cce581868 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -196,10 +196,10 @@ class CarState(CarStateBase): if not self.CP.openpilotLongitudinalControl: speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS cp_cruise_info = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam - ret.cruiseState.speed = cp_cruise_info.vl["CRUISE_INFO"]["SET_SPEED"] * speed_factor - ret.cruiseState.standstill = cp_cruise_info.vl["CRUISE_INFO"]["CRUISE_STANDSTILL"] == 1 - ret.cruiseState.enabled = cp_cruise_info.vl["CRUISE_INFO"]["CRUISE_STATUS"] != 0 - self.cruise_info = copy.copy(cp_cruise_info.vl["CRUISE_INFO"]) + ret.cruiseState.speed = cp_cruise_info.vl["SCC_CONTROL"]["VSetDis"] * speed_factor + ret.cruiseState.standstill = cp_cruise_info.vl["SCC_CONTROL"]["CRUISE_STANDSTILL"] == 1 + ret.cruiseState.enabled = cp_cruise_info.vl["SCC_CONTROL"]["ACCMode"] in (1, 2) + self.cruise_info = copy.copy(cp_cruise_info.vl["SCC_CONTROL"]) cruise_btn_msg = "CRUISE_BUTTONS_ALT" if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS" self.prev_cruise_buttons = self.cruise_buttons[-1] @@ -464,12 +464,12 @@ class CarState(CarStateBase): if CP.flags & HyundaiFlags.CANFD_HDA2 and not CP.openpilotLongitudinalControl: signals += [ - ("CRUISE_STATUS", "CRUISE_INFO"), - ("SET_SPEED", "CRUISE_INFO"), - ("CRUISE_STANDSTILL", "CRUISE_INFO"), + ("ACCMode", "SCC_CONTROL"), + ("VSetDis", "SCC_CONTROL"), + ("CRUISE_STANDSTILL", "SCC_CONTROL"), ] checks += [ - ("CRUISE_INFO", 50), + ("SCC_CONTROL", 50), ] if CP.carFingerprint in EV_CAR: @@ -497,20 +497,20 @@ class CarState(CarStateBase): checks = [("CAM_0x2a4", 20)] else: signals = [ - ("COUNTER", "CRUISE_INFO"), - ("NEW_SIGNAL_1", "CRUISE_INFO"), - ("CRUISE_MAIN", "CRUISE_INFO"), - ("CRUISE_STATUS", "CRUISE_INFO"), - ("CRUISE_INACTIVE", "CRUISE_INFO"), - ("ZEROS_9", "CRUISE_INFO"), - ("CRUISE_STANDSTILL", "CRUISE_INFO"), - ("ZEROS_5", "CRUISE_INFO"), - ("DISTANCE_SETTING", "CRUISE_INFO"), - ("SET_SPEED", "CRUISE_INFO"), + ("COUNTER", "SCC_CONTROL"), + ("NEW_SIGNAL_1", "SCC_CONTROL"), + ("MainMode_ACC", "SCC_CONTROL"), + ("ACCMode", "SCC_CONTROL"), + ("CRUISE_INACTIVE", "SCC_CONTROL"), + ("ZEROS_9", "SCC_CONTROL"), + ("CRUISE_STANDSTILL", "SCC_CONTROL"), + ("ZEROS_5", "SCC_CONTROL"), + ("DISTANCE_SETTING", "SCC_CONTROL"), + ("VSetDis", "SCC_CONTROL"), ] checks = [ - ("CRUISE_INFO", 50), + ("SCC_CONTROL", 50), ] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 6) diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index e1478e6f18..8b53e7c378 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -1,3 +1,4 @@ +from common.numpy_fast import clip from selfdrive.car.hyundai.values import HyundaiFlags @@ -52,10 +53,9 @@ def create_buttons(packer, CP, cnt, btn): def create_acc_cancel(packer, CP, cruise_info_copy): values = cruise_info_copy values.update({ - "CRUISE_STATUS": 0, - "CRUISE_INACTIVE": 1, + "ACCMode": 4, }) - return packer.make_can_msg("CRUISE_INFO", get_e_can_bus(CP), values) + return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values) def create_lfahda_cluster(packer, CP, enabled): values = { @@ -65,33 +65,37 @@ def create_lfahda_cluster(packer, CP, enabled): return packer.make_can_msg("LFAHDA_CLUSTER", get_e_can_bus(CP), values) -def create_acc_control(packer, CP, enabled, accel, stopping, gas_override, set_speed): - cruise_status = 0 if not enabled else (4 if gas_override else 2) +def create_acc_control(packer, CP, enabled, accel_last, accel, stopping, gas_override, set_speed): + jerk = 5 + jn = jerk / 50 if not enabled or gas_override: - accel = 0 + a_val, a_raw = 0, 0 + else: + a_raw = accel + a_val = clip(accel, accel_last - jn, accel_last + jn) + if stopping: + a_raw = 0 + values = { - "CRUISE_STATUS": cruise_status, - "CRUISE_INACTIVE": 0 if enabled else 1, - "CRUISE_MAIN": 1, - "CRUISE_STANDSTILL": 0, - "STOP_REQ": 1 if stopping else 0, - "ACCEL_REQ": accel, - "ACCEL_REQ2": accel, - "SET_SPEED": set_speed, - "DISTANCE_SETTING": 4, + "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, "ACC_ObjDist": 1, - "ObjValid": 1, + "ObjValid": 0, "OBJ_STATUS": 2, - "SET_ME_2": 0x2, + "SET_ME_2": 0x4, "SET_ME_3": 0x3, "SET_ME_TMP_64": 0x64, - - "NEW_SIGNAL_9": 2, "NEW_SIGNAL_10": 4, + "DISTANCE_SETTING": 4, } - return packer.make_can_msg("CRUISE_INFO", get_e_can_bus(CP), values) + return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 6a1d741dec..4b4d51f3f1 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -202,14 +202,10 @@ class CarInterface(CarInterfaceBase): if candidate in CANFD_CAR: ret.longitudinalTuning.kpV = [0.1] ret.longitudinalTuning.kiV = [0.0] - ret.longitudinalActuatorDelayLowerBound = 0.15 - ret.longitudinalActuatorDelayUpperBound = 0.5 ret.experimentalLongitudinalAvailable = bool(ret.flags & HyundaiFlags.CANFD_HDA2) else: ret.longitudinalTuning.kpV = [0.5] ret.longitudinalTuning.kiV = [0.0] - ret.longitudinalActuatorDelayLowerBound = 0.5 - ret.longitudinalActuatorDelayUpperBound = 0.5 ret.experimentalLongitudinalAvailable = candidate not in (LEGACY_SAFETY_MODE_CAR | CAMERA_SCC_CAR) ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable ret.pcmCruise = not ret.openpilotLongitudinalControl @@ -218,6 +214,8 @@ class CarInterface(CarInterfaceBase): ret.startingState = True ret.vEgoStarting = 0.1 ret.startAccel = 2.0 + ret.longitudinalActuatorDelayLowerBound = 0.5 + ret.longitudinalActuatorDelayUpperBound = 0.5 # *** feature detection *** if candidate in CANFD_CAR: diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index c7d81c3496..0e4ac94784 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -e56c5a6ac5b87ee6083c9f92921e7198591f7b5d +fc3a044c567a8702ed1500d745170c365dd6b3d4 \ No newline at end of file