diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4fb00a7ce..6fa2d6ff0 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -680,7 +680,7 @@ class Controls: self.display_timer -= 1 - FPCC = self.update_frogpilot_variables(CS, self.sm['frogpilotCarState']) + FPCC = self.update_frogpilot_variables(CS, self.sm['frogpilotCarState'], self.sm['frogpilotPlan']) return CC, lac_log, FPCC @@ -862,7 +862,8 @@ class Controls: def params_thread(self, evt): while not evt.is_set(): self.is_metric = self.params.get_bool("IsMetric") - self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl + if self.CP.openpilotLongitudinalControl and not self.frogpilot_toggles.conditional_experimental_mode: + self.experimental_mode = self.params.get_bool("ExperimentalMode") self.personality = self.read_personality_param() if self.CP.notCar: self.joystick_mode = self.params.get_bool("JoystickDebugMode") @@ -892,7 +893,7 @@ class Controls: self.events.add(EventName.openpilotCrashed) self.openpilot_crashed_triggered = True - def update_frogpilot_variables(self, CS, frogpilotCarState): + def update_frogpilot_variables(self, CS, frogpilotCarState, frogpilotPlan): driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown) self.always_on_lateral_active |= self.frogpilot_toggles.always_on_lateral_main or CS.cruiseState.enabled @@ -918,6 +919,9 @@ class Controls: self.params_tracking.put_int_nonblocking("FrogPilotDrives", self.total_drives) self.drive_added = True + if self.frogpilot_toggles.conditional_experimental_mode: + self.experimental_mode = frogpilotPlan.conditionalExperimentalActive + FPCC = custom.FrogPilotCarControl.new_message() FPCC.alwaysOnLateral = self.always_on_lateral_active diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index ef1f2d437..4694c5c86 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -134,7 +134,7 @@ class VCruiseHelper: if self.CP.pcmCruise: return - initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL + initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode and not frogpilot_toggles.conditional_experimental_mode else V_CRUISE_INITIAL # 250kph or above probably means we never had a set speed if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250: diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index eb4b684dc..6804bb2ab 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -13,6 +13,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHA get_jerk_factor, get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, Lead, get_max_accel +from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator, calculate_lane_width, calculate_road_curvature from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, PROBABILITY @@ -22,6 +23,8 @@ class FrogPilotPlanner: def __init__(self): self.params_memory = Params("/dev/shm/params") + self.cem = ConditionalExperimentalMode(self) + self.tracking_lead = False self.acceleration_jerk = 0 @@ -44,6 +47,10 @@ class FrogPilotPlanner: lead_distance = self.lead_one.dRel stopping_distance = STOP_DISTANCE + run_cem = frogpilot_toggles.conditional_experimental_mode + if run_cem and (controlsState.enabled or frogpilotCarControl.alwaysOnLateral) and driving_gear: + self.cem.update(carState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles) + if v_ego >= frogpilot_toggles.minimum_lane_change_speed: self.lane_width_left = calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0]) self.lane_width_right = calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1]) @@ -111,6 +118,8 @@ class FrogPilotPlanner: frogpilotPlan.speedJerkStock = float(J_EGO_COST * self.base_speed_jerk) frogpilotPlan.tFollow = float(self.t_follow) + frogpilotPlan.conditionalExperimentalActive = self.cem.experimental_mode + frogpilotPlan.laneWidthLeft = self.lane_width_left frogpilotPlan.laneWidthRight = self.lane_width_right diff --git a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py new file mode 100644 index 000000000..9781171ed --- /dev/null +++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py @@ -0,0 +1,31 @@ +from openpilot.common.params import Params + +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, PROBABILITY + +class ConditionalExperimentalMode: + def __init__(self, FrogPilotPlanner): + self.params_memory = Params("/dev/shm/params") + + self.frogpilot_planner = FrogPilotPlanner + + self.experimental_mode = False + + def update(self, carState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles): + if 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 + + 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 + below_speed_with_lead = frogpilot_toggles.conditional_limit_lead > v_ego >= 1 and tracking_lead + if below_speed or below_speed_with_lead: + self.status_value = 7 if tracking_lead else 8 + return True + + return False + + def update_conditions(self, tracking_lead, v_ego, v_lead, frogpilot_toggles): diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 933a49ae0..d90cbbe7b 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -180,7 +180,13 @@ void TogglesPanel::updateToggles() { op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable()); if (hasLongitudinalControl(CP)) { // normal description and toggle - experimental_mode_toggle->setEnabled(true); + bool conditional_experimental = params.getBool("ConditionalExperimental"); + if (conditional_experimental) { + params.putBool("ExperimentalMode", true); + params.putBool("ExperimentalModeConfirmed", true); + experimental_mode_toggle->refresh(); + } + experimental_mode_toggle->setEnabled(!conditional_experimental); experimental_mode_toggle->setDescription(e2e_description); long_personality_setting->setEnabled(true); } else { diff --git a/selfdrive/ui/qt/onroad/alerts.cc b/selfdrive/ui/qt/onroad/alerts.cc index 10d484aa6..e68d86e48 100644 --- a/selfdrive/ui/qt/onroad/alerts.cc +++ b/selfdrive/ui/qt/onroad/alerts.cc @@ -16,6 +16,7 @@ void OnroadAlerts::updateState(const UIState &s) { const UIScene &scene = s.scene; showAOLStatusBar = scene.show_aol_status_bar; + showCEMStatusBar = scene.show_cem_status_bar; } void OnroadAlerts::clear() { @@ -72,7 +73,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { int margin = 40; int radius = 30; - int offset = showAOLStatusBar ? 25 : 0; + int offset = showAOLStatusBar || showCEMStatusBar ? 25 : 0; if (alert.size == cereal::ControlsState::AlertSize::FULL) { margin = 0; radius = 0; diff --git a/selfdrive/ui/qt/onroad/alerts.h b/selfdrive/ui/qt/onroad/alerts.h index 7672ba663..a518b9782 100644 --- a/selfdrive/ui/qt/onroad/alerts.h +++ b/selfdrive/ui/qt/onroad/alerts.h @@ -42,4 +42,5 @@ protected: // FrogPilot variables bool showAOLStatusBar; + bool showCEMStatusBar; }; diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 180813f66..3a8f55d83 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -287,7 +287,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) if (rightHandDM && map_settings_btn->isEnabled()) { x -= 250; } - offset += showAlwaysOnLateralStatusBar ? 25 : 0; + offset += showAlwaysOnLateralStatusBar || showConditionalExperimentalStatusBar ? 25 : 0; int y = height() - offset; float opacity = dmActive ? 0.65 : 0.2; drawIcon(painter, QPoint(x, y), dm_img, blackColor(70), opacity); @@ -497,10 +497,15 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce alwaysOnLateralActive = scene.always_on_lateral_active; showAlwaysOnLateralStatusBar = scene.show_aol_status_bar; - if (showAlwaysOnLateralStatusBar) { + if (showAlwaysOnLateralStatusBar || showConditionalExperimentalStatusBar) { drawStatusBar(painter); } + conditionalSpeed = scene.conditional_speed; + conditionalSpeedLead = scene.conditional_speed_lead; + conditionalStatus = scene.conditional_status; + showConditionalExperimentalStatusBar = scene.show_cem_status_bar; + experimentalMode = scene.experimental_mode; mapOpen = scene.map_open; @@ -531,8 +536,23 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { p.setOpacity(1.0); p.drawRoundedRect(statusBarRect, 30, 30); + std::map conditionalStatusMap = { + {0, tr("Conditional Experimental Mode ready")}, + {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"))}, + {10, tr("Experimental Mode activated for intersection")}, + {11, tr("Experimental Mode activated for upcoming turn")}, + {12, tr("Experimental Mode activated for curve")}, + {13, tr("Experimental Mode activated for stopped lead")}, + {14, tr("Experimental Mode activated for slower lead")}, + {15, tr("Experimental Mode activated for stop light") + (mapOpen ? tr("") : tr(" or stop sign"))}, + }; + if (alwaysOnLateralActive && showAlwaysOnLateralStatusBar) { newStatus = tr("Always On Lateral active") + (mapOpen ? "" : tr(". Press the \"Cruise Control\" button to disable")); + } else if (showConditionalExperimentalStatusBar) { + newStatus = conditionalStatusMap.at(conditionalStatus); } if (newStatus != lastShownStatus) { diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 89ba14c0a..31d8bdb39 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -56,12 +56,16 @@ private: bool experimentalMode; bool mapOpen; bool showAlwaysOnLateralStatusBar; + bool showConditionalExperimentalStatusBar; float accelerationConversion; float distanceConversion; float speedConversion; int alertSize; + int conditionalSpeed; + int conditionalSpeedLead; + int conditionalStatus; QString accelerationUnit; QString leadDistanceUnit; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 619848da7..eecb05d24 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -273,6 +273,11 @@ void ui_update_frogpilot_params(UIState *s, Params ¶ms) { bool always_on_lateral = params.getBool("AlwaysOnLateral"); scene.show_aol_status_bar = always_on_lateral && !params.getBool("HideAOLStatusBar"); + scene.conditional_experimental = scene.longitudinal_control && params.getBool("ConditionalExperimental"); + scene.conditional_speed = scene.conditional_experimental ? params.getInt("CESpeed") : 0; + scene.conditional_speed_lead = scene.conditional_experimental ? params.getInt("CESpeedLead") : 0; + scene.show_cem_status_bar = scene.conditional_experimental && !params.getBool("HideCEMStatusBar"); + scene.tethering_config = params.getInt("TetheringEnabled"); if (scene.tethering_config == 2) { WifiManager(s).setTetheringEnabled(true); @@ -358,6 +363,7 @@ void UIState::update() { } // FrogPilot variables that need to be constantly updated + scene.conditional_status = scene.conditional_experimental && scene.enabled ? paramsMemory.getInt("CEStatus") : 0; } void UIState::setPrimeType(PrimeType type) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 0d27b0db1..aa64852d9 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -119,6 +119,7 @@ typedef struct UIScene { // FrogPilot variables bool always_on_lateral_active; + bool conditional_experimental; bool enabled; bool experimental_mode; bool map_open; @@ -126,9 +127,13 @@ typedef struct UIScene { bool parked; bool right_hand_drive; bool show_aol_status_bar; + bool show_cem_status_bar; bool tethering_enabled; int alert_size; + int conditional_speed; + int conditional_speed_lead; + int conditional_status; int tethering_config; } UIScene;