diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 854988decf..58628a193b 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -207,6 +207,11 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { distToSpeedLimit @1 :Float32; source @2 :Source; speedLimitOffset @3 :Float32; + speedLimitLast @4 :Float32; + speedLimitFinal @5 :Float32; + speedLimitFinalLast @6 :Float32; + speedLimitValid @7 :Bool; + speedLimitLastValid @8 :Bool; } enum Source { diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc index 6da6baef3d..a3e31455a5 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc @@ -30,9 +30,11 @@ void HudRendererSP::updateState(const UIState &s) { 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 = speedLimit > 0; - speedLimitLastValid = speedLimitLast > 0; + speedLimitValid = lp_sp.getSpeedLimit().getResolver().getSpeedLimitValid(); + speedLimitLastValid = lp_sp.getSpeedLimit().getResolver().getSpeedLimitLastValid(); + speedLimitFinalLast = lp_sp.getSpeedLimit().getResolver().getSpeedLimitFinalLast(); speedLimitMode = static_cast(s.scene.speed_limit_mode); roadName = s.scene.road_name; if (sm.updated("liveMapDataSP")) { @@ -48,10 +50,6 @@ void HudRendererSP::updateState(const UIState &s) { } speedLimitAheadDistancePrev = speedLimitAheadDistance; - if (speedLimitValid) { - speedLimitLast = speedLimit; - } - static int reverse_delay = 0; bool reverse_allowed = false; if (int(car_state.getGearShifter()) != 4) { @@ -351,12 +349,10 @@ void HudRendererSP::drawStandstillTimer(QPainter &p, int x, int y) { } void HudRendererSP::drawSpeedLimitSigns(QPainter &p) { - bool hasSpeedLimit = speedLimitValid || speedLimitLastValid; - int speedLimitFinal = std::nearbyint(speedLimitValid ? speedLimit : speedLimitLast); - int speedLimitOffsetFinal = speedLimitFinal + std::nearbyint(speedLimitOffset); - bool overspeed = hasSpeedLimit && speedLimitOffsetFinal < std::nearbyint(speed); bool speedLimitWarningEnabled = speedLimitMode == SpeedLimitMode::WARNING; // TODO-SP: update to include SpeedLimitMode::ASSIST - QString speedLimitStr = hasSpeedLimit ? QString::number(speedLimitFinal) : "---"; + bool hasSpeedLimit = speedLimitValid || speedLimitLastValid; + bool overspeed = hasSpeedLimit && speedLimitFinalLast < std::nearbyint(speed); + QString speedLimitStr = hasSpeedLimit ? QString::number(std::nearbyint(speedLimitLast)) : "---"; // Offset display text QString speedLimitSubText = ""; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.h b/selfdrive/ui/sunnypilot/qt/onroad/hud.h index 1e153f1548..d77892fa2d 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.h @@ -77,6 +77,7 @@ private: float speedLimitOffset; bool speedLimitValid; bool speedLimitLastValid; + float speedLimitFinalLast; bool speedLimitAheadValid; float speedLimitAhead; float speedLimitAheadDistance; diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index af5f5dbd7d..40d30100c5 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -96,6 +96,11 @@ class LongitudinalPlannerSP: speedLimit = longitudinalPlanSP.speedLimit resolver = speedLimit.resolver resolver.speedLimit = float(self.resolver.speed_limit) + resolver.speedLimitLast = float(self.resolver.speed_limit_last) + resolver.speedLimitFinal = float(self.resolver.speed_limit_final) + resolver.speedLimitFinalLast = float(self.resolver.speed_limit_final_last) + resolver.speedLimitValid = self.resolver.speed_limit_valid + resolver.speedLimitLastValid = self.resolver.speed_limit_last_valid resolver.speedLimitOffset = float(self.resolver.speed_limit_offset) resolver.distToSpeedLimit = float(self.resolver.distance) resolver.source = self.resolver.source diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py index 2a74a01d13..f145b16c10 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py @@ -26,6 +26,9 @@ class SpeedLimitResolver: distance_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float] v_ego: float speed_limit: float + speed_limit_last: float + speed_limit_final: float + speed_limit_final_last: float distance: float source: custom.LongitudinalPlanSP.SpeedLimit.Source speed_limit_offset: float @@ -54,6 +57,21 @@ class SpeedLimitResolver: self.offset_type = self.params.get("SpeedLimitOffsetType", return_default=True) self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True) + def update_speed_limit_states(self) -> None: + self.speed_limit_final = self.speed_limit + self.speed_limit_offset + + if self.speed_limit > 0.: + self.speed_limit_last = self.speed_limit + self.speed_limit_final_last = self.speed_limit_final + + @property + def speed_limit_valid(self) -> bool: + return self.speed_limit > 0. + + @property + def speed_limit_last_valid(self) -> bool: + return self.speed_limit_last > 0. + def update_params(self): if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: self.policy = self.params.get("SpeedLimitPolicy", return_default=True) @@ -149,4 +167,6 @@ class SpeedLimitResolver: self.speed_limit, self.distance, self.source = self._resolve_limit_sources(sm) self.speed_limit_offset = self._get_speed_limit_offset() + self.update_speed_limit_states() + self.frame += 1