From 2e865c2fcfbc9b96a73c73b11f11b77c6604deaa Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Wed, 10 Apr 2024 18:47:32 -0700 Subject: [PATCH] Experimental Mode activation via steering wheel / onroad UI Added toggles to control the current state of "Experimental Mode" from either the LKAS/distance buttons or the screen. --- cereal/custom.capnp | 5 ++ common/params.cc | 5 ++ opendbc/hyundai_canfd.dbc | 2 +- panda/python/__init__.py | 1 + selfdrive/car/chrysler/carstate.py | 6 +++ selfdrive/car/chrysler/interface.py | 5 +- selfdrive/car/ford/carstate.py | 3 ++ selfdrive/car/ford/interface.py | 5 +- selfdrive/car/gm/carstate.py | 6 +++ selfdrive/car/gm/interface.py | 3 +- selfdrive/car/honda/carstate.py | 6 +++ selfdrive/car/honda/interface.py | 1 + selfdrive/car/hyundai/carstate.py | 16 +++++++ selfdrive/car/hyundai/interface.py | 9 +++- selfdrive/car/hyundai/values.py | 1 + selfdrive/car/interfaces.py | 29 ++++++++++++ selfdrive/car/mazda/carstate.py | 3 ++ selfdrive/car/mazda/interface.py | 5 +- selfdrive/car/nissan/carstate.py | 1 + selfdrive/car/nissan/interface.py | 5 +- selfdrive/car/subaru/carstate.py | 4 +- selfdrive/car/subaru/interface.py | 4 ++ selfdrive/car/toyota/carstate.py | 5 ++ selfdrive/car/toyota/interface.py | 5 +- selfdrive/controls/controlsd.py | 17 ++++++- .../lib/conditional_experimental_mode.py | 10 +++- .../ui/qt/offroad/control_settings.cc | 18 ++++++- .../ui/qt/offroad/control_settings.h | 2 +- selfdrive/ui/qt/onroad.cc | 47 ++++++++++++++++++- selfdrive/ui/qt/onroad.h | 3 ++ selfdrive/ui/ui.cc | 2 + selfdrive/ui/ui.h | 1 + 32 files changed, 220 insertions(+), 15 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 911b36af2..40cef22e9 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -13,6 +13,11 @@ struct FrogPilotCarControl @0x81c2f05a394cf4af { } struct FrogPilotCarState @0xaedffd8f31e7b55d { + struct ButtonEvent { + enum Type { + lkas @0; + } + } } struct FrogPilotDeviceState @0xf35cc4560bbf6ec2 { diff --git a/common/params.cc b/common/params.cc index 16220e47a..5aaf9af57 100644 --- a/common/params.cc +++ b/common/params.cc @@ -255,10 +255,15 @@ std::unordered_map keys = { {"DisableOnroadUploads", PERSISTENT}, {"DisableOpenpilotLongitudinal", PERSISTENT}, {"DisengageVolume", PERSISTENT}, + {"DistanceLongPressed", PERSISTENT}, {"DragonPilotTune", PERSISTENT}, {"DriverCamera", PERSISTENT}, {"DynamicPathWidth", PERSISTENT}, {"EngageVolume", PERSISTENT}, + {"ExperimentalModeActivation", PERSISTENT}, + {"ExperimentalModeViaDistance", PERSISTENT}, + {"ExperimentalModeViaLKAS", PERSISTENT}, + {"ExperimentalModeViaTap", PERSISTENT}, {"FrogPilotTogglesUpdated", PERSISTENT}, {"FrogsGoMoo", PERSISTENT}, {"GoatScream", PERSISTENT}, diff --git a/opendbc/hyundai_canfd.dbc b/opendbc/hyundai_canfd.dbc index 1ef67c044..00991a714 100644 --- a/opendbc/hyundai_canfd.dbc +++ b/opendbc/hyundai_canfd.dbc @@ -316,7 +316,7 @@ BO_ 441 CAM_0x1b9: 32 CAMERA BO_ 463 CRUISE_BUTTONS: 8 XXX SG_ _CHECKSUM : 0|8@1+ (1,0) [0|65535] "" XXX - SG_ LKAS_BTN : 23|1@1+ (1,0) [0|1] "" XXX + SG_ LFA_BTN : 23|1@1+ (1,0) [0|1] "" XXX SG_ SET_ME_1 : 29|1@1+ (1,0) [0|1] "" XXX SG_ ADAPTIVE_CRUISE_MAIN_BTN : 19|1@1+ (1,0) [0|1] "" XXX SG_ NORMAL_CRUISE_MAIN_BTN : 21|1@1+ (1,0) [0|1] "" XXX diff --git a/panda/python/__init__.py b/panda/python/__init__.py index c148de0c0..d9904ebaf 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -211,6 +211,7 @@ class Panda: FLAG_HYUNDAI_CANFD_ALT_BUTTONS = 32 FLAG_HYUNDAI_ALT_LIMITS = 64 FLAG_HYUNDAI_CANFD_HDA2_ALT_STEERING = 128 + FLAG_HYUNDAI_LFA_BTN = 256 FLAG_TESLA_POWERTRAIN = 1 FLAG_TESLA_LONG_CONTROL = 2 diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 8d4ffa469..9c5fedaf7 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -101,6 +101,12 @@ class CarState(CarStateBase): self.lkas_car_model = cp_cam.vl["DAS_6"]["CAR_MODEL"] self.button_counter = cp.vl["CRUISE_BUTTONS"]["COUNTER"] + self.lkas_previously_enabled = self.lkas_enabled + if self.CP.carFingerprint in RAM_CARS: + self.lkas_enabled = cp.vl["Center_Stack_2"]["LKAS_Button"] or cp.vl["Center_Stack_1"]["LKAS_Button"] + else: + self.lkas_enabled = cp.vl["TRACTION_BUTTON"]["TOGGLE_LKAS"] == 1 + return ret @staticmethod diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index d2d6dd381..d45efc617 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -79,7 +79,10 @@ class CarInterface(CarInterfaceBase): def _update(self, c, frogpilot_variables): ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) - ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) + ret.buttonEvents = [ + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] # events events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low]) diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 74e641c80..60cee680b 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -106,6 +106,9 @@ class CarState(CarStateBase): self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"] self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"] + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"]) + return ret @staticmethod diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 37b8b105e..84260bb92 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -63,7 +63,10 @@ class CarInterface(CarInterfaceBase): def _update(self, c, frogpilot_variables): ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) - ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) + ret.buttonEvents = [ + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic]) if not self.CS.vehicle_sensors_valid: diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 331f29b37..fc66bf58d 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -160,6 +160,12 @@ class CarState(CarStateBase): ret.leftBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1 ret.rightBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1 + self.lkas_previously_enabled = self.lkas_enabled + if self.CP.carFingerprint in SDGM_CAR: + self.lkas_enabled = cam_cp.vl["ASCMSteeringButton"]["LKAButton"] + else: + self.lkas_enabled = pt_cp.vl["ASCMSteeringButton"]["LKAButton"] + return ret @staticmethod diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index bc806c6e0..18f8ed1f5 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -278,7 +278,8 @@ class CarInterface(CarInterfaceBase): *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, unpressed_btn=CruiseButtons.UNPRESS), *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, - {1: ButtonType.gapAdjustCruise}) + {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), ] # The ECM allows enabling on falling edge of set, but only rising edge of resume diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index d0f8a6593..d8a88106f 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -267,6 +267,12 @@ class CarState(CarStateBase): ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]["BSM_ALERT"] == 1 ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1 + self.prev_distance_button = self.distance_button + self.distance_button = self.cruise_setting == 3 + + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = self.cruise_setting == 1 + return ret def get_can_parser(self, CP): diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 960ddcf67..8d7f18669 100644 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -249,6 +249,7 @@ class CarInterface(CarInterfaceBase): ret.buttonEvents = [ *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT), *create_button_events(self.CS.cruise_setting, self.CS.prev_cruise_setting, SETTINGS_BUTTONS_DICT), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), ] # events diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 0687a653e..b7b5d81fc 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -170,6 +170,13 @@ class CarState(CarStateBase): if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0: self.main_enabled = not self.main_enabled + self.prev_distance_button = self.distance_button + self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST + + if self.CP.flags & HyundaiFlags.CAN_LFA_BTN: + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"] + return ret def update_canfd(self, cp, cp_cam, frogpilot_variables): @@ -255,6 +262,12 @@ class CarState(CarStateBase): self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING else cp_cam.vl["CAM_0x2a4"]) + self.prev_distance_button = self.distance_button + self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0 + + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = cp.vl[self.cruise_btns_msg_canfd]["LFA_BTN"] + return ret def get_can_parser(self, CP): @@ -304,6 +317,9 @@ class CarState(CarStateBase): else: messages.append(("LVR12", 100)) + if CP.flags & HyundaiFlags.CAN_LFA_BTN: + messages.append(("BCM_PO_11", 50)) + return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0) @staticmethod diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 8f0bc2e16..2dba91b77 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -126,6 +126,10 @@ class CarInterface(CarInterfaceBase): if candidate in CAMERA_SCC_CAR: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC + if 0x391 in fingerprint[0]: + ret.flags |= HyundaiFlags.CAN_LFA_BTN.value + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_LFA_BTN + if ret.openpilotLongitudinalControl: ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_LONG if ret.flags & HyundaiFlags.HYBRID: @@ -157,7 +161,10 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) if self.CS.CP.openpilotLongitudinalControl: - ret.buttonEvents = create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT) + ret.buttonEvents = [ + *create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] # On some newer model years, the CANCEL button acts as a pause/resume button based on the PCM state # To avoid re-engaging when openpilot cancels, check user engagement intention via buttons diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 6a00fe772..915a58118 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -96,6 +96,7 @@ class HyundaiFlags(IntFlag): MIN_STEER_32_MPH = 2 ** 23 + CAN_LFA_BTN = 2 ** 24 class Footnote(Enum): CANFD = CarFootnote( diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 36f8739ff..609c8bd0f 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -108,8 +108,11 @@ class CarInterfaceBase(ABC): self.belowSteerSpeed_shown = False self.disable_belowSteerSpeed = False self.disable_resumeRequired = False + self.prev_distance_button = False self.resumeRequired_shown = False + self.gap_counter = 0 + @staticmethod def get_pid_accel_limits(CP, current_speed, cruise_speed, frogpilot_variables): if frogpilot_variables.sport_plus: @@ -259,6 +262,10 @@ class CarInterfaceBase(ABC): if ret.cruiseState.speedCluster == 0: ret.cruiseState.speedCluster = ret.cruiseState.speed + distance_button = self.CS.distance_button or self.params_memory.get_bool("OnroadDistanceButtonPressed") + self.params_memory.put_bool("DistanceLongPressed", self.frogpilot_distance_functions(distance_button, self.prev_distance_button, frogpilot_variables)) + self.prev_distance_button = distance_button + # copy back for next iteration reader = ret.as_reader() if self.CS is not None: @@ -343,6 +350,22 @@ class CarInterfaceBase(ABC): return events + def frogpilot_distance_functions(self, distance_button, prev_distance_button, frogpilot_variables): + if distance_button: + self.gap_counter += 1 + elif not prev_distance_button: + self.gap_counter = 0 + + if self.gap_counter == CRUISE_LONG_PRESS * 1.5 and frogpilot_variables.experimental_mode_via_distance: + if frogpilot_variables.conditional_experimental_mode: + conditional_status = self.params_memory.get_int("CEStatus") + override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 1 if conditional_status >= 7 else 2 + self.params_memory.put_int("CEStatus", override_value) + else: + experimental_mode = self.params.get_bool("ExperimentalMode") + self.params.put_bool("ExperimentalMode", not experimental_mode) + + return self.gap_counter >= CRUISE_LONG_PRESS class RadarInterfaceBase(ABC): def __init__(self, CP): @@ -385,6 +408,12 @@ class CarStateBase(ABC): # FrogPilot variables self.params_memory = Params("/dev/shm/params") + self.lkas_enabled = False + self.lkas_previously_enabled = False + + self.prev_distance_button = 0 + self.distance_button = 0 + def update_speed_kf(self, v_ego_raw): if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed self.v_ego_kf.set_x([[v_ego_raw], [0.0]]) diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 86096d572..9b236b283 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -110,6 +110,9 @@ class CarState(CarStateBase): self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"] ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1 + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = not self.lkas_disabled + return ret @staticmethod diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index a09a140f6..7d31d575a 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -36,7 +36,10 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) # TODO: add button types for inc and dec - ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) + ret.buttonEvents = [ + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] # events events = self.create_common_events(ret) diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index ca3e15585..13f7fe396 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -107,6 +107,7 @@ class CarState(CarStateBase): can_gear = int(cp.vl["GEARBOX"]["GEAR_SHIFTER"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + self.lkas_previously_enabled = self.lkas_enabled if self.CP.carFingerprint == CAR.ALTIMA: self.lkas_enabled = bool(cp.vl["LKAS_SETTINGS"]["LKAS_ENABLED"]) else: diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 1d21c36ad..cd89d2988 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -33,7 +33,10 @@ class CarInterface(CarInterfaceBase): def _update(self, c, frogpilot_variables): ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam, frogpilot_variables) - ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) + ret.buttonEvents = [ + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.brake]) diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 41e1571ee..4ab1e0130 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -125,6 +125,9 @@ class CarState(CarStateBase): if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"]) + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = cp_cam.vl["ES_LKAS_State"]["LKAS_Dash_State"] + return ret @staticmethod @@ -226,4 +229,3 @@ class CarState(CarStateBase): ] return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.alt) - diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index acba1ab8f..98c868d14 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -107,6 +107,10 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam, self.cp_body, frogpilot_variables) + ret.buttonEvents = [ + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] + ret.events = self.create_common_events(ret).to_msg() return ret diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 7a21551f2..84e3a6980 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -182,6 +182,11 @@ class CarState(CarStateBase): else: self.distance_button = cp.vl["SDSU"]["FD_BUTTON"] + if self.CP.carFingerprint != CAR.PRIUS_V: + self.lkas_previously_enabled = self.lkas_enabled + message_keys = ["LDA_ON_MESSAGE", "SET_ME_X02"] + self.lkas_enabled = any(self.lkas_hud.get(key) == 1 for key in message_keys) + return ret @staticmethod diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 211f05e06..48d5c2317 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -206,7 +206,10 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER): - ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) + ret.buttonEvents = [ + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] # events events = self.create_common_events(ret) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index db2ea1b21..3fdb4abd6 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -680,8 +680,9 @@ class Controls: # decrement personality on distance button press if self.CP.openpilotLongitudinalControl: if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents): - self.personality = (self.personality - 1) % 3 - self.params.put_nonblocking('LongitudinalPersonality', str(self.personality)) + if not self.params_memory.get_bool("DistanceLongPressed"): + self.personality = (self.personality - 1) % 3 + self.params.put_nonblocking('LongitudinalPersonality', str(self.personality)) return CC, lac_log @@ -903,6 +904,14 @@ class Controls: if self.CP.openpilotLongitudinalControl and self.frogpilot_variables.conditional_experimental_mode: self.experimental_mode = self.sm['frogpilotPlan'].conditionalExperimental + if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents) and self.experimental_mode_via_lkas: + if self.frogpilot_variables.conditional_experimental_mode: + conditional_status = self.params_memory.get_int("CEStatus") + override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 3 if conditional_status >= 7 else 4 + self.params_memory.put_int("CEStatus", override_value) + else: + self.params.put_bool_nonblocking("ExperimentalMode", not self.experimental_mode) + fpcc_send = messaging.new_message('frogpilotCarControl') fpcc_send.valid = CS.canValid fpcc_send.frogpilotCarControl = self.FPCC @@ -926,6 +935,10 @@ class Controls: device_management = self.params.get_bool("DeviceManagement") self.increase_thermal_limits = device_management and self.params.get_bool("IncreaseThermalLimits") + experimental_mode_activation = self.CP.openpilotLongitudinalControl and self.params.get_bool("ExperimentalModeActivation") + self.frogpilot_variables.experimental_mode_via_distance = experimental_mode_activation and self.params.get_bool("ExperimentalModeViaDistance") + self.experimental_mode_via_lkas = experimental_mode_activation and self.params.get_bool("ExperimentalModeViaLKAS") + lateral_tune = self.params.get_bool("LateralTune") longitudinal_tune = self.CP.openpilotLongitudinalControl and self.params.get_bool("LongitudinalTune") diff --git a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py index 0cf0451ea..401f2ae24 100644 --- a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py +++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py @@ -39,15 +39,21 @@ class ConditionalExperimentalMode: standstill = carState.standstill v_lead = lead.vLead + if self.experimental_mode_via_press and enabled: + overridden = self.params_memory.get_int("CEStatus") + else: + overridden = 0 + self.update_conditions(lead_distance, lead.status, modelData, road_curvature, standstill, t_follow, v_ego, v_lead) condition_met = self.check_conditions(carState, frogpilotNavigation, lead, modelData, standstill, v_ego) and enabled - if condition_met: + if condition_met and overridden not in {1, 3, 5} or overridden in {2, 4, 6}: self.experimental_mode = True else: self.experimental_mode = False self.status_value = 0 + self.status_value = overridden if overridden in {1, 2, 3, 4, 5, 6} else self.status_value if self.status_value != self.previous_status_value: self.params_memory.put_int("CEStatus", self.status_value) self.previous_status_value = self.status_value @@ -168,6 +174,8 @@ class ConditionalExperimentalMode: self.curves = self.params.get_bool("CECurves") self.curves_lead = self.curves and self.params.get_bool("CECurvesLead") + self.experimental_mode_via_press = self.params.get_bool("ExperimentalModeActivation") + self.limit = self.params.get_int("CESpeed") * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS) self.limit_lead = self.params.get_int("CESpeedLead") * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS) diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc index a6e99ad8b..5915cd61f 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc @@ -28,6 +28,11 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"LowVoltageShutdown", tr("Low Voltage Shutdown Threshold"), tr("Automatically shut the device down when your battery reaches a specific voltage level to prevent killing your battery."), ""}, {"OfflineMode", tr("Offline Mode"), tr("Allow the device to be offline indefinitely."), ""}, + {"ExperimentalModeActivation", tr("Experimental Mode Activation"), tr("Toggle Experimental Mode with either buttons on the steering wheel or the screen.\n\nOverrides 'Conditional Experimental Mode'."), "../assets/img_experimental_white.svg"}, + {"ExperimentalModeViaLKAS", tr("Double Clicking the LKAS Button"), tr("Enable/disable 'Experimental Mode' by double clicking the 'LKAS' button on your steering wheel."), ""}, + {"ExperimentalModeViaTap", tr("Double Tapping the Onroad UI"), tr("Enable/disable 'Experimental Mode' by double tapping the onroad UI within a 0.5 second time frame."), ""}, + {"ExperimentalModeViaDistance", tr("Long Pressing the Distance Button"), tr("Enable/disable 'Experimental Mode' by holding down the 'distance' button on your steering wheel for 0.5 seconds."), ""}, + {"LateralTune", tr("Lateral Tuning"), tr("Modify openpilot's steering behavior."), "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"}, {"LongitudinalTune", tr("Longitudinal Tuning"), tr("Modify openpilot's acceleration and braking behavior."), "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"}, @@ -162,6 +167,16 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil } else if (param == "LowVoltageShutdown") { toggle = new FrogPilotParamValueControl(param, title, desc, icon, 11.8, 12.5, std::map(), this, false, tr(" volts"), 1, 0.01); + } else if (param == "ExperimentalModeActivation") { + FrogPilotParamManageControl *experimentalModeActivationToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(experimentalModeActivationToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + openParentToggle(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(experimentalModeActivationKeys.find(key.c_str()) != experimentalModeActivationKeys.end()); + } + }); + toggle = experimentalModeActivationToggle; + } else if (param == "LateralTune") { FrogPilotParamManageControl *lateralTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); QObject::connect(lateralTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { @@ -367,7 +382,8 @@ void FrogPilotControlsPanel::hideToggles() { standardProfile->setVisible(false); relaxedProfile->setVisible(false); - std::set longitudinalKeys = {"ConditionalExperimental", "CustomPersonalities", "LongitudinalTune"}; + std::set longitudinalKeys = {"ConditionalExperimental", "CustomPersonalities", "ExperimentalModeActivation", + "LongitudinalTune"}; for (auto &[key, toggle] : toggles) { toggle->setVisible(false); diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h index baff29f98..47926e9ea 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h @@ -34,7 +34,7 @@ private: std::set aolKeys = {"AlwaysOnLateralMain", "HideAOLStatusBar", "PauseAOLOnBrake"}; std::set conditionalExperimentalKeys = {"CECurves", "CECurvesLead", "CENavigation", "CESignal", "CESlowerLead", "CEStopLights", "HideCEMStatusBar"}; std::set deviceManagementKeys = {"DeviceShutdown", "IncreaseThermalLimits", "LowVoltageShutdown", "NoLogging", "NoUploads", "OfflineMode"}; - std::set experimentalModeActivationKeys = {}; + std::set experimentalModeActivationKeys = {"ExperimentalModeViaDistance", "ExperimentalModeViaLKAS", "ExperimentalModeViaTap"}; std::set laneChangeKeys = {}; std::set lateralTuneKeys = {}; std::set longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile"}; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 751cadaaa..916740640 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -6,6 +6,7 @@ #include #include +#include #include #include @@ -66,6 +67,12 @@ OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent), scene(uiState()-> QObject::connect(uiState(), &UIState::uiUpdate, this, &OnroadWindow::updateState); QObject::connect(uiState(), &UIState::offroadTransition, this, &OnroadWindow::offroadTransition); QObject::connect(uiState(), &UIState::primeChanged, this, &OnroadWindow::primeChanged); + + QObject::connect(&clickTimer, &QTimer::timeout, this, [this]() { + clickTimer.stop(); + QMouseEvent *event = new QMouseEvent(QEvent::MouseButtonPress, timeoutPoint, Qt::LeftButton, Qt::LeftButton, Qt::NoModifier); + QApplication::postEvent(this, event); + }); } void OnroadWindow::updateState(const UIState &s) { @@ -96,6 +103,25 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { // FrogPilot clickable widgets bool widgetClicked = false; + // If the click wasn't for anything specific, change the value of "ExperimentalMode" + if (scene.experimental_mode_via_screen && e->pos() != timeoutPoint) { + if (clickTimer.isActive()) { + clickTimer.stop(); + + if (scene.conditional_experimental) { + int override_value = (scene.conditional_status >= 1 && scene.conditional_status <= 6) ? 0 : scene.conditional_status >= 7 ? 5 : 6; + paramsMemory.putIntNonBlocking("CEStatus", override_value); + } else { + bool experimentalMode = params.getBool("ExperimentalMode"); + params.putBoolNonBlocking("ExperimentalMode", !experimentalMode); + } + + } else { + clickTimer.start(500); + } + widgetClicked = true; + } + #ifdef ENABLE_MAPS if (map != nullptr && !widgetClicked) { // Switch between map and sidebar when using navigate on openpilot @@ -269,7 +295,12 @@ void ExperimentalButton::changeMode() { const auto cp = (*uiState()->sm)["carParams"].getCarParams(); bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed"); if (can_change) { - params.putBool("ExperimentalMode", !experimental_mode); + if (scene.conditional_experimental) { + int override_value = (scene.conditional_status >= 1 && scene.conditional_status <= 4) ? 0 : scene.conditional_status >= 5 ? 5 : 6; + paramsMemory.putIntNonBlocking("ConditionalStatus", override_value); + } else { + params.putBool("ExperimentalMode", !experimental_mode); + } } } @@ -1160,6 +1191,20 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { newStatus = conditionalStatusMap[status != STATUS_DISENGAGED ? conditionalStatus : 0]; } + QString distanceSuffix = tr(". Long press the \"distance\" button to revert"); + QString lkasSuffix = tr(". Double press the \"LKAS\" button to revert"); + QString screenSuffix = tr(". Double tap the screen to revert"); + + if (!alwaysOnLateralActive && !mapOpen && status != STATUS_DISENGAGED && !newStatus.isEmpty()) { + if (conditionalStatus == 1 || conditionalStatus == 2) { + newStatus += distanceSuffix; + } else if (conditionalStatus == 3 || conditionalStatus == 4) { + newStatus += lkasSuffix; + } else if (conditionalStatus == 5 || conditionalStatus == 6) { + newStatus += screenSuffix; + } + } + if (newStatus != lastShownStatus) { displayStatusText = true; lastShownStatus = newStatus; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 16e3b311d..fda1a134e 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -200,6 +200,9 @@ private: Params params; Params paramsMemory{"/dev/shm/params"}; + QPoint timeoutPoint = QPoint(420, 69); + QTimer clickTimer; + private slots: void offroadTransition(bool offroad); void primeChanged(bool prime); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 8920b403b..1e341ca8c 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -317,6 +317,8 @@ void ui_update_frogpilot_params(UIState *s) { scene.custom_icons = custom_theme ? params.getInt("CustomIcons") : 0; scene.custom_signals = custom_theme ? params.getInt("CustomSignals") : 0; + scene.experimental_mode_via_screen = scene.longitudinal_control && params.getBool("ExperimentalModeActivation") && params.getBool("ExperimentalModeViaTap"); + scene.model_ui = params.getBool("ModelUI"); scene.dynamic_path_width = scene.model_ui && params.getBool("DynamicPathWidth"); scene.hide_lead_marker = scene.model_ui && params.getBool("HideLeadMarker"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index cabd5af8d..59fc2fd67 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -183,6 +183,7 @@ typedef struct UIScene { bool dynamic_path_width; bool enabled; bool experimental_mode; + bool experimental_mode_via_screen; bool has_auto_tune; bool hide_lead_marker; bool lead_info;