From f9a961f6fb69f36f8a113246920b2d71e6af8ae1 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 30 Apr 2021 13:55:17 -0700 Subject: [PATCH] delay controls start (#20761) * delay controls start * shouldn't need this stuff anymore * only delay board * bail after 2s * fix loopback test * fix process replay * update refs * bump cereal Co-authored-by: Comma Device --- cereal | 2 +- common/params_pyx.pyx | 1 + selfdrive/boardd/boardd.cc | 10 ++++-- .../boardd/tests/test_boardd_loopback.py | 1 + selfdrive/controls/controlsd.py | 32 +++++++++++-------- selfdrive/controls/lib/events.py | 4 +++ selfdrive/test/process_replay/compare_logs.py | 5 ++- .../test/process_replay/process_replay.py | 13 +++++--- selfdrive/test/process_replay/ref_commit | 2 +- 9 files changed, 47 insertions(+), 23 deletions(-) diff --git a/cereal b/cereal index 9532238f1..2b2860900 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 9532238f186a5022235decc22f3eaf91b636cedc +Subproject commit 2b286090032f671a55eeb838ad815551eab033aa diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index c300f954a..87d8c35f6 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -25,6 +25,7 @@ keys = { b"CarParamsCache": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], b"CarVin": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], b"CommunityFeaturesToggle": [TxType.PERSISTENT], + b"ControlsReady": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], b"EnableLteOnroad": [TxType.PERSISTENT], b"EndToEndToggle": [TxType.PERSISTENT], b"CompletedTrainingVersion": [TxType.PERSISTENT], diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 2abd69a71..dd5f22359 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -63,6 +63,8 @@ void safety_setter_thread() { // diagnostic only is the default, needed for VIN query panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327); + Params p = Params(); + // switch to SILENT when CarVin param is read while (true) { if (do_exit || !panda->connected){ @@ -70,7 +72,7 @@ void safety_setter_thread() { return; }; - std::string value_vin = Params().get("CarVin"); + std::string value_vin = p.get("CarVin"); if (value_vin.size() > 0) { // sanity check VIN format assert(value_vin.size() == 17); @@ -91,8 +93,10 @@ void safety_setter_thread() { return; }; - params = Params().get("CarParams"); - if (params.size() > 0) break; + if (p.getBool("ControlsReady")) { + params = p.get("CarParams"); + if (params.size() > 0) break; + } util::sleep_for(100); } LOGW("got %d bytes CarParams", params.size()); diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py index 3144274bd..267d396cf 100755 --- a/selfdrive/boardd/tests/test_boardd_loopback.py +++ b/selfdrive/boardd/tests/test_boardd_loopback.py @@ -48,6 +48,7 @@ def test_boardd_loopback(): cp = car.CarParams.new_message() cp.safetyModel = car.CarParams.SafetyModel.allOutput Params().put("CarVin", b"0"*17) + Params().put_bool("ControlsReady", True) Params().put("CarParams", cp.to_bytes()) sendcan = messaging.pub_sock('sendcan') diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index ab1cb685c..661d87536 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -96,7 +96,7 @@ class Controls: if self.read_only: self.CP.safetyModel = car.CarParams.SafetyModel.noOutput - # Write CarParams for radard and boardd safety mode + # Write CarParams for radard cp_bytes = self.CP.to_bytes() params.put("CarParams", cp_bytes) put_nonblocking("CarParamsCache", cp_bytes) @@ -117,6 +117,7 @@ class Controls: elif self.CP.lateralTuning.which() == 'lqr': self.LaC = LatControlLQR(self.CP) + self.initialized = False self.state = State.disabled self.enabled = False self.active = False @@ -134,11 +135,7 @@ class Controls: self.current_alert_types = [ET.PERMANENT] self.logged_comm_issue = False - self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED - self.sm['deviceState'].freeSpacePercent = 100 - self.sm['driverMonitoringState'].events = [] - self.sm['driverMonitoringState'].awarenessStatus = 1. - self.sm['driverMonitoringState'].faceDetected = False + # TODO: no longer necessary, aside from process replay self.sm['liveParameters'].valid = True self.startup_event = get_startup_event(car_recognized, controller_available, fuzzy_fingerprint) @@ -168,6 +165,11 @@ class Controls: self.events.add(self.startup_event) self.startup_event = None + # Don't add any more events if not initialized + if not self.initialized: + self.events.add(EventName.controlsInitializing) + return + # Create events for battery, temperature, disk space, and memory if self.sm['deviceState'].batteryPercent < 1 and self.sm['deviceState'].chargingError: # at zero percent battery, while discharging, OP should not allowed @@ -211,12 +213,11 @@ class Controls: LaneChangeState.laneChangeFinishing]: self.events.add(EventName.laneChange) - if self.can_rcv_error or (not CS.canValid and self.sm.frame > 5 / DT_CTRL): + if self.can_rcv_error or not CS.canValid: self.events.add(EventName.canError) - safety_mismatch = self.sm['pandaState'].safetyModel != self.CP.safetyModel - safety_mismatch = safety_mismatch or self.sm['pandaState'].safetyParam != self.CP.safetyParam - if (safety_mismatch and self.sm.frame > 2 / DT_CTRL) or self.mismatch_counter >= 200: + safety_mismatch = self.sm['pandaState'].safetyModel != self.CP.safetyModel or self.sm['pandaState'].safetyParam != self.CP.safetyParam + if safety_mismatch or self.mismatch_counter >= 200: self.events.add(EventName.controlsMismatch) if not self.sm['liveParameters'].valid: @@ -253,7 +254,7 @@ class Controls: (not TICI or self.enable_lte_onroad): # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes self.events.add(EventName.noGps) - if not self.sm.all_alive(['roadCameraState', 'driverCameraState']) and (self.sm.frame > 5 / DT_CTRL): + if not self.sm.all_alive(['roadCameraState', 'driverCameraState']): self.events.add(EventName.cameraMalfunction) if self.sm['modelV2'].frameDropPerc > 20: self.events.add(EventName.modeldLagging) @@ -277,6 +278,11 @@ class Controls: self.sm.update(0) + all_valid = CS.canValid and self.sm.all_alive_and_valid() + if not self.initialized and (all_valid or self.sm.frame * DT_CTRL > 2.0): + self.initialized = True + Params().put_bool("ControlsReady", True) + # Check for CAN timeout if not can_strs: self.can_error_counter += 1 @@ -484,7 +490,7 @@ class Controls: self.AM.process_alerts(self.sm.frame, clear_event) CC.hudControl.visualAlert = self.AM.visual_alert - if not self.read_only: + if not self.read_only and self.initialized: # send car controls over can can_sends = self.CI.apply(CC) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) @@ -584,7 +590,7 @@ class Controls: self.update_events(CS) - if not self.read_only: + if not self.read_only and self.initialized: # Update control state self.state_transition(CS) self.prof.checkpoint("State transition") diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index eca43c473..0e409b352 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -228,6 +228,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo Priority.LOW, VisualAlert.none, AudibleAlert.none, .1, .1, .1), }, + EventName.controlsInitializing: { + ET.NO_ENTRY: NoEntryAlert("Controls Initializing"), + }, + EventName.startup: { ET.PERMANENT: Alert( "Be ready to take over at any time", diff --git a/selfdrive/test/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py index 5327d0ebd..f3b191450 100755 --- a/selfdrive/test/process_replay/compare_logs.py +++ b/selfdrive/test/process_replay/compare_logs.py @@ -4,6 +4,7 @@ import os import sys import numbers import dictdiffer +from collections import Counter if "CI" in os.environ: def tqdm(x): @@ -60,7 +61,9 @@ def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=Non log1, log2 = [list(filter(lambda m: m.which() not in ignore_msgs, log)) for log in (log1, log2)] if len(log1) != len(log2): - raise Exception(f"logs are not same length: {len(log1)} VS {len(log2)}") + cnt1 = Counter([m.which() for m in log1]) + cnt2 = Counter([m.which() for m in log2]) + raise Exception(f"logs are not same length: {len(log1)} VS {len(log2)}\n\t\t{cnt1}\n\t\t{cnt2}") diff = [] for msg1, msg2 in tqdm(zip(log1, log2)): diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 3abf6cf71..43c3d8780 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -90,7 +90,6 @@ class FakeSubMaster(messaging.SubMaster): self.sock = {s: DumbSocket(s) for s in services} self.update_called = threading.Event() self.update_ready = threading.Event() - self.wait_on_getitem = False def __getitem__(self, s): @@ -183,6 +182,13 @@ def get_car_params(msgs, fsm, can_sock): _, CP = get_car(can, sendcan) Params().put("CarParams", CP.to_bytes()) +def controlsd_rcv_callback(msg, CP, cfg, fsm): + # no sendcan until controlsd is initialized + socks = [s for s in cfg.pub_sub[msg.which()] if + (fsm.frame + 1) % int(service_list[msg.which()].frequency / service_list[s].frequency) == 0] + if "sendcan" in socks and fsm.frame < 2000: + socks.remove("sendcan") + return socks, len(socks) > 0 def radar_rcv_callback(msg, CP, cfg, fsm): if msg.which() != "can": @@ -232,7 +238,7 @@ CONFIGS = [ }, ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], init_callback=fingerprint, - should_recv_callback=None, + should_recv_callback=controlsd_rcv_callback, tolerance=NUMPY_TOLERANCE, fake_pubsubmaster=True, ), @@ -388,7 +394,7 @@ def python_replay_process(cfg, lr): recv_socks, should_recv = cfg.should_recv_callback(msg, CP, cfg, fsm) else: recv_socks = [s for s in cfg.pub_sub[msg.which()] if - (fsm.frame + 1) % int(service_list[msg.which()].frequency / service_list[s].frequency) == 0] + (fsm.frame + 1) % int(service_list[msg.which()].frequency / service_list[s].frequency) == 0] should_recv = bool(len(recv_socks)) if msg.which() == 'can': @@ -404,7 +410,6 @@ def python_replay_process(cfg, lr): while recv_cnt > 0: m = fpm.wait_for_msg() log_msgs.append(m) - recv_cnt -= m.which() in recv_socks return log_msgs diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 1a93c3eb8..2ab9488bc 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -d48ac24ef8432244c8074337666c17bc9bfef442 \ No newline at end of file +8bc705247a6f316da0a74d28d393101e8dcfcf57 \ No newline at end of file