controlsd: split up publishing and logic (#33483)

* split up the pubs

* move all event adds

* split out alerts

* lil more

* clenaup

* update test

* cleanup

selfdrive/controls/controlsd.py

@@ -117,6 +117,8 @@ class Controls:
       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()
 
@@ -216,16 +218,10 @@ class Controls:
     if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
       self.events.add(EventName.overheat)
     if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
-      # under 7% of space free no enable allowed
       self.events.add(EventName.outOfSpace)
     if self.sm['deviceState'].memoryUsagePercent > 90 and not SIMULATION:
       self.events.add(EventName.lowMemory)
 
-    # TODO: enable this once loggerd CPU usage is more reasonable
-    #cpus = list(self.sm['deviceState'].cpuUsagePercent)
-    #if max(cpus, default=0) > 95 and not SIMULATION:
-    #  self.events.add(EventName.highCpuUsage)
-
     # 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:
@@ -248,6 +244,13 @@ class Controls:
       else:
         self.events.add(EventName.calibrationInvalid)
 
+    # Lane departure warning
+    if self.sm.valid['modelV2'] and CS.canValid:
+      self.ldw.update(self.sm.frame, self.sm['modelV2'], CS, self.CC_prev)
+      if self.is_ldw_enabled and self.sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.calibrated:
+        if self.ldw.warning:
+          self.events.add(EventName.ldw)
+
     # Handle lane change
     if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
       direction = self.sm['modelV2'].meta.laneChangeDirection
@@ -348,6 +351,37 @@ class Controls:
       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
@@ -521,36 +555,6 @@ class Controls:
         lac_log.output = actuators.steer
         lac_log.saturated = abs(actuators.steer) >= 0.9
 
-    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
-
-    # Send a "steering required alert" if saturation count has reached the limit
-    if lac_log.active and not recent_steer_pressed and not self.CP.notCar:
-      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:
-          lac_log.active and self.events.add(EventName.steerSaturated)
-      elif lac_log.saturated:
-        # TODO probably should not use dpath_points but curvature
-        dpath_points = model_v2.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 = actuators.steeringAngleDeg
-          else:
-            steering_value = 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)
-
     # Ensure no NaNs/Infs
     for p in ACTUATOR_FIELDS:
       attr = getattr(actuators, p)
@@ -563,9 +567,20 @@ class Controls:
 
     return CC, lac_log
 
-  def publish_logs(self, CS, CC, lac_log):
+  def update_alerts(self, CS):
-    """Send actuators and hud commands to the car, send controlsstate and MPC logging"""
+    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_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:
@@ -591,26 +606,12 @@ class Controls:
     hudControl.rightLaneVisible = True
     hudControl.leftLaneVisible = True
 
-    self.ldw.update(self.sm.frame, self.sm['modelV2'], CS, CC)
+    if self.sm.valid['modelV2'] and CS.canValid and self.is_ldw_enabled and self.sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.calibrated:
-    if self.is_ldw_enabled and  self.sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.calibrated:
       hudControl.leftLaneDepart = self.ldw.left
       hudControl.rightLaneDepart = self.ldw.right
-      if self.ldw.warning:
-        self.events.add(EventName.ldw)
 
-    clear_event_types = set()
+    if self.AM.current_alert:
-    if ET.WARNING not in self.state_machine.current_alert_types:
+      hudControl.visualAlert = self.AM.current_alert.visual_alert
-      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)
-    current_alert = self.AM.process_alerts(self.sm.frame, clear_event_types)
-    if current_alert:
-      hudControl.visualAlert = current_alert.visual_alert
 
     if not self.CP.passive and self.initialized:
       CO = self.sm['carOutput']
@@ -620,53 +621,57 @@ class Controls:
       else:
         self.steer_limited = abs(CC.actuators.steer - CO.actuatorsOutput.steer) > 1e-2
 
-    force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
-                  (self.state_machine.state == State.softDisabling)
-
-    # Curvature & Steering angle
-    lp = self.sm['liveParameters']
-
-    steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
-    curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll)
 
     # controlsState
     dat = messaging.new_message('controlsState')
     dat.valid = CS.canValid
-    controlsState = dat.controlsState
+    cs = dat.controlsState
-    controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
+
-    controlsState.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
+    lp = self.sm['liveParameters']
-    controlsState.curvature = curvature
+    steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
-    controlsState.desiredCurvature = self.desired_curvature
+    cs.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll)
-    controlsState.longControlState = self.LoC.long_control_state
+
-    controlsState.upAccelCmd = float(self.LoC.pid.p)
+    cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
-    controlsState.uiAccelCmd = float(self.LoC.pid.i)
+    cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
-    controlsState.ufAccelCmd = float(self.LoC.pid.f)
+    cs.desiredCurvature = self.desired_curvature
-    controlsState.cumLagMs = -self.rk.remaining * 1000.
+    cs.longControlState = self.LoC.long_control_state
-    controlsState.forceDecel = bool(force_decel)
+    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))
 
     lat_tuning = self.CP.lateralTuning.which()
     if self.joystick_mode:
