Merge remote-tracking branch 'upstream/master' into draw-map-with-ui

pull/24632/head
Shane Smiskol 3 years ago
commit 59bb3c2364
  1. 1
      common/params.cc
  2. 10
      selfdrive/athena/athenad.py
  3. 1
      selfdrive/car/hyundai/values.py
  4. 5
      selfdrive/car/toyota/interface.py
  5. 12
      selfdrive/car/toyota/tunes.py
  6. 2
      selfdrive/car/toyota/values.py
  7. 4
      selfdrive/controls/lib/latcontrol_torque.py
  8. 13
      selfdrive/debug/test_fw_query_on_routes.py
  9. 25
      selfdrive/hardware/tici/hardware.py
  10. 2
      selfdrive/test/process_replay/ref_commit
  11. 30
      selfdrive/test/process_replay/test_processes.py
  12. 2
      selfdrive/ui/qt/sidebar.cc
  13. 111
      tools/latencylogger/README.md
  14. 16
      tools/latencylogger/latency_logger.py

@ -144,7 +144,6 @@ std::unordered_map<std::string, uint32_t> keys = {
{"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"PandaSignatures", CLEAR_ON_MANAGER_START},
{"Passive", PERSISTENT},
{"PrimeRedirected", PERSISTENT},
{"PrimeType", PERSISTENT},
{"RecordFront", PERSISTENT},
{"RecordFrontLock", PERSISTENT}, // for the internal fleet

@ -725,7 +725,6 @@ def main():
enable_multithread=True,
timeout=30.0)
cloudlog.event("athenad.main.connected_ws", ws_uri=ws_uri)
params.delete("PrimeRedirected")
conn_retries = 0
cur_upload_items.clear()
@ -735,22 +734,13 @@ def main():
break
except (ConnectionError, TimeoutError, WebSocketException):
conn_retries += 1
params.delete("PrimeRedirected")
params.delete("LastAthenaPingTime")
except socket.timeout:
try:
r = requests.get("http://api.commadotai.com/v1/me", allow_redirects=False,
headers={"User-Agent": f"openpilot-{get_version()}"}, timeout=15.0)
if r.status_code == 302 and r.headers['Location'].startswith("http://u.web2go.com"):
params.put_bool("PrimeRedirected", True)
except Exception:
cloudlog.exception("athenad.socket_timeout.exception")
params.delete("LastAthenaPingTime")
except Exception:
cloudlog.exception("athenad.main.exception")
conn_retries += 1
params.delete("PrimeRedirected")
params.delete("LastAthenaPingTime")
time.sleep(backoff(conn_retries))

@ -946,6 +946,7 @@ FW_VERSIONS = {
b'\xf1\x8799110Q4000\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 ',
b'\xf1\x8799110Q4100\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4100 ',
b'\xf1\x8799110Q4500\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4500 ',
b'\xf1\x8799110Q4600\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4600 ',
b'\xf1\x8799110Q4600\xf1\x00DEev SCC FNCUP 1.00 1.00 99110-Q4600 ',
b'\xf1\x8799110Q4600\xf1\x00DEev SCC FHCUP 1.00 1.00 99110-Q4600 ',
],

@ -38,8 +38,7 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 15.74 # unknown end-to-end spec
tire_stiffness_factor = 0.6371 # hand-tune
ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS)
ret.steerActuatorDelay = 0.3
set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.7, FRICTION=0.06)
elif candidate == CAR.PRIUS_V:
stop_and_go = True
@ -62,7 +61,7 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 18.27
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
set_lat_tune(ret.lateralTuning, LatTunes.PID_A)
set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.8, FRICTION=0.024)
elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RXH, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2):
stop_and_go = True

@ -50,19 +50,9 @@ def set_long_tune(tune, name):
###### LAT ######
def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=.1):
def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=.1, use_steering_angle=True):
if name == LatTunes.TORQUE:
set_torque_tune(tune, MAX_LAT_ACCEL, FRICTION)
elif name == LatTunes.INDI_PRIUS:
tune.init('indi')
tune.indi.innerLoopGainBP = [0.]
tune.indi.innerLoopGainV = [4.0]
tune.indi.outerLoopGainBP = [0.]
tune.indi.outerLoopGainV = [3.0]
tune.indi.timeConstantBP = [0.]
tune.indi.timeConstantV = [1.0]
tune.indi.actuatorEffectivenessBP = [0.]
tune.indi.actuatorEffectivenessV = [1.0]
elif 'PID' in str(name):
tune.init('pid')
tune.pid.kiBP = [0.0]

