plannerd & selfdriveStateSP: poll modelV2, relay button state via bitmask (#1893)

* selfdrived: continuous button state and release counters in selfdriveStateSP

* tests and more

openpilot/cereal/custom.capnp

@@ -69,6 +69,8 @@ struct LeadData {
 struct SelfdriveStateSP @0x81c2f05a394cf4af {
   mads @0 :ModularAssistiveDrivingSystem;
   intelligentCruiseButtonManagement @1 :IntelligentCruiseButtonManagement;
+  buttonsPressed @2 :UInt16;
+  buttonsReleaseToggle @3 :UInt16;
 
   enum AudibleAlert {
     none @0;

openpilot/selfdrive/controls/plannerd.py

@@ -29,12 +29,12 @@ def main():
   longitudinal_planner = LongitudinalPlanner(CP, CP_SP)
   pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
   sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
-                            'liveMapDataSP', 'carStateSP', gps_location_service],
-                           poll='carState', ignore_alive=ignore_services, ignore_avg_freq=ignore_services, ignore_valid=ignore_services)
+                            'liveMapDataSP', 'carStateSP', 'selfdriveStateSP', gps_location_service],
+                           poll='modelV2', ignore_alive=ignore_services, ignore_avg_freq=ignore_services, ignore_valid=ignore_services)
 
   while True:
     sm.update()
-    longitudinal_planner.sla.update_car_state(sm['carState'])
+    longitudinal_planner.sla.update_buttons(sm['selfdriveStateSP'].buttonsReleaseToggle)
     if sm.updated['modelV2']:
       longitudinal_planner.update(sm)
       longitudinal_planner.publish(sm, pm)

openpilot/selfdrive/selfdrived/selfdrived.py

@@ -30,6 +30,7 @@ from openpilot.sunnypilot import get_sanitize_int_param
 from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP
 from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper
 from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.controller import IntelligentCruiseButtonManagement
+from openpilot.sunnypilot.selfdrive.selfdrived.button_state_tracker import ButtonStateTracker
 from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 
 REPLAY = "REPLAY" in os.environ
@@ -177,6 +178,7 @@ class SelfdriveD(CruiseHelper):
     self.car_events_sp = CarSpecificEventsSP(self.CP, self.CP_SP)
 
     CruiseHelper.__init__(self, self.CP)
+    self.button_state_tracker = ButtonStateTracker()
 
   def update_events(self, CS):
     """Compute onroadEvents from carState"""
@@ -597,6 +599,8 @@ class SelfdriveD(CruiseHelper):
     icbm.sendButton = self.icbm.cruise_button
     icbm.vTarget = self.icbm.v_target
 
+    self.button_state_tracker.publish(ss_sp)
+
     self.pm.send('selfdriveStateSP', ss_sp_msg)
 
     # onroadEventsSP - logged every second or on change
@@ -616,6 +620,7 @@ class SelfdriveD(CruiseHelper):
       self.mads.update(CS)
     self.update_alerts(CS)
 
+    self.button_state_tracker.update(CS)
     self.publish_selfdriveState(CS)
 
     self.CS_prev = CS

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

@@ -91,7 +91,7 @@ class SpeedLimitAssist:
 
     self._plus_hold = 0.
     self._minus_hold = 0.
-    self._last_carstate_ts = 0.
+    self._release_toggle_prev = 0
 
     # TODO-SP: SLA's own output_a_target for planner
     # Solution functions mapped to respective states
@@ -146,16 +146,16 @@ class SpeedLimitAssist:
       set_speed_limit_assist_availability(self.CP, self.CP_SP, self.params)
       self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
 
-  def update_car_state(self, CS: car.CarState) -> None:
+  def update_buttons(self, release_toggle: int) -> None:
+    released = self._release_toggle_prev ^ release_toggle
+    self._release_toggle_prev = release_toggle
+    if not released:
+      return
     now = time.monotonic()
