Controlsd refactor (#1487)

* it's a class

* more refactor

* remove that

* car interface should create that

* that too

* not a dict

* don't create permanent events every iteration

* break up long lines

* fix honda

* small optimization

* less long lines

* dict is faster

* latcontrol less args

* longcontrol less args

* update profiling script

* few optimizations

* create events together

* clean up

* more clean up

* remove comment

* clean up

* simplify state transition

* more clean up

* update comments
old-commit-hash: 08832ff29d

selfdrive/car/interfaces.py

@@ -4,6 +4,7 @@ from cereal import car
 from common.kalman.simple_kalman import KF1D
 from common.realtime import DT_CTRL
 from selfdrive.car import gen_empty_fingerprint
+from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
 from selfdrive.controls.lib.vehicle_model import VehicleModel
 
@@ -96,6 +97,10 @@ class CarInterfaceBase():
       events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if cs_out.gasPressed:
       events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
+    if cs_out.stockAeb:
+      events.append(create_event('stockAeb', []))
+    if cs_out.vEgo > 92 * CV.MPH_TO_MS:
+      events.append(create_event('speedTooHigh', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
 
     if cs_out.steerError:
       events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))

selfdrive/controls/controlsd.py

@@ -26,6 +26,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel
 from selfdrive.controls.lib.planner import LON_MPC_STEP
 from selfdrive.locationd.calibration_helpers import Calibration, Filter
 
+LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
 LANE_DEPARTURE_THRESHOLD = 0.1
 STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL
 STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
@@ -33,30 +34,12 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
 ThermalStatus = log.ThermalData.ThermalStatus
 State = log.ControlsState.OpenpilotState
 HwType = log.HealthData.HwType
-
+LongitudinalPlanSource = log.Plan.LongitudinalPlanSource
+Desire = log.PathPlan.Desire
 LaneChangeState = log.PathPlan.LaneChangeState
 LaneChangeDirection = log.PathPlan.LaneChangeDirection
 
 
-def add_lane_change_event(events, path_plan):
-  if path_plan.laneChangeState == LaneChangeState.preLaneChange:
-    if path_plan.laneChangeDirection == LaneChangeDirection.left:
-      events.append(create_event('preLaneChangeLeft', [ET.WARNING]))
-    else:
-      events.append(create_event('preLaneChangeRight', [ET.WARNING]))
-  elif path_plan.laneChangeState in [LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing]:
-      events.append(create_event('laneChange', [ET.WARNING]))
-
-
-def isActive(state):
-  """Check if the actuators are enabled"""
-  return state in [State.enabled, State.softDisabling]
-
-
-def isEnabled(state):
-  """Check if openpilot is engaged"""
-  return (isActive(state) or state == State.preEnabled)
-
 def events_to_bytes(events):
   # optimization when comparing capnp structs: str() or tree traverse are much slower
   ret = []
@@ -69,540 +52,550 @@ def events_to_bytes(events):
   return ret
 
 
-def data_sample(CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter, params):
-  """Receive data from sockets and create events for battery, temperature and disk space"""
+class Controls:
+  def __init__(self, sm=None, pm=None, can_sock=None):
+    gc.disable()
+    set_realtime_priority(3)
+
+    # Setup sockets
+    self.pm = pm
+    if self.pm is None:
+      self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', \
+                                     'carControl', 'carEvents', 'carParams'])
 
-  # Update carstate from CAN and create events
-  can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True)
-  CS = CI.update(CC, can_strs)
+    self.sm = sm
+    if self.sm is None:
+      self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', \
+                                     'dMonitoringState', 'plan', 'pathPlan'])
 
-  sm.update(0)
+    self.can_sock = can_sock
+    if can_sock is None:
+      can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
+      self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
 
-  events = list(CS.events)
-  events += list(sm['dMonitoringState'].events)
-  add_lane_change_event(events, sm['pathPlan'])
-  enabled = isEnabled(state)
+    # wait for one health and one CAN packet
+    hw_type = messaging.recv_one(self.sm.sock['health']).health.hwType
+    has_relay = hw_type in [HwType.blackPanda, HwType.uno]
+    print("Waiting for CAN messages...")
+    messaging.get_one_can(self.can_sock)
 
-  # Check for CAN timeout
-  if not can_strs:
-    can_error_counter += 1
-    events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+    self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], has_relay)
 
-  overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red
-  free_space = sm['thermal'].freeSpace < 0.07  # under 7% of space free no enable allowed
-  low_battery = sm['thermal'].batteryPercent < 1 and sm['thermal'].chargingError  # at zero percent battery, while discharging, OP should not allowed
-  mem_low = sm['thermal'].memUsedPercent > 90
+    # read params
+    params = Params()
+    self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
+    self.is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
+    internet_needed = params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None
+    community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1"
+    openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1"
+    passive = params.get("Passive", encoding='utf8') == "1" or \
+              internet_needed or not openpilot_enabled_toggle
+
+    car_recognized = self.CP.carName != 'mock'
+    # If stock camera is disconnected, we loaded car controls and it's not dashcam mode
+    controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive
+    community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
+    self.read_only = not car_recognized or not controller_available or \
+                       self.CP.dashcamOnly or community_feature_disallowed
+    if self.read_only:
+      self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
 
