mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-11-03 03:03:43 +08:00
must check manual presses this way
This commit is contained in:
@@ -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 = []
|
||||
|
||||
Reference in New Issue
Block a user