selfdrive/debug

5 years ago · f467642a1c
parent da079d47d7
commit f467642a1c
66 changed files with 4113 additions and 0 deletions
--- a/selfdrive/debug/init.py
+++ b/selfdrive/debug/init.py
--- a/selfdrive/debug/can_printer.py
+++ b/selfdrive/debug/can_printer.py
@ -0,0 +1,42 @@
+#!/usr/bin/env python3
+import binascii
+import os
+import sys
+from collections import defaultdict
+
+import cereal.messaging as messaging
+from common.realtime import sec_since_boot
+
+
+def can_printer(bus=0, max_msg=None, addr="127.0.0.1"):
+  logcan = messaging.sub_sock('can', addr=addr)
+
+  start = sec_since_boot()
+  lp = sec_since_boot()
+  msgs = defaultdict(list)
+  canbus = int(os.getenv("CAN", bus))
+  while 1:
+    can_recv = messaging.drain_sock(logcan, wait_for_one=True)
+    for x in can_recv:
+      for y in x.can:
+        if y.src == canbus:
+          msgs[y.address].append(y.dat)
+
+    if sec_since_boot() - lp > 0.1:
+      dd = chr(27) + "[2J"
+      dd += "%5.2f\n" % (sec_since_boot() - start)
+      for k,v in sorted(zip(msgs.keys(), map(lambda x: binascii.hexlify(x[-1]), msgs.values()))):
+        if max_msg is None or k < max_msg:
+          dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k,k),len(msgs[k]), v.decode('ascii'))
+      print(dd)
+      lp = sec_since_boot()
+
+if __name__ == "__main__":
+  if len(sys.argv) > 3:
+    can_printer(int(sys.argv[1]), int(sys.argv[2]), sys.argv[3])
+  elif len(sys.argv) > 2:
+    can_printer(int(sys.argv[1]), int(sys.argv[2]))
+  elif len(sys.argv) > 1:
+    can_printer(int(sys.argv[1]))
+  else:
+    can_printer()
--- a/selfdrive/debug/check_freq.py
+++ b/selfdrive/debug/check_freq.py
@ -0,0 +1,43 @@
+#!/usr/bin/env python3
+import argparse
+import numpy as np
+from collections import defaultdict, deque
+from common.realtime import sec_since_boot
+import cereal.messaging as messaging
+
+
+if __name__ == "__main__":
+  context = messaging.Context()
+  poller = messaging.Poller()
+
+  parser = argparse.ArgumentParser()
+  parser.add_argument("socket", type=str, nargs='*', help="socket name")
+  args = parser.parse_args()
+
+  socket_names = args.socket
+  sockets = {}
+
+  rcv_times = defaultdict(lambda: deque(maxlen=100))
+
+  t = sec_since_boot()
+  for name in socket_names:
+    sock = messaging.sub_sock(name, poller=poller)
+    sockets[sock] = name
+
+  prev_print = t
+  while True:
+    for socket in poller.poll(100):
+      msg = messaging.recv_one(socket)
+      name = msg.which()
+
+      t = sec_since_boot()
+      rcv_times[name].append(msg.logMonoTime / 1e9)
+
+    if t - prev_print > 1:
+      print()
+      for name in socket_names:
+        dts = np.diff(rcv_times[name])
+        mean = np.mean(dts)
+        print("%s: Freq %.2f Hz, Min %.2f%%, Max %.2f%%" % (name, 1.0 / mean, np.min(dts) / mean * 100, np.max(dts) / mean * 100))
+
+      prev_print = t
--- a/selfdrive/debug/compare_fingerprints.py
+++ b/selfdrive/debug/compare_fingerprints.py
@ -0,0 +1,18 @@
+#!/usr/bin/env python3
+
+# put 2 fingeprints and print the diffs
+f1 = {
+168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8
+}
+
+f2 = {
+168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8
+}
+
+for k in f1:
+  if k not in f2 or f1[k] != f2[k]:
+    print(k, "not in f2")
+
+for k in f2:
+  if k not in f1 or f2[k] != f1[k]:
+    print(k, "not in f1")
--- a/selfdrive/debug/cpu_usage_stat.py
+++ b/selfdrive/debug/cpu_usage_stat.py
@ -0,0 +1,120 @@
+#!/usr/bin/env python3
+import psutil
+import time
+import os
+import sys
+import numpy as np
+import argparse
+import re
+from collections import defaultdict
+
+'''
+System tools like top/htop can only show current cpu usage values, so I write this script to do statistics jobs.
+  Features:
+    Use psutil library to sample cpu usage(avergage for all cores) of OpenPilot processes, at a rate of 5 samples/sec.
+    Do cpu usage statistics periodically, 5 seconds as a cycle.
+    Caculate the average cpu usage within this cycle.
+    Caculate minumium/maximium/accumulated_average cpu usage as long term inspections.
+    Monitor multiple processes simuteneously.
+  Sample usage:
+    root@localhost:/data/openpilot$ python selfdrive/debug/cpu_usage_stat.py boardd,ubloxd
+    ('Add monitored proc:', './boardd')
+    ('Add monitored proc:', 'python locationd/ubloxd.py')
+    boardd: 1.96%, min: 1.96%, max: 1.96%, acc: 1.96%
+    ubloxd.py: 0.39%, min: 0.39%, max: 0.39%, acc: 0.39%
+'''
+
+# Do statistics every 5 seconds
+PRINT_INTERVAL = 5
+SLEEP_INTERVAL = 0.2
+
+monitored_proc_names = [
+  'ubloxd', 'thermald', 'uploader', 'deleter', 'controlsd', 'plannerd', 'radard', 'mapd', 'loggerd' , 'logmessaged', 'tombstoned',
+  'logcatd', 'proclogd', 'boardd', 'pandad', './ui', 'ui',  'calibrationd', 'params_learner', 'modeld', 'monitoringd', 'camerad', 'sensord', 'updated', 'gpsd', 'athena']
+cpu_time_names = ['user', 'system', 'children_user', 'children_system']
+
+timer = getattr(time, 'monotonic', time.time)
+
+def get_arg_parser():
+  parser = argparse.ArgumentParser(
+    description="Unlogger and UI",
+    formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+
+  parser.add_argument("proc_names", nargs="?", default='',
+                      help="Process names to be monitored, comma seperated")
+  parser.add_argument("--list_all", action='store_true',
+                      help="Show all running processes' cmdline")
+  parser.add_argument("--detailed_times", action='store_true',
+                      help="show cpu time details (split by user, system, child user, child system)")
+  return parser
+
+
+if __name__ == "__main__":
+  args = get_arg_parser().parse_args(sys.argv[1:])
+  if args.list_all:
+    for p in psutil.process_iter():
+      print('cmdline', p.cmdline(), 'name', p.name())
+    sys.exit(0)
+
+  if len(args.proc_names) > 0:
+    monitored_proc_names = args.proc_names.split(',')
+  monitored_procs = []
+  stats = {}
+  for p in psutil.process_iter():
+    if p == psutil.Process():
+      continue
+    matched = any([l for l in p.cmdline() if any([pn for pn in monitored_proc_names if re.match(r'.*{}.*'.format(pn), l, re.M | re.I)])])
+    if matched:
+      k = ' '.join(p.cmdline())
+      print('Add monitored proc:', k)
+      stats[k] = {'cpu_samples': defaultdict(list), 'min': defaultdict(lambda: None), 'max': defaultdict(lambda: None),
+                  'avg': defaultdict(lambda: 0.0), 'last_cpu_times': None, 'last_sys_time':None}
+      stats[k]['last_sys_time'] = timer()
+      stats[k]['last_cpu_times'] = p.cpu_times()
+      monitored_procs.append(p)
+  i = 0
+  interval_int = int(PRINT_INTERVAL / SLEEP_INTERVAL)
+  while True:
+    for p in monitored_procs:
+      k = ' '.join(p.cmdline())
+      cur_sys_time = timer()
+      cur_cpu_times = p.cpu_times()
+      cpu_times = np.subtract(cur_cpu_times, stats[k]['last_cpu_times']) / (cur_sys_time - stats[k]['last_sys_time'])
+      stats[k]['last_sys_time'] = cur_sys_time
+      stats[k]['last_cpu_times'] = cur_cpu_times
+      cpu_percent = 0
+      for num, name in enumerate(cpu_time_names):
+        stats[k]['cpu_samples'][name].append(cpu_times[num])
+        cpu_percent += cpu_times[num]
+      stats[k]['cpu_samples']['total'].append(cpu_percent)
+    time.sleep(SLEEP_INTERVAL)
+    i += 1
+    if i % interval_int == 0:
+      l = []
+      for k, stat in stats.items():
+        if len(stat['cpu_samples']) <= 0:
+          continue
+        for name, samples in stat['cpu_samples'].items():
+          samples = np.array(samples)
+          avg = samples.mean()
+          c = samples.size
+          min_cpu = np.amin(samples)
+          max_cpu = np.amax(samples)
+          if stat['min'][name] is None or min_cpu < stat['min'][name]:
+            stat['min'][name] = min_cpu
+          if stat['max'][name] is None or max_cpu > stat['max'][name]:
+            stat['max'][name] = max_cpu
+          stat['avg'][name] = (stat['avg'][name] * (i - c) + avg * c) / (i)
+          stat['cpu_samples'][name] = []
+
+        msg = 'avg: {1:.2%}, min: {2:.2%}, max: {3:.2%} {0}'.format(os.path.basename(k), stat['avg']['total'], stat['min']['total'], stat['max']['total'])
+        if args.detailed_times:
+          for stat_type in ['avg', 'min', 'max']:
+            msg += '\n {}: {}'.format(stat_type, [name + ':' + str(round(stat[stat_type][name]*100, 2)) for name in cpu_time_names])
+        l.append((os.path.basename(k), stat['avg']['total'], msg))
+      l.sort(key= lambda x: -x[1])
+      for x in l:
+        print(x[2])
+      print('avg sum: {0:.2%} over {1} samples {2} seconds\n'.format(
+        sum([stat['avg']['total'] for k, stat in stats.items()]), i, i * SLEEP_INTERVAL
+      ))
--- a/selfdrive/debug/dump.py
+++ b/selfdrive/debug/dump.py
@ -0,0 +1,64 @@
+#!/usr/bin/env python3
+import os
+import sys
+import argparse
+import json
+from hexdump import hexdump
+
+from cereal import log
+import cereal.messaging as messaging
+from cereal.services import service_list
+
+if __name__ == "__main__":
+
+  parser = argparse.ArgumentParser(description='Sniff a communcation socket')
+  parser.add_argument('--pipe', action='store_true')
+  parser.add_argument('--raw', action='store_true')
+  parser.add_argument('--json', action='store_true')
+  parser.add_argument('--dump-json', action='store_true')
+  parser.add_argument('--no-print', action='store_true')
+  parser.add_argument('--addr', default='127.0.0.1')
+  parser.add_argument('--values', help='values to monitor (instead of entire event)')
+  parser.add_argument("socket", type=str, nargs='*', help="socket name")
+  args = parser.parse_args()
+
+  if args.addr != "127.0.0.1":
+    os.environ["ZMQ"] = "1"
+    messaging.context = messaging.Context()
+
+  poller = messaging.Poller()
+
+  for m in args.socket if len(args.socket) > 0 else service_list:
+    sock = messaging.sub_sock(m, poller, addr=args.addr)
+
+  values = None
+  if args.values:
+    values = [s.strip().split(".") for s in args.values.split(",")]
+
+  while 1:
+    polld = poller.poll(1000)
+    for sock in polld:
+      msg = sock.receive()
+      evt = log.Event.from_bytes(msg)
+
+      if not args.no_print:
+        if args.pipe:
+          sys.stdout.write(msg)
+          sys.stdout.flush()
+        elif args.raw:
+          hexdump(msg)
+        elif args.json:
+          print(json.loads(msg))
+        elif args.dump_json:
+          print(json.dumps(evt.to_dict()))
+        elif values:
+          print("logMonotime = {}".format(evt.logMonoTime))
+          for value in values:
+            if hasattr(evt, value[0]):
+              item = evt
+              for key in value:
+                item = getattr(item, key)
+              print("{} = {}".format(".".join(value), item))
+          print("")
+        else:
+          print(evt)
--- a/selfdrive/debug/get_fingerprint.py
+++ b/selfdrive/debug/get_fingerprint.py
@ -0,0 +1,29 @@
+#!/usr/bin/env python3
+
+# simple script to get a vehicle fingerprint.
+
+# Instructions:
+# - connect to a Panda
+# - run selfdrive/boardd/boardd
+# - launching this script
+# - turn on the car in STOCK MODE (set giraffe switches properly).
+#   Note: it's very important that the car is in stock mode, in order to collect a complete fingerprint
+# - since some messages are published at low frequency, keep this script running for at least 30s,
+#   until all messages are received at least once
+
+import cereal.messaging as messaging
+
+logcan = messaging.sub_sock('can')
+msgs = {}
+while True:
+  lc = messaging.recv_sock(logcan, True)
+  for c in lc.can:
+    # read also msgs sent by EON on CAN bus 0x80 and filter out the
+    # addr with more than 11 bits
+    if c.src in [0, 2] and c.address < 0x800:
+      msgs[c.address] = len(c.dat)
+
+  fingerprint = ', '.join("%d: %d" % v for v in sorted(msgs.items()))
+
+  print("number of messages {0}:".format(len(msgs)))
+  print("fingerprint {0}".format(fingerprint))
--- a/selfdrive/debug/internal/init.py
+++ b/selfdrive/debug/internal/init.py
--- a/selfdrive/debug/internal/benchmark_sleep.py
+++ b/selfdrive/debug/internal/benchmark_sleep.py
@ -0,0 +1,21 @@
+import time
+import numpy as np
+
+from common.realtime import sec_since_boot
+
+N = 1000
+
+times = []
+for i in range(1000):
+    t1 = sec_since_boot()
+    time.sleep(0.01)
+    t2 = sec_since_boot()
+    dt = t2 - t1
+    times.append(dt)
+
+
+print("Mean", np.mean(times))
+print("Max", np.max(times))
+print("Min", np.min(times))
+print("Variance", np.var(times))
+print("STD", np.sqrt(np.var(times)))
--- a/selfdrive/debug/internal/camera_small_box.py
+++ b/selfdrive/debug/internal/camera_small_box.py
@ -0,0 +1,113 @@
+#!/usr/bin/env python3
+import os
+
+from common.basedir import BASEDIR
+os.environ['BASEDIR'] = BASEDIR
+SCALE = 3
+
+import argparse
+import zmq
+import pygame
+import numpy as np
+import cv2
+import sys
+import traceback
+from collections import namedtuple
+
+from cereal import car
+from common.params import Params
+from common.lazy_property import lazy_property
+from cereal.messaging import sub_sock, recv_one_or_none, recv_one
+from cereal.services import service_list
+
+_BB_OFFSET = 290, 332
+_BB_TO_FULL_FRAME = np.asarray([[1., 0., _BB_OFFSET[0]], [0., 1., _BB_OFFSET[1]],
+                                [0., 0., 1.]])
+_FULL_FRAME_TO_BB = np.linalg.inv(_BB_TO_FULL_FRAME)
+_FULL_FRAME_SIZE = 1164, 874
+
+
+
+def pygame_modules_have_loaded():
+  return pygame.display.get_init() and pygame.font.get_init()
+
+
+def ui_thread(addr, frame_address):
+  context = zmq.Context()
+
+  pygame.init()
+  pygame.font.init()
+  assert pygame_modules_have_loaded()
+
+  size = (640 * SCALE, 480 * SCALE)
+  pygame.display.set_caption("comma one debug UI")
+  screen = pygame.display.set_mode(size, pygame.DOUBLEBUF)
+
+  camera_surface = pygame.surface.Surface((640 * SCALE, 480 * SCALE), 0, 24).convert()
+
+  frame = context.socket(zmq.SUB)
+  frame.connect(frame_address or "tcp://%s:%d" % (addr, 'frame'))
+  frame.setsockopt(zmq.SUBSCRIBE, "")
+
+  img = np.zeros((480, 640, 3), dtype='uint8')
+  imgff = np.zeros((_FULL_FRAME_SIZE[1], _FULL_FRAME_SIZE[0], 3), dtype=np.uint8)
+
+  while 1:
+    list(pygame.event.get())
+    screen.fill((64, 64, 64))
+
+    # ***** frame *****
+    fpkt = recv_one(frame)
+    yuv_img = fpkt.frame.image
+
+    if fpkt.frame.transform:
+      yuv_transform = np.array(fpkt.frame.transform).reshape(3, 3)
+    else:
+      # assume frame is flipped
+      yuv_transform = np.array([[-1.0, 0.0, _FULL_FRAME_SIZE[0] - 1],
+                                [0.0, -1.0, _FULL_FRAME_SIZE[1] - 1], [0.0, 0.0, 1.0]])
+
+    if yuv_img and len(yuv_img) == _FULL_FRAME_SIZE[0] * _FULL_FRAME_SIZE[1] * 3 // 2:
+      yuv_np = np.frombuffer(
+        yuv_img, dtype=np.uint8).reshape(_FULL_FRAME_SIZE[1] * 3 // 2, -1)
+      cv2.cvtColor(yuv_np, cv2.COLOR_YUV2RGB_I420, dst=imgff)
+      cv2.warpAffine(
+        imgff,
+        np.dot(yuv_transform, _BB_TO_FULL_FRAME)[:2], (img.shape[1], img.shape[0]),
+        dst=img,
+        flags=cv2.WARP_INVERSE_MAP)
+    else:
+      img.fill(0)
+
+    height, width = img.shape[:2]
+    img_resized = cv2.resize(
+      img, (SCALE * width, SCALE * height), interpolation=cv2.INTER_CUBIC)
+    # *** blits ***
+    pygame.surfarray.blit_array(camera_surface, img_resized.swapaxes(0, 1))
+    screen.blit(camera_surface, (0, 0))
+
+    # this takes time...vsync or something
+    pygame.display.flip()
+
+
+def get_arg_parser():
+  parser = argparse.ArgumentParser(
+    description="Show replay data in a UI.",
+    formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+
+  parser.add_argument(
+    "ip_address",
+    nargs="?",
+    default="127.0.0.1",
+    help="The ip address on which to receive zmq messages.")
+
+  parser.add_argument(
+    "--frame-address",
+    default=None,
+    help="The ip address on which to receive zmq messages.")
+  return parser
+
+
+if __name__ == "__main__":
+  args = get_arg_parser().parse_args(sys.argv[1:])
+  ui_thread(args.ip_address, args.frame_address)
--- a/selfdrive/debug/internal/convert_csv_to_route.py
+++ b/selfdrive/debug/internal/convert_csv_to_route.py
@ -0,0 +1,98 @@
+#!/usr/bin/env python
+from common.kalman.ned import ecef2geodetic
+
+import csv
+import numpy as np
+import webbrowser
+import os
+import sys
+import json
+import numpy.linalg as LA
+import gmplot
+from dateutil.parser import parse
+from common.numpy_helpers import deep_interp
+# import cvxpy as cvx
+MPH_TO_MS = 0.44704
+
+
+def downsample(positions, speeds, start_idx, end_idx, dist):
+    # TODO: save headings too
+    track = []
+    last_position = positions[start_idx]
+    valid_indeces = []
+    track_speeds = []
+    for pi in range(start_idx, end_idx):
+        # only save points that are at least 10 cm far away
+        if LA.norm(positions[pi] - last_position) >= dist:
+            #print LA.norm(positions[pi] - last_position)
+            last_position = positions[pi]
+            track.append(positions[pi])
+            valid_indeces.append(pi)
+            track_speeds.append(speeds[pi])
+    print(-start_idx + end_idx, len(valid_indeces))
+    # this compare the original point count vs the filtered count 
+            
+    track = np.array(track)
+    track_speeds = np.array(track_speeds)
+    return track, track_speeds
+
+def converter(date):
+
+  filename = "/home/batman/one/selfdrive/locationd/liveloc_dumps/" + date + "/canonical.csv"  # Point one (OK!)
+  
+  c = csv.DictReader(open(filename, 'rb'), delimiter=',')
+  
+  start_time = None
+  
+  t = []
+  ll_positions = []
+  positions = []
+  sats = []
+  flag = []
+  speeds = []
+  
+  for row in c:
+      t.append(float(row['pctime']))
+      x = float(row['ecefX'])
+      y = float(row['ecefY'])
+      z = float(row['ecefZ'])
+      ecef = np.array((x, y, z))
+      speeds.append(float(row['velSpeed']))
+                      
+      pos = ecef2geodetic(ecef)
+      ll_positions.append(pos)
+      positions.append(ecef)
+  
+  t = np.array(t)
+  ll_positions = np.array(ll_positions)
+  positions = np.array(positions)
+                      
+  #distances = ll_positions[:,0:2] - START_POS[:2]
+  #i_start = np.argmin(LA.norm(distances, axis=1))
+  
+  #for i in range(i_start + 500):
+  #    distances[i] += np.array([100, 100])
+  #i_end = np.argmin(LA.norm(distances, axis=1))
+
+  i_start = 0
+  i_end = len(positions)
+  
+  print(i_start, i_end)
+  track, track_speeds = downsample(positions, speeds, i_start, i_end, 0.2)
+  ll_track = np.array([ecef2geodetic(pos) for pos in track])
+  
+  track_struct = {}
+  print(track_speeds.shape)
+  print(track.shape)
+  track_struct['race'] = np.hstack((track, 
+                                    np.expand_dims(track_speeds, axis=1), 
+                                    np.zeros((len(track_speeds), 1))))
+  
+  f = open('/home/batman/one/selfdrive/controls/tracks/loop_city.npy', 'w')
+  np.save(f, track_struct)
+  f.close()
+  print("SAVED!")
+
+
+if __name__ == "__main__":
+  converter(sys.argv[1])
--- a/selfdrive/debug/internal/cycle_alerts.py
+++ b/selfdrive/debug/internal/cycle_alerts.py
@ -0,0 +1,53 @@
+# USAGE: python cycle_alerts.py [duration_millis=1000]
+# Then start manager
+
+import argparse
+import time
+import zmq
+
+import cereal.messaging as messaging
+from cereal.services import service_list
+from selfdrive.controls.lib.alerts import ALERTS
+
+def now_millis(): return time.time() * 1000
+
+default_alerts = sorted(ALERTS, key=lambda alert: (alert.alert_size, len(alert.alert_text_2)))
+
+def cycle_alerts(duration_millis, alerts=None):
+    if alerts is None:
+      alerts = default_alerts
+
+    controls_state = messaging.pub_sock('controlsState')
+
+    last_pop_millis = now_millis()
+    alert = alerts.pop()
+    while 1:
+        if (now_millis() - last_pop_millis) > duration_millis:
+            alerts.insert(0, alert)
+            alert = alerts.pop()
+            last_pop_millis = now_millis()
+            print('sending {}'.format(str(alert)))
+
+        dat = messaging.new_message()
+        dat.init('controlsState')
+
+        dat.controlsState.alertType = alert.alert_type
+        dat.controlsState.alertText1 = alert.alert_text_1
+        dat.controlsState.alertText2 = alert.alert_text_2
+        dat.controlsState.alertSize = alert.alert_size
+        dat.controlsState.alertStatus = alert.alert_status
+        dat.controlsState.alertSound = alert.audible_alert
+        controls_state.send(dat.to_bytes())
+
+        time.sleep(0.01)
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser()
+    parser.add_argument('--duration', type=int, default=1000)
+    parser.add_argument('--alert-types', nargs='+')
+    args = parser.parse_args()
+    alerts = None
+    if args.alert_types:
+      alerts = [next(a for a in ALERTS if a.alert_type==alert_type) for alert_type in args.alert_types]
+
+    cycle_alerts(args.duration, alerts=alerts)
--- a/selfdrive/debug/internal/design_lqr.py
+++ b/selfdrive/debug/internal/design_lqr.py
@ -0,0 +1,32 @@
+#!/usr/bin/env python3
+import numpy as np
+import control
+
+dt = 0.01
+A = np.array([[ 0.        ,  1.        ],       [-0.78823806,  1.78060701]])
+B = np.array([[-2.23399437e-05],       [ 7.58330763e-08]])
+C = np.array([[1., 0.]])
+
+
+# Kalman tuning
+Q = np.diag([1, 1])
+R = np.atleast_2d(1e5)
+
+(_, _, L) = control.dare(A.T, C.T, Q, R)
+L = L.T
+
+# LQR tuning
+Q = np.diag([2e5, 1e-5])
+R = np.atleast_2d(1)
+(_, _, K) = control.dare(A, B, Q, R)
+
+A_cl = (A - B.dot(K))
+sys = control.ss(A_cl, B, C, 0, dt)
+dc_gain = control.dcgain(sys)
+
+print(("self.A = np." + A.__repr__()).replace('\n', ''))
+print(("self.B = np." + B.__repr__()).replace('\n', ''))
+print(("self.C = np." + C.__repr__()).replace('\n', ''))
+print(("self.K = np." + K.__repr__()).replace('\n', ''))
+print(("self.L = np." + L.__repr__()).replace('\n', ''))
+print("self.dc_gain = " + str(dc_gain))
--- a/selfdrive/debug/internal/eval_live_location.py
+++ b/selfdrive/debug/internal/eval_live_location.py
@ -0,0 +1,48 @@
+#!/usr/bin/env python3
+import time
+import sys
+import argparse
+import zmq
+import json
+import pyproj
+import numpy as np
+ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
+lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')
+
+import cereal.messaging as messaging
+from cereal.services import service_list
+
+poller = zmq.Poller()
+ll = messaging.sub_sock("liveLocation", poller)
+tll = messaging.sub_sock("testLiveLocation", poller)
+
+l, tl = None, None
+
+lp = time.time()
+
+while 1:
+  polld = poller.poll(timeout=1000)
+  for sock, mode in polld:
+    if mode != zmq.POLLIN:
+      continue
+    if sock == ll:
+      l = messaging.recv_one(sock)
+    elif sock == tll:
+      tl = messaging.recv_one(sock)
+  if l is None or tl is None:
+    continue
+
+  alt_err = np.abs(l.liveLocation.alt - tl.liveLocation.alt)
+  l1 = pyproj.transform(lla, ecef, l.liveLocation.lon, l.liveLocation.lat, l.liveLocation.alt)
+  l2 = pyproj.transform(lla, ecef, tl.liveLocation.lon, tl.liveLocation.lat, tl.liveLocation.alt)
+
+  al1 = pyproj.transform(lla, ecef, l.liveLocation.lon, l.liveLocation.lat, l.liveLocation.alt)
+  al2 = pyproj.transform(lla, ecef, tl.liveLocation.lon, tl.liveLocation.lat, l.liveLocation.alt)
+
+  tdiff = np.abs(l.logMonoTime - tl.logMonoTime) / 1e9
+
+  if time.time()-lp > 0.1:
+    print("tm: %f   mse: %f   mse(flat): %f   alterr: %f" % (tdiff, np.mean((np.array(l1)-np.array(l2))**2), np.mean((np.array(al1)-np.array(al2))**2), alt_err))
+    lp = time.time()
+
+
--- a/selfdrive/debug/internal/get_fingerprint_from_route.py
+++ b/selfdrive/debug/internal/get_fingerprint_from_route.py
@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+
+import sys
+from tools.lib.logreader import MultiLogIterator
+from xx.chffr.lib.route import Route
+
+
+def get_fingerprint(lr):
+  can_msgs = [m for m in lr if m.which() == 'can']
+
+  msgs = {}
+
+  for msg in can_msgs:
+    for c in msg.can:
+      # read also msgs sent by EON on CAN bus 0x80 and filter out the
+      # addr with more than 11 bits
+      if c.src % 0x80 == 0 and c.address < 0x800:
+        msgs[c.address] = len(c.dat)
+
+    fingerprint = ', '.join("%d: %d" % v for v in sorted(msgs.items()))
+  print("number of messages {0}:".format(len(msgs)))
+  print("fingerprint {0}".format(fingerprint))
+
+
+if __name__ == "__main__":
+  if len(sys.argv) < 2:
+    print("Usage: ./get_fingerprint_internal.py <route>")
+    sys.exit(1)
+
+  route = sys.argv[1]
+  route = Route(route)
+  lr = MultiLogIterator(route.log_paths(), wraparound=False)
+  get_fingerprint(lr)
--- a/selfdrive/debug/internal/gps_planner.py
+++ b/selfdrive/debug/internal/gps_planner.py
@ -0,0 +1,89 @@
+#!/usr/bin/env python3
+import sys
+import time
+
+import matplotlib.pyplot as plt
+import numpy as np
+import cereal.messaging as messaging
+import zmq
+from common.transformations.coordinates import LocalCoord
+from cereal.services import service_list
+
+SCALE = 20.
+
+def mpc_vwr_thread(addr="127.0.0.1"):
+  plt.ion()
+  fig = plt.figure(figsize=(15, 15))
+  ax = fig.add_subplot(1,1,1)
+  ax.set_xlim([-SCALE, SCALE])
+  ax.set_ylim([-SCALE, SCALE])
+  ax.grid(True)
+
+  line, = ax.plot([0.0], [0.0], ".b")
+  line2, = ax.plot([0.0], [0.0], 'r')
+
+  ax.set_aspect('equal', 'datalim')
+  plt.show()
+
+  live_location = messaging.sub_sock('liveLocation', addr=addr, conflate=True)
+  gps_planner_points = messaging.sub_sock('gpsPlannerPoints', conflate=True)
+  gps_planner_plan = messaging.sub_sock('gpsPlannerPlan', conflate=True)
+
+  last_points = messaging.recv_one(gps_planner_points)
+  last_plan = messaging.recv_one(gps_planner_plan)
+  while True:
+    p = messaging.recv_one_or_none(gps_planner_points)
+    pl = messaging.recv_one_or_none(gps_planner_plan)
+    ll = messaging.recv_one(live_location).liveLocation
+
+    if p is not None:
+      last_points = p
+    if pl is not None:
+      last_plan = pl
+
+    if not last_plan.gpsPlannerPlan.valid:
+      time.sleep(0.1)
+      line2.set_color('r')
+      continue
+
+    p0 = last_points.gpsPlannerPoints.points[0]
+    p0 = np.array([p0.x, p0.y, p0.z])
+
+    n = LocalCoord.from_geodetic(np.array([ll.lat, ll.lon, ll.alt]))
+    points = []
+    print(len(last_points.gpsPlannerPoints.points))
+    for p in last_points.gpsPlannerPoints.points:
+      ecef = np.array([p.x, p.y, p.z])
+      points.append(n.ecef2ned(ecef))
+
+    points = np.vstack(points)
+    line.set_xdata(points[:, 1])
+    line.set_ydata(points[:, 0])
+
+    y = np.matrix(np.arange(-100, 100.0, 0.5))
+    x = -np.matrix(np.polyval(last_plan.gpsPlannerPlan.poly, y))
+    xy = np.hstack([x.T, y.T])
+
+    cur_heading = np.radians(ll.heading - 90)
+    c, s = np.cos(cur_heading), np.sin(cur_heading)
+    R = np.array([[c, -s], [s, c]])
+    xy = xy.dot(R)
+
+    line2.set_xdata(xy[:, 1])
+    line2.set_ydata(-xy[:, 0])
+    line2.set_color('g')
+
+
+    ax.set_xlim([-SCALE, SCALE])
+    ax.set_ylim([-SCALE, SCALE])
+
+    fig.canvas.draw()
+    fig.canvas.flush_events()
+
+
+
+if __name__ == "__main__":
+  if len(sys.argv) > 1:
+    mpc_vwr_thread(sys.argv[1])
+  else:
+    mpc_vwr_thread()
--- a/selfdrive/debug/internal/honda_sniff_steering.py
+++ b/selfdrive/debug/internal/honda_sniff_steering.py
@ -0,0 +1,113 @@
+#!/usr/bin/env python3
+import os
+import zmq
+
+import cereal.messaging as messaging
+from cereal.services import service_list
+
+from panda.lib.panda import Panda
+from hexdump import hexdump
+import time
+
+def raw_panda():
+  p = Panda()
+  print(p)
+
+  p.set_uart_baud(2, 9600)
+  p.set_uart_baud(3, 9600)
+
+  p.set_uart_parity(2, 1)
+  p.set_uart_parity(3, 1)
+
+  p.set_uart_callback(2, 1)
+  p.set_uart_callback(3, 1)
+
+  idx = 0
+  while 1:
+    """
+    dat = p.serial_read(2)
+    if len(dat) > 0:
+      print "2:",
+      hexdump(dat)
+
+    dat = p.serial_read(3)
+    if len(dat) > 0:
+      print "3:",
+      hexdump(dat)
+
+    print "read done, waiting"
+    time.sleep(0.01)
+    """
+
+    if idx%2 == 1:
+      dat = "\x20\x80\xc0\xa0"
+    else:
+      dat = "\x00\x80\xc0\xc0"
+    p.can_send(0, dat, 8)
+
+    for r in p.can_recv():
+      if r[-1] in [8, 9]:
+        print(r[-1], r[2].encode("hex"))
+
+    time.sleep(0.01)
+    idx += 1
+
+if __name__ == "__main__":
+  #raw_panda()
+  #exit(0)
+
+  logcan = messaging.sub_sock('can')
+
+  t1 = []
+  t2 = []
+  t3 = []
+
+  while len(t1) < 1000 or os.uname()[-1] == "aarch64":
+    rr = messaging.recv_sock(logcan, wait=True)
+    for c in rr.can:
+      if c.src in [9] and len(c.dat) == 5:
+        aa = map(lambda x: ord(x)&0x7f, c.dat)
+
+        # checksum
+        assert (-(aa[0]+aa[1]+aa[2]+aa[3]))&0x7f == aa[4]
+
+        #print map(bin, aa[0:4])
+
+        aa[0] &= ~0x20
+        aa[1] &= ~0x20
+
+        st = (aa[0] << 5) + aa[1]
+        if st >= 256:
+          st = -(512-st)
+
+        mt = ((aa[2] >> 3) << 7) + aa[3]
+        if mt >= 512:
+          mt = -(1024-mt)
+
+        print(st, mt)
+        t1.append(st)
+        t2.append(mt)
+        #print map(bin, aa), "apply", st
+
+      if c.src in [8] and len(c.dat) == 4:
+        aa = map(lambda x: ord(x)&0x7f, c.dat)
+
+        # checksum
+        assert (-(aa[0]+aa[1]+aa[2]))&0x7f == aa[3]
+
+        aa[0] &= ~0x20
+        aa[1] &= ~0x20
+
+        st = (aa[0] << 5) + aa[1]
+        if st >= 256:
+          st = -(512-st)
+        print(aa, "apply", st)
+
+        t3.append(st)
+
+  import matplotlib.pyplot as plt
+  plt.plot(t1)
+  plt.plot(t2)
+  plt.plot(t3)
+  plt.show()
+
--- a/selfdrive/debug/internal/measure_torque_time_to_max.py
+++ b/selfdrive/debug/internal/measure_torque_time_to_max.py
@ -0,0 +1,57 @@
+#!/usr/bin/env python3
+import os
+import sys
+import argparse
+import struct
+
+from cereal import log
+import cereal.messaging as messaging
+from cereal.services import service_list
+
+if __name__ == "__main__":
+
+  parser = argparse.ArgumentParser(description='Sniff a communcation socket')
+  parser.add_argument('--addr', default='127.0.0.1')
+  args = parser.parse_args()
+
+  if args.addr != "127.0.0.1":
+    os.environ["ZMQ"] = "1"
+    messaging.context = messaging.Context()
+
+  poller = messaging.Poller()
+  messaging.sub_sock('can', poller, addr=args.addr)
+
+  active = 0
+  start_t = 0
+  start_v = 0
+  max_v = 0
+  max_t = 0
+  window = [0] * 10
+  avg = 0
+  while 1:
+    polld = poller.poll(1000)
+    for sock in polld:
+      msg = sock.receive()
+      evt = log.Event.from_bytes(msg)
+
+      for item in evt.can:
+        if item.address == 0xe4 and item.src == 128:
+          torque_req = struct.unpack('!h', item.dat[0:2])[0]
+          # print(torque_req)
+          active = abs(torque_req) > 0
+          if abs(torque_req) < 100:
+            if max_v > 5:
+              print(f'{start_v} -> {max_v} = {round(max_v - start_v, 2)} over {round(max_t - start_t, 2)}s')
+            start_t = evt.logMonoTime / 1e9
+            start_v = avg
+            max_t = 0
+            max_v = 0
+        if item.address == 0x1ab and item.src == 0:
+          motor_torque = ((item.dat[0] & 0x3) << 8) + item.dat[1]
+          window.append(motor_torque)
+          window.pop(0)
+          avg = sum(window) / len(window)
+          #print(f'{evt.logMonoTime}: {avg}')
+          if active and avg > max_v + 0.5:
+            max_v = avg
+            max_t = evt.logMonoTime / 1e9
--- a/selfdrive/debug/internal/messaging/boost/.gitignore
+++ b/selfdrive/debug/internal/messaging/boost/.gitignore
@ -0,0 +1,2 @@
+sender
+receiver
--- a/selfdrive/debug/internal/messaging/boost/Makefile
+++ b/selfdrive/debug/internal/messaging/boost/Makefile
@ -0,0 +1,61 @@
+CC = clang
+CXX = clang++
+
+ARCH := $(shell uname -m)
+OS := $(shell uname -o)
+
+BASEDIR = ../../../..
+PHONELIBS = ../../../../phonelibs
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+             -Werror=incompatible-pointer-types \
+             -Werror=int-conversion \
+             -Werror=return-type \
+             -Werror=format-extra-args
+
+CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) -Wall
+CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) -Wall
+
+ifeq ($(ARCH),aarch64)
+CFLAGS += -mcpu=cortex-a57
+CXXFLAGS += -mcpu=cortex-a57
+endif
+
+
+EXTRA_LIBS = -lpthread
+
+ifeq ($(ARCH),x86_64)
+BOOST_LIBS = -lboost_system -lboost_locale -lrt
+else
+EXTRA_LIBS += -llog -luuid
+endif
+
+.PHONY: all
+all: sender receiver
+
+receiver: receiver.o
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+            $(CEREAL_LIBS) \
+            $(BOOST_LIBS) \
+            $(EXTRA_LIBS)
+
+sender: sender.o
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+            $(CEREAL_LIBS) \
+            $(BOOST_LIBS) \
+            $(EXTRA_LIBS)
+
+%.o: %.cc
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) -MMD \
+          -Iinclude -I.. -I../.. \
+           -I../ \
+           -I../../ \
+           -c -o '$@' '$<'
+
+
+.PHONY: clean
+clean:
+	rm -f *.d sender receiver *.o
--- a/selfdrive/debug/internal/messaging/boost/receiver.cc
+++ b/selfdrive/debug/internal/messaging/boost/receiver.cc
@ -0,0 +1,54 @@
+#include <boost/interprocess/ipc/message_queue.hpp>
+#include <iostream>
+#include <vector>
+#include <thread>
+
+using namespace boost::interprocess;
+#define N 1024
+
+message_queue *sub_queue(const char *name){
+  while (true){
+    try {
+      message_queue *mq = new message_queue(open_only, name);
+      return mq;
+    }
+    catch(interprocess_exception &ex){
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+
+  }
+}
+
+message_queue *pub_queue(const char *name){
+  message_queue::remove(name);
+  message_queue *mq = new message_queue(create_only, name, 100, N);
+  return mq;
+}
+
+
+
+int main ()
+{
+
+  message_queue::remove("queue_1");
+  message_queue::remove("queue_2");
+
+  message_queue *pq = pub_queue("queue_2");
+  message_queue *sq = sub_queue("queue_1");
+  std::cout << "Ready" << std::endl;
+
+  unsigned int priority;
+  std::size_t recvd_size;
+
+  char * rcv_msg = new char[N];
+
+  while (true){
+
+    sq->receive(rcv_msg, N, recvd_size, priority);
+    assert(N == recvd_size);
+
+    pq->send(rcv_msg, N, 0);
+  }
+
+  return 0;
+}
--- a/selfdrive/debug/internal/messaging/boost/sender.cc
+++ b/selfdrive/debug/internal/messaging/boost/sender.cc
@ -0,0 +1,62 @@
+#include <boost/interprocess/ipc/message_queue.hpp>
+#include <iostream>
+#include <vector>
+#include <thread>
+#include <chrono>
+#include <cassert>
+
+#define N 1024
+#define MSGS 1e5
+
+using namespace boost::interprocess;
+
+message_queue *sub_queue(const char *name){
+  while (true){
+    try {
+      message_queue *mq = new message_queue(open_only, name);
+      return mq;
+    }
+    catch(interprocess_exception &ex){
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+
+  }
+}
+
+message_queue *pub_queue(const char *name){
+  message_queue::remove(name);
+  message_queue *mq = new message_queue(create_only, name, 100, N);
+  return mq;
+}
+
+
+
+int main ()
+{
+  message_queue *pq = pub_queue("queue_1");
+  message_queue *sq = sub_queue("queue_2");
+  std::cout << "Ready" << std::endl;
+
+  auto start = std::chrono::steady_clock::now();
+  char * rcv_msg = new char[N];
+  char * snd_msg = new char[N];
+
+  unsigned int priority;
+  std::size_t recvd_size;
+
+  for (int i = 0; i < MSGS; i++){
+    sprintf(snd_msg, "%d", i);
+
+    pq->send(snd_msg, N, 0);
+    sq->receive(rcv_msg, N, recvd_size, priority);
+  }
+
+  auto end = std::chrono::steady_clock::now();
+  double elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count() / 1e9;
+  double throughput = ((double) MSGS / (double) elapsed);
+
+  std::cout << "Elapsed: " << elapsed << " s" << std::endl;
+  std::cout << "Throughput: " << throughput << " msg/s" << std::endl;
+
+  return 0;
+}
--- a/selfdrive/debug/internal/messaging/nanomsg/.gitignore
+++ b/selfdrive/debug/internal/messaging/nanomsg/.gitignore
@ -0,0 +1,2 @@
+receiver
+sender
--- a/selfdrive/debug/internal/messaging/nanomsg/Makefile
+++ b/selfdrive/debug/internal/messaging/nanomsg/Makefile
@ -0,0 +1,65 @@
+CC = clang
+CXX = clang++
+
+ARCH := $(shell uname -m)
+OS := $(shell uname -o)
+
+BASEDIR = ../../../..
+PHONELIBS = ../../../../phonelibs
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+             -Werror=incompatible-pointer-types \
+             -Werror=int-conversion \
+             -Werror=return-type \
+             -Werror=format-extra-args
+
+CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) -Wall
+CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) -Wall
+# NANOMSG_LIBS = -l:libnanomsg.a
+
+ifeq ($(ARCH),aarch64)
+CFLAGS += -mcpu=cortex-a57
+CXXFLAGS += -mcpu=cortex-a57
+endif
+
+
+EXTRA_LIBS = -lpthread
+
+ifeq ($(ARCH),x86_64)
+NANOMSG_FLAGS = -I$(BASEDIR)/phonelibs/nanomsg/x64/include
+NANOMSG_LIBS = -L$(BASEDIR)/phonelibs/nanomsg/x64/lib \
+           -lnanomsg -Wl,-rpath,$(BASEDIR)/phonelibs/nanomsg/x64/lib
+else
+EXTRA_LIBS += -llog -luuid
+endif
+
+.PHONY: all
+all: sender receiver
+
+receiver: receiver.o
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+            $(NANOMSG_LIBS) \
+            $(EXTRA_LIBS)
+
+sender: sender.o
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+            $(NANOMSG_LIBS) \
+            $(EXTRA_LIBS)
+
+%.o: %.cc
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) -MMD \
+          -Iinclude -I.. -I../.. \
+           $(NANOMSG_FLAGS) \
+           $(JSON11_FLAGS) \
+           $(JSON_FLAGS) \
+           -I../ \
+           -I../../ \
+           -c -o '$@' '$<'
+
+
+.PHONY: clean
+clean:
+	rm -f *.d sender receiver
--- a/selfdrive/debug/internal/messaging/nanomsg/receiver.cc
+++ b/selfdrive/debug/internal/messaging/nanomsg/receiver.cc
@ -0,0 +1,48 @@
+#include <future>
+#include <cassert>
+#include <iostream>
+#include <cstring>
+#include <thread>
+
+#include <nanomsg/nn.h>
+#include <nanomsg/pubsub.h>
+#include <nanomsg/tcp.h>
+
+#define N 1024
+
+int sub_sock(const char *endpoint) {
+  int sock = nn_socket(AF_SP, NN_SUB);
+  assert(sock >= 0);
+
+  nn_setsockopt(sock, NN_SUB, NN_SUB_SUBSCRIBE,  "", 0);
+  assert(nn_connect(sock, endpoint) >= 0);
+
+  return sock;
+}
+
+int pub_sock(const char *endpoint){
+  int sock = nn_socket(AF_SP, NN_PUB);
+  assert(sock >= 0);
+
+  int b = 1;
+  nn_setsockopt(sock, NN_TCP, NN_TCP_NODELAY, &b, sizeof(b));
+
+  assert(nn_bind(sock, endpoint) >= 0);
+
+  return sock;
+}
+
+int main(int argc, char *argv[]) {
+  auto p_sock = pub_sock("tcp://*:10011");
+  auto s_sock = sub_sock("tcp://127.0.0.1:10010");
+  std::cout << "Ready!" << std::endl;
+
+  char * msg = new char[N];
+
+  while (true){
+    int bytes = nn_recv(s_sock, msg, N, 0);
+    nn_send(p_sock, msg, bytes, 0);
+  }
+
+  return 0;
+}
--- a/selfdrive/debug/internal/messaging/nanomsg/sender.cc
+++ b/selfdrive/debug/internal/messaging/nanomsg/sender.cc
@ -0,0 +1,73 @@
+#include <iostream>
+#include <cassert>
+#include <chrono>
+#include <thread>
+
+#include <nanomsg/nn.h>
+#include <nanomsg/pubsub.h>
+#include <nanomsg/tcp.h>
+
+
+#define N 1024
+#define MSGS 1e5
+
+int sub_sock(const char *endpoint) {
+  int sock = nn_socket(AF_SP, NN_SUB);
+  assert(sock >= 0);
+
+  nn_setsockopt(sock, NN_SUB, NN_SUB_SUBSCRIBE,  "", 0);
+
+  int timeout = 100;
+  nn_setsockopt(sock, NN_SOL_SOCKET, NN_RCVTIMEO, &timeout , sizeof(timeout));
+
+  assert(nn_connect(sock, endpoint) >= 0);
+  return sock;
+}
+
+int pub_sock(const char *endpoint){
+  int sock = nn_socket(AF_SP, NN_PUB);
+  assert(sock >= 0);
+
+  int b = 1;
+  nn_setsockopt(sock, NN_TCP, NN_TCP_NODELAY, &b, sizeof(b));
+
+  assert(nn_bind(sock, endpoint) >= 0);
+
+  return sock;
+}
+
+
+int main(int argc, char *argv[]) {
+  auto p_sock = pub_sock("tcp://*:10010");
+  auto s_sock = sub_sock("tcp://127.0.0.1:10011");
+  std::cout << "Ready!" << std::endl;
+
+  // auto p_sock = pub_sock("ipc:///tmp/feeds/3");
+  // auto s_sock = sub_sock("ipc:///tmp/feeds/2");
+
+  char * msg = new char[N];
+  auto start = std::chrono::steady_clock::now();
+
+
+  for (int i = 0; i < MSGS; i++){
+    sprintf(msg, "%d", i);
+
+    nn_send(p_sock, msg, N, 0);
+    int bytes = nn_recv(s_sock, msg, N, 0);
+
+    if (bytes < 0) {
+      std::cout << "Timeout" << std::endl;
+    }
+
+  }
+  auto end = std::chrono::steady_clock::now();
+
+
+  double elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count() / 1e9;
+  double throughput = ((double) MSGS / (double) elapsed);
+
+  std::cout << "Elapsed: " << elapsed << " s" << std::endl;
+  std::cout << "Throughput: " << throughput << " msg/s" << std::endl;
+
+  return 0;
+}
--- a/selfdrive/debug/internal/messaging/nng/.gitignore
+++ b/selfdrive/debug/internal/messaging/nng/.gitignore
@ -0,0 +1,2 @@
+receiver
+sender
--- a/selfdrive/debug/internal/messaging/nng/Makefile
+++ b/selfdrive/debug/internal/messaging/nng/Makefile
@ -0,0 +1,68 @@
+CC = clang
+CXX = clang++
+
+ARCH := $(shell uname -m)
+OS := $(shell uname -o)
+
+BASEDIR = ../../../..
+PHONELIBS = ../../../../phonelibs
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+             -Werror=incompatible-pointer-types \
+             -Werror=int-conversion \
+             -Werror=return-type \
+             -Werror=format-extra-args
+
+CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) -Wall
+CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) -Wall
+NNG_LIBS = -l:libnng.a
+
+ifeq ($(ARCH),aarch64)
+CFLAGS += -mcpu=cortex-a57
+CXXFLAGS += -mcpu=cortex-a57
+endif
+
+
+EXTRA_LIBS = -lpthread
+
+ifeq ($(ARCH),x86_64)
+ZMQ_FLAGS = -I$(BASEDIR)/phonelibs/nng/x64/include
+NNG_LIBS = -L$(BASEDIR)/phonelibs/nng/x64/lib \
+           -l:libnng.a
+else
+EXTRA_LIBS += -llog -luuid
+endif
+
+.PHONY: all
+all: sender receiver
+
+receiver: receiver.o
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+            $(CEREAL_LIBS) \
+            $(NNG_LIBS) \
+            $(EXTRA_LIBS)
+
+sender: sender.o
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+            $(CEREAL_LIBS) \
+            $(NNG_LIBS) \
+            $(EXTRA_LIBS)
+
+%.o: %.cc
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) -MMD \
+          -Iinclude -I.. -I../.. \
+           $(CEREAL_CXXFLAGS) \
+           $(ZMQ_FLAGS) \
+           $(JSON11_FLAGS) \
+           $(JSON_FLAGS) \
+           -I../ \
+           -I../../ \
+           -c -o '$@' '$<'
+
+
+.PHONY: clean
+clean:
+	rm -f *.d sender receiver
--- a/selfdrive/debug/internal/messaging/nng/receiver.cc
+++ b/selfdrive/debug/internal/messaging/nng/receiver.cc
@ -0,0 +1,56 @@
+#include <future>
+#include <cassert>
+#include <iostream>
+#include <cstring>
+#include <thread>
+
+#include <nng/nng.h>
+#include <nng/protocol/pubsub0/pub.h>
+#include <nng/protocol/pubsub0/sub.h>
+
+nng_socket sub_sock(const char *endpoint) {
+  nng_socket sock;
+  int r;
+  r = nng_sub0_open(&sock);
+  assert(r == 0);
+
+  nng_setopt(sock, NNG_OPT_SUB_SUBSCRIBE, "", 0);
+
+  while (true){
+    r = nng_dial(sock, endpoint, NULL, 0);
+
+    if (r == 0){
+      break;
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+
+  }
+  return sock;
+}
+
+nng_socket pub_sock(const char *endpoint){
+  nng_socket sock;
+  int r;
+  r = nng_pub0_open(&sock);
+  assert(r == 0);
+  r = nng_listen(sock, endpoint, NULL, 0);
+  assert(r == 0);
+
+  return sock;
+}
+
+int main(int argc, char *argv[]) {
+  // auto p_sock = pub_sock("tcp://*:10004");
+  // auto s_sock = sub_sock("tcp://127.0.0.1:10003");
+
+  auto p_sock = pub_sock("ipc:///tmp/feeds/2");
+  auto s_sock = sub_sock("ipc:///tmp/feeds/3");
+
+  while (true){
+    nng_msg *msg;
+    nng_recvmsg(s_sock, &msg, 0);
+    nng_sendmsg(p_sock, msg, 0);
+  }
+
+  return 0;
+}
--- a/selfdrive/debug/internal/messaging/nng/sender.cc
+++ b/selfdrive/debug/internal/messaging/nng/sender.cc
@ -0,0 +1,78 @@
+#include <iostream>
+#include <cassert>
+#include <chrono>
+#include <thread>
+
+#include <nng/nng.h>
+#include <nng/protocol/pubsub0/pub.h>
+#include <nng/protocol/pubsub0/sub.h>
+
+
+#define N 1024
+#define MSGS 1e5
+
+nng_socket sub_sock(const char *endpoint) {
+  nng_socket sock;
+  int r;
+  r = nng_sub0_open(&sock);
+  assert(r == 0);
+
+  nng_setopt(sock, NNG_OPT_SUB_SUBSCRIBE, "", 0);
+  nng_setopt_ms(sock, NNG_OPT_RECVTIMEO, 100);
+
+  while (true){
+    r = nng_dial(sock, endpoint, NULL, 0);
+
+    if (r == 0){
+      break;
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+  }
+  return sock;
+}
+
+nng_socket pub_sock(const char *endpoint){
+  nng_socket sock;
+  int r;
+  r = nng_pub0_open(&sock);
+  assert(r == 0);
+  r = nng_listen(sock, endpoint, NULL, 0);
+  assert(r == 0);
+
+  return sock;
+}
+
+
+int main(int argc, char *argv[]) {
+  // auto p_sock = pub_sock("tcp://*:10003");
+  // auto s_sock = sub_sock("tcp://127.0.0.1:10004");
+
+  auto p_sock = pub_sock("ipc:///tmp/feeds/3");
+  auto s_sock = sub_sock("ipc:///tmp/feeds/2");
+
+
+  auto start = std::chrono::steady_clock::now();
+
+  for (int i = 0; i < MSGS; i++){
+    nng_msg *msg;
+    nng_msg_alloc(&msg, N);
+    nng_sendmsg(p_sock, msg, 0);
+
+    nng_msg *rmsg;
+    int r = nng_recvmsg(s_sock, &rmsg, 0);
+
+    if (r) {
+      std::cout << "Timeout" << std::endl;
+    }
+  }
+  auto end = std::chrono::steady_clock::now();
+
+
+  double elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count() / 1e9;
+  double throughput = ((double) MSGS / (double) elapsed);
+
+  std::cout << "Elapsed: " << elapsed << " s" << std::endl;
+  std::cout << "Throughput: " << throughput << " msg/s" << std::endl;
+
+  return 0;
+}
--- a/selfdrive/debug/internal/messaging/zmq/.gitignore
+++ b/selfdrive/debug/internal/messaging/zmq/.gitignore
@ -0,0 +1,2 @@
+receiver
+sender
--- a/selfdrive/debug/internal/messaging/zmq/Makefile
+++ b/selfdrive/debug/internal/messaging/zmq/Makefile
@ -0,0 +1,71 @@
+CC = clang
+CXX = clang++
+
+ARCH := $(shell uname -m)
+OS := $(shell uname -o)
+
+BASEDIR = ../../../../../
+PHONELIBS = ../../../../../phonelibs
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+             -Werror=incompatible-pointer-types \
+             -Werror=int-conversion \
+             -Werror=return-type \
+             -Werror=format-extra-args
+
+CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) -Wall
+CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) -Wall
+ZMQ_LIBS = -l:libczmq.a -l:libzmq.a
+
+ifeq ($(ARCH),aarch64)
+CFLAGS += -mcpu=cortex-a57
+CXXFLAGS += -mcpu=cortex-a57
+ZMQ_LIBS += -lgnustl_shared
+endif
+
+
+EXTRA_LIBS = -lpthread
+
+ifeq ($(ARCH),x86_64)
+ZMQ_FLAGS = -I$(BASEDIR)/phonelibs/zmq/x64/include
+ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib \
+           -l:libczmq.a -l:libzmq.a
+ZMQ_SHARED_LIBS = -L$(BASEDIR)/external/zmq/lib \
+                  -lczmq -lzmq
+else
+EXTRA_LIBS += -llog -luuid
+endif
+
+.PHONY: all
+all: sender receiver
+
+receiver: receiver.o
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+            $(CEREAL_LIBS) \
+            $(ZMQ_LIBS) \
+            $(EXTRA_LIBS)
+
+sender: sender.o
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+            $(CEREAL_LIBS) \
+            $(ZMQ_LIBS) \
+            $(EXTRA_LIBS)
+
+%.o: %.cc
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) -MMD \
+          -Iinclude -I.. -I../.. \
+           $(CEREAL_CXXFLAGS) \
+           $(ZMQ_FLAGS) \
+           $(JSON11_FLAGS) \
+           $(JSON_FLAGS) \
+           -I../ \
+           -I../../ \
+           -c -o '$@' '$<'
+
+
+.PHONY: clean
+clean:
+	rm -f *.d sender receiver
--- a/selfdrive/debug/internal/messaging/zmq/receiver.cc
+++ b/selfdrive/debug/internal/messaging/zmq/receiver.cc
@ -0,0 +1,49 @@
+#include <future>
+#include <iostream>
+#include <cstring>
+
+#include <zmq.h>
+
+// #define IPC
+
+void *sub_sock(void *ctx, const char *endpoint) {
+  void* sock = zmq_socket(ctx, ZMQ_SUB);
+  zmq_connect(sock, endpoint);
+  zmq_setsockopt(sock, ZMQ_SUBSCRIBE, "", 0);
+
+  return sock;
+}
+
+void *pub_sock(void *ctx, const char *endpoint){
+  void * sock = zmq_socket(ctx, ZMQ_PUB);
+
+  zmq_bind(sock, endpoint);
+
+  return sock;
+}
+
+int main(int argc, char *argv[]) {
+  auto ctx = zmq_ctx_new();
+
+#ifdef IPC
+  auto s_sock = sub_sock(ctx, "ipc:///tmp/q0");
+  auto p_sock = pub_sock(ctx, "ipc:///tmp/q1");
+ #else
+  auto s_sock = sub_sock(ctx, "tcp://localhost:10005");
+  auto p_sock = pub_sock(ctx, "tcp://*:10004");
+ #endif
+
+  zmq_msg_t msg;
+  zmq_msg_init(&msg);
+
+
+  while (true){
+    zmq_msg_recv(&msg, s_sock, 0);
+    zmq_msg_send(&msg, p_sock, ZMQ_DONTWAIT);
+  }
+
+  zmq_msg_close(&msg);
+  zmq_close(p_sock);
+  zmq_close(s_sock);
+  return 0;
+}
--- a/selfdrive/debug/internal/messaging/zmq/sender.cc
+++ b/selfdrive/debug/internal/messaging/zmq/sender.cc
@ -0,0 +1,65 @@
+#include <iostream>
+#include <zmq.h>
+#include <chrono>
+
+#define N 1024
+#define MSGS 1e5
+
+// #define IPC
+
+void *sub_sock(void *ctx, const char *endpoint) {
+  void* sock = zmq_socket(ctx, ZMQ_SUB);
+  zmq_connect(sock, endpoint);
+  zmq_setsockopt(sock, ZMQ_SUBSCRIBE, "", 0);
+
+  int timeout = 100;
+  zmq_setsockopt(sock, ZMQ_RCVTIMEO, &timeout, sizeof(int));
+
+  return sock;
+}
+
+void *pub_sock(void *ctx, const char *endpoint){
+  void * sock = zmq_socket(ctx, ZMQ_PUB);
+  zmq_bind(sock, endpoint);
+
+  return sock;
+}
+
+
+int main(int argc, char *argv[]) {
+  auto ctx = zmq_ctx_new();
+
+#ifdef IPC
+  auto s_sock = sub_sock(ctx, "ipc:///tmp/q1");
+  auto p_sock = pub_sock(ctx, "ipc:///tmp/q0");
+#else
+  auto s_sock = sub_sock(ctx, "tcp://127.0.0.1:10004");
+  auto p_sock = pub_sock(ctx, "tcp://*:10005");
+#endif
+
+  zmq_msg_t msg;
+  zmq_msg_init_size (&msg, N);
+
+  auto start = std::chrono::steady_clock::now();
+
+  for (int i = 0; i < MSGS; i++){
+    zmq_msg_send(&msg, p_sock, ZMQ_DONTWAIT);
+    int r = zmq_msg_recv(&msg, s_sock, 0);
+    if (r) {
+      start = std::chrono::steady_clock::now();
+      std::cout << "Timeout" << std::endl;
+    }
+  }
+  auto end = std::chrono::steady_clock::now();
+
+
+  double elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count() / 1e9;
+  double throughput = ((double) MSGS / (double) elapsed);
+
+  std::cout << "Elapsed: " << elapsed << " s" << std::endl;
+  std::cout << "Throughput: " << throughput << " msg/s" << std::endl;
+
+  zmq_close(p_sock);
+  zmq_close(s_sock);
+  return 0;
+}
--- a/selfdrive/debug/internal/messaging_benchmark.py
+++ b/selfdrive/debug/internal/messaging_benchmark.py
@ -0,0 +1,17 @@
+#!/usr/bin/env python3
+import time
+import cereal.messaging as messaging
+
+
+def init_message_bench(N=100000):
+    t = time.time()
+    for _ in range(N):
+        dat = messaging.new_message()
+        dat.init('controlsState')
+
+    dt = time.time() - t
+    print("Init message %d its, %f s" % (N, dt))
+
+
+if __name__ == "__main__":
+    init_message_bench()
--- a/selfdrive/debug/internal/mock_process/fake_UiNavigationEvent.py
+++ b/selfdrive/debug/internal/mock_process/fake_UiNavigationEvent.py
@ -0,0 +1,21 @@
+#!/usr/bin/env python3
+import zmq
+import time
+from cereal.services import service_list
+import cereal.messaging as messaging
+from cereal import log 
+  
+def mock():
+  traffic_events = messaging.pub_sock('uiNavigationEvent')
+
+  while 1:
+    m = messaging.new_message()
+    m.init('uiNavigationEvent')
+    m.uiNavigationEvent.type = log.UiNavigationEvent.Type.mergeRight
+    m.uiNavigationEvent.status = log.UiNavigationEvent.Status.active
+    m.uiNavigationEvent.distanceTo = 100.
+    traffic_events.send(m.to_bytes())
+    time.sleep(0.01)
+
+if __name__=="__main__":
+  mock()
--- a/selfdrive/debug/internal/mock_process/fake_controls_state.py
+++ b/selfdrive/debug/internal/mock_process/fake_controls_state.py
@ -0,0 +1,22 @@
+#!/usr/bin/env python3
+import time
+import zmq
+from hexdump import hexdump
+
+from common.realtime import Ratekeeper
+import cereal.messaging as messaging
+from cereal.services import service_list
+
+if __name__ == "__main__":
+  controls_state = messaging.pub_sock('controlsState')
+
+  rk = Ratekeeper(100)
+  while 1:
+    dat = messaging.new_message()
+    dat.init('controlsState')
+
+    dat.controlsState.vEgo = 25.
+    dat.controlsState.enabled = True
+    controls_state.send(dat.to_bytes())
+
+    rk.keep_time()
--- a/selfdrive/debug/internal/mock_process/fake_fusion_state.py
+++ b/selfdrive/debug/internal/mock_process/fake_fusion_state.py
@ -0,0 +1,27 @@
+#!/usr/bin/env python3
+import zmq
+import time
+from hexdump import hexdump
+import cereal.messaging as messaging
+from cereal.services import service_list
+from cereal import log
+
+def leadRange(start, end, step):
+    x = start
+    while x < end:
+        yield x
+        x += (x * step)
+
+def mock_lead():
+  radarState = messaging.pub_sock('radarState')
+  while 1:
+    m = messaging.new_message()
+    m.init('radarState')
+    m.radarState.leadOne.status = True
+    for x in leadRange(3.0, 65.0, 0.005):
+        m.radarState.leadOne.dRel = x
+        radarState.send(m.to_bytes())
+        time.sleep(0.01)
+
+if __name__=="__main__":
+  mock_lead()
--- a/selfdrive/debug/internal/mock_process/fake_gps.py
+++ b/selfdrive/debug/internal/mock_process/fake_gps.py
@ -0,0 +1,46 @@
+# mock_gps.py: Publishes a generated path moving at 15m/s to gpsLocation
+# USAGE: python mock_gps.py
+# Then start manager
+
+from itertools import cycle
+import time
+import zmq
+
+from cereal import log
+import cereal.messaging as messaging
+from cereal.services import service_list
+
+degrees_per_meter = 0.000009000009 # approximation
+start_lat = 43.64199141443989
+start_lng = -94.97520411931725
+
+def gen_path(length_seconds, speed=15):
+    return [{"lat": start_lat,
+             "lng": start_lng + speed * i * degrees_per_meter, # moving along longitudinal axis at speed m/s
+             "speed": speed}
+             for i in range(1, length_seconds + 1)]
+
+if __name__ == '__main__':
+    gpsLocation = messaging.pub_sock('gpsLocation')
+
+    path_stopped_5s = [{"lat": start_lat, "lng": start_lng, "speed": 0}] * 5
+    path_moving = gen_path(30, speed=15)
+    path_stopped_5s_then_moving = path_stopped_5s + path_moving
+
+    for point in cycle(path_stopped_5s_then_moving):
+        print('sending gpsLocation from point: {}'.format(str(point)))
+        dat = messaging.new_message()
+        dat.init('gpsLocation')
+        dat.gpsLocation.latitude = point['lat']
+        dat.gpsLocation.longitude = point['lng']
+        dat.gpsLocation.speed = point['speed']
+        dat.gpsLocation.flags = 0
+        dat.gpsLocation.altitude = 0
+        dat.gpsLocation.bearing = 0 # todo we can mock this
+        dat.gpsLocation.accuracy = 1
+        dat.gpsLocation.timestamp = int(time.time() * 1000)
+        dat.gpsLocation.source = log.GpsLocationData.SensorSource.android
+
+        gpsLocation.send(dat.to_bytes())
+        time.sleep(1)
+
--- a/selfdrive/debug/internal/mock_process/fake_gps_external.py
+++ b/selfdrive/debug/internal/mock_process/fake_gps_external.py
@ -0,0 +1,27 @@
+#!/usr/bin/env python3
+import time
+import zmq
+
+from cereal import log
+import cereal.messaging as messaging
+from cereal.services import service_list
+
+
+if __name__ == '__main__':
+    gpsLocationExternal = messaging.pub_sock('gpsLocationExternal')
+
+    while True:
+        dat = messaging.new_message()
+        dat.init('gpsLocationExternal')
+        dat.gpsLocationExternal.latitude = 37.6513687
+        dat.gpsLocationExternal.longitude = -122.4535056
+        dat.gpsLocationExternal.speed = 28.2
+        dat.gpsLocationExternal.flags = 1
+        dat.gpsLocationExternal.altitude = 75.
+        dat.gpsLocationExternal.bearing = 145.5
+        dat.gpsLocationExternal.accuracy = 1.
+        dat.gpsLocationExternal.timestamp = int(time.time() * 1000)
+        dat.gpsLocationExternal.source = log.GpsLocationData.SensorSource.ublox
+
+        gpsLocationExternal.send(dat.to_bytes())
+        time.sleep(.1)
--- a/selfdrive/debug/internal/mock_process/fake_liveMpc.py
+++ b/selfdrive/debug/internal/mock_process/fake_liveMpc.py
@ -0,0 +1,22 @@
+#!/usr/bin/env python3
+import zmq
+import time
+from hexdump import hexdump
+import cereal.messaging as messaging
+from cereal.services import service_list
+from cereal import log
+
+def mock_x():
+  liveMpc = messaging.pub_sock('liveMpc')
+  while 1:
+    m = messaging.new_message()
+    mx = []
+    m.init('liveMpc')
+    for x in range(0, 100):
+        mx.append(x*1.0)
+        m.liveMpc.x = mx
+
+    liveMpc.send(m.to_bytes())
+
+if __name__=="__main__":
+  mock_x()
--- a/selfdrive/debug/internal/mock_process/fake_trafficsignd.py
+++ b/selfdrive/debug/internal/mock_process/fake_trafficsignd.py
@ -0,0 +1,22 @@
+#!/usr/bin/env python3
+import zmq
+import time
+from cereal.services import service_list
+import cereal.messaging as messaging
+from cereal import log
+
+def mock():
+  traffic_events = messaging.pub_sock('trafficEvents')
+
+  while 1:
+    m = messaging.new_message()
+    m.init('trafficEvents', 1)
+    m.trafficEvents[0].type = log.TrafficEvent.Type.stopSign
+    m.trafficEvents[0].resuming = False
+    m.trafficEvents[0].distance = 100.
+    m.trafficEvents[0].action = log.TrafficEvent.Action.stop
+    traffic_events.send(m.to_bytes())
+    time.sleep(0.01)
+
+if __name__=="__main__":
+  mock()
--- a/selfdrive/debug/internal/model_timing_checker.py
+++ b/selfdrive/debug/internal/model_timing_checker.py
@ -0,0 +1,41 @@
+#!/usr/bin/env python3
+import zmq
+
+import cereal.messaging as messaging
+from cereal.services import service_list
+
+if __name__ == "__main__":
+  poller = zmq.Poller()
+
+  fsock = messaging.sub_sock("frame", poller)
+  msock = messaging.sub_sock("model", poller)
+
+  frmTimes = {}
+  proc = []
+
+  last100 = []
+
+  while 1:
+    polld = poller.poll(timeout=1000)
+    for sock, mode in polld:
+      if mode != zmq.POLLIN:
+        continue
+      if sock == fsock:
+        f = messaging.recv_one(sock)
+        frmTimes[f.frame.frameId] = f.frame.timestampEof
+      else:
+        proc.append(messaging.recv_one(sock))
+        nproc = []
+        for mm in proc:
+          fid = mm.model.frameId
+
+          if fid in frmTimes:
+            tm = (mm.logMonoTime-frmTimes[fid])/1e6
+            del frmTimes[fid]
+            last100.append(tm)
+            last100 = last100[-100:]
+            print("%10d: %.2f ms    min: %.2f  max: %.2f" %  (fid, tm, min(last100), max(last100)))
+          else:
+            nproc.append(mm)
+        proc = nproc
+
--- a/selfdrive/debug/internal/polyfit_bench.py
+++ b/selfdrive/debug/internal/polyfit_bench.py
@ -0,0 +1,96 @@
+import timeit
+
+import numpy as np
+import numpy.linalg
+from scipy.linalg import cho_factor, cho_solve
+
+# We are trying to solve the following system
+# (A.T * A) * x = A.T * b
+# Where x are the polynomial coefficients and b is are the input points
+
+# First we build A
+deg = 3
+x = np.arange(50 * 1.0)
+A = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
+
+# The first way to solve this is using the pseudoinverse, which can be precomputed
+# x = (A.T * A)^-1 * A^T * b = PINV b
+PINV = np.linalg.pinv(A)
+
+# Another way is using the Cholesky decomposition
+# We can note that at (A.T * A) is always positive definite
+# By precomputing the Cholesky decomposition we can efficiently solve
+# systems of the form (A.T * A) x = c
+CHO = cho_factor(np.dot(A.T, A))
+
+
+def model_polyfit_old(points, deg=3):
+  A = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
+  pinv = np.linalg.pinv(A)
+  return np.dot(pinv, map(float, points))
+
+
+def model_polyfit(points, deg=3):
+  A = np.vander(x, deg + 1)
+  pinv = np.linalg.pinv(A)
+  return np.dot(pinv, map(float, points))
+
+
+def model_polyfit_cho(points, deg=3):
+  A = np.vander(x, deg + 1)
+  cho = cho_factor(np.dot(A.T, A))
+  c = np.dot(A.T, points)
+  return cho_solve(cho, c, check_finite=False)
+
+
+def model_polyfit_np(points, deg=3):
+  return np.polyfit(x, points, deg)
+
+
+def model_polyfit_lstsq(points, deg=3):
+  A = np.vander(x, deg + 1)
+  return np.linalg.lstsq(A, points, rcond=None)[0]
+
+
+TEST_DATA = np.linspace(0, 5, num=50) + 1.
+
+
+def time_pinv_old():
+  model_polyfit_old(TEST_DATA)
+
+
+def time_pinv():
+  model_polyfit(TEST_DATA)
+
+
+def time_cho():
+  model_polyfit_cho(TEST_DATA)
+
+
+def time_np():
+  model_polyfit_np(TEST_DATA)
+
+
+def time_lstsq():
+  model_polyfit_lstsq(TEST_DATA)
+
+
+if __name__ == "__main__":
+  # Verify correct results
+  pinv_old = model_polyfit_old(TEST_DATA)
+  pinv = model_polyfit(TEST_DATA)
+  cho = model_polyfit_cho(TEST_DATA)
+  numpy = model_polyfit_np(TEST_DATA)
+  lstsq = model_polyfit_lstsq(TEST_DATA)
+
+  assert all(np.isclose(pinv, pinv_old))
+  assert all(np.isclose(pinv, cho))
+  assert all(np.isclose(pinv, numpy))
+  assert all(np.isclose(pinv, lstsq))
+
+  # Run benchmark
+  print("Pseudo inverse (old)", timeit.timeit("time_pinv_old()", setup="from __main__ import time_pinv_old", number=10000))
+  print("Pseudo inverse", timeit.timeit("time_pinv()", setup="from __main__ import time_pinv", number=10000))
+  print("Cholesky", timeit.timeit("time_cho()", setup="from __main__ import time_cho", number=10000))
+  print("Numpy leastsq", timeit.timeit("time_lstsq()", setup="from __main__ import time_lstsq", number=10000))
+  print("Numpy polyfit", timeit.timeit("time_np()", setup="from __main__ import time_np", number=10000))
--- a/selfdrive/debug/internal/power_monitor.py
+++ b/selfdrive/debug/internal/power_monitor.py
@ -0,0 +1,64 @@
+#!/usr/bin/env python3
+import os
+import time
+import sys
+from datetime import datetime
+
+def average(avg, sample):
+  # Weighted avg between existing value and new sample
+  return ((avg[0] * avg[1] + sample) / (avg[1] + 1), avg[1] + 1)
+
+
+if __name__ == '__main__':
+  try:
+    if len(sys.argv) > 1 and sys.argv[1] == "--charge":
+      print("not disabling charging")
+    else:
+      print("disabling charging")
+      os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled')
+
+    voltage_average = (0., 0) # average, count
+    current_average = (0., 0)
+    power_average = (0., 0)
+    capacity_average = (0., 0)
+    bat_temp_average = (0., 0)
+    start_time = datetime.now()
+    while 1:
+      with open("/sys/class/power_supply/bms/voltage_now") as f:
+        voltage = int(f.read()) / 1e6   # volts
+
+      with open("/sys/class/power_supply/bms/current_now") as f:
+        current = int(f.read()) / 1e3   # ma
+
+      power = voltage * current
+
+      with open("/sys/class/power_supply/bms/capacity_raw") as f:
+        capacity = int(f.read()) / 1e2  # percent
+
+      with open("/sys/class/power_supply/bms/temp") as f:
+        bat_temp = int(f.read()) / 1e1  # celsius
+
+      # compute averages
+      voltage_average = average(voltage_average, voltage)
+      current_average = average(current_average, current)
+      power_average = average(power_average, power)
+      capacity_average = average(capacity_average, capacity)
+      bat_temp_average = average(bat_temp_average, bat_temp)
+
+      print("%.2f volts %12.2f ma %12.2f mW %8.2f%% battery %8.1f degC" % (voltage, current, power, capacity, bat_temp))
+      time.sleep(0.1)
+  finally:
+    stop_time = datetime.now()
+    print("\n----------------------Average-----------------------------------")
+    voltage = voltage_average[0]
+    current = current_average[0]
+    power = power_average[0]
+    capacity = capacity_average[0]
+    bat_temp = bat_temp_average[0]
+    print("%.2f volts %12.2f ma %12.2f mW %8.2f%% battery %8.1f degC" % (voltage, current, power, capacity, bat_temp))
+    print("  {:.2f} Seconds     {} samples".format((stop_time-start_time).total_seconds(), voltage_average[1]))
+    print("----------------------------------------------------------------")
+
+    # reenable charging
+    os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled')
+    print("charging enabled\n")
--- a/selfdrive/debug/internal/realtime_benchmark.py
+++ b/selfdrive/debug/internal/realtime_benchmark.py
@ -0,0 +1,22 @@
+#!/usr/bin/env python3
+import time
+
+from common.realtime import sec_since_boot, monotonic_time
+
+
+if __name__ == "__main__":
+    N = 100000
+
+    t = time.time()
+    for _ in range(N):
+        monotonic_time()
+    dt = time.time() - t
+
+    print("Monotonic", dt)
+
+    t = time.time()
+    for _ in range(N):
+        sec_since_boot()
+    dt = time.time() - t
+
+    print("Boot", dt)
--- a/selfdrive/debug/internal/run_segment_through_visiond.py
+++ b/selfdrive/debug/internal/run_segment_through_visiond.py
@ -0,0 +1,79 @@
+#!/usr/bin/env python3
+
+import argparse
+import time
+import os
+
+from tqdm import tqdm
+
+from cereal.messaging import PubMaster, recv_one, sub_sock
+from cereal.services import service_list
+from tools.lib.logreader import LogReader
+from xx.chffr.lib.route import Route, RouteSegment
+from tools.lib.route_framereader import RouteFrameReader
+from common.column_store import save_dict_as_column_store
+from xx.pipeline.lib.log_time_series import append_dict
+from selfdrive.test.process_replay.compare_logs import save_log
+
+if __name__ == "__main__":
+  parser = argparse.ArgumentParser(description="Run visiond on segment")
+  parser.add_argument("segment_name", help="The segment to run")
+  parser.add_argument("output_path", help="The output file")
+
+  args = parser.parse_args()
+  segment = RouteSegment.from_canonical_name(args.segment_name)
+  route = Route(segment._name._route_name)
+
+  frame_id_lookup = {}
+  frame_reader = RouteFrameReader(route.camera_paths(), None, frame_id_lookup, readahead=True)
+
+  msgs = list(LogReader(segment.log_path))
+
+  pm = PubMaster(['liveCalibration', 'frame'])
+  model_sock = sub_sock('model')
+
+  # Read encodeIdx
+  for msg in msgs:
+    if msg.which() == 'encodeIdx':
+      frame_id_lookup[msg.encodeIdx.frameId] = (msg.encodeIdx.segmentNum, msg.encodeIdx.segmentId)
+
+  # Send some livecalibration messages to initalize visiond
+  for msg in msgs:
+    if msg.which() == 'liveCalibration':
+      pm.send('liveCalibration', msg.as_builder())
+
+  time.sleep(1.0)
+  values = {}
+
+  out_msgs = []
+  for msg in tqdm(msgs):
+    w = msg.which()
+
+    if w == 'liveCalibration':
+      pm.send(w, msg.as_builder())
+
+    if w == 'frame':
+      msg = msg.as_builder()
+
+      frame_id = msg.frame.frameId
+      img = frame_reader.get(frame_id, pix_fmt="rgb24")[:,:,::-1]
+
+      msg.frame.image = img.flatten().tobytes()
+      pm.send(w, msg)
+
+      model = recv_one(model_sock)
+      model = model.as_builder()
+      model.logMonoTime = 0
+      model = model.as_reader()
+      out_msgs.append(model)
+
+  save_log(args.output_path, out_msgs)
+
+      # tm = model.logMonoTime / 1.0e9
+      # model = model.model
+  #     append_dict("model/data/path", tm, model.path.to_dict(), values)
+  #     append_dict("model/data/left_lane", tm, model.leftLane.to_dict(), values)
+  #     append_dict("model/data/right_lane", tm, model.rightLane.to_dict(), values)
+  #     append_dict("model/data/lead", tm, model.lead.to_dict(), values)
+
+  # save_dict_as_column_store(values, os.path.join(args.output_path, "LiveVisionD", args.segment_name))
--- a/selfdrive/debug/internal/send_alert.py
+++ b/selfdrive/debug/internal/send_alert.py
@ -0,0 +1,22 @@
+#!/usr/bin/env python3
+import time
+import zmq
+from hexdump import hexdump
+
+import cereal.messaging as messaging
+from cereal.services import service_list
+
+if __name__ == "__main__":
+  controls_state = messaging.pub_sock('controlsState')
+
+  while 1:
+    dat = messaging.new_message()
+    dat.init('controlsState')
+
+    dat.controlsState.alertText1 = "alert text 1"
+    dat.controlsState.alertText2 = "alert text 2"
+    dat.controlsState.alertType = "test"
+    dat.controlsState.alertSound = "chimeDisengage"
+    controls_state.send(dat.to_bytes())
+
+    time.sleep(0.01)
--- a/selfdrive/debug/internal/status.py
+++ b/selfdrive/debug/internal/status.py
@ -0,0 +1,107 @@
+import os
+import sys
+
+import zmq
+from lru import LRU
+
+from cereal import log
+from common.realtime import Ratekeeper
+import cereal.messaging as messaging
+from cereal.services import service_list
+
+def cputime_total(ct):
+  return ct.user+ct.nice+ct.system+ct.idle+ct.iowait+ct.irq+ct.softirq
+
+def cputime_busy(ct):
+  return ct.user+ct.nice+ct.system+ct.irq+ct.softirq
+
+def cpu_dtotal(l1, l2):
+  t1_total = sum(cputime_total(ct) for ct in l1.cpuTimes)
+  t2_total = sum(cputime_total(ct) for ct in l2.cpuTimes)
+  return t2_total - t1_total
+
+def cpu_percent(l1, l2):
+  dtotal = cpu_dtotal(l1, l2)
+  t1_busy = sum(cputime_busy(ct) for ct in l1.cpuTimes)
+  t2_busy = sum(cputime_busy(ct) for ct in l2.cpuTimes)
+
+  dbusy = t2_busy - t1_busy
+
+  if dbusy < 0 or dtotal <= 0:
+    return 0.0
+  return dbusy / dtotal
+
+def proc_cpu_percent(proc1, proc2, l1, l2):
+  dtotal = cpu_dtotal(l1, l2)
+
+  dproc = (proc2.cpuUser+proc2.cpuSystem) - (proc1.cpuUser+proc1.cpuSystem)
+  if dproc < 0:
+    return 0.0
+
+  return dproc / dtotal
+
+def display_cpu(pl1, pl2):
+  l1, l2 = pl1.procLog, pl2.procLog
+
+  print(cpu_percent(l1, l2))
+
+  procs1 = dict((proc.pid, proc) for proc in l1.procs)
+  procs2 = dict((proc.pid, proc) for proc in l2.procs)
+
+  procs_print = 4
+
+  procs_with_percent = sorted((proc_cpu_percent(procs1[proc.pid], proc, l1, l2), proc) for proc in l2.procs
+                               if proc.pid in procs1)
+  for percent, proc in procs_with_percent[-1:-procs_print-1:-1]:
+    print(percent, proc.name)
+
+  print()
+
+
+def main():
+  frame_cache = LRU(16)
+  md_cache = LRU(16)
+  plan_cache = LRU(16)
+
+  frame_sock = messaging.sub_sock('frame')
+  md_sock = messaging.sub_sock('model')
+  plan_sock = messaging.sub_sock('plan')
+  controls_state_sock = messaging.sub_sock('controlsState')
+
+  proc = messaging.sub_sock('procLog')
+  pls = [None, None]
+
+  rk = Ratekeeper(10)
+  while True:
+
+    for msg in messaging.drain_sock(frame_sock):
+      frame_cache[msg.frame.frameId] = msg
+
+    for msg in messaging.drain_sock(md_sock):
+      md_cache[msg.logMonoTime] = msg
+
+    for msg in messaging.drain_sock(plan_sock):
+      plan_cache[msg.logMonoTime] = msg
+
+    controls_state = messaging.recv_sock(controls_state_sock)
+    if controls_state is not None:
+      plan_time = controls_state.controlsState.planMonoTime
+      if plan_time != 0 and plan_time in plan_cache:
+        plan = plan_cache[plan_time]
+        md_time = plan.plan.mdMonoTime
+        if md_time != 0 and md_time in md_cache:
+          md = md_cache[md_time]
+          frame_id = md.model.frameId
+          if frame_id != 0 and frame_id in frame_cache:
+            frame = frame_cache[frame_id]
+            print("controls lag: %.2fms" % ((controls_state.logMonoTime - frame.frame.timestampEof) / 1e6))
+
+
+    pls = (pls+messaging.drain_sock(proc))[-2:]
+    if None not in pls:
+      display_cpu(*pls)
+
+    rk.keep_time()
+
+if __name__ == "__main__":
+  main()
--- a/selfdrive/debug/internal/time_to_collision.py
+++ b/selfdrive/debug/internal/time_to_collision.py
@ -0,0 +1,83 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+from matplotlib import cm
+from matplotlib.ticker import LinearLocator, FormatStrFormatter
+from scipy.optimize import minimize
+
+a = -9.81
+dt = 0.1
+
+r = 2.0
+
+v_ls = []
+x_ls = []
+v_egos = []
+
+for vv_ego in np.arange(35, 40, 1):
+    for vv_l in np.arange(35, 40, 1):
+        for xx_l in np.arange(0, 100, 1.0):
+            x_l = xx_l
+            v_l = vv_l
+            v_ego = vv_ego
+            x_ego = 0.0
+
+            ttc = None
+            for t in np.arange(0, 100, dt):
+                x_l += v_l * dt
+                v_l += a * dt
+                v_l = max(v_l, 0.0)
+
+                x_ego += v_ego * dt
+                if t > r:
+                    v_ego += a * dt
+                    v_ego = max(v_ego, 0.0)
+
+                if x_ego >= x_l:
+                    ttc = t
+                    break
+
+            if ttc is None:
+                if xx_l < 0.1:
+                    break
+
+                v_ls.append(vv_l)
+                x_ls.append(xx_l)
+                v_egos.append(vv_ego)
+                break
+
+
+def eval_f(x, v_ego, v_l):
+    est = x[0] * v_l + x[1] * v_l**2 \
+            + x[2] * v_ego + x[3] * v_ego**2
+    return est
+
+def f(x):
+    r = 0.0
+    for v_ego, v_l, x_l in zip(v_egos, v_ls, x_ls):
+        est = eval_f(x, v_ego, v_l)
+        r += (x_l - est)**2
+
+    return r
+
+x0 = [0.5, 0.5, 0.5, 0.5]
+res = minimize(f, x0, method='Nelder-Mead')
+print(res)
+print(res.x)
+
+g = 9.81
+t_r = 1.8
+
+estimated = [4.0 + eval_f(res.x, v_ego, v_l) for (v_ego, v_l) in zip(v_egos, v_ls)]
+new_formula = [4.0 + v_ego * t_r - (v_l - v_ego) * t_r + v_ego**2/(2*g) - v_l**2 / (2*g)  for (v_ego, v_l) in zip(v_egos, v_ls)]
+
+fig = plt.figure()
+ax = fig.add_subplot(111, projection='3d')
+surf = ax.scatter(v_egos, v_ls, x_ls, s=1)
+# surf = ax.scatter(v_egos, v_ls, estimated, s=1)
+surf = ax.scatter(v_egos, v_ls, new_formula, s=1)
+
+ax.set_xlabel('v ego')
+ax.set_ylabel('v lead')
+ax.set_zlabel('min distance')
+plt.show()
--- a/selfdrive/debug/internal/topdown.py
+++ b/selfdrive/debug/internal/topdown.py
@ -0,0 +1,178 @@
+#!/usr/bin/env python3
+import sys
+import math
+import pygame
+import pyproj
+
+import zmq
+import cereal.messaging as messaging
+from cereal.services import service_list
+import numpy as np
+
+METER = 25
+YSCALE = 1
+
+def to_grid(pt):
+  return (int(round(pt[0] * METER + 100)), int(round(pt[1] * METER * YSCALE + 500)))
+
+def gps_latlong_to_meters(gps_values, zero):
+  inProj = pyproj.Proj(init='epsg:4326')
+  outProj = pyproj.Proj(("+proj=tmerc +lat_0={:f} +lon_0={:f} +units=m"
+                         " +k=1. +x_0=0 +y_0=0 +ellps=WGS84 +datum=WGS84 +no_defs"
+                         "+towgs84=-90.7,-106.1,-119.2,4.09,0.218,-1.05,1.37").format(*zero))
+  gps_x, gps_y = pyproj.transform(inProj, outProj, gps_values[1], gps_values[0])
+  return gps_x, gps_y
+
+def rot(hrad):
+  return [[math.cos(hrad), -math.sin(hrad)],
+          [math.sin(hrad),  math.cos(hrad)]]
+
+class Car():
+  CAR_WIDTH = 2.0
+  CAR_LENGTH = 4.5
+
+  def __init__(self, c):
+    self.car = pygame.Surface((METER*self.CAR_LENGTH*YSCALE, METER*self.CAR_LENGTH))
+    self.car.set_alpha(64)
+    self.car.fill((0,0,0))
+    self.car.set_colorkey((0,0,0))
+    pygame.draw.rect(self.car, c, (METER*1.25*YSCALE, 0, METER*self.CAR_WIDTH*YSCALE, METER*self.CAR_LENGTH), 1)
+
+    self.x = 0.0
+    self.y = 0.0
+    self.heading = 0.0
+
+  def from_car_frame(self, pts):
+    ret = []
+    for x, y in pts:
+      rx, ry = np.dot(rot(math.radians(self.heading)), [x,y])
+      ret.append((self.x + rx, self.y + ry))
+    return ret
+
+  def draw(self, screen):
+    cars = pygame.transform.rotate(self.car, 90-self.heading)
+    pt = (self.x - self.CAR_LENGTH/2, self.y - self.CAR_LENGTH/2)
+    screen.blit(cars, to_grid(pt))
+
+
+def ui_thread(addr="127.0.0.1"):
+  #from selfdrive.radar.nidec.interface import RadarInterface
+  #RI = RadarInterface()
+
+  pygame.display.set_caption("comma top down UI")
+  size = (1920,1000)
+  screen = pygame.display.set_mode(size, pygame.DOUBLEBUF)
+
+  liveLocation = messaging.sub_sock('liveLocation', addr=addr)
+
+  #model = messaging.sub_sock('testModel', addr=addr)
+  model = messaging.sub_sock('model', addr=addr)
+
+  plan = messaging.sub_sock('plan', addr=addr)
+  frame = messaging.sub_sock('frame', addr=addr)
+  liveTracks = messaging.sub_sock('liveTracks', addr=addr)
+
+  car = Car((255,0,255))
+
+  base = None
+
+  lb = []
+
+  ts_map = {}
+
+  while 1:
+    lloc = messaging.recv_sock(liveLocation, wait=True)
+    lloc_ts = lloc.logMonoTime
+    lloc = lloc.liveLocation
+
+    # 50 ms of lag
+    lb.append(lloc)
+    if len(lb) < 2:
+      continue
+    lb = lb[-1:]
+
+    lloc = lb[0]
+
+    # spacebar reset
+    for event in pygame.event.get():
+      if event.type == pygame.KEYDOWN and event.key == pygame.K_SPACE:
+        base = None
+
+    # offscreen reset
+    rp = to_grid((car.x, car.y))
+    if rp[0] > (size[0] - 100) or rp[1] > (size[1] - 100) or rp[0] < 0 or rp[1] < 100:
+      base = None
+
+
+    if base == None:
+      screen.fill((10,10,10))
+      base = lloc
+
+    # transform pt into local
+    pt = gps_latlong_to_meters((lloc.lat, lloc.lon), (base.lat, base.lon))
+    hrad = math.radians(270+base.heading)
+    pt = np.dot(rot(hrad), pt)
+
+    car.x, car.y = pt[0], -pt[1]
+    car.heading = lloc.heading - base.heading
+
+    #car.draw(screen)
+    pygame.draw.circle(screen, (192,64,192,128), to_grid((car.x, car.y)), 4)
+
+    """
+    lt = messaging.recv_sock(liveTracks, wait=False)
+    if lt is not None:
+      for track in lt.liveTracks:
+        pt = car.from_car_frame([[track.dRel, -track.yRel]])[0]
+        if track.stationary:
+          pygame.draw.circle(screen, (192,128,32,64), to_grid(pt), 1)
+    """
+
+
+    """
+    rr = RI.update()
+    for pt in rr.points:
+      cpt = car.from_car_frame([[pt.dRel + 2.7, -pt.yRel]])[0]
+      if (pt.vRel + lloc.speed) < 1.0:
+        pygame.draw.circle(screen, (192,128,32,64), to_grid(cpt), 1)
+    """
+
+
+    for f in messaging.drain_sock(frame):
+      ts_map[f.frame.frameId] = f.frame.timestampEof
+
+    def draw_model_data(mm, c):
+      pts = car.from_car_frame(zip(np.arange(0.0, 50.0), -np.array(mm)))
+      lt = 255
+      for pt in pts:
+        screen.set_at(to_grid(pt), (c[0]*lt,c[1]*lt,c[2]*lt,lt))
+        lt -= 2
+      #pygame.draw.lines(screen, (c[0]*lt,c[1]*lt,c[2]*lt,lt), False, map(to_grid, pts), 1)
+
+    md = messaging.recv_sock(model, wait=False)
+    if md:
+      if md.model.frameId in ts_map:
+        f_ts = ts_map[md.model.frameId]
+        print((lloc_ts - f_ts) * 1e-6,"ms")
+
+      #draw_model_data(md.model.path.points, (1,0,0))
+      if md.model.leftLane.prob > 0.3:
+        draw_model_data(md.model.leftLane.points, (0,1,0))
+      if md.model.rightLane.prob > 0.3:
+        draw_model_data(md.model.rightLane.points, (0,1,0))
+      #if md.model.leftLane.prob > 0.3 and md.model.rightLane.prob > 0.3:
+      #  draw_model_data([(x+y)/2 for x,y in zip(md.model.leftLane.points, md.model.rightLane.points)], (1,1,0))
+
+    tplan = messaging.recv_sock(plan, wait=False)
+    if tplan:
+      pts = np.polyval(tplan.plan.dPoly, np.arange(0.0, 50.0))
+      draw_model_data(pts, (1,1,1))
+
+    pygame.display.flip()
+
+if __name__ == "__main__":
+  if len(sys.argv) > 1:
+    ui_thread(sys.argv[1])
+  else:
+    ui_thread()
+
--- a/selfdrive/debug/internal/toyota/blindspot.py
+++ b/selfdrive/debug/internal/toyota/blindspot.py
@ -0,0 +1,152 @@
+#!/usr/bin/env python3
+import zmq
+import time
+import random
+from collections import defaultdict, OrderedDict
+
+from selfdrive.boardd.boardd import can_list_to_can_capnp
+from selfdrive.car.toyota.toyotacan import make_can_msg
+import cereal.messaging as messaging
+from cereal.services import service_list
+
+
+fields = range(0, 256)
+fields = [105, 225]
+field_results = defaultdict(lambda: "\x00\x00")
+cur_field = 97
+
+def send(sendcan, addr, m):
+   packet = make_can_msg(addr, m, 0, False)
+   packets = can_list_to_can_capnp([packet], msgtype='sendcan')
+   sendcan.send(packets.to_bytes())
+
+
+def recv(can, addr):
+   received = False
+   r = []
+
+   while not received:
+      c = messaging.recv_one(can)
+      for msg in c.can:
+         if msg.address == addr:
+            r.append(msg)
+            received = True
+   return r
+
+
+def recv_timeout(can, addr):
+   received = False
+   r = []
+   t = time.time()
+
+   while not received:
+      c = messaging.recv_one_or_none(can)
+
+      if c is not None:
+         for msg in c.can:
+            if msg.address == addr:
+               r.append(msg)
+               received = True
+
+      if time.time() - t > 0.05:
+         received = True
+
+   return r
+
+
+def print_hex(d):
+   s = map(ord, d)
+   s = "".join(["{:02X}".format(b) for b in s])
+   print(s)
+
+
+TYPES = {
+   0: 'single',
+   1: 'first',
+   2: 'consecutive',
+   3: 'flow'
+}
+
+FIRST = "\x42\x02\xA8\x01\x00\x00\x00\x00"
+CONTINUE = "\x42\x30\x01\x00\x00\x00\x00\x00"
+
+TEST_ON = "\x42\x02\x10\x60\x00\x00\x00\x00"
+TEST_OFF = "\x42\x02\x10\x5F\x00\x00\x00\x00"
+
+POLL = "\x42\x02\x21\x69\x00\x00\x00\x00"
+
+prev_rcv_t = ""
+recv_data = []
+l = 0
+index = 0
+
+
+can = messaging.sub_sock('can')
+sendcan = messaging.pub_sock('sendcan')
+
+time.sleep(0.5)
+
+send(sendcan, 1872, FIRST)
+results = []
+
+test_mode = False
+
+while True:
+   # Send flow control if necessary
+   if prev_rcv_t == 'first' or prev_rcv_t == 'consecutive':
+      send(sendcan, 1872, CONTINUE)
+
+   received = recv_timeout(can, 1880)
+
+   if len(received) == 0:
+      print(chr(27) + "[2J")
+      print(time.time())
+      if results[0] != "\x7F\x21\x31":
+         field_results[cur_field] = results[0]
+      else:
+         fields.remove(cur_field)
+      for k in fields:
+         if field_results[k] == "\x00\x00":
+            continue
+         print(k, end=' ')
+         print_hex(field_results[k])
+      results = []
+
+      if not test_mode:
+         send(sendcan, 1872, TEST_ON)
+         test_mode = True
+      else:
+         cur_field = random.choice(fields)
+         send(sendcan, 1872, POLL.replace('\x69', chr(cur_field)))
+
+   for r in received:
+      data = r.dat
+
+      # Check message type
+      t = TYPES[ord(data[1]) >> 4]
+      if t == 'single':
+         l = ord(data[1]) & 0x0F
+      elif t == 'first':
+         a = ord(data[1]) & 0x0F
+         b = ord(data[2])
+         l = b + (a << 8)
+         recv_data = []
+
+      prev_rcv_t = t
+
+      if t == 'single':
+         recv_data = data[2: 2 + l]
+         results.append(recv_data)
+      if t == 'first':
+         index = 0
+         recv_data += data[3: min(8, 3 + l)]
+      if t == 'consecutive':
+         index += 1
+         assert index == ord(data[1]) & 0x0F
+
+         pending_l = l - len(recv_data)
+         recv_data += data[2: min(8, 2 + pending_l)]
+
+         if len(recv_data) == l:
+            prev_rcv_t = ""
+            results.append(recv_data)
--- a/selfdrive/debug/internal/toyota/blindspot2.py
+++ b/selfdrive/debug/internal/toyota/blindspot2.py
@ -0,0 +1,151 @@
+#!/usr/bin/env python3
+import zmq
+import time
+import random
+from collections import defaultdict, OrderedDict
+
+from selfdrive.boardd.boardd import can_list_to_can_capnp
+from selfdrive.car.toyota.toyotacan import make_can_msg
+import cereal.messaging as messaging
+from cereal.services import service_list
+
+
+fields = range(0, 256)
+# fields = [105, 225]
+fields = [105]
+field_results = defaultdict(lambda: "\x00\x00")
+cur_field = 97
+
+def send(sendcan, addr, m):
+   packet = make_can_msg(addr, m, 0, False)
+   packets = can_list_to_can_capnp([packet], msgtype='sendcan')
+   sendcan.send(packets.to_bytes())
+
+
+def recv(can, addr):
+   received = False
+   r = []
+
+   while not received:
+      c = messaging.recv_one(can)
+      for msg in c.can:
+         if msg.address == addr:
+            r.append(msg)
+            received = True
+   return r
+
+
+def recv_timeout(can, addr):
+   received = False
+   r = []
+   t = time.time()
+
+   while not received:
+      c = messaging.recv_one_or_none(can)
+
+      if c is not None:
+         for msg in c.can:
+            if msg.address == addr:
+               r.append(msg)
+               received = True
+
+      if time.time() - t > 0.05:
+         received = True
+
+   return r
+
+
+def print_hex(d):
+   s = map(ord, d)
+   s = "".join(["{:02X}".format(b) for b in s])
+   print(s)
+
+
+TYPES = {
+   0: 'single',
+   1: 'first',
+   2: 'consecutive',
+   3: 'flow'
+}
+
+FIRST = "\xFF\x02\xA8\x01\x00\x00\x00\x00"
+CONTINUE = "\xFF\x30\x01\x00\x00\x00\x00\x00"
+TEST_ON = "\xFF\x02\x10\x01\x00\x00\x00\x00"
+POLL = "\xFF\x02\x21\x69\x00\x00\x00\x00"
+
+prev_rcv_t = ""
+recv_data = []
+l = 0
+index = 0
+
+
+can = messaging.sub_sock('can')
+sendcan = messaging.pub_sock('sendcan')
+
+time.sleep(0.5)
+
+send(sendcan, 1872, FIRST)
+results = []
+
+test_mode = False
+
+while True:
+   # Send flow control if necessary
+   if prev_rcv_t == 'first' or prev_rcv_t == 'consecutive':
+      send(sendcan, 1872, CONTINUE)
+
+   received = recv_timeout(can, 1880)
+
+   if len(received) == 0:
+      print_hex(results[0])
+      # print chr(27) + "[2J"
+      # print time.time()
+      # if results[0] != "\x7F\x21\x31":
+      #    field_results[cur_field] = results[0]
+      # else:
+      #    fields.remove(cur_field)
+      # for k in fields:
+      #    if field_results[k] == "\x00\x00":
+      #       continue
+      #    print k,
+      #    print_hex(field_results[k])
+      results = []
+
+      if not test_mode:
+         send(sendcan, 1872, TEST_ON)
+         test_mode = True
+      else:
+         cur_field = random.choice(fields)
+         send(sendcan, 1872, POLL.replace('\x69', chr(cur_field)))
+
+   for r in received:
+      data = r.dat
+
+      # Check message type
+      t = TYPES[ord(data[1]) >> 4]
+      if t == 'single':
+         l = ord(data[1]) & 0x0F
+      elif t == 'first':
+         a = ord(data[1]) & 0x0F
+         b = ord(data[2])
+         l = b + (a << 8)
+         recv_data = []
+
+      prev_rcv_t = t
+
+      if t == 'single':
+         recv_data = data[2: 2 + l]
+         results.append(recv_data)
+      if t == 'first':
+         index = 0
+         recv_data += data[3: min(8, 3 + l)]
+      if t == 'consecutive':
+         index += 1
+         assert index == ord(data[1]) & 0x0F
+
+         pending_l = l - len(recv_data)
+         recv_data += data[2: min(8, 2 + pending_l)]
+
+         if len(recv_data) == l:
+            prev_rcv_t = ""
+            results.append(recv_data)
--- a/selfdrive/debug/internal/toyota/blinker_toyota.py
+++ b/selfdrive/debug/internal/toyota/blinker_toyota.py
@ -0,0 +1,41 @@
+#!/usr/bin/env python3
+import zmq
+import time
+from collections import defaultdict, OrderedDict
+
+from selfdrive.boardd.boardd import can_list_to_can_capnp
+from selfdrive.car.toyota.toyotacan import make_can_msg
+import cereal.messaging as messaging
+from cereal.services import service_list
+
+can = messaging.sub_sock('can')
+sendcan = messaging.pub_sock('sendcan')
+
+
+BEFORE = [
+"\x10\x15\x30\x0B\x00\x00\x00\x00",
+"\x21\x00\x00\x00\x00\x00\x00\x00",
+]
+
+LEFT = "\x22\x00\x00\x08\x00\x00\x00\x00"
+RIGHT = "\x22\x00\x00\x04\x00\x00\x00\x00"
+OFF = "\x22\x00\x00\x00\x00\x00\x00\x00"
+
+AFTER = "\x23\x00\x00\x00\x00\x00\x00\x00"
+
+i = 0
+j = 0
+while True:
+   i += 1
+
+   if i % 10 == 0:
+     j += 1
+
+   cur = RIGHT if j % 2 == 0 else OFF
+   can_list = [make_can_msg(1984, d, 0, False) for d in BEFORE]
+   can_list.append(make_can_msg(1984, cur, 0, False))
+   can_list.append(make_can_msg(1984, AFTER, 0, False))
+
+   for m in can_list:
+     sendcan.send(can_list_to_can_capnp([m], msgtype='sendcan').to_bytes())
+     time.sleep(0.01)
--- a/selfdrive/debug/internal/toyota/continental_radar.py
+++ b/selfdrive/debug/internal/toyota/continental_radar.py
@ -0,0 +1,61 @@
+#!/usr/bin/env python3
+import zmq
+import time
+from collections import defaultdict, OrderedDict
+
+from selfdrive.boardd.boardd import can_list_to_can_capnp
+from selfdrive.car.toyota.toyotacan import make_can_msg
+import cereal.messaging as messaging
+from cereal.services import service_list
+
+
+def send(sendcan, addr, m):
+   packet = make_can_msg(addr, m, 0, False)
+   packets = can_list_to_can_capnp([packet], msgtype='sendcan')
+   sendcan.send(packets.to_bytes())
+
+
+def recv_timeout(can, addr):
+   received = False
+   r = []
+   t = time.time()
+
+   while not received:
+      c = messaging.recv_one_or_none(can)
+
+      if c is not None:
+         for msg in c.can:
+            if msg.address == addr:
+               r.append(msg)
+               received = True
+
+      if time.time() - t > 0.1:
+         received = True
+
+   return r
+
+
+can = messaging.sub_sock('can')
+sendcan = messaging.pub_sock('sendcan')
+
+PUBLIC = 0
+PRIVATE = 1
+
+time.sleep(0.5)
+
+# 1, 112
+
+TEST_ON = "\xFF\x02\x10\x70\x00\x00\x00\x00"
+POLL = "\xFF\x02\x21\x69\x00\x00\x00\x00"
+send(sendcan, 1872, TEST_ON)
+r = recv_timeout(can, 1880)
+print(r)
+
+
+for i in range(0, 256):
+   send(sendcan, 1872, POLL.replace('\x69', chr(i)))
+   r = recv_timeout(can, 1880)
+   if len(r):
+      print(i, end=' ')
+      for m in r:
+         print(m.dat.encode('hex'))
--- a/selfdrive/debug/internal/toyota/simple_can_printer.py
+++ b/selfdrive/debug/internal/toyota/simple_can_printer.py
@ -0,0 +1,26 @@
+#!/usr/bin/env python3
+import zmq
+from collections import OrderedDict
+import cereal.messaging as messaging
+from cereal.services import service_list
+
+can = messaging.sub_sock('can')
+
+addr = OrderedDict()
+
+while True:
+  c = messaging.recv_one(can)
+  for msg in c.can:
+    s = map(ord, msg.dat)
+    s = "".join(["\\x{:02X}".format(b) for b in s])
+    s = "\"" + s + "\","
+
+    if msg.address == 1872:
+      print("s:", s)
+    if msg.address == 1880:
+      print("r:", s)
+
+    if msg.address not in addr:
+       addr[msg.address] = list()
+    if msg.dat not in addr[msg.address]:
+      addr[msg.address].append(s)
--- a/selfdrive/debug/internal/toyota/sonar.py
+++ b/selfdrive/debug/internal/toyota/sonar.py
@ -0,0 +1,159 @@
+#!/usr/bin/env python3
+import sys
+import zmq
+import os
+import time
+import random
+from collections import defaultdict, OrderedDict
+
+from selfdrive.boardd.boardd import can_list_to_can_capnp
+from selfdrive.car.toyota.toyotacan import make_can_msg
+import cereal.messaging as messaging
+from cereal.services import service_list
+
+changing = []
+fields = range(0, 256)
+# fields = [225, 50, 39, 40]
+fields = [50]
+field_results = defaultdict(lambda: "\x00\x00")
+cur_field = 97
+
+def send(sendcan, addr, m):
+   packet = make_can_msg(addr, m, 0, False)
+   packets = can_list_to_can_capnp([packet], msgtype='sendcan')
+   sendcan.send(packets.to_bytes())
+
+
+def recv(can, addr):
+   received = False
+   r = []
+
+   while not received:
+      c = messaging.recv_one(can)
+      for msg in c.can:
+         if msg.address == addr:
+            r.append(msg)
+            received = True
+   return r
+
+
+def recv_timeout(can, addr):
+   received = False
+   r = []
+   t = time.time()
+
+   while not received:
+      c = messaging.recv_one_or_none(can)
+
+      if c is not None:
+         for msg in c.can:
+            if msg.address == addr:
+               r.append(msg)
+               received = True
+
+      if time.time() - t > 0.05:
+         received = True
+
+   return r
+
+
+def print_hex(d):
+   s = map(ord, d)
+   s = "".join(["{:02X}".format(b) for b in s])
+   print(s)
+
+
+TYPES = {
+   0: 'single',
+   1: 'first',
+   2: 'consecutive',
+   3: 'flow'
+}
+
+CONTINUE = "\x67\x30\x01\x00\x00\x00\x00\x00"
+TEST_ON = "\x67\x02\x10\x74\x00\x00\x00\x00"
+POLL = "\x67\x02\x21\x69\x00\x00\x00\x00"
+# POLL = "\x67\x02\x10\x69\x00\x00\x00\x00"
+
+prev_rcv_t = ""
+recv_data = []
+l = 0
+index = 0
+
+
+can = messaging.sub_sock('can')
+sendcan = messaging.pub_sock('sendcan')
+
+time.sleep(0.5)
+
+results = []
+
+test_mode = False
+
+while True:
+   # Send flow control if necessary
+   if prev_rcv_t == 'first' or prev_rcv_t == 'consecutive':
+      send(sendcan, 1872, CONTINUE)
+
+   received = recv_timeout(can, 1880)
+
+   if len(received) == 0:
+      sys.stdout.flush()
+      print(chr(27) + "[2J")
+      print(time.time())
+      print(changing)
+
+      if len(results):
+         if results[0] != "\x7F\x21\x31":
+            old = field_results[cur_field]
+            if old != '\x00\x00' and old != results[0] and cur_field not in changing:
+               changing.append(cur_field)
+            field_results[cur_field] = results[0]
+         else:
+            fields.remove(cur_field)
+
+      for k in fields:
+         # if field_results[k] == "\x00\x00":
+         #    continue
+         print(k, end=' ')
+         print_hex(field_results[k])
+      results = []
+
+      if not test_mode:
+         send(sendcan, 1872, TEST_ON)
+         test_mode = True
+      else:
+         cur_field = random.choice(fields)
+         send(sendcan, 1872, POLL.replace('\x69', chr(cur_field)))
+
+   for r in received:
+      data = r.dat
+
+      # Check message type
+      t = TYPES[ord(data[1]) >> 4]
+      if t == 'single':
+         l = ord(data[1]) & 0x0F
+      elif t == 'first':
+         a = ord(data[1]) & 0x0F
+         b = ord(data[2])
+         l = b + (a << 8)
+         recv_data = []
+
+      prev_rcv_t = t
+
+      if t == 'single':
+         recv_data = data[2: 2 + l]
+         results.append(recv_data)
+      if t == 'first':
+         index = 0
+         recv_data += data[3: min(8, 3 + l)]
+      if t == 'consecutive':
+         index += 1
+         assert index == ord(data[1]) & 0x0F
+
+         pending_l = l - len(recv_data)
+         recv_data += data[2: min(8, 2 + pending_l)]
+
+         if len(recv_data) == l:
+            prev_rcv_t = ""
+            results.append(recv_data)
--- a/selfdrive/debug/internal/vehicle_model_sim.py
+++ b/selfdrive/debug/internal/vehicle_model_sim.py
@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+import numpy as np
+from selfdrive.controls.lib.vehicle_model import VehicleModel, calc_slip_factor
+from selfdrive.car.honda.interface import CarInterface
+
+def mpc_path_prediction(sa, u, psi_0, dt, VM):
+  # sa and u needs to be numpy arrays
+  sa_w = sa * np.pi / 180. / VM.CP.steerRatio
+  x = np.zeros(len(sa))
+  y = np.zeros(len(sa))
+  psi = np.ones(len(sa)) * psi_0
+
+  for i in range(0, len(sa)-1):
+    x[i+1] = x[i] + np.cos(psi[i]) * u[i] * dt
+    y[i+1] = y[i] + np.sin(psi[i]) * u[i] * dt
+    psi[i+1] = psi[i] + sa_w[i] * u[i] * dt * VM.curvature_factor(u[i])
+
+  return x, y, psi
+
+
+def model_path_prediction(sa, u, psi_0, dt, VM):
+  # steady state solution
+  sa_r = sa * np.pi / 180.
+  x = np.zeros(len(sa))
+  y = np.zeros(len(sa))
+  psi = np.ones(len(sa)) * psi_0
+  for i in range(0, len(sa)-1):
+
+    out = VM.steady_state_sol(sa_r[i], u[i])
+
+    x[i+1] = x[i] + np.cos(psi[i]) * u[i] * dt - np.sin(psi[i]) * out[0] * dt
+    y[i+1] = y[i] + np.sin(psi[i]) * u[i] * dt + np.cos(psi[i]) * out[0] * dt
+    psi[i+1] = psi[i] + out[1] * dt
+
+  return x, y, psi
+
+if __name__ == "__main__":
+  CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING")
+  print(CP)
+  VM = VehicleModel(CP)
+  print(VM.steady_state_sol(.1, 0.15))
+  print(calc_slip_factor(VM))
+  print("Curv", VM.curvature_factor(30.))
+
+  dt = 0.05
+  st = 20
+  u = np.ones(st) * 1.
+  sa = np.ones(st) * 1.
+
+  out = mpc_path_prediction(sa, u, dt, VM)
+  out_model = model_path_prediction(sa, u, dt, VM)
+
+  print("mpc", out)
+  print("model", out_model)
--- a/selfdrive/debug/live_cpu_and_temp.py
+++ b/selfdrive/debug/live_cpu_and_temp.py
@ -0,0 +1,49 @@
+#!/usr/bin/env python3
+import numpy as np
+
+from cereal.messaging import SubMaster
+
+def cputime_total(ct):
+    return ct.user + ct.nice + ct.system + ct.idle + ct.iowait + ct.irq + ct.softirq
+
+
+def cputime_busy(ct):
+    return ct.user + ct.nice + ct.system + ct.irq + ct.softirq
+
+
+
+sm = SubMaster(['thermal', 'procLog'])
+
+last_temp = 0.0
+total_times = [0., 0., 0., 0.]
+busy_times = [0., 0., 0.0, 0.]
+
+
+while True:
+  sm.update()
+
+  if sm.updated['thermal']:
+    t = sm['thermal']
+    last_temp = np.mean([t.cpu0, t.cpu1, t.cpu2, t.cpu3]) / 10.
+
+  if sm.updated['procLog']:
+    m = sm['procLog']
+
+    cores = [0., 0., 0., 0.]
+    total_times_new = [0., 0., 0., 0.]
+    busy_times_new = [0., 0., 0.0, 0.]
+
+    for c in m.cpuTimes:
+      n = c.cpuNum
+      total_times_new[n] = cputime_total(c)
+      busy_times_new[n] = cputime_busy(c)
+
+    for n in range(4):
+      t_busy = busy_times_new[n] - busy_times[n]
+      t_total = total_times_new[n] - total_times[n]
+      cores[n] = t_busy / t_total
+
+    total_times = total_times_new[:]
+    busy_times = busy_times_new[:]
+
+    print("CPU %.2f%% - Temp %.2f" % (100. * np.mean(cores), last_temp ))
--- a/selfdrive/debug/mpc/live_lateral_mpc.py
+++ b/selfdrive/debug/mpc/live_lateral_mpc.py
@ -0,0 +1,110 @@
+#!/usr/bin/env python3
+import matplotlib
+matplotlib.use('TkAgg')
+
+import sys
+import cereal.messaging as messaging
+import numpy as np
+import matplotlib.pyplot as plt
+
+# debug liateral MPC by plotting its trajectory. To receive liveLongitudinalMpc packets,
+# set on LOG_MPC env variable and run plannerd on a replay
+
+
+def mpc_vwr_thread(addr="127.0.0.1"):
+
+  plt.ion()
+  fig = plt.figure(figsize=(15, 20))
+  ax = fig.add_subplot(131)
+  aa = fig.add_subplot(132, sharey=ax)
+  ap = fig.add_subplot(133, sharey=ax)
+
+  ax.set_xlim([-10, 10])
+  ax.set_ylim([0., 100.])
+  aa.set_xlim([-20., 20])
+  ap.set_xlim([-5, 5])
+
+  ax.set_xlabel('x [m]')
+  ax.set_ylabel('y [m]')
+  aa.set_xlabel('steer_angle [deg]')
+  ap.set_xlabel('asset angle [deg]')
+  ax.grid(True)
+  aa.grid(True)
+  ap.grid(True)
+
+  path_x = np.arange(0, 100)
+  mpc_path_x = np.arange(0, 49)
+
+  p_path_y = np.zeros(100)
+
+  l_path_y = np.zeros(100)
+  r_path_y = np.zeros(100)
+  mpc_path_y = np.zeros(49)
+  mpc_steer_angle = np.zeros(49)
+  mpc_psi = np.zeros(49)
+
+  line1, = ax.plot(mpc_path_y, mpc_path_x)
+  # line1b, = ax.plot(mpc_path_y, mpc_path_x, 'o')
+
+  lineP, = ax.plot(p_path_y, path_x)
+  lineL, = ax.plot(l_path_y, path_x)
+  lineR, = ax.plot(r_path_y, path_x)
+  line3, = aa.plot(mpc_steer_angle, mpc_path_x)
+  line4, = ap.plot(mpc_psi, mpc_path_x)
+  ax.invert_xaxis()
+  aa.invert_xaxis()
+  plt.show()
+
+
+  # *** log ***
+  livempc = messaging.sub_sock('liveMpc', addr=addr)
+  model = messaging.sub_sock('model', addr=addr)
+  path_plan_sock = messaging.sub_sock('pathPlan', addr=addr)
+
+  while 1:
+    lMpc = messaging.recv_sock(livempc, wait=True)
+    md = messaging.recv_sock(model)
+    pp = messaging.recv_sock(path_plan_sock)
+
+    if md is not None:
+      p_poly = np.array(md.model.path.poly)
+      l_poly = np.array(md.model.leftLane.poly)
+      r_poly = np.array(md.model.rightLane.poly)
+
+      p_path_y = np.polyval(p_poly, path_x)
+      l_path_y = np.polyval(r_poly, path_x)
+      r_path_y = np.polyval(l_poly, path_x)
+
+    if pp is not None:
+      p_path_y = np.polyval(pp.pathPlan.dPoly, path_x)
+      lineP.set_xdata(p_path_y)
+      lineP.set_ydata(path_x)
+
+    if lMpc is not None:
+      mpc_path_x  = list(lMpc.liveMpc.x)[1:]
+      mpc_path_y  = list(lMpc.liveMpc.y)[1:]
+      mpc_steer_angle  = list(lMpc.liveMpc.delta)[1:]
+      mpc_psi  = list(lMpc.liveMpc.psi)[1:]
+
+      line1.set_xdata(mpc_path_y)
+      line1.set_ydata(mpc_path_x)
+      lineL.set_xdata(l_path_y)
+      lineL.set_ydata(path_x)
+      lineR.set_xdata(r_path_y)
+      lineR.set_ydata(path_x)
+      line3.set_xdata(np.asarray(mpc_steer_angle)*180./np.pi * 14)
+      line3.set_ydata(mpc_path_x)
+      line4.set_xdata(np.asarray(mpc_psi)*180./np.pi)
+      line4.set_ydata(mpc_path_x)
+
+      aa.relim()
+      aa.autoscale_view(True, scaley=True, scalex=True)
+
+      fig.canvas.draw()
+      fig.canvas.flush_events()
+
+if __name__ == "__main__":
+  if len(sys.argv) > 1:
+    mpc_vwr_thread(sys.argv[1])
+  else:
+    mpc_vwr_thread()
--- a/selfdrive/debug/mpc/live_longitudinal_mpc.py
+++ b/selfdrive/debug/mpc/live_longitudinal_mpc.py
@ -0,0 +1,106 @@
+#!/usr/bin/env python3
+
+import sys
+import cereal.messaging as messaging
+import numpy as np
+import matplotlib.pyplot as plt
+
+N = 21
+
+# debug longitudinal MPC by plotting its trajectory. To receive liveLongitudinalMpc packets,
+# set on LOG_MPC env variable and run plannerd on a replay
+
+def plot_longitudinal_mpc(addr="127.0.0.1"):
+  # *** log ***
+  livempc = messaging.sub_sock('liveLongitudinalMpc', addr=addr, conflate=True)
+  radarstate = messaging.sub_sock('radarState', addr=addr, conflate=True)
+
+  plt.ion()
+  fig = plt.figure()
+
+  t = np.hstack([np.arange(0.0, 0.8, 0.2), np.arange(0.8, 10.6, 0.6)])
+
+  p_x_ego = fig.add_subplot(3, 2, 1)
+  p_v_ego = fig.add_subplot(3, 2, 3)
+  p_a_ego = fig.add_subplot(3, 2, 5)
+  # p_x_l = fig.add_subplot(3, 2, 2)
+  # p_a_l = fig.add_subplot(3, 2, 6)
+  p_d_l = fig.add_subplot(3, 2, 2)
+  p_d_l_v = fig.add_subplot(3, 2, 4)
+  p_d_l_vv = fig.add_subplot(3, 2, 6)
+
+  p_v_ego.set_ylim([0, 30])
+  p_a_ego.set_ylim([-4, 4])
+  p_d_l.set_ylim([-1, 10])
+
+  p_x_ego.set_title('x')
+  p_v_ego.set_title('v')
+  p_a_ego.set_title('a')
+  p_d_l.set_title('rel dist')
+
+  l_x_ego, = p_x_ego.plot(t, np.zeros(N))
+  l_v_ego, = p_v_ego.plot(t, np.zeros(N))
+  l_a_ego, = p_a_ego.plot(t, np.zeros(N))
+  l_x_l, = p_x_ego.plot(t, np.zeros(N))
+  l_v_l, = p_v_ego.plot(t, np.zeros(N))
+  l_a_l, = p_a_ego.plot(t, np.zeros(N))
+  l_d_l, = p_d_l.plot(t, np.zeros(N))
+  l_d_l_v, = p_d_l_v.plot(np.zeros(N))
+  l_d_l_vv, = p_d_l_vv.plot(np.zeros(N))
+  p_x_ego.legend(['ego', 'l'])
+  p_v_ego.legend(['ego', 'l'])
+  p_a_ego.legend(['ego', 'l'])
+  p_d_l_v.set_xlabel('d_rel')
+  p_d_l_v.set_ylabel('v_rel')
+  p_d_l_v.set_ylim([-20, 20])
+  p_d_l_v.set_xlim([0, 100])
+  p_d_l_vv.set_xlabel('d_rel')
+  p_d_l_vv.set_ylabel('v_rel')
+  p_d_l_vv.set_ylim([-5, 5])
+  p_d_l_vv.set_xlim([10, 40])
+
+  while True:
+    lMpc = messaging.recv_sock(livempc, wait=True)
+    rs = messaging.recv_sock(radarstate, wait=True)
+
+    if lMpc is not None:
+
+      if lMpc.liveLongitudinalMpc.mpcId != 1:
+        continue
+
+      x_ego = list(lMpc.liveLongitudinalMpc.xEgo)
+      v_ego = list(lMpc.liveLongitudinalMpc.vEgo)
+      a_ego = list(lMpc.liveLongitudinalMpc.aEgo)
+      x_l = list(lMpc.liveLongitudinalMpc.xLead)
+      v_l = list(lMpc.liveLongitudinalMpc.vLead)
+      # a_l = list(lMpc.liveLongitudinalMpc.aLead)
+      a_l = rs.radarState.leadOne.aLeadK * np.exp(-lMpc.liveLongitudinalMpc.aLeadTau * t**2 / 2)
+      #print(min(a_ego), lMpc.liveLongitudinalMpc.qpIterations)
+
+      l_x_ego.set_ydata(x_ego)
+      l_v_ego.set_ydata(v_ego)
+      l_a_ego.set_ydata(a_ego)
+
+      l_x_l.set_ydata(x_l)
+      l_v_l.set_ydata(v_l)
+      l_a_l.set_ydata(a_l)
+
+      l_d_l.set_ydata(np.array(x_l) - np.array(x_ego))
+      l_d_l_v.set_ydata(np.array(v_l) - np.array(v_ego))
+      l_d_l_v.set_xdata(np.array(x_l) - np.array(x_ego))
+      l_d_l_vv.set_ydata(np.array(v_l) - np.array(v_ego))
+      l_d_l_vv.set_xdata(np.array(x_l) - np.array(x_ego))
+
+      p_x_ego.relim()
+      p_x_ego.autoscale_view(True, scaley=True, scalex=True)
+      fig.canvas.draw()
+      fig.canvas.flush_events()
+
+
+
+
+if __name__ == "__main__":
+  if len(sys.argv) > 1:
+    plot_longitudinal_mpc(sys.argv[1])
+  else:
+    plot_longitudinal_mpc()
--- a/selfdrive/debug/mpc/test_mpc_wobble.py
+++ b/selfdrive/debug/mpc/test_mpc_wobble.py
@ -0,0 +1,129 @@
+#! /usr/bin/env python
+import matplotlib.pyplot as plt
+from selfdrive.controls.lib.lateral_mpc import libmpc_py
+from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
+import math
+
+libmpc = libmpc_py.libmpc
+libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, 1.)
+
+cur_state = libmpc_py.ffi.new("state_t *")
+cur_state[0].x = 0.0
+cur_state[0].y = 0.0
+cur_state[0].psi = 0.0
+cur_state[0].delta = 0.0
+
+mpc_solution = libmpc_py.ffi.new("log_t *")
+xx = []
+yy = []
+deltas = []
+psis = []
+times = []
+
+curvature_factor = 0.3
+v_ref = 1.0 * 20.12  # 45 mph
+
+LANE_WIDTH = 3.7
+p = [0.0, 0.0, 0.0, 0.0]
+p_l = p[:]
+p_l[3] += LANE_WIDTH / 2.0
+
+p_r = p[:]
+p_r[3] -= LANE_WIDTH / 2.0
+
+
+l_poly = libmpc_py.ffi.new("double[4]", p_l)
+r_poly = libmpc_py.ffi.new("double[4]", p_r)
+p_poly = libmpc_py.ffi.new("double[4]", p)
+
+l_prob = 1.0
+r_prob = 1.0
+p_prob = 1.0
+
+for i in range(1):
+  cur_state[0].delta = math.radians(510. / 13.)
+  libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
+                 curvature_factor, v_ref, LANE_WIDTH)
+
+timesi = []
+ct = 0
+for i in range(21):
+  timesi.append(ct)
+  if i <= 4:
+    ct += 0.05
+  else:
+    ct += 0.15
+
+
+xi = list(mpc_solution[0].x)
+yi = list(mpc_solution[0].y)
+psii = list(mpc_solution[0].psi)
+deltai = list(mpc_solution[0].delta)
+print("COST: ", mpc_solution[0].cost)
+
+
+plt.figure(0)
+plt.subplot(3, 1, 1)
+plt.plot(timesi, psii)
+plt.ylabel('psi')
+plt.grid(True)
+plt.subplot(3, 1, 2)
+plt.plot(timesi, deltai)
+plt.ylabel('delta')
+plt.grid(True)
+plt.subplot(3, 1, 3)
+plt.plot(timesi, yi)
+plt.ylabel('y')
+plt.grid(True)
+plt.show()
+
+
+####  UNCOMMENT TO CHECK ITERATIVE SOLUTION
+####
+####for i in range(100):
+####  libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
+####                 curvature_factor, v_ref, LANE_WIDTH)
+####  print "x", list(mpc_solution[0].x)
+####  print "y", list(mpc_solution[0].y)
+####  print "delta", list(mpc_solution[0].delta)
+####  print "psi", list(mpc_solution[0].psi)
+####  # cur_state[0].x = mpc_solution[0].x[1]
+####  # cur_state[0].y = mpc_solution[0].y[1]
+####  # cur_state[0].psi = mpc_solution[0].psi[1]
+####  cur_state[0].delta = radians(200 / 13.)#mpc_solution[0].delta[1]
+####
+####  xx.append(cur_state[0].x)
+####  yy.append(cur_state[0].y)
+####  psis.append(cur_state[0].psi)
+####  deltas.append(cur_state[0].delta)
+####  times.append(i * 0.05)
+####
+####
+####def f(x):
+####  return p_poly[0] * x**3 + p_poly[1] * x**2 + p_poly[2] * x + p_poly[3]
+####
+####
+##### planned = map(f, xx)
+##### plt.figure(1)
+##### plt.plot(yy, xx, 'r-')
+##### plt.plot(planned, xx, 'b--', linewidth=0.5)
+##### plt.axes().set_aspect('equal', 'datalim')
+##### plt.gca().invert_xaxis()
+####
+##### planned = map(f, map(float, list(mpc_solution[0].x)[1:]))
+##### plt.figure(1)
+##### plt.plot(map(float, list(mpc_solution[0].y)[1:]), map(float, list(mpc_solution[0].x)[1:]), 'r-')
+##### plt.plot(planned, map(float, list(mpc_solution[0].x)[1:]), 'b--', linewidth=0.5)
+##### plt.axes().set_aspect('equal', 'datalim')
+##### plt.gca().invert_xaxis()
+####
+####plt.figure(2)
+####plt.subplot(2, 1, 1)
+####plt.plot(times, psis)
+####plt.ylabel('psi')
+####plt.subplot(2, 1, 2)
+####plt.plot(times, deltas)
+####plt.ylabel('delta')
+####
+####
+####plt.show()
--- a/selfdrive/debug/mpc/tune_lateral.py
+++ b/selfdrive/debug/mpc/tune_lateral.py
@ -0,0 +1,186 @@
+#! /usr/bin/env python
+import numpy as np
+from collections import OrderedDict
+import matplotlib.pyplot as plt
+from selfdrive.car.honda.interface import CarInterface
+from selfdrive.controls.lib.lateral_mpc import libmpc_py
+from selfdrive.controls.lib.vehicle_model import VehicleModel
+
+# plot lateral MPC trajectory by defining boundary conditions:
+# lane lines, p_poly and vehicle states. Use this script to tune MPC costs
+
+libmpc = libmpc_py.libmpc
+
+mpc_solution = libmpc_py.ffi.new("log_t *")
+
+points_l = np.array([1.1049711, 1.1053879, 1.1073375, 1.1096942, 1.1124474, 1.1154714, 1.1192677, 1.1245866, 1.1321017, 1.1396152, 1.146443, 1.1555313, 1.1662073, 1.1774249, 1.1888939, 1.2009926, 1.2149779, 1.2300836, 1.2450289, 1.2617753, 1.2785473, 1.2974714, 1.3151019, 1.3331807, 1.3545501, 1.3763691, 1.3983455, 1.4215056, 1.4446729, 1.4691089, 1.4927692, 1.5175346, 1.5429921, 1.568854, 1.5968665, 1.6268958, 1.657122, 1.6853137, 1.7152609, 1.7477539, 1.7793678, 1.8098511, 1.8428392, 1.8746407, 1.9089606, 1.9426043, 1.9775689, 2.0136933, 2.0520134, 2.0891454])
+
+points_r = np.array([-2.4442139, -2.4449506, -2.4448867, -2.44377, -2.4422617, -2.4393811, -2.4374201, -2.4334245, -2.4286852, -2.4238286, -2.4177458, -2.4094386, -2.3994849, -2.3904033, -2.380136, -2.3699453, -2.3594661, -2.3474073, -2.3342307, -2.3194637, -2.3046403, -2.2881098, -2.2706163, -2.2530098, -2.235604, -2.2160542, -2.1967411, -2.1758952, -2.1544619, -2.1325269, -2.1091819, -2.0850561, -2.0621953, -2.0364127, -2.0119917, -1.9851667, -1.9590458, -1.9306552, -1.9024918, -1.8745357, -1.8432863, -1.8131843, -1.7822732, -1.7507075, -1.7180918, -1.6845931, -1.650871, -1.6157099, -1.5787286, -1.5418037])
+
+
+points_c = (points_l + points_r) / 2.0
+
+def compute_path_pinv():
+  deg = 3
+  x = np.arange(50.0)
+  X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
+  pinv = np.linalg.pinv(X)
+  return pinv
+
+
+def model_polyfit(points):
+  path_pinv = compute_path_pinv()
+  return np.dot(path_pinv, map(float, points))
+
+
+xx = []
+yy = []
+deltas = []
+psis = []
+times = []
+
+CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING")
+VM = VehicleModel(CP)
+
+v_ref = 32.00  # 45 mph
+curvature_factor = VM.curvature_factor(v_ref)
+print(curvature_factor)
+
+LANE_WIDTH = 3.9
+p_l = map(float, model_polyfit(points_l))
+p_r = map(float, model_polyfit(points_r))
+p_p = map(float, model_polyfit(points_c))
+
+l_poly = libmpc_py.ffi.new("double[4]", p_l)
+r_poly = libmpc_py.ffi.new("double[4]", p_r)
+p_poly = libmpc_py.ffi.new("double[4]", p_p)
+l_prob = 1.0
+r_prob = 1.0
+p_prob = 1.0  # This is always 1
+
+
+mpc_x_points = np.linspace(0., 2.5*v_ref, num=50)
+points_poly_l = np.polyval(p_l, mpc_x_points)
+points_poly_r = np.polyval(p_r, mpc_x_points)
+points_poly_p = np.polyval(p_p, mpc_x_points)
+print(points_poly_l)
+
+lanes_x = np.linspace(0, 49)
+
+cur_state = libmpc_py.ffi.new("state_t *")
+cur_state[0].x = 0.0
+cur_state[0].y = 0.5
+cur_state[0].psi = 0.0
+cur_state[0].delta = 0.0
+
+xs = []
+ys = []
+deltas = []
+titles = [
+  'Steer rate cost',
+  'Heading cost',
+  'Lane cost',
+  'Path cost',
+]
+
+# Steer rate cost
+sol_x = OrderedDict()
+sol_y = OrderedDict()
+delta = OrderedDict()
+for cost in np.logspace(-1, 1.0, 5):
+  libmpc.init(1.0, 3.0, 1.0, cost)
+  for _ in range(10):
+    libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
+                  curvature_factor, v_ref, LANE_WIDTH)
+  sol_x[cost] = map(float, list(mpc_solution[0].x))
+  sol_y[cost] = map(float, list(mpc_solution[0].y))
+  delta[cost] = map(float, list(mpc_solution[0].delta))
+xs.append(sol_x)
+ys.append(sol_y)
+deltas.append(delta)
+
+# Heading cost
+sol_x = OrderedDict()
+sol_y = OrderedDict()
+delta = OrderedDict()
+for cost in np.logspace(-1, 1.0, 5):
+  libmpc.init(1.0, 3.0, cost, 1.0)
+  for _ in range(10):
+    libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
+                  curvature_factor, v_ref, LANE_WIDTH)
+  sol_x[cost] = map(float, list(mpc_solution[0].x))
+  sol_y[cost] = map(float, list(mpc_solution[0].y))
+  delta[cost] = map(float, list(mpc_solution[0].delta))
+xs.append(sol_x)
+ys.append(sol_y)
+deltas.append(delta)
+
+# Lane cost
+sol_x = OrderedDict()
+sol_y = OrderedDict()
+delta = OrderedDict()
+for cost in np.logspace(-1, 2.0, 5):
+  libmpc.init(1.0, cost, 1.0, 1.0)
+  for _ in range(10):
+    libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
+                  curvature_factor, v_ref, LANE_WIDTH)
+  sol_x[cost] = map(float, list(mpc_solution[0].x))
+  sol_y[cost] = map(float, list(mpc_solution[0].y))
+  delta[cost] = map(float, list(mpc_solution[0].delta))
+xs.append(sol_x)
+ys.append(sol_y)
+deltas.append(delta)
+
+
+# Path cost
+sol_x = OrderedDict()
+sol_y = OrderedDict()
+delta = OrderedDict()
+for cost in np.logspace(-1, 1.0, 5):
+  libmpc.init(cost, 3.0, 1.0, 1.0)
+  for _ in range(10):
+    libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
+                  curvature_factor, v_ref, LANE_WIDTH)
+  sol_x[cost] = map(float, list(mpc_solution[0].x))
+  sol_y[cost] = map(float, list(mpc_solution[0].y))
+  delta[cost] = map(float, list(mpc_solution[0].delta))
+xs.append(sol_x)
+ys.append(sol_y)
+deltas.append(delta)
+
+
+
+plt.figure()
+
+for i in range(len(xs)):
+  ax = plt.subplot(2, 2, i + 1)
+  sol_x = xs[i]
+  sol_y = ys[i]
+  for cost in sol_x.keys():
+    plt.plot(sol_x[cost], sol_y[cost])
+
+  plt.plot(lanes_x, points_r, '.b')
+  plt.plot(lanes_x, points_l, '.b')
+  plt.plot(lanes_x, (points_l + points_r) / 2.0, '--g')
+  plt.plot(mpc_x_points, points_poly_l, 'b')
+  plt.plot(mpc_x_points, points_poly_r, 'b')
+  plt.plot(mpc_x_points, (points_poly_l + points_poly_r) / 2.0, 'g')
+  plt.legend(map(lambda x: str(round(x, 2)), sol_x.keys()) + ['right', 'left', 'center'], loc=3)
+  plt.title(titles[i])
+  plt.grid(True)
+  # ax.set_aspect('equal', 'datalim')
+
+
+plt.figure()
+for i in range(len(xs)):
+  plt.subplot(2, 2, i + 1)
+  sol_x = xs[i]
+  delta = deltas[i]
+
+  for cost in sol_x.keys():
+    plt.plot(delta[cost])
+  plt.title(titles[i])
+  plt.legend(map(lambda x: str(round(x, 2)), sol_x.keys()), loc=3)
+  plt.grid(True)
+
+plt.show()
--- a/selfdrive/debug/mpc/tune_longitudinal.py
+++ b/selfdrive/debug/mpc/tune_longitudinal.py
@ -0,0 +1,168 @@
+#! /usr/bin/env python
+import numpy as np
+import matplotlib.pyplot as plt
+from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
+from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG
+import math
+
+# plot liongitudinal MPC trajectory by defining boundary conditions:
+# ego and lead vehicles state. Use this script to tune MPC costs
+
+def RW(v_ego, v_l):
+    TR = 1.8
+    G = 9.81
+    return (v_ego * TR - (v_l - v_ego) * TR + v_ego*v_ego/(2*G) - v_l*v_l / (2*G))
+
+
+def NORM_RW_ERROR(v_ego, v_l, p):
+    return (RW(v_ego, v_l) + 4.0 - p)
+    return (RW(v_ego, v_l) + 4.0 - p) / (np.sqrt(v_ego + 0.5) + 0.1)
+
+
+v_ego = 20.0
+a_ego = 0
+
+x_lead = 10.0
+v_lead = 20.0
+a_lead = -3.0
+a_lead_tau = 0.
+
+# v_ego = 7.02661012716
+# a_ego = -1.26143024772
+
+# x_lead = 29.625 + 20
+# v_lead = 0.725235462189 + 1
+# a_lead = -1.00025629997
+
+# a_lead_tau = 2.90729817665
+
+min_a_lead_tau = (a_lead**2 * math.pi) / (2 * (v_lead + 0.01)**2)
+min_a_lead_tau = 0.0
+
+print(a_lead_tau, min_a_lead_tau)
+a_lead_tau = max(a_lead_tau, min_a_lead_tau)
+
+ffi, libmpc = libmpc_py.get_libmpc(1)
+libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+libmpc.init_with_simulation(v_ego, x_lead, v_lead, a_lead, a_lead_tau)
+
+cur_state = ffi.new("state_t *")
+cur_state[0].x_ego = 0.0
+cur_state[0].v_ego = v_ego
+cur_state[0].a_ego = a_ego
+cur_state[0].x_l = x_lead
+cur_state[0].v_l = v_lead
+
+mpc_solution = ffi.new("log_t *")
+
+for _ in range(10):
+    print(libmpc.run_mpc(cur_state, mpc_solution, a_lead_tau, a_lead))
+
+
+for i in range(21):
+  print("t: %.2f\t x_e: %.2f\t v_e: %.2f\t a_e: %.2f\t" % (mpc_solution[0].t[i], mpc_solution[0].x_ego[i], mpc_solution[0].v_ego[i], mpc_solution[0].a_ego[i]))
+  print("x_l: %.2f\t v_l: %.2f\t \t" % (mpc_solution[0].x_l[i], mpc_solution[0].v_l[i]))
+
+t = np.hstack([np.arange(0., 1.0, 0.2), np.arange(1.0, 10.1, 0.6)])
+
+print(map(float, mpc_solution[0].x_ego)[-1])
+print(map(float, mpc_solution[0].x_l)[-1] - map(float, mpc_solution[0].x_ego)[-1])
+
+plt.figure(figsize=(8, 8))
+
+plt.subplot(4, 1, 1)
+x_l = np.array(map(float, mpc_solution[0].x_l))
+plt.plot(t, map(float, mpc_solution[0].x_ego))
+plt.plot(t, x_l)
+plt.legend(['ego', 'lead'])
+plt.title('x')
+plt.grid()
+
+plt.subplot(4, 1, 2)
+v_ego = np.array(map(float, mpc_solution[0].v_ego))
+v_l = np.array(map(float, mpc_solution[0].v_l))
+plt.plot(t, v_ego)
+plt.plot(t, v_l)
+plt.legend(['ego', 'lead'])
+plt.ylim([-1, max(max(v_ego), max(v_l))])
+plt.title('v')
+plt.grid()
+
+plt.subplot(4, 1, 3)
+plt.plot(t, map(float, mpc_solution[0].a_ego))
+plt.plot(t, map(float, mpc_solution[0].a_l))
+plt.legend(['ego', 'lead'])
+plt.title('a')
+plt.grid()
+
+
+plt.subplot(4, 1, 4)
+d_l = np.array(map(float, mpc_solution[0].x_l)) - np.array(map(float, mpc_solution[0].x_ego))
+desired = 4.0 + RW(v_ego, v_l)
+
+plt.plot(t, d_l)
+plt.plot(t, desired, '--')
+plt.ylim(-1, max(max(desired), max(d_l)))
+plt.legend(['relative distance', 'desired distance'])
+plt.grid()
+
+plt.show()
+
+# c1 = np.exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l))
+# c2 = np.exp(4.5 - d_l)
+# print(c1)
+# print(c2)
+
+# plt.figure()
+# plt.plot(t, c1, label="NORM_RW_ERROR")
+# plt.plot(t, c2, label="penalty function")
+# plt.legend()
+
+# ## OLD MPC
+# a_lead_tau = 1.5
+# a_lead_tau = max(a_lead_tau, -a_lead / (v_lead + 0.01))
+
+# ffi, libmpc = libmpc_py.get_libmpc(1)
+# libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+# libmpc.init_with_simulation(v_ego, x_lead, v_lead, a_lead, a_lead_tau)
+
+# cur_state = ffi.new("state_t *")
+# cur_state[0].x_ego = 0.0
+# cur_state[0].v_ego = v_ego
+# cur_state[0].a_ego = a_ego
+# cur_state[0].x_lead = x_lead
+# cur_state[0].v_lead = v_lead
+# cur_state[0].a_lead = a_lead
+
+# mpc_solution = ffi.new("log_t *")
+
+# for _ in range(10):
+#     print libmpc.run_mpc(cur_state, mpc_solution, a_lead_tau)
+
+# t = np.hstack([np.arange(0., 1.0, 0.2), np.arange(1.0, 10.1, 0.6)])
+
+# print(map(float, mpc_solution[0].x_ego)[-1])
+# print(map(float, mpc_solution[0].x_lead)[-1] - map(float, mpc_solution[0].x_ego)[-1])
+# plt.subplot(4, 2, 2)
+# plt.plot(t, map(float, mpc_solution[0].x_ego))
+# plt.plot(t, map(float, mpc_solution[0].x_lead))
+# plt.legend(['ego', 'lead'])
+# plt.title('x')
+
+# plt.subplot(4, 2, 4)
+# plt.plot(t, map(float, mpc_solution[0].v_ego))
+# plt.plot(t, map(float, mpc_solution[0].v_lead))
+# plt.legend(['ego', 'lead'])
+# plt.title('v')
+
+# plt.subplot(4, 2, 6)
+# plt.plot(t, map(float, mpc_solution[0].a_ego))
+# plt.plot(t, map(float, mpc_solution[0].a_lead))
+# plt.legend(['ego', 'lead'])
+# plt.title('a')
+
+
+# plt.subplot(4, 2, 8)
+# plt.plot(t, np.array(map(float, mpc_solution[0].x_lead)) - np.array(map(float, mpc_solution[0].x_ego)))
+
+# plt.show()
--- a/selfdrive/debug/show_matching_cars.py
+++ b/selfdrive/debug/show_matching_cars.py
@ -0,0 +1,27 @@
+#!/usr/bin/env python3
+from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_known_cars
+import cereal.messaging as messaging
+
+
+# Prius and Leuxs es 300H
+fingerprint = {898: 8, 905: 8, 810: 2, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 921: 8, 800: 8, 944: 8, 1570: 8, 1059: 1, 36: 8, 37: 8, 550: 8, 295: 8, 296: 8, 170: 8, 1071: 8, 560: 7, 945: 8, 562: 6, 180: 8, 1077: 8, 950: 8, 951: 8, 953: 8, 1595: 8, 1084: 8, 829: 2, 1086: 8, 1568: 8, 452: 8, 581: 5, 1057: 8, 713: 8, 971: 7, 975: 5, 1571: 8, 466: 8, 467: 8, 1572: 8, 1114: 8, 933: 8, 863: 8, 608: 8, 993: 8, 610: 8, 955: 8, 166: 8, 1056: 8, 956: 8, 1132: 8, 1085: 8, 552: 4, 1779: 8, 1017: 8, 1020: 8, 426: 6, 1279: 8}
+
+# rav4 2019 and corolla tss2
+fingerprint = {896: 8, 898: 8, 976: 1, 1541: 8, 905: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1552: 8, 1553: 8, 1556: 8, 921: 8, 1056: 8, 544: 4, 1570: 8, 1059: 1, 36: 8, 37: 8, 550: 8, 552: 4, 170: 8, 812: 8, 944: 8, 945: 8, 562: 6, 180: 8, 1077: 8, 951: 8, 824: 8, 1076: 8, 186: 4, 955: 8, 956: 8, 705: 8, 452: 8, 1592: 8, 464: 8, 1571: 8, 466: 8, 467: 8, 761: 8, 728: 8, 1572: 8, 1114: 8, 933: 8, 800: 8, 608: 8, 865: 8, 610: 8, 1595: 8, 1745: 8, 764: 8, 1002: 8, 1649: 8, 1779: 8, 1568: 8, 1017: 8, 1279: 8, 1020: 8, 810: 2, 426: 6}
+
+# rav4 2019 and corolla tss2
+fingerprint = {896: 8, 898: 8, 900: 6, 976: 1, 1541: 8, 902: 6, 905: 8, 810: 2, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1552: 8, 1553: 8, 1556: 8, 1571: 8, 921: 8, 1056: 8, 544: 4, 1570: 8, 1059: 1, 36: 8, 37: 8, 550: 8, 935: 8, 552: 4, 170: 8, 812: 8, 944: 8, 945: 8, 562: 6, 180: 8, 1077: 8, 951: 8, 1592: 8, 1076: 8, 186: 4, 955: 8, 956: 8, 1001: 8, 705: 8, 452: 8, 1788: 8, 464: 8, 824: 8, 466: 8, 467: 8, 761: 8, 728: 8, 1572: 8, 1114: 8, 933: 8, 800: 8, 608: 8, 865: 8, 610: 8, 1595: 8, 934: 8, 998: 5, 1745: 8, 1000: 8, 764: 8, 1002: 8, 999: 7, 1789: 8, 1649: 8, 1779: 8, 1568: 8, 1017: 8, 1786: 8, 1787: 8, 1020: 8, 426: 6, 1279: 8}
+
+candidate_cars = all_known_cars()
+
+
+for addr, l in fingerprint.items():
+    dat = messaging.new_message()
+    dat.init('can', 1)
+
+    msg = dat.can[0]
+    msg.address = addr
+    msg.dat = " " * l
+
+    candidate_cars = eliminate_incompatible_cars(msg, candidate_cars)
+    print(candidate_cars)
--- a/selfdrive/debug/tuner.py
+++ b/selfdrive/debug/tuner.py
@ -0,0 +1,67 @@
+#!/usr/bin/env python3
+"""
+This tool can be used to quickly changes the values in a JSON file used for tuning
+Keys like in vim:
+ - h: decrease by 0.05
+ - l: increase by 0.05
+ - k: move pointer up
+ - j: move pointer down
+"""
+
+import tty
+import sys
+import json
+import termios
+from collections import OrderedDict
+
+FILENAME = '/data/tuning.json'
+
+def read_tuning():
+  while True:
+    try:
+      return json.loads(open(FILENAME).read())
+    except:
+      pass
+
+def main():
+  dat = json.loads(open(FILENAME, 'r').read())
+  dat = OrderedDict(sorted(dat.items(), key=lambda i: i[0]))
+
+  cur = 0
+  while True:
+    sys.stdout.write("\x1Bc")
+
+    for i, (k, v) in enumerate(dat.items()):
+      prefix = "> " if cur == i else "  "
+      print((prefix + k).ljust(20) + "%.2f" % v)
+
+    key = sys.stdin.read(1)[0]
+
+    write = False
+    if key == "k":
+      cur = max(0, cur - 1)
+    elif key == "j":
+      cur = min(len(dat.keys()) - 1, cur + 1)
+    elif key == "l":
+      dat[dat.keys()[cur]] += 0.05
+      write = True
+    elif key == "h":
+      dat[dat.keys()[cur]] -= 0.05
+      write = True
+    elif key == "q":
+      break
+
+    if write:
+      open(FILENAME, 'w').write(json.dumps(dat))
+
+
+if __name__ == "__main__":
+  orig_settings = termios.tcgetattr(sys.stdin)
+  tty.setcbreak(sys.stdin)
+
+  try:
+    main()
+    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, orig_settings)
+  except:
+    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, orig_settings)
+    raise