-  # Create events for battery, temperature and disk space
-  if low_battery:
+    # Write CarParams for radard and boardd safety mode
+    cp_bytes = self.CP.to_bytes()
+    params.put("CarParams", cp_bytes)
+    put_nonblocking("CarParamsCache", cp_bytes)
+    put_nonblocking("LongitudinalControl", "1" if self.CP.openpilotLongitudinalControl else "0")
+
+    self.CC = car.CarControl.new_message()
+    self.AM = AlertManager()
+
+    self.LoC = LongControl(self.CP, self.CI.compute_gb)
+    self.VM = VehicleModel(self.CP)
+
+    if self.CP.lateralTuning.which() == 'pid':
+      self.LaC = LatControlPID(self.CP)
+    elif self.CP.lateralTuning.which() == 'indi':
+      self.LaC = LatControlINDI(self.CP)
+    elif self.CP.lateralTuning.which() == 'lqr':
+      self.LaC = LatControlLQR(self.CP)
+
+    self.state = State.disabled
+    self.enabled = False
+    self.active = False
+    self.can_rcv_error = False
+    self.soft_disable_timer = 0
+    self.v_cruise_kph = 255
+    self.v_cruise_kph_last = 0
+    self.mismatch_counter = 0
+    self.can_error_counter = 0
+    self.last_blinker_frame = 0
+    self.saturated_count = 0
+    self.events_prev = ""
+
+    self.sm['liveCalibration'].calStatus = Calibration.INVALID
+    self.sm['pathPlan'].sensorValid = True
+    self.sm['pathPlan'].posenetValid = True
+    self.sm['thermal'].freeSpace = 1.
+    self.sm['dMonitoringState'].events = []
+    self.sm['dMonitoringState'].awarenessStatus = 1.
+    self.sm['dMonitoringState'].faceDetected = False
+
+    startup_alert = get_startup_alert(car_recognized, controller_available)
+    self.AM.add(self.sm.frame, startup_alert, False)
+
+    # controlsd is driven by can recv, expected at 100Hz
+    self.rk = Ratekeeper(100, print_delay_threshold=None)
+
+    self.prof = Profiler(False)  # off by default
+
+    # detect sound card presence and ensure successful init
+    sounds_available = not os.path.isfile('/EON') or (os.path.isdir('/proc/asound/card0') \
+                        and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
+
+    self.static_events = []
+    if not sounds_available:
+      self.static_events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
+    if internet_needed:
+      self.static_events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT]))
+    if community_feature_disallowed:
+      self.static_events.append(create_event('communityFeatureDisallowed', [ET.PERMANENT]))
+    if self.read_only and not passive:
+      self.static_events.append(create_event('carUnrecognized', [ET.PERMANENT]))
+
+
+  def create_events(self, CS):
+    """Compute carEvents from carState"""
+
+    events = self.static_events.copy()
+    events.extend(CS.events)
+    events.extend(self.sm['dMonitoringState'].events)
+
+    # Create events for battery, temperature, disk space, and memory
+    if self.sm['thermal'].batteryPercent < 1 and self.sm['thermal'].chargingError:
+      # at zero percent battery, while discharging, OP should not allowed
       events.append(create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
-  if overtemp:
+    if self.sm['thermal'].thermalStatus >= ThermalStatus.red:
       events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
-  if free_space:
+    if self.sm['thermal'].freeSpace < 0.07:
+      # under 7% of space free no enable allowed
       events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
-  if mem_low:
+    if self.sm['thermal'].memUsedPercent > 90:
       events.append(create_event('lowMemory', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
 
-  if CS.stockAeb:
-    events.append(create_event('stockAeb', []))
-
-  # Handle calibration
-  cal_status = sm['liveCalibration'].calStatus
-  cal_perc = sm['liveCalibration'].calPerc
-
+    # Handle calibration status
+    cal_status = self.sm['liveCalibration'].calStatus
     if cal_status != Calibration.CALIBRATED:
       if cal_status == Calibration.UNCALIBRATED:
         events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
       else:
         events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
 
-  if CS.vEgo > 92 * CV.MPH_TO_MS:
-    events.append(create_event('speedTooHigh', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+    # Handle lane change
+    if self.sm['pathPlan'].laneChangeState == LaneChangeState.preLaneChange:
+      if self.sm['path_plan'].laneChangeDirection == LaneChangeDirection.left:
+        events.append(create_event('preLaneChangeLeft', [ET.WARNING]))
+      else:
+        events.append(create_event('preLaneChangeRight', [ET.WARNING]))
+    elif self.sm['pathPlan'].laneChangeState in [LaneChangeState.laneChangeStarting, \
+                                        LaneChangeState.laneChangeFinishing]:
+      events.append(create_event('laneChange', [ET.WARNING]))
+
+    if self.can_rcv_error:
+      events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+    if self.mismatch_counter >= 200:
+      events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
+    if not self.sm.alive['plan'] and self.sm.alive['pathPlan']:
+      # only plan not being received: radar not communicating
+      events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+    elif not self.sm.all_alive_and_valid():
+      events.append(create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+    if not self.sm['pathPlan'].mpcSolutionValid:
+      events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+    if not self.sm['pathPlan'].sensorValid and os.getenv("NOSENSOR") is None:
+      events.append(create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
+    if not self.sm['pathPlan'].paramsValid:
+      events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
+    if not self.sm['pathPlan'].posenetValid:
+      events.append(create_event('posenetInvalid', [ET.NO_ENTRY, ET.WARNING]))
+    if not self.sm['plan'].radarValid:
+      events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+    if self.sm['plan'].radarCanError:
+      events.append(create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+    if not CS.canValid:
+      events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+    if log.HealthData.FaultType.relayMalfunction in self.sm['health'].faults:
+      events.append(create_event('relayMalfunction', [ET.NO_ENTRY, ET.PERMANENT, ET.IMMEDIATE_DISABLE]))
+
+    # Only allow engagement with brake pressed when stopped behind another stopped car
+    if CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \
+        and not self.CP.radarOffCan and CS.vEgo < 0.3:
+      events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+
+
+    # TODO: clean up this alert creation in alerts refactor
+
+    if self.active:
+      for e in get_events(events, [ET.WARNING]):
+        # TODO: handle non static text in a cleaner way, like a callback
+        extra_text = ""
+        if e == "belowSteerSpeed":
+          if self.is_metric:
+            extra_text = str(int(round(self.CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph"
+          else:
+            extra_text = str(int(round(self.CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph"
+        self.AM.add(self.sm.frame, e, self.enabled, extra_text_2=extra_text)
+
+    for e in get_events(events, [ET.PERMANENT]):
+      # TODO: handle non static text in a cleaner way, like a callback
+      extra_text_1, extra_text_2 = "", ""
+      if e == "calibrationIncomplete":
+        extra_text_1 = str(self.sm['liveCalibration'].calPerc) + "%"
+        if self.is_metric:
+          extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph"
+        else:
+          extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph"
+      self.AM.add(self.sm.frame, str(e) + "Permanent", self.enabled, \
+                    extra_text_1=extra_text_1, extra_text_2=extra_text_2)
+
+    return events
+
+
+  def data_sample(self):
+    """Receive data from sockets and update carState"""
+
+    # Update carState from CAN
+    can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
+    CS = self.CI.update(self.CC, can_strs)
+
+    self.sm.update(0)
+
+    # Check for CAN timeout
+    if not can_strs:
+      self.can_error_counter += 1
+      self.can_rcv_error = True
+    else:
+      self.can_rcv_error = False
 
     # When the panda and controlsd do not agree on controls_allowed
     # we want to disengage openpilot. 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 enabled:
-    mismatch_counter = 0
+    if not self.enabled:
+      self.mismatch_counter = 0
 
-  controls_allowed = sm['health'].controlsAllowed
-  if not controls_allowed and enabled:
-    mismatch_counter += 1
-  if mismatch_counter >= 200:
-    events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
+    if not self.sm['health'].controlsAllowed and self.enabled:
+      self.mismatch_counter += 1
 
-  return CS, events, cal_perc, mismatch_counter, can_error_counter
+    return CS
 
 
-def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM):
+  def state_transition(self, CS, events):
     """Compute conditional state transitions and execute actions on state transitions"""
-  enabled = isEnabled(state)
 
-  v_cruise_kph_last = v_cruise_kph
+    self.v_cruise_kph_last = self.v_cruise_kph
 
     # if stock cruise is completely disabled, then we can use our own set speed logic
-  if not CP.enableCruise:
-    v_cruise_kph = update_v_cruise(v_cruise_kph, CS.buttonEvents, enabled)
-  elif CP.enableCruise and CS.cruiseState.enabled:
-    v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
+    if not self.CP.enableCruise:
+      self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.buttonEvents, self.enabled)
+    elif self.CP.enableCruise and CS.cruiseState.enabled:
+      self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
 
     # decrease the soft disable timer at every step, as it's reset on
     # entrance in SOFT_DISABLING state
-  soft_disable_timer = max(0, soft_disable_timer - 1)
+    self.soft_disable_timer = max(0, self.soft_disable_timer - 1)
 
-  # DISABLED
-  if state == State.disabled:
-    if get_events(events, [ET.ENABLE]):
-      if get_events(events, [ET.NO_ENTRY]):
-        for e in get_events(events, [ET.NO_ENTRY]):
-          AM.add(frame, str(e) + "NoEntry", enabled)
+    alert_types = []
 
-      else:
-        if get_events(events, [ET.PRE_ENABLE]):
-          state = State.preEnabled
-        else:
-          state = State.enabled
-        AM.add(frame, "enable", enabled)
-        v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, v_cruise_kph_last)
-
-  # ENABLED
-  elif state == State.enabled:
+    # ENABLED, PRE ENABLING, SOFT DISABLING
+    if self.state != State.disabled:
+      # user and immediate disable always have priority in a non-disabled state
       if get_events(events, [ET.USER_DISABLE]):
-      state = State.disabled
-      AM.add(frame, "disable", enabled)
+        self.state = State.disabled
+        self.AM.add(self.sm.frame, "disable", self.enabled)
 
       elif get_events(events, [ET.IMMEDIATE_DISABLE]):
-      state = State.disabled
-      for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
-        AM.add(frame, e, enabled)
+        self.state = State.disabled
+        alert_types = [ET.IMMEDIATE_DISABLE]
 
-    elif get_events(events, [ET.SOFT_DISABLE]):
-      state = State.softDisabling
-      soft_disable_timer = 300   # 3s
-      for e in get_events(events, [ET.SOFT_DISABLE]):
-        AM.add(frame, e, enabled)
+      else:
+        # ENABLED
+        if self.state == State.enabled:
+          if get_events(events, [ET.SOFT_DISABLE]):
+            self.state = State.softDisabling
+            self.soft_disable_timer = 300   # 3s
+            alert_types = [ET.SOFT_DISABLE]
 
         # SOFT DISABLING
-  elif state == State.softDisabling:
-    if get_events(events, [ET.USER_DISABLE]):
-      state = State.disabled
-      AM.add(frame, "disable", enabled)
-
-    elif get_events(events, [ET.IMMEDIATE_DISABLE]):
-      state = State.disabled
-      for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
-        AM.add(frame, e, enabled)
-
-    elif not get_events(events, [ET.SOFT_DISABLE]):
+        elif self.state == State.softDisabling:
+          if not get_events(events, [ET.SOFT_DISABLE]):
             # no more soft disabling condition, so go back to ENABLED
-      state = State.enabled
+            self.state = State.enabled
 
-    elif get_events(events, [ET.SOFT_DISABLE]) and soft_disable_timer > 0:
-      for e in get_events(events, [ET.SOFT_DISABLE]):
-        AM.add(frame, e, enabled)
+          elif get_events(events, [ET.SOFT_DISABLE]) and self.soft_disable_timer > 0:
+            alert_types = [ET.SOFT_DISABLE]
 
-    elif soft_disable_timer <= 0:
-      state = State.disabled
+          elif self.soft_disable_timer <= 0:
+            self.state = State.disabled
 
         # PRE ENABLING
-  elif state == State.preEnabled:
-    if get_events(events, [ET.USER_DISABLE]):
-      state = State.disabled
-      AM.add(frame, "disable", enabled)
+        elif self.state == State.preEnabled:
+          if not get_events(events, [ET.PRE_ENABLE]):
+            self.state = State.enabled
+
+    # DISABLED
+    elif self.state == State.disabled:
+      if get_events(events, [ET.ENABLE]):
+        if get_events(events, [ET.NO_ENTRY]):
+          for e in get_events(events, [ET.NO_ENTRY]):
+            self.AM.add(self.sm.frame, str(e) + "NoEntry", self.enabled)
+
+        else:
+          if get_events(events, [ET.PRE_ENABLE]):
+            self.state = State.preEnabled
+          else:
+            self.state = State.enabled
+          self.AM.add(self.sm.frame, "enable", self.enabled)
+          self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)
 
-    elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
-      state = State.disabled
-      for e in get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
-        AM.add(frame, e, enabled)
+    for e in get_events(events, alert_types):
+      self.AM.add(self.sm.frame, e, self.enabled)
 
-    elif not get_events(events, [ET.PRE_ENABLE]):
-      state = State.enabled
+    # Check if actuators are enabled
+    self.active = self.state == State.enabled or self.state == State.softDisabling
 
-  return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last
+    # Check if openpilot is engaged
+    self.enabled = self.active or self.state == State.preEnabled
 
 
-def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
-                  AM, rk, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, saturated_count):
+  def state_control(self, CS, events):
     """Given the state, this function returns an actuators packet"""
 
-  actuators = car.CarControl.Actuators.new_message()
+    plan = self.sm['plan']
+    path_plan = self.sm['pathPlan']
 
-  enabled = isEnabled(state)
-  active = isActive(state)
+    actuators = car.CarControl.Actuators.new_message()
 
     if CS.leftBlinker or CS.rightBlinker:
-    last_blinker_frame = frame
+      self.last_blinker_frame = self.sm.frame
 
     if plan.fcw:
       # send FCW alert if triggered by planner
-    AM.add(frame, "fcw", enabled)
+      self.AM.add(self.sm.frame, "fcw", self.enabled)
 
     elif CS.stockFcw:
       # send a silent alert when stock fcw triggers, since the car is already beeping
-    AM.add(frame, "fcwStock", enabled)
+      self.AM.add(self.sm.frame, "fcwStock", self.enabled)
 
     # State specific actions
 
-  if state in [State.preEnabled, State.disabled]:
-    LaC.reset()
-    LoC.reset(v_pid=CS.vEgo)
+    if not self.active:
+      self.LaC.reset()
+      self.LoC.reset(v_pid=CS.vEgo)
 
-  elif state in [State.enabled, State.softDisabling]:
-    # parse warnings from car specific interface
-    for e in get_events(events, [ET.WARNING]):
-      extra_text = ""
-      if e == "belowSteerSpeed":
-        if is_metric:
-          extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph"
-        else:
-          extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph"
-      AM.add(frame, e, enabled, extra_text_2=extra_text)
-
-  plan_age = DT_CTRL * (frame - rcv_frame['plan'])
-  dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL  # no greater than dt mpc + dt, to prevent too high extraps
+    plan_age = DT_CTRL * (self.sm.frame - self.sm.rcv_frame['plan'])
+    # no greater than dt mpc + dt, to prevent too high extraps
+    dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL
 
     a_acc_sol = plan.aStart + (dt / LON_MPC_STEP) * (plan.aTarget - plan.aStart)
     v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0
 
     # Gas/Brake PID loop
-  actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
-                                              v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP)
+    actuators.gas, actuators.brake = self.LoC.update(self.active, CS, v_acc_sol, plan.vTargetFuture, a_acc_sol, self.CP)
     # Steering PID loop and lateral MPC
-  actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CS.steeringRateLimited, CP, path_plan)
+    actuators.steer, actuators.steerAngle, lac_log = self.LaC.update(self.active, CS, self.CP, path_plan)
 
     # Check for difference between desired angle and angle for angle based control
-  angle_control_saturated = CP.steerControlType == car.CarParams.SteerControlType.angle and \
+    angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
       abs(actuators.steerAngle - CS.steeringAngle) > STEER_ANGLE_SATURATION_THRESHOLD
 
-  saturated_count = saturated_count + 1 if angle_control_saturated and not CS.steeringPressed and active else 0
+    if angle_control_saturated and not CS.steeringPressed and self.active:
+      self.saturated_count += 1
 
     # Send a "steering required alert" if saturation count has reached the limit
-  if (lac_log.saturated and not CS.steeringPressed) or (saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
+    if (lac_log.saturated and not CS.steeringPressed) or \
+        (self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
       # Check if we deviated from the path
       left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1
       right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1
 
       if left_deviation or right_deviation:
-      AM.add(frame, "steerSaturated", enabled)
-
-  # Parse permanent warnings to display constantly
-  for e in get_events(events, [ET.PERMANENT]):
-    extra_text_1, extra_text_2 = "", ""
-    if e == "calibrationIncomplete":
-      extra_text_1 = str(cal_perc) + "%"
-      if is_metric:
-        extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph"
-      else:
-        extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph"
-    AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2)
+        self.AM.add(self.sm.frame, "steerSaturated", self.enabled)
 
-  return actuators, v_cruise_kph, v_acc_sol, a_acc_sol, lac_log, last_blinker_frame, saturated_count
+    return actuators, v_acc_sol, a_acc_sol, lac_log
 
 
-def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM,
-              LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev,
-              last_blinker_frame, is_ldw_enabled, can_error_counter):
+  def publish_logs(self, CS, events, start_time, actuators, v_acc, a_acc, lac_log):
     """Send actuators and hud commands to the car, send controlsstate and MPC logging"""
 
     CC = car.CarControl.new_message()
-  CC.enabled = isEnabled(state)
+    CC.enabled = self.enabled
     CC.actuators = actuators
 
     CC.cruiseControl.override = True
-  CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled)
+    CC.cruiseControl.cancel = not self.CP.enableCruise or (not self.enabled and CS.cruiseState.enabled)
 
     # Some override values for Honda
-  brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0))  # brake discount removes a sharp nonlinearity
-  CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)
-  CC.cruiseControl.accelOverride = CI.calc_accel_override(CS.aEgo, sm['plan'].aTarget, CS.vEgo, sm['plan'].vTarget)
-
-  CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
-  CC.hudControl.speedVisible = isEnabled(state)
-  CC.hudControl.lanesVisible = isEnabled(state)
-  CC.hudControl.leadVisible = sm['plan'].hasLead
-
-  right_lane_visible = sm['pathPlan'].rProb > 0.5
-  left_lane_visible = sm['pathPlan'].lProb > 0.5
+    # brake discount removes a sharp nonlinearity
+    brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0))
+    speed_override = max(0.0, (self.LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount)
+    CC.cruiseControl.speedOverride = float(speed_override if self.CP.enableCruise else 0.0)
+    CC.cruiseControl.accelOverride = self.CI.calc_accel_override(CS.aEgo, self.sm['plan'].aTarget, CS.vEgo, self.sm['plan'].vTarget)
+
+    CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
+    CC.hudControl.speedVisible = self.enabled
+    CC.hudControl.lanesVisible = self.enabled
+    CC.hudControl.leadVisible = self.sm['plan'].hasLead
+
+    right_lane_visible = self.sm['pathPlan'].rProb > 0.5
+    left_lane_visible = self.sm['pathPlan'].lProb > 0.5
     CC.hudControl.rightLaneVisible = bool(right_lane_visible)
     CC.hudControl.leftLaneVisible = bool(left_lane_visible)
 
-  recent_blinker = (sm.frame - last_blinker_frame) * DT_CTRL < 5.0  # 5s blinker cooldown
-  calibrated = sm['liveCalibration'].calStatus == Calibration.CALIBRATED
-  ldw_allowed = CS.vEgo > 31 * CV.MPH_TO_MS and not recent_blinker and is_ldw_enabled and not isActive(state) and calibrated
-
-  md = sm['model']
-  if len(md.meta.desirePrediction):
-    l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1]
-    r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 1]
+    recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0  # 5s blinker cooldown
+    ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
+                    and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
 
