selfdrived: controlsd only does controls (#33485)

* selfdrived

* process replay

* lil more

* set the valids

* rename that

cereal/car.capnp

@@ -100,7 +100,7 @@ struct OnroadEvent @0x9b1657f34caf3ad3 {
     cameraFrameRate @110;
     processNotRunning @95;
     dashcamMode @96;
-    controlsInitializing @98;
+    selfdriveInitializing @98;
     usbError @99;
     roadCameraError @100;
     driverCameraError @101;
@@ -109,7 +109,7 @@ struct OnroadEvent @0x9b1657f34caf3ad3 {
     cruiseMismatch @106;
     lkasDisabled @107;
     canBusMissing @111;
-    controlsdLagging @112;
+    selfdrivedLagging @112;
     resumeBlocked @113;
     steerTimeLimit @115;
     vehicleSensorsInvalid @116;

cereal/log.capnp

@@ -698,6 +698,7 @@ struct SelfdriveState {
   alertSize @6 :AlertSize;
   alertType @7 :Text;
   alertSound @8 :Car.CarControl.HUDControl.AudibleAlert;
+  alertHudVisual @12 :Car.CarControl.HUDControl.VisualAlert;
 
   # configurable driving settings
   experimentalMode @10 :Bool;
@@ -726,7 +727,6 @@ struct SelfdriveState {
 }
 
 struct ControlsState @0x97ff69c53601abf1 {
-  cumLagMs @15 :Float32;
   longitudinalPlanMonoTime @28 :UInt64;
   lateralPlanMonoTime @50 :UInt64;
 
@@ -880,6 +880,7 @@ struct ControlsState @0x97ff69c53601abf1 {
   vCruiseDEPRECATED @22 :Float32;  # actual set speed
   vCruiseClusterDEPRECATED @63 :Float32;  # set speed to display in the UI
   startMonoTimeDEPRECATED @48 :UInt64;
+  cumLagMsDEPRECATED @15 :Float32;
 }
 
 struct DrivingModelData {

selfdrive/car/car_specific.py

@@ -5,7 +5,7 @@ from opendbc.car.interfaces import MAX_CTRL_SPEED, CarStateBase, CarControllerBa
 from opendbc.car.volkswagen.values import CarControllerParams as VWCarControllerParams
 from opendbc.car.hyundai.interface import ENABLE_BUTTONS as HYUNDAI_ENABLE_BUTTONS
 
-from openpilot.selfdrive.controls.lib.events import Events
+from openpilot.selfdrive.selfdrived.events import Events
 
 ButtonType = structs.CarState.ButtonEvent.Type
 GearShifter = structs.CarState.GearShifter

selfdrive/car/card.py

@@ -22,7 +22,7 @@ from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
 from openpilot.selfdrive.car.cruise import VCruiseHelper
 from openpilot.selfdrive.car.car_specific import CarSpecificEvents, MockCarState
 from openpilot.selfdrive.car.helpers import convert_carControl, convert_to_capnp
-from openpilot.selfdrive.controls.lib.events import Events, ET
+from openpilot.selfdrive.selfdrived.events import Events, ET
 
 REPLAY = "REPLAY" in os.environ
 
@@ -266,7 +266,7 @@ class Car:
 
     self.state_publish(CS, RD)
 
-    initialized = (not any(e.name == EventName.controlsInitializing for e in self.sm['onroadEvents']) and
+    initialized = (not any(e.name == EventName.selfdriveInitializing for e in self.sm['onroadEvents']) and
                    self.sm.seen['onroadEvents'])
     if not self.CP.passive and initialized:
       self.controls_update(CS, self.sm['carControl'])

selfdrive/controls/controlsd.py

@@ -1,132 +1,53 @@
 #!/usr/bin/env python3
-import os
 import math
-import time
-import threading
 from typing import SupportsFloat
 
-import cereal.messaging as messaging
-
 from cereal import car, log
-from msgq.visionipc import VisionIpcClient, VisionStreamType
+import cereal.messaging as messaging
-
-
 from openpilot.common.conversions import Conversions as CV
-from openpilot.common.git import get_short_branch
-from openpilot.common.numpy_fast import clip
 from openpilot.common.params import Params
-from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL
+from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper
 from openpilot.common.swaglog import cloudlog
-from openpilot.common.gps import get_gps_location_service
 
 from opendbc.car.car_helpers import get_car_interface
-from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
+from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
-from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature, get_startup_event
-from openpilot.selfdrive.controls.lib.events import Events, ET
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
 from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from openpilot.selfdrive.controls.lib.longcontrol import LongControl
 from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
-from openpilot.selfdrive.controls.lib.selfdrive import StateMachine
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 
-from openpilot.system.hardware import HARDWARE
-
-JOYSTICK_MAX_LAT_ACCEL = 2.5  # m/s^2
-
-REPLAY = "REPLAY" in os.environ
-SIMULATION = "SIMULATION" in os.environ
-TESTING_CLOSET = "TESTING_CLOSET" in os.environ
-IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"}
 
-ThermalStatus = log.DeviceState.ThermalStatus
 State = log.SelfdriveState.OpenpilotState
-PandaType = log.PandaState.PandaType
 LaneChangeState = log.LaneChangeState
 LaneChangeDirection = log.LaneChangeDirection
-EventName = car.OnroadEvent.EventName
-ButtonType = car.CarState.ButtonEvent.Type
-SafetyModel = car.CarParams.SafetyModel
 
-IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
-CSID_MAP = {"1": EventName.roadCameraError, "2": EventName.wideRoadCameraError, "0": EventName.driverCameraError}
 ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
 
-
 class Controls:
-  def __init__(self, CI=None):
+  def __init__(self) -> None:
     self.params = Params()
-
-    if CI is None:
     cloudlog.info("controlsd is waiting for CarParams")
     self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams)
     cloudlog.info("controlsd got CarParams")
 
-      # Uses car interface helper functions, altering state won't be considered by card for actuation
     self.CI = get_car_interface(self.CP)
-    else:
-      self.CI, self.CP = CI, CI.CP
-
-    # Ensure the current branch is cached, otherwise the first iteration of controlsd lags
-    self.branch = get_short_branch()
-
-    # Setup sockets
-    self.pm = messaging.PubMaster(['selfdriveState', 'controlsState', 'carControl', 'onroadEvents'])
-
-    self.gps_location_service = get_gps_location_service(self.params)
-    self.gps_packets = [self.gps_location_service]
-    self.sensor_packets = ["accelerometer", "gyroscope"]
-    self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"]
-
-    self.log_sock = messaging.sub_sock('androidLog')
 
-    # TODO: de-couple controlsd with card/conflate on carState without introducing controls mismatches
+    self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
-    self.car_state_sock = messaging.sub_sock('carState', timeout=20)
+                                   'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
+                                   'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='carState')
+    self.pm = messaging.PubMaster(['carControl', 'controlsState'])
 
-    ignore = self.sensor_packets + self.gps_packets + ['testJoystick']
+    self.steer_limited = False
-    if SIMULATION:
+    self.desired_curvature = 0.0
-      ignore += ['driverCameraState', 'managerState']
-    if REPLAY:
-      # no vipc in replay will make them ignored anyways
-      ignore += ['roadCameraState', 'wideRoadCameraState']
-    self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
-                                   'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose',
-                                   'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
-                                   'testJoystick', 'driverAssistance'] + self.camera_packets + self.sensor_packets + self.gps_packets,
-                                  ignore_alive=ignore, ignore_avg_freq=ignore+['testJoystick'], ignore_valid=['testJoystick', ],
-                                  frequency=int(1/DT_CTRL))
-
-    self.joystick_mode = self.params.get_bool("JoystickDebugMode")
-
-    # read params
-    self.is_metric = self.params.get_bool("IsMetric")
-    self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
-
-    # detect sound card presence and ensure successful init
-    sounds_available = HARDWARE.get_sound_card_online()
-
-    car_recognized = self.CP.carName != 'mock'
-
-    # cleanup old params
-    if not self.CP.experimentalLongitudinalAvailable:
-      self.params.remove("ExperimentalLongitudinalEnabled")
-    if not self.CP.openpilotLongitudinalControl:
-      self.params.remove("ExperimentalMode")
-
-    self.CS_prev = car.CarState.new_message()
-    self.CC_prev = car.CarControl.new_message()
-    self.lac_log_prev = None
-    self.AM = AlertManager()
-    self.events = Events()
 
     self.pose_calibrator = PoseCalibrator()
     self.calibrated_pose: Pose|None = None
 
     self.LoC = LongControl(self.CP)
     self.VM = VehicleModel(self.CP)
-
     self.LaC: LatControl
     if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
       self.LaC = LatControlAngle(self.CP, self.CI)
@@ -135,341 +56,16 @@ class Controls:
     elif self.CP.lateralTuning.which() == 'torque':
       self.LaC = LatControlTorque(self.CP, self.CI)
 
-    self.initialized = False
+  def update(self):
-    self.enabled = False
+    self.sm.update(15)
-    self.active = False
-    self.mismatch_counter = 0
-    self.cruise_mismatch_counter = 0
-    self.last_steering_pressed_frame = 0
-    self.distance_traveled = 0
-    self.last_functional_fan_frame = 0
-    self.events_prev = []
-    self.logged_comm_issue = None
-    self.not_running_prev = None
-    self.steer_limited = False
-    self.desired_curvature = 0.0
-    self.experimental_mode = False
-    self.personality = self.read_personality_param()
-    self.recalibrating_seen = False
-    self.state_machine = StateMachine()
-
-    self.can_log_mono_time = 0
-
-    self.startup_event = get_startup_event(car_recognized, not self.CP.passive, len(self.CP.carFw) > 0)
-
-    if not sounds_available:
-      self.events.add(EventName.soundsUnavailable, static=True)
-    if not car_recognized:
-      self.events.add(EventName.carUnrecognized, static=True)
-      if len(self.CP.carFw) > 0:
-        set_offroad_alert("Offroad_CarUnrecognized", True)
-      else:
-        set_offroad_alert("Offroad_NoFirmware", True)
-    elif self.CP.passive:
-      self.events.add(EventName.dashcamMode, static=True)
-
-    # controlsd is driven by carState, expected at 100Hz
-    self.rk = Ratekeeper(100, print_delay_threshold=None)
-
-  def set_initial_state(self):
-    if REPLAY and any(ps.controlsAllowed for ps in self.sm['pandaStates']):
-        self.state_machine.state = State.enabled
-
-  def update_events(self, CS):
-    """Compute onroadEvents from carState"""
-
-    self.events.clear()
-
-    # Add joystick event, static on cars, dynamic on nonCars
-    if self.joystick_mode:
-      self.events.add(EventName.joystickDebug)
-      self.startup_event = None
-
-    # Add startup event
-    if self.startup_event is not None:
-      self.events.add(self.startup_event)
-      self.startup_event = None
-
-    # Don't add any more events if not initialized
-    if not self.initialized:
-      self.events.add(EventName.controlsInitializing)
-      return
-
-    # no more events while in dashcam mode
-    if self.CP.passive:
-      return
-
-    # Block resume if cruise never previously enabled
-    resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents)
-    if not self.CP.pcmCruise and CS.vCruise > 250 and resume_pressed:
-      self.events.add(EventName.resumeBlocked)
-
-    if not self.CP.notCar:
-      self.events.add_from_msg(self.sm['driverMonitoringState'].events)
-
-    # Add car events, ignore if CAN isn't valid
-    if CS.canValid:
-      self.events.add_from_msg(CS.events)
-
-    # Create events for temperature, disk space, and memory
-    if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
-      self.events.add(EventName.overheat)
-    if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
-      self.events.add(EventName.outOfSpace)
-    if self.sm['deviceState'].memoryUsagePercent > 90 and not SIMULATION:
-      self.events.add(EventName.lowMemory)
-
-    # Alert if fan isn't spinning for 5 seconds
-    if self.sm['peripheralState'].pandaType != log.PandaState.PandaType.unknown:
-      if self.sm['peripheralState'].fanSpeedRpm < 500 and self.sm['deviceState'].fanSpeedPercentDesired > 50:
-        # allow enough time for the fan controller in the panda to recover from stalls
-        if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 15.0:
-          self.events.add(EventName.fanMalfunction)
-      else:
-        self.last_functional_fan_frame = self.sm.frame
-
-    # Handle calibration status
-    cal_status = self.sm['liveCalibration'].calStatus
-    if cal_status != log.LiveCalibrationData.Status.calibrated:
-      if cal_status == log.LiveCalibrationData.Status.uncalibrated:
-        self.events.add(EventName.calibrationIncomplete)
-      elif cal_status == log.LiveCalibrationData.Status.recalibrating:
-        if not self.recalibrating_seen:
-          set_offroad_alert("Offroad_Recalibration", True)
-        self.recalibrating_seen = True
-        self.events.add(EventName.calibrationRecalibrating)
-      else:
-        self.events.add(EventName.calibrationInvalid)
-
-    # Lane departure warning
-    if self.is_ldw_enabled and self.sm.valid['driverAssistance']:
-      if self.sm['driverAssistance'].leftLaneDeparture or self.sm['driverAssistance'].rightLaneDeparture:
-        self.events.add(EventName.ldw)
-
-    # Handle lane change
-    if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
-      direction = self.sm['modelV2'].meta.laneChangeDirection
-      if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
-         (CS.rightBlindspot and direction == LaneChangeDirection.right):
-        self.events.add(EventName.laneChangeBlocked)
-      else:
-        if direction == LaneChangeDirection.left:
-          self.events.add(EventName.preLaneChangeLeft)
-        else:
-          self.events.add(EventName.preLaneChangeRight)
-    elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting,
-                                                    LaneChangeState.laneChangeFinishing):
-      self.events.add(EventName.laneChange)
-
-    for i, pandaState in enumerate(self.sm['pandaStates']):
-      # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
-      if i < len(self.CP.safetyConfigs):
-        safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \
-                          pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \
-                          pandaState.alternativeExperience != self.CP.alternativeExperience
-      else:
-        safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES
-
-      # safety mismatch allows some time for pandad to set the safety mode and publish it back from panda
-      if (safety_mismatch and self.sm.frame*DT_CTRL > 10.) or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200:
-        self.events.add(EventName.controlsMismatch)
-
-      if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
-        self.events.add(EventName.relayMalfunction)
-
-    # Handle HW and system malfunctions
-    # Order is very intentional here. Be careful when modifying this.
-    # All events here should at least have NO_ENTRY and SOFT_DISABLE.
-    num_events = len(self.events)
-
-    not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning}
-    if self.sm.recv_frame['managerState'] and (not_running - IGNORE_PROCESSES):
-      self.events.add(EventName.processNotRunning)
-      if not_running != self.not_running_prev:
-        cloudlog.event("process_not_running", not_running=not_running, error=True)
-      self.not_running_prev = not_running
-    else:
-      if not SIMULATION and not self.rk.lagging:
-        if not self.sm.all_alive(self.camera_packets):
-          self.events.add(EventName.cameraMalfunction)
-        elif not self.sm.all_freq_ok(self.camera_packets):
-          self.events.add(EventName.cameraFrameRate)
-    if not REPLAY and self.rk.lagging:
-      self.events.add(EventName.controlsdLagging)
-    if not self.sm.valid['radarState']:
-      self.events.add(EventName.radarFault)
-    if not self.sm.valid['pandaStates']:
-      self.events.add(EventName.usbError)
-    if CS.canTimeout:
-      self.events.add(EventName.canBusMissing)
-    elif not CS.canValid:
-      self.events.add(EventName.canError)
-
-    # generic catch-all. ideally, a more specific event should be added above instead
-    has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE))
-    no_system_errors = (not has_disable_events) or (len(self.events) == num_events)
-    if not self.sm.all_checks() and no_system_errors:
-      if not self.sm.all_alive():
-        self.events.add(EventName.commIssue)
-      elif not self.sm.all_freq_ok():
-        self.events.add(EventName.commIssueAvgFreq)
-      else:
-        self.events.add(EventName.commIssue)
-
-      logs = {
-        'invalid': [s for s, valid in self.sm.valid.items() if not valid],
-        'not_alive': [s for s, alive in self.sm.alive.items() if not alive],
-        'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok],
-      }
-      if logs != self.logged_comm_issue:
-        cloudlog.event("commIssue", error=True, **logs)
-        self.logged_comm_issue = logs
-    else:
-      self.logged_comm_issue = None
-
-    if not (self.CP.notCar and self.joystick_mode):
-      if not self.sm['livePose'].posenetOK:
-        self.events.add(EventName.posenetInvalid)
-      if not self.sm['livePose'].inputsOK:
-        self.events.add(EventName.locationdTemporaryError)
-      if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY):
-        self.events.add(EventName.paramsdTemporaryError)
-
-    # conservative HW alert. if the data or frequency are off, locationd will throw an error
-    if any((self.sm.frame - self.sm.recv_frame[s])*DT_CTRL > 10. for s in self.sensor_packets):
-      self.events.add(EventName.sensorDataInvalid)
-
-    if not REPLAY:
-      # Check for mismatch between openpilot and car's PCM
-      cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
-      self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0
-      if self.cruise_mismatch_counter > int(6. / DT_CTRL):
-        self.events.add(EventName.cruiseMismatch)
-
-    # Send a "steering required alert" if saturation count has reached the limit
-    lac_log = self.lac_log_prev
-    if CS.steeringPressed:
-      self.last_steering_pressed_frame = self.sm.frame
-    recent_steer_pressed = (self.sm.frame - self.last_steering_pressed_frame)*DT_CTRL < 2.0
-    if self.lac_log_prev is not None and self.lac_log_prev.active and not recent_steer_pressed and not self.CP.notCar:
-      lac_log = self.lac_log_prev
-      if self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode:
-        undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2
-        turning = abs(lac_log.desiredLateralAccel) > 1.0
-        good_speed = CS.vEgo > 5
-        max_torque = abs(self.sm['carOutput'].actuatorsOutput.steer) > 0.99
-        if undershooting and turning and good_speed and max_torque:
-          self.events.add(EventName.steerSaturated)
-      elif lac_log.saturated:
-        # TODO probably should not use dpath_points but curvature
-        dpath_points = self.sm['modelV2'].position.y
-        if len(dpath_points):
-          # Check if we deviated from the path
-          # TODO use desired vs actual curvature
-          if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
-            steering_value = self.CC_prev.actuators.steeringAngleDeg
-          else:
-            steering_value = self.CC_prev.actuators.steer
-
-          left_deviation = steering_value > 0 and dpath_points[0] < -0.20
-          right_deviation = steering_value < 0 and dpath_points[0] > 0.20
-
-          if left_deviation or right_deviation:
-            self.events.add(EventName.steerSaturated)
-
-    # Check for FCW
-    stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25
-    model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking
-    planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled
-    if (planner_fcw or model_fcw) and not (self.CP.notCar and self.joystick_mode):
-      self.events.add(EventName.fcw)
-
-    for m in messaging.drain_sock(self.log_sock, wait_for_one=False):
-      try:
-        msg = m.androidLog.message
-        if any(err in msg for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED")):
-          csid = msg.split("CSID:")[-1].split(" ")[0]
-          evt = CSID_MAP.get(csid, None)
-          if evt is not None:
-            self.events.add(evt)
-      except UnicodeDecodeError:
-        pass
-
-    # TODO: fix simulator
-    if not SIMULATION or REPLAY:
-      # Not show in first 1.5 km to allow for driving out of garage. This event shows after 5 minutes
-      gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_CTRL < 2.0
-      if not gps_ok and self.sm['livePose'].inputsOK and (self.distance_traveled > 1500):
-        self.events.add(EventName.noGps)
-      if gps_ok:
-        self.distance_traveled = 0
-      self.distance_traveled += CS.vEgo * DT_CTRL
-
-      if self.sm['modelV2'].frameDropPerc > 20:
-        self.events.add(EventName.modeldLagging)
-
-    # decrement personality on distance button press
-    if self.CP.openpilotLongitudinalControl:
-      if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents):
-        self.personality = (self.personality - 1) % 3
-        self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
-        self.events.add(EventName.personalityChanged)
-
-  def data_sample(self):
-    """Receive data from sockets"""
-
-    car_state = messaging.recv_one(self.car_state_sock)
-    CS = car_state.carState if car_state else self.CS_prev
-
-    self.sm.update(0)
-
-    if not self.initialized:
-      all_valid = CS.canValid and self.sm.all_checks()
-      timed_out = self.sm.frame * DT_CTRL > 6.
-      if all_valid or timed_out or (SIMULATION and not REPLAY):
-        available_streams = VisionIpcClient.available_streams("camerad", block=False)
-        if VisionStreamType.VISION_STREAM_ROAD not in available_streams:
-          self.sm.ignore_alive.append('roadCameraState')
-        if VisionStreamType.VISION_STREAM_WIDE_ROAD not in available_streams:
-          self.sm.ignore_alive.append('wideRoadCameraState')
-
-        self.initialized = True
-        self.set_initial_state()
-
-        cloudlog.event(
-          "controlsd.initialized",
-          dt=self.sm.frame*DT_CTRL,
-          timeout=timed_out,
-          canValid=CS.canValid,
-          invalid=[s for s, valid in self.sm.valid.items() if not valid],
-          not_alive=[s for s, alive in self.sm.alive.items() if not alive],
-          not_freq_ok=[s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok],
-          error=True,
-        )
-
-    # 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 self.enabled:
-      self.mismatch_counter = 0
-
-    # All pandas not in silent mode must have controlsAllowed when openpilot is enabled
-    if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates']
-           if ps.safetyModel not in IGNORED_SAFETY_MODES):
-      self.mismatch_counter += 1
-
-    # calibrate the live pose and save it for later use
     if self.sm.updated["liveCalibration"]:
       self.pose_calibrator.feed_live_calib(self.sm['liveCalibration'])
     if self.sm.updated["livePose"]:
       device_pose = Pose.from_live_pose(self.sm['livePose'])
       self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose)
 
