diff --git a/selfdrive/ui/qt/onroad/model.cc b/selfdrive/ui/qt/onroad/model.cc index a1e3756cb..a4165be21 100644 --- a/selfdrive/ui/qt/onroad/model.cc +++ b/selfdrive/ui/qt/onroad/model.cc @@ -34,7 +34,6 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) { drawLead(painter, lead_two, lead_vertices[1], surface_rect); } } - drawLeadStatus(painter, surface_rect.height(), surface_rect.width()); painter.restore(); } @@ -175,172 +174,6 @@ QColor ModelRenderer::blendColors(const QColor &start, const QColor &end, float } -void ModelRenderer::drawLeadStatus(QPainter &painter, int height, int width) { - auto *s = uiState(); - auto &sm = *(s->sm); - - if (!sm.alive("radarState")) return; - - const auto &radar_state = sm["radarState"].getRadarState(); - const auto &lead_one = radar_state.getLeadOne(); - const auto &lead_two = radar_state.getLeadTwo(); - - // Check if we have any active leads - bool has_lead_one = lead_one.getStatus(); - bool has_lead_two = lead_two.getStatus(); - - if (!has_lead_one && !has_lead_two) { - // Fade out status display - lead_status_alpha = std::max(0.0f, lead_status_alpha - 0.05f); - if (lead_status_alpha <= 0.0f) return; - } else { - // Fade in status display - lead_status_alpha = std::min(1.0f, lead_status_alpha + 0.1f); - } - - // Draw status for each lead vehicle under its chevron - if (true) { - drawLeadStatusAtPosition(painter, lead_one, lead_vertices[0], height, width, "L1"); - } - - if (has_lead_two && std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0) { - drawLeadStatusAtPosition(painter, lead_two, lead_vertices[1], height, width, "L2"); - } -} - -void ModelRenderer::drawLeadStatusAtPosition(QPainter &painter, - const cereal::RadarState::LeadData::Reader &lead_data, - const QPointF &chevron_pos, - int height, int width, - const QString &label) { - - float d_rel = lead_data.getDRel(); - float v_rel = lead_data.getVRel(); - auto *s = uiState(); - auto &sm = *(s->sm); - float v_ego = sm["carState"].getCarState().getVEgo(); - - int chevron_data = std::atoi(Params().get("ChevronInfo").c_str()); - - // Calculate chevron size (same logic as drawLead) - float sz = std::clamp((25 * 30) / (d_rel / 3 + 30), 15.0f, 30.0f) * 2.35; - - QFont content_font = painter.font(); - content_font.setPixelSize(35); - content_font.setBold(true); - painter.setFont(content_font); - - QFontMetrics fm(content_font); - bool is_metric = s->scene.is_metric; - - QStringList text_lines; - - const int chevron_types = 3; - const int chevron_all = chevron_types + 1; // All metrics (value 4) - QStringList chevron_text[chevron_types]; - int position; - float val; - - // Distance display (chevron_data == 1 or all) - if (chevron_data == 1 || chevron_data == chevron_all) { - position = 0; - val = std::max(0.0f, d_rel); - QString distance_unit = is_metric ? "m" : "ft"; - if (!is_metric) { - val *= 3.28084f; // Convert meters to feet - } - chevron_text[position].append(QString::number(val, 'f', 0) + " " + distance_unit); - } - - // Absolute velocity display (chevron_data == 2 or all) - if (chevron_data == 2 || chevron_data == chevron_all) { - position = (chevron_data == 2) ? 0 : 1; - val = std::max(0.0f, (v_rel + v_ego) * (is_metric ? static_cast(MS_TO_KPH) : static_cast(MS_TO_MPH))); - chevron_text[position].append(QString::number(val, 'f', 0) + " " + (is_metric ? "km/h" : "mph")); - } - - // Time-to-contact display (chevron_data == 3 or all) - if (chevron_data == 3 || chevron_data == chevron_all) { - position = (chevron_data == 3) ? 0 : 2; - val = (d_rel > 0 && v_ego > 0) ? std::max(0.0f, d_rel / v_ego) : 0.0f; - QString ttc_str = (val > 0 && val < 200) ? QString::number(val, 'f', 1) + "s" : "---"; - chevron_text[position].append(ttc_str); - } - - // Collect all non-empty text lines - for (int i = 0; i < chevron_types; ++i) { - if (!chevron_text[i].isEmpty()) { - text_lines.append(chevron_text[i]); - } - } - - // If no text to display, return early - if (text_lines.isEmpty()) { - return; - } - - // Text box dimensions - float str_w = 150; // Width of text area - float str_h = 45; // Height per line - - // Position text below chevron, centered horizontally - float text_x = chevron_pos.x() - str_w / 2; - float text_y = chevron_pos.y() + sz + 15; - - // Clamp to screen bounds - text_x = std::clamp(text_x, 10.0f, (float)width - str_w - 10); - - // Shadow offset - QPoint shadow_offset(2, 2); - - // Draw each line of text with shadow - for (int i = 0; i < text_lines.size(); ++i) { - if (!text_lines[i].isEmpty()) { - QRect textRect(text_x, text_y + (i * str_h), str_w, str_h); - - // Draw shadow - painter.setPen(QColor(0x0, 0x0, 0x0, (int)(200 * lead_status_alpha))); - painter.drawText(textRect.translated(shadow_offset.x(), shadow_offset.y()), - Qt::AlignBottom | Qt::AlignHCenter, text_lines[i]); - - // Determine text color based on content and danger level - QColor text_color; - - // Check if this is a distance line (contains 'm' or 'ft') - if (text_lines[i].contains("m") || text_lines[i].contains("ft")) { - if (d_rel < 20.0f) { - text_color = QColor(255, 80, 80, (int)(255 * lead_status_alpha)); // Red - danger - } else if (d_rel < 40.0f) { - text_color = QColor(255, 200, 80, (int)(255 * lead_status_alpha)); // Yellow - caution - } else { - text_color = QColor(80, 255, 120, (int)(255 * lead_status_alpha)); // Green - safe - } - } - // Enhanced color coding for time-to-contact - else if (text_lines[i].contains("s") && !text_lines[i].contains("---")) { - float ttc_val = text_lines[i].left(text_lines[i].length() - 1).toFloat(); - if (ttc_val < 3.0f) { - text_color = QColor(255, 80, 80, (int)(255 * lead_status_alpha)); // Red - urgent - } else if (ttc_val < 6.0f) { - text_color = QColor(255, 200, 80, (int)(255 * lead_status_alpha)); // Yellow - caution - } else { - text_color = QColor(0xff, 0xff, 0xff, (int)(255 * lead_status_alpha)); // White - safe - } - } - else { - text_color = QColor(0xff, 0xff, 0xff, (int)(255 * lead_status_alpha)); // White for other lines - } - - // Draw main text - painter.setPen(text_color); - painter.drawText(textRect, Qt::AlignBottom | Qt::AlignHCenter, text_lines[i]); - } - } - - // Reset pen - painter.setPen(Qt::NoPen); -} - void ModelRenderer::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd, const QRect &surface_rect) { const float speedBuff = 10.; diff --git a/selfdrive/ui/qt/onroad/model.h b/selfdrive/ui/qt/onroad/model.h index e40f832c3..3ef8ba532 100644 --- a/selfdrive/ui/qt/onroad/model.h +++ b/selfdrive/ui/qt/onroad/model.h @@ -34,12 +34,6 @@ protected: bool mapToScreen(float in_x, float in_y, float in_z, QPointF *out); void mapLineToPolygon(const cereal::XYZTData::Reader &line, float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert = true); - void drawLeadStatus(QPainter &painter, int height, int width); - void drawLeadStatusAtPosition(QPainter &painter, - const cereal::RadarState::LeadData::Reader &lead_data, - const QPointF &chevron_pos, - int height, int width, - const QString &label); void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd, const QRect &surface_rect); void update_leads(const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line); virtual void update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead); @@ -65,8 +59,4 @@ protected: Eigen::Matrix3f car_space_transform = Eigen::Matrix3f::Zero(); QRectF clip_region; - float lead_status_alpha = 0.0f; - QPointF lead_status_pos; - QString lead_status_text; - QColor lead_status_color; }; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc index 0324cd70f..0e42f777c 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc @@ -140,6 +140,40 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) { vlayout->addWidget(sunnypilotScroller); main_layout->addWidget(sunnypilotScreen); + + QObject::connect(uiState(), &UIState::offroadTransition, this, &VisualsPanel::refreshLongitudinalStatus); + + refreshLongitudinalStatus(); +} + +void VisualsPanel::refreshLongitudinalStatus() { + auto cp_bytes = params.get("CarParamsPersistent"); + if (!cp_bytes.empty()) { + AlignedBuffer aligned_buf; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); + cereal::CarParams::Reader CP = cmsg.getRoot(); + + has_longitudinal_control = hasLongitudinalControl(CP); + } else { + has_longitudinal_control = false; + } + + if (chevron_info_settings) { + QString chevronEnabledDescription = tr("Display useful metrics below the chevron that tracks the lead car (only applicable to cars with openpilot longitudinal control)."); + QString chevronNoLongDescription = tr("This feature requires openpilot longitudinal control to be available."); + + if (has_longitudinal_control) { + chevron_info_settings->setDescription(chevronEnabledDescription); + } else { + // Reset to "Off" when longitudinal not available + params.put("ChevronInfo", "0"); + chevron_info_settings->setDescription(chevronNoLongDescription); + } + + // Enable only when longitudinal is available + chevron_info_settings->setEnabled(has_longitudinal_control); + chevron_info_settings->refresh(); + } } void VisualsPanel::paramsRefresh() { diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.h index 30ff31c30..76a846dd4 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.h @@ -19,6 +19,7 @@ public: explicit VisualsPanel(QWidget *parent = nullptr); void paramsRefresh(); + void refreshLongitudinalStatus(); protected: QStackedLayout* main_layout = nullptr; @@ -29,4 +30,6 @@ protected: ParamWatcher * param_watcher; ButtonParamControlSP *chevron_info_settings; ButtonParamControlSP *dev_ui_settings; + + bool has_longitudinal_control = false; }; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/model.cc b/selfdrive/ui/sunnypilot/qt/onroad/model.cc index 5d92838f8..875ec6f0b 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/model.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/model.cc @@ -87,4 +87,139 @@ void ModelRendererSP::drawPath(QPainter &painter, const cereal::ModelDataV2::Rea // Normal path rendering ModelRenderer::drawPath(painter, model, surface_rect.height()); } + + drawLeadStatus(painter, surface_rect.height(), surface_rect.width()); +} + +void ModelRendererSP::drawLeadStatus(QPainter &painter, int height, int width) { + auto *s = uiState(); + auto &sm = *(s->sm); + + bool longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl(); + if (!longitudinal_control) { + lead_status_alpha = std::max(0.0f, lead_status_alpha - 0.05f); + return; + } + + if (!sm.alive("radarState")) { + lead_status_alpha = std::max(0.0f, lead_status_alpha - 0.05f); + return; + } + + const auto &radar_state = sm["radarState"].getRadarState(); + const auto &lead_one = radar_state.getLeadOne(); + const auto &lead_two = radar_state.getLeadTwo(); + + bool has_lead_one = lead_one.getStatus(); + bool has_lead_two = lead_two.getStatus(); + + if (!has_lead_one && !has_lead_two) { + lead_status_alpha = std::max(0.0f, lead_status_alpha - 0.05f); + if (lead_status_alpha <= 0.0f) return; + } else { + lead_status_alpha = std::min(1.0f, lead_status_alpha + 0.1f); + } + + if (has_lead_one) { + drawLeadStatusAtPosition(painter, lead_one, lead_vertices[0], height, width, "L1"); + } + + if (has_lead_two && std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0) { + drawLeadStatusAtPosition(painter, lead_two, lead_vertices[1], height, width, "L2"); + } +} + +void ModelRendererSP::drawLeadStatusAtPosition(QPainter &painter, + const cereal::RadarState::LeadData::Reader &lead_data, + const QPointF &chevron_pos, + int height, int width, + const QString &label) { + float d_rel = lead_data.getDRel(); + float v_rel = lead_data.getVRel(); + auto *s = uiState(); + auto &sm = *(s->sm); + float v_ego = sm["carState"].getCarState().getVEgo(); + + int chevron_data = s->scene.chevron_info; + float sz = std::clamp((25 * 30) / (d_rel / 3 + 30), 15.0f, 30.0f) * 2.35; + + QFont content_font = painter.font(); + content_font.setPixelSize(50); + content_font.setBold(true); + painter.setFont(content_font); + + bool is_metric = s->scene.is_metric; + QStringList text_lines; + const int chevron_all = 4; + QStringList chevron_text[3]; + + // Distance display + if (chevron_data == 1 || chevron_data == chevron_all) { + int pos = 0; + float val = std::max(0.0f, d_rel); + QString unit = is_metric ? "m" : "ft"; + if (!is_metric) val *= 3.28084f; + chevron_text[pos].append(QString::number(val, 'f', 0) + " " + unit); + } + + // Speed display + if (chevron_data == 2 || chevron_data == chevron_all) { + int pos = (chevron_data == 2) ? 0 : 1; + float multiplier = is_metric ? static_cast(MS_TO_KPH) : static_cast(MS_TO_MPH); + float val = std::max(0.0f, (v_rel + v_ego) * multiplier); + QString unit = is_metric ? "km/h" : "mph"; + chevron_text[pos].append(QString::number(val, 'f', 0) + " " + unit); + } + + // Time to contact + if (chevron_data == 3 || chevron_data == chevron_all) { + int pos = (chevron_data == 3) ? 0 : 2; + float val = (d_rel > 0 && v_ego > 0) ? std::max(0.0f, d_rel / v_ego) : 0.0f; + QString ttc = (val > 0 && val < 200) ? QString::number(val, 'f', 1) + "s" : "---"; + chevron_text[pos].append(ttc); + } + + for (int i = 0; i < 3; ++i) { + if (!chevron_text[i].isEmpty()) text_lines.append(chevron_text[i]); + } + + if (text_lines.isEmpty()) return; + + QFontMetrics fm(content_font); + float text_width = 120.0f; + for (const QString &line : text_lines) { + text_width = std::max(text_width, fm.horizontalAdvance(line) + 20.0f); + } + text_width = std::min(text_width, 250.0f); + + float line_height = 50.0f; + float total_height = text_lines.size() * line_height; + float margin = 20.0f; + + float text_y = chevron_pos.y() + sz + 15; + if (text_y + total_height > height - margin) { + float y_max = chevron_pos.y() > (height - margin) ? (height - margin) : chevron_pos.y(); + text_y = y_max - 15 - total_height; + text_y = std::max(margin, text_y); + } + + float text_x = chevron_pos.x() - text_width / 2; + text_x = std::clamp(text_x, margin, (float)width - text_width - margin); + + QPoint shadow_offset(2, 2); + QColor text_color = QColor(255, 255, 255, (int)(255 * lead_status_alpha)); + for (int i = 0; i < text_lines.size(); ++i) { + float y = text_y + (i * line_height); + if (y + line_height > height - margin) break; + + QRect rect(text_x, y, text_width, line_height); + + // Draw shadow + painter.setPen(QColor(0, 0, 0, (int)(200 * lead_status_alpha))); + painter.drawText(rect.translated(shadow_offset), Qt::AlignCenter, text_lines[i]); + painter.setPen(text_color); + painter.drawText(rect, Qt::AlignCenter, text_lines[i]); + } + + painter.setPen(Qt::NoPen); } diff --git a/selfdrive/ui/sunnypilot/qt/onroad/model.h b/selfdrive/ui/sunnypilot/qt/onroad/model.h index 24404f32f..73999f005 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/model.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/model.h @@ -17,6 +17,17 @@ private: void update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead) override; void drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, const QRect &rect) override; + // Lead status display methods + void drawLeadStatus(QPainter &painter, int height, int width); + void drawLeadStatusAtPosition(QPainter &painter, + const cereal::RadarState::LeadData::Reader &lead_data, + const QPointF &chevron_pos, + int height, int width, + const QString &label); + QPolygonF left_blindspot_vertices; QPolygonF right_blindspot_vertices; + + // Lead status animation + float lead_status_alpha = 0.0f; }; diff --git a/selfdrive/ui/sunnypilot/ui.cc b/selfdrive/ui/sunnypilot/ui.cc index 2244e2319..16d2bac49 100644 --- a/selfdrive/ui/sunnypilot/ui.cc +++ b/selfdrive/ui/sunnypilot/ui.cc @@ -73,6 +73,7 @@ void ui_update_params_sp(UIStateSP *s) { s->scene.onroadScreenOffTimerParam = std::atoi(params.get("OnroadScreenOffTimer").c_str()); s->scene.turn_signals = params.getBool("ShowTurnSignals"); + s->scene.chevron_info = std::atoi(params.get("ChevronInfo").c_str()); } void UIStateSP::reset_onroad_sleep_timer(OnroadTimerStatusToggle toggleTimerStatus) { diff --git a/selfdrive/ui/sunnypilot/ui_scene.h b/selfdrive/ui/sunnypilot/ui_scene.h index 233f1e15d..353f4ed8d 100644 --- a/selfdrive/ui/sunnypilot/ui_scene.h +++ b/selfdrive/ui/sunnypilot/ui_scene.h @@ -18,4 +18,5 @@ typedef struct UISceneSP : UIScene { bool trueVEgoUI; bool hideVEgoUI; bool turn_signals = false; + int chevron_info; } UISceneSP;