spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

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>
pull/20779/head
Adeeb Shihadeh 2021-04-30 13:55:17 -07:00 committed by GitHub
parent b4c21fe348
commit f9a961f6fb
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 47 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cereal

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

1
common/params_pyx.pyx
View File

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

10
selfdrive/boardd/boardd.cc
View File

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

1
selfdrive/boardd/tests/test_boardd_loopback.py
View File

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

32
selfdrive/controls/controlsd.py
View File

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

4
selfdrive/controls/lib/events.py
View File

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

5
selfdrive/test/process_replay/compare_logs.py
View File

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

13
selfdrive/test/process_replay/process_replay.py
View File

@ -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

2
selfdrive/test/process_replay/ref_commit
View File

@ -1 +1 @@
d48ac24ef8432244c8074337666c17bc9bfef442
8bc705247a6f316da0a74d28d393101e8dcfcf57