-    l_lane_close = left_lane_visible and (sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET))
-    r_lane_close = right_lane_visible and (sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET))
+    meta = self.sm['model'].meta
+    if len(meta.desirePrediction) and ldw_allowed:
+      l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1]
+      r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1]
+      l_lane_close = left_lane_visible and (self.sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET))
+      r_lane_close = right_lane_visible and (self.sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET))
 
-    if ldw_allowed:
       CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
       CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
 
     if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart:
-    AM.add(sm.frame, 'ldwPermanent', False)
+      self.AM.add(self.sm.frame, 'ldwPermanent', False)
       events.append(create_event('ldw', [ET.PERMANENT]))
 
-  AM.process_alerts(sm.frame)
-  CC.hudControl.visualAlert = AM.visual_alert
+    self.AM.process_alerts(self.sm.frame)
+    CC.hudControl.visualAlert = self.AM.visual_alert
 
-  if not read_only:
+    if not self.read_only:
       # send car controls over can
-    can_sends = CI.apply(CC)
-    pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
+      can_sends = self.CI.apply(CC)
+      self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
 
-  force_decel = (sm['dMonitoringState'].awarenessStatus < 0.) or (state == State.softDisabling)
+    force_decel = (self.sm['dMonitoringState'].awarenessStatus < 0.) or \
+                    (self.state == State.softDisabling)
+
+    steer_angle_rad = (CS.steeringAngle - self.sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD
 
     # controlsState
     dat = messaging.new_message('controlsState')
     dat.valid = CS.canValid
-  dat.controlsState = {
-    "alertText1": AM.alert_text_1,
-    "alertText2": AM.alert_text_2,
-    "alertSize": AM.alert_size,
-    "alertStatus": AM.alert_status,
-    "alertBlinkingRate": AM.alert_rate,
-    "alertType": AM.alert_type,
-    "alertSound": AM.audible_alert,
-    "driverMonitoringOn": sm['dMonitoringState'].faceDetected,
-    "canMonoTimes": list(CS.canMonoTimes),
-    "planMonoTime": sm.logMonoTime['plan'],
-    "pathPlanMonoTime": sm.logMonoTime['pathPlan'],
-    "enabled": isEnabled(state),
-    "active": isActive(state),
-    "vEgo": CS.vEgo,
-    "vEgoRaw": CS.vEgoRaw,
-    "angleSteers": CS.steeringAngle,
-    "curvature": VM.calc_curvature((CS.steeringAngle - sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD, CS.vEgo),
-    "steerOverride": CS.steeringPressed,
-    "state": state,
-    "engageable": not bool(get_events(events, [ET.NO_ENTRY])),
-    "longControlState": LoC.long_control_state,
-    "vPid": float(LoC.v_pid),
-    "vCruise": float(v_cruise_kph),
-    "upAccelCmd": float(LoC.pid.p),
-    "uiAccelCmd": float(LoC.pid.i),
-    "ufAccelCmd": float(LoC.pid.f),
-    "angleSteersDes": float(LaC.angle_steers_des),
-    "vTargetLead": float(v_acc),
-    "aTarget": float(a_acc),
-    "jerkFactor": float(sm['plan'].jerkFactor),
-    "gpsPlannerActive": sm['plan'].gpsPlannerActive,
-    "vCurvature": sm['plan'].vCurvature,
-    "decelForModel": sm['plan'].longitudinalPlanSource == log.Plan.LongitudinalPlanSource.model,
-    "cumLagMs": -rk.remaining * 1000.,
-    "startMonoTime": int(start_time * 1e9),
-    "mapValid": sm['plan'].mapValid,
-    "forceDecel": bool(force_decel),
-    "canErrorCounter": can_error_counter,
-  }
-
-  if CP.lateralTuning.which() == 'pid':
-    dat.controlsState.lateralControlState.pidState = lac_log
-  elif CP.lateralTuning.which() == 'lqr':
-    dat.controlsState.lateralControlState.lqrState = lac_log
-  elif CP.lateralTuning.which() == 'indi':
-    dat.controlsState.lateralControlState.indiState = lac_log
-  pm.send('controlsState', dat)
+    controlsState = dat.controlsState
+    controlsState.alertText1 = self.AM.alert_text_1
+    controlsState.alertText2 = self.AM.alert_text_2
+    controlsState.alertSize = self.AM.alert_size
+    controlsState.alertStatus = self.AM.alert_status
+    controlsState.alertBlinkingRate = self.AM.alert_rate
+    controlsState.alertType = self.AM.alert_type
+    controlsState.alertSound = self.AM.audible_alert
+    controlsState.driverMonitoringOn = self.sm['dMonitoringState'].faceDetected
+    controlsState.canMonoTimes = list(CS.canMonoTimes)
+    controlsState.planMonoTime = self.sm.logMonoTime['plan']
+    controlsState.pathPlanMonoTime = self.sm.logMonoTime['pathPlan']
+    controlsState.enabled = self.enabled
+    controlsState.active = self.active
+    controlsState.vEgo = CS.vEgo
+    controlsState.vEgoRaw = CS.vEgoRaw
+    controlsState.angleSteers = CS.steeringAngle
+    controlsState.curvature = self.VM.calc_curvature(steer_angle_rad, CS.vEgo)
+    controlsState.steerOverride = CS.steeringPressed
+    controlsState.state = self.state
+    controlsState.engageable = not bool(get_events(events, [ET.NO_ENTRY]))
+    controlsState.longControlState = self.LoC.long_control_state
+    controlsState.vPid = float(self.LoC.v_pid)
+    controlsState.vCruise = float(self.v_cruise_kph)
+    controlsState.upAccelCmd = float(self.LoC.pid.p)
+    controlsState.uiAccelCmd = float(self.LoC.pid.i)
+    controlsState.ufAccelCmd = float(self.LoC.pid.f)
+    controlsState.angleSteersDes = float(self.LaC.angle_steers_des)
+    controlsState.vTargetLead = float(v_acc)
+    controlsState.aTarget = float(a_acc)
+    controlsState.jerkFactor = float(self.sm['plan'].jerkFactor)
+    controlsState.gpsPlannerActive = self.sm['plan'].gpsPlannerActive
+    controlsState.vCurvature = self.sm['plan'].vCurvature
+    controlsState.decelForModel = self.sm['plan'].longitudinalPlanSource == LongitudinalPlanSource.model
+    controlsState.cumLagMs = -self.rk.remaining * 1000.
+    controlsState.startMonoTime = int(start_time * 1e9)
+    controlsState.mapValid = self.sm['plan'].mapValid
+    controlsState.forceDecel = bool(force_decel)
+    controlsState.canErrorCounter = self.can_error_counter
+
+    if self.CP.lateralTuning.which() == 'pid':
+      controlsState.lateralControlState.pidState = lac_log
+    elif self.CP.lateralTuning.which() == 'lqr':
+      controlsState.lateralControlState.lqrState = lac_log
+    elif self.CP.lateralTuning.which() == 'indi':
+      controlsState.lateralControlState.indiState = lac_log
+    self.pm.send('controlsState', dat)
 
     # carState
     cs_send = messaging.new_message('carState')
     cs_send.valid = CS.canValid
     cs_send.carState = CS
     cs_send.carState.events = events
-  pm.send('carState', cs_send)
+    self.pm.send('carState', cs_send)
 
     # carEvents - logged every second or on change
     events_bytes = events_to_bytes(events)
-  if (sm.frame % int(1. / DT_CTRL) == 0) or (events_bytes != events_prev):
+    if (self.sm.frame % int(1. / DT_CTRL) == 0) or (events_bytes != self.events_prev):
       ce_send = messaging.new_message('carEvents', len(events))
       ce_send.carEvents = events
-    pm.send('carEvents', ce_send)
+      self.pm.send('carEvents', ce_send)
+    self.events_prev = events_bytes
 
     # carParams - logged every 50 seconds (> 1 per segment)
-  if (sm.frame % int(50. / DT_CTRL) == 0):
+    if (self.sm.frame % int(50. / DT_CTRL) == 0):
       cp_send = messaging.new_message('carParams')
-    cp_send.carParams = CP
-    pm.send('carParams', cp_send)
+      cp_send.carParams = self.CP
+      self.pm.send('carParams', cp_send)
 
     # carControl
     cc_send = messaging.new_message('carControl')
     cc_send.valid = CS.canValid
     cc_send.carControl = CC
-  pm.send('carControl', cc_send)
-
-  return CC, events_bytes
-
+    self.pm.send('carControl', cc_send)
 
-def controlsd_thread(sm=None, pm=None, can_sock=None):
-  gc.disable()
-
-  # start the loop
-  set_realtime_priority(3)
-
-  params = Params()
-
-  is_metric = params.get("IsMetric", encoding='utf8') == "1"
-  is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
-  passive = params.get("Passive", encoding='utf8') == "1"
-  openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1"
-  community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1"
+    # copy CarControl to pass to CarInterface on the next iteration
+    self.CC = CC
 
-  passive = passive or not openpilot_enabled_toggle
-
-  # Passive if internet needed
-  internet_needed = params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None
-  passive = passive or internet_needed
-
-  # Pub/Sub Sockets
-  if pm is None:
-    pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams'])
-
-  if sm is None:
-    sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'dMonitoringState', 'plan', 'pathPlan', \
-                              'model'])
-
-  if can_sock is None:
-    can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
-    can_sock = messaging.sub_sock('can', timeout=can_timeout)
-
-  # wait for health and CAN packets
-  hw_type = messaging.recv_one(sm.sock['health']).health.hwType
-  has_relay = hw_type in [HwType.blackPanda, HwType.uno]
-  print("Waiting for CAN messages...")
-  messaging.get_one_can(can_sock)
-
-  CI, CP = get_car(can_sock, pm.sock['sendcan'], has_relay)
-
-  car_recognized = CP.carName != 'mock'
-  # If stock camera is disconnected, we loaded car controls and it's not chffrplus
-  controller_available = CP.enableCamera and CI.CC is not None and not passive
-  community_feature_disallowed = CP.communityFeature and not community_feature_toggle
-  read_only = not car_recognized or not controller_available or CP.dashcamOnly or community_feature_disallowed
-  if read_only:
-    CP.safetyModel = car.CarParams.SafetyModel.noOutput
-
-  # Write CarParams for radard and boardd safety mode
-  cp_bytes = CP.to_bytes()
-  params.put("CarParams", cp_bytes)
-  put_nonblocking("CarParamsCache", cp_bytes)
-  put_nonblocking("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0")
-
-  CC = car.CarControl.new_message()
-  AM = AlertManager()
-
-  startup_alert = get_startup_alert(car_recognized, controller_available)
-  AM.add(sm.frame, startup_alert, False)
-
-  LoC = LongControl(CP, CI.compute_gb)
-  VM = VehicleModel(CP)
-
-  if CP.lateralTuning.which() == 'pid':
-    LaC = LatControlPID(CP)
-  elif CP.lateralTuning.which() == 'indi':
-    LaC = LatControlINDI(CP)
-  elif CP.lateralTuning.which() == 'lqr':
-    LaC = LatControlLQR(CP)
-
-  state = State.disabled
-  soft_disable_timer = 0
-  v_cruise_kph = 255
-  v_cruise_kph_last = 0
-  mismatch_counter = 0
-  can_error_counter = 0
-  last_blinker_frame = 0
-  saturated_count = 0
-  events_prev = []
-
-  sm['liveCalibration'].calStatus = Calibration.INVALID
-  sm['pathPlan'].sensorValid = True
-  sm['pathPlan'].posenetValid = True
-  sm['thermal'].freeSpace = 1.
-  sm['dMonitoringState'].events = []
-  sm['dMonitoringState'].awarenessStatus = 1.
-  sm['dMonitoringState'].faceDetected = False
-
-  # detect sound card presence
-  sounds_available = not os.path.isfile('/EON') or (os.path.isdir('/proc/asound/card0') and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
-
-  # controlsd is driven by can recv, expected at 100Hz
-  rk = Ratekeeper(100, print_delay_threshold=None)
-
-
-  prof = Profiler(False)  # off by default
-
-  while True:
+  def step(self):
     start_time = sec_since_boot()
-    prof.checkpoint("Ratekeeper", ignore=True)
+    self.prof.checkpoint("Ratekeeper", ignore=True)
 
-    # Sample data and compute car events
-    CS, events, cal_perc, mismatch_counter, can_error_counter = data_sample(CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter, params)
-    prof.checkpoint("Sample")
+    # Sample data from sockets and get a carState
+    CS = self.data_sample()
+    self.prof.checkpoint("Sample")
 
-    # Create alerts
-    if not sm.alive['plan'] and sm.alive['pathPlan']:  # only plan not being received: radar not communicating
-      events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
-    elif not sm.all_alive_and_valid():
-      events.append(create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
-    if not sm['pathPlan'].mpcSolutionValid:
-      events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
-    if not sm['pathPlan'].sensorValid and os.getenv("NOSENSOR") is None:
-      events.append(create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
-    if not sm['pathPlan'].paramsValid:
-      events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
-    if not sm['pathPlan'].posenetValid:
-      events.append(create_event('posenetInvalid', [ET.NO_ENTRY, ET.WARNING]))
-    if not sm['plan'].radarValid:
-      events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
-    if sm['plan'].radarCanError:
-      events.append(create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
-    if not CS.canValid:
-      events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
-    if not sounds_available:
-      events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
-    if internet_needed:
-      events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT]))
-    if community_feature_disallowed:
-      events.append(create_event('communityFeatureDisallowed', [ET.PERMANENT]))
-    if read_only and not passive:
-      events.append(create_event('carUnrecognized', [ET.PERMANENT]))
-    if log.HealthData.FaultType.relayMalfunction in sm['health'].faults:
-      events.append(create_event('relayMalfunction', [ET.NO_ENTRY, ET.PERMANENT, ET.IMMEDIATE_DISABLE]))
+    events = self.create_events(CS)
 
-
-    # Only allow engagement with brake pressed when stopped behind another stopped car
-    if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
-      events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
-
-    if not read_only:
-      # update control state
-      state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
-        state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
-      prof.checkpoint("State transition")
+    if not self.read_only:
+      # Update control state
+      self.state_transition(CS, events)
+      self.prof.checkpoint("State transition")
 
     # Compute actuators (runs PID loops and lateral MPC)
-    actuators, v_cruise_kph, v_acc, a_acc, lac_log, last_blinker_frame, saturated_count = \
-      state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
-                    LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, saturated_count)
+    actuators, v_acc, a_acc, lac_log = self.state_control(CS, events)
 
