controlsd: use latest actuatorsOutput (#32390)

use current actuatorsOutput
old-commit-hash: ef1693433f
pull/32199/head
Shane Smiskol 12 months ago committed by GitHub
parent d9c32672d1
commit 753dbe8f6e
  1. 2
      selfdrive/car/card.py
  2. 7
      selfdrive/controls/controlsd.py

@ -78,7 +78,7 @@ class CarD:
"""Initialize CarInterface, once controls are ready"""
self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
def state_update(self):
def state_update(self) -> car.CarState:
"""carState update loop, driven by can"""
# Update carState from CAN

@ -142,7 +142,6 @@ class Controls:
self.logged_comm_issue = None
self.not_running_prev = None
self.steer_limited = False
self.last_actuators = car.CarControl.Actuators.new_message()
self.desired_curvature = 0.0
self.experimental_mode = False
self.personality = self.read_personality_param()
@ -620,7 +619,7 @@ class Controls:
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.last_actuators.steer) > 0.99
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:
@ -661,8 +660,6 @@ class Controls:
def publish_logs(self, CS, start_time, CC, lac_log):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
CO = self.sm['carOutput']
# Orientation and angle rates can be useful for carcontroller
# Only calibrated (car) frame is relevant for the carcontroller
orientation_value = list(self.sm['liveLocationKalman'].calibratedOrientationNED.value)
@ -727,7 +724,7 @@ class Controls:
if not self.CP.passive and self.initialized:
self.card.controls_update(CC)
self.last_actuators = CO.actuatorsOutput
CO = self.sm['carOutput']
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.steer_limited = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \
STEER_ANGLE_SATURATION_THRESHOLD

Loading…
Cancel
Save