hmm it's just becoming controlsd

cereal

@@ -1 +1 @@
-Subproject commit 0cebb30e41c436b023910a7a03a22e601648cb58
+Subproject commit d3a844c9b0d78c2af2b94d7bd92ff01cc1000ccb

selfdrive/car/card.py

@@ -1,6 +1,8 @@
 #!/usr/bin/env python3
 import os
+import math
 import time
+from typing import SupportsFloat
 
 import cereal.messaging as messaging
 
@@ -8,6 +10,8 @@ from cereal import car, log
 
 from panda import ALTERNATIVE_EXPERIENCE
 
+from openpilot.common.conversions import Conversions as CV
+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.swaglog import cloudlog
@@ -15,15 +19,25 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp
 from openpilot.selfdrive.car.car_helpers import get_car, get_one_can
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase
-from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper
+from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature
-from openpilot.selfdrive.controls.lib.events import Events
+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
 
 REPLAY = "REPLAY" in os.environ
 
 State = log.ControlsState.OpenpilotState
+LaneChangeState = log.LaneChangeState
+LaneChangeDirection = log.LaneChangeDirection
 EventName = car.CarEvent.EventName
 ButtonType = car.CarState.ButtonEvent.Type
 
+ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
+
 
 class Car:
   CI: CarInterfaceBase
@@ -32,9 +46,9 @@ class Car:
     self.POLL = False
 
     self.can_sock = messaging.sub_sock('can', timeout=20)
-    self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'controlsState'],
+    self.sm = messaging.SubMaster(['pandaStates', 'testJoystick', 'controlsState'],
                                   poll='carControl' if self.POLL else None)
-    self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput'])
+    self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams'])
 
     self.can_rcv_timeout_counter = 0  # consecutive timeout count
     self.can_rcv_cum_timeout_counter = 0  # cumulative timeout count
@@ -43,8 +57,6 @@ class Car:
     self.CS_prev = car.CarState.new_message()
     self.controlsState_prev = car.CarState.new_message()
 
-    self.last_actuators_output = car.CarControl.Actuators.new_message()
-
     self.params = Params()
 
     if CI is None:
@@ -59,6 +71,7 @@ class Car:
       self.CI, self.CP = CI, CI.CP
 
     # read params
+    self.joystick_mode = self.params.get_bool("JoystickDebugMode")
     self.is_metric = self.params.get_bool("IsMetric")
 
     # set alternative experiences from parameters
@@ -91,6 +104,21 @@ class Car:
     self.events = Events()
     self.v_cruise_helper = VCruiseHelper(self.CP)
 
+    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)
+    elif self.CP.lateralTuning.which() == 'pid':
+      self.LaC = LatControlPID(self.CP, self.CI)
+    elif self.CP.lateralTuning.which() == 'torque':
+      self.LaC = LatControlTorque(self.CP, self.CI)
+
+    self.last_steering_pressed_frame = 0
+    self.steer_limited = False
+    self.desired_curvature = 0.0
+
     # card is driven by can recv, expected at 100Hz
     self.rk = Ratekeeper(100, print_delay_threshold=None)
 
@@ -152,7 +180,7 @@ class Car:
       if controlsState.state in (State.preEnabled, State.overriding, State.enabled):
        self.v_cruise_helper.initialize_v_cruise(CS, controlsState.experimentalMode)
 
-  def state_publish(self, CS: car.CarState):
+  def state_publish(self, CS: car.CarState, CC: car.CarControl, lac_log):
     """carState and carParams publish loop"""
 
     # carParams - logged every 50 seconds (> 1 per segment)
@@ -162,37 +190,176 @@ class Car:
       cp_send.carParams = self.CP
       self.pm.send('carParams', cp_send)
 
-    # publish new carOutput
+    # carState
-    co_send = messaging.new_message('carOutput')
-    co_send.valid = self.sm.all_checks(['carControl'])
-    co_send.carOutput.actuatorsOutput = self.last_actuators_output
-    self.pm.send('carOutput', co_send)
-
-    # kick off controlsd step now while we actuate the latest carControl packet
     cs_send = messaging.new_message('carState')
     cs_send.valid = CS.canValid
     cs_send.carState = CS
     cs_send.carState.canRcvTimeout = self.can_rcv_timeout
     cs_send.carState.canErrorCounter = self.can_rcv_cum_timeout_counter
     cs_send.carState.cumLagMs = -self.rk.remaining * 1000.
+
+    # TODO: this
+    # lat_tuning = self.CP.lateralTuning.which()
+    # if self.joystick_mode:
+    #   carState.lateralControlState.debugState = lac_log
+    # elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
+    #   carState.lateralControlState.angleState = lac_log
+    # elif lat_tuning == 'pid':
+    #   carState.lateralControlState.pidState = lac_log
+    # elif lat_tuning == 'torque':
+    #   carState.lateralControlState.torqueState = lac_log
+
     self.pm.send('carState', cs_send)
     cloudlog.timestamp('Sent carState')
 