-    prof.checkpoint("State Control")
+    self.prof.checkpoint("State Control")
 
     # Publish data
-    CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, LaC,
-                                LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame,
-                                is_ldw_enabled, can_error_counter)
-    prof.checkpoint("Sent")
-
-    rk.monitor_time()
-    prof.display()
+    self.publish_logs(CS, events, start_time, actuators, v_acc, a_acc, lac_log)
+    self.prof.checkpoint("Sent")
 
+  def controlsd_thread(self):
+    while True:
+      self.step()
+      self.rk.monitor_time()
+      self.prof.display()
 
 def main(sm=None, pm=None, logcan=None):
-  controlsd_thread(sm, pm, logcan)
+  controls = Controls(sm, pm, logcan)
+  controls.controlsd_thread()
 
 
 if __name__ == "__main__":

selfdrive/controls/lib/latcontrol_indi.py

@@ -62,9 +62,9 @@ class LatControlINDI():
 
     return self.sat_count > self.sat_limit
 
-  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan):
+  def update(self, active, CS, CP, path_plan):
     # Update Kalman filter
-    y = np.matrix([[math.radians(angle_steers)], [math.radians(angle_steers_rate)]])
+    y = np.matrix([[math.radians(CS.steeringAngle)], [math.radians(CS.steeringRate)]])
     self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
 
     indi_log = log.ControlsState.LateralINDIState.new_message()
