diff --git a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py index 45abba518..95a38ed96 100644 --- a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py +++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py @@ -16,12 +16,17 @@ class ConditionalExperimentalMode: self.stop_light_mac = MovingAverageCalculator() def update(self, carState, frogpilotNavigation, lead, modelData, model_length, road_curvature, slower_lead, tracking_lead, 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(lead.dRel, model_length, road_curvature, slower_lead, tracking_lead, v_ego, v_lead, frogpilot_toggles) self.experimental_mode = self.check_conditions(carState, frogpilotNavigation, modelData, 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): if (tracking_lead and v_ego <= frogpilot_toggles.conditional_limit_lead) or (not tracking_lead and v_ego <= frogpilot_toggles.conditional_limit): diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 169b683da..134dc479b 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -547,6 +547,12 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { static const 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) + leadSpeedUnit)}, {8, tr("Experimental Mode activated for") + (mapOpen ? tr(" low speed") : tr(" speed being less than ") + QString::number(conditionalSpeed) + leadSpeedUnit)}, {9, tr("Experimental Mode activated for turn") + (mapOpen ? "" : tr(" / lane change"))}, @@ -564,6 +570,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(". Double press the \"LKAS\" button to revert")}, + {4, tr(". Double press 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 31682d603..76fd0e612 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 c21cdcaa1..3721737f1 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 53c091261..a2c00bf93 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -56,6 +56,7 @@ typedef enum UIStatus { // FrogPilot statuses STATUS_ALWAYS_ON_LATERAL_ACTIVE, + STATUS_CONDITIONAL_OVERRIDDEN, STATUS_EXPERIMENTAL_MODE_ACTIVE, STATUS_NAVIGATION_ACTIVE, } UIStatus; @@ -78,6 +79,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), };