+    # carControl
+    if not self.CP.passive and self.sm['controlsState'].initialized:
+      # send car controls over can
+      now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9)
+      CC.actuatorsOutput, can_sends = self.CI.apply(CC, now_nanos)
+      self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
+      if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
+        self.steer_limited = abs(CC.actuators.steeringAngleDeg - CC.actuatorsOutput.steeringAngleDeg) > \
+                             STEER_ANGLE_SATURATION_THRESHOLD
+      else:
+        self.steer_limited = abs(CC.actuators.steer - CC.actuatorsOutput.steer) > 1e-2
+
+
+
+    cc_send = messaging.new_message('carControl')
+    cc_send.valid = CS.canValid
+    cc_send.carControl = CC
+    self.pm.send('carControl', cc_send)
+
     if self.POLL:
       # wait for latest carControl
       self.sm.update(20)
 
   def controls_update(self, CS: car.CarState, CC: car.CarControl):
-    """control update loop, driven by carControl"""
+    # """control update loop, driven by carControl"""
+
+    # Update VehicleModel
+    lp = self.sm['liveParameters']
+    x = max(lp.stiffnessFactor, 0.1)
+    sr = max(lp.steerRatio, 0.1)
+    self.VM.update_params(x, sr)
+
+    # Update Torque Params
+    if self.CP.lateralTuning.which() == 'torque':
+      torque_params = self.sm['liveTorqueParameters']
+      if self.sm.all_checks(['liveTorqueParameters']) and torque_params.useParams:
+        self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
+                                           torque_params.frictionCoefficientFiltered)
+
+    long_plan = self.sm['longitudinalPlan']
+    model_v2 = self.sm['modelV2']
+    controls_state = self.sm['controlsState']
+
+    CC = car.CarControl.new_message()
+    CC.enabled = controls_state.enabled
+
+    # Check which actuators can be enabled
+    standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
+    CC.latActive = controls_state.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
+                   (not standstill or self.joystick_mode)
+    CC.longActive = controls_state.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
+
+    actuators = CC.actuators
+    actuators.longControlState = self.LoC.long_control_state
+
+    # Enable blinkers while lane changing
+    if model_v2.meta.laneChangeState != LaneChangeState.off:
+      CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left
+      CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right
+
+    # State specific actions
+
+    if not CC.latActive:
+      self.LaC.reset()
+    if not CC.longActive:
+      self.LoC.reset(v_pid=CS.vEgo)
+
+    if not self.joystick_mode:
+      # accel PID loop
+      pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS)
+      t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL
+      actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan)
+
+      # Steering PID loop and lateral MPC
+      self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature)
+      actuators.curvature = self.desired_curvature
+      actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
+                                                                             self.steer_limited, self.desired_curvature,
+                                                                             self.sm['liveLocationKalman'])
+    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:
+          steer = clip(joystick_axes[1], -1, 1)
+          # max angle is 45 for angle-based cars, max curvature is 0.02
+          actuators.steer, actuators.steeringAngleDeg, actuators.curvature = steer, steer * 90., steer * -0.02
+
+        lac_log.active = controls_state.active
+        lac_log.steeringAngleDeg = CS.steeringAngleDeg
+        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.CC_prev.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)
+      if not isinstance(attr, SupportsFloat):
+        continue
+
+      if not math.isfinite(attr):
+        cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
+        setattr(actuators, p, 0.0)
 
-    if self.sm.all_checks(['carControl']):
     # send car controls over can
     now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9)
-      # TODO: CC shouldn't be builder
+    CC.actuatorsOutput, can_sends = self.CI.apply(CC, now_nanos)
-      self.last_actuators_output, can_sends = self.CI.apply(CC.as_builder(), now_nanos)
     self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
 
-      self.CC_prev = CC
+    return CC, lac_log
 
   def step(self):
     cloudlog.timestamp("Start card")
@@ -204,16 +371,17 @@ class Car:
     if not self.CP.passive and self.sm['controlsState'].initialized:
       self.state_transition(CS)
 
-    self.state_publish(CS)
-    cloudlog.timestamp("State published")
-
     controlsState = self.sm['controlsState']
-    if not self.CP.passive and controlsState.initialized:
+    # if not self.CP.passive and controlsState.initialized:
-      self.controls_update(CS, self.sm['carControl'])
+    CC, lac_log = self.controls_update(CS, self.sm['carControl'])
     cloudlog.timestamp("Controls updated")
 
+    self.state_publish(CS, CC, lac_log)
+    cloudlog.timestamp("State published")
+
     self.controlsState_prev = controlsState
     self.CS_prev = CS.as_reader()
+    self.CC_prev = CC
 
   def card_thread(self):
     while True:

selfdrive/controls/controlsd.py

@@ -23,11 +23,11 @@ from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offr
 from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
 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_pid import LatControlPID
-from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
+# 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.latcontrol_torque import LatControlTorque
-from openpilot.selfdrive.controls.lib.longcontrol import LongControl
+# from openpilot.selfdrive.controls.lib.longcontrol import LongControl
-from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
+# from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
 
 from openpilot.system.hardware import HARDWARE
 
