diff --git a/selfdrive/ui/qt/onroad/model.cc b/selfdrive/ui/qt/onroad/model.cc index 7312facf40..402ad789d4 100644 --- a/selfdrive/ui/qt/onroad/model.cc +++ b/selfdrive/ui/qt/onroad/model.cc @@ -35,7 +35,7 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) { update_model(model, lead_one); drawLaneLines(painter); - drawPath(painter, model, surface_rect.height()); + drawPath(painter, model, surface_rect.height(), surface_rect.width()); if (longitudinal_control && sm.alive("radarState")) { update_leads(radar_state, model.getPosition()); @@ -105,7 +105,7 @@ void ModelRenderer::drawLaneLines(QPainter &painter) { } } -void ModelRenderer::drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, int height) { +void ModelRenderer::drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, int height, int width) { QLinearGradient bg(0, height, 0, 0); auto *s = uiState(); auto &sm = *(s->sm); @@ -173,6 +173,9 @@ void ModelRenderer::drawPath(QPainter &painter, const cereal::ModelDataV2::Reade painter.setBrush(bg); painter.drawPolygon(track_vertices); + + LongFuel(painter,height, width); + LateralFuel(painter, height, width); } @@ -220,6 +223,161 @@ QColor ModelRenderer::blendColors(const QColor &start, const QColor &end, float (1 - t) * start.alphaF() + t * end.alphaF()); } +void ModelRenderer::LongFuel(QPainter &painter, int height, int width) { + qreal rectWidth = static_cast(width); + qreal rectHeight = static_cast(height); + UIState *s = uiState(); + + float currentAcceleration = (*s->sm)["carControl"].getCarControl().getActuators().getAccel(); + //float currentAcceleration = (*s->sm)["carState"].getCarState().getAEgo(); + + qreal gaugeSize = 140.0; // Diameter of the semicircle + qreal backgroundSize = gaugeSize * 1.4; // Background is 30% larger than the gague + qreal centerX = rectWidth / 17; // Center the gague horz + qreal centerY = rectHeight / 2 + 120; // Center the gauge vertical offset + + // Draw a dark circular background + painter.setPen(Qt::NoPen); + painter.setBrush(QColor(0, 0, 0, 80)); // Semi-transparent black + painter.drawEllipse(QPointF(centerX, centerY), backgroundSize / 2, backgroundSize / 2); + + // Add a subtle border/glow around the background + QPen borderPen(QColor(0, 0, 0, 100)); + borderPen.setWidth(2); + painter.setPen(borderPen); + painter.drawEllipse(QPointF(centerX, centerY), backgroundSize / 2 + 1, backgroundSize / 2 + 1); + + // Draw the background semicircle + QPen semicirclePen(QColor(50, 50, 50)); // Dark gray for the semicircle + semicirclePen.setWidth(30); // Thicker pen for the semicircle + semicirclePen.setCapStyle(Qt::RoundCap); + painter.setPen(semicirclePen); + painter.drawArc(QRectF(centerX - gaugeSize / 2, centerY - gaugeSize / 2, gaugeSize, gaugeSize), 0, 180 * 16); + + // Determine the color based on the magnitude of acceleration + QColor indicatorColor; + float absoluteAcceleration = std::abs(currentAcceleration); + if (absoluteAcceleration < 0.3) { + indicatorColor = QColor(23, 241, 66, 200); // Green for low acceleration + } else if (absoluteAcceleration < 0.6) { + indicatorColor = QColor(255, 166, 0, 200); // Yellow for moderate acceleration + } else { + indicatorColor = QColor(245, 0, 0, 200); // Red for high acceleration + } + + // Calculate the span of the arc based on acceleration + int spanAngle = static_cast(90 * absoluteAcceleration * 16); // Scale for better visibility + spanAngle = std::clamp(spanAngle, 0, 90 * 16); // Ensure the arc does not exceed 90 degrees + + // Starting angle is at the middle of the semicircle (90 degrees) + int startAngle = 90 * 16; + + // Draw the acceleration arc if there's significant acceleration + if (absoluteAcceleration > 0.01) { + semicirclePen.setColor(indicatorColor); + painter.setPen(semicirclePen); + + QRectF arcRect(centerX - gaugeSize / 2, centerY - gaugeSize / 2, gaugeSize, gaugeSize); + + // For positive acceleration, draw the arc to the left + if (currentAcceleration > 0) { + painter.drawArc(arcRect, startAngle, -spanAngle); // Negative span for left side + } else { + // For negative acceleration (deceleration), draw the arc to the right + painter.drawArc(arcRect, startAngle, spanAngle); // Positive span for right side + } + } + + // Draw the text center + painter.setPen(Qt::white); + QFont font = painter.font(); + font.setPixelSize(20); + font.setBold(true); + painter.setFont(font); + painter.drawText(QRectF(centerX - 50, centerY + 10, 100, 20), Qt::AlignCenter, "LONG"); +} + + +void ModelRenderer::LateralFuel(QPainter &painter, int height, int width) { + qreal rectWidth = static_cast(width); + qreal rectHeight = static_cast(height); + UIState *s = uiState(); + + float currentLateral = (*s->sm)["carState"].getCarState().getSteeringAngleDeg(); + + qreal gaugeSize = 140.0; // Diameter of the semicircle + qreal backgroundSize = gaugeSize * 1.4; // Background is 30% larger than the gague + qreal centerX = rectWidth / 17; // Center the gague horz + qreal centerY = rectHeight / 2 - 120; // Center the gague vertical offset + + // Draw a dark circular background + painter.setPen(Qt::NoPen); + painter.setBrush(QColor(0, 0, 0, 80)); // Semi-transparent black + painter.drawEllipse(QPointF(centerX, centerY), backgroundSize / 2, backgroundSize / 2); + + // Add a subtle border/glow around the background + QPen borderPen(QColor(0, 0, 0, 100)); + borderPen.setWidth(2); + painter.setPen(borderPen); + painter.drawEllipse(QPointF(centerX, centerY), backgroundSize / 2 + 1, backgroundSize / 2 + 1); + + // Draw the background semicircle + QPen semicirclePen(QColor(50, 50, 50)); // Dark gray for the semicircle + semicirclePen.setWidth(30); // Thicker pen for the semicircle + semicirclePen.setCapStyle(Qt::RoundCap); + painter.setPen(semicirclePen); + painter.drawArc(QRectF(centerX - gaugeSize / 2, centerY - gaugeSize / 2, gaugeSize, gaugeSize), 0, 180 * 16); + + // Determine the color based on the magnitude of lateral force + QColor indicatorColor; + float absoluteLateral = std::abs(currentLateral); // TODO: its too choppy, something is wrong here + if (absoluteLateral < 5.0) { // Low lateral force + indicatorColor = QColor(23, 241, 66, 200); // Green + } else if (absoluteLateral < 15.0) { // Moderate lateral force + indicatorColor = QColor(255, 166, 0, 200); // Yellow + } else { // High lateral force + indicatorColor = QColor(245, 0, 0, 200); // Red + } + + // Calculate the span of the arc based on lateral force + int spanAngle = static_cast(90 * (absoluteLateral / 15.0) * 16); // Scale for better visibility + spanAngle = std::clamp(spanAngle, 0, 90 * 16); // Ensure the arc does not exceed 90 degrees + + // Starting angle is at the middle of the semicircle (90 degrees) + int startAngle = 90 * 16; + + // Draw the lateral arc if there's significant lateral force + if (absoluteLateral > 0.1) { + semicirclePen.setColor(indicatorColor); + painter.setPen(semicirclePen); + + QRectF arcRect(centerX - gaugeSize / 2, centerY - gaugeSize / 2, gaugeSize, gaugeSize); + + // For left turn (negative lateral), draw the arc on the left side + if (currentLateral < 0) { + painter.drawArc(arcRect, startAngle, -spanAngle); // Negative span for left side + } + // For right turn (positive lateral), draw the arc on the right side + else { + painter.drawArc(arcRect, startAngle, spanAngle); // Positive span for right side + } + } + + // Draw the text in the center + painter.setPen(Qt::white); + QFont font = painter.font(); + font.setPixelSize(20); + font.setBold(true); + painter.setFont(font); + painter.drawText(QRectF(centerX - 50, centerY + 10, 100, 20), Qt::AlignCenter, "LAT"); +} + + + + + + + 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 7e1b43acb4..5876fa7d5d 100644 --- a/selfdrive/ui/qt/onroad/model.h +++ b/selfdrive/ui/qt/onroad/model.h @@ -23,10 +23,11 @@ private: void update_leads(const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line); void update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead); void drawLaneLines(QPainter &painter); - void drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, int height); + void drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, int height, int width); void updatePathGradient(QLinearGradient &bg); QColor blendColors(const QColor &start, const QColor &end, float t); - + void LongFuel(QPainter &p, int height, int width); + void LateralFuel(QPainter &p, int height, int width); bool longitudinal_control = false; bool experimental_mode = false; float blend_factor = 1.0f; diff --git a/selfdrive/ui/sunnypilot/ui.cc b/selfdrive/ui/sunnypilot/ui.cc index 37da83949b..891f56bf53 100644 --- a/selfdrive/ui/sunnypilot/ui.cc +++ b/selfdrive/ui/sunnypilot/ui.cc @@ -18,7 +18,7 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) { "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2", "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan", - "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", + "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "carControl", }); // update timer