diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 0251877dfb..69b7873d20 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -20,6 +20,7 @@ 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_startup_event, get_one_can +from openpilot.selfdrive.car.interfaces import CarInterfaceBase 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 @@ -60,22 +61,111 @@ ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding) ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES) +class CarD: + CI: CarInterfaceBase + CS: car.CarState + + def __init__(self, CI=None): + self.can_sock = messaging.sub_sock('can', timeout=20) + self.sm = messaging.SubMaster(['pandaStates']) + self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams']) + + self.can_rcv_timeout_counter = 0 # conseuctive timeout count + self.can_rcv_cum_timeout_counter = 0 # cumulative timeout count + + self.params = Params() + + if CI is None: + # wait for one pandaState and one CAN packet + print("Waiting for CAN messages...") + get_one_can(self.can_sock) + + num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) + experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled") + self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], experimental_long_allowed, num_pandas) + else: + self.CI, self.CP = CI, CI.CP + + def initialize(self): + """Initialize CarInterface, once controls are ready""" + self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) + + def state_update(self, CC: car.CarControl): + """carState update loop, driven by can""" + + # TODO: This should not depend on carControl + + # Update carState from CAN + can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) + self.CS = self.CI.update(CC, can_strs) + + self.sm.update(0) + + can_rcv_valid = len(can_strs) > 0 + + # Check for CAN timeout + if not can_rcv_valid: + self.can_rcv_timeout_counter += 1 + self.can_rcv_cum_timeout_counter += 1 + else: + self.can_rcv_timeout_counter = 0 + + self.can_rcv_timeout = self.can_rcv_timeout_counter >= 5 + + if can_rcv_valid and REPLAY: + self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime + + return self.CS + + def state_publish(self, car_events): + """carState and carParams publish loop""" + + # TODO: carState should be independent of the event loop + + # carState + cs_send = messaging.new_message('carState') + cs_send.valid = self.CS.canValid + cs_send.carState = self.CS + cs_send.carState.events = car_events + self.pm.send('carState', cs_send) + + # carParams - logged every 50 seconds (> 1 per segment) + if (self.sm.frame % int(50. / DT_CTRL) == 0): + cp_send = messaging.new_message('carParams') + cp_send.valid = True + cp_send.carParams = self.CP + self.pm.send('carParams', cp_send) + + def controls_update(self, CC: car.CarControl): + """control update loop, driven by carControl""" + + # send car controls over can + now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) + actuators_output, can_sends = self.CI.apply(CC, now_nanos) + self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=self.CS.canValid)) + + return actuators_output + + class Controls: def __init__(self, CI=None): + self.card = CarD(CI) + + self.CP = self.card.CP + self.CI = self.card.CI + config_realtime_process(4, Priority.CTRL_HIGH) # Ensure the current branch is cached, otherwise the first iteration of controlsd lags self.branch = get_short_branch() # Setup sockets - self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', - 'carControl', 'onroadEvents', 'carParams']) + self.pm = messaging.PubMaster(['controlsState', 'carControl', 'onroadEvents']) self.sensor_packets = ["accelerometer", "gyroscope"] self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"] self.log_sock = messaging.sub_sock('androidLog') - self.can_sock = messaging.sub_sock('can', timeout=20) self.params = Params() ignore = self.sensor_packets + ['testJoystick'] @@ -88,17 +178,6 @@ class Controls: ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ], frequency=int(1/DT_CTRL)) - if CI is None: - # wait for one pandaState and one CAN packet - print("Waiting for CAN messages...") - get_one_can(self.can_sock) - - num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) - experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled") - self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], experimental_long_allowed, num_pandas) - else: - self.CI, self.CP = CI, CI.CP - self.joystick_mode = self.params.get_bool("JoystickDebugMode") # set alternative experiences from parameters @@ -164,8 +243,6 @@ class Controls: self.soft_disable_timer = 0 self.mismatch_counter = 0 self.cruise_mismatch_counter = 0 - self.can_rcv_timeout_counter = 0 # conseuctive timeout count - self.can_rcv_cum_timeout_counter = 0 # cumulative timeout count self.last_blinker_frame = 0 self.last_steering_pressed_frame = 0 self.distance_traveled = 0 @@ -353,10 +430,9 @@ class Controls: self.events.add(EventName.canError) # generic catch-all. ideally, a more specific event should be added above instead - can_rcv_timeout = self.can_rcv_timeout_counter >= 5 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 can_rcv_timeout) and no_system_errors: + if (not self.sm.all_checks() or self.card.can_rcv_timeout) and no_system_errors: if not self.sm.all_alive(): self.events.add(EventName.commIssue) elif not self.sm.all_freq_ok(): @@ -368,7 +444,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': can_rcv_timeout, + 'can_rcv_timeout': self.card.can_rcv_timeout, } if logs != self.logged_comm_issue: cloudlog.event("commIssue", error=True, **logs) @@ -430,11 +506,7 @@ class Controls: def data_sample(self): """Receive data from sockets and update carState""" - # Update carState from CAN - can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) - CS = self.CI.update(self.CC, can_strs) - if len(can_strs) and REPLAY: - self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime + CS = self.card.state_update(self.CC) self.sm.update(0) @@ -449,7 +521,7 @@ class Controls: self.sm.ignore_alive.append('wideRoadCameraState') if not self.CP.passive: - self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) + self.card.initialize() self.initialized = True self.set_initial_state() @@ -466,13 +538,6 @@ class Controls: error=True, ) - # Check for CAN timeout - if not can_strs: - self.can_rcv_timeout_counter += 1 - self.can_rcv_cum_timeout_counter += 1 - else: - self.can_rcv_timeout_counter = 0 - # 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. @@ -761,10 +826,7 @@ class Controls: hudControl.visualAlert = current_alert.visual_alert if not self.CP.passive and self.initialized: - # send car controls over can - now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) - self.last_actuators, can_sends = self.CI.apply(CC, now_nanos) - self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) + self.last_actuators = self.card.controls_update(CC) CC.actuatorsOutput = self.last_actuators if self.CP.steerControlType == car.CarParams.SteerControlType.angle: self.steer_limited = abs(CC.actuators.steeringAngleDeg - CC.actuatorsOutput.steeringAngleDeg) > \ @@ -812,7 +874,7 @@ class Controls: controlsState.cumLagMs = -self.rk.remaining * 1000. controlsState.startMonoTime = int(start_time * 1e9) controlsState.forceDecel = bool(force_decel) - controlsState.canErrorCounter = self.can_rcv_cum_timeout_counter + controlsState.canErrorCounter = self.card.can_rcv_cum_timeout_counter controlsState.experimentalMode = self.experimental_mode lat_tuning = self.CP.lateralTuning.which() @@ -827,13 +889,9 @@ class Controls: self.pm.send('controlsState', dat) - # carState car_events = self.events.to_msg() - cs_send = messaging.new_message('carState') - cs_send.valid = CS.canValid - cs_send.carState = CS - cs_send.carState.events = car_events - self.pm.send('carState', cs_send) + + self.card.state_publish(car_events) # onroadEvents - logged every second or on change if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev): @@ -843,13 +901,6 @@ class Controls: self.pm.send('onroadEvents', ce_send) self.events_prev = self.events.names.copy() - # carParams - logged every 50 seconds (> 1 per segment) - if (self.sm.frame % int(50. / DT_CTRL) == 0): - cp_send = messaging.new_message('carParams') - cp_send.valid = True - cp_send.carParams = self.CP - self.pm.send('carParams', cp_send) - # carControl cc_send = messaging.new_message('carControl') cc_send.valid = CS.canValid diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index e98cf09b9a..de8a4420b3 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -29,7 +29,7 @@ from openpilot.tools.lib.logreader import LogReader # Baseline CPU usage by process PROCS = { - "selfdrive.controls.controlsd": 39.0, + "selfdrive.controls.controlsd": 41.0, "./loggerd": 14.0, "./encoderd": 17.0, "./camerad": 14.5,