diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index e78f7a75c..c81aa104f 100644 --- a/openpilot/cereal/custom.capnp +++ b/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; diff --git a/openpilot/selfdrive/controls/plannerd.py b/openpilot/selfdrive/controls/plannerd.py index 3a05ea174..24b07af7f 100755 --- a/openpilot/selfdrive/controls/plannerd.py +++ b/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) diff --git a/openpilot/selfdrive/selfdrived/selfdrived.py b/openpilot/selfdrive/selfdrived/selfdrived.py index 2edc0bce1..cfad1433d 100755 --- a/openpilot/selfdrive/selfdrived/selfdrived.py +++ b/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 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py b/openpilot/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py index 1ff541167..40c8c6fdf 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py +++ b/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() diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py b/openpilot/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py index 7cd6fef52..1f23fb229 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py +++ b/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) diff --git a/openpilot/sunnypilot/selfdrive/selfdrived/button_state_tracker.py b/openpilot/sunnypilot/selfdrive/selfdrived/button_state_tracker.py new file mode 100644 index 000000000..ffbff7b44 --- /dev/null +++ b/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 diff --git a/openpilot/sunnypilot/selfdrive/selfdrived/tests/__init__.py b/openpilot/sunnypilot/selfdrive/selfdrived/tests/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/openpilot/sunnypilot/selfdrive/selfdrived/tests/test_button_state_tracker.py b/openpilot/sunnypilot/selfdrive/selfdrived/tests/test_button_state_tracker.py new file mode 100644 index 000000000..798fab650 --- /dev/null +++ b/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