From e2d62fa7b5ca3ea9286557cd980d655c61e873c5 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 8 May 2024 22:51:01 -0700 Subject: [PATCH] standalone process --- cereal | 2 +- selfdrive/car/card.py | 47 +++++++++++++++++++++++------ selfdrive/controls/controlsd.py | 36 +++++++++++----------- selfdrive/manager/process_config.py | 1 + 4 files changed, 58 insertions(+), 28 deletions(-) diff --git a/cereal b/cereal index 84af7ef665..c2adf2fbc4 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 84af7ef6654b2e810a2871b8ddd27ad170696599 +Subproject commit c2adf2fbc460b3854334a9430b918565547b3757 diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 7731a0c1da..c4383f78f4 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -9,7 +9,7 @@ from cereal import car from panda import ALTERNATIVE_EXPERIENCE from openpilot.common.params import Params -from openpilot.common.realtime import DT_CTRL +from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp from openpilot.selfdrive.car.car_helpers import get_car, get_one_can @@ -18,19 +18,20 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase REPLAY = "REPLAY" in os.environ -class CarD: +class Car: 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.sm = messaging.SubMaster(['pandaStates', 'carControl', 'controlsState']) self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput']) self.can_rcv_timeout_counter = 0 # consecutive timeout count self.can_rcv_cum_timeout_counter = 0 # cumulative timeout count self.CC_prev = car.CarControl.new_message() + self.CoS_prev = car.CarState.new_message() self.last_actuators = None @@ -75,9 +76,8 @@ class CarD: self.params.put_nonblocking("CarParamsCache", cp_bytes) self.params.put_nonblocking("CarParamsPersistent", cp_bytes) - def initialize(self): - """Initialize CarInterface, once controls are ready""" - self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) + # card is driven by can recv, expected at 100Hz + self.rk = Ratekeeper(100, print_delay_threshold=None) def state_update(self): """carState update loop, driven by can""" @@ -102,10 +102,6 @@ class CarD: if can_rcv_valid and REPLAY: self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime - self.state_publish() - - return self.CS - def state_publish(self): """carState and carParams publish loop""" @@ -113,6 +109,9 @@ class CarD: cs_send = messaging.new_message('carState') cs_send.valid = self.CS.canValid cs_send.carState = self.CS + cs_send.canRcvTimeout = self.can_rcv_timeout + cs_send.canErrorCounter = self.can_rcv_cum_timeout_counter + cs_send.cumLagMs = -self.rk.remaining * 1000. self.pm.send('carState', cs_send) # carParams - logged every 50 seconds (> 1 per segment) @@ -138,3 +137,31 @@ class CarD: self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=self.CS.canValid)) self.CC_prev = CC + + def step(self): + if self.sm['controlsState'].initialized and not self.CoS_prev.initialized: + self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) + + self.state_update() + + self.state_publish() + + if not self.CP.passive and self.sm['controlsState'].initialized: + self.controls_update(self.sm['carControl']) + + self.CoS_prev = self.sm['controlsState'] + + def card_thread(self): + while True: + self.step() + self.rk.monitor_time() + + +def main(): + config_realtime_process(4, Priority.CTRL_HIGH) + car = Car() + car.card_thread() + + +if __name__ == "__main__": + main() diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 35dad3f81a..dcbb65de48 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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() @@ -439,7 +440,7 @@ class Controls: # All pandas not in silent mode must have controlsAllowed when openpilot is enabled if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates'] - if ps.safetyModel not in IGNORED_SAFETY_MODES): + if ps.safetyModel not in IGNORED_SAFETY_MODES): self.mismatch_counter += 1 return CS @@ -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: diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 4c34e7339e..72a4400a28 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -64,6 +64,7 @@ procs = [ PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad), PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad), PythonProcess("controlsd", "selfdrive.controls.controlsd", only_onroad), + PythonProcess("card", "selfdrive.car.card", only_onroad), PythonProcess("deleter", "system.loggerd.deleter", always_run), PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(not PC or WEBCAM)), PythonProcess("qcomgpsd", "system.qcomgpsd.qcomgpsd", qcomgps, enabled=TICI),