Speed Limit Resolver: abstract more speed limit attributes (#1284)

* Speed Limit Resolver: abstract more speed limit attributes

* update ui

* lint

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 {

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<SpeedLimitMode>(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 = "";

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;

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

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