-    self._last_carstate_ts = now
-
-    for b in CS.buttonEvents:
-      if not b.pressed:
-        if b.type in CRUISE_BUTTONS_PLUS:
-          self._plus_hold = max(self._plus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
-        elif b.type in CRUISE_BUTTONS_MINUS:
-          self._minus_hold = max(self._minus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
+    if any((released >> b) & 1 for b in CRUISE_BUTTONS_PLUS):
+      self._plus_hold = max(self._plus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
+    if any((released >> b) & 1 for b in CRUISE_BUTTONS_MINUS):
+      self._minus_hold = max(self._minus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
 
   def _get_button_release(self, req_plus: bool, req_minus: bool) -> bool:
     now = time.monotonic()

openpilot/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py

@@ -5,11 +5,14 @@ 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
+
 import pytest
 
 from openpilot.cereal import custom
 from opendbc.car.car_helpers import interfaces
 from opendbc.car.rivian.values import CAR as RIVIAN
+from opendbc.car.structs import car
 from opendbc.car.tesla.values import CAR as TESLA
 from opendbc.car.toyota.values import CAR as TOYOTA
 from openpilot.common.constants import CV
@@ -21,9 +24,13 @@ from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfac
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist, \
-  PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES
+  PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES, CRUISE_BUTTON_CONFIRM_HOLD
+from openpilot.sunnypilot.selfdrive.selfdrived.button_state_tracker import ButtonStateTracker
 from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 
+ButtonEvent = car.CarState.ButtonEvent
+ButtonType = car.CarState.ButtonEvent.Type
+
 SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
 
 ALL_STATES = tuple(SpeedLimitAssistState.schema.enumerants.values())
@@ -276,3 +283,86 @@ class TestSpeedLimitAssist:
         assert self.sla.state in [SpeedLimitAssistState.preActive, SpeedLimitAssistState.active]
       elif initial_state in ACTIVE_STATES:
         assert self.sla.state in ACTIVE_STATES
+
+
+class TestButtonStateTrackerSLAIntegration:
+
+  def setup_method(self, method):
+    self.tracker = ButtonStateTracker()
+    self.params = Params()
+    self.params.put("IsReleaseSpBranch", True, block=True)
+    self.params.put("SpeedLimitMode", int(Mode.assist), block=True)
+    self.params.put_bool("IsMetric", False, block=True)
+    self.params.put("SpeedLimitOffsetType", 0, block=True)
+    self.params.put("SpeedLimitValueOffset", 0, block=True)
+
+    CarInterface = interfaces[DEFAULT_CAR]
+    CP = CarInterface.get_non_essential_params(DEFAULT_CAR)
+    CP.openpilotLongitudinalControl = True
+    CP_SP = CarInterface.get_non_essential_params_sp(CP, DEFAULT_CAR)
+    self.sla = SpeedLimitAssist(CP, CP_SP)
+
+  def _make_cs(self, events=None) -> car.CarState:
+    CS = car.CarState()
+    CS.buttonEvents = events or []
+    return CS
+
+  def _run_ctrl_frames(self, frames: list[car.CarState]) -> None:
+    for cs in frames:
+      self.tracker.update(cs)
+
+  def test_button_confirm_via_tracker(self) -> None:
+    self._run_ctrl_frames([
+      self._make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]),
+      self._make_cs(),
+      self._make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]),
+      self._make_cs(),
+      self._make_cs(),
+    ])
+    self.sla.update_buttons(self.tracker.release_toggle)
+    assert self.sla._get_button_release(req_plus=True, req_minus=False)
+
+  def test_rapid_press_release_between_polls(self) -> None:
+    self.sla.update_buttons(self.tracker.release_toggle)
+
+    self._run_ctrl_frames([
+      self._make_cs([ButtonEvent(type=ButtonType.decelCruise, pressed=True)]),
+      self._make_cs([ButtonEvent(type=ButtonType.decelCruise, pressed=False)]),
+      self._make_cs(),
+      self._make_cs(),
+      self._make_cs(),
+    ])
+    self.sla.update_buttons(self.tracker.release_toggle)
+    assert self.sla._get_button_release(req_plus=False, req_minus=True)
+
+  def test_multiple_releases_between_polls(self) -> None:
+    self.sla.update_buttons(self.tracker.release_toggle)
+
+    self._run_ctrl_frames([
+      self._make_cs([
+        ButtonEvent(type=ButtonType.accelCruise, pressed=True),
+        ButtonEvent(type=ButtonType.decelCruise, pressed=True),
+      ]),
+      self._make_cs([
+        ButtonEvent(type=ButtonType.accelCruise, pressed=False),
+        ButtonEvent(type=ButtonType.decelCruise, pressed=False),
+      ]),
+    ])
+    self.sla.update_buttons(self.tracker.release_toggle)
+    assert self.sla._get_button_release(req_plus=True, req_minus=False)
+    assert self.sla._get_button_release(req_plus=False, req_minus=True)
+
+  def test_no_false_positive_same_toggle(self) -> None:
+    self.sla.update_buttons(self.tracker.release_toggle)
+    self.sla.update_buttons(self.tracker.release_toggle)
+    assert not self.sla._get_button_release(req_plus=True, req_minus=False)
+    assert not self.sla._get_button_release(req_plus=False, req_minus=True)
+
+  def test_button_confirm_expires(self) -> None:
+    self._run_ctrl_frames([
+      self._make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]),
+      self._make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]),
+    ])
+    self.sla.update_buttons(self.tracker.release_toggle)
+    time.sleep(CRUISE_BUTTON_CONFIRM_HOLD + 0.1)
+    assert not self.sla._get_button_release(req_plus=True, req_minus=False)

