diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc index f4ff62227..cc5368c2a 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.cc +++ b/selfdrive/ui/qt/onroad/onroad_home.cc @@ -87,8 +87,11 @@ void OnroadWindow::updateState(const UIState &s) { blindSpotLeft = scene.blind_spot_left; blindSpotRight = scene.blind_spot_right; showBlindspot = scene.show_blind_spot && (blindSpotLeft || blindSpotRight); + showSteering = scene.show_steering; + steer = scene.steer; + steeringAngleDeg = scene.steering_angle_deg; - if (showBlindspot) { + if (showBlindspot || showSteering) { shouldUpdate = true; } @@ -212,6 +215,41 @@ void OnroadWindow::paintEvent(QPaintEvent *event) { QColor bgColor(bg.red(), bg.green(), bg.blue(), 255); p.fillRect(rect, bgColor); + if (showSteering) { + static float smoothedSteer = 0.0; + + smoothedSteer = 0.1 * std::abs(steer) + 0.9 * smoothedSteer; + + if (std::abs(smoothedSteer - steer) < 0.01) { + smoothedSteer = steer; + } + + int visibleHeight = rect.height() * smoothedSteer; + + QLinearGradient gradient(rect.topLeft(), rect.bottomLeft()); + gradient.setColorAt(0.0, bg_colors[STATUS_TRAFFIC_MODE_ACTIVE]); + gradient.setColorAt(0.15, bg_colors[STATUS_EXPERIMENTAL_MODE_ACTIVE]); + gradient.setColorAt(0.5, bg_colors[STATUS_CONDITIONAL_OVERRIDDEN]); + gradient.setColorAt(0.85, bg_colors[STATUS_ENGAGED]); + gradient.setColorAt(1.0, bg_colors[STATUS_ENGAGED]); + + QBrush brush(gradient); + int fillWidth = UI_BORDER_SIZE; + + if (steeringAngleDeg != 0) { + QRect rectToFill, rectToHide; + if (steeringAngleDeg < 0) { + rectToFill = QRect(rect.x(), rect.y() + rect.height() - visibleHeight, fillWidth, visibleHeight); + rectToHide = QRect(rect.x(), rect.y(), fillWidth, rect.height() - visibleHeight); + } else { + rectToFill = QRect(rect.x() + rect.width() - fillWidth, rect.y() + rect.height() - visibleHeight, fillWidth, visibleHeight); + rectToHide = QRect(rect.x() + rect.width() - fillWidth, rect.y(), fillWidth, rect.height() - visibleHeight); + } + p.fillRect(rectToFill, brush); + p.fillRect(rectToHide, bgColor); + } + } + if (showBlindspot) { QColor blindspotColorLeft = bgColor; QColor blindspotColorRight = bgColor; diff --git a/selfdrive/ui/qt/onroad/onroad_home.h b/selfdrive/ui/qt/onroad/onroad_home.h index 509fb7fb1..47647a04b 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.h +++ b/selfdrive/ui/qt/onroad/onroad_home.h @@ -28,6 +28,11 @@ private: bool blindSpotLeft; bool blindSpotRight; bool showBlindspot; + bool showSteering; + + float steer; + + int steeringAngleDeg; QPoint timeoutPoint = QPoint(420, 69); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 04fabec1a..f41ae3162 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -214,6 +214,7 @@ static void update_state(UIState *s) { } if (sm.updated("carControl")) { auto carControl = sm["carControl"].getCarControl(); + scene.steer = carControl.getActuators().getSteer(); } if (sm.updated("carParams")) { scene.longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl(); @@ -341,6 +342,7 @@ void ui_update_frogpilot_params(UIState *s, Params ¶ms) { bool developer_ui = params.getBool("DeveloperUI"); bool border_metrics = developer_ui && params.getBool("BorderMetrics"); scene.show_blind_spot = border_metrics && params.getBool("BlindSpotMetrics"); + scene.show_steering = border_metrics && params.getBool("ShowSteering"); scene.disable_smoothing_mtsc = params.getBool("MTSCEnabled") && params.getBool("DisableMTSCSmoothing"); scene.disable_smoothing_vtsc = params.getBool("VisionTurnControl") && params.getBool("DisableVTSCSmoothing"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index a404ad21c..cf77740d8 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -159,6 +159,7 @@ typedef struct UIScene { bool show_cem_status_bar; bool show_slc_offset; bool show_slc_offset_ui; + bool show_steering; bool show_stopping_point; bool show_stopping_point_metrics; bool speed_limit_changed; @@ -183,6 +184,7 @@ typedef struct UIScene { float speed_limit; float speed_limit_offset; float speed_limit_overridden_speed; + float steer; float unconfirmed_speed_limit; int alert_size;