@@ -72,7 +72,7 @@ class LatControlINDI():
     indi_log.steerRate = math.degrees(self.x[1])
     indi_log.steerAccel = math.degrees(self.x[2])
 
-    if v_ego < 0.3 or not active:
+    if CS.vEgo < 0.3 or not active:
       indi_log.active = False
       self.output_steer = 0.0
       self.delayed_output = 0.0
@@ -105,7 +105,7 @@ class LatControlINDI():
       else:
         self.output_steer = self.delayed_output + delta_u
 
-      steers_max = get_steer_max(CP, v_ego)
+      steers_max = get_steer_max(CP, CS.vEgo)
       self.output_steer = clip(self.output_steer, -steers_max, steers_max)
 
       indi_log.active = True
@@ -116,7 +116,7 @@ class LatControlINDI():
       indi_log.delta = float(delta_u)
       indi_log.output = float(self.output_steer)
 
-      check_saturation = (v_ego > 10.) and not rate_limited and not steer_override
+      check_saturation = (CS.vEgo> 10.) and not CS.steeringRateLimited and not CS.steeringPressed 
       indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
 
     return float(self.output_steer), float(self.angle_steers_des), indi_log

selfdrive/controls/lib/latcontrol_lqr.py

@@ -43,22 +43,24 @@ class LatControlLQR():
 
     return self.sat_count > self.sat_limit
 
