diff --git a/cereal b/cereal index 0a9b426e55..5812f2c075 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 0a9b426e55653daea6cc9d3c40c3f7600ec0db49 +Subproject commit 5812f2c075a5364cecafe4e8ed68a12b12a5631f diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index db34320caa..259126c416 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -75,7 +75,7 @@ class CarController(CarControllerBase): can_sends = [] can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r)) - new_actuators = CC.actuators.copy() + new_actuators = CC.actuators.as_builder() new_actuators.accel = torque_l new_actuators.steer = torque_r new_actuators.steerOutputCan = torque_r diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index ed916a6fe1..2705a3c01c 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 @@ -21,22 +21,24 @@ REPLAY = "REPLAY" in os.environ EventName = car.CarEvent.EventName -class CarD: +class Car: CI: CarInterfaceBase 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', 'onroadEvents']) 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.CS_prev = car.CarState.new_message() + self.initialized_prev = False - self.last_actuators = None + self.last_actuators_output = car.CarControl.Actuators.new_message() - self.params = Params() + params = Params() if CI is None: # wait for one pandaState and one CAN packet @@ -44,18 +46,18 @@ class CarD: 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") + experimental_long_allowed = 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 # set alternative experiences from parameters - self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") + self.disengage_on_accelerator = params.get_bool("DisengageOnAccelerator") self.CP.alternativeExperience = 0 if not self.disengage_on_accelerator: self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS - openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") + openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle") controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly @@ -66,22 +68,20 @@ class CarD: self.CP.safetyConfigs = [safety_config] # Write previous route's CarParams - prev_cp = self.params.get("CarParamsPersistent") + prev_cp = params.get("CarParamsPersistent") if prev_cp is not None: - self.params.put("CarParamsPrevRoute", prev_cp) + params.put("CarParamsPrevRoute", prev_cp) # Write CarParams for controls and radard cp_bytes = self.CP.to_bytes() - self.params.put("CarParams", cp_bytes) - self.params.put_nonblocking("CarParamsCache", cp_bytes) - self.params.put_nonblocking("CarParamsPersistent", cp_bytes) + params.put("CarParams", cp_bytes) + params.put_nonblocking("CarParamsCache", cp_bytes) + params.put_nonblocking("CarParamsPersistent", cp_bytes) - self.CS_prev = car.CarState.new_message() self.events = Events() - 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) -> car.CarState: """carState update loop, driven by can""" @@ -106,11 +106,6 @@ class CarD: if can_rcv_valid and REPLAY: self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime - self.update_events(CS) - self.state_publish(CS) - - CS = CS.as_reader() - self.CS_prev = CS return CS def update_events(self, CS: car.CarState) -> car.CarState: @@ -129,12 +124,6 @@ class CarD: def state_publish(self, CS: car.CarState): """carState and carParams publish loop""" - # carState - cs_send = messaging.new_message('carState') - cs_send.valid = CS.canValid - cs_send.carState = CS - 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') @@ -144,17 +133,60 @@ class CarD: # publish new carOutput co_send = messaging.new_message('carOutput') - co_send.valid = True - if self.last_actuators is not None: - co_send.carOutput.actuatorsOutput = self.last_actuators + 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 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. + self.pm.send('carState', cs_send) + def controls_update(self, CS: car.CarState, 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) - 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)) + if not self.initialized_prev: + # Initialize CarInterface, once controls are ready + self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) + + if self.sm.all_alive(['carControl']): + # send car controls over can + now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) + self.last_actuators_output, 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.CC_prev = CC + + def step(self): + CS = self.state_update() + + self.update_events(CS) + + self.state_publish(CS) + + initialized = (not any(e.name == EventName.controlsInitializing for e in self.sm['onroadEvents']) and + self.sm.seen['onroadEvents']) + if not self.CP.passive and initialized: + self.controls_update(CS, self.sm['carControl']) + + self.initialized_prev = initialized + self.CS_prev = CS.as_reader() + + 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() + - self.CC_prev = CC +if __name__ == "__main__": + main() diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 39248f3f75..85f53f68eb 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -78,7 +78,7 @@ class CarController(CarControllerBase): self.frame += 1 - new_actuators = CC.actuators.copy() + new_actuators = CC.actuators.as_builder() new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 47082fb56f..7be3b2ebe9 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -113,7 +113,7 @@ class CarController(CarControllerBase): self.steer_alert_last = steer_alert self.lead_distance_bars_last = hud_control.leadDistanceBars - new_actuators = actuators.copy() + new_actuators = actuators.as_builder() new_actuators.curvature = self.apply_curvature_last self.frame += 1 diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index f8d747029b..b204d3b80f 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -155,7 +155,7 @@ class CarController(CarControllerBase): if self.frame % 10 == 0: can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, CS.pscm_status)) - new_actuators = actuators.copy() + new_actuators = actuators.as_builder() new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last new_actuators.gas = self.apply_gas diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 6fe8c27585..fe023ea17d 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -244,7 +244,7 @@ class CarController(CarControllerBase): self.speed = pcm_speed self.gas = pcm_accel / self.params.NIDEC_GAS_MAX - new_actuators = actuators.copy() + new_actuators = actuators.as_builder() new_actuators.speed = self.speed new_actuators.accel = self.accel new_actuators.gas = self.gas diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 7829d764b0..4038ddcca9 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -163,7 +163,7 @@ class CarController(CarControllerBase): if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl: can_sends.append(hyundaican.create_frt_radar_opt(self.packer)) - new_actuators = actuators.copy() + new_actuators = actuators.as_builder() new_actuators.steer = apply_steer / self.params.STEER_MAX new_actuators.steerOutputCan = apply_steer new_actuators.accel = accel diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 8d3a59b4ea..3d41634879 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -58,7 +58,7 @@ class CarController(CarControllerBase): can_sends.append(mazdacan.create_steering_control(self.packer, self.CP, self.frame, apply_steer, CS.cam_lkas)) - new_actuators = CC.actuators.copy() + new_actuators = CC.actuators.as_builder() new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX new_actuators.steerOutputCan = apply_steer diff --git a/selfdrive/car/mock/carcontroller.py b/selfdrive/car/mock/carcontroller.py index 2b2da954ff..0cd37c0369 100644 --- a/selfdrive/car/mock/carcontroller.py +++ b/selfdrive/car/mock/carcontroller.py @@ -2,4 +2,4 @@ from openpilot.selfdrive.car.interfaces import CarControllerBase class CarController(CarControllerBase): def update(self, CC, CS, now_nanos): - return CC.actuators.copy(), [] + return CC.actuators.as_builder(), [] diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 83775462b7..c7bd231398 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -75,7 +75,7 @@ class CarController(CarControllerBase): self.packer, CS.lkas_hud_info_msg, steer_hud_alert )) - new_actuators = actuators.copy() + new_actuators = actuators.as_builder() new_actuators.steeringAngleDeg = apply_angle self.frame += 1 diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 22a1475b5b..d89ae8c639 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -136,7 +136,7 @@ class CarController(CarControllerBase): if self.frame % 2 == 0: can_sends.append(subarucan.create_es_static_2(self.packer)) - new_actuators = actuators.copy() + new_actuators = actuators.as_builder() new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index f217c4692d..e460111e32 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -60,7 +60,7 @@ class CarController(CarControllerBase): # TODO: HUD control - new_actuators = actuators.copy() + new_actuators = actuators.as_builder() new_actuators.steeringAngleDeg = self.apply_angle_last self.frame += 1 diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 4bbecd99fe..dfdf44215d 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -91,14 +91,14 @@ class TestCarInterfaces: CC = car.CarControl.new_message(**cc_msg) for _ in range(10): car_interface.update(CC, []) - car_interface.apply(CC, now_nanos) + car_interface.apply(CC.as_reader(), now_nanos) now_nanos += DT_CTRL * 1e9 # 10 ms CC = car.CarControl.new_message(**cc_msg) CC.enabled = True for _ in range(10): car_interface.update(CC, []) - car_interface.apply(CC, now_nanos) + car_interface.apply(CC.as_reader(), now_nanos) now_nanos += DT_CTRL * 1e9 # 10ms # Test controller initialization diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 026693bdce..8807db69d9 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -15,7 +15,7 @@ from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import gen_empty_fingerprint -from openpilot.selfdrive.car.card import CarD +from openpilot.selfdrive.car.card import Car from openpilot.selfdrive.car.fingerprints import all_known_cars, MIGRATION from openpilot.selfdrive.car.car_helpers import FRAME_FINGERPRINT, interfaces from openpilot.selfdrive.car.honda.values import CAR as HONDA, HondaFlags @@ -215,7 +215,7 @@ class TestCarModelBase(unittest.TestCase): # TODO: also check for checksum violations from can parser can_invalid_cnt = 0 can_valid = False - CC = car.CarControl.new_message() + CC = car.CarControl.new_message().as_reader() for i, msg in enumerate(self.can_msgs): CS = self.CI.update(CC, (msg.as_builder().to_bytes(),)) @@ -308,17 +308,17 @@ class TestCarModelBase(unittest.TestCase): # Make sure we can send all messages while inactive CC = car.CarControl.new_message() - test_car_controller(CC) + test_car_controller(CC.as_reader()) # Test cancel + general messages (controls_allowed=False & cruise_engaged=True) self.safety.set_cruise_engaged_prev(True) CC = car.CarControl.new_message(cruiseControl={'cancel': True}) - test_car_controller(CC) + test_car_controller(CC.as_reader()) # Test resume + general messages (controls_allowed=True & cruise_engaged=True) self.safety.set_controls_allowed(True) CC = car.CarControl.new_message(cruiseControl={'resume': True}) - test_car_controller(CC) + test_car_controller(CC.as_reader()) # Skip stdout/stderr capture with pytest, causes elevated memory usage @pytest.mark.nocapture @@ -406,7 +406,7 @@ class TestCarModelBase(unittest.TestCase): controls_allowed_prev = False CS_prev = car.CarState.new_message() checks = defaultdict(int) - card = CarD(CI=self.CI) + card = Car(CI=self.CI) for idx, can in enumerate(self.can_msgs): CS = self.CI.update(CC, (can.as_builder().to_bytes(), )) for msg in filter(lambda m: m.src in range(64), can.can): diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 354d10a90e..f9b7a478e0 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -168,7 +168,7 @@ class CarController(CarControllerBase): if self.frame % 20 == 0 and self.CP.flags & ToyotaFlags.DISABLE_RADAR.value: can_sends.append([0x750, 0, b"\x0F\x02\x3E\x00\x00\x00\x00\x00", 0]) - new_actuators = actuators.copy() + new_actuators = actuators.as_builder() new_actuators.steer = apply_steer / self.params.STEER_MAX new_actuators.steerOutputCan = apply_steer new_actuators.steeringAngleDeg = self.last_angle diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 5fc47c51c6..a4e0c8946a 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -112,7 +112,7 @@ class CarController(CarControllerBase): can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, self.ext_bus, CS.gra_stock_values, cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume)) - new_actuators = actuators.copy() + new_actuators = actuators.as_builder() new_actuators.steer = self.apply_steer_last / self.CCP.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e3c456766e..d007cd9478 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,19 @@ 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 + if CI is None: + cloudlog.info("controlsd is waiting for CarParams") + 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() + cloudlog.info("controlsd got CarParams") + # Uses car interface helper functions, altering state won't be considered by card for actuation + self.CI = get_car_interface(self.CP) + else: + self.CI, self.CP = CI, CI.CP # Ensure the current branch is cached, otherwise the first iteration of controlsd lags self.branch = get_short_branch() @@ -83,6 +85,9 @@ class Controls: self.log_sock = messaging.sub_sock('androidLog') + # TODO: de-couple controlsd with card/conflate on carState without introducing controls mismatches + self.car_state_sock = messaging.sub_sock('carState', timeout=20) + ignore = self.sensor_packets + ['testJoystick'] if SIMULATION: ignore += ['driverCameraState', 'managerState'] @@ -110,6 +115,7 @@ class Controls: if not self.CP.openpilotLongitudinalControl: self.params.remove("ExperimentalMode") + self.CS_prev = car.CarState.new_message() self.AM = AlertManager() self.events = Events() @@ -161,7 +167,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): @@ -308,7 +314,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(): @@ -320,7 +326,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) @@ -380,9 +386,10 @@ class Controls: self.events.add(EventName.modeldLagging) def data_sample(self): - """Receive data from sockets and update carState""" + """Receive data from sockets""" - CS = self.card.state_update() + car_state = messaging.recv_one(self.car_state_sock) + CS = car_state.carState if car_state else self.CS_prev self.sm.update(0) @@ -396,9 +403,6 @@ 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() - self.initialized = True self.set_initial_state() self.params.put_bool_nonblocking("ControlsReady", True) @@ -709,7 +713,6 @@ class Controls: hudControl.visualAlert = current_alert.visual_alert if not self.CP.passive and self.initialized: - self.card.controls_update(CS, CC) CO = self.sm['carOutput'] if self.CP.steerControlType == car.CarParams.SteerControlType.angle: self.steer_limited = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ @@ -757,7 +760,6 @@ 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.experimentalMode = self.experimental_mode controlsState.personality = self.personality @@ -807,6 +809,8 @@ class Controls: # Publish data self.publish_logs(CS, start_time, CC, lac_log) + self.CS_prev = CS + def read_personality_param(self): try: return int(self.params.get('LongitudinalPersonality')) diff --git a/selfdrive/controls/tests/test_startup.py b/selfdrive/controls/tests/test_startup.py index 23cc96a2e4..5b69a7cd82 100644 --- a/selfdrive/controls/tests/test_startup.py +++ b/selfdrive/controls/tests/test_startup.py @@ -87,6 +87,7 @@ def test_startup_alert(expected_event, car_model, fw_versions, brand): os.environ['SKIP_FW_QUERY'] = '1' managed_processes['controlsd'].start() + managed_processes['card'].start() assert pm.wait_for_readers_to_update('can', 5) pm.send('can', can_list_to_can_capnp([[0, 0, b"", 0]])) @@ -104,7 +105,7 @@ def test_startup_alert(expected_event, car_model, fw_versions, brand): msgs = [[addr, 0, b'\x00'*length, 0] for addr, length in finger.items()] for _ in range(1000): - # controlsd waits for boardd to echo back that it has changed the multiplexing mode + # card waits for boardd to echo back that it has changed the multiplexing mode if not params.get_bool("ObdMultiplexingChanged"): params.put_bool("ObdMultiplexingChanged", True) diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index c2aa3b44e2..a8ab204da4 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), diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py index be78c92d73..c8d0504def 100644 --- a/selfdrive/test/process_replay/migration.py +++ b/selfdrive/test/process_replay/migration.py @@ -14,6 +14,7 @@ def migrate_all(lr, old_logtime=False, manager_states=False, panda_states=False, msgs = migrate_carParams(msgs, old_logtime) msgs = migrate_gpsLocation(msgs) msgs = migrate_deviceState(msgs) + msgs = migrate_carOutput(msgs) if manager_states: msgs = migrate_managerState(msgs) if panda_states: @@ -69,6 +70,23 @@ def migrate_deviceState(lr): return all_msgs +def migrate_carOutput(lr): + # migration needed only for routes before carOutput + if any(msg.which() == 'carOutput' for msg in lr): + return lr + + all_msgs = [] + for msg in lr: + if msg.which() == 'carControl': + co = messaging.new_message('carOutput') + co.valid = msg.valid + co.logMonoTime = msg.logMonoTime + co.carOutput.actuatorsOutput = msg.carControl.actuatorsOutputDEPRECATED + all_msgs.append(co.as_reader()) + all_msgs.append(msg) + return all_msgs + + def migrate_pandaStates(lr): all_msgs = [] # TODO: safety param migration should be handled automatically diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index f55f2c4e2a..fd1d753467 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -317,12 +317,12 @@ class ProcessContainer: return output_msgs -def controlsd_fingerprint_callback(rc, pm, msgs, fingerprint): +def card_fingerprint_callback(rc, pm, msgs, fingerprint): print("start fingerprinting") params = Params() canmsgs = [msg for msg in msgs if msg.which() == "can"][:300] - # controlsd expects one arbitrary can and pandaState + # card expects one arbitrary can and pandaState rc.send_sync(pm, "can", messaging.new_message("can", 1)) pm.send("pandaStates", messaging.new_message("pandaStates", 1)) rc.send_sync(pm, "can", messaging.new_message("can", 1)) @@ -356,12 +356,20 @@ def get_car_params_callback(rc, pm, msgs, fingerprint): for m in canmsgs[:300]: can.send(m.as_builder().to_bytes()) _, CP = get_car(can, sendcan, Params().get_bool("ExperimentalLongitudinalEnabled")) + + if not params.get_bool("DisengageOnAccelerator"): + CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS + params.put("CarParams", CP.to_bytes()) return CP def controlsd_rcv_callback(msg, cfg, frame): - # no sendcan until controlsd is initialized + return (frame - 1) == 0 or msg.which() == 'carState' + + +def card_rcv_callback(msg, cfg, frame): + # no sendcan until card is initialized if msg.which() != "can": return False @@ -461,18 +469,28 @@ CONFIGS = [ ProcessConfig( proc_name="controlsd", pubs=[ - "can", "deviceState", "pandaStates", "peripheralState", "liveCalibration", "driverMonitoringState", + "carState", "deviceState", "pandaStates", "peripheralState", "liveCalibration", "driverMonitoringState", "longitudinalPlan", "liveLocationKalman", "liveParameters", "radarState", "modelV2", "driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState", - "testJoystick", "liveTorqueParameters", "accelerometer", "gyroscope" + "testJoystick", "liveTorqueParameters", "accelerometer", "gyroscope", "carOutput" ], - subs=["controlsState", "carState", "carControl", "carOutput", "sendcan", "onroadEvents", "carParams"], + subs=["controlsState", "carControl", "onroadEvents"], ignore=["logMonoTime", "controlsState.startMonoTime", "controlsState.cumLagMs"], config_callback=controlsd_config_callback, - init_callback=controlsd_fingerprint_callback, + init_callback=get_car_params_callback, should_recv_callback=controlsd_rcv_callback, tolerance=NUMPY_TOLERANCE, processing_time=0.004, + ), + ProcessConfig( + proc_name="card", + pubs=["pandaStates", "carControl", "onroadEvents", "can"], + subs=["sendcan", "carState", "carParams", "carOutput"], + ignore=["logMonoTime", "carState.cumLagMs"], + init_callback=card_fingerprint_callback, + should_recv_callback=card_rcv_callback, + tolerance=NUMPY_TOLERANCE, + processing_time=0.004, main_pub="can", ), ProcessConfig( diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 2af30b5b47..dc7e59b54f 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -cc4e23ca0fceb300a3048c187ae9bc793794c095 \ No newline at end of file +c84b55c256ccb0ee042643a8a7f7f4dc429ff157 \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_fuzzy.py b/selfdrive/test/process_replay/test_fuzzy.py index d295092b20..6bcc94911f 100755 --- a/selfdrive/test/process_replay/test_fuzzy.py +++ b/selfdrive/test/process_replay/test_fuzzy.py @@ -12,7 +12,7 @@ import openpilot.selfdrive.test.process_replay.process_replay as pr # These processes currently fail because of unrealistic data breaking assumptions # that openpilot makes causing error with NaN, inf, int size, array indexing ... # TODO: Make each one testable -NOT_TESTED = ['controlsd', 'plannerd', 'calibrationd', 'dmonitoringd', 'paramsd', 'dmonitoringmodeld', 'modeld'] +NOT_TESTED = ['controlsd', 'card', 'plannerd', 'calibrationd', 'dmonitoringd', 'paramsd', 'dmonitoringmodeld', 'modeld'] TEST_CASES = [(cfg.proc_name, copy.deepcopy(cfg)) for cfg in pr.CONFIGS if cfg.proc_name not in NOT_TESTED] diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index b3dcbb03db..c0a425b599 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -41,23 +41,23 @@ source_segments = [ ] segments = [ - ("BODY", "regen02ECB79BACC|2024-05-18--04-29-29--0"), - ("HYUNDAI", "regen845DC1916E6|2024-05-18--04-30-52--0"), - ("HYUNDAI2", "regenBAE0915FE22|2024-05-18--04-33-11--0"), - ("TOYOTA", "regen1108D19FC2E|2024-05-18--04-34-34--0"), - ("TOYOTA2", "regen846521F39C7|2024-05-18--04-35-58--0"), - ("TOYOTA3", "regen788C3623D11|2024-05-18--04-38-21--0"), - ("HONDA", "regenDE43F170E99|2024-05-18--04-39-47--0"), - ("HONDA2", "regen1EE0FA383C3|2024-05-18--04-41-12--0"), - ("CHRYSLER", "regen9C5A30F471C|2024-05-18--04-42-36--0"), - ("RAM", "regenCCA313D117D|2024-05-18--04-44-53--0"), - ("SUBARU", "regenA41511F882A|2024-05-18--04-47-14--0"), - ("GM", "regen9D7B9CE4A66|2024-05-18--04-48-36--0"), - ("GM2", "regen07CECA52D41|2024-05-18--04-50-55--0"), - ("NISSAN", "regen2D6B856D0AE|2024-05-18--04-52-17--0"), - ("VOLKSWAGEN", "regen2D3AC6A6F05|2024-05-18--04-53-41--0"), - ("MAZDA", "regen0D5A777DD16|2024-05-18--04-56-02--0"), - ("FORD", "regen235D0937965|2024-05-18--04-58-16--0"), + ("BODY", "regen29FD9FF7760|2024-05-21--06-58-51--0"), + ("HYUNDAI", "regen0B1B76A1C27|2024-05-21--06-57-53--0"), + ("HYUNDAI2", "regen3BB55FA5E20|2024-05-21--06-59-03--0"), + ("TOYOTA", "regenF6FB954C1E2|2024-05-21--06-57-53--0"), + ("TOYOTA2", "regen0AC637CE7BA|2024-05-21--06-57-54--0"), + ("TOYOTA3", "regenC7BE3FAE496|2024-05-21--06-59-01--0"), + ("HONDA", "regen58E9F8B695A|2024-05-21--06-57-55--0"), + ("HONDA2", "regen8695608EB15|2024-05-21--06-57-55--0"), + ("CHRYSLER", "regenB0F8C25C902|2024-05-21--06-59-47--0"), + ("RAM", "regenB3B2C7A105B|2024-05-21--07-00-47--0"), + ("SUBARU", "regen860FD736DCC|2024-05-21--07-00-50--0"), + ("GM", "regen8CB3048DEB9|2024-05-21--06-59-49--0"), + ("GM2", "regen379D446541D|2024-05-21--07-00-51--0"), + ("NISSAN", "regen24871108F80|2024-05-21--07-00-38--0"), + ("VOLKSWAGEN", "regenF390392F275|2024-05-21--07-00-52--0"), + ("MAZDA", "regenE5A36020581|2024-05-21--07-01-51--0"), + ("FORD", "regenDC288ED0D78|2024-05-21--07-02-18--0"), ] # dashcamOnly makes don't need to be tested until a full port is done @@ -109,7 +109,7 @@ def test_process(cfg, lr, segment, ref_log_path, new_log_path, ignore_fields=Non if not check_openpilot_enabled(log_msgs): return f"Route did not enable at all or for long enough: {new_log_path}", log_msgs - if cfg.proc_name != 'ubloxd' or segment != 'regenBAE0915FE22|2024-05-18--04-33-11--0': + if cfg.proc_name != 'ubloxd' or segment != 'regen3BB55FA5E20|2024-05-21--06-59-03--0': seen_msgs = {m.which() for m in log_msgs} expected_msgs = set(cfg.subs) if seen_msgs != expected_msgs: diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 14f7773302..038bba7b2e 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -35,7 +35,8 @@ CPU usage budget MAX_TOTAL_CPU = 250. # total for all 8 cores PROCS = { # Baseline CPU usage by process - "selfdrive.controls.controlsd": 46.0, + "selfdrive.controls.controlsd": 32.0, + "selfdrive.car.card": 22.0, "./loggerd": 14.0, "./encoderd": 17.0, "./camerad": 14.5,