From aab08b8527ca3016d744b0a9b98a04d8a3ed1cf3 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 15 Apr 2025 00:56:44 -0400 Subject: [PATCH] init --- selfdrive/selfdrived/selfdrived.py | 1 + selfdrive/ui/qt/onroad/buttons.h | 5 ++- selfdrive/ui/sunnypilot/qt/onroad/buttons.cc | 25 ++++++++++++++ selfdrive/ui/sunnypilot/qt/onroad/buttons.h | 1 + sunnypilot/selfdrive/car/cruise_helpers.py | 36 +++++++++++++++----- 5 files changed, 56 insertions(+), 12 deletions(-) diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index c410adcf1b..732c772d6f 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -546,6 +546,7 @@ class SelfdriveD(CruiseHelper): self.mads.read_params() self.car_events_sp.read_params() + CruiseHelper.read_params(self) time.sleep(0.1) def run(self): diff --git a/selfdrive/ui/qt/onroad/buttons.h b/selfdrive/ui/qt/onroad/buttons.h index fca909f9b2..b56796b2d4 100644 --- a/selfdrive/ui/qt/onroad/buttons.h +++ b/selfdrive/ui/qt/onroad/buttons.h @@ -20,13 +20,12 @@ public: private: void paintEvent(QPaintEvent *event) override; - void changeMode(); - - Params params; protected: virtual void drawButton(QPainter &p); + virtual void changeMode(); + Params params; QPixmap engage_img; QPixmap experimental_img; bool experimental_mode; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/buttons.cc b/selfdrive/ui/sunnypilot/qt/onroad/buttons.cc index 97f6a391ce..529737c56f 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/buttons.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/buttons.cc @@ -9,11 +9,36 @@ #include +#include "selfdrive/ui/qt/util.h" + ExperimentalButtonSP::ExperimentalButtonSP(QWidget *parent) : ExperimentalButton(parent) { QObject::disconnect(uiState(), &UIState::uiUpdate, this, &ExperimentalButton::updateState); QObject::connect(uiState(), &UIState::uiUpdate, this, &ExperimentalButtonSP::updateState); } +void ExperimentalButtonSP::changeMode() { + const auto cp = (*uiState()->sm)["carParams"].getCarParams(); + bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed"); + if (can_change) { + bool experimental_mode_new; + bool dynamic_experimental_control_new; + + if (!experimental_mode && dynamic_experimental_control) { + experimental_mode_new = true; + dynamic_experimental_control_new = false; + } else if (experimental_mode && !dynamic_experimental_control) { + experimental_mode_new = true; + dynamic_experimental_control_new = true; + } else { + experimental_mode_new = false; + dynamic_experimental_control_new = false; + } + + params.putBool("ExperimentalMode", experimental_mode_new); + params.putBool("DynamicExperimentalControl", dynamic_experimental_control_new); + } +} + void ExperimentalButtonSP::updateState(const UIState &s) { ExperimentalButton::updateState(s); const auto long_plan_sp = (*s.sm)["longitudinalPlanSP"].getLongitudinalPlanSP(); diff --git a/selfdrive/ui/sunnypilot/qt/onroad/buttons.h b/selfdrive/ui/sunnypilot/qt/onroad/buttons.h index ce62a69e2d..16568e681f 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/buttons.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/buttons.h @@ -18,6 +18,7 @@ public: private: void drawButton(QPainter &p) override; + void changeMode() override; bool dynamic_experimental_control; int dec_mpc_mode; diff --git a/sunnypilot/selfdrive/car/cruise_helpers.py b/sunnypilot/selfdrive/car/cruise_helpers.py index 1c0026e12f..47e192d02c 100644 --- a/sunnypilot/selfdrive/car/cruise_helpers.py +++ b/sunnypilot/selfdrive/car/cruise_helpers.py @@ -7,7 +7,6 @@ See the LICENSE.md file in the root directory for more details. from cereal import car, custom from opendbc.car import structs -from openpilot.common.params import Params ButtonType = car.CarState.ButtonEvent.Type EventNameSP = custom.OnroadEventSP.EventName @@ -16,21 +15,26 @@ DISTANCE_LONG_PRESS = 50 class CruiseHelper: - def __init__(self, CP: structs.CarParams): + def __init__(self, CP: structs.CarParams, selfdrived): self.CP = CP - self.params = Params() + self.selfdrived = selfdrived + self.params = self.selfdrived.params self.button_frame_counts = {ButtonType.gapAdjustCruise: 0} - self._experimental_mode = False self.experimental_mode_switched = False + self.experimental_mode = self.selfdrived.experimental_mode + self.dynamic_experimental_control = False - def update(self, CS, events, experimental_mode) -> None: + def read_params(self): + self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl") + + def update(self, CS, events) -> None: if self.CP.openpilotLongitudinalControl: if CS.cruiseState.available: self.update_button_frame_counts(CS) # toggle experimental mode once on distance button hold - self.update_experimental_mode(events, experimental_mode) + self.update_experimental_mode(events) def update_button_frame_counts(self, CS) -> None: for button in self.button_frame_counts: @@ -42,9 +46,23 @@ class CruiseHelper: if button in self.button_frame_counts: self.button_frame_counts[button] = int(button_event.pressed) - def update_experimental_mode(self, events, experimental_mode) -> None: + def update_experimental_mode(self, events) -> None: if self.button_frame_counts[ButtonType.gapAdjustCruise] >= DISTANCE_LONG_PRESS and not self.experimental_mode_switched: - self._experimental_mode = not experimental_mode - self.params.put_bool_nonblocking("ExperimentalMode", self._experimental_mode) + if not self.experimental_mode and not self.dynamic_experimental_control: + # State 1 -> 2: Turn on experimental mode only + experimental_mode = True + dynamic_experimental_control = False + elif self.experimental_mode and not self.dynamic_experimental_control: + # State 2 -> 3: Keep experimental mode on, turn on DEC + experimental_mode = True + dynamic_experimental_control = True + else: + # State 3 -> 1: Turn everything off, so back to chill mode + experimental_mode = False + dynamic_experimental_control = False + + self.params.put_bool_nonblocking("ExperimentalMode", experimental_mode) + self.params.put_bool_nonblocking("DynamicExperimentalControl", dynamic_experimental_control) + events.add(EventNameSP.experimentalModeSwitched) self.experimental_mode_switched = True