-    return CS
+  def state_control(self):
-
+    CS = self.sm['carState']
-  def state_control(self, CS):
-    """Given the state, this function returns a CarControl packet"""
 
     # Update VehicleModel
     lp = self.sm['liveParameters']
@@ -488,13 +84,12 @@ class Controls:
     model_v2 = self.sm['modelV2']
 
     CC = car.CarControl.new_message()
-    CC.enabled = self.enabled
+    CC.enabled = self.sm['selfdriveState'].enabled
 
     # Check which actuators can be enabled
     standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
-    CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
+    CC.latActive = self.sm['selfdriveState'].active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and not standstill
-                   (not standstill or self.joystick_mode)
+    CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and self.CP.openpilotLongitudinalControl
-    CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
 
     actuators = CC.actuators
     actuators.longControlState = self.LoC.long_control_state
@@ -504,17 +99,11 @@ class Controls:
       CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left
       CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right
 
-    if CS.leftBlinker or CS.rightBlinker:
-      self.last_blinker_frame = self.sm.frame
-
-    # State specific actions
-
     if not CC.latActive:
       self.LaC.reset()
     if not CC.longActive:
       self.LoC.reset()
 
-    if not self.joystick_mode:
     # accel PID loop
     pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
     actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)
@@ -525,30 +114,6 @@ class Controls:
     actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
                                                                             self.steer_limited, self.desired_curvature,
                                                                             self.calibrated_pose) # TODO what if not available