-  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan):
+  def update(self, active, CS, CP, path_plan):
     lqr_log = log.ControlsState.LateralLQRState.new_message()
 
-    steers_max = get_steer_max(CP, v_ego)
-    torque_scale = (0.45 + v_ego / 60.0)**2  # Scale actuator model with speed
+    steers_max = get_steer_max(CP, CS.vEgo)
+    torque_scale = (0.45 + CS.vEgo / 60.0)**2  # Scale actuator model with speed
+
+    steering_angle = CS.steeringAngle
 
     # Subtract offset. Zero angle should correspond to zero torque
     self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset
-    angle_steers -= path_plan.angleOffset
+    steering_angle -= path_plan.angleOffset
 
     # Update Kalman filter
     angle_steers_k = float(self.C.dot(self.x_hat))
-    e = angle_steers - angle_steers_k
-    self.x_hat = self.A.dot(self.x_hat) + self.B.dot(eps_torque / torque_scale) + self.L.dot(e)
+    e = steering_angle - angle_steers_k
+    self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
 
-    if v_ego < 0.3 or not active:
+    if CS.vEgo < 0.3 or not active:
       lqr_log.active = False
       lqr_output = 0.
       self.reset()
@@ -70,7 +72,7 @@ class LatControlLQR():
       lqr_output = torque_scale * u_lqr / self.scale
 
       # Integrator
