diff --git a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py index d37760eb2..c03c51b60 100644 --- a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py +++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py @@ -22,12 +22,17 @@ class ConditionalExperimentalMode: self.stop_light_detected = False def update(self, carState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles): - if not carState.standstill: + if frogpilot_toggles.experimental_mode_via_press: + self.status_value = self.params_memory.get_int("CEStatus") + else: + self.status_value = 0 + + if self.status_value not in {1, 2, 3, 4, 5, 6} and not carState.standstill: self.update_conditions(self.frogpilot_planner.tracking_lead, v_ego, v_lead, frogpilot_toggles) self.experimental_mode = self.check_conditions(carState, frogpilotNavigation, modelData, self.frogpilot_planner.tracking_lead, v_ego, v_lead, frogpilot_toggles) self.params_memory.put_int("CEStatus", self.status_value if self.experimental_mode else 0) else: - self.experimental_mode = carState.standstill and self.experimental_mode + self.experimental_mode = self.status_value in {2, 4, 6} or carState.standstill and self.experimental_mode def check_conditions(self, carState, frogpilotNavigation, modelData, tracking_lead, v_ego, v_lead, frogpilot_toggles): below_speed = frogpilot_toggles.conditional_limit > v_ego >= 1 and not tracking_lead diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 2694a736f..4cf77d4b0 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -551,6 +551,12 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { std::map conditionalStatusMap = { {0, tr("Conditional Experimental Mode ready")}, + {1, tr("Conditional Experimental overridden")}, + {2, tr("Experimental Mode manually activated")}, + {3, tr("Conditional Experimental overridden")}, + {4, tr("Experimental Mode manually activated")}, + {5, tr("Conditional Experimental overridden")}, + {6, tr("Experimental Mode manually activated")}, {7, tr("Experimental Mode activated for") + (mapOpen ? tr(" low speed") : tr(" speed being less than ") + QString::number(conditionalSpeedLead) + (is_metric ? tr("kph") : tr("mph")))}, {8, tr("Experimental Mode activated for") + (mapOpen ? tr(" low speed") : tr(" speed being less than ") + QString::number(conditionalSpeed) + (is_metric ? tr("kph") : tr("mph")))}, {9, tr("Experimental Mode activated for turn") + (mapOpen ? "" : tr(" / lane change"))}, @@ -568,6 +574,21 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { newStatus = conditionalStatusMap.at(conditionalStatus); } + static const std::map suffixMap = { + {1, tr(". Long press the \"distance\" button to revert")}, + {2, tr(". Long press the \"distance\" button to revert")}, + {3, tr(". Click the \"LKAS\" button to revert")}, + {4, tr(". Click the \"LKAS\" button to revert")}, + {5, tr(". Double tap the screen to revert")}, + {6, tr(". Double tap the screen to revert")}, + }; + + if (!alwaysOnLateralActive && !mapOpen && !newStatus.isEmpty()) { + if (suffixMap.find(conditionalStatus) != suffixMap.end()) { + newStatus += suffixMap.at(conditionalStatus); + } + } + if (newStatus != lastShownStatus) { lastShownStatus = newStatus; displayStatusText = true; diff --git a/selfdrive/ui/qt/onroad/buttons.cc b/selfdrive/ui/qt/onroad/buttons.cc index 764b49638..148eaee4b 100644 --- a/selfdrive/ui/qt/onroad/buttons.cc +++ b/selfdrive/ui/qt/onroad/buttons.cc @@ -28,7 +28,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 (conditionalExperimental) { + int override_value = (conditionalStatus >= 1 && conditionalStatus <= 6) ? 0 : conditionalStatus >= 7 ? 5 : 6; + paramsMemory.putIntNonBlocking("CEStatus", override_value); + } else { + params.putBool("ExperimentalMode", !experimental_mode); + } } } @@ -43,6 +48,9 @@ void ExperimentalButton::updateState(const UIState &s) { // FrogPilot variables const UIScene &scene = s.scene; + + conditionalExperimental = scene.conditional_experimental; + conditionalStatus = scene.conditional_status; } void ExperimentalButton::paintEvent(QPaintEvent *event) { diff --git a/selfdrive/ui/qt/onroad/buttons.h b/selfdrive/ui/qt/onroad/buttons.h index ecd73efbb..1b71fe492 100644 --- a/selfdrive/ui/qt/onroad/buttons.h +++ b/selfdrive/ui/qt/onroad/buttons.h @@ -25,6 +25,10 @@ private: bool engageable; // FrogPilot variables + bool conditionalExperimental; + + int conditionalStatus; + Params paramsMemory{"/dev/shm/params"}; }; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index afebca34e..2cf2cc625 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -57,6 +57,7 @@ typedef enum UIStatus { // FrogPilot statuses STATUS_ALWAYS_ON_LATERAL_ACTIVE, + STATUS_CONDITIONAL_OVERRIDDEN, STATUS_EXPERIMENTAL_MODE_ACTIVE, STATUS_NAVIGATION_ACTIVE, } UIStatus; @@ -79,6 +80,7 @@ const QColor bg_colors [] = { // FrogPilot colors [STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(0x0a, 0xba, 0xb5, 0xf1), + [STATUS_CONDITIONAL_OVERRIDDEN] = QColor(0xff, 0xff, 0x00, 0xf1), [STATUS_EXPERIMENTAL_MODE_ACTIVE] = QColor(0xda, 0x6f, 0x25, 0xf1), [STATUS_NAVIGATION_ACTIVE] = QColor(0x31, 0xa1, 0xee, 0xf1), };