-    else:
-      lac_log = log.ControlsState.LateralDebugState.new_message()
-      if self.sm.recv_frame['testJoystick'] > 0:
-        # reset joystick if it hasn't been received in a while
-        should_reset_joystick = (self.sm.frame - self.sm.recv_frame['testJoystick'])*DT_CTRL > 0.2
-        if not should_reset_joystick:
-          joystick_axes = self.sm['testJoystick'].axes
-        else:
-          joystick_axes = [0.0, 0.0]
-
-        if CC.longActive:
-          actuators.accel = 4.0*clip(joystick_axes[0], -1, 1)
-
-        if CC.latActive:
-          max_curvature = JOYSTICK_MAX_LAT_ACCEL / max(CS.vEgo ** 2, 1)
-          max_angle = math.degrees(self.VM.get_steer_from_curvature(max_curvature, CS.vEgo, lp.roll))
-
-          steer = clip(joystick_axes[1], -1, 1)
-          actuators.steer, actuators.steeringAngleDeg, actuators.curvature = steer, steer * max_angle, steer * -max_curvature
-
-        lac_log.active = self.active
-        lac_log.steeringAngleDeg = CS.steeringAngleDeg
-        lac_log.output = actuators.steer
-        lac_log.saturated = abs(actuators.steer) >= 0.9
 
     # Ensure no NaNs/Infs
     for p in ACTUATOR_FIELDS:
@@ -562,53 +127,37 @@ class Controls:
 
     return CC, lac_log
 
-  def update_alerts(self, CS):
+  def publish(self, CC, lac_log):
-    clear_event_types = set()
+    CS = self.sm['carState']
-    if ET.WARNING not in self.state_machine.current_alert_types:
-      clear_event_types.add(ET.WARNING)
-    if self.enabled:
-      clear_event_types.add(ET.NO_ENTRY)
-
-    pers = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()}[self.personality]
-    alerts = self.events.create_alerts(self.state_machine.current_alert_types, [self.CP, CS, self.sm, self.is_metric,
-                                                                                self.state_machine.soft_disable_timer, pers])
-    self.AM.add_many(self.sm.frame, alerts)
-    self.AM.process_alerts(self.sm.frame, clear_event_types)
 
-  def publish_carControl(self, CS, CC, lac_log):
     # Orientation and angle rates can be useful for carcontroller
     # Only calibrated (car) frame is relevant for the carcontroller
     if self.calibrated_pose is not None:
       CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist()
       CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist()
 
-    CC.cruiseControl.override = self.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl
+    CC.cruiseControl.override = CC.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl
-    CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
+    CC.cruiseControl.cancel = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise)
-    if self.joystick_mode and self.sm.recv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
-      CC.cruiseControl.cancel = True
 
     speeds = self.sm['longitudinalPlan'].speeds
     if len(speeds):
-      CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1
+      CC.cruiseControl.resume = CC.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1
 
     hudControl = CC.hudControl
     hudControl.setSpeed = float(CS.vCruiseCluster * CV.KPH_TO_MS)
-    hudControl.speedVisible = self.enabled
+    hudControl.speedVisible = CC.enabled
-    hudControl.lanesVisible = self.enabled
+    hudControl.lanesVisible = CC.enabled
     hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
-    hudControl.leadDistanceBars = self.personality + 1
+    hudControl.leadDistanceBars = self.sm['selfdriveState'].personality.raw + 1
+    hudControl.visualAlert = self.sm['selfdriveState'].alertHudVisual
 
     hudControl.rightLaneVisible = True
     hudControl.leftLaneVisible = True
-
+    if self.sm.valid['driverAssistance']:
-    if self.is_ldw_enabled and self.sm.valid['driverAssistance']:
       hudControl.leftLaneDepart = self.sm['driverAssistance'].leftLaneDeparture
       hudControl.rightLaneDepart = self.sm['driverAssistance'].rightLaneDeparture
 
-    if self.AM.current_alert:
+    if self.sm['selfdriveState'].active:
-      hudControl.visualAlert = self.AM.current_alert.visual_alert
-
-    if not self.CP.passive and self.initialized:
       CO = self.sm['carOutput']
       if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
         self.steer_limited = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \
@@ -616,6 +165,8 @@ class Controls:
       else:
         self.steer_limited = abs(CC.actuators.steer - CO.actuatorsOutput.steer) > 1e-2
 
+    # TODO: both controlsState and carControl valids should be set by
+    #       sm.all_checks(), but this creates a circular dependency
 
     # controlsState
     dat = messaging.new_message('controlsState')
@@ -633,14 +184,11 @@ class Controls:
     cs.upAccelCmd = float(self.LoC.pid.p)
     cs.uiAccelCmd = float(self.LoC.pid.i)
     cs.ufAccelCmd = float(self.LoC.pid.f)
-    cs.cumLagMs = -self.rk.remaining * 1000.
     cs.forceDecel = bool((self.sm['driverMonitoringState'].awarenessStatus < 0.) or
-                         (self.state_machine.state == State.softDisabling))
+                         (self.sm['selfdriveState'].state == State.softDisabling))
 
     lat_tuning = self.CP.lateralTuning.which()