-      if steer_override:
+      if CS.steeringPressed:
         self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
       else:
         error = self.angle_steers_des - angle_steers_k
@@ -84,7 +86,7 @@ class LatControlLQR():
       self.output_steer = lqr_output + self.i_lqr
       self.output_steer = clip(self.output_steer, -steers_max, steers_max)
 
-    check_saturation = (v_ego > 10) and not rate_limited and not steer_override
+    check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
     saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
 
     lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset

selfdrive/controls/lib/latcontrol_pid.py

@@ -14,31 +14,31 @@ class LatControlPID():
   def reset(self):
     self.pid.reset()
 
-  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan):
+  def update(self, active, CS, CP, path_plan):
     pid_log = log.ControlsState.LateralPIDState.new_message()
-    pid_log.steerAngle = float(angle_steers)
-    pid_log.steerRate = float(angle_steers_rate)
+    pid_log.steerAngle = float(CS.steeringAngle)
+    pid_log.steerRate = float(CS.steeringRate)
 
-    if v_ego < 0.3 or not active:
+    if CS.vEgo < 0.3 or not active:
       output_steer = 0.0
       pid_log.active = False
       self.pid.reset()
     else:
       self.angle_steers_des = path_plan.angleSteers  # get from MPC/PathPlanner
 
