use consolidate method to evaluate set speed status

sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py

@@ -7,6 +7,8 @@ See the LICENSE.md file in the root directory for more details.
 LIMIT_ADAPT_ACC = -1.  # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits.
 LIMIT_MAX_MAP_DATA_AGE = 10.  # s Maximum time to hold to map data, then consider it invalid inside limits controllers.
 
-# Speed Limit Assist Auto mode constants
-REQUIRED_INITIAL_MAX_SET_SPEED = 35.7632  # m/s 80 MPH  # TODO-SP: customizable with params
-CRUISE_SPEED_TOLERANCE = 0.44704  # m/s ±1 MPH tolerance  # TODO-SP: metric vs imperial
+# Speed Limit Assist constants
+PCM_LONG_REQUIRED_MAX_SET_SPEED = {
+  True: 130,  # km/h
+  False: 80,  # mph
+}

sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py

@@ -116,8 +116,15 @@ class SpeedLimitAssist:
       self.is_metric = self.params.get_bool("IsMetric")
       self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
 
-  def initial_max_set_confirmed(self) -> bool:
-    return bool(abs(self.v_cruise_cluster - REQUIRED_INITIAL_MAX_SET_SPEED) <= CRUISE_SPEED_TOLERANCE)
+  @property
+  def target_set_speed_confirmed(self) -> bool:
+    speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH
+    speed_limit_final_conv = round(self.speed_limit_final * speed_conv)
+    v_cruise_cluster_conv = round(self.v_cruise_cluster * speed_conv)
+
+    target_set_speed_conv = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.is_metric] if self.pcm_op_long else speed_limit_final_conv
+
+    return v_cruise_cluster_conv == target_set_speed_conv
 
   def update_calculations(self, v_cruise_cluster: float) -> None:
     self.v_cruise_cluster = v_cruise_cluster if not np.isnan(v_cruise_cluster) else 0.0
@@ -146,7 +153,7 @@ class SpeedLimitAssist:
     else:
       self.state = SpeedLimitAssistState.pending
 
-  def update_state_machine(self):
+  def update_state_machine_pcm_op_long(self):
     self._state_prev = self.state
 
     self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
@@ -182,7 +189,7 @@ class SpeedLimitAssist:
 
         # PRE_ACTIVE
         elif self.state == SpeedLimitAssistState.preActive:
-          if self.initial_max_set_confirmed():
+          if self.target_set_speed_confirmed:
             self._update_pcm_long_confirmed_state()
           elif self.pre_active_timer <= PRE_ACTIVE_GUARD_PERIOD:
             # Timeout - session ended
@@ -200,7 +207,7 @@ class SpeedLimitAssist:
           self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
 
         elif self.long_engaged_timer <= 0:
-          if self.initial_max_set_confirmed():
+          if self.target_set_speed_confirmed:
             self._update_pcm_long_confirmed_state()
           else:
             self.state = SpeedLimitAssistState.preActive
@@ -241,7 +248,12 @@ class SpeedLimitAssist:
 
     self.update_params()
     self.update_calculations(v_cruise_cluster)
-    self.is_enabled, self.is_active = self.update_state_machine()
+
+    if self.pcm_op_long:
+      self.is_enabled, self.is_active = self.update_state_machine_pcm_op_long()
+    else:
+      pass
+
     self.update_events(events_sp)
 
     # Update change tracking variables