-    if self.joystick_mode:
+    if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
-      cs.lateralControlState.debugState = lac_log
-    elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
       cs.lateralControlState.angleState = lac_log
     elif lat_tuning == 'pid':
       cs.lateralControlState.pidState = lac_log
@@ -655,84 +203,18 @@ class Controls:
     cc_send.carControl = CC
     self.pm.send('carControl', cc_send)
 
-  def publish_selfdriveState(self, CS):
+  def run(self):
-    # selfdriveState
+    rk = Ratekeeper(100, print_delay_threshold=None)
-    ss_msg = messaging.new_message('selfdriveState')
-    ss_msg.valid = CS.canValid
-    ss = ss_msg.selfdriveState
-    if self.AM.current_alert:
-      ss.alertText1 = self.AM.current_alert.alert_text_1
-      ss.alertText2 = self.AM.current_alert.alert_text_2
-      ss.alertSize = self.AM.current_alert.alert_size
-      ss.alertStatus = self.AM.current_alert.alert_status
-      ss.alertType = self.AM.current_alert.alert_type
-      ss.alertSound = self.AM.current_alert.audible_alert
-    ss.enabled = self.enabled
-    ss.active = self.active
-    ss.state = self.state_machine.state
-    ss.engageable = not self.events.contains(ET.NO_ENTRY)
-    ss.experimentalMode = self.experimental_mode
-    ss.personality = self.personality
-    self.pm.send('selfdriveState', ss_msg)
-
-    # onroadEvents - logged every second or on change
-    if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev):
-      ce_send = messaging.new_message('onroadEvents', len(self.events))
-      ce_send.valid = True
-      ce_send.onroadEvents = self.events.to_msg()
-      self.pm.send('onroadEvents', ce_send)
-    self.events_prev = self.events.names.copy()
-
-  def step(self):
-    CS = self.data_sample()
-    self.update_events(CS)
-    if not self.CP.passive and self.initialized:
-      self.enabled, self.active = self.state_machine.update(self.events)
-    self.update_alerts(CS)
-
-    # Compute actuators (runs PID loops and lateral MPC)
-    CC, lac_log = self.state_control(CS)
-
-    # Publish data
-    self.publish_carControl(CS, CC, lac_log)
-    self.publish_selfdriveState(CS)
-
-    self.CS_prev = CS
-    self.CC_prev = CC
-    self.lac_log_prev = lac_log
-
-  def read_personality_param(self):
-    try:
-      return int(self.params.get('LongitudinalPersonality'))
-    except (ValueError, TypeError):
-      return log.LongitudinalPersonality.standard
-
-  def params_thread(self, evt):
-    while not evt.is_set():
-      self.is_metric = self.params.get_bool("IsMetric")
-      self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
-      self.personality = self.read_personality_param()
-      if self.CP.notCar:
-        self.joystick_mode = self.params.get_bool("JoystickDebugMode")
-      time.sleep(0.1)
-
-  def controlsd_thread(self):
-    e = threading.Event()
-    t = threading.Thread(target=self.params_thread, args=(e, ))
-    try:
-      t.start()
     while True:
-        self.step()
+      self.update()
-        self.rk.monitor_time()
+      CC, lac_log = self.state_control()
-    finally:
+      self.publish(CC, lac_log)
-      e.set()
+      rk.keep_time()
-      t.join()
-
 
 def main():
   config_realtime_process(4, Priority.CTRL_HIGH)
   controls = Controls()
-  controls.controlsd_thread()
+  controls.run()
 
 
 if __name__ == "__main__":

selfdrive/controls/lib/drive_helpers.py

@@ -1,9 +1,6 @@
-from cereal import car, log
+from cereal import log
 from openpilot.common.numpy_fast import clip
 from openpilot.common.realtime import DT_CTRL
-from openpilot.system.version import get_build_metadata
-
-EventName = car.OnroadEvent.EventName
 
 MIN_SPEED = 1.0
 CONTROL_N = 17
@@ -29,20 +26,3 @@ def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float:
     vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
     return float(vel_err)
   return 0.0
-
-
-def get_startup_event(car_recognized, controller_available, fw_seen):
-  build_metadata = get_build_metadata()
-  if build_metadata.openpilot.comma_remote and build_metadata.tested_channel:
-    event = EventName.startup
-  else:
-    event = EventName.startupMaster
-
-  if not car_recognized:
-    if fw_seen:
-      event = EventName.startupNoCar
-    else:
-      event = EventName.startupNoFw
-  elif car_recognized and not controller_available:
-    event = EventName.startupNoControl
-  return event

selfdrive/controls/tests/test_startup.py

