must check manual presses this way

This commit is contained in:
Jason Wen
2024-09-15 02:10:56 -04:00
parent f113940993
commit deafc23ea3
3 changed files with 23 additions and 4 deletions
@@ -7,8 +7,9 @@ from openpilot.selfdrive.controls.lib.sunnypilot.speed_limit_controller import A
from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.states import InactiveState, \
AcceleratingState, DeceleratingState, HoldingState, ResettingState, LoadingState
from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.helpers import get_set_point, \
speed_hysteresis
speed_hysteresis, update_manual_button_timers
ButtonType = car.CarState.ButtonEvent.Type
ButtonControlState = custom.CarControlSP.CustomStockLongitudinalControl.ButtonControlState
SpeedLimitControlState = custom.LongitudinalPlanSP.SpeedLimitControlState
TurnSpeedControlState = custom.LongitudinalPlanSP.SpeedLimitControlState
@@ -46,7 +47,7 @@ class CustomStockLongitudinalControllerBase(ABC):
self.accel_button = None
self.decel_button = None
self.set_speed_buttons = None
self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0}
self.button_states = {
ButtonControlState.inactive: InactiveState(self, car_state),
@@ -99,8 +100,11 @@ class CustomStockLongitudinalControllerBase(ABC):
self.v_cruise = round(CS.cruiseState.speed * (CV.MS_TO_MPH if not self.car_state.params_list.is_metric else CV.MS_TO_KPH))
def update_state(self, CS: car.CarState, CC: car.CarControl) -> None:
update_manual_button_timers(CS, self.cruise_buttons)
ready = CS.cruiseState.enabled and not CC.cruiseControl.cancel and not CC.cruiseControl.resume
button_pressed = any(be.type in self.set_speed_buttons for be in CS.buttonEvents)
button_pressed = any(self.cruise_buttons[k] > 0 for k in CS.buttonEvents)
self.is_ready = ready and not button_pressed
self.cruise_button = self.button_states[self.button_state]()
@@ -1,5 +1,8 @@
from cereal import car
from openpilot.common.conversions import Conversions as CV
ButtonType = car.CarState.ButtonEvent.Type
def get_set_point(is_metric: bool) -> float:
return 30 if is_metric else int(20 * CV.MPH_TO_KPH)
@@ -11,3 +14,15 @@ def speed_hysteresis(speed: float, speed_steady: float, hyst: float) -> float:
elif speed < speed_steady - hyst:
speed_steady = speed + hyst
return speed_steady
def update_manual_button_timers(CS: car.CarState, button_timers: dict[ButtonType, int]) -> None:
# increment timer for buttons still pressed
for k in button_timers:
if button_timers[k] > 0:
button_timers[k] += 1
for b in CS.buttonEvents:
if b.type.raw in button_timers:
# Start/end timer and store current state on change of button pressed
button_timers[b.type.raw] = 1 if b.pressed else 0
@@ -13,7 +13,7 @@ class CustomStockLongitudinalController(CustomStockLongitudinalControllerBase):
super().__init__(car, car_controller, car_state, CP)
self.accel_button = Buttons.RES_ACCEL
self.decel_button = Buttons.SET_DECEL
self.set_speed_buttons = (ButtonType.accelCruise, ButtonType.decelCruise, ButtonType.cancel)
self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, ButtonType.gapAdjustCruise: 0}
def create_can_mock_button_messages(self) -> list[SendCan]:
can_sends = []