diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 004a86f433..6e78191765 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -123,7 +123,8 @@ class Car: self.mads_enabled_toggle = True # TODO-SP: Apply with toggle data_services = list(self.sm.data.keys()) + ['selfdriveStateSP'] self.sm = messaging.SubMaster(data_services, poll='selfdriveStateSP') - self.CP.alternativeExperience |= ModifiedAssistDrivingSystem(self).set_alternative_experience() + if self.mads_enabled_toggle: + self.CP.alternativeExperience |= ModifiedAssistDrivingSystem().set_alternative_experience() openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") @@ -258,8 +259,6 @@ class Car: if self.mads_enabled_toggle: self.CI.mads_enabled = self.sm['selfdriveStateSP'].mads.enabled - else: - self.CI.mads_enabled = self.sm['carControl'].enabled if self.sm.all_alive(['carControl']): # send car controls over can diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index b6d5442b44..daa5008e2e 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -442,7 +442,7 @@ class SelfdriveD: def publish_selfdriveStateSP(self): # selfdriveStateSP ss_sp_msg = messaging.new_message('selfdriveStateSP') - ss_sp_msg.valid = self.sm.all_checks(['selfdriveState']) + ss_sp_msg.valid = True ss_sp = ss_sp_msg.selfdriveStateSP # mads diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index fdc6036d92..d5ab0c05d5 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -10,9 +10,7 @@ EventName = car.OnroadEvent.EventName class ModifiedAssistDrivingSystem: - def __init__(self, selfdrive): - self.selfdrive = selfdrive - + def __init__(self, selfdrive=None): self.current_state = State.disabled self.enabled = False self.active = False @@ -24,6 +22,9 @@ class ModifiedAssistDrivingSystem: State.overriding: Overriding(self), } + if selfdrive is not None: + self.selfdrive = selfdrive + self.enabled_toggle = True # TODO-SP: Apply with toggle self.main_enabled_toggle = True # TODO-SP: Apply with toggle self.disengage_lateral_on_brake_toggle = False # TODO-SP: Apply with toggle @@ -32,10 +33,9 @@ class ModifiedAssistDrivingSystem: self.mads_alt_button_enabled = False def set_alternative_experience(self, alt_experience: int = 0): - if self.enabled_toggle: - alt_experience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS - if not self.disengage_lateral_on_brake_toggle: - alt_experience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_LATERAL_ON_BRAKE + alt_experience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS + if not self.disengage_lateral_on_brake_toggle: + alt_experience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_LATERAL_ON_BRAKE return alt_experience diff --git a/sunnypilot/mads/state.py b/sunnypilot/mads/state.py index f18196da14..ae69a3aa9b 100644 --- a/sunnypilot/mads/state.py +++ b/sunnypilot/mads/state.py @@ -16,7 +16,7 @@ class StateMachineBase(ABC): def __init__(self, mads): self.mads = mads self.selfdrive = mads.selfdrive - self.ss_state_machine = mads.selfdrive.ss_state_machine + self.ss_state_machine = mads.selfdrive.state_machine self.state = State.disabled @@ -48,7 +48,7 @@ class StateMachineBase(ABC): def add_current_alert_types(self, alert_type): if not self.selfdrive.active: - self.add_current_alert_types(alert_type) + self.ss_state_machine.current_alert_types(alert_type) @abstractmethod def handle(self, events: Events):