@@ -1,121 +0,0 @@
-import os
-from parameterized import parameterized
-
-from cereal import log, car
-import cereal.messaging as messaging
-from opendbc.car.fingerprints import _FINGERPRINTS
-from opendbc.car.toyota.values import CAR as TOYOTA
-from opendbc.car.mazda.values import CAR as MAZDA
-from openpilot.common.params import Params
-from openpilot.selfdrive.pandad.pandad_api_impl import can_list_to_can_capnp
-from openpilot.selfdrive.controls.lib.events import EVENT_NAME
-from openpilot.system.manager.process_config import managed_processes
-
-EventName = car.OnroadEvent.EventName
-Ecu = car.CarParams.Ecu
-
-COROLLA_FW_VERSIONS = [
-  (Ecu.engine, 0x7e0, None, b'\x0230ZC2000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00'),
-  (Ecu.abs, 0x7b0, None, b'F152602190\x00\x00\x00\x00\x00\x00'),
-  (Ecu.eps, 0x7a1, None, b'8965B02181\x00\x00\x00\x00\x00\x00'),
-  (Ecu.fwdRadar, 0x750, 0xf, b'8821F4702100\x00\x00\x00\x00'),
-  (Ecu.fwdCamera, 0x750, 0x6d, b'8646F0201101\x00\x00\x00\x00'),
-  (Ecu.dsu, 0x791, None, b'881510201100\x00\x00\x00\x00'),
-]
-COROLLA_FW_VERSIONS_FUZZY = COROLLA_FW_VERSIONS[:-1] + [(Ecu.dsu, 0x791, None, b'xxxxxx')]
-COROLLA_FW_VERSIONS_NO_DSU = COROLLA_FW_VERSIONS[:-1]
-
-CX5_FW_VERSIONS = [
-  (Ecu.engine, 0x7e0, None, b'PYNF-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
-  (Ecu.abs, 0x760, None, b'K123-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
-  (Ecu.eps, 0x730, None, b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
-  (Ecu.fwdRadar, 0x764, None, b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
-  (Ecu.fwdCamera, 0x706, None, b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
-  (Ecu.transmission, 0x7e1, None, b'PYNC-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
-]
-
-
-@parameterized.expand([
-  # TODO: test EventName.startup for release branches
-
-  # officially supported car
-  (EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS, "toyota"),
-  (EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS, "toyota"),
-
-  # dashcamOnly car
-  (EventName.startupNoControl, MAZDA.MAZDA_CX5, CX5_FW_VERSIONS, "mazda"),
-  (EventName.startupNoControl, MAZDA.MAZDA_CX5, CX5_FW_VERSIONS, "mazda"),
-
-  # unrecognized car with no fw
-  (EventName.startupNoFw, None, None, ""),
-  (EventName.startupNoFw, None, None, ""),
-
-  # unrecognized car
-  (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"),
-  (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"),
-
-  # fuzzy match
-  (EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"),
-  (EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"),
-])
-def test_startup_alert(expected_event, car_model, fw_versions, brand):
-  controls_sock = messaging.sub_sock("selfdriveState")
-  pm = messaging.PubMaster(['can', 'pandaStates'])
-
-  params = Params()
-  params.put_bool("OpenpilotEnabledToggle", True)
-
-  # Build capnn version of FW array
-  if fw_versions is not None:
-    car_fw = []
-    cp = car.CarParams.new_message()
-    for ecu, addr, subaddress, version in fw_versions:
-      f = car.CarParams.CarFw.new_message()
-      f.ecu = ecu
-      f.address = addr
-      f.fwVersion = version
-      f.brand = brand
-
-      if subaddress is not None:
-        f.subAddress = subaddress
-
-      car_fw.append(f)
-    cp.carVin = "1" * 17
-    cp.carFw = car_fw
-    params.put("CarParamsCache", cp.to_bytes())
-  else:
-    os.environ['SKIP_FW_QUERY'] = '1'
-
-  managed_processes['controlsd'].start()
-  managed_processes['card'].start()
-
-  assert pm.wait_for_readers_to_update('can', 5)
-  pm.send('can', can_list_to_can_capnp([[0, b"", 0]]))
-
-  assert pm.wait_for_readers_to_update('pandaStates', 5)
-  msg = messaging.new_message('pandaStates', 1)
-  msg.pandaStates[0].pandaType = log.PandaState.PandaType.uno
-  pm.send('pandaStates', msg)
-
-  # fingerprint
-  if (car_model is None) or (fw_versions is not None):
-    finger = {addr: 1 for addr in range(1, 100)}
-  else:
-    finger = _FINGERPRINTS[car_model][0]
-
-  msgs = [[addr, b'\x00'*length, 0] for addr, length in finger.items()]
-  for _ in range(1000):
-    # card waits for pandad to echo back that it has changed the multiplexing mode
-    if not params.get_bool("ObdMultiplexingChanged"):
-      params.put_bool("ObdMultiplexingChanged", True)
-
-    pm.send('can', can_list_to_can_capnp(msgs))
-    assert pm.wait_for_readers_to_update('can', 5, dt=0.001), f"step: {_}"
-
-    ctrls = messaging.drain_sock(controls_sock)
-    if len(ctrls):
-      event_name = ctrls[0].selfdriveState.alertType.split("/")[0]
-      assert EVENT_NAME[expected_event] == event_name, f"expected {EVENT_NAME[expected_event]} for '{car_model}', got {event_name}"
-      break
-  else:
-    raise Exception(f"failed to fingerprint {car_model}")

selfdrive/debug/cycle_alerts.py

@@ -6,8 +6,8 @@ from cereal import car, log
 import cereal.messaging as messaging
 from opendbc.car.honda.interface import CarInterface
 from openpilot.common.realtime import DT_CTRL
-from openpilot.selfdrive.controls.lib.events import ET, Events
+from openpilot.selfdrive.selfdrived.events import ET, Events
-from openpilot.selfdrive.controls.lib.alertmanager import AlertManager
+from openpilot.selfdrive.selfdrived.alertmanager import AlertManager
 from openpilot.system.manager.process_config import managed_processes
 
 EventName = car.OnroadEvent.EventName

selfdrive/monitoring/helpers.py

@@ -2,7 +2,7 @@ from math import atan2
 
 from cereal import car
 import cereal.messaging as messaging
-from openpilot.selfdrive.controls.lib.events import Events
+from openpilot.selfdrive.selfdrived.events import Events
 from openpilot.common.numpy_fast import interp
 from openpilot.common.realtime import DT_DMON
 from openpilot.common.filter_simple import FirstOrderFilter

selfdrive/selfdrived/alertmanager.py

@@ -6,10 +6,10 @@ from dataclasses import dataclass
 
 from openpilot.common.basedir import BASEDIR
 from openpilot.common.params import Params
-from openpilot.selfdrive.controls.lib.events import Alert
+from openpilot.selfdrive.selfdrived.events import Alert
 
 
-with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f:
+with open(os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json")) as f:
   OFFROAD_ALERTS = json.load(f)
 
 

selfdrive/selfdrived/events.py

@@ -318,10 +318,11 @@ def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM
 
 
 def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
-  axes = sm['testJoystick'].axes
+  # TODO: add some info back
-  gb, steer = list(axes)[:2] if len(axes) else (0., 0.)
+  #axes = sm['testJoystick'].axes
-  vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%"
+  #gb, steer = list(axes)[:2] if len(axes) else (0., 0.)
-  return NormalPermanentAlert("Joystick Mode", vals)
+  #vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%"
+  return NormalPermanentAlert("Joystick Mode", "")
 
 def personality_changed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
   personality = str(personality).title()
@@ -342,7 +343,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.PERMANENT: NormalPermanentAlert("Joystick Mode"),
   },
 
-  EventName.controlsInitializing: {
+  EventName.selfdriveInitializing: {
     ET.NO_ENTRY: NoEntryAlert("System Initializing"),
   },
 
@@ -773,9 +774,9 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.NO_ENTRY: NoEntryAlert("Low Communication Rate Between Processes"),
   },
 
-  EventName.controlsdLagging: {
+  EventName.selfdrivedLagging: {
-    ET.SOFT_DISABLE: soft_disable_alert("Controls Lagging"),
+    ET.SOFT_DISABLE: soft_disable_alert("System Lagging"),
-    ET.NO_ENTRY: NoEntryAlert("Controls Process Lagging: Reboot Your Device"),
+    ET.NO_ENTRY: NoEntryAlert("Selfdrive Process Lagging: Reboot Your Device"),
   },
 
   # Thrown when manager detects a service exited unexpectedly while driving

selfdrive/selfdrived/selfdrived.py

@@ -0,0 +1,518 @@
+#!/usr/bin/env python3
+import os
+import time
+import threading
+
+import cereal.messaging as messaging
+
+from cereal import car, log
+from msgq.visionipc import VisionIpcClient, VisionStreamType
+
+
+from openpilot.common.git import get_short_branch
+from openpilot.common.params import Params
+from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL
+from openpilot.common.swaglog import cloudlog
+from openpilot.common.gps import get_gps_location_service
+
+from openpilot.selfdrive.selfdrived.events import Events, ET
+from openpilot.selfdrive.selfdrived.state import StateMachine
+from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert
+
+from openpilot.system.hardware import HARDWARE
+from openpilot.system.version import get_build_metadata
+
+REPLAY = "REPLAY" in os.environ
+SIMULATION = "SIMULATION" in os.environ
+TESTING_CLOSET = "TESTING_CLOSET" in os.environ
+IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"}
+
+ThermalStatus = log.DeviceState.ThermalStatus
+State = log.SelfdriveState.OpenpilotState
+PandaType = log.PandaState.PandaType
+LaneChangeState = log.LaneChangeState
+LaneChangeDirection = log.LaneChangeDirection
+EventName = car.OnroadEvent.EventName
+ButtonType = car.CarState.ButtonEvent.Type
+SafetyModel = car.CarParams.SafetyModel
+
+IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
+CSID_MAP = {"1": EventName.roadCameraError, "2": EventName.wideRoadCameraError, "0": EventName.driverCameraError}
+
+def get_startup_event(car_recognized, controller_available, fw_seen):
+  build_metadata = get_build_metadata()
+  if build_metadata.openpilot.comma_remote and build_metadata.tested_channel:
+    event = EventName.startup
+  else:
+    event = EventName.startupMaster
+
+  if not car_recognized:
+    if fw_seen:
+      event = EventName.startupNoCar
+    else:
+      event = EventName.startupNoFw
+  elif car_recognized and not controller_available:
+    event = EventName.startupNoControl
+  return event
+
+
+class SelfdriveD:
+  def __init__(self):
+    self.params = Params()
+
+    # Ensure the current branch is cached, otherwise the first cycle lags
+    self.branch = get_short_branch()
+
+    cloudlog.info("selfdrived is waiting for CarParams")
+    self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams)
+    cloudlog.info("selfdrived got CarParams")
+
+    # Setup sockets
+    self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'])
+
+    self.gps_location_service = get_gps_location_service(self.params)
+    self.gps_packets = [self.gps_location_service]
+    self.sensor_packets = ["accelerometer", "gyroscope"]
+    self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"]
+
+    self.log_sock = messaging.sub_sock('androidLog')
+
+    # TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches
+    self.car_state_sock = messaging.sub_sock('carState', timeout=20)
+
+    ignore = self.sensor_packets + self.gps_packets
+    if SIMULATION:
+      ignore += ['driverCameraState', 'managerState']
+    if REPLAY:
+      # no vipc in replay will make them ignored anyways
+      ignore += ['roadCameraState', 'wideRoadCameraState']
+    self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
+                                   'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose',
+                                   'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
+                                   'controlsState', 'carControl', 'driverAssistance'] + \
+                                   self.camera_packets + self.sensor_packets + self.gps_packets,
+                                  ignore_alive=ignore, ignore_avg_freq=ignore+['radarState',],
+                                  frequency=int(1/DT_CTRL))
+
+    # read params
+    self.is_metric = self.params.get_bool("IsMetric")
+    self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
+
+    # detect sound card presence and ensure successful init
+    sounds_available = HARDWARE.get_sound_card_online()
+
+    car_recognized = self.CP.carName != 'mock'
+
+    # cleanup old params
+    if not self.CP.experimentalLongitudinalAvailable:
+      self.params.remove("ExperimentalLongitudinalEnabled")
+    if not self.CP.openpilotLongitudinalControl:
+      self.params.remove("ExperimentalMode")
+
+    self.CS_prev = car.CarState.new_message()
+    self.AM = AlertManager()
+    self.events = Events()
+
+    self.initialized = False
+    self.enabled = False
+    self.active = False
+    self.mismatch_counter = 0
+    self.cruise_mismatch_counter = 0
+    self.last_steering_pressed_frame = 0
+    self.distance_traveled = 0
+    self.last_functional_fan_frame = 0
+    self.events_prev = []
+    self.logged_comm_issue = None
+    self.not_running_prev = None
+    self.experimental_mode = False
+    self.personality = self.read_personality_param()
+    self.recalibrating_seen = False
+    self.state_machine = StateMachine()
+
+    self.startup_event = get_startup_event(car_recognized, not self.CP.passive, len(self.CP.carFw) > 0)
+
+    if not sounds_available:
+      self.events.add(EventName.soundsUnavailable, static=True)
+    if not car_recognized:
+      self.events.add(EventName.carUnrecognized, static=True)
+      if len(self.CP.carFw) > 0:
+        set_offroad_alert("Offroad_CarUnrecognized", True)
+      else:
+        set_offroad_alert("Offroad_NoFirmware", True)
+    elif self.CP.passive:
+      self.events.add(EventName.dashcamMode, static=True)
+
+    # selfdrived is driven by carState, expected at 100Hz
+    self.rk = Ratekeeper(100, print_delay_threshold=None)
+
+  def set_initial_state(self):
+    if REPLAY and any(ps.controlsAllowed for ps in self.sm['pandaStates']):
+        self.state_machine.state = State.enabled
+
+  def update_events(self, CS):
+    """Compute onroadEvents from carState"""
+
+    self.events.clear()
+
+    if self.sm['controlsState'].lateralControlState.which() == 'debugState':
+      self.events.add(EventName.joystickDebug)
+      self.startup_event = None
+
+    # Add startup event
+    if self.startup_event is not None:
+      self.events.add(self.startup_event)
+      self.startup_event = None
+
+    # Don't add any more events if not initialized
+    if not self.initialized:
+      self.events.add(EventName.selfdriveInitializing)
+      return
+
+    # no more events while in dashcam mode
+    if self.CP.passive:
+      return
+
+    # Block resume if cruise never previously enabled
+    resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents)
+    if not self.CP.pcmCruise and CS.vCruise > 250 and resume_pressed:
+      self.events.add(EventName.resumeBlocked)
+
+    if not self.CP.notCar:
+      self.events.add_from_msg(self.sm['driverMonitoringState'].events)
+
+    # Add car events, ignore if CAN isn't valid
+    if CS.canValid:
+      self.events.add_from_msg(CS.events)
+
+    # Create events for temperature, disk space, and memory
+    if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
+      self.events.add(EventName.overheat)
+    if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
+      self.events.add(EventName.outOfSpace)
+    if self.sm['deviceState'].memoryUsagePercent > 90 and not SIMULATION:
+      self.events.add(EventName.lowMemory)
+
+    # Alert if fan isn't spinning for 5 seconds
+    if self.sm['peripheralState'].pandaType != log.PandaState.PandaType.unknown:
+      if self.sm['peripheralState'].fanSpeedRpm < 500 and self.sm['deviceState'].fanSpeedPercentDesired > 50:
+        # allow enough time for the fan controller in the panda to recover from stalls
+        if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 15.0:
+          self.events.add(EventName.fanMalfunction)
+      else:
+        self.last_functional_fan_frame = self.sm.frame
+
+    # Handle calibration status
+    cal_status = self.sm['liveCalibration'].calStatus
+    if cal_status != log.LiveCalibrationData.Status.calibrated:
+      if cal_status == log.LiveCalibrationData.Status.uncalibrated:
+        self.events.add(EventName.calibrationIncomplete)
+      elif cal_status == log.LiveCalibrationData.Status.recalibrating:
+        if not self.recalibrating_seen:
+          set_offroad_alert("Offroad_Recalibration", True)
+        self.recalibrating_seen = True
+        self.events.add(EventName.calibrationRecalibrating)
+      else:
+        self.events.add(EventName.calibrationInvalid)
+
+    # Lane departure warning
+    if self.is_ldw_enabled and self.sm.valid['driverAssistance']:
+      if self.sm['driverAssistance'].leftLaneDeparture or self.sm['driverAssistance'].rightLaneDeparture:
+        self.events.add(EventName.ldw)
+
+    # Handle lane change
+    if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
+      direction = self.sm['modelV2'].meta.laneChangeDirection
+      if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
+         (CS.rightBlindspot and direction == LaneChangeDirection.right):
+        self.events.add(EventName.laneChangeBlocked)
+      else:
+        if direction == LaneChangeDirection.left:
+          self.events.add(EventName.preLaneChangeLeft)
+        else:
+          self.events.add(EventName.preLaneChangeRight)
+    elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting,
+                                                    LaneChangeState.laneChangeFinishing):
+      self.events.add(EventName.laneChange)
+
+    for i, pandaState in enumerate(self.sm['pandaStates']):
+      # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
+      if i < len(self.CP.safetyConfigs):
+        safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \
+                          pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \
+                          pandaState.alternativeExperience != self.CP.alternativeExperience
+      else:
+        safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES
+
+      # safety mismatch allows some time for pandad to set the safety mode and publish it back from panda
+      if (safety_mismatch and self.sm.frame*DT_CTRL > 10.) or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200:
+        self.events.add(EventName.controlsMismatch)
+
+      if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
+        self.events.add(EventName.relayMalfunction)
+
+    # Handle HW and system malfunctions
+    # Order is very intentional here. Be careful when modifying this.
+    # All events here should at least have NO_ENTRY and SOFT_DISABLE.
+    num_events = len(self.events)
+
+    not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning}
+    if self.sm.recv_frame['managerState'] and (not_running - IGNORE_PROCESSES):
+      self.events.add(EventName.processNotRunning)
+      if not_running != self.not_running_prev:
+        cloudlog.event("process_not_running", not_running=not_running, error=True)
+      self.not_running_prev = not_running
+    else:
+      if not SIMULATION and not self.rk.lagging:
+        if not self.sm.all_alive(self.camera_packets):
+          self.events.add(EventName.cameraMalfunction)
+        elif not self.sm.all_freq_ok(self.camera_packets):
+          self.events.add(EventName.cameraFrameRate)
+    if not REPLAY and self.rk.lagging:
+      self.events.add(EventName.selfdrivedLagging)
+    if len(self.sm['radarState'].radarErrors) or ((not self.rk.lagging or REPLAY) and not self.sm.all_checks(['radarState'])):
+      self.events.add(EventName.radarFault)
+    if not self.sm.valid['pandaStates']:
+      self.events.add(EventName.usbError)
+    if CS.canTimeout:
+      self.events.add(EventName.canBusMissing)
+    elif not CS.canValid:
+      self.events.add(EventName.canError)
+
+    # generic catch-all. ideally, a more specific event should be added above instead
+    has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE))
+    no_system_errors = (not has_disable_events) or (len(self.events) == num_events)
+    if not self.sm.all_checks() and no_system_errors:
+      if not self.sm.all_alive():
+        self.events.add(EventName.commIssue)
+      elif not self.sm.all_freq_ok():
+        self.events.add(EventName.commIssueAvgFreq)
+      else:
+        self.events.add(EventName.commIssue)
+
+      logs = {
+        'invalid': [s for s, valid in self.sm.valid.items() if not valid],
+        'not_alive': [s for s, alive in self.sm.alive.items() if not alive],
+        'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok],
+      }
+      if logs != self.logged_comm_issue:
+        cloudlog.event("commIssue", error=True, **logs)
+        self.logged_comm_issue = logs
+    else:
+      self.logged_comm_issue = None
+
+    if not self.CP.notCar:
+      if not self.sm['livePose'].posenetOK:
+        self.events.add(EventName.posenetInvalid)
+      if not self.sm['livePose'].inputsOK:
+        self.events.add(EventName.locationdTemporaryError)
+      if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY):
+        self.events.add(EventName.paramsdTemporaryError)
+
+    # conservative HW alert. if the data or frequency are off, locationd will throw an error
+    if any((self.sm.frame - self.sm.recv_frame[s])*DT_CTRL > 10. for s in self.sensor_packets):
+      self.events.add(EventName.sensorDataInvalid)
+
+    if not REPLAY:
+      # Check for mismatch between openpilot and car's PCM
+      cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
+      self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0
+      if self.cruise_mismatch_counter > int(6. / DT_CTRL):
+        self.events.add(EventName.cruiseMismatch)
+
+    # Send a "steering required alert" if saturation count has reached the limit
+    if CS.steeringPressed:
+      self.last_steering_pressed_frame = self.sm.frame
+    recent_steer_pressed = (self.sm.frame - self.last_steering_pressed_frame)*DT_CTRL < 2.0
+    lac = getattr(self.sm['controlsState'].lateralControlState, self.sm['controlsState'].lateralControlState.which())
+    if lac.active and not recent_steer_pressed and not self.CP.notCar:
+      if self.CP.lateralTuning.which() == 'torque':
+        undershooting = abs(lac.desiredLateralAccel) / abs(1e-3 + lac.actualLateralAccel) > 1.2
+        turning = abs(lac.desiredLateralAccel) > 1.0
+        good_speed = CS.vEgo > 5
+        max_torque = abs(self.sm['carOutput'].actuatorsOutput.steer) > 0.99
+        if undershooting and turning and good_speed and max_torque:
+          self.events.add(EventName.steerSaturated)
+      elif lac.saturated:
+        # TODO probably should not use dpath_points but curvature
+        dpath_points = self.sm['modelV2'].position.y
+        if len(dpath_points):
+          # Check if we deviated from the path
+          # TODO use desired vs actual curvature
+          if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
+            steering_value = self.sm['carControl'].actuators.steeringAngleDeg
+          else:
+            steering_value = self.sm['carControl'].actuators.steer
+
+          left_deviation = steering_value > 0 and dpath_points[0] < -0.20
+          right_deviation = steering_value < 0 and dpath_points[0] > 0.20
+          if left_deviation or right_deviation:
+            self.events.add(EventName.steerSaturated)
+
+    # Check for FCW
+    stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25
+    model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking
+    planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled
+    if (planner_fcw or model_fcw) and not self.CP.notCar:
+      self.events.add(EventName.fcw)
+
+    # Camera errors from the kernel
+    for m in messaging.drain_sock(self.log_sock, wait_for_one=False):
+      try:
+        msg = m.androidLog.message
+        if any(err in msg for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED")):
+          csid = msg.split("CSID:")[-1].split(" ")[0]
+          evt = CSID_MAP.get(csid, None)
+          if evt is not None:
+            self.events.add(evt)
+      except UnicodeDecodeError:
+        pass
+
+    # TODO: fix simulator
+    if not SIMULATION or REPLAY:
+      # Not show in first 1.5 km to allow for driving out of garage. This event shows after 5 minutes
+      gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_CTRL < 2.0
+      if not gps_ok and self.sm['livePose'].inputsOK and (self.distance_traveled > 1500):
+        self.events.add(EventName.noGps)
+      if gps_ok:
+        self.distance_traveled = 0
+      self.distance_traveled += CS.vEgo * DT_CTRL
+
+      if self.sm['modelV2'].frameDropPerc > 20:
+        self.events.add(EventName.modeldLagging)
+
+    # decrement personality on distance button press
+    if self.CP.openpilotLongitudinalControl:
+      if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents):
+        self.personality = (self.personality - 1) % 3
+        self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
+        self.events.add(EventName.personalityChanged)
+
+  def data_sample(self):
+    car_state = messaging.recv_one(self.car_state_sock)
+    CS = car_state.carState if car_state else self.CS_prev
+
+    self.sm.update(0)
+
+    if not self.initialized:
+      all_valid = CS.canValid and self.sm.all_checks()
+      timed_out = self.sm.frame * DT_CTRL > 6.
+      if all_valid or timed_out or (SIMULATION and not REPLAY):
+        available_streams = VisionIpcClient.available_streams("camerad", block=False)
+        if VisionStreamType.VISION_STREAM_ROAD not in available_streams:
+          self.sm.ignore_alive.append('roadCameraState')
+        if VisionStreamType.VISION_STREAM_WIDE_ROAD not in available_streams:
+          self.sm.ignore_alive.append('wideRoadCameraState')
+
+        self.initialized = True
+        self.set_initial_state()
+
+        cloudlog.event(
+          "selfdrived.initialized",
+          dt=self.sm.frame*DT_CTRL,
+          timeout=timed_out,
+          canValid=CS.canValid,
+          invalid=[s for s, valid in self.sm.valid.items() if not valid],
+          not_alive=[s for s, alive in self.sm.alive.items() if not alive],
+          not_freq_ok=[s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok],
+          error=True,
+        )
+
+    # When the panda and selfdrived 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 self.enabled:
+      self.mismatch_counter = 0
+
+    # All pandas not in silent mode must have controlsAllowed when openpilot is enabled
+    if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates']
+           if ps.safetyModel not in IGNORED_SAFETY_MODES):
+      self.mismatch_counter += 1
+
+    return CS
+
+  def update_alerts(self, CS):
+    clear_event_types = set()
+    if ET.WARNING not in self.state_machine.current_alert_types:
+      clear_event_types.add(ET.WARNING)
+    if self.enabled:
+      clear_event_types.add(ET.NO_ENTRY)
+
+    pers = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()}[self.personality]
+    alerts = self.events.create_alerts(self.state_machine.current_alert_types, [self.CP, CS, self.sm, self.is_metric,
+                                                                                self.state_machine.soft_disable_timer, pers])
+    self.AM.add_many(self.sm.frame, alerts)
+    self.AM.process_alerts(self.sm.frame, clear_event_types)
+
+  def publish_selfdriveState(self, CS):
+    # selfdriveState
+    ss_msg = messaging.new_message('selfdriveState')
+    ss_msg.valid = True
+    ss = ss_msg.selfdriveState
+    if self.AM.current_alert:
+      ss.alertText1 = self.AM.current_alert.alert_text_1
+      ss.alertText2 = self.AM.current_alert.alert_text_2
+      ss.alertSize = self.AM.current_alert.alert_size
+      ss.alertStatus = self.AM.current_alert.alert_status
+      ss.alertType = self.AM.current_alert.alert_type
+      ss.alertSound = self.AM.current_alert.audible_alert
+    ss.enabled = self.enabled
+    ss.active = self.active
+    ss.state = self.state_machine.state
+    ss.engageable = not self.events.contains(ET.NO_ENTRY)
+    ss.experimentalMode = self.experimental_mode
+    ss.personality = self.personality
+    self.pm.send('selfdriveState', ss_msg)
+
+    # onroadEvents - logged every second or on change
+    if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev):
+      ce_send = messaging.new_message('onroadEvents', len(self.events))
+      ce_send.valid = True
+      ce_send.onroadEvents = self.events.to_msg()
+      self.pm.send('onroadEvents', ce_send)
+    self.events_prev = self.events.names.copy()
+
+  def step(self):
+    CS = self.data_sample()
+    self.update_events(CS)
+    if not self.CP.passive and self.initialized:
+      self.enabled, self.active = self.state_machine.update(self.events)
+    self.update_alerts(CS)
+
+    self.publish_selfdriveState(CS)
+
+    self.CS_prev = CS
+
+  def read_personality_param(self):
+    try:
+      return int(self.params.get('LongitudinalPersonality'))
+    except (ValueError, TypeError):
+      return log.LongitudinalPersonality.standard
+
+  def params_thread(self, evt):
+    while not evt.is_set():
+      self.is_metric = self.params.get_bool("IsMetric")
+      self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
+      self.personality = self.read_personality_param()
+      time.sleep(0.1)
+
+  def run(self):
+    e = threading.Event()
+    t = threading.Thread(target=self.params_thread, args=(e, ))
+    try:
+      t.start()
+      while True:
+        self.step()
+        self.rk.monitor_time()
+    finally:
+      e.set()
+      t.join()
+
+
+def main():
+  config_realtime_process(4, Priority.CTRL_HIGH)
+  s = SelfdriveD()
+  s.run()
+
+if __name__ == "__main__":
+  main()

