github-actions[bot]
f9fcc7adab
date: 2026-06-28T09:48:35 master commit: da6313dbe95b3f24bb5d8018b0e5f950f5823ca7
223 lines
9.0 KiB
Python
223 lines
9.0 KiB
Python
"""
|
|
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 cereal import log, custom
|
|
|
|
from opendbc.car import structs
|
|
from opendbc.car.hyundai.values import HyundaiFlags
|
|
from openpilot.common.params import Params
|
|
from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake, read_steering_mode_param, MADS_NO_ACC_MAIN_BUTTON
|
|
from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT
|
|
|
|
State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
|
|
ButtonType = structs.CarState.ButtonEvent.Type
|
|
EventName = log.OnroadEvent.EventName
|
|
EventNameSP = custom.OnroadEventSP.EventName
|
|
GearShifter = structs.CarState.GearShifter
|
|
SafetyModel = structs.CarParams.SafetyModel
|
|
|
|
SET_SPEED_BUTTONS = (ButtonType.accelCruise, ButtonType.resumeCruise, ButtonType.decelCruise, ButtonType.setCruise)
|
|
IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
|
|
|
|
|
|
class ModularAssistiveDrivingSystem:
|
|
def __init__(self, selfdrive):
|
|
self.CP = selfdrive.CP
|
|
self.CP_SP = selfdrive.CP_SP
|
|
self.params = selfdrive.params
|
|
|
|
self.enabled = False
|
|
self.active = False
|
|
self.available = False
|
|
self.lateral_mismatch_counter = 0
|
|
self.allow_always = False
|
|
self.no_main_cruise = False
|
|
self.selfdrive = selfdrive
|
|
self.selfdrive.enabled_prev = False
|
|
self.state_machine = StateMachine(self)
|
|
self.events = self.selfdrive.events
|
|
self.events_sp = self.selfdrive.events_sp
|
|
self.disengage_on_accelerator = Params().get_bool("DisengageOnAccelerator")
|
|
if self.CP.brand == "hyundai":
|
|
if self.CP.flags & (HyundaiFlags.HAS_LDA_BUTTON | HyundaiFlags.CANFD):
|
|
self.allow_always = True
|
|
if self.CP.brand == "tesla":
|
|
self.allow_always = True
|
|
|
|
if self.CP.brand in MADS_NO_ACC_MAIN_BUTTON:
|
|
self.no_main_cruise = True
|
|
|
|
# read params on init
|
|
self.enabled_toggle = self.params.get_bool("Mads")
|
|
self.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed")
|
|
self.steering_mode_on_brake = read_steering_mode_param(self.CP, self.CP_SP, self.params)
|
|
self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode")
|
|
|
|
def read_params(self):
|
|
self.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed")
|
|
self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode")
|
|
|
|
def pedal_pressed_non_gas_pressed(self, CS: structs.CarState) -> bool:
|
|
# ignore `pedalPressed` events caused by gas presses
|
|
if self.events.has(EventName.pedalPressed) and not (CS.gasPressed and not self.selfdrive.CS_prev.gasPressed and self.disengage_on_accelerator):
|
|
return True
|
|
|
|
return False
|
|
|
|
def should_silent_lkas_enable(self, CS: structs.CarState) -> bool:
|
|
if self.steering_mode_on_brake == MadsSteeringModeOnBrake.PAUSE and self.pedal_pressed_non_gas_pressed(CS):
|
|
return False
|
|
|
|
if self.events_sp.contains_in_list(GEARS_ALLOW_PAUSED_SILENT):
|
|
return False
|
|
|
|
return True
|
|
|
|
def block_unified_engagement_mode(self) -> bool:
|
|
# UEM disabled
|
|
if not self.unified_engagement_mode:
|
|
return True
|
|
|
|
if self.enabled:
|
|
return True
|
|
|
|
if self.selfdrive.enabled and self.selfdrive.enabled_prev:
|
|
return True
|
|
|
|
return False
|
|
|
|
def get_wrong_car_mode(self, alert_only: bool) -> None:
|
|
if alert_only:
|
|
if self.events.has(EventName.wrongCarMode):
|
|
self.replace_event(EventName.wrongCarMode, EventNameSP.wrongCarModeAlertOnly)
|
|
else:
|
|
self.events.remove(EventName.wrongCarMode)
|
|
|
|
def transition_paused_state(self):
|
|
if self.state_machine.state != State.paused:
|
|
self.events_sp.add(EventNameSP.silentLkasDisable)
|
|
|
|
def replace_event(self, old_event: int, new_event: int):
|
|
self.events.remove(old_event)
|
|
self.events_sp.add(new_event)
|
|
|
|
def data_sample(self):
|
|
# When the safety and selfdrived do not agree on controls_allowed_lateral
|
|
# we want to disengage sunnypilot. However the status from the panda goes through
|
|
# another socket other than the CAN messages and one can arrive earlier than the other.
|
|
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
|
|
if not self.active or self.selfdrive.enabled:
|
|
self.lateral_mismatch_counter = 0
|
|
elif any(not ps.controlsAllowedLateral for ps in self.selfdrive.sm['pandaStates']
|
|
if ps.safetyModel not in IGNORED_SAFETY_MODES):
|
|
self.lateral_mismatch_counter += 1
|
|
|
|
def update_events(self, CS: structs.CarState):
|
|
if not self.selfdrive.enabled and self.enabled:
|
|
if CS.standstill:
|
|
if self.events.has(EventName.doorOpen):
|
|
self.replace_event(EventName.doorOpen, EventNameSP.silentDoorOpen)
|
|
self.transition_paused_state()
|
|
if self.events.has(EventName.seatbeltNotLatched):
|
|
self.replace_event(EventName.seatbeltNotLatched, EventNameSP.silentSeatbeltNotLatched)
|
|
self.transition_paused_state()
|
|
if self.events.has(EventName.wrongGear) and (CS.vEgo < 2.5 or CS.gearShifter == GearShifter.reverse):
|
|
self.replace_event(EventName.wrongGear, EventNameSP.silentWrongGear)
|
|
self.transition_paused_state()
|
|
if self.events.has(EventName.reverseGear):
|
|
self.replace_event(EventName.reverseGear, EventNameSP.silentReverseGear)
|
|
self.transition_paused_state()
|
|
if self.events.has(EventName.brakeHold):
|
|
self.replace_event(EventName.brakeHold, EventNameSP.silentBrakeHold)
|
|
self.transition_paused_state()
|
|
if self.events.has(EventName.parkBrake):
|
|
self.replace_event(EventName.parkBrake, EventNameSP.silentParkBrake)
|
|
self.transition_paused_state()
|
|
|
|
if self.steering_mode_on_brake == MadsSteeringModeOnBrake.PAUSE:
|
|
if self.pedal_pressed_non_gas_pressed(CS):
|
|
self.transition_paused_state()
|
|
|
|
self.events.remove(EventName.preEnableStandstill)
|
|
self.events.remove(EventName.belowEngageSpeed)
|
|
self.events.remove(EventName.speedTooLow)
|
|
self.events.remove(EventName.cruiseDisabled)
|
|
self.events.remove(EventName.manualRestart)
|
|
self.events.remove(EventName.espActive)
|
|
|
|
selfdrive_enable_events = self.events.has(EventName.pcmEnable) or self.events.has(EventName.buttonEnable)
|
|
set_speed_btns_enable = any(be.type in SET_SPEED_BUTTONS for be in CS.buttonEvents)
|
|
|
|
# wrongCarMode alert only or actively block control
|
|
self.get_wrong_car_mode(selfdrive_enable_events or set_speed_btns_enable)
|
|
|
|
if selfdrive_enable_events:
|
|
if self.pedal_pressed_non_gas_pressed(CS):
|
|
self.events_sp.add(EventNameSP.pedalPressedAlertOnly)
|
|
|
|
if self.block_unified_engagement_mode():
|
|
self.events.remove(EventName.pcmEnable)
|
|
self.events.remove(EventName.buttonEnable)
|
|
else:
|
|
if self.main_enabled_toggle:
|
|
if CS.cruiseState.available and not self.selfdrive.CS_prev.cruiseState.available:
|
|
self.events_sp.add(EventNameSP.lkasEnable)
|
|
|
|
for be in CS.buttonEvents:
|
|
if be.type == ButtonType.cancel:
|
|
if not self.selfdrive.enabled and self.selfdrive.enabled_prev:
|
|
self.events_sp.add(EventNameSP.manualLongitudinalRequired)
|
|
if be.type == ButtonType.lkas and be.pressed and (CS.cruiseState.available or self.allow_always):
|
|
if self.enabled:
|
|
if self.selfdrive.enabled:
|
|
self.events_sp.add(EventNameSP.manualSteeringRequired)
|
|
else:
|
|
self.events_sp.add(EventNameSP.lkasDisable)
|
|
else:
|
|
self.events_sp.add(EventNameSP.lkasEnable)
|
|
|
|
if not CS.cruiseState.available and not self.no_main_cruise:
|
|
self.events.remove(EventName.buttonEnable)
|
|
if self.selfdrive.CS_prev.cruiseState.available:
|
|
self.events_sp.add(EventNameSP.lkasDisable)
|
|
|
|
if self.steering_mode_on_brake == MadsSteeringModeOnBrake.DISENGAGE:
|
|
if self.pedal_pressed_non_gas_pressed(CS):
|
|
if self.enabled:
|
|
self.events_sp.add(EventNameSP.lkasDisable)
|
|
else:
|
|
# block lkasEnable if being sent, then send pedalPressedAlertOnly event
|
|
if self.events_sp.contains(EventNameSP.lkasEnable):
|
|
self.events_sp.remove(EventNameSP.lkasEnable)
|
|
self.events_sp.add(EventNameSP.pedalPressedAlertOnly)
|
|
|
|
if self.should_silent_lkas_enable(CS):
|
|
if self.state_machine.state == State.paused:
|
|
self.events_sp.add(EventNameSP.silentLkasEnable)
|
|
|
|
if self.lateral_mismatch_counter >= 200:
|
|
self.events_sp.add(EventNameSP.controlsMismatchLateral)
|
|
|
|
self.events.remove(EventName.pcmDisable)
|
|
self.events.remove(EventName.buttonCancel)
|
|
self.events.remove(EventName.pedalPressed)
|
|
self.events.remove(EventName.wrongCruiseMode)
|
|
|
|
def update(self, CS: structs.CarState):
|
|
if not self.enabled_toggle:
|
|
return
|
|
|
|
self.data_sample()
|
|
|
|
self.update_events(CS)
|
|
|
|
if not self.CP.passive and self.selfdrive.initialized:
|
|
self.enabled, self.active = self.state_machine.update()
|
|
|
|
# Copy of previous SelfdriveD states for MADS events handling
|
|
self.selfdrive.enabled_prev = self.selfdrive.enabled
|