-      steers_max = get_steer_max(CP, v_ego)
+      steers_max = get_steer_max(CP, CS.vEgo)
       self.pid.pos_limit = steers_max
       self.pid.neg_limit = -steers_max
       steer_feedforward = self.angle_steers_des   # feedforward desired angle
       if CP.steerControlType == car.CarParams.SteerControlType.torque:
         # TODO: feedforward something based on path_plan.rateSteers
         steer_feedforward -= path_plan.angleOffset   # subtract the offset, since it does not contribute to resistive torque
-        steer_feedforward *= v_ego**2  # proportional to realigning tire momentum (~ lateral accel)
+        steer_feedforward *= CS.vEgo**2  # proportional to realigning tire momentum (~ lateral accel)
       deadzone = 0.0
 
-      check_saturation = (v_ego > 10) and not rate_limited and not steer_override
-      output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=check_saturation, override=steer_override,
-                                     feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone)
+      check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
+      output_steer = self.pid.update(self.angle_steers_des, CS.steeringAngle, check_saturation=check_saturation, override=CS.steeringPressed,
+                                     feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone)
       pid_log.active = True
       pid_log.p = self.pid.p
       pid_log.i = self.pid.i

selfdrive/controls/lib/longcontrol.py

@@ -71,19 +71,19 @@ class LongControl():
     self.pid.reset()
     self.v_pid = v_pid
 
-  def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP):
+  def update(self, active, CS, v_target, v_target_future, a_target, CP):
     """Update longitudinal control. This updates the state machine and runs a PID loop"""
     # Actuation limits
-    gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
-    brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
+    gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
+    brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
 
     # Update state machine
     output_gb = self.last_output_gb
-    self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,
+    self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo,
                                                        v_target_future, self.v_pid, output_gb,
-                                                       brake_pressed, cruise_standstill)
+                                                       CS.brakePressed, CS.cruiseState.standstill)
 
-    v_ego_pid = max(v_ego, MIN_CAN_SPEED)  # Without this we get jumps, CAN bus reports 0 when speed < 0.3
+    v_ego_pid = max(CS.vEgo, MIN_CAN_SPEED)  # Without this we get jumps, CAN bus reports 0 when speed < 0.3
 
     if self.long_control_state == LongCtrlState.off:
       self.v_pid = v_ego_pid
@@ -98,7 +98,7 @@ class LongControl():
 
       # Toyota starts braking more when it thinks you want to stop
       # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
-      prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7
+      prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
       deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
 
       output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)
@@ -109,18 +109,18 @@ class LongControl():
     # Intention is to stop, switch to a different brake control until we stop
     elif self.long_control_state == LongCtrlState.stopping:
       # Keep applying brakes until the car is stopped
-      if not standstill or output_gb > -BRAKE_STOPPING_TARGET:
+      if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
         output_gb -= STOPPING_BRAKE_RATE / RATE
       output_gb = clip(output_gb, -brake_max, gas_max)
 
-      self.v_pid = v_ego
+      self.v_pid = CS.vEgo
       self.pid.reset()
 
     # Intention is to move again, release brake fast before handing control to PID
     elif self.long_control_state == LongCtrlState.starting:
       if output_gb < -0.2:
         output_gb += STARTING_BRAKE_RATE / RATE
-      self.v_pid = v_ego
+      self.v_pid = CS.vEgo
       self.pid.reset()
 
     self.last_output_gb = output_gb

selfdrive/test/profiling/controlsd.py

@@ -7,7 +7,7 @@ import pprofile
 import pyprof2calltree
 
 from tools.lib.logreader import LogReader
-from selfdrive.controls.controlsd import controlsd_thread
+from selfdrive.controls.controlsd import main as controlsd_thread
 from selfdrive.test.profiling.lib import SubMaster, PubMaster, SubSocket, ReplayDone
 from selfdrive.test.process_replay.process_replay import CONFIGS