selfdrive/selfdrived/state.py

@@ -1,5 +1,5 @@
 from cereal import log
-from openpilot.selfdrive.controls.lib.events import Events, ET
+from openpilot.selfdrive.selfdrived.events import Events, ET
 from openpilot.common.realtime import DT_CTRL
 
 State = log.SelfdriveState.OpenpilotState

selfdrive/selfdrived/tests/test_alertmanager.py

@@ -1,7 +1,7 @@
 import random
 
-from openpilot.selfdrive.controls.lib.events import Alert, EVENTS
+from openpilot.selfdrive.selfdrived.events import Alert, EVENTS
-from openpilot.selfdrive.controls.lib.alertmanager import AlertManager
+from openpilot.selfdrive.selfdrived.alertmanager import AlertManager
 
 
 class TestAlertManager:

selfdrive/selfdrived/tests/test_alerts.py

@@ -8,13 +8,13 @@ from cereal import log, car
 from cereal.messaging import SubMaster
 from openpilot.common.basedir import BASEDIR
 from openpilot.common.params import Params
-from openpilot.selfdrive.controls.lib.events import Alert, EVENTS, ET
+from openpilot.selfdrive.selfdrived.events import Alert, EVENTS, ET
-from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
+from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
 from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS
 
 AlertSize = log.SelfdriveState.AlertSize
 
