diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f6c59559ad..b715d1d4f7 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -38,9 +38,7 @@ NOSENSOR = "NOSENSOR" in os.environ IGNORE_PROCESSES = {"rtshield", "uploader", "deleter", "loggerd", "logmessaged", "tombstoned", "logcatd", "proclogd", "clocksd", "updated", "timezoned", "manage_athenad", "statsd", "shutdownd"} | \ - {k for k, v in managed_processes.items() if not v.enabled} - -ACTUATOR_FIELDS = set(car.CarControl.Actuators.schema.fields.keys()) + {k for k, v in managed_processes.items() if not v.enabled} ThermalStatus = log.DeviceState.ThermalStatus State = log.ControlsState.OpenpilotState @@ -52,11 +50,15 @@ EventName = car.CarEvent.EventName ButtonEvent = car.CarState.ButtonEvent SafetyModel = car.CarParams.SafetyModel -IGNORED_SAFETY_MODES = [SafetyModel.silent, SafetyModel.noOutput] +IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) CSID_MAP = {"0": EventName.roadCameraError, "1": EventName.wideRoadCameraError, "2": EventName.driverCameraError} +ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys()) +ACTIVE_STATES = (State.enabled, State.softDisabling) +ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES) + class Controls: - def __init__(self, sm=None, pm=None, can_sock=None): + def __init__(self, sm=None, pm=None, can_sock=None, CI=None): config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH) # Setup sockets @@ -89,11 +91,15 @@ class Controls: if TICI: self.log_sock = messaging.sub_sock('androidLog') - # wait for one pandaState and one CAN packet - print("Waiting for CAN messages...") - get_one_can(self.can_sock) + if CI is None: + # wait for one pandaState and one CAN packet + print("Waiting for CAN messages...") + get_one_can(self.can_sock) + + self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan']) + else: + self.CI, self.CP = CI, CI.CP - self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan']) self.CP.alternativeExperience = 0 # see panda/board/safety_declarations.h for allowed values # read params @@ -415,7 +421,7 @@ class Controls: self.current_alert_types = [ET.PERMANENT] - # ENABLED, PRE ENABLING, SOFT DISABLING + # ENABLED, SOFT DISABLING, PRE ENABLING if self.state != State.disabled: # user and immediate disable always have priority in a non-disabled state if self.events.any(ET.USER_DISABLE): @@ -448,7 +454,10 @@ class Controls: # PRE ENABLING elif self.state == State.preEnabled: - if not self.events.any(ET.PRE_ENABLE): + if self.events.any(ET.NO_ENTRY): + self.state = State.disabled + self.current_alert_types.append(ET.NO_ENTRY) + elif not self.events.any(ET.PRE_ENABLE): self.state = State.enabled else: self.current_alert_types.append(ET.PRE_ENABLE) @@ -468,14 +477,12 @@ class Controls: if not self.CP.pcmCruise: self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last) - # Check if actuators are enabled - self.active = self.state in (State.enabled, State.softDisabling) + # Check if openpilot is engaged and actuators are enabled + self.enabled = self.state in ENABLED_STATES + self.active = self.state in ACTIVE_STATES if self.active: self.current_alert_types.append(ET.WARNING) - # Check if openpilot is engaged - self.enabled = self.active or self.state == State.preEnabled - def state_control(self, CS): """Given the state, this function returns a CarControl packet""" diff --git a/selfdrive/controls/tests/test_state_machine.py b/selfdrive/controls/tests/test_state_machine.py new file mode 100755 index 0000000000..99a111e612 --- /dev/null +++ b/selfdrive/controls/tests/test_state_machine.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 +import unittest + +from cereal import car, log +from common.realtime import DT_CTRL +from selfdrive.car.car_helpers import interfaces +from selfdrive.controls.controlsd import Controls, SOFT_DISABLE_TIME +from selfdrive.controls.lib.events import Events, ET, Alert, Priority, AlertSize, AlertStatus, VisualAlert, \ + AudibleAlert, EVENTS + +State = log.ControlsState.OpenpilotState + +# The event types that maintain the current state +MAINTAIN_STATES = {State.enabled: None, State.disabled: None, State.softDisabling: ET.SOFT_DISABLE, + State.preEnabled: ET.PRE_ENABLE} +ALL_STATES = tuple((state for state in State.schema.enumerants.values() if + state != State.overriding)) # TODO: remove overriding exception +# The event types checked in DISABLED section of state machine +ENABLE_EVENT_TYPES = (ET.ENABLE, ET.PRE_ENABLE) + + +def make_event(event_types): + event = {} + for ev in event_types: + event[ev] = Alert("", "", AlertStatus.normal, AlertSize.small, Priority.LOW, + VisualAlert.none, AudibleAlert.none, 1.) + EVENTS[0] = event + return 0 + + +class TestStateMachine(unittest.TestCase): + + def setUp(self): + CarInterface, CarController, CarState = interfaces["mock"] + CP = CarInterface.get_params("mock") + CI = CarInterface(CP, CarController, CarState) + + self.controlsd = Controls(CI=CI) + self.controlsd.events = Events() + self.controlsd.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) + self.CS = car.CarState() + + def test_immediate_disable(self): + for state in ALL_STATES: + self.controlsd.events.add(make_event([MAINTAIN_STATES[state], ET.IMMEDIATE_DISABLE])) + self.controlsd.state = state + self.controlsd.state_transition(self.CS) + self.assertEqual(State.disabled, self.controlsd.state) + self.controlsd.events.clear() + + def test_user_disable(self): + for state in ALL_STATES: + self.controlsd.events.add(make_event([MAINTAIN_STATES[state], ET.USER_DISABLE])) + self.controlsd.state = state + self.controlsd.state_transition(self.CS) + self.assertEqual(State.disabled, self.controlsd.state) + self.controlsd.events.clear() + + def test_soft_disable(self): + for state in ALL_STATES: + if state == State.preEnabled: # preEnabled considers NO_ENTRY instead + continue + self.controlsd.events.add(make_event([MAINTAIN_STATES[state], ET.SOFT_DISABLE])) + self.controlsd.state = state + self.controlsd.state_transition(self.CS) + self.assertEqual(self.controlsd.state, State.disabled if state == State.disabled else State.softDisabling) + self.controlsd.events.clear() + + def test_soft_disable_timer(self): + self.controlsd.state = State.enabled + self.controlsd.events.add(make_event([ET.SOFT_DISABLE])) + self.controlsd.state_transition(self.CS) + for _ in range(int(SOFT_DISABLE_TIME / DT_CTRL)): + self.assertEqual(self.controlsd.state, State.softDisabling) + self.controlsd.state_transition(self.CS) + + self.assertEqual(self.controlsd.state, State.disabled) + + def test_no_entry(self): + # disabled with enable events + for et in ENABLE_EVENT_TYPES: + self.controlsd.events.add(make_event([ET.NO_ENTRY, et])) + self.controlsd.state_transition(self.CS) + self.assertEqual(self.controlsd.state, State.disabled) + self.controlsd.events.clear() + + def test_no_entry_pre_enable(self): + # preEnabled with preEnabled event + self.controlsd.state = State.preEnabled + self.controlsd.events.add(make_event([ET.NO_ENTRY, ET.PRE_ENABLE])) + self.controlsd.state_transition(self.CS) + self.assertEqual(self.controlsd.state, State.disabled) + + def test_maintain_states(self): + # Given current state's event type, we should maintain state + for state in ALL_STATES: + self.controlsd.state = state + self.controlsd.events.add(make_event([MAINTAIN_STATES[state]])) + self.controlsd.state_transition(self.CS) + self.assertEqual(self.controlsd.state, state) + self.controlsd.events.clear() + + +if __name__ == "__main__": + unittest.main()