-      controlsState.lateralControlState.debugState = lac_log
+      cs.lateralControlState.debugState = lac_log
     elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
-      controlsState.lateralControlState.angleState = lac_log
+      cs.lateralControlState.angleState = lac_log
     elif lat_tuning == 'pid':
-      controlsState.lateralControlState.pidState = lac_log
+      cs.lateralControlState.pidState = lac_log
     elif lat_tuning == 'torque':
-      controlsState.lateralControlState.torqueState = lac_log
+      cs.lateralControlState.torqueState = lac_log
 
     self.pm.send('controlsState', dat)
 
+    # carControl
+    cc_send = messaging.new_message('carControl')
+    cc_send.valid = CS.canValid
+    cc_send.carControl = CC
+    self.pm.send('carControl', cc_send)
+
+  def publish_selfdriveState(self, CS):
     # selfdriveState
     ss_msg = messaging.new_message('selfdriveState')
     ss_msg.valid = CS.canValid
     ss = ss_msg.selfdriveState
-    if current_alert:
+    if self.AM.current_alert:
-      ss.alertText1 = current_alert.alert_text_1
+      ss.alertText1 = self.AM.current_alert.alert_text_1
-      ss.alertText2 = current_alert.alert_text_2
+      ss.alertText2 = self.AM.current_alert.alert_text_2
-      ss.alertSize = current_alert.alert_size
+      ss.alertSize = self.AM.current_alert.alert_size
-      ss.alertStatus = current_alert.alert_status
+      ss.alertStatus = self.AM.current_alert.alert_status
-      ss.alertType = current_alert.alert_type
+      ss.alertType = self.AM.current_alert.alert_type
-      ss.alertSound = current_alert.audible_alert
+      ss.alertSound = self.AM.current_alert.audible_alert
     ss.enabled = self.enabled
     ss.active = self.active
     ss.state = self.state_machine.state
@@ -683,30 +688,23 @@ class Controls:
       self.pm.send('onroadEvents', ce_send)
     self.events_prev = self.events.names.copy()
 
-    # carControl
-    cc_send = messaging.new_message('carControl')
-    cc_send.valid = CS.canValid
-    cc_send.carControl = CC
-    self.pm.send('carControl', cc_send)
-
   def step(self):
-    # Sample data from sockets and get a carState
     CS = self.data_sample()
-    cloudlog.timestamp("Data sampled")
-
     self.update_events(CS)
-    cloudlog.timestamp("Events updated")
-
     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_logs(CS, CC, lac_log)
+    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:

selfdrive/controls/lib/alertmanager.py

@@ -34,6 +34,7 @@ class AlertEntry:
 class AlertManager:
   def __init__(self):
     self.alerts: dict[str, AlertEntry] = defaultdict(AlertEntry)
+    self.current_alert: Alert | None = None
 
   def add_many(self, frame: int, alerts: list[Alert]) -> None:
     for alert in alerts:
@@ -44,8 +45,8 @@ class AlertManager:
       min_end_frame = entry.start_frame + alert.duration
       entry.end_frame = max(frame + 1, min_end_frame)
 
-  def process_alerts(self, frame: int, clear_event_types: set) -> Alert | None:
+  def process_alerts(self, frame: int, clear_event_types: set):
-    current_alert = AlertEntry()
+    ae = AlertEntry()
     for v in self.alerts.values():
       if not v.alert:
         continue
@@ -54,8 +55,8 @@ class AlertManager:
         v.end_frame = -1
 
       # sort by priority first and then by start_frame
-      greater = current_alert.alert is None or (v.alert.priority, v.start_frame) > (current_alert.alert.priority, current_alert.start_frame)
+      greater = ae.alert is None or (v.alert.priority, v.start_frame) > (ae.alert.priority, ae.start_frame)
       if v.active(frame) and greater:
-        current_alert = v
+        ae = v
 
-    return current_alert.alert
+    self.current_alert = ae.alert

selfdrive/controls/lib/tests/test_alertmanager.py

@@ -32,8 +32,8 @@ class TestAlertManager:
         for frame in range(duration+10):
           if frame < add_duration:
             AM.add_many(frame, [alert, ])
-          current_alert = AM.process_alerts(frame, {})
+          AM.process_alerts(frame, {})
 
-          shown = current_alert is not None
+          shown = AM.current_alert is not None
           should_show = frame <= show_duration
           assert shown == should_show, f"{frame=} {add_duration=} {duration=}"

selfdrive/debug/count_events.py

@@ -33,8 +33,8 @@ if __name__ == "__main__":
       if len(events) == 0 or ae != events[-1][1]:
         events.append((t, ae))
 
-    elif msg.which() == 'controlsState':
+    elif msg.which() == 'selfdriveState':
-      at = msg.controlsState.alertType
+      at = msg.selfdriveState.alertType
       if "/override" not in at or "lanechange" in at.lower():
         if len(alerts) == 0 or alerts[-1][1] != at:
           alerts.append((t, at))

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-c2022c388da6eb2e26bb23ad6009be9d5314c0be
+fb0827a00b21b97733f8385cf7ce87a21a526f4c