-OFFROAD_ALERTS_PATH = os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")
+OFFROAD_ALERTS_PATH = os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json")
 
 # TODO: add callback alerts
 ALERTS = []

selfdrive/selfdrived/tests/test_state_machine.py

@@ -1,7 +1,7 @@
 from cereal import log
 from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.controls.lib.selfdrive import StateMachine, SOFT_DISABLE_TIME
-from openpilot.selfdrive.controls.lib.events import Events, ET, EVENTS, NormalPermanentAlert
+from openpilot.selfdrive.selfdrived.events import Events, ET, EVENTS, NormalPermanentAlert
 
 State = log.SelfdriveState.OpenpilotState
 

selfdrive/test/process_replay/compare_logs.py

@@ -141,7 +141,7 @@ def format_diff(results, log_paths, ref_commit):
 if __name__ == "__main__":
   log1 = list(LogReader(sys.argv[1]))
   log2 = list(LogReader(sys.argv[2]))
-  ignore_fields = sys.argv[3:] or ["logMonoTime", "controlsState.cumLagMs"]
+  ignore_fields = sys.argv[3:] or ["logMonoTime"]
   results = {"segment": {"proc": compare_logs(log1, log2, ignore_fields)}}
   log_paths = {"segment": {"proc": {"ref": sys.argv[1], "new": sys.argv[2]}}}
   diff_short, diff_long, failed = format_diff(results, log_paths, None)

selfdrive/test/process_replay/process_replay.py

@@ -373,7 +373,7 @@ def get_car_params_callback(rc, pm, msgs, fingerprint):
   params.put("CarParams", convert_to_capnp(CP).to_bytes())
 
 
-def controlsd_rcv_callback(msg, cfg, frame):
+def selfdrived_rcv_callback(msg, cfg, frame):
   return (frame - 1) == 0 or msg.which() == 'carState'
 
 
@@ -452,7 +452,7 @@ class FrequencyBasedRcvCallback:
     return bool(len(resp_sockets))
 
 
-def controlsd_config_callback(params, cfg, lr):
+def selfdrived_config_callback(params, cfg, lr):
   ublox = params.get_bool("UbloxAvailable")
   sub_keys = ({"gpsLocation", } if ublox else {"gpsLocationExternal", })
 
