|
|
|
|
@ -18,8 +18,7 @@ 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.selfdrive.car.car_helpers import get_startup_event |
|
|
|
|
from openpilot.selfdrive.car.card import CarD |
|
|
|
|
from openpilot.selfdrive.car.car_helpers import get_car_interface, get_startup_event |
|
|
|
|
from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert |
|
|
|
|
from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature |
|
|
|
|
from openpilot.selfdrive.controls.lib.events import Events, ET |
|
|
|
|
@ -61,16 +60,16 @@ ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES) |
|
|
|
|
|
|
|
|
|
class Controls: |
|
|
|
|
def __init__(self, CI=None): |
|
|
|
|
self.card = CarD(CI) |
|
|
|
|
|
|
|
|
|
self.params = Params() |
|
|
|
|
|
|
|
|
|
with car.CarParams.from_bytes(self.params.get("CarParams", block=True)) as msg: |
|
|
|
|
# TODO: this shouldn't need to be a builder |
|
|
|
|
self.CP = msg.as_builder() |
|
|
|
|
|
|
|
|
|
self.CI = self.card.CI |
|
|
|
|
|
|
|
|
|
# TODO: if we want to remove usage of CI, we need to move controllers and other fields to card (like curvature & desiredCurvature) |
|
|
|
|
# TODO: we need to ensure no CI calls here updates any state that we expect to be used to change sendcan over in card, because it won't |
|
|
|
|
# TODO: perhaps there's a different attribute or object that just provides car-specific functions for long and lat |
|
|
|
|
self.CI = get_car_interface(self.CP) |
|
|
|
|
|
|
|
|
|
# Ensure the current branch is cached, otherwise the first iteration of controlsd lags |
|
|
|
|
self.branch = get_short_branch() |
|
|
|
|
@ -89,7 +88,8 @@ class Controls: |
|
|
|
|
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', |
|
|
|
|
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', |
|
|
|
|
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', |
|
|
|
|
'testJoystick'] + self.camera_packets + self.sensor_packets, |
|
|
|
|
'carState', 'testJoystick'] + self.camera_packets + self.sensor_packets, |
|
|
|
|
poll='carState', |
|
|
|
|
ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ], |
|
|
|
|
frequency=int(1/DT_CTRL)) |
|
|
|
|
|
|
|
|
|
@ -165,7 +165,7 @@ class Controls: |
|
|
|
|
elif self.CP.passive: |
|
|
|
|
self.events.add(EventName.dashcamMode, static=True) |
|
|
|
|
|
|
|
|
|
# controlsd is driven by can recv, expected at 100Hz |
|
|
|
|
# controlsd is driven by carState, expected at 100Hz |
|
|
|
|
self.rk = Ratekeeper(100, print_delay_threshold=None) |
|
|
|
|
|
|
|
|
|
def set_initial_state(self): |
|
|
|
|
@ -324,7 +324,7 @@ class Controls: |
|
|
|
|
# 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() or self.card.can_rcv_timeout) and no_system_errors: |
|
|
|
|
if (not self.sm.all_checks() or CS.canRcvTimeout) and no_system_errors: |
|
|
|
|
if not self.sm.all_alive(): |
|
|
|
|
self.events.add(EventName.commIssue) |
|
|
|
|
elif not self.sm.all_freq_ok(): |
|
|
|
|
@ -336,7 +336,7 @@ class Controls: |
|
|
|
|
'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], |
|
|
|
|
'can_rcv_timeout': self.card.can_rcv_timeout, |
|
|
|
|
'can_rcv_timeout': CS.canRcvTimeout, |
|
|
|
|
} |
|
|
|
|
if logs != self.logged_comm_issue: |
|
|
|
|
cloudlog.event("commIssue", error=True, **logs) |
|
|
|
|
@ -398,9 +398,10 @@ class Controls: |
|
|
|
|
def data_sample(self): |
|
|
|
|
"""Receive data from sockets and update carState""" |
|
|
|
|
|
|
|
|
|
CS = self.card.state_update() |
|
|
|
|
# TODO: is this right? |
|
|
|
|
self.sm.update(20) |
|
|
|
|
|
|
|
|
|
self.sm.update(0) |
|
|
|
|
CS = self.sm['carState'] |
|
|
|
|
|
|
|
|
|
if not self.initialized: |
|
|
|
|
all_valid = CS.canValid and self.sm.all_checks() |
|
|
|
|
@ -412,8 +413,8 @@ class Controls: |
|
|
|
|
if VisionStreamType.VISION_STREAM_WIDE_ROAD not in available_streams: |
|
|
|
|
self.sm.ignore_alive.append('wideRoadCameraState') |
|
|
|
|
|
|
|
|
|
if not self.CP.passive: |
|
|
|
|
self.card.initialize() |
|
|
|
|
# if not self.CP.passive: |
|
|
|
|
# self.card.initialize() |
|
|
|
|
|
|
|
|
|
self.initialized = True |
|
|
|
|
self.set_initial_state() |
|
|
|
|
@ -727,7 +728,7 @@ class Controls: |
|
|
|
|
hudControl.visualAlert = current_alert.visual_alert |
|
|
|
|
|
|
|
|
|
if not self.CP.passive and self.initialized: |
|
|
|
|
self.card.controls_update(CC) |
|
|
|
|
# self.card.controls_update(CC) |
|
|
|
|
self.last_actuators = CO.actuatorsOutput |
|
|
|
|
if self.CP.steerControlType == car.CarParams.SteerControlType.angle: |
|
|
|
|
self.steer_limited = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ |
|
|
|
|
@ -775,9 +776,10 @@ class Controls: |
|
|
|
|
controlsState.cumLagMs = -self.rk.remaining * 1000. |
|
|
|
|
controlsState.startMonoTime = int(start_time * 1e9) |
|
|
|
|
controlsState.forceDecel = bool(force_decel) |
|
|
|
|
controlsState.canErrorCounter = self.card.can_rcv_cum_timeout_counter |
|
|
|
|
# controlsState.canErrorCounter = self.card.can_rcv_cum_timeout_counter |
|
|
|
|
controlsState.experimentalMode = self.experimental_mode |
|
|
|
|
controlsState.personality = self.personality |
|
|
|
|
controlsState.initialized = self.initialized |
|
|
|
|
|
|
|
|
|
lat_tuning = self.CP.lateralTuning.which() |
|
|
|
|
if self.joystick_mode: |
|
|
|
|
|
Shane Smiskol
1 year ago