From 52dc4141c5e5ec3b4809ad12bb6eb10731a6e5a5 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 17 May 2025 10:09:50 -0400 Subject: [PATCH 1/2] MADS: evaluate brakePressed and regenBraking for Pause Lateral mode (#926) * simplify * MADS: evaluate brakePressed and regenBraking for Pause Lateral mode * nah --- sunnypilot/mads/mads.py | 34 ++++++++++++++++++++++++++-------- 1 file changed, 26 insertions(+), 8 deletions(-) diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index 9aeb67972f..1132a0df2f 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -9,6 +9,7 @@ from cereal import log, custom from opendbc.car import structs from opendbc.car.hyundai.values import HyundaiFlags +from opendbc.safety import ALTERNATIVE_EXPERIENCE from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState @@ -24,6 +25,8 @@ IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) class ModularAssistiveDrivingSystem: def __init__(self, selfdrive): + self.CP = selfdrive.CP + self.CS_prev = selfdrive.CS_prev self.params = selfdrive.params self.enabled = False @@ -35,9 +38,10 @@ class ModularAssistiveDrivingSystem: self.state_machine = StateMachine(self) self.events = self.selfdrive.events self.events_sp = self.selfdrive.events_sp + self.disengage_on_accelerator = not self.CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS - if self.selfdrive.CP.brand == "hyundai": - if self.selfdrive.CP.flags & (HyundaiFlags.HAS_LDA_BUTTON | HyundaiFlags.CANFD): + if self.CP.brand == "hyundai": + if self.CP.flags & (HyundaiFlags.HAS_LDA_BUTTON | HyundaiFlags.CANFD): self.allow_always = True # read params on init @@ -50,6 +54,21 @@ class ModularAssistiveDrivingSystem: 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: + if self.events.has(EventName.pedalPressed) and not (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator): + return True + + return False + + def should_silent_lkas_enable(self, CS: structs.CarState) -> bool: + if self.pause_lateral_on_brake_toggle 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 update_events(self, CS: structs.CarState): def update_unified_engagement_mode(): uem_blocked = self.enabled or (self.selfdrive.enabled and self.selfdrive.enabled_prev) @@ -86,7 +105,7 @@ class ModularAssistiveDrivingSystem: transition_paused_state() if self.pause_lateral_on_brake_toggle: - if CS.brakePressed: + if self.pedal_pressed_non_gas_pressed(CS): transition_paused_state() self.events.remove(EventName.preEnableStandstill) @@ -99,7 +118,7 @@ class ModularAssistiveDrivingSystem: update_unified_engagement_mode() else: if self.main_enabled_toggle: - if CS.cruiseState.available and not self.selfdrive.CS_prev.cruiseState.available: + if CS.cruiseState.available and not self.CS_prev.cruiseState.available: self.events_sp.add(EventNameSP.lkasEnable) for be in CS.buttonEvents: @@ -117,11 +136,10 @@ class ModularAssistiveDrivingSystem: if not CS.cruiseState.available: self.events.remove(EventName.buttonEnable) - if self.selfdrive.CS_prev.cruiseState.available: + if self.CS_prev.cruiseState.available: self.events_sp.add(EventNameSP.lkasDisable) - if not (self.pause_lateral_on_brake_toggle and CS.brakePressed) and \ - not self.events_sp.contains_in_list(GEARS_ALLOW_PAUSED_SILENT): + if self.should_silent_lkas_enable(CS): if self.state_machine.state == State.paused: self.events_sp.add(EventNameSP.silentLkasEnable) @@ -141,7 +159,7 @@ class ModularAssistiveDrivingSystem: self.update_events(CS) - if not self.selfdrive.CP.passive and self.selfdrive.initialized: + 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 From 7565dd2545488f0b2100c03c753f562da158898a Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 17 May 2025 10:34:38 -0400 Subject: [PATCH 2/2] MADS: Refactor Unified Engagement Mode (#927) --- sunnypilot/mads/mads.py | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index 1132a0df2f..dbaec258ab 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -69,13 +69,20 @@ class ModularAssistiveDrivingSystem: return True - def update_events(self, CS: structs.CarState): - def update_unified_engagement_mode(): - uem_blocked = self.enabled or (self.selfdrive.enabled and self.selfdrive.enabled_prev) - if (self.unified_engagement_mode and uem_blocked) or not self.unified_engagement_mode: - self.events.remove(EventName.pcmEnable) - self.events.remove(EventName.buttonEnable) + 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 update_events(self, CS: structs.CarState): def transition_paused_state(): if self.state_machine.state != State.paused: self.events_sp.add(EventNameSP.silentLkasDisable) @@ -115,7 +122,9 @@ class ModularAssistiveDrivingSystem: self.events.remove(EventName.manualRestart) if self.events.has(EventName.pcmEnable) or self.events.has(EventName.buttonEnable): - update_unified_engagement_mode() + 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.CS_prev.cruiseState.available: