diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc index fa57e2b334..d8176051fb 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc @@ -26,31 +26,54 @@ HudRendererSP::HudRendererSP() { void HudRendererSP::updateState(const UIState &s) { HudRenderer::updateState(s); + float speedConv = is_metric ? MS_TO_KPH : MS_TO_MPH; + devUiInfo = s.scene.dev_ui_info; + roadName = s.scene.road_name; + showTurnSignals = s.scene.turn_signals; + speedLimitMode = static_cast(s.scene.speed_limit_mode); + speedUnit = is_metric ? tr("km/h") : tr("mph"); + standstillTimer = s.scene.standstill_timer; + const SubMaster &sm = *(s.sm); 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 char *gps_source = is_gps_location_external ? "gpsLocationExternal" : "gpsLocation"; + const auto gpsLocation = is_gps_location_external ? sm[gps_source].getGpsLocationExternal() : sm[gps_source].getGpsLocation(); const auto ltp = sm["liveTorqueParameters"].getLiveTorqueParameters(); const auto car_params = sm["carParams"].getCarParams(); const auto car_params_sp = sm["carParamsSP"].getCarParamsSP(); const auto lp_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP(); const auto lmd = sm["liveMapDataSP"].getLiveMapDataSP(); - float speedConv = is_metric ? MS_TO_KPH : MS_TO_MPH; - speedLimit = lp_sp.getSpeedLimit().getResolver().getSpeedLimit() * speedConv; - speedLimitLast = lp_sp.getSpeedLimit().getResolver().getSpeedLimitLast() * speedConv; - speedLimitOffset = lp_sp.getSpeedLimit().getResolver().getSpeedLimitOffset() * speedConv; - speedLimitValid = lp_sp.getSpeedLimit().getResolver().getSpeedLimitValid(); - speedLimitLastValid = lp_sp.getSpeedLimit().getResolver().getSpeedLimitLastValid(); - speedLimitFinalLast = lp_sp.getSpeedLimit().getResolver().getSpeedLimitFinalLast() * speedConv; - speedLimitSource = lp_sp.getSpeedLimit().getResolver().getSource(); - speedLimitMode = static_cast(s.scene.speed_limit_mode); - speedLimitAssistState = lp_sp.getSpeedLimit().getAssist().getState(); - speedLimitAssistActive = lp_sp.getSpeedLimit().getAssist().getActive(); - roadName = s.scene.road_name; + if (sm.updated("carParams")) { + steerControlType = car_params.getSteerControlType(); + } + + if (sm.updated("carParamsSP")) { + pcmCruiseSpeed = car_params_sp.getPcmCruiseSpeed(); + } + + if (sm.updated("longitudinalPlanSP")) { + speedLimit = lp_sp.getSpeedLimit().getResolver().getSpeedLimit() * speedConv; + speedLimitLast = lp_sp.getSpeedLimit().getResolver().getSpeedLimitLast() * speedConv; + speedLimitOffset = lp_sp.getSpeedLimit().getResolver().getSpeedLimitOffset() * speedConv; + speedLimitValid = lp_sp.getSpeedLimit().getResolver().getSpeedLimitValid(); + speedLimitLastValid = lp_sp.getSpeedLimit().getResolver().getSpeedLimitLastValid(); + speedLimitFinalLast = lp_sp.getSpeedLimit().getResolver().getSpeedLimitFinalLast() * speedConv; + speedLimitSource = lp_sp.getSpeedLimit().getResolver().getSource(); + speedLimitAssistState = lp_sp.getSpeedLimit().getAssist().getState(); + speedLimitAssistActive = lp_sp.getSpeedLimit().getAssist().getActive(); + smartCruiseControlVisionEnabled = lp_sp.getSmartCruiseControl().getVision().getEnabled(); + smartCruiseControlVisionActive = lp_sp.getSmartCruiseControl().getVision().getActive(); + smartCruiseControlMapEnabled = lp_sp.getSmartCruiseControl().getMap().getEnabled(); + smartCruiseControlMapActive = lp_sp.getSmartCruiseControl().getMap().getActive(); + greenLightAlert = lp_sp.getE2eAlerts().getGreenLightAlert(); + leadDepartAlert = lp_sp.getE2eAlerts().getLeadDepartAlert(); + } + if (sm.updated("liveMapDataSP")) { roadNameStr = QString::fromStdString(lmd.getRoadName()); speedLimitAheadValid = lmd.getSpeedLimitAheadValid(); @@ -66,7 +89,7 @@ void HudRendererSP::updateState(const UIState &s) { static int reverse_delay = 0; bool reverse_allowed = false; - if (int(car_state.getGearShifter()) != 4) { + if (car_state.getGearShifter() != cereal::CarState::GearShifter::REVERSE) { reverse_delay = 0; reverse_allowed = false; } else { @@ -78,46 +101,47 @@ void HudRendererSP::updateState(const UIState &s) { reversing = reverse_allowed; + if (sm.updated("liveParameters")) { + roll = sm["liveParameters"].getLiveParameters().getRoll(); + } + + if (sm.updated("deviceState")) { + memoryUsagePercent = sm["deviceState"].getDeviceState().getMemoryUsagePercent(); + } + + if (sm.updated(gps_source)) { + gpsAccuracy = is_gps_location_external ? gpsLocation.getHorizontalAccuracy() : 1.0; // External reports accuracy, internal does not. + altitude = gpsLocation.getAltitude(); + bearingAccuracyDeg = gpsLocation.getBearingAccuracyDeg(); + bearingDeg = gpsLocation.getBearingDeg(); + } + + if (sm.updated("liveTorqueParameters")) { + torquedUseParams = ltp.getUseParams(); + latAccelFactorFiltered = ltp.getLatAccelFactorFiltered(); + frictionCoefficientFiltered = ltp.getFrictionCoefficientFiltered(); + liveValid = ltp.getLiveValid(); + } + latActive = car_control.getLatActive(); + actuators = car_control.getActuators(); + longOverride = car_control.getCruiseControl().getOverride(); + carControlEnabled = car_control.getEnabled(); + 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(); - standstillTimer = s.scene.standstill_timer; isStandstill = car_state.getStandstill(); if (not s.scene.started) standstillElapsedTime = 0.0; - longOverride = car_control.getCruiseControl().getOverride(); - smartCruiseControlVisionEnabled = lp_sp.getSmartCruiseControl().getVision().getEnabled(); - smartCruiseControlVisionActive = lp_sp.getSmartCruiseControl().getVision().getActive(); - smartCruiseControlMapEnabled = lp_sp.getSmartCruiseControl().getMap().getEnabled(); - smartCruiseControlMapActive = lp_sp.getSmartCruiseControl().getMap().getActive(); - - greenLightAlert = lp_sp.getE2eAlerts().getGreenLightAlert(); - leadDepartAlert = lp_sp.getE2eAlerts().getLeadDepartAlert(); // override stock current speed values float v_ego = (v_ego_cluster_seen && !s.scene.trueVEgoUI) ? car_state.getVEgoCluster() : car_state.getVEgo(); @@ -128,11 +152,8 @@ void HudRendererSP::updateState(const UIState &s) { rightBlinkerOn = car_state.getRightBlinker(); leftBlindspot = car_state.getLeftBlindspot(); rightBlindspot = car_state.getRightBlindspot(); - showTurnSignals = s.scene.turn_signals; - carControlEnabled = car_control.getEnabled(); speedCluster = car_state.getCruiseState().getSpeedCluster() * speedConv; - pcmCruiseSpeed = car_params_sp.getPcmCruiseSpeed(); } void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) { diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.h b/selfdrive/ui/sunnypilot/qt/onroad/hud.h index f9135ee9ef..a32fdb28a7 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.h @@ -121,5 +121,5 @@ private: bool carControlEnabled; float speedCluster = 0; int icbm_active_counter = 0; - bool pcmCruiseSpeed; + bool pcmCruiseSpeed = true; };