@ -963,10 +963,12 @@ FW_VERSIONS = {
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301400\x00\x00\x00\x00',
b'\x018821F6201200\x00\x00\x00\x00',
b'\x018821F6201300\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F0E02100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
b'\x028646F4803000\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
b'\x028646F4803000\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
],
},
CAR.LEXUS_IS: {

@ -55,7 +55,9 @@ class LatControlTorque(LatControl):
if self.use_steering_angle:
actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
else:
actual_curvature = llk.angularVelocityCalibrated.value[2] / CS.vEgo
actual_curvature_vm = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo
actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk])
desired_lateral_accel = desired_curvature * CS.vEgo ** 2
desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2
actual_lateral_accel = actual_curvature * CS.vEgo ** 2

@ -73,6 +73,7 @@ if __name__ == "__main__":
lr = LogReader(qlog_path)
dongles.append(dongle_id)
CP = None
for msg in lr:
if msg.which() == "pandaStates":
if msg.pandaStates[0].pandaType not in ('uno', 'blackPanda', 'dos'):
@ -80,14 +81,13 @@ if __name__ == "__main__":
break
elif msg.which() == "carParams":
bts = msg.carParams.as_builder().to_bytes()
car_fw = msg.carParams.carFw
CP = msg.carParams
car_fw = CP.carFw
if len(car_fw) == 0:
print("no fw")
break
live_fingerprint = msg.carParams.carFingerprint
live_fingerprint = CP.carFingerprint
live_fingerprint = migration.get(live_fingerprint, live_fingerprint)
if args.car is not None:
@ -116,7 +116,7 @@ if __name__ == "__main__":
break
print(f"{dongle_id}|{time}")
print("Old style:", live_fingerprint, "Vin", msg.carParams.carVin)
print("Old style:", live_fingerprint, "Vin", CP.carVin)
print("New style (exact):", exact_matches)
print("New style (fuzzy):", fuzzy_matches)
@ -164,6 +164,9 @@ if __name__ == "__main__":
print("Fuzzy match wrong! Fuzzy:", fuzzy_matches, "Live:", live_fingerprint)
break
if CP is None:
print("no CarParams in logs")
except Exception:
traceback.print_exc()
except KeyboardInterrupt:

@ -457,22 +457,23 @@ class Tici(HardwareBase):
def configure_modem(self):
sim_id = self.get_sim_info().get('sim_id', '')
# configure modem as data-centric
cmds = [
'AT+QNVW=5280,0,"0102000000000000"',
'AT+QNVFW="/nv/item_files/ims/IMS_enable",00',
'AT+QNVFW="/nv/item_files/modem/mmode/ue_usage_setting",01',
]
modem = self.get_modem()
for cmd in cmds:
try:
modem.Command(cmd, math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
except Exception:
pass
# blue prime config
if sim_id.startswith('8901410'):
cmds = [
'AT+QNVW=5280,0,"0102000000000000"',
'AT+QNVFW="/nv/item_files/ims/IMS_enable",00',
'AT+QNVFW="/nv/item_files/modem/mmode/ue_usage_setting",01',
]
modem = self.get_modem()
for cmd in cmds:
try:
modem.Command(cmd, math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
except Exception:
pass
os.system('mmcli -m 0 --3gpp-set-initial-eps-bearer-settings="apn=Broadband"')
def get_networks(self):
r = {}

@ -1 +1 @@
336d77ad17b90af17b7eb24cc832e80b62d05a24
0956446adfa91506f0a3d88f893e041bfb2890c1

@ -57,13 +57,13 @@ REF_COMMIT_FN = os.path.join(PROC_REPLAY_DIR, "ref_commit")
def run_test_process(data):
segment, cfg, args, cur_log_fn, lr, ref_commit = data
segment, cfg, args, cur_log_fn, ref_log_path, lr = data
res = None
if not args.upload_only:
ref_log_fn = os.path.join(PROC_REPLAY_DIR, f"{segment}_{cfg.proc_name}_{ref_commit}.bz2")
res, log_msgs = test_process(cfg, lr, ref_log_fn, args.ignore_fields, args.ignore_msgs)
res, log_msgs = test_process(cfg, lr, ref_log_path, args.ignore_fields, args.ignore_msgs)
# save logs so we can upload when updating refs
save_log(cur_log_fn, log_msgs)
if args.update_refs or args.upload_only:
print(f'Uploading: {os.path.basename(cur_log_fn)}')
assert os.path.exists(cur_log_fn), f"Cannot find log to upload: {cur_log_fn}"
@ -78,13 +78,12 @@ def get_logreader(segment):
return (segment, lr)
def test_process(cfg, lr, ref_log_fn, ignore_fields=None, ignore_msgs=None):
def test_process(cfg, lr, ref_log_path, ignore_fields=None, ignore_msgs=None):
if ignore_fields is None:
ignore_fields = []
if ignore_msgs is None:
ignore_msgs = []
ref_log_path = ref_log_fn if os.path.exists(ref_log_fn) else BASE_URL + os.path.basename(ref_log_fn)
ref_log_msgs = list(LogReader(ref_log_path))
log_msgs = replay_process(cfg, lr)
@ -92,7 +91,7 @@ def test_process(cfg, lr, ref_log_fn, ignore_fields=None, ignore_msgs=None):
# check to make sure openpilot is engaged in the route
if cfg.proc_name == "controlsd":
if not check_enabled(log_msgs):
segment = ref_log_fn.split("/")[-1].split("_")[0]
segment = os.path.basename(ref_log_path).split("/")[-1].split("_")[0]
raise Exception(f"Route never enabled: {segment}")
try:
@ -191,13 +190,21 @@ if __name__ == "__main__":
if (len(args.whitelist_cars) and car_brand.upper() not in args.whitelist_cars) or \
(not len(args.whitelist_cars) and car_brand.upper() in args.blacklist_cars):
continue
for cfg in CONFIGS:
if (len(args.whitelist_procs) and cfg.proc_name not in args.whitelist_procs) or \
(not len(args.whitelist_procs) and cfg.proc_name in args.blacklist_procs):
continue
cur_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{cur_commit}.bz2")
if args.update_refs: # reference logs will not exist if routes were just regenerated
ref_log_path = get_url(*segment.rsplit("--", 1))
else:
ref_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{ref_commit}.bz2")
ref_log_path = ref_log_fn if os.path.exists(ref_log_fn) else BASE_URL + os.path.basename(ref_log_fn)
lr = None if args.upload_only else lreaders[segment]
pool_args.append((segment, cfg, args, cur_log_fn, lr, ref_commit))
pool_args.append((segment, cfg, args, cur_log_fn, ref_log_path, lr))
results: Any = defaultdict(dict)
p2 = pool.map(run_test_process, pool_args)
@ -206,20 +213,19 @@ if __name__ == "__main__":
results[segment][proc] = result
diff1, diff2, failed = format_diff(results, ref_commit)
if not args.upload_only:
if not upload:
with open(os.path.join(PROC_REPLAY_DIR, "diff.txt"), "w") as f:
f.write(diff2)
print(diff1)
if failed:
print("TEST FAILED")
if not args.update_refs:
print("\n\nTo push the new reference logs for this commit run:")
print("./test_processes.py --upload-only")
print("\n\nTo push the new reference logs for this commit run:")
print("./test_processes.py --upload-only")
else:
print("TEST SUCCEEDED")
if upload:
else:
with open(REF_COMMIT_FN, "w") as f:
f.write(cur_commit)
print(f"\n\nUpdated reference logs for commit: {cur_commit}")

@ -57,7 +57,7 @@ void Sidebar::updateState(const UIState &s) {
ItemStatus connectStatus;
auto last_ping = deviceState.getLastAthenaPingTime();
if (last_ping == 0) {
connectStatus = params.getBool("PrimeRedirected") ? ItemStatus{"NO\nPRIME", danger_color} : ItemStatus{"CONNECT\nOFFLINE", warning_color};
connectStatus = ItemStatus{"CONNECT\nOFFLINE", warning_color};
} else {
connectStatus = nanos_since_boot() - last_ping < 80e9 ? ItemStatus{"CONNECT\nONLINE", good_color} : ItemStatus{"CONNECT\nERROR", danger_color};
}

@ -19,77 +19,74 @@ optional arguments:
--relative Make timestamps relative to the start of each frame (default: False)
--demo Use the demo route instead of providing one (default: False)
--plot If a plot should be generated (default: False)
--offset Offset service to better visualize overlap (default: False)
```
To timestamp an event, use `LOGT("msg")` in c++ code or `cloudlog.timestamp("msg")` in python code. If the print is warning for frameId assignment ambiguity, use `LOGT(frameId ,"msg")`.
## Examples
Plotting with relative starts each process at time=0 and gives a nice overview. Timestamps are visualized as diamonds. The opacity allows for visualization of overlapping services.
![relplot-1](https://user-images.githubusercontent.com/42323981/162108651-e0beee14-56e4-466d-8af1-cb37129fd94a.png)
Plotting without relative provides info about the frames relative time.
![plot-1](https://user-images.githubusercontent.com/42323981/162108694-fbfe907b-a1ee-4cc7-bc8b-162a7d9305d4.png)
Timestamps are visualized as diamonds
| | Relative | Absolute |
| ------------- | ------------- | ------------- |
| Inline | ![inrel](https://user-images.githubusercontent.com/42323981/170559939-465df3b1-bf87-46d5-b5ee-5cc87dc49470.png) | ![inabs](https://user-images.githubusercontent.com/42323981/170559985-a82f87e7-82c4-4e48-a348-4221568dd589.png) |
| Offset | ![offrel](https://user-images.githubusercontent.com/42323981/170559854-93fba90f-acc4-4d08-b317-d3f8fc649ea8.png) | ![offabs](https://user-images.githubusercontent.com/42323981/170559782-06ed5599-d4e3-4701-ad78-5c1eec6cb61e.png) |
Printed timestamps of a frame with internal durations.
```
Frame ID: 371
Frame ID: 1202
camerad
wideRoadCameraState start of frame 0.0
roadCameraState start of frame 0.072395
wideRoadCameraState published 47.804745
WideRoadCamera: Image set 47.839849
roadCameraState published 48.319166
RoadCamera: Image set 48.354478
RoadCamera: Transformed 48.430258
wideRoadCameraState.processingTime 16.733376309275627
roadCameraState.processingTime 16.218071803450584
roadCameraState start of frame 0.049583
wideRoadCameraState published 35.01206
WideRoadCamera: Image set 35.020028
roadCameraState published 38.508261
RoadCamera: Image set 38.520344
RoadCamera: Transformed 38.616176
wideRoadCameraState.processingTime 3.152403049170971
roadCameraState.processingTime 6.453451234847307
modeld
Image added 51.346522
Extra image added 53.179467
Execution finished 71.584437
modelV2 published 71.76881
modelV2.modelExecutionTime 22.54236489534378
Image added 40.909841
Extra image added 42.515027
Execution finished 63.002552
modelV2 published 63.148747
modelV2.modelExecutionTime 23.62649142742157
modelV2.gpuExecutionTime 0.0
plannerd
lateralPlan published 77.381862
longitudinalPlan published 84.207972
lateralPlan.solverExecutionTime 1.3547739945352077
longitudinalPlan.solverExecutionTime 2.0179999992251396
lateralPlan published 66.915049
longitudinalPlan published 69.715999
lateralPlan.solverExecutionTime 0.8170719956979156
longitudinalPlan.solverExecutionTime 0.5619999719783664
controlsd
Data sampled 78.909759
Events updated 79.711884
sendcan published 80.721038
controlsState published 81.081398
Data sampled 88.663748
Events updated 89.535403
sendcan published 90.587889
controlsState published 91.019707
Data sampled 98.667003
Events updated 99.661261
sendcan published 100.776507
controlsState published 101.198794
Data sampled 108.967078
Events updated 109.95842
sendcan published 111.263142
controlsState published 111.678085
Data sampled 118.574923
Events updated 119.608555
sendcan published 120.73427
controlsState published 121.111036
Data sampled 128.596408
Events updated 129.382283
sendcan published 130.330083
controlsState published 130.676485
Data sampled 70.217763
Events updated 71.037178
sendcan published 72.278775
controlsState published 72.825226
Data sampled 80.008354
Events updated 80.787666
sendcan published 81.849682
controlsState published 82.238323
Data sampled 90.521123
Events updated 91.626003
sendcan published 93.413218
controlsState published 94.143989
Data sampled 100.991497
Events updated 101.973774
sendcan published 103.565575
controlsState published 104.146088
Data sampled 110.284387
Events updated 111.183541
sendcan published 112.981692
controlsState published 113.731994
boardd
sending sendcan to panda: 250027001751393037323631 90.7257
sendcan sent to panda: 250027001751393037323631 91.078143
sending sendcan to panda: 250027001751393037323631 100.941766
sendcan sent to panda: 250027001751393037323631 101.306865
sending sendcan to panda: 250027001751393037323631 111.411786
sendcan sent to panda: 250027001751393037323631 111.754074
sending sendcan to panda: 250027001751393037323631 120.875987
sendcan sent to panda: 250027001751393037323631 121.188535
sending sendcan to panda: 250027001751393037323631 130.454248
sendcan sent to panda: 250027001751393037323631 130.757994
sending sendcan to panda: 250027001751393037323631 140.353234
sending sendcan to panda: 250027001751393037323631 81.928119
sendcan sent to panda: 250027001751393037323631 82.164834
sending sendcan to panda: 250027001751393037323631 93.569986
sendcan sent to panda: 250027001751393037323631 93.92795
sending sendcan to panda: 250027001751393037323631 103.689167
sendcan sent to panda: 250027001751393037323631 104.012235
sending sendcan to panda: 250027001751393037323631 113.109555
sendcan sent to panda: 250027001751393037323631 113.525487
sending sendcan to panda: 250027001751393037323631 122.508434
sendcan sent to panda: 250027001751393037323631 122.834314
```

@ -175,17 +175,18 @@ def print_timestamps(timestamps, durations, start_times, relative):
for event, time in durations[frame_id][service]:
print(" "+'%-53s%-53s' %(event, str(time*1000)))
def graph_timestamps(timestamps, start_times, end_times, relative, title=""):
def graph_timestamps(timestamps, start_times, end_times, relative, offset_services=False, title=""):
# mpld3 doesn't convert properly to D3 font sizes
plt.rcParams.update({'font.size': 18})
t0 = find_t0(start_times)
fig, ax = plt.subplots()
ax.set_xlim(0, 150 if relative else 750)
ax.set_ylim(0, 15)
ax.set_xlim(0, 130 if relative else 750)
ax.set_ylim(0, 17)
ax.set_xlabel('Time (milliseconds)')
ax.set_ylabel('Frame ID')
colors = ['blue', 'green', 'red', 'yellow', 'purple']
offsets = [[0, -5*j] for j in range(len(SERVICES))] if offset_services else None
height = 0.3 if offset_services else 0.9
assert len(colors) == len(SERVICES), 'Each service needs a color'
points = {"x": [], "y": [], "labels": []}
@ -202,16 +203,16 @@ def graph_timestamps(timestamps, start_times, end_times, relative, title=""):
points['x'].append((event[1]-t0)/1e6)
points['y'].append(i)
points['labels'].append(event[0])
ax.broken_barh(service_bars, (i-0.45, 0.9), facecolors=(colors), alpha=0.5)
ax.broken_barh(service_bars, (i-height/2, height), facecolors=(colors), alpha=0.5, offsets=offsets)
scatter = ax.scatter(points['x'], points['y'], marker='d', edgecolor='black')
tooltip = mpld3.plugins.PointLabelTooltip(scatter, labels=points['labels'])
mpld3.plugins.connect(fig, tooltip)
plt.title(title)
# Set size relative window size is not trivial: https://github.com/mpld3/mpld3/issues/65
fig.set_size_inches(18, 9)
plt.legend(handles=[mpatches.Patch(color=colors[i], label=SERVICES[i]) for i in range(len(SERVICES))])
return fig
def get_timestamps(lr):
@ -226,6 +227,7 @@ if __name__ == "__main__":
parser.add_argument("--relative", action="store_true", help="Make timestamps relative to the start of each frame")
parser.add_argument("--demo", action="store_true", help="Use the demo route instead of providing one")
parser.add_argument("--plot", action="store_true", help="If a plot should be generated")
parser.add_argument("--offset", action="store_true", help="Vertically offset service to better visualize overlap")
parser.add_argument("route_or_segment_name", nargs='?', help="The route to print")
if len(sys.argv) == 1:
@ -239,4 +241,4 @@ if __name__ == "__main__":
data, _ = get_timestamps(lr)
print_timestamps(data['timestamp'], data['duration'], data['start'], args.relative)
if args.plot:
mpld3.show(graph_timestamps(data['timestamp'], data['start'], data['end'], args.relative, r))
mpld3.show(graph_timestamps(data['timestamp'], data['start'], data['end'], args.relative, offset_services=args.offset, title=r))

Loading…
Cancel
Save