diff --git a/common/params.cc b/common/params.cc index f2b3b64..82f7fb1 100644 --- a/common/params.cc +++ b/common/params.cc @@ -244,6 +244,9 @@ std::unordered_map keys = { {"Compass", PERSISTENT}, {"ConditionalExperimental", PERSISTENT}, {"CurrentRandomEvent", PERSISTENT}, + {"CSLCAvailable", PERSISTENT}, + {"CSLCEnabled", PERSISTENT}, + {"CSLCSpeed", PERSISTENT}, {"CurveSensitivity", PERSISTENT}, {"CustomColors", PERSISTENT}, {"CustomIcons", PERSISTENT}, diff --git a/notes b/notes index 730711a..4e6faaf 100644 --- a/notes +++ b/notes @@ -6,6 +6,7 @@ Goals: 3 - chirp when speed limit changes 4 - Use LFA button to set speed of cruise to desired speed - 0-36 speed limit +3, 36-59 + 5, >= 60 +7 5 - On experimental mode engage, adjust cruise speed to experimental model speed + - Verify pause lateral under 30 is enabled Wishlist: - When car reports seeing lanes, turn lane keep over to car, resume when car can't see lanes or turn signal is activated - Get hyundai speed limit reader merged into frogpilot diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h index cc2dc41..5d3b29e 100644 --- a/panda/board/safety/safety_hyundai.h +++ b/panda/board/safety/safety_hyundai.h @@ -273,7 +273,8 @@ static bool hyundai_tx_hook(CANPacket_t *to_send) { bool allowed_resume = (button == 1) && controls_allowed; bool allowed_cancel = (button == 4) && cruise_engaged_prev; - if (!(allowed_resume || allowed_cancel)) { + bool allowed_cslc = (button == 1 || button == 2) && controls_allowed; + if (!(allowed_resume || allowed_cancel || allowed_cslc)) { tx = false; } } diff --git a/panda/board/safety/safety_hyundai_canfd.h b/panda/board/safety/safety_hyundai_canfd.h index daa72d2..e246c88 100644 --- a/panda/board/safety/safety_hyundai_canfd.h +++ b/panda/board/safety/safety_hyundai_canfd.h @@ -247,8 +247,9 @@ static bool hyundai_canfd_tx_hook(CANPacket_t *to_send) { int button = GET_BYTE(to_send, 2) & 0x7U; bool is_cancel = (button == HYUNDAI_BTN_CANCEL); bool is_resume = (button == HYUNDAI_BTN_RESUME); + bool is_cslc = (button == HYUNDAI_BTN_RESUME || button == HYUNDAI_BTN_SET); - bool allowed = (is_cancel && cruise_engaged_prev) || (is_resume && controls_allowed); + bool allowed = (is_cancel && cruise_engaged_prev) || (is_resume && controls_allowed) || (is_cslc && controls_allowed); if (!allowed) { tx = false; } diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 9aa1eb7..38e3a14 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -6,11 +6,15 @@ from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus -from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR +from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR, LEGACY_SAFETY_MODE_CAR +from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX +from openpilot.common.params import Params VisualAlert = car.CarControl.HUDControl.VisualAlert LongCtrlState = car.CarControl.Actuators.LongControlState +params_memory = Params("/dev/shm/params") + # EPS faults if you apply torque while the steering angle is above 90 degrees for more than 1 second # All slightly below EPS thresholds to avoid fault MAX_ANGLE = 85 @@ -60,6 +64,10 @@ class CarController: actuators = CC.actuators hud_control = CC.hudControl + 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)) apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) @@ -146,6 +154,25 @@ class CarController: if not self.CP.openpilotLongitudinalControl: 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: + 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 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) + if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl: # TODO: unclear if this is needed jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0 @@ -179,12 +206,14 @@ class CarController: if CS.custom_speed_down: CS.custom_speed_down = False - if self.CP.openpilotLongitudinalControl: - CC.cruiseControl.resume = True - self.CP.openpilotLongitudinalControl = False - else: - CC.cruiseControl.cancel = True - self.CP.openpilotLongitudinalControl = True + # Test me. + can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, 1, Buttons.RES_ACCEL)) + # if self.CP.openpilotLongitudinalControl: + # CC.cruiseControl.resume = True + # self.CP.openpilotLongitudinalControl = False + # else: + # CC.cruiseControl.cancel = True + # self.CP.openpilotLongitudinalControl = True if use_clu11: if CC.cruiseControl.cancel: @@ -221,3 +250,26 @@ class CarController: # can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.SET_DECEL, self.CP.carFingerprint)] * 25) return can_sends + +def get_set_speed(self, hud_v_cruise): + v_cruise_kph = min(hud_v_cruise * CV.MS_TO_KPH, V_CRUISE_MAX) + v_cruise = int(round(v_cruise_kph * CV.KPH_TO_MPH)) + + v_cruise_slc: int = 0 + v_cruise_slc = params_memory.get_int("CSLCSpeed") + + if v_cruise_slc > 0: + v_cruise = v_cruise_slc + return v_cruise + +def get_cslc_button(self, cslcSetSpeed, CS): + cruiseBtn = Buttons.NONE + speedSetPoint = int(round(CS.out.cruiseState.speed * CV.MS_TO_MPH)) + + if cslcSetSpeed < speedSetPoint and speedSetPoint > 25: + cruiseBtn = Buttons.SET_DECEL + elif cslcSetSpeed > speedSetPoint: + cruiseBtn = Buttons.RES_ACCEL + else: + cruiseBtn = Buttons.NONE + return cruiseBtn diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index a368c87..639870a 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -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": 2 if enabled else 0, + "LFA_ICON": 0 if enabled else 0, + # "LFA_ICON": 2 if enabled else 0, } return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 6aa2cc2..0a85483 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -9,6 +9,9 @@ from openpilot.selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.car.disable_ecu import disable_ecu +from openpilot.common.params import Params + +params_memory = Params("/dev/shm/params") Ecu = car.CarParams.Ecu SafetyModel = car.CarParams.SafetyModel @@ -35,6 +38,7 @@ class CarInterface(CarInterfaceBase): def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "hyundai" ret.radarUnavailable = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None + params_memory.put_bool("CSLCAvailable", True) # These cars have been put into dashcam only due to both a lack of users and test coverage. # These cars likely still work fine. Once a user confirms each car works and a test route is @@ -50,6 +54,7 @@ class CarInterface(CarInterfaceBase): if hda2: if 0x110 in fingerprint[CAN.CAM]: ret.flags |= HyundaiFlags.CANFD_HDA2_ALT_STEERING.value + params_memory.put_bool("CSLCAvailable", False) else: # non-HDA2 if 0x1cf not in fingerprint[CAN.ECAN]: @@ -315,6 +320,7 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC if ret.openpilotLongitudinalControl: ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_LONG + params_memory.put_bool("CSLCAvailable", False) if candidate in HYBRID_CAR: ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_HYBRID_GAS elif candidate in EV_CAR: diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index c9eac9b..985c986 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -604,7 +604,7 @@ class Controls: else: self.state = State.enabled self.current_alert_types.append(ET.ENABLE) - 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,6 +1002,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") 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 d6186be..99ca671 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -56,7 +56,7 @@ class VCruiseHelper: self.v_cruise_kph_last = self.v_cruise_kph if CS.cruiseState.available: - if not self.CP.pcmCruise: + if not self.CP.pcmCruise or frogpilot_variables.CSLC: # if stock cruise is completely disabled, then we can use our own set speed logic self._update_v_cruise_non_pcm(CS, enabled, is_metric, reverse_cruise_increase, set_speed_offset) self.v_cruise_cluster_kph = self.v_cruise_kph @@ -134,9 +134,14 @@ 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, experimental_mode: bool, conditional_experimental_mode) -> None: + def initialize_v_cruise(self, CS, frogpilot_variables, experimental_mode: bool, conditional_experimental_mode) -> None: # initializing is handled by the PCM - if self.CP.pcmCruise: + if self.CP.pcmCruise and not frogpilot_variables.CSLC: + 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 diff --git a/selfdrive/frogpilot/assets/custom_themes/frog_theme/images/button_settings.png b/selfdrive/frogpilot/assets/custom_themes/frog_theme/images/button_settings.png index 5592759..549e2be 100644 Binary files a/selfdrive/frogpilot/assets/custom_themes/frog_theme/images/button_settings.png and b/selfdrive/frogpilot/assets/custom_themes/frog_theme/images/button_settings.png differ diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 67654ff..be00581 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -56,6 +56,7 @@ def calculate_lane_width(lane, current_lane, road_edge): class FrogPilotPlanner: def __init__(self, params, params_memory): + self.params_memory = params_memory self.cem = ConditionalExperimentalMode() self.mtsc = MapTurnSpeedController() @@ -105,6 +106,8 @@ class FrogPilotPlanner: self.x_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, mpc.x_solution) self.x_desired_trajectory = self.x_desired_trajectory_full[:CONTROL_N] + self.params_memory.put_int("CSLCSpeed", int(round(self.v_cruise * CV.MS_TO_MPH))) + def update_v_cruise(self, carState, controlsState, modelData, enabled, v_cruise, v_ego): # Pfeiferj's Map Turn Speed Controller if self.map_turn_speed_controller: @@ -168,7 +171,10 @@ class FrogPilotPlanner: else: self.vtsc_target = v_cruise - v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0) + if self.CSLC: + v_ego_diff = 0 + else: + v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0) return min(v_cruise, self.mtsc_target, self.slc_target, self.vtsc_target) - v_ego_diff def publish_lateral(self, sm, pm, DH): @@ -206,6 +212,8 @@ class FrogPilotPlanner: def update_frogpilot_params(self, params, params_memory): self.is_metric = params.get_bool("IsMetric") + self.CSLC = params.get_bool("CSLCEnabled") + self.blindspot_path = params.get_bool("CustomUI") and params.get_bool("BlindSpotPath") self.conditional_experimental_mode = params.get_bool("ConditionalExperimental") diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index fbb93a1..686c040 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -12,7 +12,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"CESlowerLead", "Slower Lead Detected Ahead", "Switch to 'Experimental Mode' when a slower lead vehicle is detected ahead.", ""}, {"CEStopLights", "Stop Lights and Stop Signs", "Switch to 'Experimental Mode' when a stop light or stop sign is detected.", ""}, {"CESignal", "Turn Signal When Below Highway Speeds", "Switch to 'Experimental Mode' when using turn signals below highway speeds to help assit with turns.", ""}, - + {"CSLCEnabled", "Custom Stock Longitudinal Control", "Use cruise control button spamming to adjust cruise set speed based on MTSC, VTSC, and SLC", "../frogpilot/assets/toggle_icons/icon_custom.png"}, {"CustomPersonalities", "Custom Driving Personalities", "Customize the driving personality profiles to your driving style.", "../frogpilot/assets/toggle_icons/icon_custom.png"}, {"DeviceShutdown", "Device Shutdown Timer", "Configure the timer for automatic device shutdown when offroad conserving energy and preventing battery drain.", "../frogpilot/assets/toggle_icons/icon_time.png"}, {"ExperimentalModeActivation", "Experimental Mode Via", "Toggle Experimental Mode by double-clicking the 'Lane Departure'/'LKAS' button or double tapping screen.\n\nOverrides 'Conditional Experimental Mode'.", "../assets/img_experimental_white.svg"},