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 <device@comma.ai>
old-commit-hash: f9a961f6fb

cereal

@@ -1 +1 @@
-Subproject commit 9532238f186a5022235decc22f3eaf91b636cedc
+Subproject commit 2b286090032f671a55eeb838ad815551eab033aa

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],

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());

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')

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")

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",

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)):

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
 

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-d48ac24ef8432244c8074337666c17bc9bfef442
+8bc705247a6f316da0a74d28d393101e8dcfcf57