diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 76c8bade68..ddb09e34ff 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -15,6 +15,7 @@ struct SelfdriveStateSP @0x81c2f05a394cf4af { state @0 :ModifiedAssistDrivingSystemState; enabled @1 :Bool; active @2 :Bool; + available @3 :Bool; enum ModifiedAssistDrivingSystemState { disabled @0; diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 44a5f0ee8c..789fe0d2d9 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -92,7 +92,7 @@ class Controls: # Check which actuators can be enabled standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill CC.latActive = self.sm['selfdriveState'].active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and not standstill - CC.latActive = CC.latActive or self.sm['selfdriveStateSP'].mads.enabled + CC.latActive = CC.latActive or (self.sm['selfdriveStateSP'].mads.enabled and self.sm['selfdriveStateSP'].mads.available) CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and self.CP.openpilotLongitudinalControl actuators = CC.actuators diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index da1703f579..fa9a47f22f 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -450,6 +450,7 @@ class SelfdriveD: mads.state = self.mads.state_machine.state mads.enabled = self.mads.enabled mads.active = self.mads.active + mads.available = self.mads.available self.pm.send('selfdriveStateSP', ss_sp_msg) diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index ffae0ff1a1..07b9eb3857 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -13,6 +13,7 @@ class ModifiedAssistDrivingSystem: def __init__(self, selfdrive=None): self.enabled = False self.active = False + self.available = False self.state_machine = StateMachine(self) if selfdrive is not None: @@ -22,9 +23,6 @@ class ModifiedAssistDrivingSystem: self.main_enabled_toggle = True # TODO-SP: Apply with toggle self.disengage_lateral_on_brake_toggle = False # TODO-SP: Apply with toggle - self.available = False - self.mads_alt_button_enabled = False - def set_alternative_experience(self, alt_experience: int = 0): alt_experience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS if not self.disengage_lateral_on_brake_toggle: @@ -36,8 +34,6 @@ class ModifiedAssistDrivingSystem: if self.main_enabled_toggle: available |= CS.cruiseState.available - available |= self.mads_alt_button_enabled - return available def update_events(self, CS: car.CarState): @@ -49,7 +45,7 @@ class ModifiedAssistDrivingSystem: self.selfdrive.events.add(EventName.silentWrongGear) self.selfdrive.events.remove(EventName.wrongGear) - if not self.selfdrive.enabled: + else: self.selfdrive.events.remove(EventName.buttonEnable) self.selfdrive.events.remove(EventName.buttonCancel) self.selfdrive.events.remove(EventName.wrongCruiseMode) @@ -65,7 +61,7 @@ class ModifiedAssistDrivingSystem: self.selfdrive.events.add(EventName.silentPedalPressed) if not CS.brakePressed and not CS.brakeHoldActive and not CS.parkingBrake and not CS.regenBraking: - if self.current_state == State.paused and self.available: + if self.state_machine.state == State.paused and self.available: self.selfdrive.events.add(EventName.silentButtonEnable) for be in CS.buttonEvents: @@ -86,9 +82,9 @@ class ModifiedAssistDrivingSystem: if not self.enabled_toggle: return - self.available = self.update_availability(CS) - self.update_events(CS) + self.available = self.update_availability(CS) + if not self.selfdrive.CP.passive and self.selfdrive.initialized: self.enabled, self.active = self.state_machine.update(self.selfdrive.events)