openpilot/sunnypilot/selfdrive/selfdrived/button_state_tracker.py

@@ -0,0 +1,26 @@
+"""
+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.
+"""
+from opendbc.car import structs
+
+
+class ButtonStateTracker:
+  def __init__(self) -> None:
+    self.pressed: int = 0
+    self.release_toggle: int = 0
+
+  def update(self, CS: structs.CarState) -> None:
+    for b in CS.buttonEvents:
+      bit = 1 << b.type.raw
+      if b.pressed:
+        self.pressed |= bit
+      else:
+        self.pressed &= ~bit
+        self.release_toggle ^= bit
+
+  def publish(self, ss_sp) -> None:
+    ss_sp.buttonsPressed = self.pressed
+    ss_sp.buttonsReleaseToggle = self.release_toggle

openpilot/sunnypilot/selfdrive/selfdrived/tests/test_button_state_tracker.py

@@ -0,0 +1,67 @@
+"""
+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.
+"""
+from opendbc.car.structs import car
+from openpilot.sunnypilot.selfdrive.selfdrived.button_state_tracker import ButtonStateTracker
+
+ButtonEvent = car.CarState.ButtonEvent
+ButtonType = car.CarState.ButtonEvent.Type
+
+
+class TestButtonStateTracker:
+  def setup_method(self) -> None:
+    self.tracker = ButtonStateTracker()
+
+  def make_cs(self, events: list) -> car.CarState:
+    CS = car.CarState()
+    CS.buttonEvents = events
+    return CS
+
+  def test_initial_state(self) -> None:
+    assert self.tracker.pressed == 0
+    assert self.tracker.release_toggle == 0
+
+  def test_press_sets_bit(self) -> None:
+    self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]))
+    assert self.tracker.pressed == (1 << ButtonType.accelCruise)
+    assert self.tracker.release_toggle == 0
+
+  def test_release_clears_and_toggles(self) -> None:
+    self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]))
+    self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]))
+    assert self.tracker.pressed == 0
+    assert self.tracker.release_toggle == (1 << ButtonType.accelCruise)
+
+  def test_multiple_buttons(self) -> None:
+    self.tracker.update(self.make_cs([
+      ButtonEvent(type=ButtonType.accelCruise, pressed=True),
+      ButtonEvent(type=ButtonType.decelCruise, pressed=True),
+    ]))
+    assert self.tracker.pressed == (1 << ButtonType.accelCruise) | (1 << ButtonType.decelCruise)
+
+    self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]))
+    assert self.tracker.pressed == (1 << ButtonType.decelCruise)
+    assert self.tracker.release_toggle == (1 << ButtonType.accelCruise)
+
+  def test_release_toggle_flips(self) -> None:
+    for _ in range(2):
+      self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=True)]))
+      self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=False)]))
+    assert self.tracker.release_toggle == 0
+
+  def test_publish(self) -> None:
+    self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]))
+    self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.decelCruise, pressed=True)]))
+    self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]))
+
+    class MockSP:
+      buttonsPressed = 0
+      buttonsReleaseToggle = 0
+
+    sp = MockSP()
+    self.tracker.publish(sp)
+    assert sp.buttonsPressed == self.tracker.pressed
+    assert sp.buttonsReleaseToggle == self.tracker.release_toggle