@@ -461,22 +461,33 @@ def controlsd_config_callback(params, cfg, lr):
 
 CONFIGS = [
   ProcessConfig(
-    proc_name="controlsd",
+    proc_name="selfdrived",
     pubs=[
       "carState", "deviceState", "pandaStates", "peripheralState", "liveCalibration", "driverMonitoringState",
       "longitudinalPlan", "livePose", "liveParameters", "radarState",
       "modelV2", "driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState",
-      "testJoystick", "liveTorqueParameters", "accelerometer", "gyroscope", "carOutput",
+      "liveTorqueParameters", "accelerometer", "gyroscope", "carOutput",
-      "gpsLocationExternal", "gpsLocation", "driverAssistance"
+      "gpsLocationExternal", "gpsLocation", "controlsState", "carControl", "driverAssistance",
     ],
-    subs=["selfdriveState", "controlsState", "carControl", "onroadEvents"],
+    subs=["selfdriveState", "onroadEvents"],
-    ignore=["logMonoTime", "controlsState.cumLagMs"],
+    ignore=["logMonoTime"],
-    config_callback=controlsd_config_callback,
+    config_callback=selfdrived_config_callback,
     init_callback=get_car_params_callback,
-    should_recv_callback=controlsd_rcv_callback,
+    should_recv_callback=selfdrived_rcv_callback,
     tolerance=NUMPY_TOLERANCE,
     processing_time=0.004,
   ),
+  ProcessConfig(
+    proc_name="controlsd",
+    pubs=["liveParameters", "liveTorqueParameters", "modelV2", "selfdriveState",
+          "liveCalibration", "livePose", "longitudinalPlan", "carState", "carOutput",
+          "driverMonitoringState", "onroadEvents", "driverAssistance"],
+    subs=["carControl", "controlsState"],
+    ignore=["logMonoTime", ],
+    init_callback=get_car_params_callback,
+    should_recv_callback=MessageBasedRcvCallback("carState"),
+    tolerance=NUMPY_TOLERANCE,
+  ),
   ProcessConfig(
     proc_name="card",
     pubs=["pandaStates", "carControl", "onroadEvents", "can"],

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-fc1344b16b802cdb4ec542791f3b4d76a82da68b
+eac137f456f25bf138677315b7c4907e2fe9971b

selfdrive/test/process_replay/test_fuzzy.py

@@ -11,7 +11,7 @@ import openpilot.selfdrive.test.process_replay.process_replay as pr
 # These processes currently fail because of unrealistic data breaking assumptions
 # that openpilot makes causing error with NaN, inf, int size, array indexing ...
 # TODO: Make each one testable
-NOT_TESTED = ['controlsd', 'card', 'plannerd', 'calibrationd', 'dmonitoringd', 'paramsd', 'dmonitoringmodeld', 'modeld']
+NOT_TESTED = ['selfdrived', 'controlsd', 'card', 'plannerd', 'calibrationd', 'dmonitoringd', 'paramsd', 'dmonitoringmodeld', 'modeld']
 
 TEST_CASES = [(cfg.proc_name, copy.deepcopy(cfg)) for cfg in pr.CONFIGS if cfg.proc_name not in NOT_TESTED]
 

selfdrive/test/process_replay/test_processes.py

@@ -12,7 +12,7 @@ from openpilot.common.git import get_commit
 from openpilot.tools.lib.openpilotci import get_url, upload_file
 from openpilot.selfdrive.test.process_replay.compare_logs import compare_logs, format_diff
 from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, PROC_REPLAY_DIR, FAKEDATA, replay_process, \
-                                                                   check_openpilot_enabled, check_most_messages_valid
+                                                                   check_most_messages_valid
 from openpilot.tools.lib.filereader import FileReader
 from openpilot.tools.lib.logreader import LogReader, save_log
 
@@ -103,10 +103,6 @@ def test_process(cfg, lr, segment, ref_log_path, new_log_path, ignore_fields=Non
   except Exception as e:
     raise Exception("failed on segment: " + segment) from e
 
-  # check to make sure openpilot is engaged in the route
-  if cfg.proc_name == "controlsd":
-    if not check_openpilot_enabled(log_msgs):
-      return f"Route did not enable at all or for long enough: {new_log_path}", log_msgs
   if not check_most_messages_valid(log_msgs):
     return f"Route did not have enough valid messages: {new_log_path}", log_msgs
 

selfdrive/test/test_onroad.py

@@ -19,7 +19,7 @@ from cereal.services import SERVICE_LIST
 from openpilot.common.basedir import BASEDIR
 from openpilot.common.timeout import Timeout
 from openpilot.common.params import Params
-from openpilot.selfdrive.controls.lib.events import EVENTS, ET
+from openpilot.selfdrive.selfdrived.events import EVENTS, ET
 from openpilot.selfdrive.test.helpers import set_params_enabled, release_only
 from openpilot.system.hardware import HARDWARE
 from openpilot.system.hardware.hw import Paths
@@ -35,7 +35,8 @@ CPU usage budget
 MAX_TOTAL_CPU = 260.  # total for all 8 cores
 PROCS = {
   # Baseline CPU usage by process
-  "selfdrive.controls.controlsd": 32.0,
+  "selfdrive.controls.controlsd": 18.0,
+  "selfdrive.selfdrived.selfdrived": 21.0,
   "selfdrive.car.card": 30.0,
   "./loggerd": 14.0,
   "./encoderd": 17.0,
@@ -87,6 +88,7 @@ TIMINGS = {
   "carControl": [2.5, 0.35],
   "controlsState": [2.5, 0.35],
   "longitudinalPlan": [2.5, 0.5],
+  "driverAssistance": [2.5, 0.5],
   "roadCameraState": [2.5, 0.35],
   "driverCameraState": [2.5, 0.35],
   "modelV2": [2.5, 0.35],

selfdrive/test/test_time_to_onroad.py

@@ -20,7 +20,7 @@ def test_time_to_onroad():
   proc = subprocess.Popen(["python", manager_path])
 
   start_time = time.monotonic()
-  sm = messaging.SubMaster(['selfdriveState', 'controlsState', 'deviceState', 'onroadEvents'])
+  sm = messaging.SubMaster(['selfdriveState', 'deviceState', 'onroadEvents'])
   try:
     # wait for onroad. timeout assumes panda is up to date
     with Timeout(10, "timed out waiting to go onroad"):
@@ -34,7 +34,7 @@ def test_time_to_onroad():
         while True:
           sm.update(100)
 
-          if sm.seen['onroadEvents'] and not any(EventName.controlsInitializing == e.name for e in sm['onroadEvents']):
+          if sm.seen['onroadEvents'] and not any(EventName.selfdriveInitializing == e.name for e in sm['onroadEvents']):
             initialized = True
 
           if initialized:
@@ -51,7 +51,6 @@ def test_time_to_onroad():
       sm.update(100)
       assert sm.all_alive(), sm.alive
       assert sm['selfdriveState'].engageable, f"events: {sm['onroadEvents']}"
-      assert sm['controlsState'].cumLagMs < 10.
   finally:
     proc.terminate()
     if proc.wait(20) is None:

selfdrive/ui/qt/widgets/offroad_alerts.cc

@@ -70,7 +70,7 @@ AbstractAlert::AbstractAlert(bool hasRebootBtn, QWidget *parent) : QFrame(parent
 int OffroadAlert::refresh() {
   // build widgets for each offroad alert on first refresh
   if (alerts.empty()) {
-    QString json = util::read_file("../controls/lib/alerts_offroad.json").c_str();
+    QString json = util::read_file("../selfdrived/alerts_offroad.json").c_str();
     QJsonObject obj = QJsonDocument::fromJson(json.toUtf8()).object();
 
     // descending sort labels by severity

selfdrive/ui/tests/cycle_offroad_alerts.py

@@ -6,12 +6,12 @@ import json
 
 from openpilot.common.basedir import BASEDIR
 from openpilot.common.params import Params
-from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
+from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
 
 if __name__ == "__main__":
   params = Params()
 
-  with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f:
+  with open(os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json")) as f:
     offroad_alerts = json.load(f)
 
   t = 10 if len(sys.argv) < 2 else int(sys.argv[1])

selfdrive/ui/tests/test_ui/run.py

@@ -17,7 +17,7 @@ from openpilot.common.basedir import BASEDIR
 from openpilot.common.params import Params
 from openpilot.common.prefix import OpenpilotPrefix
 from openpilot.common.transformations.camera import CameraConfig, DEVICE_CAMERAS
-from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
+from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
 from openpilot.selfdrive.test.helpers import with_processes
 from openpilot.selfdrive.test.process_replay.migration import migrate_controlsState
 from openpilot.tools.lib.logreader import LogReader

selfdrive/ui/update_translations.py

@@ -16,7 +16,7 @@ def generate_translations_include():
   # offroad alerts
   # TODO translate events from openpilot.selfdrive/controls/lib/events.py
   content = "// THIS IS AN AUTOGENERATED FILE, PLEASE EDIT alerts_offroad.json\n"
-  with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f:
+  with open(os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json")) as f:
     for alert in json.load(f).values():
       content += f'QT_TRANSLATE_NOOP("OffroadAlert", R"({alert["text"]})");\n'
 

system/athena/registration.py

@@ -8,7 +8,7 @@ from datetime import datetime, timedelta, UTC
 from openpilot.common.api import api_get
 from openpilot.common.params import Params
 from openpilot.common.spinner import Spinner
-from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
+from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
 from openpilot.system.hardware import HARDWARE, PC
 from openpilot.system.hardware.hw import Paths
 from openpilot.common.swaglog import cloudlog

system/camerad/snapshot/snapshot.py

@@ -10,7 +10,7 @@ from msgq.visionipc import VisionIpcClient, VisionStreamType
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
 from openpilot.system.hardware import PC
-from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
+from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
 from openpilot.system.manager.process_config import managed_processes
 
 

system/hardware/hardwared.py

@@ -16,7 +16,7 @@ from openpilot.common.dict_helpers import strip_deprecated_keys
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_HW
-from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
+from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
 from openpilot.system.hardware import HARDWARE, TICI, AGNOS
 from openpilot.system.loggerd.config import get_available_percent
 from openpilot.system.statsd import statlog

system/manager/process_config.py

@@ -64,6 +64,7 @@ procs = [
   PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad),
   PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad),
   PythonProcess("controlsd", "selfdrive.controls.controlsd", only_onroad),
+  PythonProcess("selfdrived", "selfdrive.selfdrived.selfdrived", only_onroad),
   PythonProcess("card", "selfdrive.car.card", only_onroad),
   PythonProcess("deleter", "system.loggerd.deleter", always_run),
   PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(not PC or WEBCAM)),

system/updated/updated.py

@@ -17,7 +17,7 @@ from openpilot.common.params import Params
 from openpilot.common.time import system_time_valid
 from openpilot.common.markdown import parse_markdown
 from openpilot.common.swaglog import cloudlog
-from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
+from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
 from openpilot.system.hardware import AGNOS, HARDWARE
 from openpilot.system.version import get_build_metadata