diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 00e4cd919a..7f07c08a0f 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -228,7 +228,7 @@ class CarController(CarControllerBase): if hda2: can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame)) if self.frame % 2 == 0: - can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled and CS.out.cruiseState.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, + can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CS, CC.enabled and CS.out.cruiseState.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, set_speed_in_units, hud_control)) self.accel_last = accel else: diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index d8329c5ba7..fcd44cc1cd 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -62,6 +62,11 @@ class CarState(CarStateBase): self.escc_aeb_dec_cmd = 0 self._speed_limit_clu = 0 + def get_main_enabled(self, ret) -> bool: + if self.prev_main_buttons != 1 and self.main_buttons[-1] == 1: + self.mainEnabled = not self.mainEnabled + return ret.cruiseState.available and self.mainEnabled + def update(self, cp, cp_cam): if self.CP.carFingerprint in CANFD_CAR: return self.update_canfd(cp, cp_cam) @@ -112,7 +117,7 @@ class CarState(CarStateBase): # cruise state if self.CP.openpilotLongitudinalControl: # These are not used for engage/disengage since openpilot keeps track of state using the buttons - ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0 and self.mainEnabled + ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0 ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1 ret.cruiseState.standstill = False ret.cruiseState.nonAdaptive = False @@ -207,17 +212,12 @@ class CarState(CarStateBase): self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"]) self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"]) if self.CP.openpilotLongitudinalControl: - if self.prev_main_buttons != 1: - if self.main_buttons[-1] == 1: - self.mainEnabled = not self.mainEnabled - ret.cruiseState.available = ret.cruiseState.available and self.mainEnabled + ret.cruiseState.available = self.get_main_enabled(ret) self.prev_mads_enabled = self.mads_enabled self.prev_lfa_enabled = self.lfa_enabled if self.CP.spFlags & HyundaiFlagsSP.SP_CAN_LFA_BTN: self.lfa_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"] - self.mads_enabled = False if not self.control_initialized else ret.cruiseState.available - if self.CP.spFlags & HyundaiFlagsSP.SP_NAV_MSG or self.CP.spFlags & HyundaiFlagsSP.SP_LKAS12: self._update_traffic_signals(cp, cp_cam) ret.cruiseState.speedLimit = self._calculate_speed_limit() * speed_conv @@ -282,6 +282,7 @@ class CarState(CarStateBase): ret.cruiseState.standstill = False else: cp_cruise_info = cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp + ret.cruiseState.available = cp_cruise_info.vl["SCC_CONTROL"]["MainMode_ACC"] == 1 ret.cruiseState.enabled = cp_cruise_info.vl["SCC_CONTROL"]["ACCMode"] in (1, 2) ret.cruiseState.standstill = cp_cruise_info.vl["SCC_CONTROL"]["CRUISE_STANDSTILL"] == 1 ret.cruiseState.speed = cp_cruise_info.vl["SCC_CONTROL"]["VSetDis"] * speed_factor @@ -298,6 +299,8 @@ class CarState(CarStateBase): self.prev_main_buttons = self.main_buttons[-1] self.cruise_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"]) self.main_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"]) + if self.CP.openpilotLongitudinalControl: + ret.cruiseState.available = self.get_main_enabled(ret) self.prev_mads_enabled = self.mads_enabled self.prev_lfa_enabled = self.lfa_enabled self.lfa_enabled = cp.vl[self.cruise_btns_msg_canfd]["LFA_BTN"] diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index 8225166bfa..12b84ad0f6 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -121,7 +121,7 @@ def create_lfahda_cluster(packer, CAN, enabled, lat_active, lateral_paused, blin return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values) -def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control): +def create_acc_control(packer, CAN, CS, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control): jerk = 5 jn = jerk / 50 if not enabled or gas_override: @@ -132,7 +132,7 @@ def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_ov values = { "ACCMode": 0 if not enabled else (2 if gas_override else 1), - "MainMode_ACC": 1, + "MainMode_ACC": 1 if CS.mainEnabled else 0, "StopReq": 1 if stopping else 0, "aReqValue": a_val, "aReqRaw": a_raw, diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 02897f9765..c2140d2925 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -210,22 +210,25 @@ class CarInterface(CarInterfaceBase): *create_button_events(self.CS.main_buttons[-1], self.CS.prev_main_buttons, {1: ButtonType.altButton3}), ] + self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS) + self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled, self.CS.button_events, c.vCruise) - self.CS.mads_enabled = False if not self.mads_main_toggle else self.CS.mads_enabled - if ret.cruiseState.available: if not self.CP.pcmCruiseSpeed: if any(b.type in (ButtonType.altButton3, ButtonType.cancel) and not b.pressed for b in self.CS.button_events): self.CS.accEnabled = True - if self.enable_mads: - if not self.CS.prev_mads_enabled and self.CS.mads_enabled: - self.CS.madsEnabled = True - if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events): - self.CS.madsEnabled = not self.CS.madsEnabled - self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled) - else: + + if self.enable_mads: + if not self.CS.prev_mads_enabled and self.CS.mads_enabled and \ + any(b.type == ButtonType.altButton3 for b in self.CS.button_events): + self.CS.madsEnabled = True + if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events): + self.CS.madsEnabled = not self.CS.madsEnabled + self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled) + + if not ret.cruiseState.available and self.CS.out.cruiseState.available: self.CS.madsEnabled = False if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed: diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index ca4a508554..53cca37458 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -470,7 +470,7 @@ class CarInterfaceBase(ABC): events.add(EventName.spReverseGear) elif cs_out.vEgo >= 5: events.add(EventName.reverseGear) - if not cs_out.cruiseState.available: + if not cs_out.cruiseState.available and cs_out.cruiseState.enabled: events.add(EventName.wrongCarMode) if cs_out.espDisabled: events.add(EventName.espDisabled)