diff --git a/common/params_keys.h b/common/params_keys.h index afb6b348e..fc7842720 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -146,6 +146,7 @@ inline static std::unordered_map keys = { {"CustomAccLongPressIncrement", {PERSISTENT | BACKUP, INT, "5"}}, {"CustomAccShortPressIncrement", {PERSISTENT | BACKUP, INT, "1"}}, {"DeviceBootMode", {PERSISTENT | BACKUP, INT, "0"}}, + {"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}}, {"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}}, {"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}}, {"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}}, diff --git a/selfdrive/ui/qt/onroad/alerts.cc b/selfdrive/ui/qt/onroad/alerts.cc index d6829c6b0..2e8f3612e 100644 --- a/selfdrive/ui/qt/onroad/alerts.cc +++ b/selfdrive/ui/qt/onroad/alerts.cc @@ -4,6 +4,9 @@ #include #include "selfdrive/ui/qt/util.h" +#ifdef SUNNYPILOT +#include "selfdrive/ui/sunnypilot/ui.h" +#endif void OnroadAlerts::updateState(const UIState &s) { Alert a = getAlert(*(s.sm), s.scene.started_frame); @@ -73,6 +76,12 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { } QRect r = QRect(0 + margin, height() - h + margin, width() - margin*2, h - margin*2); +#ifdef SUNNYPILOT + const int dev_ui_info = uiStateSP()->scene.dev_ui_info; + const int adjustment = dev_ui_info > 1 && alert.size != cereal::SelfdriveState::AlertSize::FULL ? 30 : 0; + r = QRect(0 + margin, height() - h + margin - adjustment, width() - margin*2, h - margin*2); +#endif + QPainter p(this); // draw background + gradient diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index e3ca83790..5d9d21ab6 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -12,6 +12,7 @@ #include "selfdrive/ui/sunnypilot/qt/onroad/model.h" #define ExperimentalButton ExperimentalButtonSP #define ModelRenderer ModelRendererSP +#define HudRenderer HudRendererSP #else #include "selfdrive/ui/qt/onroad/buttons.h" #include "selfdrive/ui/qt/onroad/hud.h" diff --git a/selfdrive/ui/qt/onroad/driver_monitoring.cc b/selfdrive/ui/qt/onroad/driver_monitoring.cc index 49f2c950b..e67c48304 100644 --- a/selfdrive/ui/qt/onroad/driver_monitoring.cc +++ b/selfdrive/ui/qt/onroad/driver_monitoring.cc @@ -73,6 +73,11 @@ void DriverMonitorRenderer::draw(QPainter &painter, const QRect &surface_rect) { float y = surface_rect.height() - offset; float opacity = is_active ? 0.65f : 0.2f; +#ifdef SUNNYPILOT + const int dev_ui_info = uiStateSP()->scene.dev_ui_info; + y -= dev_ui_info > 1 ? 50 : 0; +#endif + drawIcon(painter, QPoint(x, y), dm_img, QColor(0, 0, 0, 70), opacity); QPointF keypoints[std::size(DEFAULT_FACE_KPTS_3D)]; diff --git a/selfdrive/ui/sunnypilot/SConscript b/selfdrive/ui/sunnypilot/SConscript index 2f3c8ddd8..807bf0247 100644 --- a/selfdrive/ui/sunnypilot/SConscript +++ b/selfdrive/ui/sunnypilot/SConscript @@ -39,6 +39,7 @@ qt_src = [ "sunnypilot/qt/offroad/settings/visuals_panel.cc", "sunnypilot/qt/onroad/annotated_camera.cc", "sunnypilot/qt/onroad/buttons.cc", + "sunnypilot/qt/onroad/developer_ui/developer_ui.cc", "sunnypilot/qt/onroad/hud.cc", "sunnypilot/qt/onroad/model.cc", "sunnypilot/qt/onroad/onroad_home.cc", diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc index dd2f05416..c3aaf12d2 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc @@ -72,6 +72,15 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) { list->addItem(chevron_info_settings); param_watcher->addParam("ChevronInfo"); + // Visuals: Developer UI Info (Dev UI) + std::vector dev_ui_settings_texts{tr("Off"), tr("Right"), tr("Right &&\nBottom")}; + dev_ui_settings = new ButtonParamControlSP( + "DevUIInfo", tr("Developer UI"), tr("Display real-time parameters and metrics from various sources."), + "", + dev_ui_settings_texts, + 380); + list->addItem(dev_ui_settings); + sunnypilotScroller = new ScrollViewSP(list, this); vlayout->addWidget(sunnypilotScroller); @@ -90,4 +99,7 @@ void VisualsPanel::paramsRefresh() { if (chevron_info_settings) { chevron_info_settings->refresh(); } + if (dev_ui_settings) { + dev_ui_settings->refresh(); + } } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.h index f342662c2..30ff31c30 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.h @@ -28,4 +28,5 @@ protected: std::map toggles; ParamWatcher * param_watcher; ButtonParamControlSP *chevron_info_settings; + ButtonParamControlSP *dev_ui_settings; }; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.cc b/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.cc index 3721a3d19..1d5567161 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.cc @@ -14,3 +14,8 @@ AnnotatedCameraWidgetSP::AnnotatedCameraWidgetSP(VisionStreamType type, QWidget void AnnotatedCameraWidgetSP::updateState(const UIState &s) { AnnotatedCameraWidget::updateState(s); } + +void AnnotatedCameraWidgetSP::showEvent(QShowEvent *event) { + AnnotatedCameraWidget::showEvent(event); + ui_update_params_sp(uiState()); +} diff --git a/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.h b/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.h index 46ce7d4be..8c0a38565 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.h @@ -15,4 +15,7 @@ class AnnotatedCameraWidgetSP : public AnnotatedCameraWidget { public: explicit AnnotatedCameraWidgetSP(VisionStreamType type, QWidget *parent = nullptr); void updateState(const UIState &s) override; + +protected: + void showEvent(QShowEvent *event) override; }; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.cc b/selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.cc new file mode 100644 index 000000000..292ba6f7b --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.cc @@ -0,0 +1,227 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ +#include + +#include "common/util.h" +#include "selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.h" + + +// Add Relative Distance to Primary Lead Car +// Unit: Meters +UiElement DeveloperUi::getDRel(bool lead_status, float lead_d_rel) { + QString value = lead_status ? QString::number(lead_d_rel, 'f', 0) : "-"; + QColor color = QColor(255, 255, 255, 255); + + if (lead_status) { + // Orange if close, Red if very close + if (lead_d_rel < 5) { + color = QColor(255, 0, 0, 255); + } else if (lead_d_rel < 15) { + color = QColor(255, 188, 0, 255); + } + } + + return UiElement(value, "REL DIST", "m", color); +} + +// Add Relative Velocity vs Primary Lead Car +// Unit: kph if metric, else mph +UiElement DeveloperUi::getVRel(bool lead_status, float lead_v_rel, bool is_metric, const QString &speed_unit) { + QString value = lead_status ? QString::number(lead_v_rel * (is_metric ? MS_TO_KPH : MS_TO_MPH), 'f', 0) : "-"; + QColor color = QColor(255, 255, 255, 255); + + if (lead_status) { + // Red if approaching faster than 10mph + // Orange if approaching (negative) + if (lead_v_rel < -4.4704) { + color = QColor(255, 0, 0, 255); + } else if (lead_v_rel < 0) { + color = QColor(255, 188, 0, 255); + } + } + + return UiElement(value, "REL SPEED", speed_unit, color); +} + +// Add Real Steering Angle +// Unit: Degrees +UiElement DeveloperUi::getSteeringAngleDeg(float angle_steers, bool lat_active, bool steer_override) { + QString value = QString("%1%2%3").arg(QString::number(angle_steers, 'f', 1)).arg("°").arg(""); + QColor color = lat_active ? (steer_override ? QColor(0x91, 0x9b, 0x95, 0xff) : QColor(0, 255, 0, 255)) : QColor(255, 255, 255, 255); + + // Red if large steering angle + // Orange if moderate steering angle + if (std::fabs(angle_steers) > 180) { + color = QColor(255, 0, 0, 255); + } else if (std::fabs(angle_steers) > 90) { + color = QColor(255, 188, 0, 255); + } + + return UiElement(value, "REAL STEER", "", color); +} + +// Add Actual Lateral Acceleration (roll compensated) when using Torque +// Unit: m/s² +UiElement DeveloperUi::getActualLateralAccel(float curvature, float v_ego, float roll, bool lat_active, bool steer_override) { + double actualLateralAccel = (curvature * pow(v_ego, 2)) - (roll * 9.81); + + QString value = QString::number(actualLateralAccel, 'f', 2); + QColor color = lat_active ? (steer_override ? QColor(0x91, 0x9b, 0x95, 0xff) : QColor(0, 255, 0, 255)) : QColor(255, 255, 255, 255); + + return UiElement(value, "ACTUAL L.A.", "m/s²", color); +} + +// Add Desired Steering Angle when using PID +// Unit: Degrees +UiElement DeveloperUi::getSteeringAngleDesiredDeg(bool lat_active, float steer_angle_desired, float angle_steers) { + QString value = lat_active ? QString("%1%2%3").arg(QString::number(steer_angle_desired, 'f', 1)).arg("°").arg("") : "-"; + QColor color = QColor(255, 255, 255, 255); + + if (lat_active) { + // Red if large steering angle + // Orange if moderate steering angle + if (std::fabs(angle_steers) > 180) { + color = QColor(255, 0, 0, 255); + } else if (std::fabs(angle_steers) > 90) { + color = QColor(255, 188, 0, 255); + } else { + color = QColor(0, 255, 0, 255); + } + } + + return UiElement(value, "DESIRED STEER", "", color); +} + +// Add Device Memory (RAM) Usage +// Unit: Percent +UiElement DeveloperUi::getMemoryUsagePercent(int memory_usage_percent) { + QString value = QString("%1%2").arg(QString::number(memory_usage_percent, 'd', 0)).arg("%"); + QColor color = (memory_usage_percent > 85) ? QColor(255, 188, 0, 255) : QColor(255, 255, 255, 255); + + return UiElement(value, "RAM", "", color); +} + +// Add Vehicle Current Acceleration +// Unit: m/s² +UiElement DeveloperUi::getAEgo(float a_ego) { + QString value = QString::number(a_ego, 'f', 1); + QColor color = QColor(255, 255, 255, 255); + + return UiElement(value, "ACC.", "m/s²", color); +} + +// Add Relative Velocity to Primary Lead Car +// Unit: kph if metric, else mph +UiElement DeveloperUi::getVEgoLead(bool lead_status, float lead_v_rel, float v_ego, bool is_metric, const QString &speed_unit) { + QString value = lead_status ? QString::number((lead_v_rel + v_ego) * (is_metric ? MS_TO_KPH : MS_TO_MPH), 'f', 0) : "-"; + QColor color = QColor(255, 255, 255, 255); + + if (lead_status) { + // Red if approaching faster than 10mph + // Orange if approaching (negative) + if (lead_v_rel < -4.4704) { + color = QColor(255, 0, 0, 255); + } else if (lead_v_rel < 0) { + color = QColor(255, 188, 0, 255); + } + } + + return UiElement(value, "L.S.", speed_unit, color); +} + +// Add Friction Coefficient Raw from torqued +// Unit: None +UiElement DeveloperUi::getFrictionCoefficientFiltered(float friction_coefficient_filtered, bool live_valid) { + QString value = QString::number(friction_coefficient_filtered, 'f', 3); + QColor color = live_valid ? QColor(0, 255, 0, 255) : QColor(255, 255, 255, 255); + + return UiElement(value, "FRIC.", "", color); +} + +// Add Lateral Acceleration Factor Raw from torqued +// Unit: m/s² +UiElement DeveloperUi::getLatAccelFactorFiltered(float lat_accel_factor_filtered, bool live_valid) { + QString value = QString::number(lat_accel_factor_filtered, 'f', 3); + QColor color = live_valid ? QColor(0, 255, 0, 255) : QColor(255, 255, 255, 255); + + return UiElement(value, "L.A.", "m/s²", color); +} + +// Add Steering Torque from Car EPS +// Unit: Newton Meters +UiElement DeveloperUi::getSteeringTorqueEps(float steering_torque_eps) { + QString value = QString::number(std::fabs(steering_torque_eps), 'f', 1); + QColor color = QColor(255, 255, 255, 255); + + return UiElement(value, "E.T.", "N·dm", color); +} + +// Add Bearing Degree and Direction from Car (Compass) +// Unit: Meters +UiElement DeveloperUi::getBearingDeg(float bearing_accuracy_deg, float bearing_deg) { + QString value = (bearing_accuracy_deg != 180.00) ? QString("%1%2%3").arg(QString::number(bearing_deg, 'd', 0)).arg("°").arg("") : "-"; + QColor color = QColor(255, 255, 255, 255); + QString dir_value; + + if (bearing_accuracy_deg != 180.00) { + if (((bearing_deg >= 337.5) && (bearing_deg <= 360)) || ((bearing_deg >= 0) && (bearing_deg <= 22.5))) { + dir_value = "N"; + } else if ((bearing_deg > 22.5) && (bearing_deg < 67.5)) { + dir_value = "NE"; + } else if ((bearing_deg >= 67.5) && (bearing_deg <= 112.5)) { + dir_value = "E"; + } else if ((bearing_deg > 112.5) && (bearing_deg < 157.5)) { + dir_value = "SE"; + } else if ((bearing_deg >= 157.5) && (bearing_deg <= 202.5)) { + dir_value = "S"; + } else if ((bearing_deg > 202.5) && (bearing_deg < 247.5)) { + dir_value = "SW"; + } else if ((bearing_deg >= 247.5) && (bearing_deg <= 292.5)) { + dir_value = "W"; + } else if ((bearing_deg > 292.5) && (bearing_deg < 337.5)) { + dir_value = "NW"; + } + } else { + dir_value = "OFF"; + } + + return UiElement(QString("%1 | %2").arg(dir_value).arg(value), "B.D.", "", color); +} + +// Add Altitude of Current Location +// Unit: Meters +UiElement DeveloperUi::getAltitude(float gps_accuracy, float altitude) { + QString value = (gps_accuracy != 0.00) ? QString::number(altitude, 'f', 1) : "-"; + QColor color = QColor(255, 255, 255, 255); + + return UiElement(value, "ALT.", "m", color); +} + +// Add Actuators Output +// Unit: Degree (angle) or m/s² (torque) +UiElement DeveloperUi::getActuatorsOutputLateral(cereal::CarParams::SteerControlType steerControlType, + cereal::CarControl::Actuators::Reader &actuators, + float desiredCurvature, float v_ego, float roll, bool lat_active, bool steer_override) { + QString label; + QString value; + QString unit; + + if (steerControlType == cereal::CarParams::SteerControlType::ANGLE) { + label = "DESIRED STEER"; + value = QString("%1%2%3").arg(QString::number(actuators.getSteeringAngleDeg(), 'f', 1)).arg("°").arg(""); + } else { + label = "DESIRED L.A."; + double desiredLateralAccel = (desiredCurvature * pow(v_ego, 2)) - (roll * 9.81); + value = QString::number(desiredLateralAccel, 'f', 2); + unit = "m/s²"; + } + + value = lat_active ? value : "-"; + QColor color = lat_active ? (steer_override ? QColor(0x91, 0x9b, 0x95, 0xff) : QColor(0, 255, 0, 255)) : QColor(255, 255, 255, 255); + + return UiElement(value, label, unit, color); +} diff --git a/selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.h b/selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.h new file mode 100644 index 000000000..0c5c47220 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.h @@ -0,0 +1,31 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ +#pragma once + +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/sunnypilot/qt/onroad/developer_ui/ui_elements.h" + +class DeveloperUi { + +public: + static UiElement getDRel(bool lead_status, float lead_d_rel); + static UiElement getVRel(bool lead_status, float lead_v_rel, bool is_metric, const QString &speed_unit); + static UiElement getSteeringAngleDeg(float angle_steers, bool lat_active, bool steer_override); + static UiElement getActualLateralAccel(float curvature, float v_ego, float roll, bool lat_active, bool steer_override); + static UiElement getSteeringAngleDesiredDeg(bool lat_active, float steer_angle_desired, float angle_steers); + static UiElement getMemoryUsagePercent(int memory_usage_percent); + static UiElement getAEgo(float a_ego); + static UiElement getVEgoLead(bool lead_status, float lead_v_rel, float v_ego, bool is_metric, const QString &speed_unit); + static UiElement getFrictionCoefficientFiltered(float friction_coefficient_filtered, bool live_valid); + static UiElement getLatAccelFactorFiltered(float lat_accel_factor_filtered, bool live_valid); + static UiElement getSteeringTorqueEps(float steering_torque_eps); + static UiElement getBearingDeg(float bearing_accuracy_deg, float bearing_deg); + static UiElement getAltitude(float gps_accuracy, float altitude); + static UiElement getActuatorsOutputLateral(cereal::CarParams::SteerControlType steerControlType, + cereal::CarControl::Actuators::Reader &actuators, + float desiredCurvature, float v_ego, float roll, bool lat_active, bool steer_override); +}; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/developer_ui/ui_elements.h b/selfdrive/ui/sunnypilot/qt/onroad/developer_ui/ui_elements.h new file mode 100644 index 000000000..3711e5ac0 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/onroad/developer_ui/ui_elements.h @@ -0,0 +1,19 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ +#pragma once + +#include + +struct UiElement { + QString value{}; + QString label{}; + QString units{}; + QColor color{}; + + explicit UiElement(const QString &value = "", const QString &label = "", const QString &units = "", const QColor &color = QColor(255, 255, 255, 255)) + : value(value), label(label), units(units), color(color) {} +}; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc index 233ca59f9..15722cc9f 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc @@ -7,12 +7,202 @@ #include "selfdrive/ui/sunnypilot/qt/onroad/hud.h" +#include "selfdrive/ui/qt/util.h" + + HudRendererSP::HudRendererSP() {} void HudRendererSP::updateState(const UIState &s) { HudRenderer::updateState(s); + + const SubMaster &sm = *(s.sm); + const bool cs_alive = sm.alive("controlsState"); + const auto cs = sm["controlsState"].getControlsState(); + const auto car_state = sm["carState"].getCarState(); + const auto car_control = sm["carControl"].getCarControl(); + const auto radar_state = sm["radarState"].getRadarState(); + const auto is_gps_location_external = sm.rcv_frame("gpsLocationExternal") > 1; + const auto gpsLocation = is_gps_location_external ? sm["gpsLocationExternal"].getGpsLocationExternal() : sm["gpsLocation"].getGpsLocation(); + const auto ltp = sm["liveTorqueParameters"].getLiveTorqueParameters(); + const auto car_params = sm["carParams"].getCarParams(); + + static int reverse_delay = 0; + bool reverse_allowed = false; + if (int(car_state.getGearShifter()) != 4) { + reverse_delay = 0; + reverse_allowed = false; + } else { + reverse_delay += 50; + if (reverse_delay >= 1000) { + reverse_allowed = true; + } + } + + reversing = reverse_allowed; + is_metric = s.scene.is_metric; + + // Handle older routes where vEgoCluster is not set + v_ego_cluster_seen = v_ego_cluster_seen || car_state.getVEgoCluster() != 0.0; + float v_ego = v_ego_cluster_seen ? car_state.getVEgoCluster() : car_state.getVEgo(); + speed = cs_alive ? std::max(0.0, v_ego) : 0.0; + speed *= is_metric ? MS_TO_KPH : MS_TO_MPH; + + latActive = car_control.getLatActive(); + steerOverride = car_state.getSteeringPressed(); + + devUiInfo = s.scene.dev_ui_info; + + speedUnit = is_metric ? tr("km/h") : tr("mph"); + lead_d_rel = radar_state.getLeadOne().getDRel(); + lead_v_rel = radar_state.getLeadOne().getVRel(); + lead_status = radar_state.getLeadOne().getStatus(); + steerControlType = car_params.getSteerControlType(); + actuators = car_control.getActuators(); + torqueLateral = steerControlType == cereal::CarParams::SteerControlType::TORQUE; + angleSteers = car_state.getSteeringAngleDeg(); + desiredCurvature = cs.getDesiredCurvature(); + curvature = cs.getCurvature(); + roll = sm["liveParameters"].getLiveParameters().getRoll(); + memoryUsagePercent = sm["deviceState"].getDeviceState().getMemoryUsagePercent(); + gpsAccuracy = is_gps_location_external ? gpsLocation.getHorizontalAccuracy() : 1.0; // External reports accuracy, internal does not. + altitude = gpsLocation.getAltitude(); + vEgo = car_state.getVEgo(); + aEgo = car_state.getAEgo(); + steeringTorqueEps = car_state.getSteeringTorqueEps(); + bearingAccuracyDeg = gpsLocation.getBearingAccuracyDeg(); + bearingDeg = gpsLocation.getBearingDeg(); + torquedUseParams = ltp.getUseParams(); + latAccelFactorFiltered = ltp.getLatAccelFactorFiltered(); + frictionCoefficientFiltered = ltp.getFrictionCoefficientFiltered(); + liveValid = ltp.getLiveValid(); } void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) { HudRenderer::draw(p, surface_rect); + if (!reversing) { + // Bottom Dev UI + if (devUiInfo == 2) { + QRect rect_bottom(surface_rect.left(), surface_rect.bottom() - 60, surface_rect.width(), 61); + p.setPen(Qt::NoPen); + p.setBrush(QColor(0, 0, 0, 100)); + p.drawRect(rect_bottom); + drawBottomDevUI(p, rect_bottom.left(), rect_bottom.center().y()); + } + + // Right Dev UI + if (devUiInfo != 0) { + QRect rect_right(surface_rect.right() - (UI_BORDER_SIZE * 2), UI_BORDER_SIZE * 1.5, 184, 170); + drawRightDevUI(p, surface_rect.right() - 184 - UI_BORDER_SIZE * 2, UI_BORDER_SIZE * 2 + rect_right.height()); + } + } +} + +void HudRendererSP::drawText(QPainter &p, int x, int y, const QString &text, QColor color) { + QRect real_rect = p.fontMetrics().boundingRect(text); + real_rect.moveCenter({x, y - real_rect.height() / 2}); + p.setPen(color); + p.drawText(real_rect.x(), real_rect.bottom(), text); +} + +int HudRendererSP::drawRightDevUIElement(QPainter &p, int x, int y, const QString &value, const QString &label, const QString &units, QColor &color) { + + p.setFont(InterFont(28, QFont::Bold)); + x += 92; + y += 80; + drawText(p, x, y, label); + + p.setFont(InterFont(30 * 2, QFont::Bold)); + y += 65; + drawText(p, x, y, value, color); + + p.setFont(InterFont(28, QFont::Bold)); + + if (units.length() > 0) { + p.save(); + x += 120; + y -= 25; + p.translate(x, y); + p.rotate(-90); + drawText(p, 0, 0, units); + p.restore(); + } + + return 130; +} + +void HudRendererSP::drawRightDevUI(QPainter &p, int x, int y) { + int rh = 5; + int ry = y; + + UiElement dRelElement = DeveloperUi::getDRel(lead_status, lead_d_rel); + rh += drawRightDevUIElement(p, x, ry, dRelElement.value, dRelElement.label, dRelElement.units, dRelElement.color); + ry = y + rh; + + UiElement vRelElement = DeveloperUi::getVRel(lead_status, lead_v_rel, is_metric, speedUnit); + rh += drawRightDevUIElement(p, x, ry, vRelElement.value, vRelElement.label, vRelElement.units, vRelElement.color); + ry = y + rh; + + UiElement steeringAngleDegElement = DeveloperUi::getSteeringAngleDeg(angleSteers, latActive, steerOverride); + rh += drawRightDevUIElement(p, x, ry, steeringAngleDegElement.value, steeringAngleDegElement.label, steeringAngleDegElement.units, steeringAngleDegElement.color); + ry = y + rh; + + UiElement actuatorsOutputLateralElement = DeveloperUi::getActuatorsOutputLateral(steerControlType, actuators, desiredCurvature, vEgo, roll, latActive, steerOverride); + rh += drawRightDevUIElement(p, x, ry, actuatorsOutputLateralElement.value, actuatorsOutputLateralElement.label, actuatorsOutputLateralElement.units, actuatorsOutputLateralElement.color); + ry = y + rh; + + UiElement actualLateralAccelElement = DeveloperUi::getActualLateralAccel(curvature, vEgo, roll, latActive, steerOverride); + rh += drawRightDevUIElement(p, x, ry, actualLateralAccelElement.value, actualLateralAccelElement.label, actualLateralAccelElement.units, actualLateralAccelElement.color); +} + +int HudRendererSP::drawBottomDevUIElement(QPainter &p, int x, int y, const QString &value, const QString &label, const QString &units, QColor &color) { + p.setFont(InterFont(38, QFont::Bold)); + QFontMetrics fm(p.font()); + QRect init_rect = fm.boundingRect(label + " "); + QRect real_rect = fm.boundingRect(init_rect, 0, label + " "); + real_rect.moveCenter({x, y}); + + QRect init_rect2 = fm.boundingRect(value); + QRect real_rect2 = fm.boundingRect(init_rect2, 0, value); + real_rect2.moveTop(real_rect.top()); + real_rect2.moveLeft(real_rect.right() + 10); + + QRect init_rect3 = fm.boundingRect(units); + QRect real_rect3 = fm.boundingRect(init_rect3, 0, units); + real_rect3.moveTop(real_rect.top()); + real_rect3.moveLeft(real_rect2.right() + 10); + + p.setPen(QColorConstants::White); + p.drawText(real_rect, Qt::AlignLeft | Qt::AlignVCenter, label); + + p.setPen(color); + p.drawText(real_rect2, Qt::AlignRight | Qt::AlignVCenter, value); + p.drawText(real_rect3, Qt::AlignLeft | Qt::AlignVCenter, units); + return 430; +} + +void HudRendererSP::drawBottomDevUI(QPainter &p, int x, int y) { + int rw = 90; + + UiElement aEgoElement = DeveloperUi::getAEgo(aEgo); + rw += drawBottomDevUIElement(p, rw, y, aEgoElement.value, aEgoElement.label, aEgoElement.units, aEgoElement.color); + + UiElement vEgoLeadElement = DeveloperUi::getVEgoLead(lead_status, lead_v_rel, vEgo, is_metric, speedUnit); + rw += drawBottomDevUIElement(p, rw, y, vEgoLeadElement.value, vEgoLeadElement.label, vEgoLeadElement.units, vEgoLeadElement.color); + + if (torqueLateral && torquedUseParams) { + UiElement frictionCoefficientFilteredElement = DeveloperUi::getFrictionCoefficientFiltered(frictionCoefficientFiltered, liveValid); + rw += drawBottomDevUIElement(p, rw, y, frictionCoefficientFilteredElement.value, frictionCoefficientFilteredElement.label, frictionCoefficientFilteredElement.units, frictionCoefficientFilteredElement.color); + + UiElement latAccelFactorFilteredElement = DeveloperUi::getLatAccelFactorFiltered(latAccelFactorFiltered, liveValid); + rw += drawBottomDevUIElement(p, rw, y, latAccelFactorFilteredElement.value, latAccelFactorFilteredElement.label, latAccelFactorFilteredElement.units, latAccelFactorFilteredElement.color); + } else { + UiElement steeringTorqueEpsElement = DeveloperUi::getSteeringTorqueEps(steeringTorqueEps); + rw += drawBottomDevUIElement(p, rw, y, steeringTorqueEpsElement.value, steeringTorqueEpsElement.label, steeringTorqueEpsElement.units, steeringTorqueEpsElement.color); + + UiElement bearingDegElement = DeveloperUi::getBearingDeg(bearingAccuracyDeg, bearingDeg); + rw += drawBottomDevUIElement(p, rw, y, bearingDegElement.value, bearingDegElement.label, bearingDegElement.units, bearingDegElement.color); + } + + UiElement altitudeElement = DeveloperUi::getAltitude(gpsAccuracy, altitude); + rw += drawBottomDevUIElement(p, rw, y, altitudeElement.value, altitudeElement.label, altitudeElement.units, altitudeElement.color); } diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.h b/selfdrive/ui/sunnypilot/qt/onroad/hud.h index 1e98cd3a5..d869d989d 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.h @@ -7,9 +7,8 @@ #pragma once -#include - #include "selfdrive/ui/qt/onroad/hud.h" +#include "selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.h" class HudRendererSP : public HudRenderer { Q_OBJECT @@ -18,4 +17,40 @@ public: HudRendererSP(); void updateState(const UIState &s) override; void draw(QPainter &p, const QRect &surface_rect) override; + +private: + Params params; + void drawText(QPainter &p, int x, int y, const QString &text, QColor color = QColorConstants::White); + void drawRightDevUI(QPainter &p, int x, int y); + int drawRightDevUIElement(QPainter &p, int x, int y, const QString &value, const QString &label, const QString &units, QColor &color); + int drawBottomDevUIElement(QPainter &p, int x, int y, const QString &value, const QString &label, const QString &units, QColor &color); + void drawBottomDevUI(QPainter &p, int x, int y); + + bool lead_status; + float lead_d_rel; + float lead_v_rel; + bool torqueLateral; + float angleSteers; + float desiredCurvature; + float curvature; + float roll; + int memoryUsagePercent; + int devUiInfo; + float gpsAccuracy; + float altitude; + float vEgo; + float aEgo; + float steeringTorqueEps; + float bearingAccuracyDeg; + float bearingDeg; + bool torquedUseParams; + float latAccelFactorFiltered; + float frictionCoefficientFiltered; + bool liveValid; + QString speedUnit; + bool latActive; + bool steerOverride; + bool reversing; + cereal::CarParams::SteerControlType steerControlType; + cereal::CarControl::Actuators::Reader actuators; }; diff --git a/selfdrive/ui/sunnypilot/ui.cc b/selfdrive/ui/sunnypilot/ui.cc index b2701356c..1277195df 100644 --- a/selfdrive/ui/sunnypilot/ui.cc +++ b/selfdrive/ui/sunnypilot/ui.cc @@ -18,13 +18,22 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) { "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2", "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan", - "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP" + "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP", + "carControl", "gpsLocationExternal", "gpsLocation", "liveTorqueParameters", + "carStateSP", "liveParameters" }); // update timer timer = new QTimer(this); QObject::connect(timer, &QTimer::timeout, this, &UIStateSP::update); timer->start(1000 / UI_FREQ); + + // Param watcher for UIScene param updates + param_watcher = new ParamWatcher(this); + connect(param_watcher, &ParamWatcher::paramChanged, [=](const QString ¶m_name, const QString ¶m_value) { + ui_update_params_sp(this); + }); + param_watcher->addParam("DevUIInfo"); } // This method overrides completely the update method from the parent class intentionally. @@ -39,6 +48,11 @@ void UIStateSP::update() { emit uiUpdate(*this); } +void ui_update_params_sp(UIStateSP *s) { + auto params = Params(); + s->scene.dev_ui_info = std::atoi(params.get("DevUIInfo").c_str()); +} + DeviceSP::DeviceSP(QObject *parent) : Device(parent) { QObject::connect(uiStateSP(), &UIStateSP::uiUpdate, this, &DeviceSP::update); QObject::connect(this, &Device::displayPowerChanged, this, &DeviceSP::handleDisplayPowerChanged); diff --git a/selfdrive/ui/sunnypilot/ui.h b/selfdrive/ui/sunnypilot/ui.h index cf8de1c4b..393f997cb 100644 --- a/selfdrive/ui/sunnypilot/ui.h +++ b/selfdrive/ui/sunnypilot/ui.h @@ -13,6 +13,7 @@ #include "selfdrive/ui/sunnypilot/qt/network/sunnylink/models/role_model.h" #include "selfdrive/ui/sunnypilot/qt/network/sunnylink/models/sponsor_role_model.h" #include "selfdrive/ui/ui.h" +#include "selfdrive/ui/qt/util.h" class UIStateSP : public UIState { Q_OBJECT @@ -73,6 +74,7 @@ private slots: private: std::vector sunnylinkRoles = {}; std::vector sunnylinkUsers = {}; + ParamWatcher *param_watcher; }; UIStateSP *uiStateSP(); @@ -92,3 +94,5 @@ private: DeviceSP *deviceSP(); inline DeviceSP *device() { return deviceSP(); } + +void ui_update_params_sp(UIStateSP *s); diff --git a/selfdrive/ui/sunnypilot/ui_scene.h b/selfdrive/ui/sunnypilot/ui_scene.h new file mode 100644 index 000000000..93e0cd6c9 --- /dev/null +++ b/selfdrive/ui/sunnypilot/ui_scene.h @@ -0,0 +1,12 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#pragma once + +typedef struct UISceneSP : UIScene { + int dev_ui_info = 0; +} UISceneSP; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index e78b573b6..5b3872b3d 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -66,6 +66,11 @@ typedef struct UIScene { uint64_t started_frame; } UIScene; +#ifdef SUNNYPILOT +#include "sunnypilot/ui_scene.h" +#define UIScene UISceneSP +#endif + class UIState : public QObject { Q_OBJECT