From 829b75411ba0b4c584304e5e585d7b5055d9c96f Mon Sep 17 00:00:00 2001 From: concordia Date: Sat, 10 Feb 2024 14:46:06 -0600 Subject: [PATCH] wip --- selfdrive/car/hyundai/carcontroller.py | 38 +++++++++++++++++++++----- selfdrive/car/hyundai/hyundaicanfd.py | 5 ++-- 2 files changed, 34 insertions(+), 9 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index c60c64f..d9cc6ff 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -146,7 +146,7 @@ class CarController: self.accel_last = accel else: # button presses - can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False)) + can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False, set_speed_in_units = None)) else: can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, apply_steer_req, torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled, @@ -209,7 +209,7 @@ class CarController: self.frame += 1 return new_actuators, can_sends - def create_button_messages(self, CC: car.CarControl, CS: car.CarState, use_clu11: bool): + def create_button_messages(self, CC: car.CarControl, CS: car.CarState, use_clu11: bool, set_speed_in_units = None): can_sends = [] # Test me. @@ -267,7 +267,30 @@ class CarController: for _ in range(20): can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL)) self.last_button_frame = self.frame - + + elif set_speed_in_units is not None and not self.CP.openpilotLongitudinalControl and CS.cruiseState.available and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE: + # if False and not self.CP.openpilotLongitudinalControl and CC.enabled and CC.experimental_mode and CS.cruiseState.available and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE: + # # cslcSetSpeed = get_set_speed(self, hud_v_cruise) + cslcSetSpeed = set_speed_in_units + 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 + if self.frame % 2 == 0: + for _ in range(20): + 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) + self.last_button_frame = self.frame + + # can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.SET_DECEL, self.CP.carFingerprint)] * 25) return can_sends @@ -283,14 +306,15 @@ def get_set_speed(self, hud_v_cruise): v_cruise = v_cruise_slc return v_cruise -def get_cslc_button(self, cslcSetSpeed, CS): +def get_cslc_button(self, cslcSetSpeed, CS, CC): cruiseBtn = Buttons.NONE speedSetPoint = int(round(CS.out.cruiseState.speed * CV.MS_TO_MPH)) - if cslcSetSpeed < speedSetPoint and speedSetPoint > 25: + if cslcSetSpeed < speedSetPoint and speedSetPoint > 25 and CC.enabled and CC.experimental_mode: cruiseBtn = Buttons.SET_DECEL - elif cslcSetSpeed > speedSetPoint and speedSetPoint < 75: + elif cslcSetSpeed > speedSetPoint and speedSetPoint < 85 and CC.enabled: cruiseBtn = Buttons.RES_ACCEL else: - cruiseBtn = Buttons.NONE + cruiseBtn = Buttons.SET_DECEL + # cruiseBtn = Buttons.NONE return cruiseBtn diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index bbfc030..d660a2d 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -41,7 +41,7 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer): values = { "LKA_MODE": 2, - "LKA_ICON": 2 if enabled else 1, + "LKA_ICON": 2 if enabled else 1 if lat_active else 0, "TORQUE_REQUEST": apply_steer, "LKA_ASSIST": 0, "STEER_REQ": 1 if lat_active else 0, @@ -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": 3 if enabled else 0, # was green before i think in 2? 3 should be flashing? + # 0 off, 1 gray, 2 green, 3 blinking (wheel icon) + "LFA_ICON": 0 if enabled else 0, # was green before i think in 2? 3 should be flashing? # "LFA_ICON": 2 if enabled else 0, } return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)