From ba1ff9d73da04950ce4fa63c580171d99f2e2e0d Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 23 Jul 2024 00:18:31 -0700 Subject: [PATCH] Controls - Experimental Mode Activation - Long Press Distance Enable/disable 'Experimental Mode' by holding down the 'distance' button on your steering wheel for 0.5 seconds. --- selfdrive/car/honda/carstate.py | 3 +++ selfdrive/car/hyundai/carstate.py | 6 ++++++ selfdrive/car/interfaces.py | 27 ++++++++++++++++++++++++++- selfdrive/car/volkswagen/carstate.py | 8 ++++++++ selfdrive/controls/controlsd.py | 2 +- 5 files changed, 44 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 554b3b1cb..5bbbf7740 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -274,6 +274,9 @@ class CarState(CarStateBase): ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1 # FrogPilot CarState functions + 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 diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index c9938ab12..37bc6ee47 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -173,6 +173,9 @@ class CarState(CarStateBase): self.main_enabled = not self.main_enabled # FrogPilot CarState functions + self.prev_distance_button = self.distance_button + self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST + self.lkas_previously_enabled = self.lkas_enabled if self.CP.flags & HyundaiFlags.CAN_LFA_BTN: self.lkas_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"] @@ -264,6 +267,9 @@ class CarState(CarStateBase): else cp_cam.vl["CAM_0x2a4"]) # FrogPilot CarState functions + self.prev_distance_button = self.distance_button + self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST + self.lkas_previously_enabled = self.lkas_enabled self.lkas_enabled = cp.vl[self.cruise_btns_msg_canfd]["LFA_BTN"] diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 00ed49744..0fff1417f 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -17,7 +17,7 @@ from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG from openpilot.selfdrive.car.values import PLATFORMS -from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, get_friction +from openpilot.selfdrive.controls.lib.drive_helpers import CRUISE_LONG_PRESS, V_CRUISE_MAX, get_friction from openpilot.selfdrive.controls.lib.events import Events from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel @@ -120,8 +120,12 @@ class CarInterfaceBase(ABC): self.belowSteerSpeed_shown = False self.disable_belowSteerSpeed = False self.disable_resumeRequired = False + self.is_gm = self.CP.carName == "gm" + self.prev_distance_button = False self.resumeRequired_shown = False + self.gap_counter = 0 + def apply(self, c: car.CarControl, now_nanos: int, frogpilot_toggles) -> tuple[car.CarControl.Actuators, list[tuple[int, int, bytes, int]]]: return self.CC.update(c, self.CS, now_nanos, frogpilot_toggles) @@ -270,6 +274,7 @@ class CarInterfaceBase(ABC): # Add any additional frogpilotCarStates fp_ret.alwaysOnLateralDisabled = self.always_on_lateral_disabled + fp_ret.distanceLongPressed = self.frogpilot_distance_functions(frogpilot_toggles) # copy back for next iteration if self.CS is not None: @@ -359,6 +364,25 @@ class CarInterfaceBase(ABC): return events + def frogpilot_distance_functions(self, frogpilot_toggles): + distance_button = self.CS.distance_button or self.params_memory.get_bool("OnroadDistanceButtonPressed") + + if distance_button: + self.gap_counter += 1 + elif not self.prev_distance_button: + self.gap_counter = 0 + + if self.gap_counter == CRUISE_LONG_PRESS * (1.5 if self.is_gm else 1) and frogpilot_toggles.experimental_mode_via_distance: + if frogpilot_toggles.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) + + self.prev_distance_button = distance_button + return self.gap_counter >= CRUISE_LONG_PRESS class RadarInterfaceBase(ABC): def __init__(self, CP): @@ -399,6 +423,7 @@ class CarStateBase(ABC): self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K) # FrogPilot variables + self.distance_button = False self.lkas_enabled = False def update_speed_kf(self, v_ego_raw): diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index a51a4d8f8..db9c61243 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -153,6 +153,10 @@ class CarState(CarStateBase): # Digital instrument clusters expect the ACC HUD lead car distance to be scaled differently self.upscale_lead_car_signal = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"]) + # FrogPilot CarState functions + self.prev_distance_button = self.distance_button + self.distance_button = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Verstellung_Zeitluecke"]) + self.frame += 1 return ret, fp_ret @@ -253,6 +257,10 @@ class CarState(CarStateBase): # Additional safety checks performed in CarInterface. ret.espDisabled = bool(pt_cp.vl["Bremse_1"]["ESP_Passiv_getastet"]) + # FrogPilot CarState functions + self.prev_distance_button = self.distance_button + self.distance_button = bool(pt_cp.vl["GRA_Neu"]["GRA_Zeitluecke"]) + self.frame += 1 return ret, fp_ret diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 70983a84d..e9b3d6c81 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -675,7 +675,7 @@ class Controls: if self.CP.openpilotLongitudinalControl: if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents) or self.onroad_distance_pressed: menu_open = self.display_timer > 0 or not self.sm['frogpilotCarState'].hasMenu - if not self.params_memory.get_bool("OnroadDistanceButtonPressed") and menu_open: + if not (self.sm['frogpilotCarState'].distanceLongPressed or self.params_memory.get_bool("OnroadDistanceButtonPressed")) and menu_open: self.personality = (self.personality - 1) % 3 self.params.put_nonblocking('LongitudinalPersonality', str(self.personality)) self.display_timer = 350