get buttonEvents at 100 hz and process for 20 hz consumption

selfdrive/controls/plannerd.py

@@ -26,9 +26,15 @@ def main():
                             'liveMapDataSP', 'carStateSP', gps_location_service],
                            poll='modelV2')
 
+  # Speed Limit Assist
+  sm_ext = messaging.SubMaster(['carState'])
+  speed_limit_assist = longitudinal_planner.sla
+
   while True:
     sm.update()
+    sm_ext.update()
     if sm.updated['modelV2']:
+      speed_limit_assist.update_buttons(sm_ext['carState'])
       longitudinal_planner.update(sm)
       longitudinal_planner.publish(sm, pm)
 

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -46,8 +46,7 @@ class LongitudinalPlannerSP:
     return self.dec.mode()
 
   def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
-    CS = sm['carState']
-    v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
+    v_cruise_cluster_kph = min(sm['carState'].vCruiseCluster, V_CRUISE_MAX)
     v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS
 
     long_enabled = sm['carControl'].enabled
@@ -64,7 +63,7 @@ class LongitudinalPlannerSP:
     # Speed Limit Assist
     has_speed_limit = self.resolver.speed_limit_valid or self.resolver.speed_limit_last_valid
     self.sla.update(long_enabled, long_override, v_ego, a_ego, v_cruise_cluster, self.resolver.speed_limit,
-                    self.resolver.speed_limit_final_last, has_speed_limit, self.resolver.distance, self.events_sp, CS)
+                    self.resolver.speed_limit_final_last, has_speed_limit, self.resolver.distance, self.events_sp)
 
     targets = {
       LongitudinalPlanSource.cruise: (v_cruise, a_ego),

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

@@ -4,6 +4,8 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
 This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
+import time
+
 from cereal import custom, car
 from openpilot.common.params import Params
 from openpilot.common.constants import CV
@@ -32,6 +34,7 @@ LIMIT_MIN_SPEED = 8.33  # m/s, Minimum speed limit to provide as solution on lim
 LIMIT_SPEED_OFFSET_TH = -1.  # m/s Maximum offset between speed limit and current speed for adapting state.
 V_CRUISE_UNSET = 255
 
+CONFIRM_BUTTON_TTL = 0.5  # s
 CRUISE_BUTTONS_PLUS = (ButtonType.accelCruise, ButtonType.resumeCruise)
 CRUISE_BUTTONS_MINUS = (ButtonType.decelCruise, ButtonType.setCruise)
 
@@ -72,6 +75,9 @@ class SpeedLimitAssist:
     self._state_prev = SpeedLimitAssistState.disabled
     self.pcm_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise
 
+    self._plus_hold = 0.0
+    self._minus_hold = 0.0
+
     # TODO-SP: SLA's own output_a_target for planner
     # Solution functions mapped to respective states
     self.acceleration_solutions = {
@@ -149,6 +155,37 @@ class SpeedLimitAssist:
   def get_active_state_target_acceleration(self) -> float:
     return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
 
+  def update_buttons(self, CS: car.CarState) -> None:
+    """
+      This is invoked directly in plannerd, running at 100 Hz.
+    :param CS:
+    :return:
+    """
+    now = time.monotonic()
+    for b in CS.buttonEvents:
+      if not b.pressed:
+        if b.type in CRUISE_BUTTONS_PLUS:
+          self._plus_hold = max(self._plus_hold, now + CONFIRM_BUTTON_TTL)
+        elif b.type in CRUISE_BUTTONS_MINUS:
+          self._minus_hold = max(self._minus_hold, now + CONFIRM_BUTTON_TTL)
+
+  def _get_button(self, req_plus: bool, req_minus: bool) -> bool:
+    now = time.monotonic()
+    if req_plus and now <= self._plus_hold:
+      self._plus_hold = 0.0
+      return True
+    if req_minus and now <= self._minus_hold:
+      self._minus_hold = 0.0
+      return True
+
+    # expired
+    if now > self._plus_hold:
+      self._plus_hold = 0.0
+    if now > self._minus_hold:
+      self._minus_hold = 0.0
+
+    return False
+
   def _update_confirmed_state(self):
     if self._has_speed_limit:
       if self.v_offset < LIMIT_SPEED_OFFSET_TH:
@@ -158,20 +195,16 @@ class SpeedLimitAssist:
     else:
       self.state = SpeedLimitAssistState.pending
 
-  def _update_non_pcm_long_confirmed_state(self, CS) -> bool:
+  def _update_non_pcm_long_confirmed_state(self) -> bool:
     if self.target_set_speed_confirmed:
       return True
 
-    if self.state == SpeedLimitAssistState.preActive:
-      req_plus = self.target_set_speed_conv > self.v_cruise_cluster_conv
-      req_minus = self.target_set_speed_conv < self.v_cruise_cluster_conv
-      expected = CRUISE_BUTTONS_PLUS if req_plus else CRUISE_BUTTONS_MINUS if req_minus else ()
+    if self.state != SpeedLimitAssistState.preActive:
+      return False
 
-      for b in CS.buttonEvents:
-        if b.type in expected and not b.pressed:
-          return True
-
-    return False
+    req_plus = self.target_set_speed_conv > self.v_cruise_cluster_conv
+    req_minus = self.target_set_speed_conv < self.v_cruise_cluster_conv
+    return self._get_button(req_plus, req_minus)
 
   def update_state_machine_pcm_op_long(self):
     self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
@@ -240,7 +273,7 @@ class SpeedLimitAssist:
 
     return enabled, active
 
-  def update_state_machine_non_pcm_long(self, CS: car.CarState):
+  def update_state_machine_non_pcm_long(self):
     self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
     self.pre_active_timer = max(0, self.pre_active_timer - 1)
 
@@ -260,7 +293,7 @@ class SpeedLimitAssist:
 
         # PRE_ACTIVE
         elif self.state == SpeedLimitAssistState.preActive:
-          if self._update_non_pcm_long_confirmed_state(CS):
+          if self._update_non_pcm_long_confirmed_state():
             self.state = SpeedLimitAssistState.active
           elif self.pre_active_timer <= PRE_ACTIVE_GUARD_PERIOD:
             # Timeout - session ended
@@ -271,7 +304,7 @@ class SpeedLimitAssist:
           if self.speed_limit_changed:
             self.state = SpeedLimitAssistState.preActive
             self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
-          elif self._update_non_pcm_long_confirmed_state(CS):
+          elif self._update_non_pcm_long_confirmed_state():
             self.state = SpeedLimitAssistState.active
 
     # DISABLED
@@ -282,7 +315,7 @@ class SpeedLimitAssist:
           self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
 
         elif self.long_engaged_timer <= 0:
-          if self._update_non_pcm_long_confirmed_state(CS):
+          if self._update_non_pcm_long_confirmed_state():
             self.state = SpeedLimitAssistState.active
           else:
             self.state = SpeedLimitAssistState.preActive
@@ -312,7 +345,7 @@ class SpeedLimitAssist:
           events_sp.add(EventNameSP.speedLimitChanged)
 
   def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_cluster: float, speed_limit: float,
-             speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP, CS: car.CarState) -> None:
+             speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP) -> None:
     self.long_enabled = long_enabled
     self.v_ego = v_ego
     self.a_ego = a_ego
@@ -329,7 +362,7 @@ class SpeedLimitAssist:
     if self.pcm_op_long:
       self.is_enabled, self.is_active = self.update_state_machine_pcm_op_long()
     else:
-      self.is_enabled, self.is_active = self.update_state_machine_non_pcm_long(CS)
+      self.is_enabled, self.is_active = self.update_state_machine_non_pcm_long()
 
     self.update_events(events_sp)