James
52 lines
2.7 KiB
Python
52 lines
2.7 KiB
Python
#!/usr/bin/env python3
|
|
from opendbc.safety import ALTERNATIVE_EXPERIENCE
|
|
from openpilot.common.params import Params
|
|
from openpilot.selfdrive.car.cruise import ButtonType
|
|
from openpilot.selfdrive.selfdrived.events import ET
|
|
|
|
from openpilot.frogpilot.common.frogpilot_utilities import is_FrogsGoMoo
|
|
from openpilot.frogpilot.common.frogpilot_variables import NON_DRIVING_GEARS
|
|
|
|
class FrogPilotCard:
|
|
def __init__(self, CP, FPCP):
|
|
self.CP = CP
|
|
|
|
self.params = Params(return_defaults=True)
|
|
self.params_memory = Params(memory=True)
|
|
|
|
self.accel_pressed = False
|
|
self.always_on_lateral_allowed = False
|
|
self.decel_pressed = False
|
|
|
|
self.always_on_lateral_set = bool(FPCP.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL)
|
|
self.frogs_go_moo = is_FrogsGoMoo()
|
|
|
|
def update(self, carState, frogpilotCarState, sm, frogpilot_toggles):
|
|
if self.CP.brand == "hyundai":
|
|
for be in carState.buttonEvents:
|
|
if be.type == ButtonType.lkas and be.pressed and frogpilot_toggles.always_on_lateral_lkas:
|
|
self.always_on_lateral_allowed = not self.always_on_lateral_allowed
|
|
elif be.type == ButtonType.mainCruise and be.pressed and frogpilot_toggles.always_on_lateral_main:
|
|
self.always_on_lateral_allowed = not self.always_on_lateral_allowed
|
|
elif frogpilot_toggles.always_on_lateral_main:
|
|
self.always_on_lateral_allowed = carState.cruiseState.available
|
|
|
|
self.always_on_lateral_enabled = self.always_on_lateral_allowed and self.always_on_lateral_set
|
|
self.always_on_lateral_enabled &= carState.gearShifter not in NON_DRIVING_GEARS
|
|
self.always_on_lateral_enabled &= sm["frogpilotPlan"].lateralCheck
|
|
self.always_on_lateral_enabled &= sm["liveCalibration"].calPerc >= 1
|
|
self.always_on_lateral_enabled &= (ET.IMMEDIATE_DISABLE not in sm["selfdriveState"].alertType + sm["frogpilotSelfdriveState"].alertType) or self.frogs_go_moo
|
|
self.always_on_lateral_enabled &= not (carState.brakePressed and carState.vEgo < frogpilot_toggles.always_on_lateral_pause_speed) or carState.standstill
|
|
|
|
if sm.updated["frogpilotPlan"] or any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in carState.buttonEvents):
|
|
self.accel_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in carState.buttonEvents)
|
|
|
|
if sm.updated["frogpilotPlan"] or any(be.type == ButtonType.decelCruise for be in carState.buttonEvents):
|
|
self.decel_pressed = any(be.type == ButtonType.decelCruise for be in carState.buttonEvents)
|
|
|
|
frogpilotCarState.accelPressed = self.accel_pressed
|
|
frogpilotCarState.alwaysOnLateralEnabled = self.always_on_lateral_enabled
|
|
frogpilotCarState.decelPressed = self.decel_pressed
|
|
|
|
return frogpilotCarState
|