diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 5650f580a..911b36af2 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -26,12 +26,20 @@ struct FrogPilotNavigation @0xda96579883444c35 { } struct FrogPilotPlan @0x80ae746ee2596b11 { + accelerationJerk @0 :Float32; + accelerationJerkStock @1 :Float32; conditionalExperimental @3 :Bool; + desiredFollowDistance @4 :Int16; + egoJerk @5 :Float32; + egoJerkStock @6 :Float32; jerk @7 :Float32; laneWidthLeft @8 :Float32; laneWidthRight @9 :Float32; minAcceleration @10 :Float32; maxAcceleration @11 :Float32; + safeObstacleDistance @13 :Int16; + safeObstacleDistanceStock @14 :Int16; + stoppedEquivalenceFactor @19 :Int16; tFollow @20 :Float32; vCruise @22 :Float32; } diff --git a/common/params.cc b/common/params.cc index ade7dd91f..e4461a0aa 100644 --- a/common/params.cc +++ b/common/params.cc @@ -247,6 +247,7 @@ std::unordered_map keys = { {"CustomTheme", PERSISTENT}, {"CydiaTune", PERSISTENT}, {"DecelerationProfile", PERSISTENT}, + {"DeveloperUI", PERSISTENT}, {"DeviceManagement", PERSISTENT}, {"DeviceShutdown", PERSISTENT}, {"DisableOnroadUploads", PERSISTENT}, @@ -264,6 +265,7 @@ std::unordered_map keys = { {"IncreaseThermalLimits", PERSISTENT}, {"LaneLinesWidth", PERSISTENT}, {"LateralTune", PERSISTENT}, + {"LeadInfo", PERSISTENT}, {"LongitudinalTune", PERSISTENT}, {"LowVoltageShutdown", PERSISTENT}, {"ManualUpdateInitiated", PERSISTENT}, @@ -284,13 +286,16 @@ std::unordered_map keys = { {"RoadEdgesWidth", PERSISTENT}, {"ShowCPU", PERSISTENT}, {"ShowGPU", PERSISTENT}, + {"ShowJerk", PERSISTENT}, {"ShowMemoryUsage", PERSISTENT}, {"ShowStorageLeft", PERSISTENT}, {"ShowStorageUsed", PERSISTENT}, + {"ShowTuning", PERSISTENT}, {"StandardFollow", PERSISTENT}, {"StandardJerk", PERSISTENT}, {"StockTune", PERSISTENT}, {"UnlimitedLength", PERSISTENT}, + {"UseSI", PERSISTENT}, {"WarningImmediateVolume", PERSISTENT}, {"WarningSoftVolume", PERSISTENT}, }; diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 17d021dfc..56ab59361 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -9,7 +9,8 @@ from openpilot.common.params import Params from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, STOP_DISTANCE, get_jerk_factor, get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, J_EGO_COST, COMFORT_BRAKE, STOP_DISTANCE, 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, get_max_accel from openpilot.system.version import get_short_branch @@ -92,8 +93,14 @@ class FrogPilotPlanner: if radarState.leadOne.status and self.CP.openpilotLongitudinalControl: base_jerk = get_jerk_factor(self.custom_personalities, self.aggressive_jerk, self.standard_jerk, self.relaxed_jerk, controlsState.personality) base_t_follow = get_T_FOLLOW(self.custom_personalities, self.aggressive_follow, self.standard_follow, self.relaxed_follow, controlsState.personality) + self.safe_obstacle_distance = int(np.mean(get_safe_obstacle_distance(v_ego, self.t_follow))) + self.safe_obstacle_distance_stock = int(np.mean(get_safe_obstacle_distance(v_ego, base_t_follow))) + self.stopped_equivalence_factor = int(np.mean(get_stopped_equivalence_factor(v_lead))) self.jerk, self.t_follow = self.update_follow_values(base_jerk, radarState, base_t_follow, v_ego, v_lead) else: + self.safe_obstacle_distance = 0 + self.safe_obstacle_distance_stock = 0 + self.stopped_equivalence_factor = 0 self.t_follow = 1.45 self.v_cruise = self.update_v_cruise(carState, controlsState, controlsState.enabled, liveLocationKalman, modelData, road_curvature, v_cruise, v_ego) @@ -138,8 +145,16 @@ class FrogPilotPlanner: frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) frogpilotPlan = frogpilot_plan_send.frogpilotPlan + frogpilotPlan.accelerationJerk = A_CHANGE_COST * (float(self.jerk) if self.lead_one.status else 1) + frogpilotPlan.accelerationJerkStock = A_CHANGE_COST frogpilotPlan.conditionalExperimental = self.cem.experimental_mode + frogpilotPlan.desiredFollowDistance = self.safe_obstacle_distance - self.stopped_equivalence_factor + frogpilotPlan.egoJerk = J_EGO_COST * (float(self.jerk) if self.lead_one.status else 1) + frogpilotPlan.egoJerkStock = J_EGO_COST frogpilotPlan.jerk = float(self.jerk) + frogpilotPlan.safeObstacleDistance = self.safe_obstacle_distance + frogpilotPlan.safeObstacleDistanceStock = self.safe_obstacle_distance_stock + frogpilotPlan.stoppedEquivalenceFactor = self.stopped_equivalence_factor frogpilotPlan.laneWidthLeft = self.lane_width_left frogpilotPlan.laneWidthRight = self.lane_width_right frogpilotPlan.minAcceleration = self.min_accel diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc index 87992fed0..a6e99ad8b 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc @@ -318,6 +318,8 @@ void FrogPilotControlsPanel::updateCarToggles() { auto carFingerprint = CP.getCarFingerprint(); auto carName = CP.getCarName(); + hasAutoTune = (carName == "hyundai" || carName == "toyota") && CP.getLateralTuning().which() == cereal::CarParams::LateralTuning::TORQUE; + uiState()->scene.has_auto_tune = hasAutoTune; hasOpenpilotLongitudinal = CP.getOpenpilotLongitudinalControl() && !params.getBool("DisableOpenpilotLongitudinal"); hasPCMCruise = CP.getPcmCruise(); isToyota = carName == "toyota"; diff --git a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc index abe03f96e..d667b31d1 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc @@ -1,6 +1,9 @@ #include "selfdrive/frogpilot/ui/qt/offroad/visual_settings.h" FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilotListWidget(parent) { + std::string branch = params.get("GitBranch"); + isRelease = branch == "FrogPilot"; + const std::vector> visualToggles { {"AlertVolumeControl", tr("Alert Volume Controller"), tr("Control the volume level for each individual sound in openpilot."), "../frogpilot/assets/toggle_icons/icon_mute.png"}, {"DisengageVolume", tr("Disengage Volume"), tr("Related alerts:\n\nAdaptive Cruise Disabled\nParking Brake Engaged\nBrake Pedal Pressed\nSpeed too Low"), ""}, @@ -14,6 +17,8 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot {"CustomAlerts", tr("Custom Alerts"), tr("Enable custom alerts for openpilot events."), "../frogpilot/assets/toggle_icons/icon_green_light.png"}, {"CustomUI", tr("Custom Onroad UI"), tr("Customize the Onroad UI."), "../assets/offroad/icon_road.png"}, + {"DeveloperUI", tr("Developer UI"), tr("Get various detailed information of what openpilot is doing behind the scenes."), ""}, + {"LeadInfo", tr("Lead Info and Logics"), tr("Get detailed information about the vehicle ahead, including speed and distance, and the logic behind your following distance."), ""}, {"CustomPaths", tr("Paths"), tr("Show your projected acceleration on the driving path, detected adjacent lanes, or when a vehicle is detected in your blindspot."), ""}, {"CustomTheme", tr("Custom Themes"), tr("Enable the ability to use custom themes."), "../frogpilot/assets/wheel_images/frog.png"}, @@ -99,10 +104,26 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot for (auto &[key, toggle] : toggles) { std::set modifiedCustomOnroadUIKeys = customOnroadUIKeys; + if (!hasOpenpilotLongitudinal && !hasAutoTune || isRelease) { + modifiedCustomOnroadUIKeys.erase("DeveloperUI"); + } + + if (!hasOpenpilotLongitudinal || !isRelease) { + modifiedCustomOnroadUIKeys.erase("LeadInfo"); + } + toggle->setVisible(modifiedCustomOnroadUIKeys.find(key.c_str()) != modifiedCustomOnroadUIKeys.end()); } }); toggle = customUIToggle; + } else if (param == "DeveloperUI") { + std::vector developerUIToggles{"LeadInfo", "ShowTuning", "ShowJerk", "UseSI"}; + std::vector developerUIToggleNames{tr("Lead Info"), tr("Live Tuning"), tr("Jerk"), tr("Use SI")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, developerUIToggles, developerUIToggleNames); + } else if (param == "LeadInfo") { + std::vector leadInfoToggles{"UseSI"}; + std::vector leadInfoToggleNames{tr("Use SI Values")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, leadInfoToggles, leadInfoToggleNames); } else if (param == "CustomPaths") { std::vector pathToggles{"AccelerationPath", "BlindSpotPath"}; std::vector pathToggleNames{tr("Acceleration"), tr("Blind Spot")}; @@ -204,7 +225,9 @@ void FrogPilotVisualsPanel::updateCarToggles() { AlignedBuffer aligned_buf; capnp::FlatArrayMessageReader cmsg(aligned_buf.align(carParams.data(), carParams.size())); cereal::CarParams::Reader CP = cmsg.getRoot(); + auto carName = CP.getCarName(); + hasAutoTune = (carName == "hyundai" || carName == "toyota") && CP.getLateralTuning().which() == cereal::CarParams::LateralTuning::TORQUE; hasBSM = CP.getEnableBsm(); hasOpenpilotLongitudinal = CP.getOpenpilotLongitudinalControl() && !params.getBool("DisableOpenpilotLongitudinal"); } else { diff --git a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h index a81a77f17..5df42050b 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h @@ -25,7 +25,7 @@ private: std::set alertVolumeControlKeys = {"DisengageVolume", "EngageVolume", "PromptDistractedVolume", "PromptVolume", "RefuseVolume", "WarningImmediateVolume", "WarningSoftVolume"}; std::set customAlertsKeys = {}; - std::set customOnroadUIKeys = {"CustomPaths"}; + std::set customOnroadUIKeys = {"CustomPaths", "DeveloperUI", "LeadInfo"}; std::set customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds"}; std::set modelUIKeys = {"DynamicPathWidth", "HideLeadMarker", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"}; std::set qolKeys = {"CameraView"}; @@ -36,8 +36,10 @@ private: Params params; Params paramsMemory{"/dev/shm/params"}; + bool hasAutoTune; bool hasBSM; bool hasOpenpilotLongitudinal; bool isMetric = params.getBool("IsMetric"); + bool isRelease; bool started; }; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 413023077..534440b70 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -148,6 +148,43 @@ void OnroadWindow::primeChanged(bool prime) { void OnroadWindow::paintEvent(QPaintEvent *event) { QPainter p(this); p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255)); + + QString logicsDisplayString = QString(); + if (scene.show_jerk) { + logicsDisplayString += QString("Acceleration Jerk: %1 (%2%3) | Speed Jerk: %4 (%5%6) | ") + .arg(scene.acceleration_jerk, 0, 'f', 3) + .arg(scene.acceleration_jerk_difference > 0 ? "-" : "", 0) + .arg(abs(scene.acceleration_jerk_difference), 0, 'f', 3) + .arg(scene.ego_jerk, 0, 'f', 3) + .arg(scene.ego_jerk_difference > 0 ? "-" : "", 0) + .arg(abs(scene.ego_jerk_difference), 0, 'f', 3); + } + if (scene.show_tuning) { + if (!scene.live_valid) { + logicsDisplayString += "Friction: Calculating... | Lateral Acceleration: Calculating..."; + } else { + logicsDisplayString += QString("Friction: %1 | Lateral Acceleration: %2") + .arg(scene.friction, 0, 'f', 3) + .arg(scene.lat_accel, 0, 'f', 3); + } + } + if (logicsDisplayString.endsWith(" | ")) { + logicsDisplayString.chop(3); + } + + if (!logicsDisplayString.isEmpty()) { + p.setFont(InterFont(28, QFont::DemiBold)); + p.setRenderHint(QPainter::TextAntialiasing); + p.setPen(Qt::white); + + QRect currentRect = rect(); + int logicsWidth = p.fontMetrics().horizontalAdvance(logicsDisplayString); + int logicsX = (currentRect.width() - logicsWidth) / 2; + int logicsY = currentRect.top() + 27; + + p.drawText(logicsX, logicsY, logicsDisplayString); + update(); + } } // ***** onroad widgets ***** @@ -221,7 +258,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { // ExperimentalButton ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(false), engageable(false), QPushButton(parent), scene(uiState()->scene) { - setFixedSize(btn_size, btn_size); + setFixedSize(btn_size, btn_size + 10); engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size}); experimental_img = loadPixmap("../assets/img_experimental.svg", {img_size, img_size}); @@ -236,7 +273,7 @@ void ExperimentalButton::changeMode() { } } -void ExperimentalButton::updateState(const UIState &s) { +void ExperimentalButton::updateState(const UIState &s, bool leadInfo) { const auto cs = (*s.sm)["controlsState"].getControlsState(); bool eng = cs.getEngageable() || cs.getEnabled() || scene.always_on_lateral_active; if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) { @@ -246,12 +283,13 @@ void ExperimentalButton::updateState(const UIState &s) { } // FrogPilot variables + y_offset = leadInfo ? 10 : 0; } void ExperimentalButton::paintEvent(QPaintEvent *event) { QPainter p(this); QPixmap img = experimental_mode ? experimental_img : engage_img; - drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, QColor(0, 0, 0, 166), (isDown() || !engageable) ? 0.6 : 1.0); + drawIcon(p, QPoint(btn_size / 2, btn_size / 2 + y_offset), img, QColor(0, 0, 0, 166), (isDown() || !engageable) ? 0.6 : 1.0); } @@ -327,7 +365,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { status = s.status; // update engageability/experimental mode button - experimental_btn->updateState(s); + experimental_btn->updateState(s, leadInfo); // update DM icon auto dm_state = sm["driverMonitoringState"].getDriverMonitoringState(); @@ -700,6 +738,24 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState } painter.drawPolygon(chevron, std::size(chevron)); + if (leadInfo) { + float lead_speed = std::max(lead_data.getVLead(), 0.0f); + + painter.setPen(Qt::white); + painter.setFont(InterFont(35, QFont::Bold)); + + QString text = QString("%1 %2 | %3 %4") + .arg(qRound(d_rel * distanceConversion)) + .arg(leadDistanceUnit) + .arg(qRound(lead_speed * speedConversion)) + .arg(leadSpeedUnit); + + QFontMetrics metrics(painter.font()); + int middle_x = (chevron[2].x() + chevron[0].x()) / 2; + int textWidth = metrics.horizontalAdvance(text); + painter.drawText(middle_x - textWidth / 2, chevron[0].y() + metrics.height() + 5, text); + } + painter.restore(); } @@ -854,6 +910,10 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() { experimentalMode = scene.experimental_mode; + leadInfo = scene.lead_info; + obstacleDistance = scene.obstacle_distance; + obstacleDistanceStock = scene.obstacle_distance_stock; + mapOpen = scene.map_open; turnSignalLeft = scene.turn_signal_left; @@ -904,6 +964,10 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) { animationTimer->stop(); } + if (leadInfo) { + drawLeadInfo(p); + } + map_settings_btn_bottom->setEnabled(map_settings_btn->isEnabled()); if (map_settings_btn_bottom->isEnabled()) { map_settings_btn_bottom->setVisible(!hideBottomIcons); @@ -911,6 +975,107 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) { } } +void AnnotatedCameraWidget::drawLeadInfo(QPainter &p) { + static QElapsedTimer timer; + static bool isFiveSecondsPassed = false; + static double maxAcceleration = 0.0; + constexpr int maxAccelDuration = 5000; + + QString accelerationUnit = tr(" m/s²"); + leadDistanceUnit = tr(mapOpen ? "m" : "meters"); + leadSpeedUnit = tr("m/s"); + + float accelerationConversion = 1.0f; + distanceConversion = 1.0f; + speedConversion = 1.0f; + + if (!scene.use_si) { + if (is_metric) { + leadSpeedUnit = tr("kph"); + speedConversion = MS_TO_KPH; + } else { + accelerationUnit = tr(" ft/s²"); + leadDistanceUnit = tr(mapOpen ? "ft" : "feet"); + leadSpeedUnit = tr("mph"); + + accelerationConversion = METER_TO_FOOT; + distanceConversion = METER_TO_FOOT; + speedConversion = MS_TO_MPH; + } + } + + double acceleration = std::round(scene.acceleration * 100) / 100; + + if (acceleration > maxAcceleration && status == STATUS_ENGAGED) { + maxAcceleration = acceleration; + isFiveSecondsPassed = false; + timer.start(); + } else { + isFiveSecondsPassed = timer.hasExpired(maxAccelDuration); + } + + auto createText = [&](const QString &title, const double data) { + return title + QString::number(std::round(data * distanceConversion)) + " " + leadDistanceUnit; + }; + + QString accelText = QString(tr("Accel: %1%2")) + .arg(acceleration * accelerationConversion, 0, 'f', 2) + .arg(accelerationUnit); + + QString maxAccSuffix; + if (!mapOpen) { + maxAccSuffix = tr(" - Max: %1%2") + .arg(maxAcceleration * accelerationConversion, 0, 'f', 2) + .arg(accelerationUnit); + } + + QString obstacleText = createText(mapOpen ? tr(" | Obstacle: ") : tr(" | Obstacle Factor: "), obstacleDistance); + QString stopText = createText(mapOpen ? tr(" - Stop: ") : tr(" - Stop Factor: "), scene.stopped_equivalence); + QString followText = " = " + createText(mapOpen ? tr("Follow: ") : tr("Follow Distance: "), scene.desired_follow); + + auto createDiffText = [&](const double data, const double stockData) { + double difference = std::round((data - stockData) * distanceConversion); + return difference != 0 ? QString(" (%1%2)").arg(difference > 0 ? "+" : "").arg(difference) : QString(); + }; + + p.save(); + + QRect insightsRect(rect().left() - 1, rect().top() - 60, rect().width() + 2, 100); + p.setBrush(QColor(0, 0, 0, 150)); + p.drawRoundedRect(insightsRect, 30, 30); + p.setFont(InterFont(28, QFont::Bold)); + p.setRenderHint(QPainter::TextAntialiasing); + + QRect adjustedRect = insightsRect.adjusted(0, 27, 0, 27); + int textBaseLine = adjustedRect.y() + (adjustedRect.height() + p.fontMetrics().height()) / 2 - p.fontMetrics().descent(); + + int totalTextWidth = p.fontMetrics().horizontalAdvance(accelText) + + p.fontMetrics().horizontalAdvance(maxAccSuffix) + + p.fontMetrics().horizontalAdvance(obstacleText) + + p.fontMetrics().horizontalAdvance(createDiffText(obstacleDistance, obstacleDistanceStock)) + + p.fontMetrics().horizontalAdvance(stopText) + + p.fontMetrics().horizontalAdvance(followText); + + int textStartPos = adjustedRect.x() + (adjustedRect.width() - totalTextWidth) / 2; + + auto drawText = [&](const QString &text, const QColor &color) { + p.setPen(color); + p.drawText(textStartPos, textBaseLine, text); + textStartPos += p.fontMetrics().horizontalAdvance(text); + }; + + drawText(accelText, Qt::white); + if (!maxAccSuffix.isEmpty()) { + drawText(maxAccSuffix, isFiveSecondsPassed ? Qt::white : Qt::red); + } + drawText(obstacleText, Qt::white); + drawText(createDiffText(obstacleDistance, obstacleDistanceStock), (obstacleDistance - obstacleDistanceStock) > 0 ? Qt::green : Qt::red); + drawText(stopText, Qt::white); + drawText(followText, Qt::white); + + p.restore(); +} + void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { p.save(); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index b66a10329..92470efb6 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -39,7 +39,7 @@ class ExperimentalButton : public QPushButton { public: explicit ExperimentalButton(QWidget *parent = 0); - void updateState(const UIState &s); + void updateState(const UIState &s, bool leadInfo); private: void paintEvent(QPaintEvent *event) override; @@ -54,6 +54,8 @@ private: // FrogPilot variables Params paramsMemory{"/dev/shm/params"}; UIScene &scene; + + int y_offset; }; @@ -110,6 +112,7 @@ private: void paintFrogPilotWidgets(QPainter &p); void updateFrogPilotWidgets(); + void drawLeadInfo(QPainter &p); void drawStatusBar(QPainter &p); void drawTurnSignals(QPainter &p); @@ -123,19 +126,28 @@ private: bool blindSpotLeft; bool blindSpotRight; bool experimentalMode; + bool leadInfo; bool mapOpen; bool showAlwaysOnLateralStatusBar; bool showConditionalExperimentalStatusBar; bool turnSignalLeft; bool turnSignalRight; + float distanceConversion; + float speedConversion; + int alertSize; int cameraView; int conditionalStatus; int customColors; int customSignals; + int obstacleDistance; + int obstacleDistanceStock; int totalFrames = 8; + QString leadDistanceUnit; + QString leadSpeedUnit; + size_t animationFrameIndex; std::unordered_map>> themeConfiguration; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index a43913b88..a334f5072 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -244,12 +244,26 @@ static void update_state(UIState *s) { } if (sm.updated("frogpilotPlan")) { auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan(); + scene.acceleration_jerk = frogpilotPlan.getAccelerationJerk(); + scene.acceleration_jerk_difference = frogpilotPlan.getAccelerationJerkStock() - scene.acceleration_jerk; + scene.desired_follow = frogpilotPlan.getDesiredFollowDistance(); + scene.ego_jerk = frogpilotPlan.getEgoJerk(); + scene.ego_jerk_difference = frogpilotPlan.getEgoJerkStock() - scene.ego_jerk; scene.lane_width_left = frogpilotPlan.getLaneWidthLeft(); scene.lane_width_right = frogpilotPlan.getLaneWidthRight(); + scene.obstacle_distance = frogpilotPlan.getSafeObstacleDistance(); + scene.obstacle_distance_stock = frogpilotPlan.getSafeObstacleDistanceStock(); + scene.stopped_equivalence = frogpilotPlan.getStoppedEquivalenceFactor(); } if (sm.updated("liveLocationKalman")) { auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); } + if (sm.updated("liveTorqueParameters")) { + auto torque_params = sm["liveTorqueParameters"].getLiveTorqueParameters(); + scene.friction = torque_params.getFrictionCoefficientFiltered(); + scene.lat_accel = torque_params.getLatAccelFactorFiltered(); + scene.live_valid = torque_params.getLiveValid(); + } if (sm.updated("wideRoadCameraState")) { auto cam_state = sm["wideRoadCameraState"].getWideRoadCameraState(); float scale = (cam_state.getSensor() == cereal::FrameData::ImageSensor::AR0231) ? 6.0f : 1.0f; @@ -274,6 +288,9 @@ void ui_update_frogpilot_params(UIState *s) { Params params = Params(); UIScene &scene = s->scene; + std::string branch = params.get("GitBranch"); + bool isRelease = branch == "FrogPilot"; + bool always_on_lateral = params.getBool("AlwaysOnLateral"); scene.show_aol_status_bar = always_on_lateral && !params.getBool("HideAOLStatusBar"); @@ -284,8 +301,13 @@ void ui_update_frogpilot_params(UIState *s) { bool custom_onroad_ui = params.getBool("CustomUI"); bool custom_paths = custom_onroad_ui && params.getBool("CustomPaths"); + bool developer_ui = !isRelease && custom_onroad_ui && params.getBool("DeveloperUI"); scene.acceleration_path = custom_paths && params.getBool("AccelerationPath"); scene.blind_spot_path = custom_paths && params.getBool("BlindSpotPath"); + scene.lead_info = scene.longitudinal_control && custom_onroad_ui && params.getBool("LeadInfo"); + scene.show_jerk = scene.longitudinal_control && developer_ui && params.getBool("ShowJerk"); + scene.show_tuning = scene.has_auto_tune && developer_ui && params.getBool("ShowTuning"); + scene.use_si = (scene.lead_info || developer_ui) && params.getBool("UseSI"); bool custom_theme = params.getBool("CustomTheme"); scene.custom_colors = custom_theme ? params.getInt("CustomColors") : 0; @@ -336,7 +358,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { sm = std::make_unique>({ "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2", - "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan", + "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan", "liveTorqueParameters", "frogpilotCarControl", "frogpilotDeviceState", "frogpilotPlan", }); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 71608540c..cbd7cd5e2 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -180,20 +180,32 @@ typedef struct UIScene { bool dynamic_path_width; bool enabled; bool experimental_mode; + bool has_auto_tune; bool hide_lead_marker; + bool lead_info; + bool live_valid; bool map_open; bool model_ui; bool right_hand_drive; bool show_aol_status_bar; bool show_cem_status_bar; + bool show_jerk; + bool show_tuning; bool turn_signal_left; bool turn_signal_right; bool unlimited_road_ui_length; + bool use_si; float acceleration; + float acceleration_jerk; + float acceleration_jerk_difference; + float ego_jerk; + float ego_jerk_difference; + float friction; float lane_line_width; float lane_width_left; float lane_width_right; + float lat_accel; float path_edge_width; float path_width; float road_edge_width; @@ -206,6 +218,10 @@ typedef struct UIScene { int custom_colors; int custom_icons; int custom_signals; + int desired_follow; + int obstacle_distance; + int obstacle_distance_stock; + int stopped_equivalence; QPolygonF track_adjacent_vertices[6]; QPolygonF track_edge_vertices;