@@ -115,16 +115,16 @@ class Controls:
     self.AM = AlertManager()
     self.events = Events()
 
-    self.LoC = LongControl(self.CP)
+    # self.LoC = LongControl(self.CP)
-    self.VM = VehicleModel(self.CP)
+    # self.VM = VehicleModel(self.CP)
-
+    #
-    self.LaC: LatControl
+    # self.LaC: LatControl
-    if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
+    # if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
-      self.LaC = LatControlAngle(self.CP, self.CI)
+    #   self.LaC = LatControlAngle(self.CP, self.CI)
-    elif self.CP.lateralTuning.which() == 'pid':
+    # elif self.CP.lateralTuning.which() == 'pid':
-      self.LaC = LatControlPID(self.CP, self.CI)
+    #   self.LaC = LatControlPID(self.CP, self.CI)
-    elif self.CP.lateralTuning.which() == 'torque':
+    # elif self.CP.lateralTuning.which() == 'torque':
-      self.LaC = LatControlTorque(self.CP, self.CI)
+    #   self.LaC = LatControlTorque(self.CP, self.CI)
 
     self.initialized = False
     self.state = State.disabled
@@ -510,124 +510,44 @@ class Controls:
   def state_control(self, CS):
     """Given the state, this function returns a CarControl packet"""
 
-    # Update VehicleModel
+    # # Update VehicleModel
-    lp = self.sm['liveParameters']
+    # lp = self.sm['liveParameters']
-    x = max(lp.stiffnessFactor, 0.1)
+    # x = max(lp.stiffnessFactor, 0.1)
-    sr = max(lp.steerRatio, 0.1)
+    # sr = max(lp.steerRatio, 0.1)
-    self.VM.update_params(x, sr)
+    # self.VM.update_params(x, sr)
-
+    #
-    # Update Torque Params
+    # # Update Torque Params
-    if self.CP.lateralTuning.which() == 'torque':
+    # if self.CP.lateralTuning.which() == 'torque':
-      torque_params = self.sm['liveTorqueParameters']
+    #   torque_params = self.sm['liveTorqueParameters']
-      if self.sm.all_checks(['liveTorqueParameters']) and torque_params.useParams:
+    #   if self.sm.all_checks(['liveTorqueParameters']) and torque_params.useParams:
-        self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
+    #     self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
-                                           torque_params.frictionCoefficientFiltered)
+    #                                        torque_params.frictionCoefficientFiltered)
-
+    #
-    long_plan = self.sm['longitudinalPlan']
+    # long_plan = self.sm['longitudinalPlan']
-    model_v2 = self.sm['modelV2']
+    # model_v2 = self.sm['modelV2']
-
+    #
-    CC = car.CarControl.new_message()
+    # CC = car.CarControl.new_message()
-    CC.enabled = self.enabled
+    # CC.enabled = self.enabled
-
+    #
-    # Check which actuators can be enabled
+    # # Check which actuators can be enabled
-    standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
+    # 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.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
-                   (not standstill or self.joystick_mode)
+    #                (not standstill or self.joystick_mode)
-    CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
+    # CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
-
+    #
-    actuators = CC.actuators
+    # actuators = CC.actuators
-    actuators.longControlState = self.LoC.long_control_state
+    # actuators.longControlState = self.LoC.long_control_state
-
+
-    # Enable blinkers while lane changing
+    # # Enable blinkers while lane changing
-    if model_v2.meta.laneChangeState != LaneChangeState.off:
+    # if model_v2.meta.laneChangeState != LaneChangeState.off:
-      CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left
+    #   CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left
-      CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right
+    #   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(v_pid=CS.vEgo)
-
-    if not self.joystick_mode:
-      # accel PID loop
-      pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.card.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS)
-      t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL
-      actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan)
-
-      # Steering PID loop and lateral MPC
-      self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature)
-      actuators.curvature = self.desired_curvature
-      actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
-                                                                             self.steer_limited, self.desired_curvature,
-                                                                             self.sm['liveLocationKalman'])
-    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:
-          steer = clip(joystick_axes[1], -1, 1)
-          # max angle is 45 for angle-based cars, max curvature is 0.02
-          actuators.steer, actuators.steeringAngleDeg, actuators.curvature = steer, steer * 90., steer * -0.02
-
-        lac_log.active = self.active
-        lac_log.steeringAngleDeg = CS.steeringAngleDeg
-        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)
-      if not isinstance(attr, SupportsFloat):
-        continue
-
-      if not math.isfinite(attr):
-        cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
-        setattr(actuators, p, 0.0)
-
     # 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):
@@ -636,7 +556,7 @@ class Controls:
 
     return CC, lac_log
 
-  def publish_logs(self, CS, start_time, CC, lac_log):
+  def publish_logs(self, CS, start_time):
     """Send actuators and hud commands to the car, send controlsstate and MPC logging"""
 
     # Orientation and angle rates can be useful for carcontroller