diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index a75d1ee..14d7d31 100755 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -341,6 +341,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { const bool cs_alive = sm.alive("controlsState"); const auto cs = sm["controlsState"].getControlsState(); const auto car_state = sm["carState"].getCarState(); + (void)car_state; // CLEARPILOT: suppress unused warning, will use later // Handle older routes where vCruiseCluster is not set float v_cruise = cs.getVCruiseCluster() == 0.0 ? cs.getVCruise() : cs.getVCruiseCluster(); @@ -350,11 +351,15 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { setSpeed *= KM_TO_MILE; } - // 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 && !scene.wheel_speed ? car_state.getVEgoCluster() : car_state.getVEgo(); - speed = cs_alive ? std::max(0.0, v_ego) : 0.0; - speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH; + // CLEARPILOT: use GPS speed if available, hide speed display if no GPS fix + has_gps_speed = sm.valid("gpsLocation") && sm["gpsLocation"].getGpsLocation().getHasFix(); + if (has_gps_speed) { + float gps_speed_ms = sm["gpsLocation"].getGpsLocation().getSpeed(); + speed = std::max(0.0, gps_speed_ms); + speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH; + } else { + speed = 0.0; + } // auto speed_limit_sign = nav_instruction.getSpeedLimitSign(); // speedLimit = slcOverridden ? scene.speed_limit_overridden_speed : speedLimitController ? scene.speed_limit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0; @@ -449,13 +454,10 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { // Todo: lead speed // Todo: Experimental speed - // // current speed - if (!(scene.hide_speed)) { - // CLEARPILOT changes to 120 from ~176 - // Maybe we want to hide this? + // CLEARPILOT: show GPS speed, hide when no fix + if (has_gps_speed && !scene.hide_speed) { p.setFont(InterFont(140, QFont::Bold)); drawText(p, rect().center().x(), 210, speedStr); - // CLEARPILOT changes to 40 from 66 p.setFont(InterFont(50)); drawText(p, rect().center().x(), 290, speedUnit, 200); } diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 0499164..df67925 100755 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -55,6 +55,7 @@ private: QVBoxLayout *main_layout; QPixmap dm_img; float speed; + bool has_gps_speed = false; QString speedUnit; float setSpeed; float speedLimit; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 28a8536..a85a345 100755 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -442,6 +442,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2", "wideRoadCameraState", "managerState", "uiPlan", "liveTorqueParameters", "frogpilotCarControl", "frogpilotDeviceState", "frogpilotPlan", + "gpsLocation", }); Params params;