From 90b8e9f07212343d12f632fab7e040cacddaefd1 Mon Sep 17 00:00:00 2001 From: concordia Date: Thu, 8 Feb 2024 12:46:30 -0600 Subject: [PATCH] wip --- common/params.cc | 3 ++- selfdrive/car/hyundai/carcontroller.py | 34 +++++++++++++------------ selfdrive/car/hyundai/carstate.py | 13 +++++----- selfdrive/controls/controlsd.py | 7 ++--- selfdrive/controls/lib/drive_helpers.py | 12 ++++----- 5 files changed, 37 insertions(+), 32 deletions(-) diff --git a/common/params.cc b/common/params.cc index 82f7fb1..fe5951c 100644 --- a/common/params.cc +++ b/common/params.cc @@ -246,7 +246,8 @@ std::unordered_map keys = { {"CurrentRandomEvent", PERSISTENT}, {"CSLCAvailable", PERSISTENT}, {"CSLCEnabled", PERSISTENT}, - {"CSLCSpeed", PERSISTENT}, + {"CSLCSetSpeed", PERSISTENT}, + {"CSLCTempSpeed", PERSISTENT}, {"CurveSensitivity", PERSISTENT}, {"CustomColors", PERSISTENT}, {"CustomIcons", PERSISTENT}, diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 38e3a14..9d55cd5 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -64,9 +64,9 @@ class CarController: actuators = CC.actuators hud_control = CC.hudControl - hud_v_cruise = hud_control.setSpeed - if hud_v_cruise > 70: - hud_v_cruise = 0 + # hud_v_cruise = hud_control.setSpeed + # if hud_v_cruise > 70: + # hud_v_cruise = 0 # steering torque new_steer = int(round(actuators.steer * self.params.STEER_MAX)) @@ -155,19 +155,19 @@ class CarController: can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True)) # CSLC - if frogpilot_variables.CSLC and frogpilot_variables.CSLCA and CC.enabled and not CS.out.gasPressed: #and CS.cruise_buttons == Buttons.NONE: + if CC.enabled and CC.experimental_mode and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE: cslcSetSpeed = get_set_speed(self, hud_v_cruise) 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 - elif self.frame % 2 == 0: + # 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: 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: @@ -205,7 +205,7 @@ class CarController: can_sends = [] if CS.custom_speed_down: - CS.custom_speed_down = False + CS.oscar_lane_center_btn_pressed= False # Test me. can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, 1, Buttons.RES_ACCEL)) # if self.CP.openpilotLongitudinalControl: @@ -240,8 +240,10 @@ class CarController: # cruise standstill resume elif CC.cruiseControl.resume: if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS: - # TODO: resume for alt button cars - pass + # oscar - test me + for _ in range(20): + can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL)) + self.last_button_frame = self.frame else: for _ in range(20): can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL)) diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 33a972b..927490b 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -169,11 +169,12 @@ class CarState(CarStateBase): self.main_enabled = not self.main_enabled # BBot functions for lfa and gap buttons - test speed up / down - self.custom_speed_up = False - self.custom_speed_down = False + self.oscar_lane_center_btn_pressed= False + self.oscar_slc_decel = False + self.oscar_slc_accel = False # if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0: - # self.custom_speed_down = True + # self.oscar_lane_center_btn_pressed= True # lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"] # if lkas_pressed and not self.lkas_previously_pressed: @@ -301,15 +302,15 @@ class CarState(CarStateBase): SpeedLimitController.write_car_state() self.custom_speed_up = False - self.custom_speed_down = False + self.oscar_lane_center_btn_pressed = False if ret.cruiseState.available: if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0: - self.custom_speed_down = True + self.oscar_lane_center_btn_pressed = True # if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0: - # self.custom_speed_down = True + # self.oscar_lane_center_btn_pressed= True # lkas_pressed = cp.vl_all[self.cruise_btns_msg_canfd]["LKAS_BTN"] # if lkas_pressed and not self.lkas_previously_pressed: diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 985c986..8e46743 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -604,7 +604,8 @@ class Controls: else: self.state = State.enabled self.current_alert_types.append(ET.ENABLE) - self.v_cruise_helper.initialize_v_cruise(CS, self.frogpilot_variables, self.experimental_mode, self.conditional_experimental_mode) + self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.conditional_experimental_mode) + # self.v_cruise_helper.initialize_v_cruise(CS, self.frogpilot_variables, self.experimental_mode, self.conditional_experimental_mode) # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES @@ -1002,8 +1003,8 @@ class Controls: self.average_desired_curvature = self.params.get_bool("AverageCurvature") self.conditional_experimental_mode = self.params.get_bool("ConditionalExperimental") - self.frogpilot_variables.CSLC = self.params.get_bool("CSLCEnabled") - self.frogpilot_variables.CSLCA = self.params.get_bool("CSLCAvailable") + # self.frogpilot_variables.CSLC = self.params.get_bool("CSLCEnabled") + # self.frogpilot_variables.CSLCA = self.params.get_bool("CSLCAvailable") custom_theme = self.params.get_bool("CustomTheme") custom_sounds = self.params.get_int("CustomSounds") if custom_theme else 0 diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 99ca671..0ad981e 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -134,15 +134,15 @@ class VCruiseHelper: self.button_timers[b.type.raw] = 1 if b.pressed else 0 self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} - def initialize_v_cruise(self, CS, frogpilot_variables, experimental_mode: bool, conditional_experimental_mode) -> None: + def initialize_v_cruise(self, CS, experimental_mode: bool, conditional_experimental_mode) -> None: # initializing is handled by the PCM - if self.CP.pcmCruise and not frogpilot_variables.CSLC: + if self.CP.pcmCruise: return - if frogpilot_variables.CSLC: - self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH - self.v_cruise_cluster_kph = self.v_cruise_kph - return + # if frogpilot_variables.CSLC: + # self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH + # self.v_cruise_cluster_kph = self.v_cruise_kph + # return initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode and not conditional_experimental_mode else V_CRUISE_INITIAL