diff --git a/cereal/custom.capnp b/cereal/custom.capnp index da99881..ce63df7 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -13,17 +13,25 @@ struct FrogPilotCarControl @0x81c2f05a394cf4af { } struct FrogPilotDeviceState @0xaedffd8f31e7b55d { + freeSpace @0 :Int16; + usedSpace @1 :Int16; } enum FrogPilotEvents @0xf35cc4560bbf6ec2 { } struct FrogPilotLateralPlan @0xda96579883444c35 { + laneWidthLeft @0 :Float32; + laneWidthRight @1 :Float32; } struct FrogPilotLongitudinalPlan @0x80ae746ee2596b11 { conditionalExperimental @1 :Bool; + desiredFollowDistance @2 :Int16; distances @3 :List(Float32); + safeObstacleDistance @5 :Int16; + safeObstacleDistanceStock @6 :Int16; + stoppedEquivalenceFactor @11 :Int16; } struct FrogPilotNavigation @0xa5cd762cd951a455 { diff --git a/common/params.cc b/common/params.cc index 1fb1df7..6275d79 100644 --- a/common/params.cc +++ b/common/params.cc @@ -212,7 +212,9 @@ std::unordered_map keys = { {"WheeledBody", PERSISTENT}, // FrogPilot parameters + {"AccelerationPath", PERSISTENT}, {"AccelerationProfile", PERSISTENT}, + {"AdjacentPath", PERSISTENT}, {"AggressiveAcceleration", PERSISTENT}, {"AggressiveFollow", PERSISTENT}, {"AggressiveJerk", PERSISTENT}, @@ -220,6 +222,8 @@ std::unordered_map keys = { {"AlwaysOnLateralMain", PERSISTENT}, {"ApiCache_DriveStats", PERSISTENT}, {"AverageCurvature", PERSISTENT}, + {"BlindSpotPath", PERSISTENT}, + {"CameraFPS", PERSISTENT}, {"CameraView", PERSISTENT}, {"CECurves", PERSISTENT}, {"CECurvesLead", PERSISTENT}, @@ -233,18 +237,34 @@ std::unordered_map keys = { {"CEStopLightsLead", PERSISTENT}, {"ConditionalExperimental", PERSISTENT}, {"CustomPersonalities", PERSISTENT}, + {"CustomUI", PERSISTENT}, {"FrogPilotTogglesUpdated", PERSISTENT}, {"GasRegenCmd", PERSISTENT}, + {"LaneLinesWidth", PERSISTENT}, {"LateralTune", PERSISTENT}, + {"LeadInfo", PERSISTENT}, {"LongitudinalTune", PERSISTENT}, + {"ModelUI", PERSISTENT}, {"OfflineMode", PERSISTENT}, + {"PathEdgeWidth", PERSISTENT}, + {"PathWidth", PERSISTENT}, {"RelaxedFollow", PERSISTENT}, {"RelaxedJerk", PERSISTENT}, + {"RoadEdgesWidth", PERSISTENT}, + {"ShowCPU", PERSISTENT}, + {"ShowFPS", PERSISTENT}, + {"ShowGPU", PERSISTENT}, + {"ShowMemoryUsage", PERSISTENT}, + {"ShowStorageLeft", PERSISTENT}, + {"ShowStorageUsed", PERSISTENT}, {"StandardFollow", PERSISTENT}, {"StandardJerk", PERSISTENT}, + {"UnlimitedLength", PERSISTENT}, {"Updated", PERSISTENT}, {"UpdateSchedule", PERSISTENT}, {"UpdateTime", PERSISTENT}, + {"UseSI", PERSISTENT}, + {"WideCamera", PERSISTENT}, }; } // namespace diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index b13ea6d..0dc991c 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -2,6 +2,8 @@ from cereal import log from openpilot.common.conversions import Conversions as CV from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.frogpilot.functions.frogpilot_planner import calculate_lane_width + LaneChangeState = log.LateralPlan.LaneChangeState LaneChangeDirection = log.LateralPlan.LaneChangeDirection @@ -47,6 +49,16 @@ class DesireHelper: one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN + # Calculate left and right lane widths for the blindspot path + turning = abs(carstate.steeringAngleDeg) >= 60 + if frogpilot_planner.blindspot_path and not below_lane_change_speed and not turning: + # Calculate left and right lane widths + 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]) + else: + self.lane_width_left = 0 + self.lane_width_right = 0 + if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 4a3fc29..43e2574 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -366,6 +366,16 @@ class LongitudinalMpc: t_follow_offset = np.clip((lead_xv_0[:,1] - v_ego) + (STOP_DISTANCE - v_ego), 1, distance_factor) t_follow = t_follow / t_follow_offset + # LongitudinalPlan variables for onroad driving insights + if self.status: + self.safe_obstacle_distance = int(np.mean(get_safe_obstacle_distance(v_ego, t_follow))) + self.safe_obstacle_distance_stock = int(np.mean(get_safe_obstacle_distance(v_ego, self.t_follow))) + self.stopped_equivalence_factor = int(np.mean(get_stopped_equivalence_factor(lead_xv_0[:,1]))) + else: + self.safe_obstacle_distance = 0 + self.safe_obstacle_distance_stock = 0 + self.stopped_equivalence_factor = 0 + # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance # and then treat that as a stopped car/obstacle at this new distance. diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 7c7b14d..4aa9cb0 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -34,6 +34,19 @@ def get_min_accel_sport_tune(v_ego): def get_max_accel_sport_tune(v_ego): return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT_TUNE) +def calculate_lane_width(lane, current_lane, road_edge): + lane_x, lane_y = np.array(lane.x), np.array(lane.y) + edge_x, edge_y = np.array(road_edge.x), np.array(road_edge.y) + current_x, current_y = np.array(current_lane.x), np.array(current_lane.y) + + lane_y_interp = np.interp(current_x, lane_x[lane_x.argsort()], lane_y[lane_x.argsort()]) + road_edge_y_interp = np.interp(current_x, edge_x[edge_x.argsort()], edge_y[edge_x.argsort()]) + + distance_to_lane = np.mean(np.abs(current_y - lane_y_interp)) + distance_to_road_edge = np.mean(np.abs(current_y - road_edge_y_interp)) + + return min(distance_to_lane, distance_to_road_edge) + class FrogPilotPlanner: def __init__(self, params): @@ -80,6 +93,9 @@ class FrogPilotPlanner: frogpilot_lateral_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2']) frogpilotLateralPlan = frogpilot_lateral_plan_send.frogpilotLateralPlan + frogpilotLateralPlan.laneWidthLeft = float(DH.lane_width_left) + frogpilotLateralPlan.laneWidthRight = float(DH.lane_width_right) + pm.send('frogpilotLateralPlan', frogpilot_lateral_plan_send) def publish_longitudinal(self, sm, pm, mpc): @@ -90,11 +106,18 @@ class FrogPilotPlanner: frogpilotLongitudinalPlan.conditionalExperimental = self.cem.experimental_mode frogpilotLongitudinalPlan.distances = self.x_desired_trajectory.tolist() + frogpilotLongitudinalPlan.safeObstacleDistance = mpc.safe_obstacle_distance + frogpilotLongitudinalPlan.stoppedEquivalenceFactor = mpc.stopped_equivalence_factor + frogpilotLongitudinalPlan.desiredFollowDistance = mpc.safe_obstacle_distance - mpc.stopped_equivalence_factor + frogpilotLongitudinalPlan.safeObstacleDistanceStock = mpc.safe_obstacle_distance_stock + pm.send('frogpilotLongitudinalPlan', frogpilot_longitudinal_plan_send) def update_frogpilot_params(self, params): self.is_metric = params.get_bool("IsMetric") + self.blindspot_path = params.get_bool("CustomUI") and params.get_bool("BlindSpotPath") + self.conditional_experimental_mode = params.get_bool("ConditionalExperimental") if self.conditional_experimental_mode: self.cem.update_frogpilot_params(self.is_metric, params) diff --git a/selfdrive/frogpilot/ui/visual_settings.cc b/selfdrive/frogpilot/ui/visual_settings.cc index e7ef0ee..cf453b4 100644 --- a/selfdrive/frogpilot/ui/visual_settings.cc +++ b/selfdrive/frogpilot/ui/visual_settings.cc @@ -4,6 +4,20 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilotListWidget(parent) { const std::vector> visualToggles { {"CameraView", "Camera View", "Choose your preferred camera view for the onroad UI. This is a visual change only and doesn't impact openpilot.", "../frogpilot/assets/toggle_icons/icon_camera.png"}, + + {"CustomUI", "Custom Onroad UI", "Customize the Onroad UI with some additional visual functions.", "../assets/offroad/icon_road.png"}, + {"AdjacentPath", "Adjacent Paths", "Display paths to the left and right of your car, visualizing where the model detects lanes.", ""}, + {"BlindSpotPath", "Blind Spot Path", "Visualize your blind spots with a red path when another vehicle is detected nearby.", ""}, + {"ShowFPS", "FPS Counter", "Display the Frames Per Second (FPS) of your onroad UI for monitoring system performance.", ""}, + {"LeadInfo", "Lead Info and Logics", "Get detailed information about the vehicle ahead, including speed and distance, and the logic behind your following distance.", ""}, + + {"ModelUI", "Model UI", "Personalize how the model's visualizations appear on your screen.", "../assets/offroad/icon_calibration.png"}, + {"AccelerationPath", "Acceleration Path", "Visualize the car's intended acceleration or deceleration with a color-coded path.", ""}, + {"LaneLinesWidth", "Lane Lines", "Adjust the visual thickness of lane lines on your display.\n\nDefault matches the MUTCD average of 4 inches.", ""}, + {"PathEdgeWidth", "Path Edges", "Adjust the width of the path edges shown on your UI to represent different driving modes and statuses.\n\nDefault is 20% of the total path.\n\nBlue = Navigation\nLight Blue = Always On Lateral\nGreen = Default with 'FrogPilot Colors'\nLight Green = Default with stock colors\nOrange = Experimental Mode Active\nYellow = Conditional Overriden", ""}, + {"PathWidth", "Path Width", "Customize the width of the driving path shown on your UI.\n\nDefault matches the width of a 2019 Lexus ES 350.", ""}, + {"RoadEdgesWidth", "Road Edges", "Adjust the visual thickness of road edges on your display.\n\nDefault is 1/2 of the MUTCD average lane line width of 4 inches.", ""}, + {"UnlimitedLength", "'Unlimited' Road UI Length", "Extend the display of the path, lane lines, and road edges as far as the system can detect, providing a more expansive view of the road ahead.", ""}, }; for (const auto &[param, title, desc, icon] : visualToggles) { @@ -14,6 +28,36 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot FrogPilotButtonParamControl *preferredCamera = new FrogPilotButtonParamControl(param, title, desc, icon, cameraOptions); toggle = preferredCamera; + } else if (param == "CustomUI") { + FrogPilotParamManageControl *customUIToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(customUIToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(customOnroadUIKeys.find(key.c_str()) != customOnroadUIKeys.end()); + } + }); + toggle = customUIToggle; + } else if (param == "LeadInfo") { + std::vector leadInfoToggles{tr("UseSI")}; + std::vector leadInfoToggleNames{tr("Use SI Values")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, leadInfoToggles, leadInfoToggleNames); + + } else if (param == "ModelUI") { + FrogPilotParamManageControl *modelUIToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(modelUIToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(modelUIKeys.find(key.c_str()) != modelUIKeys.end()); + } + }); + toggle = modelUIToggle; + } else if (param == "LaneLinesWidth" || param == "RoadEdgesWidth") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 24, std::map(), this, false, " inches"); + } else if (param == "PathEdgeWidth") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 100, std::map(), this, false, "%"); + } else if (param == "PathWidth") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 100, std::map(), this, false, " feet", 10); + } else { toggle = new ParamControl(param, title, desc, icon, this); } @@ -38,9 +82,9 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot }); } - customOnroadUIKeys = {}; + customOnroadUIKeys = {"AdjacentPath", "BlindSpotPath", "ShowFPS", "LeadInfo"}; customThemeKeys = {}; - modelUIKeys = {}; + modelUIKeys = {"AccelerationPath", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"}; QObject::connect(device(), &Device::interactiveTimeout, this, &FrogPilotVisualsPanel::hideSubToggles); QObject::connect(parent, &SettingsWindow::closeParentToggle, this, &FrogPilotVisualsPanel::hideSubToggles); @@ -65,12 +109,34 @@ void FrogPilotVisualsPanel::updateMetric() { if (isMetric != previousIsMetric) { double distanceConversion = isMetric ? INCH_TO_CM : CM_TO_INCH; double speedConversion = isMetric ? FOOT_TO_METER : METER_TO_FOOT; + params.putInt("LaneLinesWidth", std::nearbyint(params.getInt("LaneLinesWidth") * distanceConversion)); + params.putInt("RoadEdgesWidth", std::nearbyint(params.getInt("RoadEdgesWidth") * distanceConversion)); + params.putInt("PathWidth", std::nearbyint(params.getInt("PathWidth") * speedConversion)); } + FrogPilotParamValueControl *laneLinesWidthToggle = static_cast(toggles["LaneLinesWidth"]); + FrogPilotParamValueControl *roadEdgesWidthToggle = static_cast(toggles["RoadEdgesWidth"]); + FrogPilotParamValueControl *pathWidthToggle = static_cast(toggles["PathWidth"]); + if (isMetric) { + laneLinesWidthToggle->setDescription("Customize the lane line width.\n\nDefault matches the Vienna average of 10 centimeters."); + roadEdgesWidthToggle->setDescription("Customize the road edges width.\n\nDefault is 1/2 of the Vienna average lane line width of 10 centimeters."); + + laneLinesWidthToggle->updateControl(0, 60, " centimeters"); + roadEdgesWidthToggle->updateControl(0, 60, " centimeters"); + pathWidthToggle->updateControl(0, 30, " meters"); } else { + laneLinesWidthToggle->setDescription("Customize the lane line width.\n\nDefault matches the MUTCD average of 4 inches."); + roadEdgesWidthToggle->setDescription("Customize the road edges width.\n\nDefault is 1/2 of the MUTCD average lane line width of 4 inches."); + + laneLinesWidthToggle->updateControl(0, 24, " inches"); + roadEdgesWidthToggle->updateControl(0, 24, " inches"); + pathWidthToggle->updateControl(0, 100, " feet"); } + laneLinesWidthToggle->refresh(); + roadEdgesWidthToggle->refresh(); + previousIsMetric = isMetric; } diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 7434e94..e59f520 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -96,10 +96,28 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, PLAN_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx+1] + (1 - p) * ModelConstants.T_IDXS[tidx] # lane lines - modelV2.init('laneLines', 4) - for i in range(4): - lane_line = modelV2.laneLines[i] - fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1]) + modelV2.init('laneLines', 6) + for i in range(6): + if i < 4: + lane_line = modelV2.laneLines[i] + fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1]) + else: + lane_line = modelV2.laneLines[i] + far_lane, near_lane, road_edge = (0, 1, 0) if i == 4 else (3, 2, 1) + + y_min = net_output_data['lane_lines'][0, near_lane,:,0] + z_min = net_output_data['lane_lines'][0, near_lane,:,1] + lane_diff = np.abs(net_output_data['lane_lines'][0,near_lane] - net_output_data['lane_lines'][0,far_lane]) + road_edge_diff = np.abs(net_output_data['lane_lines'][0,near_lane] - net_output_data['road_edges'][0,road_edge]) + + y_min += np.where(lane_diff[:,0] < road_edge_diff[:,0], net_output_data['lane_lines'][0,far_lane,:,0], net_output_data['road_edges'][0,road_edge,:,0]) + z_min += np.where(lane_diff[:,1] < road_edge_diff[:,1], net_output_data['lane_lines'][0,far_lane,:,1], net_output_data['road_edges'][0,road_edge,:,1]) + + y_min /= 2 + z_min /= 2 + + fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), y_min, z_min) + modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist() modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist() diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 3854c53..413b329 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -21,7 +21,7 @@ from openpilot.common.params import Params from openpilot.common.realtime import DT_TRML from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert from openpilot.system.hardware import HARDWARE, TICI, AGNOS -from openpilot.system.loggerd.config import get_available_percent +from openpilot.system.loggerd.config import get_available_bytes, get_available_percent, get_used_bytes from openpilot.selfdrive.statsd import statlog from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.thermald.power_monitoring import PowerMonitoring @@ -247,6 +247,9 @@ def thermald_thread(end_event, hw_queue) -> None: fpmsg = messaging.new_message('frogpilotDeviceState') + fpmsg.frogpilotDeviceState.freeSpace = round(get_available_bytes(default=32.0 * (2 ** 30)) / (2 ** 30)) + fpmsg.frogpilotDeviceState.usedSpace = round(get_used_bytes(default=0.0 * (2 ** 30)) / (2 ** 30)) + pm.send("frogpilotDeviceState", fpmsg) msg.deviceState.freeSpacePercent = get_available_percent(default=100.0) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 75d26ea..d1b3cd0 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -153,6 +153,61 @@ void OnroadWindow::paintEvent(QPaintEvent *event) { QPainter p(this); p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255)); + + // Draw FPS on screen + if (scene.show_fps) { + constexpr double minAllowedFPS = 0.1; + constexpr double maxAllowedFPS = 99.9; + constexpr qint64 oneMinuteInMilliseconds = 60000; + static double avgFPS; + static double maxFPS; + static double minFPS; + static double totalFPS; + static qint64 frameCount; + + // Store the last reset time + static qint64 lastResetTime = QDateTime::currentMSecsSinceEpoch(); + const qint64 currentMillis = QDateTime::currentMSecsSinceEpoch(); + + // Reset the counter if it's been 60 seconds + if (currentMillis - lastResetTime >= oneMinuteInMilliseconds) { + avgFPS = 0; + frameCount = 0; + minFPS = maxAllowedFPS; + maxFPS = minAllowedFPS; + totalFPS = 0; + lastResetTime = currentMillis; + } + + // Update the FPS variables + fps = qBound(minAllowedFPS, fps, maxAllowedFPS); + minFPS = qMin(minFPS, fps); + maxFPS = qMax(maxFPS, fps); + frameCount++; + totalFPS += fps; + avgFPS = totalFPS / frameCount; + update(); + + // Text declarations + p.setFont(InterFont(30, QFont::DemiBold)); + p.setRenderHint(QPainter::TextAntialiasing); + p.setPen(Qt::white); + + // Construct the FPS display string + QString fpsDisplayString = QString("FPS: %1 (%2) | Min: %3 | Max: %4 | Avg: %5") + .arg(fps, 0, 'f', 2) + .arg(paramsMemory.getInt("CameraFPS")) + .arg(minFPS, 0, 'f', 2) + .arg(maxFPS, 0, 'f', 2) + .arg(avgFPS, 0, 'f', 2); + + // Calculate text positioning + const QRect currentRect = rect(); + const int textWidth = p.fontMetrics().horizontalAdvance(fpsDisplayString); + const int xPos = (currentRect.width() - textWidth) / 2; + const int yPos = currentRect.bottom() - 5; + p.drawText(xPos, yPos, fpsDisplayString); + } } // ***** onroad widgets ***** @@ -226,7 +281,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}); @@ -243,7 +298,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(); if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) { @@ -253,12 +308,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 && !scene.always_on_lateral_active)) ? 0.6 : 1.0); + drawIcon(p, QPoint(btn_size / 2, btn_size / 2 + y_offset), img, QColor(0, 0, 0, 166), (isDown() || (!engageable && !scene.always_on_lateral_active)) ? 0.6 : 1.0); } @@ -334,7 +390,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(); @@ -504,7 +560,7 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { // paint path QLinearGradient bg(0, height(), 0, 0); - if (sm["controlsState"].getControlsState().getExperimentalMode()) { + if (sm["controlsState"].getControlsState().getExperimentalMode() || accelerationPath) { // The first half of track_vertices are the points for the right side of the path // and the indices match the positions of accel from uiPlan const auto &acceleration = sm["uiPlan"].getUiPlan().getAccel(); @@ -540,6 +596,97 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { painter.setBrush(bg); painter.drawPolygon(scene.track_vertices); + // create new path with track vertices and track edge vertices + QPainterPath path; + path.addPolygon(scene.track_vertices); + path.addPolygon(scene.track_edge_vertices); + + // paint path edges + QLinearGradient pe(0, height(), 0, 0); + if (alwaysOnLateral) { + pe.setColorAt(0.0, QColor::fromHslF(178 / 360., 0.90, 0.38, 1.0)); + pe.setColorAt(0.5, QColor::fromHslF(178 / 360., 0.90, 0.38, 0.5)); + pe.setColorAt(1.0, QColor::fromHslF(178 / 360., 0.90, 0.38, 0.1)); + } else if (conditionalStatus == 1 || conditionalStatus == 3) { + pe.setColorAt(0.0, QColor::fromHslF(58 / 360., 1.00, 0.50, 1.0)); + pe.setColorAt(0.5, QColor::fromHslF(58 / 360., 1.00, 0.50, 0.5)); + pe.setColorAt(1.0, QColor::fromHslF(58 / 360., 1.00, 0.50, 0.1)); + } else if (experimentalMode) { + pe.setColorAt(0.0, QColor::fromHslF(25 / 360., 0.71, 0.50, 1.0)); + pe.setColorAt(0.5, QColor::fromHslF(25 / 360., 0.71, 0.50, 0.5)); + pe.setColorAt(1.0, QColor::fromHslF(25 / 360., 0.71, 0.50, 0.1)); + } else if (scene.navigate_on_openpilot) { + pe.setColorAt(0.0, QColor::fromHslF(205 / 360., 0.85, 0.56, 1.0)); + pe.setColorAt(0.5, QColor::fromHslF(205 / 360., 0.85, 0.56, 0.5)); + pe.setColorAt(1.0, QColor::fromHslF(205 / 360., 0.85, 0.56, 0.1)); + } else { + pe.setColorAt(0.0, QColor::fromHslF(148 / 360., 0.94, 0.51, 1.0)); + pe.setColorAt(0.5, QColor::fromHslF(112 / 360., 1.00, 0.68, 0.5)); + pe.setColorAt(1.0, QColor::fromHslF(112 / 360., 1.00, 0.68, 0.1)); + } + + painter.setBrush(pe); + painter.drawPath(path); + + // paint blindspot path + QLinearGradient bs(0, height(), 0, 0); + if (blindSpotLeft || blindSpotRight) { + bs.setColorAt(0.0, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.6)); + bs.setColorAt(0.5, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.4)); + bs.setColorAt(1.0, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.2)); + } + + painter.setBrush(bs); + if (blindSpotLeft) { + painter.drawPolygon(scene.track_adjacent_vertices[4]); + } + if (blindSpotRight) { + painter.drawPolygon(scene.track_adjacent_vertices[5]); + } + + // paint adjacent lane paths + if (adjacentPath && (laneWidthLeft != 0 || laneWidthRight != 0)) { + // Set up the units + double conversionFactor = is_metric ? 1.0 : 3.28084; + QString unit_d = is_metric ? " meters" : " feet"; + + // Declare the lane width thresholds + constexpr float minLaneWidth = 2.5f; + constexpr float maxLaneWidth = 3.5f; + + // Set gradient colors based on laneWidth and blindspot + auto setGradientColors = [](QLinearGradient &gradient, float laneWidth, bool blindspot) { + // Make the path red for smaller paths or if there's a car in the blindspot and green for larger paths + double hue = (laneWidth < minLaneWidth || blindspot) ? 0.0 : + (laneWidth >= maxLaneWidth) ? 120.0 : + 120.0 * (laneWidth - minLaneWidth) / (maxLaneWidth - minLaneWidth); + auto hue_ratio = hue / 360.0; + gradient.setColorAt(0.0, QColor::fromHslF(hue_ratio, 0.75, 0.50, 0.6)); + gradient.setColorAt(0.5, QColor::fromHslF(hue_ratio, 0.75, 0.50, 0.4)); + gradient.setColorAt(1.0, QColor::fromHslF(hue_ratio, 0.75, 0.50, 0.2)); + }; + + // Paint the lanes + auto paintLane = [&](QPainter &painter, const QPolygonF &lane, const float laneWidth, const bool blindspot) { + QLinearGradient gradient(0, height(), 0, 0); + setGradientColors(gradient, laneWidth, blindspot); + painter.setFont(InterFont(30, QFont::DemiBold)); + painter.setBrush(gradient); + painter.setPen(Qt::transparent); + painter.drawPolygon(lane); + painter.setPen(Qt::white); + + QRectF boundingRect = lane.boundingRect(); + painter.drawText(boundingRect.center(), blindspot ? "Vehicle in blind spot" : + QString("%1%2").arg(laneWidth * conversionFactor, 0, 'f', 2).arg(unit_d)); + painter.setPen(Qt::NoPen); + }; + + // Paint lanes + paintLane(painter, scene.track_adjacent_vertices[4], laneWidthLeft, blindSpotLeft); + paintLane(painter, scene.track_adjacent_vertices[5], laneWidthRight, blindSpotRight); + } + painter.restore(); } @@ -616,6 +763,47 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState painter.setBrush(redColor(fillAlpha)); painter.drawPolygon(chevron, std::size(chevron)); + // Add lead info + if (leadInfo) { + // Declare the variables + QString unit_d = "meters"; + QString unit_s = "km/h"; + float distance = d_rel; + float lead_speed = std::max(lead_data.getVLead(), 0.0f); // Ensure speed doesn't go under 0 m/s cause that's dumb + + // Conversion factors and units + constexpr float toFeet = 3.28084f; + constexpr float toMph = 2.23694f; + constexpr float toKmph = 3.6f; + + // Metric speed conversion + if (is_metric || useSI) { + lead_speed *= toKmph; + } else { + // US imperial conversion + distance *= toFeet; + lead_speed *= toMph; + unit_d = "feet"; + unit_s = "mph"; + } + + // Form the text and center it below the chevron + painter.setPen(Qt::white); + painter.setFont(InterFont(35, QFont::Bold)); + + QString text = QString("%1 %2 | %3 %4") + .arg(distance, 0, 'f', 2, '0') + .arg(unit_d) + .arg(lead_speed, 0, 'f', 2, '0') + .arg(unit_s); + + // Calculate the text starting position + 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(); } @@ -655,6 +843,7 @@ void AnnotatedCameraWidget::paintGL() { // for replay of old routes, never go to widecam wide_cam_requested = wide_cam_requested && s->scene.calibration_wide_valid; } + paramsMemory.putBoolNonBlocking("WideCamera", wide_cam_requested); CameraWidget::setStreamType(cameraView == 3 ? VISION_STREAM_DRIVER : wide_cam_requested && cameraView != 1 ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD); @@ -701,7 +890,7 @@ void AnnotatedCameraWidget::paintGL() { double cur_draw_t = millis_since_boot(); double dt = cur_draw_t - prev_draw_t; - double fps = fps_filter.update(1. / dt * 1000); + fps = fps_filter.update(1. / dt * 1000); if (fps < 15) { LOGW("slow frame rate: %.2f fps", fps); } @@ -740,13 +929,29 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() { } void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { + accelerationPath = scene.acceleration_path; + adjacentPath = scene.adjacent_path; alwaysOnLateral = scene.always_on_lateral_active; + blindSpotLeft = scene.blind_spot_left; + blindSpotRight = scene.blind_spot_right; cameraView = scene.camera_view; conditionalExperimental = scene.conditional_experimental; conditionalSpeed = scene.conditional_speed; conditionalSpeedLead = scene.conditional_speed_lead; conditionalStatus = scene.conditional_status; + desiredFollow = scene.desired_follow; experimentalMode = scene.experimental_mode; + laneWidthLeft = scene.lane_width_left; + laneWidthRight = scene.lane_width_right; + leadInfo = scene.lead_info; + obstacleDistance = scene.obstacle_distance; + obstacleDistanceStock = scene.obstacle_distance_stock; + stoppedEquivalence = scene.stopped_equivalence; + useSI = scene.use_si; + + if (leadInfo) { + drawLeadInfo(p); + } if (alwaysOnLateral || conditionalExperimental) { drawStatusBar(p); @@ -759,6 +964,110 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { } } +void AnnotatedCameraWidget::drawLeadInfo(QPainter &p) { + SubMaster &sm = *uiState()->sm; + + // Declare the variables + static QElapsedTimer timer; + static bool isFiveSecondsPassed = false; + constexpr int maxAccelDuration = 5000; + + // Constants for units and conversions + QString unit_a = " m/s²"; + QString unit_d = "meters"; + QString unit_s = "kmh"; + float distanceConversion = 1.0f; + float speedConversion = 1.0f; + + // Conversion factors and units + constexpr float toFeet = 3.28084f; + constexpr float toMph = 2.23694f; + + // Metric speed conversion + if (!(is_metric || useSI)) { + // US imperial conversion + unit_a = " ft/s²"; + unit_d = "feet"; + unit_s = "mph"; + distanceConversion = toFeet; + speedConversion = toMph; + } + + // Update acceleration + double currentAcceleration = std::round(sm["carState"].getCarState().getAEgo() * 100) / 100; + static double maxAcceleration = 0.0; + + if (currentAcceleration > maxAcceleration && status == STATUS_ENGAGED) { + maxAcceleration = currentAcceleration; + isFiveSecondsPassed = false; + timer.start(); + } else { + isFiveSecondsPassed = timer.hasExpired(maxAccelDuration); + } + + // Construct text segments + auto createText = [&](const QString &title, const double data) { + return title + QString::number(std::round(data * distanceConversion)) + " " + unit_d; + }; + + // Create segments for insights + QString accelText = QString("Accel: %1%2") + .arg(currentAcceleration * speedConversion, 0, 'f', 2) + .arg(unit_a); + + QString maxAccSuffix = QString(" - Max: %1%2") + .arg(maxAcceleration * speedConversion, 0, 'f', 2) + .arg(unit_a); + + QString obstacleText = createText(" | Obstacle Factor: ", obstacleDistance); + QString stopText = createText(" - Stop Factor: ", stoppedEquivalence); + QString followText = " = " + createText("Follow Distance: ", desiredFollow); + + // Check if the longitudinal toggles have an impact on the driving logics + 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(); + }; + + // Prepare rectangle for insights + 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(30, QFont::DemiBold)); + p.setRenderHint(QPainter::TextAntialiasing); + + // Calculate positioning for text drawing + QRect adjustedRect = insightsRect.adjusted(0, 27, 0, 27); + int textBaseLine = adjustedRect.y() + (adjustedRect.height() + p.fontMetrics().height()) / 2 - p.fontMetrics().descent(); + + // Calculate the entire text width to ensure perfect centering + 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; + + // Draw the text + 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); + 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 2124f36..d4919bc 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -2,6 +2,7 @@ #include +#include #include #include #include @@ -15,6 +16,7 @@ const int btn_size = 192; const int img_size = (btn_size / 4) * 3; // FrogPilot global variables +static double fps; // ***** onroad widgets ***** class OnroadAlerts : public QWidget { @@ -40,7 +42,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 +56,8 @@ private: // FrogPilot variables UIScene &scene; + + int y_offset; }; @@ -106,6 +110,7 @@ private: bool wide_cam_requested = false; // FrogPilot widgets + void drawLeadInfo(QPainter &p); void drawStatusBar(QPainter &p); void initializeFrogPilotWidgets(); void updateFrogPilotWidgets(QPainter &p); @@ -117,13 +122,26 @@ private: QHBoxLayout *bottom_layout; + bool accelerationPath; + bool adjacentPath; bool alwaysOnLateral; + bool blindSpotLeft; + bool blindSpotRight; bool conditionalExperimental; bool experimentalMode; + bool leadInfo; + bool useSI; + double maxAcceleration; + float laneWidthLeft; + float laneWidthRight; int cameraView; int conditionalSpeed; int conditionalSpeedLead; int conditionalStatus; + int desiredFollow; + int obstacleDistance; + int obstacleDistanceStock; + int stoppedEquivalence; protected: void paintGL() override; diff --git a/selfdrive/ui/qt/sidebar.cc b/selfdrive/ui/qt/sidebar.cc index 05fd3eb..90d3f73 100644 --- a/selfdrive/ui/qt/sidebar.cc +++ b/selfdrive/ui/qt/sidebar.cc @@ -41,11 +41,42 @@ Sidebar::Sidebar(QWidget *parent) : QFrame(parent), onroad(false), flag_pressed( pm = std::make_unique>({"userFlag"}); // FrogPilot variables + isCPU = params.getBool("ShowCPU"); + isGPU = params.getBool("ShowGPU"); + + isMemoryUsage = params.getBool("ShowMemoryUsage"); + isStorageLeft = params.getBool("ShowStorageLeft"); + isStorageUsed = params.getBool("ShowStorageUsed"); + updateFrogPilotParams(); } void Sidebar::mousePressEvent(QMouseEvent *event) { - if (onroad && home_btn.contains(event->pos())) { + // Declare the click boxes + QRect cpuRect = {30, 496, 240, 126}; + QRect memoryRect = {30, 654, 240, 126}; + + static int showChip = 0; + static int showMemory = 0; + + // Swap between the respective metrics upon tap + if (cpuRect.contains(event->pos())) { + showChip = (showChip + 1) % 3; + isCPU = (showChip == 1); + isGPU = (showChip == 2); + params.putBoolNonBlocking("ShowCPU", isCPU); + params.putBoolNonBlocking("ShowGPU", isGPU); + update(); + } else if (memoryRect.contains(event->pos())) { + showMemory = (showMemory + 1) % 4; + isMemoryUsage = (showMemory == 1); + isStorageLeft = (showMemory == 2); + isStorageUsed = (showMemory == 3); + params.putBoolNonBlocking("ShowMemoryUsage", isMemoryUsage); + params.putBoolNonBlocking("ShowStorageLeft", isStorageLeft); + params.putBoolNonBlocking("ShowStorageUsed", isStorageUsed); + update(); + } else if (onroad && home_btn.contains(event->pos())) { flag_pressed = true; update(); } else if (settings_btn.contains(event->pos())) { @@ -86,6 +117,54 @@ void Sidebar::updateState(const UIState &s) { // FrogPilot properties auto frogpilotDeviceState = sm["frogpilotDeviceState"].getFrogpilotDeviceState(); + // FrogPilot metrics + if (isCPU || isGPU) { + auto cpu_loads = deviceState.getCpuUsagePercent(); + int cpu_usage = std::accumulate(cpu_loads.begin(), cpu_loads.end(), 0) / cpu_loads.size(); + int gpu_usage = deviceState.getGpuUsagePercent(); + + QString cpu = QString::number(cpu_usage) + "%"; + QString gpu = QString::number(gpu_usage) + "%"; + + QString metric = isGPU ? gpu : cpu; + int usage = isGPU ? gpu_usage : cpu_usage; + + ItemStatus cpuStatus = {{tr(isGPU ? "GPU" : "CPU"), metric}, good_color}; + if (usage >= 85) { + cpuStatus = {{tr(isGPU ? "GPU" : "CPU"), metric}, danger_color}; + } else if (usage >= 70) { + cpuStatus = {{tr(isGPU ? "GPU" : "CPU"), metric}, warning_color}; + } + setProperty("cpuStatus", QVariant::fromValue(cpuStatus)); + } + + if (isMemoryUsage || isStorageLeft || isStorageUsed) { + int memory_usage = deviceState.getMemoryUsagePercent(); + int storage_left = frogpilotDeviceState.getFreeSpace(); + int storage_used = frogpilotDeviceState.getUsedSpace(); + + QString memory = QString::number(memory_usage) + "%"; + QString storage = QString::number(isStorageLeft ? storage_left : storage_used) + " GB"; + + if (isMemoryUsage) { + ItemStatus memoryStatus = {{tr("MEMORY"), memory}, good_color}; + if (memory_usage >= 85) { + memoryStatus = {{tr("MEMORY"), memory}, danger_color}; + } else if (memory_usage >= 70) { + memoryStatus = {{tr("MEMORY"), memory}, warning_color}; + } + setProperty("memoryStatus", QVariant::fromValue(memoryStatus)); + } else { + ItemStatus storageStatus = {{tr(isStorageLeft ? "LEFT" : "USED"), storage}, good_color}; + if (10 <= storage_left && storage_left < 25) { + storageStatus = {{tr(isStorageLeft ? "LEFT" : "USED"), storage}, warning_color}; + } else if (storage_left < 10) { + storageStatus = {{tr(isStorageLeft ? "LEFT" : "USED"), storage}, danger_color}; + } + setProperty("storageStatus", QVariant::fromValue(storageStatus)); + } + } + ItemStatus connectStatus; auto last_ping = deviceState.getLastAthenaPingTime(); if (last_ping == 0) { @@ -149,6 +228,18 @@ void Sidebar::paintEvent(QPaintEvent *event) { // metrics drawMetric(p, temp_status.first, temp_status.second, 338); - drawMetric(p, panda_status.first, panda_status.second, 496); - drawMetric(p, connect_status.first, connect_status.second, 654); + + if (isCPU || isGPU) { + drawMetric(p, cpu_status.first, cpu_status.second, 496); + } else { + drawMetric(p, panda_status.first, panda_status.second, 496); + } + + if (isMemoryUsage) { + drawMetric(p, memory_status.first, memory_status.second, 654); + } else if (isStorageLeft || isStorageUsed) { + drawMetric(p, storage_status.first, storage_status.second, 654); + } else { + drawMetric(p, connect_status.first, connect_status.second, 654); + } } diff --git a/selfdrive/ui/qt/sidebar.h b/selfdrive/ui/qt/sidebar.h index ea0d51d..b9936ab 100644 --- a/selfdrive/ui/qt/sidebar.h +++ b/selfdrive/ui/qt/sidebar.h @@ -19,6 +19,9 @@ class Sidebar : public QFrame { Q_PROPERTY(int netStrength MEMBER net_strength NOTIFY valueChanged); // FrogPilot properties + Q_PROPERTY(ItemStatus cpuStatus MEMBER cpu_status NOTIFY valueChanged) + Q_PROPERTY(ItemStatus memoryStatus MEMBER memory_status NOTIFY valueChanged) + Q_PROPERTY(ItemStatus storageStatus MEMBER storage_status NOTIFY valueChanged) public: explicit Sidebar(QWidget* parent = 0); @@ -67,4 +70,12 @@ private: // FrogPilot variables Params params; + + ItemStatus cpu_status, memory_status, storage_status; + + bool isCPU; + bool isGPU; + bool isMemoryUsage; + bool isStorageLeft; + bool isStorageUsed; }; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 38b679f..44880fe 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -89,7 +89,8 @@ void update_model(UIState *s, if (plan_position.getX().size() < model.getPosition().getX().size()) { plan_position = model.getPosition(); } - float max_distance = std::clamp(*(plan_position.getX().end() - 1), + float max_distance = scene.unlimited_road_ui_length ? *(plan_position.getX().end() - 1) : + std::clamp(*(plan_position.getX().end() - 1), MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE); // update lane lines @@ -98,7 +99,7 @@ void update_model(UIState *s, int max_idx = get_path_length_idx(lane_lines[0], max_distance); for (int i = 0; i < std::size(scene.lane_line_vertices); i++) { scene.lane_line_probs[i] = lane_line_probs[i]; - update_line_data(s, lane_lines[i], 0.025 * scene.lane_line_probs[i], 0, &scene.lane_line_vertices[i], max_idx); + update_line_data(s, lane_lines[i], scene.model_ui ? scene.lane_line_width * scene.lane_line_probs[i] : 0.025 * scene.lane_line_probs[i], 0, &scene.lane_line_vertices[i], max_idx); } // update road edges @@ -106,7 +107,7 @@ void update_model(UIState *s, const auto road_edge_stds = model.getRoadEdgeStds(); for (int i = 0; i < std::size(scene.road_edge_vertices); i++) { scene.road_edge_stds[i] = road_edge_stds[i]; - update_line_data(s, road_edges[i], 0.025, 0, &scene.road_edge_vertices[i], max_idx); + update_line_data(s, road_edges[i], scene.model_ui ? scene.road_edge_width : 0.025, 0, &scene.road_edge_vertices[i], max_idx); } // update path @@ -116,7 +117,15 @@ void update_model(UIState *s, max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance); } max_idx = get_path_length_idx(plan_position, max_distance); - update_line_data(s, plan_position, 0.9, 1.22, &scene.track_vertices, max_idx, false); + update_line_data(s, plan_position, scene.model_ui ? scene.path_width * (1 - scene.path_edge_width / 100) : 0.9, 1.22, &scene.track_vertices, max_idx, false); + + // update path edges + update_line_data(s, plan_position, scene.model_ui ? scene.path_width : 0, 1.22, &scene.track_edge_vertices, max_idx, false); + + // update adjacent paths + for (int i = 4; i <= 5; i++) { + update_line_data(s, lane_lines[i], scene.blind_spot_path ? (i == 4 ? scene.lane_width_left : scene.lane_width_right) / 2 : 0, 0, &scene.track_adjacent_vertices[i], max_idx); + } } void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &driverstate, float dm_fade_state, bool is_rhd) { @@ -205,6 +214,10 @@ static void update_state(UIState *s) { } if (sm.updated("carState")) { auto carState = sm["carState"].getCarState(); + if (scene.blind_spot_path) { + scene.blind_spot_left = carState.getLeftBlindspot(); + scene.blind_spot_right = carState.getRightBlindspot(); + } } if (sm.updated("controlsState")) { auto controlsState = sm["controlsState"].getControlsState(); @@ -219,9 +232,19 @@ static void update_state(UIState *s) { } if (sm.updated("frogpilotLateralPlan")) { auto frogpilotLateralPlan = sm["frogpilotLateralPlan"].getFrogpilotLateralPlan(); + if (scene.blind_spot_path) { + scene.lane_width_left = frogpilotLateralPlan.getLaneWidthLeft(); + scene.lane_width_right = frogpilotLateralPlan.getLaneWidthRight(); + } } if (sm.updated("frogpilotLongitudinalPlan")) { auto frogpilotLongitudinalPlan = sm["frogpilotLongitudinalPlan"].getFrogpilotLongitudinalPlan(); + if (scene.lead_info) { + scene.desired_follow = frogpilotLongitudinalPlan.getDesiredFollowDistance(); + scene.obstacle_distance = frogpilotLongitudinalPlan.getSafeObstacleDistance(); + scene.obstacle_distance_stock = frogpilotLongitudinalPlan.getSafeObstacleDistanceStock(); + scene.stopped_equivalence = frogpilotLongitudinalPlan.getStoppedEquivalenceFactor(); + } } if (sm.updated("liveLocationKalman")) { auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); @@ -254,6 +277,21 @@ void ui_update_params(UIState *s) { scene.conditional_experimental = params.getBool("ConditionalExperimental"); scene.conditional_speed = params.getInt("CESpeed"); scene.conditional_speed_lead = params.getInt("CESpeedLead"); + + scene.custom_onroad_ui = params.getBool("CustomUI"); + scene.adjacent_path = scene.custom_onroad_ui && params.getBool("AdjacentPath"); + scene.blind_spot_path = scene.custom_onroad_ui && params.getBool("BlindSpotPath"); + scene.lead_info = scene.custom_onroad_ui && params.getBool("LeadInfo"); + scene.show_fps = scene.custom_onroad_ui && params.getBool("ShowFPS"); + scene.use_si = scene.custom_onroad_ui && params.getBool("UseSI"); + + scene.model_ui = params.getBool("ModelUI"); + scene.acceleration_path = scene.model_ui && params.getBool("AccelerationPath"); + scene.lane_line_width = params.getInt("LaneLinesWidth") * (scene.is_metric ? 1 : INCH_TO_CM) / 200; + scene.path_edge_width = params.getInt("PathEdgeWidth"); + scene.path_width = params.getInt("PathWidth") / 10.0 * (scene.is_metric ? 1 : FOOT_TO_METER) / 2; + scene.road_edge_width = params.getInt("RoadEdgesWidth") * (scene.is_metric ? 1 : INCH_TO_CM) / 200; + scene.unlimited_road_ui_length = scene.model_ui && params.getBool("UnlimitedLength"); } void UIState::updateStatus() { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 43f7a8d..7834982 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -169,15 +169,38 @@ typedef struct UIScene { uint64_t started_frame; // FrogPilot variables + bool acceleration_path; + bool adjacent_path; bool always_on_lateral; bool always_on_lateral_active; + bool blind_spot_left; + bool blind_spot_path; + bool blind_spot_right; bool conditional_experimental; + bool custom_onroad_ui; bool enabled; bool experimental_mode; + bool lead_info; + bool model_ui; + bool show_fps; + bool unlimited_road_ui_length; + bool use_si; + float lane_line_width; + float lane_width_left; + float lane_width_right; + float path_edge_width; + float path_width; + float road_edge_width; int camera_view; int conditional_speed; int conditional_speed_lead; int conditional_status; + int desired_follow; + int obstacle_distance; + int obstacle_distance_stock; + int stopped_equivalence; + QPolygonF track_adjacent_vertices[6]; + QPolygonF track_edge_vertices; } UIScene; diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index e011618..0d92a00 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -935,6 +935,10 @@ void cameras_run(MultiCameraState *s) { // FrogPilot variables Params paramsMemory{"/dev/shm/params"}; + bool wide_camera_displayed = paramsMemory.getBool("WideCamera"); + static int frame_count = 0; + static int fps = 0.0; + LOG("-- Starting threads"); std::vector threads; if (s->driver_cam.enabled) threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera)); @@ -947,9 +951,22 @@ void cameras_run(MultiCameraState *s) { s->road_cam.sensors_start(); s->wide_road_cam.sensors_start(); + auto prev_time_point = std::chrono::high_resolution_clock::now(); + // poll events LOG("-- Dequeueing Video events"); while (!do_exit) { + + auto current_time_point = std::chrono::high_resolution_clock::now(); + double elapsed_time = std::chrono::duration(current_time_point - prev_time_point).count(); + + if (elapsed_time > 1.0) { + fps = frame_count / elapsed_time; + paramsMemory.putIntNonBlocking("CameraFPS", fps); + frame_count = 0; + prev_time_point = current_time_point; + } + struct pollfd fds[1] = {{0}}; fds[0].fd = s->video0_fd; @@ -979,8 +996,14 @@ void cameras_run(MultiCameraState *s) { if (event_data->session_hdl == s->road_cam.session_handle) { s->road_cam.handle_camera_event(event_data); + if (!wide_camera_displayed) { + frame_count++; + } } else if (event_data->session_hdl == s->wide_road_cam.session_handle) { s->wide_road_cam.handle_camera_event(event_data); + if (wide_camera_displayed) { + frame_count++; + } } else if (event_data->session_hdl == s->driver_cam.session_handle) { s->driver_cam.handle_camera_event(event_data); } else { diff --git a/system/loggerd/config.py b/system/loggerd/config.py index 5c2c89b..2941e39 100644 --- a/system/loggerd/config.py +++ b/system/loggerd/config.py @@ -27,3 +27,14 @@ def get_available_bytes(default=None): available_bytes = default return available_bytes + +def get_used_bytes(default=None): + try: + statvfs = os.statvfs(Paths.log_root()) + total_bytes = statvfs.f_blocks * statvfs.f_frsize + available_bytes = statvfs.f_bavail * statvfs.f_frsize + used_bytes = total_bytes - available_bytes + except OSError: + used_bytes = default + + return used_bytes